diff --git a/.github/thirdparty.repos b/.github/thirdparty.repos index c4abe867..71ff48cb 100644 --- a/.github/thirdparty.repos +++ b/.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 diff --git a/.github/workflows/kilted.yaml b/.github/workflows/kilted.yaml index 5eff233e..6d68281e 100644 --- a/.github/workflows/kilted.yaml +++ b/.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: | { diff --git a/.github/workflows/kilted_cron.yaml b/.github/workflows/kilted_cron.yaml index dc93850f..39a45694 100644 --- a/.github/workflows/kilted_cron.yaml +++ b/.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: | { 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 2cf55119..b0fb4e0e 100644 --- a/controllers/easynav_serest_controller/src/easynav_serest_controller/SerestController.cpp +++ b/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::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; } diff --git a/controllers/easynav_simple_controller/tests/simple_controller_tests.cpp b/controllers/easynav_simple_controller/tests/simple_controller_tests.cpp index eff6b1ea..4af0d5c2 100644 --- a/controllers/easynav_simple_controller/tests/simple_controller_tests.cpp +++ b/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"); 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 1030276c..04729e3a 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_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; 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 75f51b21..48bc71ea 100644 --- a/localizers/easynav_costmap_localizer/src/easynav_costmap_localizer/AMCLLocalizer.cpp +++ b/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(); 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 8bf14a01..ec2f87da 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_footprint_frame; + base_link_frame_id_ = tf_info.robot_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 694ceebf..3a852fd9 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_footprint_frame; + odom_.child_frame_id = tf_info.robot_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_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; 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 6bd2391c..c7615292 100644 --- a/localizers/easynav_navmap_localizer/src/easynav_navmap_localizer/AMCLLocalizer.cpp +++ b/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_; 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 30a62e70..693d006e 100644 --- a/localizers/easynav_simple_localizer/src/easynav_simple_localizer/AMCLLocalizer.cpp +++ b/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(); diff --git a/localizers/easynav_simple_localizer/tests/simple_localizer_tests.cpp b/localizers/easynav_simple_localizer/tests/simple_localizer_tests.cpp index fbb1ca18..731a256e 100644 --- a/localizers/easynav_simple_localizer/tests/simple_localizer_tests.cpp +++ b/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"); diff --git a/maps_managers/easynav_costmap_maps_manager/src/easynav_costmap_maps_manager/filters/ObstacleFilter.cpp b/maps_managers/easynav_costmap_maps_manager/src/easynav_costmap_maps_manager/filters/ObstacleFilter.cpp index 1dd019e9..8a9d3401 100644 --- a/maps_managers/easynav_costmap_maps_manager/src/easynav_costmap_maps_manager/filters/ObstacleFilter.cpp +++ b/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(); 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 f46ee0fe..5e5b1006 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 @@ -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");