diff --git a/control/operation_mode_transition_manager/include/operation_mode_transition_manager/data.hpp b/control/operation_mode_transition_manager/include/operation_mode_transition_manager/data.hpp index 037b6d537b593..af78430618650 100644 --- a/control/operation_mode_transition_manager/include/operation_mode_transition_manager/data.hpp +++ b/control/operation_mode_transition_manager/include/operation_mode_transition_manager/data.hpp @@ -23,13 +23,11 @@ #include #include #include -#include +#include #include #include #include #include -#include -#include #include @@ -41,12 +39,11 @@ using nav_msgs::msg::Odometry; using autoware_auto_control_msgs::msg::AckermannControlCommand; using autoware_auto_planning_msgs::msg::Trajectory; using autoware_auto_vehicle_msgs::msg::ControlModeReport; +using autoware_auto_vehicle_msgs::srv::ControlModeCommand; using operation_mode_transition_manager::msg::OperationModeTransitionManagerDebug; using tier4_system_msgs::msg::IsAutonomousAvailable; using tier4_system_msgs::msg::OperationMode; using tier4_system_msgs::srv::OperationModeRequest; -using tier4_vehicle_msgs::msg::ControlMode; -using tier4_vehicle_msgs::srv::ControlModeRequest; enum class State { STOP = 0, diff --git a/control/operation_mode_transition_manager/include/operation_mode_transition_manager/operation_mode_transition_manager.hpp b/control/operation_mode_transition_manager/include/operation_mode_transition_manager/operation_mode_transition_manager.hpp index 2bda5b300ec55..64134a23f7297 100644 --- a/control/operation_mode_transition_manager/include/operation_mode_transition_manager/operation_mode_transition_manager.hpp +++ b/control/operation_mode_transition_manager/include/operation_mode_transition_manager/operation_mode_transition_manager.hpp @@ -23,12 +23,11 @@ #include #include +#include #include #include #include #include -#include -#include #include #include diff --git a/control/operation_mode_transition_manager/include/operation_mode_transition_manager/state.hpp b/control/operation_mode_transition_manager/include/operation_mode_transition_manager/state.hpp index 2db2e91a891e0..414d281dd5388 100644 --- a/control/operation_mode_transition_manager/include/operation_mode_transition_manager/state.hpp +++ b/control/operation_mode_transition_manager/include/operation_mode_transition_manager/state.hpp @@ -19,10 +19,10 @@ #include #include +#include #include #include #include -#include #include @@ -41,7 +41,7 @@ class EngageStateBase void setParam(const StableCheckParam & param) { stable_check_param_ = param; } protected: - rclcpp::Client::SharedPtr srv_mode_change_client_; + rclcpp::Client::SharedPtr srv_mode_change_client_; rclcpp::Logger logger_; rclcpp::Clock::SharedPtr clock_; diff --git a/control/operation_mode_transition_manager/src/state.cpp b/control/operation_mode_transition_manager/src/state.cpp index 9a38c45c35c0d..f98079636d491 100644 --- a/control/operation_mode_transition_manager/src/state.cpp +++ b/control/operation_mode_transition_manager/src/state.cpp @@ -31,7 +31,7 @@ EngageStateBase::EngageStateBase(const State state, rclcpp::Node * node) : logger_(node->get_logger()), clock_(node->get_clock()), state_(state) { // TODO(Horibe): move to manager. - srv_mode_change_client_ = node->create_client("control_mode_request"); + srv_mode_change_client_ = node->create_client("control_mode_request"); } State EngageStateBase::defaultUpdateOnManual() @@ -57,11 +57,11 @@ bool EngageStateBase::sendAutonomousModeRequest() { bool success = true; - auto request = std::make_shared(); - request->mode.header.stamp = clock_->now(); - request->mode.data = ControlMode::AUTO; + auto request = std::make_shared(); + request->stamp = clock_->now(); + request->mode = ControlModeCommand::Request::AUTONOMOUS; - const auto callback = [&](rclcpp::Client::SharedFuture future) { + const auto callback = [&](rclcpp::Client::SharedFuture future) { success = future.get()->success; if (!success) { RCLCPP_WARN(logger_, "Autonomous mode change was rejected."); 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 f61359e59c3af..9bbaabf61d68f 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 @@ -24,6 +24,7 @@ #include "autoware_auto_control_msgs/msg/ackermann_control_command.hpp" #include "autoware_auto_geometry_msgs/msg/complex32.hpp" #include "autoware_auto_planning_msgs/msg/trajectory.hpp" +#include "autoware_auto_vehicle_msgs/msg/control_mode_command.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" @@ -35,6 +36,7 @@ #include "autoware_auto_vehicle_msgs/msg/turn_indicators_report.hpp" #include "autoware_auto_vehicle_msgs/msg/vehicle_control_command.hpp" #include "autoware_auto_vehicle_msgs/msg/velocity_report.hpp" +#include "autoware_auto_vehicle_msgs/srv/control_mode_command.hpp" #include "geometry_msgs/msg/accel_with_covariance_stamped.hpp" #include "geometry_msgs/msg/pose.hpp" #include "geometry_msgs/msg/pose_stamped.hpp" @@ -43,8 +45,6 @@ #include "geometry_msgs/msg/twist.hpp" #include "nav_msgs/msg/odometry.hpp" #include "tier4_external_api_msgs/srv/initialize_pose.hpp" -#include "tier4_vehicle_msgs/msg/control_mode.hpp" -#include "tier4_vehicle_msgs/srv/control_mode_request.hpp" #include #include @@ -76,6 +76,7 @@ using autoware_auto_vehicle_msgs::msg::TurnIndicatorsCommand; using autoware_auto_vehicle_msgs::msg::TurnIndicatorsReport; using autoware_auto_vehicle_msgs::msg::VehicleControlCommand; using autoware_auto_vehicle_msgs::msg::VelocityReport; +using autoware_auto_vehicle_msgs::srv::ControlModeCommand; using geometry_msgs::msg::AccelWithCovarianceStamped; using geometry_msgs::msg::Pose; using geometry_msgs::msg::PoseStamped; @@ -84,8 +85,6 @@ using geometry_msgs::msg::TransformStamped; using geometry_msgs::msg::Twist; using nav_msgs::msg::Odometry; using tier4_external_api_msgs::srv::InitializePose; -using tier4_vehicle_msgs::msg::ControlMode; -using tier4_vehicle_msgs::srv::ControlModeRequest; class DeltaTime { @@ -147,7 +146,7 @@ class PLANNING_SIMULATOR_PUBLIC SimplePlanningSimulator : public rclcpp::Node rclcpp::Subscription::SharedPtr sub_trajectory_; rclcpp::Subscription::SharedPtr sub_engage_; - rclcpp::Service::SharedPtr srv_mode_req_; + rclcpp::Service::SharedPtr srv_mode_req_; rclcpp::CallbackGroup::SharedPtr group_api_service_; tier4_api_utils::Service::SharedPtr srv_set_pose_; @@ -175,7 +174,7 @@ class PLANNING_SIMULATOR_PUBLIC SimplePlanningSimulator : public rclcpp::Node HazardLightsCommand::ConstSharedPtr current_hazard_lights_cmd_ptr_; Trajectory::ConstSharedPtr current_trajectory_ptr_; bool simulate_motion_; //!< stop vehicle motion simulation if false - ControlMode current_control_mode_; + ControlModeReport current_control_mode_; /* frame_id */ std::string simulated_frame_id_; //!< @brief simulated vehicle frame id @@ -249,8 +248,8 @@ class PLANNING_SIMULATOR_PUBLIC SimplePlanningSimulator : public rclcpp::Node * @brief ControlModeRequest server */ void on_control_mode_request( - const ControlModeRequest::Request::SharedPtr request, - const ControlModeRequest::Response::SharedPtr response); + const ControlModeCommand::Request::SharedPtr request, + const ControlModeCommand::Response::SharedPtr response); /** * @brief get z-position from trajectory 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 95c387ce6bcf5..ccf536c3892de 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 @@ -109,7 +109,7 @@ SimplePlanningSimulator::SimplePlanningSimulator(const rclcpp::NodeOptions & opt sub_trajectory_ = create_subscription( "input/trajectory", QoS{1}, std::bind(&SimplePlanningSimulator::on_trajectory, this, _1)); - srv_mode_req_ = create_service( + srv_mode_req_ = create_service( "input/control_mode_request", std::bind(&SimplePlanningSimulator::on_control_mode_request, this, _1, _2)); @@ -179,7 +179,7 @@ SimplePlanningSimulator::SimplePlanningSimulator(const rclcpp::NodeOptions & opt } // control mode - current_control_mode_.data = ControlMode::AUTO; + current_control_mode_.mode = ControlModeReport::AUTONOMOUS; current_manual_gear_cmd_.command = GearCommand::DRIVE; } @@ -260,7 +260,7 @@ void SimplePlanningSimulator::on_timer() { const float64_t dt = delta_time_.get_dt(get_clock()->now()); - if (current_control_mode_.data == ControlMode::AUTO) { + if (current_control_mode_.mode == ControlModeReport::AUTONOMOUS) { vehicle_model_ptr_->setGear(current_gear_cmd_.command); set_input(current_ackermann_cmd_); } else { @@ -385,11 +385,20 @@ void SimplePlanningSimulator::on_engage(const Engage::ConstSharedPtr msg) } void SimplePlanningSimulator::on_control_mode_request( - const ControlModeRequest::Request::SharedPtr request, - const ControlModeRequest::Response::SharedPtr response) -{ - current_control_mode_ = request->mode; - response->success = true; + const ControlModeCommand::Request::SharedPtr request, + const ControlModeCommand::Response::SharedPtr response) +{ + const auto m = request->mode; + if (m == ControlModeCommand::Request::MANUAL) { + current_control_mode_.mode = ControlModeReport::MANUAL; + response->success = true; + } else if (m == ControlModeCommand::Request::AUTONOMOUS) { + current_control_mode_.mode = ControlModeReport::AUTONOMOUS; + response->success = true; + } else { // not supported + response->success = false; + RCLCPP_ERROR(this->get_logger(), "Requested mode not supported"); + } return; } @@ -542,14 +551,8 @@ void SimplePlanningSimulator::publish_acceleration() void SimplePlanningSimulator::publish_control_mode_report() { - ControlModeReport msg; - msg.stamp = get_clock()->now(); - if (current_control_mode_.data == ControlMode::AUTO) { - msg.mode = ControlModeReport::AUTONOMOUS; - } else { - msg.mode = ControlModeReport::MANUAL; - } - pub_control_mode_report_->publish(msg); + current_control_mode_.stamp = get_clock()->now(); + pub_control_mode_report_->publish(current_control_mode_); } void SimplePlanningSimulator::publish_gear_report()