Merge pull request #40 from fmrico/set_robot_frame

Use TFInfo to unify TF specifications

Author: fmrico

common/easynav_simple_common/src/easynav_simple_common/SimpleMap.cpp

@@ -296,7 +296,8 @@ SimpleMap::downsample_factor(int factor) const
         }
       }
 
-      new_map->at(x, y) = (occupied_count > (factor * factor / 2));
+      // Mark cell as occupied if any source cell in the block is occupied
+      new_map->at(x, y) = (occupied_count > 0);
     }
   }
 

common/easynav_simple_common/tests/simple_maps_tests.cpp

@@ -172,7 +172,7 @@ TEST_F(SimpleMapTest, DownsampleIntegerFactor)
   EXPECT_TRUE(downsampled->at(0, 0));
   EXPECT_FALSE(downsampled->at(1, 0));
   EXPECT_FALSE(downsampled->at(0, 1));
-  EXPECT_FALSE(downsampled->at(1, 1));
+  EXPECT_TRUE(downsampled->at(1, 1));
 }
 
 /// \brief Downsampling to new resolution
@@ -195,9 +195,9 @@ TEST_F(SimpleMapTest, DownsampleToResolution)
   EXPECT_DOUBLE_EQ(downsampled->origin_x(), -1.0);
   EXPECT_DOUBLE_EQ(downsampled->origin_y(), -1.0);
 
-  EXPECT_FALSE(downsampled->at(0, 0));
-  EXPECT_FALSE(downsampled->at(1, 1));
-  EXPECT_FALSE(downsampled->at(2, 2));
+  EXPECT_TRUE(downsampled->at(0, 0));
+  EXPECT_TRUE(downsampled->at(1, 1));
+  EXPECT_TRUE(downsampled->at(2, 2));
 }
 
 /// \brief Downsample that results in cropped edges

controllers/easynav_mpc_controller/src/easynav_mpc_controller/MPCController.cpp

@@ -23,6 +23,8 @@
 #include "easynav_mpc_controller/MPCController.hpp"
 #include "easynav_system/GoalManager.hpp"
 
+#include "easynav_common/RTTFBuffer.hpp"
+
 namespace easynav
 {
 
@@ -201,9 +203,11 @@ MPCController::update_rt(NavState & nav_state)
   const auto & last_pose = path.poses[local_horizon].pose.position;
 
   const auto & perceptions = nav_state.get<PointPerceptions>("points");
+  const auto & tf_info = RTTFBuffer::getInstance()->get_tf_info();
+
   const auto & filtered = PointPerceptionsOpsView(perceptions)
     .filter({-2.0, -0.35, -1.0}, {0.0, 0.35, 1.0})
-    .fuse("map")
+    .fuse(tf_info.map_frame)
     .filter({NAN, NAN, 0.1}, {NAN, NAN, NAN})
     .collapse({NAN, NAN, 0.1})
     .downsample(0.1)

controllers/easynav_mppi_controller/src/easynav_mppi_controller/MPPIController.cpp

@@ -24,6 +24,8 @@
 
 #include "easynav_mppi_controller/MPPIController.hpp"
 #include "easynav_common/types/PointPerception.hpp"
+#include "easynav_common/RTTFBuffer.hpp"
+
 #include "easynav_system/GoalManager.hpp"
 
 #include "nav_msgs/msg/odometry.hpp"
@@ -78,14 +80,15 @@ void MPPIController::publish_mppi_markers(
   const std::vector<std::vector<std::pair<double, double>>> & all_trajs,
   const std::vector<std::pair<double, double>> & best_traj)
 {
+  const auto & tf_info = RTTFBuffer::getInstance()->get_tf_info();
   visualization_msgs::msg::MarkerArray candidates;
   visualization_msgs::msg::MarkerArray optimal;
   int id = 0;
 
   // Candidates in blue
   for (const auto & traj : all_trajs) {
     visualization_msgs::msg::Marker marker;
-    marker.header.frame_id = "map";
+    marker.header.frame_id = tf_info.map_frame;
     marker.header.stamp = rclcpp::Clock().now();
     marker.ns = "mppi_candidates";
     marker.id = id++;
@@ -110,7 +113,7 @@ void MPPIController::publish_mppi_markers(
 
   // Best trajectory in red
   visualization_msgs::msg::Marker best_marker;
-  best_marker.header.frame_id = "map";
+  best_marker.header.frame_id = tf_info.map_frame;
   best_marker.header.stamp = rclcpp::Clock().now();
   best_marker.ns = "mppi_optimal_path";
   best_marker.id = id++;
@@ -188,10 +191,10 @@ MPPIController::update_rt(NavState & nav_state)
 
   const auto & pose = nav_state.get<nav_msgs::msg::Odometry>("robot_pose").pose.pose;
   const auto & perceptions = nav_state.get<PointPerceptions>("points");
-
+  const auto & tf_info = RTTFBuffer::getInstance()->get_tf_info();
   const auto & filtered = PointPerceptionsOpsView(perceptions)
     .filter({-1.0, -1.0, -1.0}, {1.0, 1.0, 1.0})
-    .fuse("map")
+    .fuse(tf_info.map_frame)
     .filter({NAN, NAN, 0.1}, {NAN, NAN, NAN})
     .collapse({NAN, NAN, 0.1})
     .downsample(0.1)

controllers/easynav_serest_controller/src/easynav_serest_controller/SerestController.cpp

@@ -29,6 +29,7 @@
 #include "tf2_geometry_msgs/tf2_geometry_msgs.hpp"
 
 #include "easynav_common/types/PointPerception.hpp"
+#include "easynav_common/RTTFBuffer.hpp"
 
 #include "easynav_serest_controller/SerestController.hpp"
 #include "pluginlib/class_list_macros.hpp"
@@ -304,9 +305,11 @@ SerestController::closest_obstacle_distance(
   if (!nav_state.has("points")) {return std::numeric_limits<double>::infinity();}
 
   const auto & perceptions = nav_state.get<PointPerceptions>("points");
+  const auto & tf_info = RTTFBuffer::getInstance()->get_tf_info();
+
   auto fused = PointPerceptionsOpsView(perceptions)
     .downsample(0.3)
-    .fuse(get_tf_prefix() + "base_link")
+    .fuse(tf_info.robot_frame)
     .filter({-dist_search_radius_, -dist_search_radius_, NAN},
       {dist_search_radius_, dist_search_radius_, 2.0})
     .collapse({NAN, NAN, 0.1})
@@ -367,8 +370,10 @@ SerestController::fetch_required_inputs(
   nav_msgs::msg::Path & path,
   nav_msgs::msg::Odometry & odom)
 {
+  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, "base_link");
+    publish_stop(nav_state, tf_info.robot_frame);
     return false;
   }
 

controllers/easynav_simple_controller/tests/simple_controller_tests.cpp

@@ -144,6 +144,12 @@ TEST_F(AMCLLocalizerTest, SavemapServiceWorks)
 {
   auto node = std::make_shared<rclcpp_lifecycle::LifecycleNode>("test_savemap_node");
   auto manager = std::make_shared<easynav::SimpleController>();
+  easynav::TFInfo tf_info;
+  tf_info.map_frame = "world_map";
+  tf_info.odom_frame = "world_odom";
+  tf_info.robot_frame = "world_base";
+  easynav::RTTFBuffer::getInstance()->set_tf_info(tf_info);
+
   manager->initialize(node, "test_savemap");
 
   auto static_map = std::make_shared<easynav::SimpleMap>();

controllers/easynav_vff_controller/src/easynav_vff_controller/VffController.cpp

@@ -62,9 +62,11 @@ std::expected<void, std::string> VffController::on_initialize()
   node->get_parameter<double>(plugin_name + ".max_speed", max_speed_);
   node->get_parameter<double>(plugin_name + ".max_angular_speed", max_angular_speed_);
 
+  const auto & tf_info = RTTFBuffer::getInstance()->get_tf_info();
+
   // Initialize the odometry message
   cmd_vel_.header.stamp = node->now();
-  cmd_vel_.header.frame_id = get_tf_prefix() + "base_link";
+  cmd_vel_.header.frame_id = tf_info.robot_frame;
   cmd_vel_.twist.linear.x = 0.0;
   cmd_vel_.twist.linear.y = 0.0;
   cmd_vel_.twist.linear.z = 0.0;
@@ -213,9 +215,10 @@ void VffController::update_rt(NavState & nav_state)
   if (!nav_state.has("robot_pose")) {return;}
 
   const auto & all_goals = nav_state.get<nav_msgs::msg::Goals>("goals");
+  const auto & tf_info = RTTFBuffer::getInstance()->get_tf_info();
 
   if (all_goals.goals.empty()) {
-    cmd_vel_.header.frame_id = get_tf_prefix() + "map";
+    cmd_vel_.header.frame_id = tf_info.map_frame;
     cmd_vel_.header.stamp = get_node()->now();
     cmd_vel_.twist.linear.x = 0.0;
     cmd_vel_.twist.angular.z = 0.0;
@@ -259,17 +262,18 @@ void VffController::update_rt(NavState & nav_state)
 
     const auto & perceptions = nav_state.get<PointPerceptions>("points");
 
+    const auto & tf_info = RTTFBuffer::getInstance()->get_tf_info();
     auto fused =
       PointPerceptionsOpsView(perceptions)
       .filter({-10.0, -10.0, -10.0}, {10.0, 10.0, 10.0})
-      .fuse(get_tf_prefix() + "base_link")
+      .fuse(tf_info.robot_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, get_tf_prefix() + "base_link");
+    const VFFVectors & vff = get_vff(angle_error, fused, tf_info.robot_frame);
 
     // Use result vector to calculate output speed
     const auto & v = vff.result;

localizers/easynav_costmap_localizer/include/easynav_costmap_localizer/AMCLLocalizer.hpp

@@ -222,6 +222,9 @@ class AMCLLocalizer : public LocalizerMethodBase
   /// Time interval (in seconds) after which the particles should be reseeded.
   double reseed_time_;
 
+  /// Timestamp of the last input message (odometry or initial pose).
+  rclcpp::Time last_input_time_;
+
   /// Timestamp of the last reseed event.
   rclcpp::Time last_reseed_;
 };

localizers/easynav_costmap_localizer/src/easynav_costmap_localizer/AMCLLocalizer.cpp

@@ -322,6 +322,7 @@ AMCLLocalizer::odom_callback(nav_msgs::msg::Odometry::UniquePtr msg)
   if (compute_odom_from_tf_) {return;}
 
   tf2::fromMsg(msg->pose.pose, odom_);
+  last_input_time_ = msg->header.stamp;
 
   if (!initialized_odom_) {
     last_odom_ = odom_;
@@ -338,9 +339,9 @@ AMCLLocalizer::init_pose_callback(
   }
 
   auto logger = get_node()->get_logger();
-
+  const auto & tf_info = RTTFBuffer::getInstance()->get_tf_info();
   // Check expected frame
-  const std::string expected_frame = get_tf_prefix() + std::string("map");
+  const std::string expected_frame = tf_info.map_frame;
   if (!msg->header.frame_id.empty() && msg->header.frame_id != expected_frame) {
     RCLCPP_WARN(
       logger,
@@ -485,10 +486,13 @@ AMCLLocalizer::init_pose_callback(
 void
 AMCLLocalizer::update_odom_from_tf()
 {
+  const auto & tf_info = RTTFBuffer::getInstance()->get_tf_info();
+
   geometry_msgs::msg::TransformStamped tf_msg;
   try {
     tf_msg = RTTFBuffer::getInstance()->lookupTransform(
-      "odom", "base_footprint", tf2::TimePointZero, tf2::durationFromSec(0.0));
+      tf_info.odom_frame, tf_info.robot_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());
     return;
@@ -499,8 +503,11 @@ AMCLLocalizer::update_odom_from_tf()
 
   last_odom_ = odom_;
   odom_ = tf_odom;
+  last_input_time_ = tf_msg.header.stamp;
 
   initialized_odom_ = true;
+
+
 }
 
 void
@@ -578,9 +585,10 @@ AMCLLocalizer::correct(NavState & nav_state)
 
   const auto & map_static = nav_state.get<Costmap2D>("map.static");
 
+  const auto & tf_info = RTTFBuffer::getInstance()->get_tf_info();
   const auto & filtered = PointPerceptionsOpsView(perceptions)
     .downsample(map_static.getResolution())
-    .fuse(get_tf_prefix() + "base_footprint")
+    .fuse(tf_info.robot_frame)
     .filter({NAN, NAN, 0.1}, {NAN, NAN, NAN})
     .collapse({NAN, NAN, 0.1})
     .downsample(map_static.getResolution())
@@ -716,9 +724,10 @@ void
 AMCLLocalizer::publishTF(const tf2::Transform & map2bf)
 {
   geometry_msgs::msg::TransformStamped tf_msg;
-  tf_msg.header.stamp = get_node()->now();
-  tf_msg.header.frame_id = get_tf_prefix() + "map";
-  tf_msg.child_frame_id = get_tf_prefix() + "odom";
+  tf_msg.header.stamp = last_input_time_;
+  const auto & tf_info = RTTFBuffer::getInstance()->get_tf_info();
+  tf_msg.header.frame_id = tf_info.map_frame;
+  tf_msg.child_frame_id = tf_info.odom_frame;
   tf_msg.transform = tf2::toMsg(map2bf);
 
   RTTFBuffer::getInstance()->setTransform(tf_msg, "easynav", false);
@@ -728,10 +737,11 @@ AMCLLocalizer::publishTF(const tf2::Transform & map2bf)
 void
 AMCLLocalizer::publishParticles()
 {
-  geometry_msgs::msg::PoseArray array_msg;
-  array_msg.header.stamp = get_node()->now();
-  array_msg.header.frame_id = get_tf_prefix() + "map";
+  const auto & tf_info = RTTFBuffer::getInstance()->get_tf_info();
 
+  geometry_msgs::msg::PoseArray array_msg;
+  array_msg.header.stamp = last_input_time_;
+  array_msg.header.frame_id = tf_info.map_frame;
   array_msg.poses.reserve(particles_.size());
   for (const auto & p : particles_) {
     geometry_msgs::msg::Pose pose_msg;
@@ -789,9 +799,10 @@ AMCLLocalizer::publishEstimatedPose(const tf2::Transform & est_pose)
   tf2::Matrix3x3 cov = computeCovariance(particles_, 0, N_top, mean);
   double yaw_variance = computeYawVariance(particles_, 0, N_top);
 
+  const auto & tf_info = RTTFBuffer::getInstance()->get_tf_info();
   geometry_msgs::msg::PoseWithCovarianceStamped msg;
-  msg.header.stamp = get_node()->now();
-  msg.header.frame_id = get_tf_prefix() + "map";
+  msg.header.stamp = last_input_time_;
+  msg.header.frame_id = tf_info.map_frame;
 
   msg.pose.pose.position.x = mean.x();
   msg.pose.pose.position.y = mean.y();
@@ -813,9 +824,10 @@ AMCLLocalizer::get_pose()
 {
   nav_msgs::msg::Odometry odom_msg;
 
-  odom_msg.header.stamp = get_node()->now();
-  odom_msg.header.frame_id = get_tf_prefix() + "map";
-  odom_msg.child_frame_id = get_tf_prefix() + "base_footprint";
+  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;
 
   tf2::Transform est_pose = getEstimatedPose();
 

localizers/easynav_fusion_localizer/include/easynav_fusion_localizer/FusionLocalizer.hpp

@@ -55,10 +55,6 @@ class FusionLocalizer : public easynav::LocalizerMethodBase
   int n_imu_sensors_{0};
   int n_gps_sensors_{0};
 
-  std::string base_link_frame_id_;
-  std::string odom_frame_id_;
-  std::string world_frame_id_;
-
   geometry_msgs::msg::PoseWithCovarianceStamped
   navsatfix_to_pose(const sensor_msgs::msg::NavSatFix & navsat_msg);