diff --git a/controllers/easynav_serest_controller/src/easynav_serest_controller/SerestController.cpp b/controllers/easynav_serest_controller/src/easynav_serest_controller/SerestController.cpp index 48a7f46..a082674 100644 --- a/controllers/easynav_serest_controller/src/easynav_serest_controller/SerestController.cpp +++ b/controllers/easynav_serest_controller/src/easynav_serest_controller/SerestController.cpp @@ -306,7 +306,7 @@ SerestController::closest_obstacle_distance( auto fused = PointPerceptionsOpsView(perceptions) .downsample(0.3) - .fuse(tf_info.robot_frame) + .fuse(tf_info.robot_footprint_frame) .filter({-dist_search_radius_, -dist_search_radius_, NAN}, {dist_search_radius_, dist_search_radius_, 2.0}) .collapse({NAN, NAN, 0.1}) @@ -370,7 +370,7 @@ SerestController::fetch_required_inputs( const auto & tf_info = RTTFBuffer::getInstance()->get_tf_info(); if (!nav_state.has("path") || !nav_state.has("robot_pose") || !nav_state.has("map.dynamic")) { - publish_stop(nav_state, tf_info.robot_frame); + publish_stop(nav_state, tf_info.robot_footprint_frame); return false; } diff --git a/controllers/easynav_simple_controller/tests/simple_controller_tests.cpp b/controllers/easynav_simple_controller/tests/simple_controller_tests.cpp index e783cd6..eff6b1e 100644 --- a/controllers/easynav_simple_controller/tests/simple_controller_tests.cpp +++ b/controllers/easynav_simple_controller/tests/simple_controller_tests.cpp @@ -148,6 +148,7 @@ TEST_F(AMCLLocalizerTest, SavemapServiceWorks) tf_info.map_frame = "world_map"; tf_info.odom_frame = "world_odom"; tf_info.robot_frame = "world_base"; + tf_info.robot_footprint_frame = "world_footprint_base"; easynav::RTTFBuffer::getInstance()->set_tf_info(tf_info); manager->initialize(node, "test_savemap"); diff --git a/controllers/easynav_vff_controller/src/easynav_vff_controller/VffController.cpp b/controllers/easynav_vff_controller/src/easynav_vff_controller/VffController.cpp index 04729e3..1030276 100644 --- a/controllers/easynav_vff_controller/src/easynav_vff_controller/VffController.cpp +++ b/controllers/easynav_vff_controller/src/easynav_vff_controller/VffController.cpp @@ -64,7 +64,7 @@ void VffController::on_initialize() // Initialize the odometry message cmd_vel_.header.stamp = node->now(); - cmd_vel_.header.frame_id = tf_info.robot_frame; + cmd_vel_.header.frame_id = tf_info.robot_footprint_frame; cmd_vel_.twist.linear.x = 0.0; cmd_vel_.twist.linear.y = 0.0; cmd_vel_.twist.linear.z = 0.0; @@ -262,14 +262,14 @@ void VffController::update_rt(NavState & nav_state) auto fused = PointPerceptionsOpsView(perceptions) .filter({-10.0, -10.0, -10.0}, {10.0, 10.0, 10.0}) - .fuse(tf_info.robot_frame) + .fuse(tf_info.robot_footprint_frame) .filter({obstacle_detection_x_min_, obstacle_detection_y_min_, obstacle_detection_z_min_}, {obstacle_detection_x_max_, obstacle_detection_y_max_, obstacle_detection_z_max_}) .as_points(); // Get VFF vectors - const VFFVectors & vff = get_vff(angle_error, fused, tf_info.robot_frame); + const VFFVectors & vff = get_vff(angle_error, fused, tf_info.robot_footprint_frame); // Use result vector to calculate output speed const auto & v = vff.result; diff --git a/localizers/easynav_costmap_localizer/src/easynav_costmap_localizer/AMCLLocalizer.cpp b/localizers/easynav_costmap_localizer/src/easynav_costmap_localizer/AMCLLocalizer.cpp index 5e2da2f..022892c 100644 --- a/localizers/easynav_costmap_localizer/src/easynav_costmap_localizer/AMCLLocalizer.cpp +++ b/localizers/easynav_costmap_localizer/src/easynav_costmap_localizer/AMCLLocalizer.cpp @@ -488,7 +488,7 @@ AMCLLocalizer::update_odom_from_tf() geometry_msgs::msg::TransformStamped tf_msg; try { tf_msg = RTTFBuffer::getInstance()->lookupTransform( - tf_info.odom_frame, tf_info.robot_frame, tf2::TimePointZero, + tf_info.odom_frame, tf_info.robot_footprint_frame, tf2::TimePointZero, tf2::durationFromSec(0.0)); } catch (const tf2::TransformException & ex) { RCLCPP_WARN(get_node()->get_logger(), "AMCLLocalizer::update: TF failed: %s", ex.what()); @@ -585,7 +585,7 @@ AMCLLocalizer::correct(NavState & nav_state) const auto & tf_info = RTTFBuffer::getInstance()->get_tf_info(); const auto & filtered = PointPerceptionsOpsView(perceptions) .downsample(map_static.getResolution()) - .fuse(tf_info.robot_frame) + .fuse(tf_info.robot_footprint_frame) .filter({NAN, NAN, 0.1}, {NAN, NAN, NAN}) .collapse({NAN, NAN, 0.1}) .downsample(map_static.getResolution()) @@ -824,7 +824,7 @@ AMCLLocalizer::get_pose() odom_msg.header.stamp = last_input_time_; const auto & tf_info = RTTFBuffer::getInstance()->get_tf_info(); odom_msg.header.frame_id = tf_info.map_frame; - odom_msg.child_frame_id = tf_info.robot_frame; + odom_msg.child_frame_id = tf_info.robot_footprint_frame; tf2::Transform est_pose = getEstimatedPose(); diff --git a/localizers/easynav_fusion_localizer/src/easynav_fusion_localizer/ukf_wrapper.cpp b/localizers/easynav_fusion_localizer/src/easynav_fusion_localizer/ukf_wrapper.cpp index ec2f87d..8bf14a0 100644 --- a/localizers/easynav_fusion_localizer/src/easynav_fusion_localizer/ukf_wrapper.cpp +++ b/localizers/easynav_fusion_localizer/src/easynav_fusion_localizer/ukf_wrapper.cpp @@ -893,7 +893,7 @@ void UkfWrapper::loadParams() map_frame_id_ = tf_info.map_frame; odom_frame_id_ = tf_info.odom_frame; - base_link_frame_id_ = tf_info.robot_frame; + base_link_frame_id_ = tf_info.robot_footprint_frame; // World frame comes from Easynav TFInfo configuration world_frame_id_ = tf_info.map_frame; diff --git a/localizers/easynav_gps_localizer/src/easynav_gps_localizer/GpsLocalizer.cpp b/localizers/easynav_gps_localizer/src/easynav_gps_localizer/GpsLocalizer.cpp index 3a852fd..694ceeb 100644 --- a/localizers/easynav_gps_localizer/src/easynav_gps_localizer/GpsLocalizer.cpp +++ b/localizers/easynav_gps_localizer/src/easynav_gps_localizer/GpsLocalizer.cpp @@ -35,7 +35,7 @@ void GpsLocalizer::on_initialize() odom_.header.stamp = get_node()->now(); const auto & tf_info = RTTFBuffer::getInstance()->get_tf_info(); odom_.header.frame_id = tf_info.map_frame; - odom_.child_frame_id = tf_info.robot_frame; + odom_.child_frame_id = tf_info.robot_footprint_frame; // Create subscriber to GPS data gps_subscriber_ = node->create_subscription( @@ -114,7 +114,7 @@ void GpsLocalizer::update(NavState & nav_state) odom_.header.stamp = gps_msg_.header.stamp; const auto & tf_info = RTTFBuffer::getInstance()->get_tf_info(); odom_.header.frame_id = tf_info.map_frame; - odom_.child_frame_id = tf_info.robot_frame; + odom_.child_frame_id = tf_info.robot_footprint_frame; odom_.pose.pose.position.x = utm_x - origin_utm_.x; odom_.pose.pose.position.y = utm_y - origin_utm_.y; diff --git a/localizers/easynav_navmap_localizer/src/easynav_navmap_localizer/AMCLLocalizer.cpp b/localizers/easynav_navmap_localizer/src/easynav_navmap_localizer/AMCLLocalizer.cpp index f6edaf8..e84fb9b 100644 --- a/localizers/easynav_navmap_localizer/src/easynav_navmap_localizer/AMCLLocalizer.cpp +++ b/localizers/easynav_navmap_localizer/src/easynav_navmap_localizer/AMCLLocalizer.cpp @@ -485,7 +485,7 @@ void AMCLLocalizer::update_odom_from_tf() geometry_msgs::msg::TransformStamped tf_msg; try { tf_msg = RTTFBuffer::getInstance()->lookupTransform( - tf_info.odom_frame, tf_info.robot_frame, tf2::TimePointZero, + tf_info.odom_frame, tf_info.robot_footprint_frame, tf2::TimePointZero, tf2::durationFromSec(0.0)); } catch (const tf2::TransformException & ex) { RCLCPP_WARN(get_node()->get_logger(), "TF failed: %s", ex.what()); @@ -747,7 +747,8 @@ void AMCLLocalizer::correct(NavState & nav_state) if (!T_bf_sensor_cache.count(b.frame_id)) { const auto & tf_info = RTTFBuffer::getInstance()->get_tf_info(); try { - T_bf_sensor_cache[b.frame_id] = lookup_bf_to_sensor(tf_info.robot_frame, b.frame_id); + T_bf_sensor_cache[b.frame_id] = lookup_bf_to_sensor(tf_info.robot_footprint_frame, + b.frame_id); // const tf2::Transform & t = T_bf_sensor_cache[b.frame_id]; @@ -1092,7 +1093,7 @@ AMCLLocalizer::get_pose() odom_msg.header.stamp = last_input_time_; const auto & tf_info = RTTFBuffer::getInstance()->get_tf_info(); odom_msg.header.frame_id = tf_info.map_frame; - odom_msg.child_frame_id = tf_info.robot_frame; + odom_msg.child_frame_id = tf_info.robot_footprint_frame; pose_ = getEstimatedPose(); tf2::Transform est_pose = pose_; diff --git a/localizers/easynav_simple_localizer/src/easynav_simple_localizer/AMCLLocalizer.cpp b/localizers/easynav_simple_localizer/src/easynav_simple_localizer/AMCLLocalizer.cpp index dffc223..791eed6 100644 --- a/localizers/easynav_simple_localizer/src/easynav_simple_localizer/AMCLLocalizer.cpp +++ b/localizers/easynav_simple_localizer/src/easynav_simple_localizer/AMCLLocalizer.cpp @@ -387,7 +387,7 @@ void AMCLLocalizer::correct(NavState & nav_state) const auto & tf_info = RTTFBuffer::getInstance()->get_tf_info(); const auto & filtered = PointPerceptionsOpsView(perceptions) .downsample(map_static.resolution()) - .fuse(tf_info.robot_frame) + .fuse(tf_info.robot_footprint_frame) .filter({NAN, NAN, 0.1}, {NAN, NAN, NAN}) .collapse({NAN, NAN, 0.1}) .downsample(map_static.resolution()) @@ -629,7 +629,7 @@ AMCLLocalizer::get_pose() odom_msg.header.stamp = last_input_time_; const auto & tf_info = RTTFBuffer::getInstance()->get_tf_info(); odom_msg.header.frame_id = tf_info.map_frame; - odom_msg.child_frame_id = tf_info.robot_frame; + odom_msg.child_frame_id = tf_info.robot_footprint_frame; tf2::Transform est_pose = getEstimatedPose(); diff --git a/localizers/easynav_simple_localizer/tests/simple_localizer_tests.cpp b/localizers/easynav_simple_localizer/tests/simple_localizer_tests.cpp index 0d48e66..fbb1ca1 100644 --- a/localizers/easynav_simple_localizer/tests/simple_localizer_tests.cpp +++ b/localizers/easynav_simple_localizer/tests/simple_localizer_tests.cpp @@ -108,6 +108,7 @@ TEST_F(AMCLLocalizerTest, IncomingOccupancyGridUpdatesMaps) tf_info.map_frame = "world_map"; tf_info.odom_frame = "world_odom"; tf_info.robot_frame = "world_base"; + tf_info.robot_footprint_frame = "world_footprint_base"; easynav::RTTFBuffer::getInstance()->set_tf_info(tf_info); manager->initialize(node, "test2"); diff --git a/maps_managers/easynav_simple_maps_manager/tests/simple_mapsmanager_tests.cpp b/maps_managers/easynav_simple_maps_manager/tests/simple_mapsmanager_tests.cpp index e543725..87d5e3d 100644 --- a/maps_managers/easynav_simple_maps_manager/tests/simple_mapsmanager_tests.cpp +++ b/maps_managers/easynav_simple_maps_manager/tests/simple_mapsmanager_tests.cpp @@ -70,6 +70,7 @@ TEST_F(SimpleMapsManagerTest, BasicDynamicUpdate) tf_info.map_frame = "world_map"; tf_info.odom_frame = "world_odom"; tf_info.robot_frame = "world_base"; + tf_info.robot_footprint_frame = "world_footprint_base"; easynav::RTTFBuffer::getInstance()->set_tf_info(tf_info); manager->initialize(node, "test"); @@ -134,6 +135,7 @@ TEST_F(SimpleMapsManagerTest, IncomingOccupancyGridUpdatesMaps) tf_info.map_frame = "world_map"; tf_info.odom_frame = "world_odom"; tf_info.robot_frame = "world_base"; + tf_info.robot_footprint_frame = "world_footprint_base"; easynav::RTTFBuffer::getInstance()->set_tf_info(tf_info); manager->initialize(node, "test2"); @@ -183,6 +185,7 @@ TEST_F(SimpleMapsManagerTest, SavemapServiceWorks) tf_info.map_frame = "world_map"; tf_info.odom_frame = "world_odom"; tf_info.robot_frame = "world_base"; + tf_info.robot_footprint_frame = "world_footprint_base"; easynav::RTTFBuffer::getInstance()->set_tf_info(tf_info); manager->initialize(node, "test_savemap");