Merge pull request autowarefoundation#494 from tier4/sync-upstream

chore: sync upstream

common/motion_utils/src/resample/resample.cpp

@@ -188,21 +188,21 @@ autoware_auto_planning_msgs::msg::PathWithLaneId resamplePath(
     }
 
     const double distance_to_resampling_point = calcSignedArcLength(input_path.points, 0, i);
-    for (size_t i = 1; i < resampling_arclength.size(); ++i) {
+    for (size_t j = 1; j < resampling_arclength.size(); ++j) {
       if (
-        resampling_arclength.at(i - 1) <= distance_to_resampling_point &&
-        distance_to_resampling_point < resampling_arclength.at(i)) {
+        resampling_arclength.at(j - 1) <= distance_to_resampling_point &&
+        distance_to_resampling_point < resampling_arclength.at(j)) {
         const double dist_to_prev_point =
-          std::fabs(distance_to_resampling_point - resampling_arclength.at(i - 1));
+          std::fabs(distance_to_resampling_point - resampling_arclength.at(j - 1));
         const double dist_to_following_point =
-          std::fabs(resampling_arclength.at(i) - distance_to_resampling_point);
+          std::fabs(resampling_arclength.at(j) - distance_to_resampling_point);
         if (dist_to_prev_point < motion_utils::overlap_threshold) {
-          resampling_arclength.at(i - 1) = distance_to_resampling_point;
+          resampling_arclength.at(j - 1) = distance_to_resampling_point;
         } else if (dist_to_following_point < motion_utils::overlap_threshold) {
-          resampling_arclength.at(i) = distance_to_resampling_point;
+          resampling_arclength.at(j) = distance_to_resampling_point;
         } else {
           resampling_arclength.insert(
-            resampling_arclength.begin() + i, distance_to_resampling_point);
+            resampling_arclength.begin() + j, distance_to_resampling_point);
         }
         break;
       }

control/shift_decider/include/shift_decider/shift_decider.hpp

@@ -20,6 +20,7 @@
 #include <autoware_auto_control_msgs/msg/ackermann_control_command.hpp>
 #include <autoware_auto_system_msgs/msg/autoware_state.hpp>
 #include <autoware_auto_vehicle_msgs/msg/gear_command.hpp>
+#include <autoware_auto_vehicle_msgs/msg/gear_report.hpp>
 
 #include <memory>
 
@@ -32,6 +33,7 @@ class ShiftDecider : public rclcpp::Node
   void onTimer();
   void onControlCmd(autoware_auto_control_msgs::msg::AckermannControlCommand::SharedPtr msg);
   void onAutowareState(autoware_auto_system_msgs::msg::AutowareState::SharedPtr msg);
+  void onCurrentGear(autoware_auto_vehicle_msgs::msg::GearReport::SharedPtr msg);
   void updateCurrentShiftCmd();
   void initTimer(double period_s);
 
@@ -40,11 +42,15 @@ class ShiftDecider : public rclcpp::Node
     sub_control_cmd_;
   rclcpp::Subscription<autoware_auto_system_msgs::msg::AutowareState>::SharedPtr
     sub_autoware_state_;
+  rclcpp::Subscription<autoware_auto_vehicle_msgs::msg::GearReport>::SharedPtr sub_current_gear_;
+
   rclcpp::TimerBase::SharedPtr timer_;
 
   autoware_auto_control_msgs::msg::AckermannControlCommand::SharedPtr control_cmd_;
   autoware_auto_system_msgs::msg::AutowareState::SharedPtr autoware_state_;
   autoware_auto_vehicle_msgs::msg::GearCommand shift_cmd_;
+  autoware_auto_vehicle_msgs::msg::GearReport::SharedPtr current_gear_ptr_;
+
   bool park_on_goal_;
 };
 

control/shift_decider/src/shift_decider.cpp

@@ -38,6 +38,8 @@ ShiftDecider::ShiftDecider(const rclcpp::NodeOptions & node_options)
     "input/control_cmd", queue_size, std::bind(&ShiftDecider::onControlCmd, this, _1));
   sub_autoware_state_ = create_subscription<autoware_auto_system_msgs::msg::AutowareState>(
     "input/state", queue_size, std::bind(&ShiftDecider::onAutowareState, this, _1));
+  sub_current_gear_ = create_subscription<autoware_auto_vehicle_msgs::msg::GearReport>(
+    "input/current_gear", queue_size, std::bind(&ShiftDecider::onCurrentGear, this, _1));
 
   initTimer(0.1);
 }
@@ -53,9 +55,14 @@ void ShiftDecider::onAutowareState(autoware_auto_system_msgs::msg::AutowareState
   autoware_state_ = msg;
 }
 
+void ShiftDecider::onCurrentGear(autoware_auto_vehicle_msgs::msg::GearReport::SharedPtr msg)
+{
+  current_gear_ptr_ = msg;
+}
+
 void ShiftDecider::onTimer()
 {
-  if (!autoware_state_ || !control_cmd_) {
+  if (!autoware_state_ || !control_cmd_ || !current_gear_ptr_) {
     return;
   }
 
@@ -70,12 +77,20 @@ void ShiftDecider::updateCurrentShiftCmd()
 
   shift_cmd_.stamp = now();
   static constexpr double vel_threshold = 0.01;  // to prevent chattering
-  if (autoware_state_->state == AutowareState::ARRIVED_GOAL && park_on_goal_) {
-    shift_cmd_.command = GearCommand::PARK;
-  } else if (control_cmd_->longitudinal.speed > vel_threshold) {
-    shift_cmd_.command = GearCommand::DRIVE;
-  } else if (control_cmd_->longitudinal.speed < -vel_threshold) {
-    shift_cmd_.command = GearCommand::REVERSE;
+  if (autoware_state_->state == AutowareState::DRIVING) {
+    if (control_cmd_->longitudinal.speed > vel_threshold) {
+      shift_cmd_.command = GearCommand::DRIVE;
+    } else if (control_cmd_->longitudinal.speed < -vel_threshold) {
+      shift_cmd_.command = GearCommand::REVERSE;
+    } else {
+      shift_cmd_.command = current_gear_ptr_->report;
+    }
+  } else {
+    if (autoware_state_->state == AutowareState::ARRIVED_GOAL && park_on_goal_) {
+      shift_cmd_.command = GearCommand::PARK;
+    } else {
+      shift_cmd_.command = current_gear_ptr_->report;
+    }
   }
 }
 

control/vehicle_cmd_gate/launch/vehicle_cmd_gate.launch.xml

@@ -26,7 +26,6 @@
     <remap from="input/emergency/hazard_lights_cmd" to="/system/emergency/hazard_lights_cmd"/>
     <remap from="input/emergency/gear_cmd" to="/system/emergency/gear_cmd"/>
     <remap from="input/mrm_state" to="/system/fail_safe/mrm_state"/>
-    <remap from="input/gear_status" to="/vehicle/status/gear_status"/>
 
     <remap from="output/vehicle_cmd_emergency" to="/control/command/emergency_cmd"/>
     <remap from="output/control_cmd" to="/control/command/control_cmd"/>

control/vehicle_cmd_gate/src/vehicle_cmd_gate.cpp

@@ -88,8 +88,6 @@ VehicleCmdGate::VehicleCmdGate(const rclcpp::NodeOptions & node_options)
     [this](const OperationModeState::SharedPtr msg) { current_operation_mode_ = *msg; });
   mrm_state_sub_ = create_subscription<MrmState>(
     "input/mrm_state", 1, std::bind(&VehicleCmdGate::onMrmState, this, _1));
-  gear_status_sub_ = create_subscription<GearReport>(
-    "input/gear_status", 1, [this](GearReport::SharedPtr msg) { current_gear_ptr_ = msg; });
 
   // Subscriber for auto
   auto_control_cmd_sub_ = create_subscription<AckermannControlCommand>(
@@ -321,15 +319,6 @@ void VehicleCmdGate::onTimer()
     return;
   }
 
-  if (is_gate_mode_changed_) {
-    // If gate mode is external, is_engaged_ is always true
-    // While changing gate mode external to auto, the first is_engaged_ is always true for the first
-    // loop in this scope. So we need to wait for the second loop
-    // after gate mode is changed.
-    is_gate_mode_changed_ = false;
-    return;
-  }
-
   // Select commands
   TurnIndicatorsCommand turn_indicator;
   HazardLightsCommand hazard_light;
@@ -346,11 +335,6 @@ void VehicleCmdGate::onTimer()
 
       // Don't send turn signal when autoware is not engaged
       if (!is_engaged_) {
-        if (!current_gear_ptr_) {
-          gear.command = GearCommand::NONE;
-        } else {
-          gear.command = current_gear_ptr_.get()->report;
-        }
         turn_indicator.command = TurnIndicatorsCommand::NO_COMMAND;
         hazard_light.command = HazardLightsCommand::NO_COMMAND;
       }
@@ -577,7 +561,7 @@ void VehicleCmdGate::onGateMode(GateMode::ConstSharedPtr msg)
 {
   const auto prev_gate_mode = current_gate_mode_;
   current_gate_mode_ = *msg;
-  is_gate_mode_changed_ = true;
+
   if (current_gate_mode_.data != prev_gate_mode.data) {
     RCLCPP_INFO(
       get_logger(), "GateMode changed: %s -> %s", getGateModeName(prev_gate_mode.data),

control/vehicle_cmd_gate/src/vehicle_cmd_gate.hpp

@@ -27,7 +27,6 @@
 #include <autoware_auto_control_msgs/msg/ackermann_control_command.hpp>
 #include <autoware_auto_vehicle_msgs/msg/engage.hpp>
 #include <autoware_auto_vehicle_msgs/msg/gear_command.hpp>
-#include <autoware_auto_vehicle_msgs/msg/gear_report.hpp>
 #include <autoware_auto_vehicle_msgs/msg/hazard_lights_command.hpp>
 #include <autoware_auto_vehicle_msgs/msg/steering_report.hpp>
 #include <autoware_auto_vehicle_msgs/msg/turn_indicators_command.hpp>
@@ -52,7 +51,6 @@ using autoware_adapi_v1_msgs::msg::MrmState;
 using autoware_adapi_v1_msgs::msg::OperationModeState;
 using autoware_auto_control_msgs::msg::AckermannControlCommand;
 using autoware_auto_vehicle_msgs::msg::GearCommand;
-using autoware_auto_vehicle_msgs::msg::GearReport;
 using autoware_auto_vehicle_msgs::msg::HazardLightsCommand;
 using autoware_auto_vehicle_msgs::msg::SteeringReport;
 using autoware_auto_vehicle_msgs::msg::TurnIndicatorsCommand;
@@ -104,7 +102,6 @@ class VehicleCmdGate : public rclcpp::Node
   rclcpp::Subscription<GateMode>::SharedPtr gate_mode_sub_;
   rclcpp::Subscription<OperationModeState>::SharedPtr operation_mode_sub_;
   rclcpp::Subscription<MrmState>::SharedPtr mrm_state_sub_;
-  rclcpp::Subscription<GearReport>::SharedPtr gear_status_sub_;
   rclcpp::Subscription<Odometry>::SharedPtr kinematics_sub_;             // for filter
   rclcpp::Subscription<AccelWithCovarianceStamped>::SharedPtr acc_sub_;  // for filter
   rclcpp::Subscription<SteeringReport>::SharedPtr steer_sub_;            // for filter
@@ -116,11 +113,9 @@ class VehicleCmdGate : public rclcpp::Node
   bool is_engaged_;
   bool is_system_emergency_ = false;
   bool is_external_emergency_stop_ = false;
-  bool is_gate_mode_changed_ = false;
   double current_steer_ = 0;
   GateMode current_gate_mode_;
   MrmState current_mrm_state_;
-  GearReport::ConstSharedPtr current_gear_ptr_;
   Odometry current_kinematics_;
   double current_acceleration_ = 0.0;
 

launch/tier4_control_launch/launch/control.launch.py

@@ -121,6 +121,7 @@ def launch_setup(context, *args, **kwargs):
         remappings=[
             ("input/control_cmd", "/control/trajectory_follower/control_cmd"),
             ("input/state", "/autoware/state"),
+            ("input/current_gear", "/vehicle/status/gear_status"),
             ("output/gear_cmd", "/control/shift_decider/gear_cmd"),
         ],
         parameters=[
@@ -178,7 +179,6 @@ def launch_setup(context, *args, **kwargs):
             ("input/emergency/hazard_lights_cmd", "/system/emergency/hazard_lights_cmd"),
             ("input/emergency/gear_cmd", "/system/emergency/gear_cmd"),
             ("input/mrm_state", "/system/fail_safe/mrm_state"),
-            ("input/gear_status", "/vehicle/status/gear_status"),
             ("input/kinematics", "/localization/kinematic_state"),
             ("input/acceleration", "/localization/acceleration"),
             ("output/vehicle_cmd_emergency", "/control/command/emergency_cmd"),

perception/compare_map_segmentation/README.md

@@ -78,7 +78,7 @@ This filter is a combination of the distance_based_compare_map_filter and voxel_
 
 | Name                            | Type  | Description                                                                                                                             | Default value |
 | :------------------------------ | :---- | :-------------------------------------------------------------------------------------------------------------------------------------- | :------------ |
-| `use_dynamic_map_loading`       | bool  | map loading mode selection, `true` for dynamic map loading, `false` for static map loading                                              | true          |
+| `use_dynamic_map_loading`       | bool  | map loading mode selection, `true` for dynamic map loading, `false` for static map loading, recommended for no-split map pointcloud     | true          |
 | `distance_threshold`            | float | Threshold distance to compare input points with map points [m]                                                                          | 0.5           |
 | `map_update_distance_threshold` | float | Threshold of vehicle movement distance when map update is necessary (in dynamic map loading) [m]                                        | 10.0          |
 | `map_loader_radius`             | float | Radius of map need to be loaded (in dynamic map loading) [m]                                                                            | 150.0         |

perception/compare_map_segmentation/include/compare_map_segmentation/voxel_grid_map_loader.hpp

@@ -173,6 +173,9 @@ class VoxelGridDynamicMapLoader : public VoxelGridMapLoader
   double map_grid_size_x_ = -1.0;
   double map_grid_size_y_ = -1.0;
 
+  double origin_x_remainder_ = 0.0;
+  double origin_y_remainder_ = 0.0;
+
   /** \brief Array to hold loaded map grid positions for fast map grid searching.
    */
   std::vector<std::shared_ptr<MapGridVoxelInfo>> current_voxel_grid_array_;
@@ -236,9 +239,11 @@ class VoxelGridDynamicMapLoader : public VoxelGridMapLoader
   virtual inline void updateVoxelGridArray()
   {
     origin_x_ = std::floor((current_position_.value().x - map_loader_radius_) / map_grid_size_x_) *
-                map_grid_size_x_;
+                  map_grid_size_x_ +
+                origin_x_remainder_;
     origin_y_ = std::floor((current_position_.value().y - map_loader_radius_) / map_grid_size_y_) *
-                map_grid_size_y_;
+                  map_grid_size_y_ +
+                origin_y_remainder_;
 
     map_grids_x_ = static_cast<int>(
       std::ceil((current_position_.value().x + map_loader_radius_ - origin_x_) / map_grid_size_x_));
@@ -276,6 +281,10 @@ class VoxelGridDynamicMapLoader : public VoxelGridMapLoader
   {
     map_grid_size_x_ = map_cell_to_add.metadata.max_x - map_cell_to_add.metadata.min_x;
     map_grid_size_y_ = map_cell_to_add.metadata.max_y - map_cell_to_add.metadata.min_y;
+
+    origin_x_remainder_ = std::remainder(map_cell_to_add.metadata.min_x, map_grid_size_x_);
+    origin_y_remainder_ = std::remainder(map_cell_to_add.metadata.min_y, map_grid_size_y_);
+
     pcl::PointCloud<pcl::PointXYZ> map_cell_pc_tmp;
     pcl::fromROSMsg(map_cell_to_add.pointcloud, map_cell_pc_tmp);
 

planning/behavior_path_planner/config/lane_change/lane_change.param.yaml

@@ -8,13 +8,18 @@
       lane_change_finish_judge_buffer: 2.0      # [m]
 
       lane_changing_lateral_jerk: 0.5              # [m/s3]
-      lane_changing_lateral_acc: 0.315             # [m/s2]
-      lane_changing_lateral_acc_at_low_velocity: 0.15 # [m/s2]
       lateral_acc_switching_velocity: 4.0          #[m/s]
 
       minimum_lane_changing_velocity: 2.78        # [m/s]
       prediction_time_resolution: 0.5           # [s]
-      lane_change_sampling_num: 3
+      longitudinal_acceleration_sampling_num: 3
+      lateral_acceleration_sampling_num: 3
+
+      # lateral acceleration map
+      lateral_acceleration:
+        velocity: [0.0, 4.0, 10.0]
+        min_values: [0.15, 0.15, 0.15]
+        max_values: [0.5, 0.5, 0.5]
 
       # target object
       target_object:

planning/behavior_path_planner/docs/behavior_path_planner_lane_change_design.md

@@ -241,7 +241,6 @@ The following parameters are configurable in `lane_change.param.yaml`.
 | `backward_length_buffer_for_end_of_lane` | [m]    | double | The end of lane buffer to ensure ego vehicle has enough distance to start lane change   | 2.0           |
 | `lane_change_finish_judge_buffer`        | [m]    | double | The additional buffer used to confirm lane change process completion                    | 3.0           |
 | `lane_changing_lateral_jerk`             | [m/s3] | double | Lateral jerk value for lane change path generation                                      | 0.5           |
-| `lane_changing_lateral_acc`              | [m/s2] | double | Lateral acceleration value for lane change path generation                              | 0.5           |
 | `minimum_lane_changing_velocity`         | [m/s]  | double | Minimum speed during lane changing process.                                             | 2.78          |
 | `prediction_time_resolution`             | [s]    | double | Time resolution for object's path interpolation and collision check.                    | 0.5           |
 | `lane_change_sampling_num`               | [-]    | int    | Number of possible lane-changing trajectories that are being influenced by deceleration | 10            |

planning/behavior_path_planner/include/behavior_path_planner/parameters.hpp

@@ -15,9 +15,11 @@
 #ifndef BEHAVIOR_PATH_PLANNER__PARAMETERS_HPP_
 #define BEHAVIOR_PATH_PLANNER__PARAMETERS_HPP_
 
+#include <interpolation/linear_interpolation.hpp>
 #include <vehicle_info_util/vehicle_info_util.hpp>
 
 #include <string>
+#include <utility>
 #include <vector>
 
 struct ModuleConfigParameters
@@ -29,6 +31,47 @@ struct ModuleConfigParameters
   uint8_t max_module_size{0};
 };
 
+struct LateralAccelerationMap
+{
+  std::vector<double> base_vel;
+  std::vector<double> base_min_acc;
+  std::vector<double> base_max_acc;
+
+  void add(const double velocity, const double min_acc, const double max_acc)
+  {
+    if (base_vel.size() != base_min_acc.size() || base_vel.size() != base_max_acc.size()) {
+      return;
+    }
+
+    size_t idx = 0;
+    for (size_t i = 0; i < base_vel.size(); ++i) {
+      if (velocity < base_vel.at(i)) {
+        break;
+      }
+      idx = i + 1;
+    }
+
+    base_vel.insert(base_vel.begin() + idx, velocity);
+    base_min_acc.insert(base_min_acc.begin() + idx, min_acc);
+    base_max_acc.insert(base_max_acc.begin() + idx, max_acc);
+  }
+
+  std::pair<double, double> find(const double velocity) const
+  {
+    if (!base_vel.empty() && velocity < base_vel.front()) {
+      return std::make_pair(base_min_acc.front(), base_max_acc.front());
+    }
+    if (!base_vel.empty() && velocity > base_vel.back()) {
+      return std::make_pair(base_min_acc.back(), base_max_acc.back());
+    }
+
+    const double min_acc = interpolation::lerp(base_vel, base_min_acc, velocity);
+    const double max_acc = interpolation::lerp(base_vel, base_max_acc, velocity);
+
+    return std::make_pair(min_acc, max_acc);
+  }
+};
+
 struct BehaviorPathPlannerParameters
 {
   bool verbose;
@@ -56,12 +99,11 @@ struct BehaviorPathPlannerParameters
 
   // lane change parameters
   double lane_changing_lateral_jerk{0.5};
-  double lane_changing_lateral_acc{0.315};
-  double lane_changing_lateral_acc_at_low_velocity{0.15};
   double lateral_acc_switching_velocity{0.4};
   double minimum_lane_changing_velocity{5.6};
   double lane_change_prepare_duration{4.0};
   double minimum_prepare_length;
+  LateralAccelerationMap lane_change_lat_acc_map;
 
   double minimum_pull_over_length;
   double minimum_pull_out_length;

planning/behavior_path_planner/include/behavior_path_planner/utils/lane_change/lane_change_module_data.hpp

@@ -21,6 +21,7 @@
 
 #include <memory>
 #include <string>
+#include <utility>
 #include <vector>
 
 namespace behavior_path_planner
@@ -31,7 +32,8 @@ struct LaneChangeParameters
   double backward_lane_length{200.0};
   double lane_change_finish_judge_buffer{3.0};
   double prediction_time_resolution{0.5};
-  int lane_change_sampling_num{10};
+  int longitudinal_acc_sampling_num{10};
+  int lateral_acc_sampling_num{10};
 
   // collision check
   bool enable_prepare_segment_collision_check{true};