From 86101edd278bd572124995866d7fecf534830d21 Mon Sep 17 00:00:00 2001 From: Kosuke Takeuchi Date: Fri, 8 Jul 2022 11:05:39 +0900 Subject: [PATCH] feat(ad_service_state_monitor): support modified goal (#1280) Signed-off-by: kosuke55 --- .../ad_service_state_monitor_node.hpp | 2 ++ .../ad_service_state_monitor/state_machine.hpp | 1 + .../launch/ad_service_state_monitor.launch.xml | 2 ++ .../ad_service_state_monitor_node.cpp | 9 +++++++++ .../state_machine.cpp | 16 ++++++++++++---- 5 files changed, 26 insertions(+), 4 deletions(-) diff --git a/system/ad_service_state_monitor/include/ad_service_state_monitor/ad_service_state_monitor_node.hpp b/system/ad_service_state_monitor/include/ad_service_state_monitor/ad_service_state_monitor_node.hpp index e3041b35fe3aa..108752fd798a4 100644 --- a/system/ad_service_state_monitor/include/ad_service_state_monitor/ad_service_state_monitor_node.hpp +++ b/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::SharedPtr sub_control_mode_; rclcpp::Subscription::SharedPtr sub_route_; + rclcpp::Subscription::SharedPtr sub_modified_goal_; rclcpp::Subscription::SharedPtr sub_map_; rclcpp::Subscription::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 diff --git a/system/ad_service_state_monitor/include/ad_service_state_monitor/state_machine.hpp b/system/ad_service_state_monitor/include/ad_service_state_monitor/state_machine.hpp index dbf4420b116e6..5d503d420c092 100644 --- a/system/ad_service_state_monitor/include/ad_service_state_monitor/state_machine.hpp +++ b/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_; diff --git a/system/ad_service_state_monitor/launch/ad_service_state_monitor.launch.xml b/system/ad_service_state_monitor/launch/ad_service_state_monitor.launch.xml index 5431f32acce19..a7cbf13c4a14c 100644 --- a/system/ad_service_state_monitor/launch/ad_service_state_monitor.launch.xml +++ b/system/ad_service_state_monitor/launch/ad_service_state_monitor.launch.xml @@ -4,6 +4,7 @@ + @@ -31,6 +32,7 @@ + diff --git a/system/ad_service_state_monitor/src/ad_service_state_monitor_node/ad_service_state_monitor_node.cpp b/system/ad_service_state_monitor/src/ad_service_state_monitor_node/ad_service_state_monitor_node.cpp index 3eb84d0a1c809..5cf6718cd2f20 100644 --- a/system/ad_service_state_monitor/src/ad_service_state_monitor_node/ad_service_state_monitor_node.cpp +++ b/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( "input/route", rclcpp::QoS{1}.transient_local(), std::bind(&AutowareStateMonitorNode::onRoute, this, _1), subscriber_option); + sub_modified_goal_ = this->create_subscription( + "input/modified_goal", 1, std::bind(&AutowareStateMonitorNode::onModifiedGoal, this, _1), + subscriber_option); sub_odom_ = this->create_subscription( "input/odometry", 100, std::bind(&AutowareStateMonitorNode::onOdometry, this, _1), subscriber_option); diff --git a/system/ad_service_state_monitor/src/ad_service_state_monitor_node/state_machine.cpp b/system/ad_service_state_monitor/src/ad_service_state_monitor_node/state_machine.cpp index 9437f4628621a..8eada23c7c7eb 100644 --- a/system/ad_service_state_monitor/src/ad_service_state_monitor_node/state_machine.cpp +++ b/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);