feat(ad_service_state_monitor): support modified goal (#1280)

Signed-off-by: kosuke55 <kosuke.tnp@gmail.com>

system/ad_service_state_monitor/include/ad_service_state_monitor/ad_service_state_monitor_node.hpp

@@ -75,6 +75,7 @@ class AutowareStateMonitorNode : public rclcpp::Node
   rclcpp::Subscription<autoware_auto_vehicle_msgs::msg::ControlModeReport>::SharedPtr
     sub_control_mode_;
   rclcpp::Subscription<autoware_auto_planning_msgs::msg::HADMapRoute>::SharedPtr sub_route_;
+  rclcpp::Subscription<geometry_msgs::msg::PoseStamped>::SharedPtr sub_modified_goal_;
   rclcpp::Subscription<autoware_auto_mapping_msgs::msg::HADMapBin>::SharedPtr sub_map_;
   rclcpp::Subscription<nav_msgs::msg::Odometry>::SharedPtr sub_odom_;
 
@@ -83,6 +84,7 @@ class AutowareStateMonitorNode : public rclcpp::Node
     const autoware_auto_vehicle_msgs::msg::ControlModeReport::ConstSharedPtr msg);
   void onMap(const autoware_auto_mapping_msgs::msg::HADMapBin::ConstSharedPtr msg);
   void onRoute(const autoware_auto_planning_msgs::msg::HADMapRoute::ConstSharedPtr msg);
+  void onModifiedGoal(const geometry_msgs::msg::PoseStamped::ConstSharedPtr msg);
   void onOdometry(const nav_msgs::msg::Odometry::ConstSharedPtr msg);
 
   // Topic Buffer

system/ad_service_state_monitor/include/ad_service_state_monitor/state_machine.hpp

@@ -45,6 +45,7 @@ struct StateInput
 
   geometry_msgs::msg::PoseStamped::ConstSharedPtr current_pose;
   geometry_msgs::msg::Pose::ConstSharedPtr goal_pose;
+  geometry_msgs::msg::PoseStamped::ConstSharedPtr modified_goal_pose;
 
   autoware_auto_vehicle_msgs::msg::Engage::ConstSharedPtr autoware_engage;
   autoware_auto_vehicle_msgs::msg::ControlModeReport::ConstSharedPtr control_mode_;

system/ad_service_state_monitor/launch/ad_service_state_monitor.launch.xml

@@ -4,6 +4,7 @@
   <arg name="input_control_mode" default="/vehicle/status/control_mode"/>
   <arg name="input_vector_map" default="/map/vector_map"/>
   <arg name="input_route" default="/planning/mission_planning/route"/>
+  <arg name="input_modified_goal" default="/planning/scenario_planning/modified_goal"/>
   <arg name="input_odometry" default="/localization/kinematic_state"/>
 
   <!-- Output -->
@@ -31,6 +32,7 @@
     <remap from="input/control_mode" to="$(var input_control_mode)"/>
     <remap from="input/vector_map" to="$(var input_vector_map)"/>
     <remap from="input/route" to="$(var input_route)"/>
+    <remap from="input/modified_goal" to="$(var input_modified_goal)"/>
     <remap from="input/odometry" to="$(var input_odometry)"/>
 
     <remap from="output/autoware_state" to="$(var output_autoware_state)"/>

system/ad_service_state_monitor/src/ad_service_state_monitor_node/ad_service_state_monitor_node.cpp

@@ -94,6 +94,12 @@ void AutowareStateMonitorNode::onVehicleControlMode(
   state_input_.control_mode_ = msg;
 }
 
+void AutowareStateMonitorNode::onModifiedGoal(
+  const geometry_msgs::msg::PoseStamped::ConstSharedPtr msg)
+{
+  state_input_.modified_goal_pose = msg;
+}
+
 void AutowareStateMonitorNode::onMap(
   const autoware_auto_mapping_msgs::msg::HADMapBin::ConstSharedPtr msg)
 {
@@ -465,6 +471,9 @@ AutowareStateMonitorNode::AutowareStateMonitorNode()
   sub_route_ = this->create_subscription<autoware_auto_planning_msgs::msg::HADMapRoute>(
     "input/route", rclcpp::QoS{1}.transient_local(),
     std::bind(&AutowareStateMonitorNode::onRoute, this, _1), subscriber_option);
+  sub_modified_goal_ = this->create_subscription<geometry_msgs::msg::PoseStamped>(
+    "input/modified_goal", 1, std::bind(&AutowareStateMonitorNode::onModifiedGoal, this, _1),
+    subscriber_option);
   sub_odom_ = this->create_subscription<nav_msgs::msg::Odometry>(
     "input/odometry", 100, std::bind(&AutowareStateMonitorNode::onOdometry, this, _1),
     subscriber_option);

system/ad_service_state_monitor/src/ad_service_state_monitor_node/state_machine.cpp

@@ -192,10 +192,18 @@ bool StateMachine::isOverridden() const { return !isEngaged(); }
 
 bool StateMachine::hasArrivedGoal() const
 {
-  const auto is_valid_goal_angle = isValidAngle(
-    state_input_.current_pose->pose, *state_input_.goal_pose, state_param_.th_arrived_angle);
-  const auto is_near_goal = isNearGoal(
-    state_input_.current_pose->pose, *state_input_.goal_pose, state_param_.th_arrived_distance_m);
+  geometry_msgs::msg::Pose goal_pose = *state_input_.goal_pose;
+  if (
+    state_input_.modified_goal_pose != nullptr &&
+    rclcpp::Time(state_input_.route->header.stamp).seconds() ==
+      rclcpp::Time(state_input_.modified_goal_pose->header.stamp).seconds()) {
+    goal_pose = state_input_.modified_goal_pose->pose;
+  }
+
+  const auto is_valid_goal_angle =
+    isValidAngle(state_input_.current_pose->pose, goal_pose, state_param_.th_arrived_angle);
+  const auto is_near_goal =
+    isNearGoal(state_input_.current_pose->pose, goal_pose, state_param_.th_arrived_distance_m);
   const auto is_stopped =
     isStopped(state_input_.odometry_buffer, state_param_.th_stopped_velocity_mps);