Merge branch 'main' into feat/add_enable_all_modules_auto_mode

Signed-off-by: kyoichi-sugahara <kyoichi.sugahara@tier4.jp>

perception/radar_tracks_msgs_converter/README.md

@@ -23,7 +23,7 @@ This package converts from [radar_msgs/msg/RadarTracks](https://github.com/ros-p
 - `new_frame_id` (string): The header frame of the output topic.
   - Default parameter is "base_link"
 - `use_twist_compensation` (bool): If the parameter is true, then the twist of the output objects' topic is compensated by ego vehicle motion.
-  - Default parameter is "false"
+  - Default parameter is "true"
 - `use_twist_yaw_compensation` (bool): If the parameter is true, then the ego motion compensation will also consider yaw motion of the ego vehicle.
   - Default parameter is "false"
 - `static_object_speed_threshold` (float): Specify the threshold for static object speed which determines the flag `is_stationary` [m/s].

perception/radar_tracks_msgs_converter/launch/radar_tracks_msgs_converter.launch.xml

@@ -4,7 +4,7 @@
   <arg name="output/radar_detected_objects" default="output/radar_detected_objects"/>
   <arg name="output/radar_tracked_objects" default="output/radar_tracked_objects"/>
   <arg name="update_rate_hz" default="20.0"/>
-  <arg name="use_twist_compensation" default="false"/>
+  <arg name="use_twist_compensation" default="true"/>
   <arg name="use_twist_yaw_compensation" default="false"/>
   <arg name="static_object_speed_threshold" default="1.0"/>
 

planning/behavior_path_goal_planner_module/src/goal_planner_module.cpp

@@ -458,13 +458,6 @@ bool GoalPlannerModule::isExecutionRequested() const
     return false;
   }
 
-  // if goal modification is not allowed
-  // 1) goal_pose is in current_lanes, plan path to the original fixed goal
-  // 2) goal_pose is NOT in current_lanes, do not execute goal_planner
-  if (!utils::isAllowedGoalModification(route_handler)) {
-    return goal_is_in_current_lanes;
-  }
-
   // if (A) or (B) is met execute pull over
   // (A) target lane is `road` and same to the current lanes
   // (B) target lane is `road_shoulder` and neighboring to the current lanes

planning/behavior_path_planner/src/behavior_path_planner_node.cpp

@@ -784,6 +784,7 @@ void BehaviorPathPlannerNode::onTrafficSignals(const TrafficSignalArray::ConstSh
 {
   std::lock_guard<std::mutex> lock(mutex_pd_);
 
+  planner_data_->traffic_light_id_map.clear();
   for (const auto & signal : msg->signals) {
     TrafficSignalStamped traffic_signal;
     traffic_signal.stamp = msg->stamp;

planning/behavior_velocity_planner/src/node.cpp

@@ -323,6 +323,11 @@ void BehaviorVelocityPlannerNode::onTrafficSignals(
 {
   std::lock_guard<std::mutex> lock(mutex_);
 
+  // clear previous observation
+  planner_data_.traffic_light_id_map_raw_.clear();
+  const auto traffic_light_id_map_last_observed_old =
+    planner_data_.traffic_light_id_map_last_observed_;
+  planner_data_.traffic_light_id_map_last_observed_.clear();
   for (const auto & signal : msg->signals) {
     TrafficSignalStamped traffic_signal;
     traffic_signal.stamp = msg->stamp;
@@ -334,9 +339,12 @@ void BehaviorVelocityPlannerNode::onTrafficSignals(
       });
     // if the observation is UNKNOWN and past observation is available, only update the timestamp
     // and keep the body of the info
-    if (
-      is_unknown_observation &&
-      planner_data_.traffic_light_id_map_last_observed_.count(signal.traffic_signal_id) == 1) {
+    const auto old_data = traffic_light_id_map_last_observed_old.find(signal.traffic_signal_id);
+    if (is_unknown_observation && old_data != traffic_light_id_map_last_observed_old.end()) {
+      // copy last observation
+      planner_data_.traffic_light_id_map_last_observed_[signal.traffic_signal_id] =
+        old_data->second;
+      // update timestamp
       planner_data_.traffic_light_id_map_last_observed_[signal.traffic_signal_id].stamp =
         msg->stamp;
     } else {