Merge pull request #43 from fmrico/add_footprint_tf

Add a base_footprint frame in TFInfo

Author: fmrico

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;
   }
 

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");

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;

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();
 

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;
 

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<sensor_msgs::msg::NavSatFix>(
@@ -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;
 

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_;

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();
 

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");

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");