Kilted

.github/thirdparty.repos

@@ -2,12 +2,12 @@ repositories:
   ThirdParty/EasyNavigation:
     type: git
     url: https://github.com/EasyNavigation/EasyNavigation.git
-    version: rolling
+    version: kilted
   ThirdParty/yaets:
     type: git
     url: https://github.com/fmrico/yaets.git
-    version: rolling
+    version: kilted
   ThirdParty/NavMap:
     type: git
     url: https://github.com/EasyNavigation/NavMap.git
-    version: rolling
+    version: kilted

.github/workflows/kilted.yaml

@@ -22,9 +22,8 @@ jobs:
       - name: build and test
         uses: ros-tooling/action-ros-ci@0.4.5
         with:
-          package-name: easynav_common easynav_controller easynav_core easynav_interfaces easynav_localizer easynav_maps_manager easynav_planner easynav_sensors easynav_support_py easynav_system easynav_tools easynav_bonxai_maps_manager easynav_costmap_common easynav_costmap_localizer easynav_costmap_maps_manager easynav_costmap_planner easynav_gps_localizer easynav_mppi_controller easynav_navmap_localizer easynav_navmap_maps_manager easynav_navmap_planner easynav_serest_controller easynav_simple_common easynav_simple_controller easynav_simple_localizer easynav_simple_maps_manager easynav_simple_planner easynav_vff_controller
+          package-name: easynav_bonxai_maps_manager easynav_costmap_common easynav_costmap_localizer easynav_costmap_maps_manager easynav_costmap_planner easynav_gps_localizer easynav_mppi_controller easynav_navmap_localizer easynav_navmap_maps_manager easynav_navmap_planner easynav_serest_controller easynav_simple_common easynav_simple_controller easynav_simple_localizer easynav_simple_maps_manager easynav_simple_planner easynav_vff_controller
           target-ros2-distro: kilted
-          vcs-repo-file-url: ${GITHUB_WORKSPACE}/.github/thirdparty.repos
           skip-tests: false
           colcon-defaults: |
             {

.github/workflows/kilted_cron.yaml

@@ -18,10 +18,8 @@ jobs:
       - name: build and test
         uses: ros-tooling/action-ros-ci@0.4.5
         with:
-          package-name: easynav_common easynav_controller easynav_core easynav_interfaces easynav_localizer easynav_maps_manager easynav_planner easynav_sensors easynav_support_py easynav_system easynav_tools easynav_bonxai_maps_manager easynav_costmap_common easynav_costmap_localizer easynav_costmap_maps_manager easynav_costmap_planner easynav_gps_localizer easynav_mppi_controller easynav_navmap_localizer easynav_navmap_maps_manager easynav_navmap_planner easynav_serest_controller easynav_simple_common easynav_simple_controller easynav_simple_localizer easynav_simple_maps_manager easynav_simple_planner easynav_vff_controller
+          package-name: easynav_bonxai_maps_manager easynav_costmap_common easynav_costmap_localizer easynav_costmap_maps_manager easynav_costmap_planner easynav_gps_localizer easynav_mppi_controller easynav_navmap_localizer easynav_navmap_maps_manager easynav_navmap_planner easynav_serest_controller easynav_simple_common easynav_simple_controller easynav_simple_localizer easynav_simple_maps_manager easynav_simple_planner easynav_vff_controller
           target-ros2-distro: kilted
-          ref: kilted
-          vcs-repo-file-url: ${GITHUB_WORKSPACE}/.github/thirdparty.repos
           skip-tests: false
           colcon-defaults: |
             {

controllers/easynav_serest_controller/src/easynav_serest_controller/SerestController.cpp

@@ -306,15 +306,28 @@ SerestController::closest_obstacle_distance(
 
   auto view = PointPerceptionsOpsView(perceptions);
   view.downsample(0.3)
-  .fuse(tf_info.robot_footprint_frame)
+  .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})
   .downsample(0.3);
   const auto & fused = view.as_points();
 
-  if (last_input_ts_ < view.get_latest_stamp()) {
-    last_input_ts_ = view.get_latest_stamp();
+  auto latest_time = [](const PointPerceptions & perceptions){
+      rclcpp::Time latest_stamp;
+      bool inited = false;
+
+      for (const auto & perception : perceptions) {
+        if (!inited || perception->stamp > latest_stamp) {
+          latest_stamp = perception->stamp;
+          inited = true;
+        }
+      }
+      return latest_stamp;
+    };
+
+  if (last_input_ts_ < latest_time(perceptions)) {
+    last_input_ts_ = latest_time(perceptions);
   }
 
   double min_dist = std::numeric_limits<double>::infinity();
@@ -374,7 +387,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_footprint_frame);
+    publish_stop(nav_state, tf_info.robot_frame);
     return false;
   }
 

controllers/easynav_simple_controller/tests/simple_controller_tests.cpp

@@ -148,7 +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";
+  tf_info.robot_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_footprint_frame;
+  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;
@@ -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_footprint_frame)
+      .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, tf_info.robot_footprint_frame);
+    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/src/easynav_costmap_localizer/AMCLLocalizer.cpp

@@ -490,7 +490,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_footprint_frame, tf2::TimePointZero,
+      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());
@@ -588,7 +588,7 @@ AMCLLocalizer::correct(NavState & nav_state)
 
   auto view = PointPerceptionsOpsView(perceptions);
   view.downsample(map_static.getResolution())
-  .fuse(tf_info.robot_footprint_frame)
+  .fuse(tf_info.robot_frame)
   .filter({NAN, NAN, 0.1}, {NAN, NAN, NAN})
   .collapse({NAN, NAN, 0.1})
   .downsample(map_static.getResolution());
@@ -599,7 +599,20 @@ AMCLLocalizer::correct(NavState & nav_state)
     return;
   }
 
-  last_input_time_ = view.get_latest_stamp();
+  auto latest_time = [](const PointPerceptions & perceptions){
+      rclcpp::Time latest_stamp;
+      bool inited = false;
+
+      for (const auto & perception : perceptions) {
+        if (!inited || perception->stamp > latest_stamp) {
+          latest_stamp = perception->stamp;
+          inited = true;
+        }
+      }
+      return latest_stamp;
+    };
+
+  last_input_time_ = latest_time(perceptions);
 
   for (auto & particle : particles_) {
     int hits = 0;
@@ -829,7 +842,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_footprint_frame;
+  odom_msg.child_frame_id = tf_info.robot_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_footprint_frame;
+  base_link_frame_id_ = tf_info.robot_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_footprint_frame;
+  odom_.child_frame_id = tf_info.robot_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_footprint_frame;
+  odom_.child_frame_id = tf_info.robot_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

@@ -486,7 +486,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_footprint_frame, tf2::TimePointZero,
+     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(), "TF failed: %s", ex.what());
@@ -748,7 +748,7 @@ 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_footprint_frame,
+        T_bf_sensor_cache[b.frame_id] = lookup_bf_to_sensor(tf_info.robot_frame,
               b.frame_id);
 
         // const tf2::Transform & t = T_bf_sensor_cache[b.frame_id];
@@ -1094,7 +1094,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_footprint_frame;
+  odom_msg.child_frame_id = tf_info.robot_frame;
 
   pose_ = getEstimatedPose();
   tf2::Transform est_pose = pose_;

localizers/easynav_simple_localizer/src/easynav_simple_localizer/AMCLLocalizer.cpp

@@ -388,7 +388,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_footprint_frame)
+    .fuse(tf_info.robot_frame)
     .filter({NAN, NAN, 0.1}, {NAN, NAN, NAN})
     .collapse({NAN, NAN, 0.1})
     .downsample(map_static.resolution())
@@ -630,7 +630,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_footprint_frame;
+  odom_msg.child_frame_id = tf_info.robot_frame;
 
   tf2::Transform est_pose = getEstimatedPose();
 

localizers/easynav_simple_localizer/tests/simple_localizer_tests.cpp

@@ -108,7 +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";
+  tf_info.robot_frame = "world_footprint_base";
   easynav::RTTFBuffer::getInstance()->set_tf_info(tf_info);
 
   manager->initialize(node, "test2");

maps_managers/easynav_costmap_maps_manager/src/easynav_costmap_maps_manager/filters/ObstacleFilter.cpp

@@ -59,7 +59,7 @@ ObstacleFilter::update(NavState & nav_state)
 
   auto view = PointPerceptionsOpsView(perceptions);
   view.downsample(dynamic_map.getResolution())
-  .fuse(tf_info.map_frame, stamp, false)
+  .fuse(tf_info.map_frame)
   .filter({NAN, NAN, 0.1}, {NAN, NAN, NAN});
 
   const auto & fused = view.as_points();

maps_managers/easynav_simple_maps_manager/tests/simple_mapsmanager_tests.cpp

@@ -58,13 +58,13 @@ 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";
+  tf_info.robot_frame = "world_footprint_base";
   easynav::RTTFBuffer::getInstance()->set_tf_info(tf_info);
 
   manager->initialize(node, "test");
 
   auto tf_buffer = easynav::RTTFBuffer::getInstance(node->get_clock());
-  tf2_ros::TransformListener tf_listener(*tf_buffer, *node, true);
+  tf2_ros::TransformListener tf_listener(*tf_buffer, node, true);
 
   easynav::SimpleMap static_map;
   static_map.initialize(30, 30, 0.1, -1.5, -1.5, 0.0);
@@ -123,7 +123,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";
+  tf_info.robot_frame = "world_footprint_base";
   easynav::RTTFBuffer::getInstance()->set_tf_info(tf_info);
 
   manager->initialize(node, "test2");
@@ -173,7 +173,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";
+  tf_info.robot_frame = "world_footprint_base";
   easynav::RTTFBuffer::getInstance()->set_tf_info(tf_info);
 
   manager->initialize(node, "test_savemap");