Merge pull request #3 from autowarefoundation/tier4/proposal

chore: sync upstream

launch/tier4_localization_launch/launch/pose_estimator/pose_estimator.launch.xml

@@ -9,7 +9,6 @@
 
     <arg name="output_pose_topic" value="/localization/pose_estimator/pose"/>
     <arg name="output_pose_with_covariance_topic" value="/localization/pose_estimator/pose_with_covariance"/>
-    <arg name="output_diagnostics_topic" value="/localization/diagnostics"/>
 
     <arg name="param_file" value="$(find-pkg-share tier4_localization_launch)/config/ndt_scan_matcher.param.yaml" />
   </include>

localization/ndt_scan_matcher/launch/ndt_scan_matcher.launch.xml

@@ -8,7 +8,6 @@
 
   <arg name="output_pose_topic" default="ndt_pose" description="Estimated self position" />
   <arg name="output_pose_with_covariance_topic" default="ndt_pose_with_covariance" description="Estimated self position with covariance" />
-  <arg name="output_diagnostics_topic" default="/diagnostics" description="Diagnostic topic" />
 
 
   <arg name="node_name" default="ndt_scan_matcher" description="Use a different name for this node" />
@@ -21,7 +20,6 @@
 
     <remap from="ndt_pose" to="$(var output_pose_topic)" />
     <remap from="ndt_pose_with_covariance" to="$(var output_pose_with_covariance_topic)" />
-    <remap from="diagnostics" to="$(var output_diagnostics_topic)" />
 
     <param from="$(var param_file)" />
   </node>

localization/vehicle_velocity_converter/config/vehicle_velocity_converter.param.yaml

@@ -0,0 +1,12 @@
+/**:
+  ros__parameters:
+    frame_id: base_link
+    twist_covariance:
+      [
+        0.04, 0.0,     0.0,     0.0,     0.0,     0.0,
+        0.0,  10000.0, 0.0,     0.0,     0.0,     0.0,
+        0.0,  0.0,     10000.0, 0.0,     0.0,     0.0,
+        0.0,  0.0,     0.0,     10000.0, 0.0,     0.0,
+        0.0,  0.0,     0.0,     0.0,     10000.0, 0.0,
+        0.0,  0.0,     0.0,     0.0,     0.0,     0.01,
+      ]

localization/vehicle_velocity_converter/launch/vehicle_velocity_converter.launch.xml

@@ -1,7 +1,7 @@
 <launch>
   <arg name="input_vehicle_velocity_topic" default="velocity_status"/>
   <arg name="output_twist_with_covariance" default="twist_with_covariance"/>
-  <arg name="config_file" default="$(find-pkg-share vehicle_velocity_converter)/config/vehicle_velocity_converter.yaml"/>
+  <arg name="config_file" default="$(find-pkg-share vehicle_velocity_converter)/config/vehicle_velocity_converter.param.yaml"/>
 
   <node pkg="vehicle_velocity_converter" exec="vehicle_velocity_converter" name="vehicle_velocity_converter_node" output="screen">
     <param from="$(var config_file)"/>

planning/behavior_velocity_planner/src/scene_module/crosswalk/scene_walkway.cpp

@@ -13,6 +13,7 @@
 // limitations under the License.
 
 #include <scene_module/crosswalk/scene_walkway.hpp>
+#include <tier4_autoware_utils/trajectory/trajectory.hpp>
 #include <utilization/util.hpp>
 
 #include <cmath>
@@ -80,18 +81,21 @@ bool WalkwayModule::modifyPathVelocity(
     stop_factor.stop_factor_points.emplace_back(debug_data_.nearest_collision_point);
     planning_utils::appendStopReason(stop_factor, stop_reason);
 
-    // update state
-    const Point self_pose = {
-      planner_data_->current_pose.pose.position.x, planner_data_->current_pose.pose.position.y};
-    const Point stop_pose = {
-      debug_data_.first_stop_pose.position.x, debug_data_.first_stop_pose.position.y};
-    const double distance = bg::distance(stop_pose, self_pose);
+    // use arc length to identify if ego vehicle is in front of walkway stop or not.
+    const double signed_arc_dist_to_stop_point = tier4_autoware_utils::calcSignedArcLength(
+      path->points, planner_data_->current_pose.pose.position,
+      debug_data_.first_stop_pose.position);
     const double distance_threshold = 1.0;
     debug_data_.stop_judge_range = distance_threshold;
     if (
-      distance < distance_threshold &&
+      signed_arc_dist_to_stop_point < distance_threshold &&
       planner_data_->isVehicleStopped(planner_param_.stop_duration_sec)) {
+      // If ego vehicle is after walkway stop and stopped then move to stop state
       state_ = State::STOP;
+      if (signed_arc_dist_to_stop_point < -distance_threshold) {
+        RCLCPP_ERROR(
+          logger_, "Failed to stop near walkway but ego stopped. Change state to STOPPED");
+      }
     }
     return true;
   } else if (state_ == State::STOP) {

planning/behavior_velocity_planner/src/scene_module/intersection/scene_merge_from_private_road.cpp

@@ -120,13 +120,17 @@ bool MergeFromPrivateRoadModule::modifyPathVelocity(
       planning_utils::appendStopReason(stop_factor, stop_reason);
     }
 
-    const double distance =
-      planning_utils::calcDist2d(current_pose.pose, path->points.at(stop_line_idx).point.pose);
+    const double signed_arc_dist_to_stop_point = tier4_autoware_utils::calcSignedArcLength(
+      path->points, current_pose.pose.position, path->points.at(stop_line_idx).point.pose.position);
+
     constexpr double distance_threshold = 2.0;
     if (
-      distance < distance_threshold &&
+      signed_arc_dist_to_stop_point < distance_threshold &&
       planner_data_->isVehicleStopped(planner_param_.stop_duration_sec)) {
       state_machine_.setState(State::GO);
+      if (signed_arc_dist_to_stop_point < -distance_threshold) {
+        RCLCPP_ERROR(logger_, "Failed to stop near stop line but ego stopped. Change state to GO");
+      }
     }
 
     return true;

planning/behavior_velocity_planner/src/scene_module/no_stopping_area/scene_no_stopping_area.cpp

@@ -40,7 +40,7 @@ bool splineInterpolate(
   *output = input;
 
   if (input.points.size() <= 1) {
-    RCLCPP_WARN(logger, "Do not interpolate because path size is 1.");
+    RCLCPP_DEBUG(logger, "Do not interpolate because path size is 1.");
     return false;
   }
 

planning/behavior_velocity_planner/src/scene_module/occlusion_spot/occlusion_spot_utils.cpp

@@ -53,7 +53,7 @@ bool splineInterpolate(
   *output = input;
 
   if (input.points.size() <= 1) {
-    RCLCPP_WARN(logger, "Do not interpolate because path size is 1.");
+    RCLCPP_DEBUG(logger, "Do not interpolate because path size is 1.");
     return false;
   }
 

planning/behavior_velocity_planner/src/scene_module/stop_line/scene.cpp

@@ -13,6 +13,7 @@
 // limitations under the License.
 
 #include <scene_module/stop_line/scene.hpp>
+#include <tier4_autoware_utils/trajectory/trajectory.hpp>
 #include <utilization/util.hpp>
 
 #include <algorithm>
@@ -87,72 +88,6 @@ boost::optional<StopLineModule::SegmentIndexWithOffset> findBackwardOffsetSegmen
   return {};
 }
 
-StopLineModule::SegmentIndexWithOffset findNearestSegment(
-  const autoware_auto_planning_msgs::msg::PathWithLaneId & path,
-  const geometry_msgs::msg::Point & point)
-{
-  std::vector<double> distances;
-  distances.reserve(path.points.size());
-
-  std::transform(
-    path.points.cbegin(), path.points.cend(), std::back_inserter(distances),
-    [&point](const autoware_auto_planning_msgs::msg::PathPointWithLaneId & p) {
-      return planning_utils::calcDist2d(p.point.pose.position, point);
-    });
-
-  const auto min_itr = std::min_element(distances.cbegin(), distances.cend());
-  const auto min_idx = static_cast<size_t>(std::distance(distances.cbegin(), min_itr));
-
-  // Process boundary conditions
-  if (min_idx == 0 || min_idx == path.points.size() - 1) {
-    const size_t segment_idx = min_idx == 0 ? min_idx : min_idx - 1;
-
-    return StopLineModule::SegmentIndexWithOffset{
-      segment_idx, planning_utils::calcDist2d(path.points.at(segment_idx), point)};
-  }
-
-  // Process normal conditions(having previous and next points)
-  const auto & p_prev = path.points.at(min_idx - 1);
-  const auto d_prev = planning_utils::calcDist2d(p_prev, point);
-
-  const auto & p_next = path.points.at(min_idx + 1);
-  const auto d_next = planning_utils::calcDist2d(p_next, point);
-
-  const size_t segment_idx = (d_prev <= d_next) ? min_idx - 1 : min_idx;
-
-  return StopLineModule::SegmentIndexWithOffset{
-    segment_idx, planning_utils::calcDist2d(path.points.at(segment_idx), point)};
-}
-
-double calcSignedArcLength(
-  const autoware_auto_planning_msgs::msg::PathWithLaneId & path, const size_t & idx_front,
-  const size_t & idx_back)
-{
-  // If flipped, reverse index and take negative
-  if (idx_front > idx_back) {
-    return -calcSignedArcLength(path, idx_back, idx_front);
-  }
-
-  double sum_length = 0.0;
-  for (size_t i = idx_front; i < idx_back; ++i) {
-    const auto & p1 = path.points.at(i).point.pose.position;
-    const auto & p2 = path.points.at(i + 1).point.pose.position;
-    sum_length += planning_utils::calcDist2d(p1, p2);
-  }
-
-  return sum_length;
-}
-
-double calcSignedArcLength(
-  const autoware_auto_planning_msgs::msg::PathWithLaneId & path,
-  const geometry_msgs::msg::Point & p1, const geometry_msgs::msg::Point & p2)
-{
-  const auto seg_1 = findNearestSegment(path, p1);
-  const auto seg_2 = findNearestSegment(path, p2);
-
-  return -seg_1.offset + calcSignedArcLength(path, seg_1.index, seg_2.index) + seg_2.offset;
-}
-
 }  // namespace
 
 StopLineModule::StopLineModule(
@@ -282,19 +217,23 @@ bool StopLineModule::modifyPathVelocity(
   const auto stop_pose_with_index = calcStopPose(*path, offset_segment);
 
   // Calc dist to stop pose
-  const double signed_arc_dist_to_stop_point = calcSignedArcLength(
-    *path, planner_data_->current_pose.pose.position, stop_pose_with_index->pose.position);
+  const double signed_arc_dist_to_stop_point = tier4_autoware_utils::calcSignedArcLength(
+    path->points, planner_data_->current_pose.pose.position, stop_pose_with_index->pose.position);
 
   if (state_ == State::APPROACH) {
     // Insert stop pose
     *path = insertStopPose(*path, *stop_pose_with_index, stop_reason);
 
     // Move to stopped state if stopped
     if (
-      std::abs(signed_arc_dist_to_stop_point) < planner_param_.stop_check_dist &&
+      signed_arc_dist_to_stop_point < planner_param_.stop_check_dist &&
       planner_data_->isVehicleStopped(planner_param_.stop_duration_sec)) {
       RCLCPP_INFO(logger_, "APPROACH -> STOPPED");
       state_ = State::STOPPED;
+      if (signed_arc_dist_to_stop_point < -planner_param_.stop_check_dist) {
+        RCLCPP_ERROR(
+          logger_, "Failed to stop near stop line but ego stopped. Change state to STOPPED");
+      }
     }
   } else if (state_ == State::STOPPED) {
     // Change state after vehicle departure

simulator/simple_planning_simulator/launch/simple_planning_simulator.launch.py

@@ -55,6 +55,9 @@ def launch_setup(context, *args, **kwargs):
             vehicle_info_param,
             vehicle_characteristics_param,
             simulator_model_param,
+            {
+                "initial_engage_state": LaunchConfiguration("initial_engage_state"),
+            },
         ],
         remappings=[
             ('input/ackermann_control_command', '/control/command/control_cmd'),

simulator/simple_planning_simulator/src/simple_planning_simulator/simple_planning_simulator_core.cpp

@@ -76,12 +76,12 @@ namespace simple_planning_simulator
 SimplePlanningSimulator::SimplePlanningSimulator(const rclcpp::NodeOptions & options)
 : Node("simple_planning_simulator", options),
   tf_buffer_(get_clock()),
-  tf_listener_(tf_buffer_, std::shared_ptr<rclcpp::Node>(this, [](auto) {}), false),
-  current_engage_(false)
+  tf_listener_(tf_buffer_, std::shared_ptr<rclcpp::Node>(this, [](auto) {}), false)
 {
   simulated_frame_id_ = declare_parameter("simulated_frame_id", "base_link");
   origin_frame_id_ = declare_parameter("origin_frame_id", "odom");
   add_measurement_noise_ = declare_parameter("add_measurement_noise", false);
+  current_engage_ = declare_parameter<bool>("initial_engage_state");
 
   using rclcpp::QoS;
   using std::placeholders::_1;
@@ -242,7 +242,10 @@ void SimplePlanningSimulator::on_timer()
   // update vehicle dynamics
   {
     const float64_t dt = delta_time_.get_dt(get_clock()->now());
-    vehicle_model_ptr_->update(dt);
+
+    if (current_engage_) {
+      vehicle_model_ptr_->update(dt);
+    }
   }
 
   // set current state

system/ad_service_state_monitor/launch/ad_service_state_monitor.launch.xml

@@ -41,6 +41,7 @@
 
     <param name="update_rate" value="$(var update_rate)" />
     <param name="th_arrived_distance_m" value="$(var th_arrived_distance_m)" />
+    <param name="th_arrived_angle_deg" value="$(var th_arrived_angle_deg)" />
     <param name="th_stopped_time_sec" value="$(var th_stopped_time_sec)" />
     <param name="th_stopped_velocity_mps" value="$(var th_stopped_velocity_mps)" />
     <param name="disengage_on_route" value="$(var disengage_on_route)" />

system/ad_service_state_monitor/src/ad_service_state_monitor_node/ad_service_state_monitor_node.cpp

@@ -398,8 +398,8 @@ AutowareStateMonitorNode::AutowareStateMonitorNode()
 
   // Parameter for StateMachine
   state_param_.th_arrived_distance_m = this->declare_parameter("th_arrived_distance_m", 1.0);
-  state_param_.th_arrived_angle =
-    this->declare_parameter("th_arrived_angle_deg", tier4_autoware_utils::deg2rad(45.0));
+  const auto th_arrived_angle_deg = this->declare_parameter("th_arrived_angle_deg", 45.0);
+  state_param_.th_arrived_angle = tier4_autoware_utils::deg2rad(th_arrived_angle_deg);
   state_param_.th_stopped_time_sec = this->declare_parameter("th_stopped_time_sec", 1.0);
   state_param_.th_stopped_velocity_mps = this->declare_parameter("th_stopped_velocity_mps", 0.01);