diff --git a/simulator/simple_planning_simulator/include/simple_planning_simulator/simple_planning_simulator_core.hpp b/simulator/simple_planning_simulator/include/simple_planning_simulator/simple_planning_simulator_core.hpp index e25160bd86f9c..fd37ad1a24b0b 100644 --- a/simulator/simple_planning_simulator/include/simple_planning_simulator/simple_planning_simulator_core.hpp +++ b/simulator/simple_planning_simulator/include/simple_planning_simulator/simple_planning_simulator_core.hpp @@ -38,6 +38,7 @@ #include "autoware_auto_planning_msgs/msg/trajectory.hpp" #include "autoware_auto_vehicle_msgs/msg/steering_report.hpp" #include "autoware_auto_vehicle_msgs/msg/control_mode_report.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/turn_indicators_command.hpp" @@ -67,6 +68,7 @@ using autoware_auto_control_msgs::msg::AckermannControlCommand; using autoware_auto_geometry_msgs::msg::Complex32; using autoware_auto_planning_msgs::msg::Trajectory; using autoware_auto_vehicle_msgs::msg::ControlModeReport; +using autoware_auto_vehicle_msgs::msg::Engage; using autoware_auto_vehicle_msgs::msg::GearCommand; using autoware_auto_vehicle_msgs::msg::GearReport; using autoware_auto_vehicle_msgs::msg::TurnIndicatorsCommand; @@ -140,6 +142,7 @@ class PLANNING_SIMULATOR_PUBLIC SimplePlanningSimulator : public rclcpp::Node rclcpp::Subscription::SharedPtr sub_ackermann_cmd_; rclcpp::Subscription::SharedPtr sub_init_pose_; rclcpp::Subscription::SharedPtr sub_trajectory_; + rclcpp::Subscription::SharedPtr sub_engage_; rclcpp::CallbackGroup::SharedPtr group_api_service_; tier4_api_utils::Service::SharedPtr srv_set_pose_; @@ -165,6 +168,7 @@ class PLANNING_SIMULATOR_PUBLIC SimplePlanningSimulator : public rclcpp::Node TurnIndicatorsCommand::ConstSharedPtr current_turn_indicators_cmd_ptr_; HazardLightsCommand::ConstSharedPtr current_hazard_lights_cmd_ptr_; Trajectory::ConstSharedPtr current_trajectory_ptr_; + bool current_engage_; /* frame_id */ std::string simulated_frame_id_; //!< @brief simulated vehicle frame id @@ -239,6 +243,11 @@ class PLANNING_SIMULATOR_PUBLIC SimplePlanningSimulator : public rclcpp::Node */ void on_trajectory(const Trajectory::ConstSharedPtr msg); + /** + * @brief subscribe autoware engage + */ + void on_engage(const Engage::ConstSharedPtr msg); + /** * @brief get z-position from trajectory * @param [in] x current x-position diff --git a/simulator/simple_planning_simulator/launch/simple_planning_simulator.launch.py b/simulator/simple_planning_simulator/launch/simple_planning_simulator.launch.py index 01aa87bf7c61a..528a903670ea1 100644 --- a/simulator/simple_planning_simulator/launch/simple_planning_simulator.launch.py +++ b/simulator/simple_planning_simulator/launch/simple_planning_simulator.launch.py @@ -62,6 +62,7 @@ def launch_setup(context, *args, **kwargs): ('input/turn_indicators_command', '/control/command/turn_indicators_cmd'), ('input/hazard_lights_command', '/control/command/hazard_lights_cmd'), ('input/trajectory', '/planning/scenario_planning/trajectory'), + ('input/engage', '/vehicle/engage'), ('output/twist', '/vehicle/status/velocity_status'), ('output/odometry', '/localization/kinematic_state'), ('output/steering', '/vehicle/status/steering_status'), diff --git a/simulator/simple_planning_simulator/src/simple_planning_simulator/simple_planning_simulator_core.cpp b/simulator/simple_planning_simulator/src/simple_planning_simulator/simple_planning_simulator_core.cpp index 32bbcbe8fcad9..1aaec1adb0c73 100644 --- a/simulator/simple_planning_simulator/src/simple_planning_simulator/simple_planning_simulator_core.cpp +++ b/simulator/simple_planning_simulator/src/simple_planning_simulator/simple_planning_simulator_core.cpp @@ -76,7 +76,8 @@ 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(this, [](auto) {}), false) + tf_listener_(tf_buffer_, std::shared_ptr(this, [](auto) {}), false), + current_engage_(false) { simulated_frame_id_ = declare_parameter("simulated_frame_id", "base_link"); origin_frame_id_ = declare_parameter("origin_frame_id", "odom"); @@ -101,6 +102,8 @@ SimplePlanningSimulator::SimplePlanningSimulator(const rclcpp::NodeOptions & opt std::bind(&SimplePlanningSimulator::on_hazard_lights_cmd, this, _1)); sub_trajectory_ = create_subscription( "input/trajectory", QoS{1}, std::bind(&SimplePlanningSimulator::on_trajectory, this, _1)); + sub_engage_ = create_subscription( + "input/engage", rclcpp::QoS{1}, std::bind(&SimplePlanningSimulator::on_engage, this, _1)); pub_control_mode_report_ = create_publisher("output/control_mode_report", QoS{1}); @@ -357,6 +360,11 @@ void SimplePlanningSimulator::on_trajectory(const Trajectory::ConstSharedPtr msg current_trajectory_ptr_ = msg; } +void SimplePlanningSimulator::on_engage(const Engage::ConstSharedPtr msg) +{ + current_engage_ = msg->engage; +} + void SimplePlanningSimulator::add_measurement_noise( Odometry & odom, VelocityReport & vel, SteeringReport & steer) const { @@ -489,7 +497,11 @@ void SimplePlanningSimulator::publish_control_mode_report() { ControlModeReport msg; msg.stamp = get_clock()->now(); - msg.mode = ControlModeReport::AUTONOMOUS; + if (current_engage_) { + msg.mode = ControlModeReport::AUTONOMOUS; + } else { + msg.mode = ControlModeReport::MANUAL; + } pub_control_mode_report_->publish(msg); }