Skip to content
This repository was archived by the owner on Oct 8, 2025. It is now read-only.

Multi-robot support by namespacing TFs #13

Merged
fmrico merged 2 commits into from
Aug 20, 2025
Merged

Multi-robot support by namespacing TFs #13

Show file tree
Hide file tree
Changes from all commits
Commits
Show all changes
2 commits
Select commit Hold shift + click to select a range
072cc12
Multi-robot support by namespacing TFs
fmrico Aug 18, 2025
6745997
Set tf_prefix instead of tf_namespace
fmrico Aug 20, 2025
File filter

Filter by extension

Filter by extension
Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
6 changes: 3 additions & 3 deletions easynav_simple_controller/src/easynav_simple_controller/PIDController.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -27,9 +27,9 @@ PIDController::PIDController(double min_ref, double max_ref, double min_output,
max_output_ = max_output;
prev_error_ = int_error_ = 0.0;

KP_ = 0.41;
KI_ = 0.06;
KD_ = 0.53;
KP_ = 0.70;
KI_ = 0.15;
KD_ = 0.15;
}

void
Expand Down
14 changes: 7 additions & 7 deletions easynav_simple_localizer/src/easynav_simple_localizer/AMCLLocalizer.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -389,7 +389,7 @@ void AMCLLocalizer::correct(NavState & nav_state)

const auto & filtered = PointPerceptionsOpsView(perceptions)
.downsample(map_static.resolution())
.fuse("base_footprint")
.fuse(get_tf_prefix() + "base_footprint")
->filter({NAN, NAN, 0.1}, {NAN, NAN, NAN})
.collapse({NAN, NAN, 0.1})
->downsample(map_static.resolution())
Expand Down Expand Up @@ -527,8 +527,8 @@ AMCLLocalizer::publishTF(const tf2::Transform & map2bf)
{
geometry_msgs::msg::TransformStamped tf_msg;
tf_msg.header.stamp = get_node()->now();
tf_msg.header.frame_id = "map";
tf_msg.child_frame_id = "odom";
tf_msg.header.frame_id = get_tf_prefix() + "map";
tf_msg.child_frame_id = get_tf_prefix() + "odom";
tf_msg.transform = tf2::toMsg(map2bf);

RTTFBuffer::getInstance()->setTransform(tf_msg, "easynav", false);
Expand All @@ -540,7 +540,7 @@ AMCLLocalizer::publishParticles()
{
geometry_msgs::msg::PoseArray array_msg;
array_msg.header.stamp = get_node()->now();
array_msg.header.frame_id = "map";
array_msg.header.frame_id = get_tf_prefix() + "map";

array_msg.poses.reserve(particles_.size());
for (const auto & p : particles_) {
Expand Down Expand Up @@ -601,7 +601,7 @@ AMCLLocalizer::publishEstimatedPose(const tf2::Transform & est_pose)

geometry_msgs::msg::PoseWithCovarianceStamped msg;
msg.header.stamp = get_node()->now();
msg.header.frame_id = "map";
msg.header.frame_id = get_tf_prefix() + "map";

msg.pose.pose.position.x = mean.x();
msg.pose.pose.position.y = mean.y();
Expand All @@ -624,8 +624,8 @@ AMCLLocalizer::get_pose()
nav_msgs::msg::Odometry odom_msg;

odom_msg.header.stamp = get_node()->now();
odom_msg.header.frame_id = "map";
odom_msg.child_frame_id = "base_footprint";
odom_msg.header.frame_id = get_tf_prefix() + "map";
odom_msg.child_frame_id = get_tf_prefix() + "base_footprint";

tf2::Transform est_pose = getEstimatedPose();

Expand Down
16 changes: 8 additions & 8 deletions easynav_simple_maps_manager/src/easynav_simple_maps_manager/SimpleMapsManager.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -81,28 +81,28 @@ SimpleMapsManager::on_initialize()
}

static_occ_pub_ = node->create_publisher<nav_msgs::msg::OccupancyGrid>(
node->get_name() + std::string("/") + plugin_name + "/map",
node->get_fully_qualified_name() + std::string("/") + plugin_name + "/map",
rclcpp::QoS(1).transient_local().reliable());

dynamic_occ_pub_ = node->create_publisher<nav_msgs::msg::OccupancyGrid>(
node->get_name() + std::string("/") + plugin_name + "/dynamic_map", 100);
node->get_fully_qualified_name() + std::string("/") + plugin_name + "/dynamic_map", 100);

incoming_map_sub_ = node->create_subscription<nav_msgs::msg::OccupancyGrid>(
node->get_name() + std::string("/") + plugin_name + "/incoming_map",
node->get_fully_qualified_name() + std::string("/") + plugin_name + "/incoming_map",
rclcpp::QoS(1).transient_local().reliable(),
[this](nav_msgs::msg::OccupancyGrid::UniquePtr msg) {
static_map_.from_occupancy_grid(*msg);
dynamic_map_.from_occupancy_grid(*msg);

static_map_.to_occupancy_grid(static_grid_msg_);
static_grid_msg_.header.frame_id = "map";
static_grid_msg_.header.frame_id = get_tf_prefix() + "map";
static_grid_msg_.header.stamp = this->get_node()->now();

static_occ_pub_->publish(static_grid_msg_);
});

savemap_srv_ = node->create_service<std_srvs::srv::Trigger>(
node->get_name() + std::string("/") + plugin_name + "/savemap",
node->get_fully_qualified_name() + std::string("/") + plugin_name + "/savemap",
[this](
const std::shared_ptr<std_srvs::srv::Trigger::Request> request,
std::shared_ptr<std_srvs::srv::Trigger::Response> response)
Expand All @@ -118,7 +118,7 @@ SimpleMapsManager::on_initialize()
});

static_map_.to_occupancy_grid(static_grid_msg_);
static_grid_msg_.header.frame_id = "map";
static_grid_msg_.header.frame_id = get_tf_prefix() + "map";
static_grid_msg_.header.stamp = node->now();

static_occ_pub_->publish(static_grid_msg_);
Expand Down Expand Up @@ -155,7 +155,7 @@ SimpleMapsManager::update(NavState & nav_state)

auto fused = PointPerceptionsOpsView(perceptions)
.downsample(dynamic_map_.resolution())
.fuse("map")
.fuse(get_tf_prefix() + "map")
->filter({NAN, NAN, 0.1}, {NAN, NAN, NAN})
.as_points();

Expand All @@ -170,7 +170,7 @@ SimpleMapsManager::update(NavState & nav_state)
nav_state.set("map.dynamic", dynamic_map_);

dynamic_map_.to_occupancy_grid(dynamic_grid_msg_);
dynamic_grid_msg_.header.frame_id = "map";
dynamic_grid_msg_.header.frame_id = get_tf_prefix() + "map";
dynamic_grid_msg_.header.stamp = get_node()->now();
dynamic_occ_pub_->publish(dynamic_grid_msg_);
}
Expand Down
Loading