Merge branch 'main' into main

Author: isouf

.github/workflows/build-and-test-differential.yaml

@@ -24,7 +24,6 @@ jobs:
         rosdistro:
           - humble
         container-suffix:
-          - ""
           - -cuda
         include:
           - rosdistro: humble

control/joy_controller/README.md

@@ -4,6 +4,21 @@
 
 `joy_controller` is the package to convert a joy msg to autoware commands (e.g. steering wheel, shift, turn signal, engage) for a vehicle.
 
+## Usage
+
+### ROS 2 launch
+
+```bash
+# With default config (ds4)
+ros2 launch joy_controller joy_controller.launch.xml
+
+# Default config but select from the existing parameter files
+ros2 launch joy_controller joy_controller_param_selection.launch.xml joy_type:=ds4 # or g29, p65, xbox
+
+# Override the param file
+ros2 launch joy_controller joy_controller.launch.xml config_file:=/path/to/your/param.yaml
+```
+
 ## Input / Output
 
 ### Input topics

control/joy_controller/config/joy_controller.param.yaml control/joy_controller/config/joy_controller_ds4.param.yaml

@@ -1,6 +1,6 @@
 /**:
   ros__parameters:
-    joy_type: $(var joy_type)
+    joy_type: DS4
     update_rate: 10.0
     accel_ratio: 3.0
     brake_ratio: 5.0

control/joy_controller/config/joy_controller_g29.param.yaml

@@ -0,0 +1,16 @@
+/**:
+  ros__parameters:
+    joy_type: G29
+    update_rate: 10.0
+    accel_ratio: 3.0
+    brake_ratio: 5.0
+    steer_ratio: 0.5
+    steering_angle_velocity: 0.1
+    accel_sensitivity: 1.0
+    brake_sensitivity: 1.0
+    control_command:
+      raw_control: false
+      velocity_gain: 3.0
+      max_forward_velocity: 20.0
+      max_backward_velocity: 3.0
+      backward_accel_ratio: 1.0

control/joy_controller/config/joy_controller_p65.param.yaml

@@ -0,0 +1,16 @@
+/**:
+  ros__parameters:
+    joy_type: P65
+    update_rate: 10.0
+    accel_ratio: 3.0
+    brake_ratio: 5.0
+    steer_ratio: 0.5
+    steering_angle_velocity: 0.1
+    accel_sensitivity: 1.0
+    brake_sensitivity: 1.0
+    control_command:
+      raw_control: false
+      velocity_gain: 3.0
+      max_forward_velocity: 20.0
+      max_backward_velocity: 3.0
+      backward_accel_ratio: 1.0

control/joy_controller/config/joy_controller_xbox.param.yaml

@@ -0,0 +1,16 @@
+/**:
+  ros__parameters:
+    joy_type: XBOX
+    update_rate: 10.0
+    accel_ratio: 3.0
+    brake_ratio: 5.0
+    steer_ratio: 0.5
+    steering_angle_velocity: 0.1
+    accel_sensitivity: 1.0
+    brake_sensitivity: 1.0
+    control_command:
+      raw_control: false
+      velocity_gain: 3.0
+      max_forward_velocity: 20.0
+      max_backward_velocity: 3.0
+      backward_accel_ratio: 1.0

control/joy_controller/launch/joy_controller.launch.xml

@@ -1,5 +1,4 @@
 <launch>
-  <arg name="joy_type" default="DS4" description="options: DS4, G29, P65, XBOX"/>
   <arg name="external_cmd_source" default="remote" description="options: local, remote"/>
 
   <arg name="input_joy" default="/joy"/>
@@ -13,7 +12,7 @@
   <arg name="output_gate_mode" default="/control/gate_mode_cmd"/>
   <arg name="output_vehicle_engage" default="/vehicle/engage"/>
 
-  <arg name="config_file" default="$(find-pkg-share joy_controller)/config/joy_controller.param.yaml"/>
+  <arg name="config_file" default="$(find-pkg-share joy_controller)/config/joy_controller_ds4.param.yaml"/>
 
   <node pkg="joy_controller" exec="joy_controller" name="joy_controller" output="screen">
     <remap from="input/joy" to="$(var input_joy)"/>

control/joy_controller/launch/joy_controller_param_selection.launch.xml

@@ -0,0 +1,7 @@
+<launch>
+  <arg name="joy_type" default="ds4" description="options: ds4, g29, p65, xbox"/>
+
+  <include file="$(find-pkg-share joy_controller)/launch/joy_controller.launch.xml">
+    <arg name="config_file" value="$(find-pkg-share joy_controller)/config/joy_controller_$(var joy_type).param.yaml"/>
+  </include>
+</launch>

localization/ndt_scan_matcher/include/ndt_scan_matcher/map_update_module.hpp

@@ -55,7 +55,7 @@ class MapUpdateModule
   friend class NDTScanMatcher;
 
   // Update the specified NDT
-  void update_ndt(const geometry_msgs::msg::Point & position, NdtType & ndt);
+  bool update_ndt(const geometry_msgs::msg::Point & position, NdtType & ndt);
   void update_map(const geometry_msgs::msg::Point & position);
   [[nodiscard]] bool should_update_map(const geometry_msgs::msg::Point & position);
   void publish_partial_pcd_map();

localization/ndt_scan_matcher/src/map_update_module.cpp

@@ -70,6 +70,7 @@ void MapUpdateModule::update_map(const geometry_msgs::msg::Point & position)
   // lock and rebuild ndt_ptr_
   if (need_rebuild_) {
     ndt_ptr_mutex_->lock();
+
     auto param = ndt_ptr_->getParams();
     auto input_source = ndt_ptr_->getInputSource();
 
@@ -87,13 +88,20 @@ void MapUpdateModule::update_map(const geometry_msgs::msg::Point & position)
     // the main ndt_ptr_) overlap, the latency of updating/alignment reduces partly.
     // If the updating is done the main ndt_ptr_, either the update or the NDT
     // align will be blocked by the other.
-    update_ndt(position, *secondary_ndt_ptr_);
+    const bool updated = update_ndt(position, *secondary_ndt_ptr_);
+    if (!updated) {
+      last_update_position_ = position;
+      return;
+    }
 
     ndt_ptr_mutex_->lock();
+    auto dummy_ptr = ndt_ptr_;
     auto input_source = ndt_ptr_->getInputSource();
     ndt_ptr_ = secondary_ndt_ptr_;
     ndt_ptr_->setInputSource(input_source);
     ndt_ptr_mutex_->unlock();
+
+    dummy_ptr.reset();
   }
 
   secondary_ndt_ptr_.reset(new NdtType);
@@ -106,7 +114,7 @@ void MapUpdateModule::update_map(const geometry_msgs::msg::Point & position)
   publish_partial_pcd_map();
 }
 
-void MapUpdateModule::update_ndt(const geometry_msgs::msg::Point & position, NdtType & ndt)
+bool MapUpdateModule::update_ndt(const geometry_msgs::msg::Point & position, NdtType & ndt)
 {
   auto request = std::make_shared<autoware_map_msgs::srv::GetDifferentialPointCloudMap::Request>();
 
@@ -128,7 +136,7 @@ void MapUpdateModule::update_ndt(const geometry_msgs::msg::Point & position, Ndt
   while (status != std::future_status::ready) {
     RCLCPP_INFO(logger_, "waiting response");
     if (!rclcpp::ok()) {
-      return;
+      return false;  // No update
     }
     status = result.wait_for(std::chrono::seconds(1));
   }
@@ -140,7 +148,7 @@ void MapUpdateModule::update_ndt(const geometry_msgs::msg::Point & position, Ndt
     logger_, "Update map (Add: %lu, Remove: %lu)", maps_to_add.size(), map_ids_to_remove.size());
   if (maps_to_add.empty() && map_ids_to_remove.empty()) {
     RCLCPP_INFO(logger_, "Skip map update");
-    return;
+    return false;  // No update
   }
 
   const auto exe_start_time = std::chrono::system_clock::now();
@@ -170,6 +178,7 @@ void MapUpdateModule::update_ndt(const geometry_msgs::msg::Point & position, Ndt
     std::chrono::duration_cast<std::chrono::microseconds>(exe_end_time - exe_start_time).count();
   const auto exe_time = static_cast<double>(duration_micro_sec) / 1000.0;
   RCLCPP_INFO(logger_, "Time duration for creating new ndt_ptr: %lf [ms]", exe_time);
+  return true;  // Updated
 }
 
 void MapUpdateModule::publish_partial_pcd_map()

perception/probabilistic_occupancy_grid_map/src/pointcloud_based_occupancy_grid_map/pointcloud_based_occupancy_grid_map_node.cpp

@@ -170,7 +170,7 @@ void PointcloudBasedOccupancyGridMapNode::onPointcloudWithObstacleAndRaw(
   Pose gridmap_origin{};
   Pose scan_origin{};
   try {
-    robot_pose = utils::getPose(input_raw_msg->header, *tf2_, map_frame_);
+    robot_pose = utils::getPose(input_raw_msg->header.stamp, *tf2_, base_link_frame_, map_frame_);
     gridmap_origin =
       utils::getPose(input_raw_msg->header.stamp, *tf2_, gridmap_origin_frame_, map_frame_);
     scan_origin =

perception/radar_crossing_objects_noise_filter/CMakeLists.txt

@@ -17,9 +17,17 @@ rclcpp_components_register_node(radar_crossing_objects_noise_filter_node_compone
 
 # Tests
 if(BUILD_TESTING)
+  find_package(ament_cmake_ros REQUIRED)
   list(APPEND AMENT_LINT_AUTO_EXCLUDE ament_cmake_uncrustify)
   find_package(ament_lint_auto REQUIRED)
   ament_lint_auto_find_test_dependencies()
+
+  file(GLOB_RECURSE test_files test/**/*.cpp)
+  ament_add_ros_isolated_gtest(radar_crossing_objects_noise_filter ${test_files})
+
+  target_link_libraries(radar_crossing_objects_noise_filter
+    radar_crossing_objects_noise_filter_node_component
+  )
 endif()
 
 # Package

perception/radar_crossing_objects_noise_filter/include/radar_crossing_objects_noise_filter/radar_crossing_objects_noise_filter_node.hpp

@@ -58,6 +58,7 @@ class RadarCrossingObjectsNoiseFilterNode : public rclcpp::Node
   // Parameter
   NodeParam node_param_{};
 
+public:
   // Core
   bool isNoise(const DetectedObject & object);
 };

perception/radar_crossing_objects_noise_filter/test/test_radar_crossing_objects_filter.cpp

@@ -0,0 +1,26 @@
+// Copyright 2024 TierIV
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+//     http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+// See the License for the specific language governing permissions and
+// limitations under the License.
+
+#include <rclcpp/rclcpp.hpp>
+
+#include <gtest/gtest.h>
+
+int main(int argc, char * argv[])
+{
+  testing::InitGoogleTest(&argc, argv);
+  rclcpp::init(argc, argv);
+  bool result = RUN_ALL_TESTS();
+  rclcpp::shutdown();
+  return result;
+}