feat: add angle restriction for arrived goal (autowarefoundation#765) (…

…#144)

* add angle restriction for arrvied goal

* fix typo

* fix for pre commit

Signed-off-by: wep21 <border_goldenmarket@yahoo.co.jp>

Co-authored-by: Hiroki OTA <hiroki.ota@tier4.jp>
Co-authored-by: Takeshi Miura <57553950+1222-takeshi@users.noreply.github.com>

system/autoware_state_monitor/include/autoware_state_monitor/state_machine.hpp

@@ -18,6 +18,8 @@
 #include "autoware_state_monitor/autoware_state.hpp"
 #include "autoware_state_monitor/config.hpp"
 #include "autoware_state_monitor/module_name.hpp"
+#include "autoware_utils/math/normalization.hpp"
+#include "autoware_utils/math/unit_conversion.hpp"
 
 #include <rclcpp/time.hpp>
 
@@ -29,6 +31,8 @@
 #include <geometry_msgs/msg/pose_stamped.hpp>
 #include <nav_msgs/msg/odometry.hpp>
 
+#include <tf2/utils.h>
+
 #include <deque>
 #include <string>
 #include <vector>
@@ -56,6 +60,7 @@ struct StateInput
 struct StateParam
 {
   double th_arrived_distance_m;
+  double th_arrived_angle;
   double th_stopped_time_sec;
   double th_stopped_velocity_mps;
 };

system/autoware_state_monitor/launch/autoware_state_monitor.launch.xml

@@ -19,6 +19,7 @@
   <!-- Parameter -->
   <arg name="update_rate" default="10.0" />
   <arg name="th_arrived_distance_m" default="1.0" />
+  <arg name="th_arrived_angle_deg" default="45.0" />
   <arg name="th_stopped_time_sec" default="1.0" />
   <arg name="th_stopped_velocity_mps" default="0.01" />
   <arg name="disengage_on_route" default="true" />

system/autoware_state_monitor/package.xml

@@ -13,6 +13,7 @@
   <depend>autoware_auto_planning_msgs</depend>
   <depend>autoware_auto_system_msgs</depend>
   <depend>autoware_auto_vehicle_msgs</depend>
+  <depend>autoware_utils</depend>
   <depend>diagnostic_updater</depend>
   <depend>fmt</depend>
   <depend>geometry_msgs</depend>

system/autoware_state_monitor/src/autoware_state_monitor_node/autoware_state_monitor_node.cpp

@@ -398,6 +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", autoware_utils::deg2rad(45.0));
   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);
 

system/autoware_state_monitor/src/autoware_state_monitor_node/state_machine.cpp

@@ -32,6 +32,16 @@ double calcDistance2d(const geometry_msgs::msg::Pose & p1, const geometry_msgs::
   return calcDistance2d(p1.position, p2.position);
 }
 
+bool isValidAngle(
+  const geometry_msgs::msg::Pose & current_pose, const geometry_msgs::msg::Pose & ref_pose,
+  const double th_angle_rad)
+{
+  const double yaw_curr = tf2::getYaw(current_pose.orientation);
+  const double yaw_ref = tf2::getYaw(ref_pose.orientation);
+  const double yaw_diff = autoware_utils::normalizeRadian(yaw_curr - yaw_ref);
+  return std::fabs(yaw_diff) < th_angle_rad;
+}
+
 bool isNearGoal(
   const geometry_msgs::msg::Pose & current_pose, const geometry_msgs::msg::Pose & goal_pose,
   const double th_dist)
@@ -175,12 +185,14 @@ 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);
   const auto is_stopped =
     isStopped(state_input_.odometry_buffer, state_param_.th_stopped_velocity_mps);
 
-  if (is_near_goal && is_stopped) {
+  if (is_valid_goal_angle && is_near_goal && is_stopped) {
     return true;
   }