diff --git a/common/tier4_control_rviz_plugin/README.md b/common/tier4_control_rviz_plugin/README.md index 8bca77771eee2..e5e43a748310b 100644 --- a/common/tier4_control_rviz_plugin/README.md +++ b/common/tier4_control_rviz_plugin/README.md @@ -15,11 +15,11 @@ This package is to mimic external control for simulation. ### Output -| Name | Type | Description | -| -------------------------------- | ---------------------------------------------------------- | ----------------------- | -| `/control/gate_mode_cmd` | `tier4_control_msgs::msg::GateMode` | GATE mode | -| `/external/selected/control_cmd` | `autoware_auto_control_msgs::msg::AckermannControlCommand` | AckermannControlCommand | -| `/external/selected/gear_cmd` | `autoware_auto_vehicle_msgs::msg::GearCommand` | GEAR | +| Name | Type | Description | +| -------------------------------- | ---------------------------------------------- | ----------- | +| `/control/gate_mode_cmd` | `tier4_control_msgs::msg::GateMode` | GATE mode | +| `/external/selected/control_cmd` | `autoware_control_msgs::msg::Control` | Control | +| `/external/selected/gear_cmd` | `autoware_auto_vehicle_msgs::msg::GearCommand` | GEAR | ## Usage diff --git a/common/tier4_control_rviz_plugin/package.xml b/common/tier4_control_rviz_plugin/package.xml index 73562a766ce0b..12e1aab74cf67 100644 --- a/common/tier4_control_rviz_plugin/package.xml +++ b/common/tier4_control_rviz_plugin/package.xml @@ -10,8 +10,8 @@ ament_cmake_auto autoware_cmake - autoware_auto_control_msgs autoware_auto_vehicle_msgs + autoware_control_msgs libqt5-core libqt5-gui libqt5-widgets diff --git a/common/tier4_control_rviz_plugin/src/tools/manual_controller.cpp b/common/tier4_control_rviz_plugin/src/tools/manual_controller.cpp index 9610bf1e2b7f1..fe40048a3528a 100644 --- a/common/tier4_control_rviz_plugin/src/tools/manual_controller.cpp +++ b/common/tier4_control_rviz_plugin/src/tools/manual_controller.cpp @@ -110,11 +110,11 @@ ManualController::ManualController(QWidget * parent) : rviz_common::Panel(parent void ManualController::update() { if (!raw_node_) return; - AckermannControlCommand ackermann; + Control ackermann; { ackermann.stamp = raw_node_->get_clock()->now(); ackermann.lateral.steering_tire_angle = steering_angle_; - ackermann.longitudinal.speed = cruise_velocity_; + ackermann.longitudinal.velocity = cruise_velocity_; if (current_acceleration_) { /** * @brief Calculate desired acceleration by simple BackSteppingControl @@ -133,9 +133,9 @@ void ManualController::update() GearCommand gear_cmd; { const double eps = 0.001; - if (ackermann.longitudinal.speed > eps) { + if (ackermann.longitudinal.velocity > eps) { gear_cmd.command = GearCommand::DRIVE; - } else if (ackermann.longitudinal.speed < -eps && current_velocity_ < eps) { + } else if (ackermann.longitudinal.velocity < -eps && current_velocity_ < eps) { gear_cmd.command = GearCommand::REVERSE; ackermann.longitudinal.acceleration *= -1.0; } else { @@ -180,8 +180,8 @@ void ManualController::onInitialize() pub_gate_mode_ = raw_node_->create_publisher("/control/gate_mode_cmd", rclcpp::QoS(1)); - pub_control_command_ = raw_node_->create_publisher( - "/external/selected/control_cmd", rclcpp::QoS(1)); + pub_control_command_ = + raw_node_->create_publisher("/external/selected/control_cmd", rclcpp::QoS(1)); pub_gear_cmd_ = raw_node_->create_publisher("/external/selected/gear_cmd", 1); } diff --git a/common/tier4_control_rviz_plugin/src/tools/manual_controller.hpp b/common/tier4_control_rviz_plugin/src/tools/manual_controller.hpp index dee6f9a7aba21..ce18cd74a24e6 100644 --- a/common/tier4_control_rviz_plugin/src/tools/manual_controller.hpp +++ b/common/tier4_control_rviz_plugin/src/tools/manual_controller.hpp @@ -26,10 +26,10 @@ #include "autoware_auto_vehicle_msgs/msg/velocity_report.hpp" #include "geometry_msgs/msg/twist.hpp" -#include #include #include #include +#include #include #include @@ -37,9 +37,9 @@ namespace rviz_plugins { -using autoware_auto_control_msgs::msg::AckermannControlCommand; using autoware_auto_vehicle_msgs::msg::GearCommand; using autoware_auto_vehicle_msgs::msg::VelocityReport; +using autoware_control_msgs::msg::Control; using geometry_msgs::msg::Twist; using tier4_control_msgs::msg::GateMode; using EngageSrv = tier4_external_api_msgs::srv::Engage; @@ -74,7 +74,7 @@ public Q_SLOTS: // NOLINT for Qt rclcpp::Subscription::SharedPtr sub_velocity_; rclcpp::Subscription::SharedPtr sub_engage_; rclcpp::Publisher::SharedPtr pub_gate_mode_; - rclcpp::Publisher::SharedPtr pub_control_command_; + rclcpp::Publisher::SharedPtr pub_control_command_; rclcpp::Publisher::SharedPtr pub_gear_cmd_; rclcpp::Client::SharedPtr client_engage_; rclcpp::Subscription::SharedPtr sub_gear_; diff --git a/control/autonomous_emergency_braking/package.xml b/control/autonomous_emergency_braking/package.xml index 4f2ad6c50a9b3..53a51aa0b20fb 100644 --- a/control/autonomous_emergency_braking/package.xml +++ b/control/autonomous_emergency_braking/package.xml @@ -11,9 +11,9 @@ ament_cmake autoware_cmake - autoware_auto_control_msgs autoware_auto_system_msgs autoware_auto_vehicle_msgs + autoware_control_msgs diagnostic_updater geometry_msgs motion_utils diff --git a/control/control_performance_analysis/README.md b/control/control_performance_analysis/README.md index 1fa87c2caf3a9..30c347233fb9d 100644 --- a/control/control_performance_analysis/README.md +++ b/control/control_performance_analysis/README.md @@ -25,13 +25,13 @@ Error acceleration calculations are made based on the velocity calculations abov ### Input topics -| Name | Type | Description | -| ---------------------------------------- | -------------------------------------------------------- | ------------------------------------------- | -| `/planning/scenario_planning/trajectory` | autoware_auto_planning_msgs::msg::Trajectory | Output trajectory from planning module. | -| `/control/command/control_cmd` | autoware_auto_control_msgs::msg::AckermannControlCommand | Output control command from control module. | -| `/vehicle/status/steering_status` | autoware_auto_vehicle_msgs::msg::SteeringReport | Steering information from vehicle. | -| `/localization/kinematic_state` | nav_msgs::msg::Odometry | Use twist from odometry. | -| `/tf` | tf2_msgs::msg::TFMessage | Extract ego pose from tf. | +| Name | Type | Description | +| ---------------------------------------- | ----------------------------------------------- | ------------------------------------------- | +| `/planning/scenario_planning/trajectory` | autoware_auto_planning_msgs::msg::Trajectory | Output trajectory from planning module. | +| `/control/command/control_cmd` | autoware_control_msgs::msg::Control | Output control command from control module. | +| `/vehicle/status/steering_status` | autoware_auto_vehicle_msgs::msg::SteeringReport | Steering information from vehicle. | +| `/localization/kinematic_state` | nav_msgs::msg::Odometry | Use twist from odometry. | +| `/tf` | tf2_msgs::msg::TFMessage | Extract ego pose from tf. | ### Output topics diff --git a/control/control_performance_analysis/include/control_performance_analysis/control_performance_analysis_core.hpp b/control/control_performance_analysis/include/control_performance_analysis/control_performance_analysis_core.hpp index 275be99f418f1..d4f7cf95d92a8 100644 --- a/control/control_performance_analysis/include/control_performance_analysis/control_performance_analysis_core.hpp +++ b/control/control_performance_analysis/include/control_performance_analysis/control_performance_analysis_core.hpp @@ -24,9 +24,9 @@ #include #include -#include #include #include +#include #include #include #include @@ -38,9 +38,9 @@ namespace control_performance_analysis { -using autoware_auto_control_msgs::msg::AckermannControlCommand; using autoware_auto_planning_msgs::msg::Trajectory; using autoware_auto_vehicle_msgs::msg::SteeringReport; +using autoware_control_msgs::msg::Control; using control_performance_analysis::msg::DrivingMonitorStamped; using control_performance_analysis::msg::Error; using control_performance_analysis::msg::ErrorStamped; @@ -72,7 +72,7 @@ class ControlPerformanceAnalysisCore // Setters void setCurrentPose(const Pose & msg); void setCurrentWaypoints(const Trajectory & trajectory); - void setCurrentControlValue(const AckermannControlCommand & msg); + void setCurrentControlValue(const Control & msg); void setInterpolatedVars( const Pose & interpolated_pose, const double & interpolated_velocity, const double & interpolated_acceleration, const double & interpolated_steering_angle); @@ -102,7 +102,7 @@ class ControlPerformanceAnalysisCore std::shared_ptr current_trajectory_ptr_; std::shared_ptr current_vec_pose_ptr_; std::shared_ptr> odom_history_ptr_; // velocities at k-2, k-1, k, k+1 - std::shared_ptr current_control_ptr_; + std::shared_ptr current_control_ptr_; std::shared_ptr current_vec_steering_msg_ptr_; // State holder diff --git a/control/control_performance_analysis/include/control_performance_analysis/control_performance_analysis_node.hpp b/control/control_performance_analysis/include/control_performance_analysis/control_performance_analysis_node.hpp index 2d5ab8cce002d..bfc5f6bbd9f5f 100644 --- a/control/control_performance_analysis/include/control_performance_analysis/control_performance_analysis_node.hpp +++ b/control/control_performance_analysis/include/control_performance_analysis/control_performance_analysis_node.hpp @@ -23,9 +23,9 @@ #include #include -#include #include #include +#include #include #include @@ -36,9 +36,9 @@ namespace control_performance_analysis { -using autoware_auto_control_msgs::msg::AckermannControlCommand; using autoware_auto_planning_msgs::msg::Trajectory; using autoware_auto_vehicle_msgs::msg::SteeringReport; +using autoware_control_msgs::msg::Control; using control_performance_analysis::msg::DrivingMonitorStamped; using control_performance_analysis::msg::ErrorStamped; using geometry_msgs::msg::PoseStamped; @@ -52,9 +52,8 @@ class ControlPerformanceAnalysisNode : public rclcpp::Node private: // Subscribers and Local Variable Assignment rclcpp::Subscription::SharedPtr sub_trajectory_; // subscribe to trajectory - rclcpp::Subscription::SharedPtr - sub_control_cmd_; // subscribe to steering control value - rclcpp::Subscription::SharedPtr sub_velocity_; // subscribe to velocity + rclcpp::Subscription::SharedPtr sub_control_cmd_; // subscribe to steering control value + rclcpp::Subscription::SharedPtr sub_velocity_; // subscribe to velocity rclcpp::Subscription::SharedPtr sub_vehicle_steering_; // Publishers @@ -68,26 +67,26 @@ class ControlPerformanceAnalysisNode : public rclcpp::Node // Callback Methods void onTrajectory(const Trajectory::ConstSharedPtr msg); - void onControlRaw(const AckermannControlCommand::ConstSharedPtr control_msg); + void onControlRaw(const Control::ConstSharedPtr control_msg); void onVecSteeringMeasured(const SteeringReport::ConstSharedPtr meas_steer_msg); void onVelocity(const Odometry::ConstSharedPtr msg); // Parameters Params param_{}; // wheelbase, control period and feedback coefficients. // State holder - AckermannControlCommand::ConstSharedPtr last_control_cmd_; + Control::ConstSharedPtr last_control_cmd_; double d_control_cmd_{0}; // Subscriber Parameters Trajectory::ConstSharedPtr current_trajectory_ptr_; // ConstPtr to local traj. - AckermannControlCommand::ConstSharedPtr current_control_msg_ptr_; + Control::ConstSharedPtr current_control_msg_ptr_; SteeringReport::ConstSharedPtr current_vec_steering_msg_ptr_; Odometry::ConstSharedPtr current_odom_ptr_; PoseStamped::ConstSharedPtr current_pose_; // pose of the vehicle, x, y, heading // prev states Trajectory prev_traj; - AckermannControlCommand prev_cmd; + Control prev_cmd; SteeringReport prev_steering; // Algorithm diff --git a/control/control_performance_analysis/package.xml b/control/control_performance_analysis/package.xml index 7003b7d931fa8..817263d5cfe39 100644 --- a/control/control_performance_analysis/package.xml +++ b/control/control_performance_analysis/package.xml @@ -24,9 +24,9 @@ builtin_interfaces - autoware_auto_control_msgs autoware_auto_planning_msgs autoware_auto_vehicle_msgs + autoware_control_msgs geometry_msgs libboost-dev motion_utils diff --git a/control/control_performance_analysis/src/control_performance_analysis_core.cpp b/control/control_performance_analysis/src/control_performance_analysis_core.cpp index 777f5dee3e470..0570b37342cbc 100644 --- a/control/control_performance_analysis/src/control_performance_analysis_core.cpp +++ b/control/control_performance_analysis/src/control_performance_analysis_core.cpp @@ -74,9 +74,9 @@ void ControlPerformanceAnalysisCore::setCurrentPose(const Pose & msg) current_vec_pose_ptr_ = std::make_shared(msg); } -void ControlPerformanceAnalysisCore::setCurrentControlValue(const AckermannControlCommand & msg) +void ControlPerformanceAnalysisCore::setCurrentControlValue(const Control & msg) { - current_control_ptr_ = std::make_shared(msg); + current_control_ptr_ = std::make_shared(msg); } std::pair ControlPerformanceAnalysisCore::findClosestPrevWayPointIdx_path_direction() diff --git a/control/control_performance_analysis/src/control_performance_analysis_node.cpp b/control/control_performance_analysis/src/control_performance_analysis_node.cpp index ad845c86fc19a..a7d55d80172e8 100644 --- a/control/control_performance_analysis/src/control_performance_analysis_node.cpp +++ b/control/control_performance_analysis/src/control_performance_analysis_node.cpp @@ -24,7 +24,7 @@ namespace { -using autoware_auto_control_msgs::msg::AckermannControlCommand; +using autoware_control_msgs::msg::Control; using control_performance_analysis::msg::DrivingMonitorStamped; using control_performance_analysis::msg::ErrorStamped; @@ -62,7 +62,7 @@ ControlPerformanceAnalysisNode::ControlPerformanceAnalysisNode( "~/input/reference_trajectory", 1, std::bind(&ControlPerformanceAnalysisNode::onTrajectory, this, _1)); - sub_control_cmd_ = create_subscription( + sub_control_cmd_ = create_subscription( "~/input/control_raw", 1, std::bind(&ControlPerformanceAnalysisNode::onControlRaw, this, _1)); sub_vehicle_steering_ = create_subscription( @@ -93,8 +93,7 @@ void ControlPerformanceAnalysisNode::onTrajectory(const Trajectory::ConstSharedP current_trajectory_ptr_ = msg; } -void ControlPerformanceAnalysisNode::onControlRaw( - const AckermannControlCommand::ConstSharedPtr control_msg) +void ControlPerformanceAnalysisNode::onControlRaw(const Control::ConstSharedPtr control_msg) { if (last_control_cmd_) { const rclcpp::Duration & duration = diff --git a/control/joy_controller/README.md b/control/joy_controller/README.md index 4518de37b7034..a014b00808135 100644 --- a/control/joy_controller/README.md +++ b/control/joy_controller/README.md @@ -15,15 +15,15 @@ ### Output topics -| Name | Type | Description | -| ----------------------------------- | -------------------------------------------------------- | ---------------------------------------- | -| `~/output/control_command` | autoware_auto_control_msgs::msg::AckermannControlCommand | lateral and longitudinal control command | -| `~/output/external_control_command` | tier4_external_api_msgs::msg::ControlCommandStamped | lateral and longitudinal control command | -| `~/output/shift` | tier4_external_api_msgs::msg::GearShiftStamped | gear command | -| `~/output/turn_signal` | tier4_external_api_msgs::msg::TurnSignalStamped | turn signal command | -| `~/output/gate_mode` | tier4_control_msgs::msg::GateMode | gate mode (Auto or External) | -| `~/output/heartbeat` | tier4_external_api_msgs::msg::Heartbeat | heartbeat | -| `~/output/vehicle_engage` | autoware_auto_vehicle_msgs::msg::Engage | vehicle engage | +| Name | Type | Description | +| ----------------------------------- | --------------------------------------------------- | ---------------------------------------- | +| `~/output/control_command` | autoware_control_msgs::msg::Control | lateral and longitudinal control command | +| `~/output/external_control_command` | tier4_external_api_msgs::msg::ControlCommandStamped | lateral and longitudinal control command | +| `~/output/shift` | tier4_external_api_msgs::msg::GearShiftStamped | gear command | +| `~/output/turn_signal` | tier4_external_api_msgs::msg::TurnSignalStamped | turn signal command | +| `~/output/gate_mode` | tier4_control_msgs::msg::GateMode | gate mode (Auto or External) | +| `~/output/heartbeat` | tier4_external_api_msgs::msg::Heartbeat | heartbeat | +| `~/output/vehicle_engage` | autoware_auto_vehicle_msgs::msg::Engage | vehicle engage | ## Parameters diff --git a/control/joy_controller/include/joy_controller/joy_controller.hpp b/control/joy_controller/include/joy_controller/joy_controller.hpp index c6de42afc38d9..d3904f93e4ea0 100644 --- a/control/joy_controller/include/joy_controller/joy_controller.hpp +++ b/control/joy_controller/include/joy_controller/joy_controller.hpp @@ -19,8 +19,8 @@ #include -#include #include +#include #include #include #include @@ -80,8 +80,7 @@ class AutowareJoyControllerNode : public rclcpp::Node void onOdometry(const nav_msgs::msg::Odometry::ConstSharedPtr msg); // Publisher - rclcpp::Publisher::SharedPtr - pub_control_command_; + rclcpp::Publisher::SharedPtr pub_control_command_; rclcpp::Publisher::SharedPtr pub_external_control_command_; rclcpp::Publisher::SharedPtr pub_shift_; @@ -105,7 +104,7 @@ class AutowareJoyControllerNode : public rclcpp::Node rclcpp::Client::SharedPtr client_autoware_engage_; // Previous State - autoware_auto_control_msgs::msg::AckermannControlCommand prev_control_command_; + autoware_control_msgs::msg::Control prev_control_command_; tier4_external_api_msgs::msg::ControlCommand prev_external_control_command_; GearShiftType prev_shift_ = tier4_external_api_msgs::msg::GearShift::NONE; TurnSignalType prev_turn_signal_ = tier4_external_api_msgs::msg::TurnSignal::NONE; diff --git a/control/joy_controller/package.xml b/control/joy_controller/package.xml index 79ee9f27868f2..bbae73a971c41 100644 --- a/control/joy_controller/package.xml +++ b/control/joy_controller/package.xml @@ -16,8 +16,8 @@ ament_cmake_auto autoware_cmake - autoware_auto_control_msgs autoware_auto_vehicle_msgs + autoware_control_msgs geometry_msgs joy nav_msgs diff --git a/control/joy_controller/src/joy_controller/joy_controller_node.cpp b/control/joy_controller/src/joy_controller/joy_controller_node.cpp index 897986a7a41cf..6f45382fc8b82 100644 --- a/control/joy_controller/src/joy_controller/joy_controller_node.cpp +++ b/control/joy_controller/src/joy_controller/joy_controller_node.cpp @@ -251,7 +251,7 @@ void AutowareJoyControllerNode::onTimer() void AutowareJoyControllerNode::publishControlCommand() { - autoware_auto_control_msgs::msg::AckermannControlCommand cmd; + autoware_control_msgs::msg::Control cmd; cmd.stamp = this->now(); { cmd.lateral.steering_tire_angle = steer_ratio_ * joy_->steer(); @@ -259,24 +259,24 @@ void AutowareJoyControllerNode::publishControlCommand() if (joy_->accel()) { cmd.longitudinal.acceleration = accel_ratio_ * joy_->accel(); - cmd.longitudinal.speed = + cmd.longitudinal.velocity = twist_->twist.linear.x + velocity_gain_ * cmd.longitudinal.acceleration; - cmd.longitudinal.speed = - std::min(cmd.longitudinal.speed, static_cast(max_forward_velocity_)); + cmd.longitudinal.velocity = + std::min(cmd.longitudinal.velocity, static_cast(max_forward_velocity_)); } if (joy_->brake()) { - cmd.longitudinal.speed = 0.0; + cmd.longitudinal.velocity = 0.0; cmd.longitudinal.acceleration = -brake_ratio_ * joy_->brake(); } // Backward if (joy_->accel() && joy_->brake()) { cmd.longitudinal.acceleration = backward_accel_ratio_ * joy_->accel(); - cmd.longitudinal.speed = + cmd.longitudinal.velocity = twist_->twist.linear.x - velocity_gain_ * cmd.longitudinal.acceleration; - cmd.longitudinal.speed = - std::max(cmd.longitudinal.speed, static_cast(-max_backward_velocity_)); + cmd.longitudinal.velocity = + std::max(cmd.longitudinal.velocity, static_cast(-max_backward_velocity_)); } } @@ -484,8 +484,7 @@ AutowareJoyControllerNode::AutowareJoyControllerNode(const rclcpp::NodeOptions & // Publisher pub_control_command_ = - this->create_publisher( - "output/control_command", 1); + this->create_publisher("output/control_command", 1); pub_external_control_command_ = this->create_publisher( "output/external_control_command", 1); diff --git a/control/mpc_lateral_controller/include/mpc_lateral_controller/mpc.hpp b/control/mpc_lateral_controller/include/mpc_lateral_controller/mpc.hpp index 0d144726a0866..8990b5969435a 100644 --- a/control/mpc_lateral_controller/include/mpc_lateral_controller/mpc.hpp +++ b/control/mpc_lateral_controller/include/mpc_lateral_controller/mpc.hpp @@ -22,9 +22,9 @@ #include "mpc_lateral_controller/vehicle_model/vehicle_model_interface.hpp" #include "rclcpp/rclcpp.hpp" -#include "autoware_auto_control_msgs/msg/ackermann_lateral_command.hpp" #include "autoware_auto_planning_msgs/msg/trajectory.hpp" #include "autoware_auto_vehicle_msgs/msg/steering_report.hpp" +#include "autoware_control_msgs/msg/lateral.hpp" #include "geometry_msgs/msg/pose.hpp" #include "nav_msgs/msg/odometry.hpp" #include "tier4_debug_msgs/msg/float32_multi_array_stamped.hpp" @@ -38,9 +38,9 @@ namespace autoware::motion::control::mpc_lateral_controller { -using autoware_auto_control_msgs::msg::AckermannLateralCommand; using autoware_auto_planning_msgs::msg::Trajectory; using autoware_auto_vehicle_msgs::msg::SteeringReport; +using autoware_control_msgs::msg::Lateral; using geometry_msgs::msg::Pose; using nav_msgs::msg::Odometry; using tier4_debug_msgs::msg::Float32MultiArrayStamped; @@ -376,7 +376,7 @@ class MPC */ Float32MultiArrayStamped generateDiagData( const MPCTrajectory & reference_trajectory, const MPCData & mpc_data, - const MPCMatrix & mpc_matrix, const AckermannLateralCommand & ctrl_cmd, const VectorXd & Uex, + const MPCMatrix & mpc_matrix, const Lateral & ctrl_cmd, const VectorXd & Uex, const Odometry & current_kinematics) const; //!< @brief logging with warn and return false @@ -429,9 +429,8 @@ class MPC * @return True if the MPC calculation is successful, false otherwise. */ bool calculateMPC( - const SteeringReport & current_steer, const Odometry & current_kinematics, - AckermannLateralCommand & ctrl_cmd, Trajectory & predicted_trajectory, - Float32MultiArrayStamped & diagnostic); + const SteeringReport & current_steer, const Odometry & current_kinematics, Lateral & ctrl_cmd, + Trajectory & predicted_trajectory, Float32MultiArrayStamped & diagnostic); /** * @brief Set the reference trajectory to be followed. diff --git a/control/mpc_lateral_controller/include/mpc_lateral_controller/mpc_lateral_controller.hpp b/control/mpc_lateral_controller/include/mpc_lateral_controller/mpc_lateral_controller.hpp index ee33062854ab9..981ca96b07692 100644 --- a/control/mpc_lateral_controller/include/mpc_lateral_controller/mpc_lateral_controller.hpp +++ b/control/mpc_lateral_controller/include/mpc_lateral_controller/mpc_lateral_controller.hpp @@ -22,10 +22,10 @@ #include "rclcpp/rclcpp.hpp" #include "trajectory_follower_base/lateral_controller_base.hpp" -#include "autoware_auto_control_msgs/msg/ackermann_lateral_command.hpp" #include "autoware_auto_planning_msgs/msg/trajectory.hpp" #include "autoware_auto_vehicle_msgs/msg/steering_report.hpp" #include "autoware_auto_vehicle_msgs/msg/vehicle_odometry.hpp" +#include "autoware_control_msgs/msg/lateral.hpp" #include "geometry_msgs/msg/pose.hpp" #include "geometry_msgs/msg/pose_stamped.hpp" #include "nav_msgs/msg/odometry.hpp" @@ -42,9 +42,9 @@ namespace autoware::motion::control::mpc_lateral_controller { namespace trajectory_follower = ::autoware::motion::control::trajectory_follower; -using autoware_auto_control_msgs::msg::AckermannLateralCommand; using autoware_auto_planning_msgs::msg::Trajectory; using autoware_auto_vehicle_msgs::msg::SteeringReport; +using autoware_control_msgs::msg::Lateral; using nav_msgs::msg::Odometry; using tier4_debug_msgs::msg::Float32MultiArrayStamped; using tier4_debug_msgs::msg::Float32Stamped; @@ -95,10 +95,10 @@ class MpcLateralController : public trajectory_follower::LateralControllerBase bool m_is_mpc_history_filled{false}; // store the last mpc outputs for 1 sec - std::vector> m_mpc_steering_history{}; + std::vector> m_mpc_steering_history{}; // set the mpc steering output to history - void setSteeringToHistory(const AckermannLateralCommand & steering); + void setSteeringToHistory(const Lateral & steering); // check if the mpc steering output is converged bool isMpcConverged(); @@ -116,7 +116,7 @@ class MpcLateralController : public trajectory_follower::LateralControllerBase bool m_is_ctrl_cmd_prev_initialized = false; // Previous control command for path following. - AckermannLateralCommand m_ctrl_cmd_prev; + Lateral m_ctrl_cmd_prev; // Flag indicating whether the first trajectory has been received. bool m_has_received_first_trajectory = false; @@ -194,7 +194,7 @@ class MpcLateralController : public trajectory_follower::LateralControllerBase * @param ctrl_cmd Control command to be created. * @return Created control command. */ - AckermannLateralCommand createCtrlCmdMsg(const AckermannLateralCommand & ctrl_cmd); + Lateral createCtrlCmdMsg(const Lateral & ctrl_cmd); /** * @brief Publish the predicted future trajectory. @@ -212,13 +212,13 @@ class MpcLateralController : public trajectory_follower::LateralControllerBase * @brief Get the stop control command. * @return Stop control command. */ - AckermannLateralCommand getStopControlCommand() const; + Lateral getStopControlCommand() const; /** * @brief Get the control command applied before initialization. * @return Initial control command. */ - AckermannLateralCommand getInitialControlCommand() const; + Lateral getInitialControlCommand() const; /** * @brief Check if the ego car is in a stopped state. @@ -244,7 +244,7 @@ class MpcLateralController : public trajectory_follower::LateralControllerBase * @param cmd Steering control command to be checked. * @return True if the steering control is converged and stable, false otherwise. */ - bool isSteerConverged(const AckermannLateralCommand & cmd) const; + bool isSteerConverged(const Lateral & cmd) const; rclcpp::Node::OnSetParametersCallbackHandle::SharedPtr m_set_param_res; diff --git a/control/mpc_lateral_controller/include/mpc_lateral_controller/steering_predictor.hpp b/control/mpc_lateral_controller/include/mpc_lateral_controller/steering_predictor.hpp index 7ca2fa1a61528..16e9b165fb1a5 100644 --- a/control/mpc_lateral_controller/include/mpc_lateral_controller/steering_predictor.hpp +++ b/control/mpc_lateral_controller/include/mpc_lateral_controller/steering_predictor.hpp @@ -17,14 +17,14 @@ #include "rclcpp/rclcpp.hpp" -#include "autoware_auto_control_msgs/msg/ackermann_lateral_command.hpp" +#include "autoware_control_msgs/msg/lateral.hpp" #include #include namespace autoware::motion::control::mpc_lateral_controller { -using autoware_auto_control_msgs::msg::AckermannLateralCommand; +using autoware_control_msgs::msg::Lateral; class SteeringPredictor { @@ -61,7 +61,7 @@ class SteeringPredictor double m_input_delay; // Buffer of sent control commands. - std::vector m_ctrl_cmd_vec; + std::vector m_ctrl_cmd_vec; /** * @brief Get the sum of all steering commands over the given time range. diff --git a/control/mpc_lateral_controller/package.xml b/control/mpc_lateral_controller/package.xml index da03c4481e782..6fb40fe440a32 100644 --- a/control/mpc_lateral_controller/package.xml +++ b/control/mpc_lateral_controller/package.xml @@ -17,9 +17,9 @@ ament_cmake_auto autoware_cmake - autoware_auto_control_msgs autoware_auto_planning_msgs autoware_auto_vehicle_msgs + autoware_control_msgs diagnostic_msgs diagnostic_updater eigen diff --git a/control/mpc_lateral_controller/src/mpc.cpp b/control/mpc_lateral_controller/src/mpc.cpp index 8e99880b68050..28428ad12e74a 100644 --- a/control/mpc_lateral_controller/src/mpc.cpp +++ b/control/mpc_lateral_controller/src/mpc.cpp @@ -27,9 +27,8 @@ using tier4_autoware_utils::normalizeRadian; using tier4_autoware_utils::rad2deg; bool MPC::calculateMPC( - const SteeringReport & current_steer, const Odometry & current_kinematics, - AckermannLateralCommand & ctrl_cmd, Trajectory & predicted_trajectory, - Float32MultiArrayStamped & diagnostic) + const SteeringReport & current_steer, const Odometry & current_kinematics, Lateral & ctrl_cmd, + Trajectory & predicted_trajectory, Float32MultiArrayStamped & diagnostic) { // since the reference trajectory does not take into account the current velocity of the ego // vehicle, it needs to calculate the trajectory velocity considering the longitudinal dynamics. @@ -132,7 +131,7 @@ Trajectory MPC::calcPredictedTrajectory( Float32MultiArrayStamped MPC::generateDiagData( const MPCTrajectory & reference_trajectory, const MPCData & mpc_data, - const MPCMatrix & mpc_matrix, const AckermannLateralCommand & ctrl_cmd, const VectorXd & Uex, + const MPCMatrix & mpc_matrix, const Lateral & ctrl_cmd, const VectorXd & Uex, const Odometry & current_kinematics) const { Float32MultiArrayStamped diagnostic; diff --git a/control/mpc_lateral_controller/src/mpc_lateral_controller.cpp b/control/mpc_lateral_controller/src/mpc_lateral_controller.cpp index c7fbce7411f1e..6a6416daaf0a2 100644 --- a/control/mpc_lateral_controller/src/mpc_lateral_controller.cpp +++ b/control/mpc_lateral_controller/src/mpc_lateral_controller.cpp @@ -235,7 +235,7 @@ trajectory_follower::LateralOutput MpcLateralController::run( m_current_steering.steering_tire_angle -= steering_offset_->getOffset(); } - AckermannLateralCommand ctrl_cmd; + Lateral ctrl_cmd; Trajectory predicted_traj; Float32MultiArrayStamped debug_values; @@ -301,7 +301,7 @@ trajectory_follower::LateralOutput MpcLateralController::run( return createLateralOutput(ctrl_cmd, is_mpc_solved); } -bool MpcLateralController::isSteerConverged(const AckermannLateralCommand & cmd) const +bool MpcLateralController::isSteerConverged(const Lateral & cmd) const { // wait for a while to propagate the trajectory shape to the output command when the trajectory // shape is changed. @@ -372,17 +372,17 @@ void MpcLateralController::setTrajectory(const Trajectory & msg) } } -AckermannLateralCommand MpcLateralController::getStopControlCommand() const +Lateral MpcLateralController::getStopControlCommand() const { - AckermannLateralCommand cmd; + Lateral cmd; cmd.steering_tire_angle = static_cast(m_steer_cmd_prev); cmd.steering_tire_rotation_rate = 0.0; return cmd; } -AckermannLateralCommand MpcLateralController::getInitialControlCommand() const +Lateral MpcLateralController::getInitialControlCommand() const { - AckermannLateralCommand cmd; + Lateral cmd; cmd.steering_tire_angle = m_current_steering.steering_tire_angle; cmd.steering_tire_rotation_rate = 0.0; return cmd; @@ -420,8 +420,7 @@ bool MpcLateralController::isStoppedState() const } } -AckermannLateralCommand MpcLateralController::createCtrlCmdMsg( - const AckermannLateralCommand & ctrl_cmd) +Lateral MpcLateralController::createCtrlCmdMsg(const Lateral & ctrl_cmd) { auto out = ctrl_cmd; out.stamp = node_->now(); @@ -447,7 +446,7 @@ void MpcLateralController::publishDebugValues(Float32MultiArrayStamped & debug_v m_pub_steer_offset->publish(offset); } -void MpcLateralController::setSteeringToHistory(const AckermannLateralCommand & steering) +void MpcLateralController::setSteeringToHistory(const Lateral & steering) { const auto time = node_->now(); if (m_mpc_steering_history.empty()) { diff --git a/control/mpc_lateral_controller/src/steering_predictor.cpp b/control/mpc_lateral_controller/src/steering_predictor.cpp index f2570047d5bd2..48d8fa8c47a97 100644 --- a/control/mpc_lateral_controller/src/steering_predictor.cpp +++ b/control/mpc_lateral_controller/src/steering_predictor.cpp @@ -47,7 +47,7 @@ double SteeringPredictor::calcSteerPrediction() void SteeringPredictor::storeSteerCmd(const double steer) { const auto time_delayed = m_clock->now() + rclcpp::Duration::from_seconds(m_input_delay); - AckermannLateralCommand cmd; + Lateral cmd; cmd.stamp = time_delayed; cmd.steering_tire_angle = static_cast(steer); diff --git a/control/mpc_lateral_controller/test/test_mpc.cpp b/control/mpc_lateral_controller/test/test_mpc.cpp index 72066003696d8..33ee135d0a4c4 100644 --- a/control/mpc_lateral_controller/test/test_mpc.cpp +++ b/control/mpc_lateral_controller/test/test_mpc.cpp @@ -20,10 +20,10 @@ #include "mpc_lateral_controller/vehicle_model/vehicle_model_bicycle_kinematics.hpp" #include "mpc_lateral_controller/vehicle_model/vehicle_model_bicycle_kinematics_no_delay.hpp" -#include "autoware_auto_control_msgs/msg/ackermann_lateral_command.hpp" #include "autoware_auto_planning_msgs/msg/trajectory.hpp" #include "autoware_auto_planning_msgs/msg/trajectory_point.hpp" #include "autoware_auto_vehicle_msgs/msg/steering_report.hpp" +#include "autoware_control_msgs/msg/lateral.hpp" #include "geometry_msgs/msg/pose.hpp" #include "tier4_debug_msgs/msg/float32_multi_array_stamped.hpp" @@ -40,10 +40,10 @@ namespace autoware::motion::control::mpc_lateral_controller { -using autoware_auto_control_msgs::msg::AckermannLateralCommand; using autoware_auto_planning_msgs::msg::Trajectory; using autoware_auto_planning_msgs::msg::TrajectoryPoint; using autoware_auto_vehicle_msgs::msg::SteeringReport; +using autoware_control_msgs::msg::Lateral; using geometry_msgs::msg::Pose; using geometry_msgs::msg::PoseStamped; using tier4_debug_msgs::msg::Float32MultiArrayStamped; @@ -195,7 +195,7 @@ TEST_F(MPCTest, InitializeAndCalculate) initializeMPC(mpc); // Calculate MPC - AckermannLateralCommand ctrl_cmd; + Lateral ctrl_cmd; Trajectory pred_traj; Float32MultiArrayStamped diag; const auto odom = makeOdometry(pose_zero, default_velocity); @@ -224,7 +224,7 @@ TEST_F(MPCTest, InitializeAndCalculateRightTurn) mpc.setReferenceTrajectory(dummy_right_turn_trajectory, trajectory_param); // Calculate MPC - AckermannLateralCommand ctrl_cmd; + Lateral ctrl_cmd; Trajectory pred_traj; Float32MultiArrayStamped diag; const auto odom = makeOdometry(pose_zero, default_velocity); @@ -249,7 +249,7 @@ TEST_F(MPCTest, OsqpCalculate) ASSERT_TRUE(mpc.hasQPSolver()); // Calculate MPC - AckermannLateralCommand ctrl_cmd; + Lateral ctrl_cmd; Trajectory pred_traj; Float32MultiArrayStamped diag; // with OSQP this function returns false despite finding correct solutions @@ -275,7 +275,7 @@ TEST_F(MPCTest, OsqpCalculateRightTurn) ASSERT_TRUE(mpc.hasQPSolver()); // Calculate MPC - AckermannLateralCommand ctrl_cmd; + Lateral ctrl_cmd; Trajectory pred_traj; Float32MultiArrayStamped diag; const auto odom = makeOdometry(pose_zero, default_velocity); @@ -303,7 +303,7 @@ TEST_F(MPCTest, KinematicsNoDelayCalculate) // Init trajectory mpc.setReferenceTrajectory(dummy_straight_trajectory, trajectory_param); // Calculate MPC - AckermannLateralCommand ctrl_cmd; + Lateral ctrl_cmd; Trajectory pred_traj; Float32MultiArrayStamped diag; const auto odom = makeOdometry(pose_zero, default_velocity); @@ -331,7 +331,7 @@ TEST_F(MPCTest, KinematicsNoDelayCalculateRightTurn) mpc.initializeLowPassFilters(steering_lpf_cutoff_hz, error_deriv_lpf_cutoff_hz); // Calculate MPC - AckermannLateralCommand ctrl_cmd; + Lateral ctrl_cmd; Trajectory pred_traj; Float32MultiArrayStamped diag; const auto odom = makeOdometry(pose_zero, default_velocity); @@ -355,7 +355,7 @@ TEST_F(MPCTest, DynamicCalculate) ASSERT_TRUE(mpc.hasQPSolver()); // Calculate MPC - AckermannLateralCommand ctrl_cmd; + Lateral ctrl_cmd; Trajectory pred_traj; Float32MultiArrayStamped diag; const auto odom = makeOdometry(pose_zero, default_velocity); @@ -378,7 +378,7 @@ TEST_F(MPCTest, MultiSolveWithBuffer) mpc.m_input_buffer = {0.0, 0.0, 0.0}; // Calculate MPC - AckermannLateralCommand ctrl_cmd; + Lateral ctrl_cmd; Trajectory pred_traj; Float32MultiArrayStamped diag; const auto odom = makeOdometry(pose_zero, default_velocity); @@ -417,7 +417,7 @@ TEST_F(MPCTest, FailureCases) Pose pose_far; pose_far.position.x = pose_zero.position.x - admissible_position_error - 1.0; pose_far.position.y = pose_zero.position.y - admissible_position_error - 1.0; - AckermannLateralCommand ctrl_cmd; + Lateral ctrl_cmd; Trajectory pred_traj; Float32MultiArrayStamped diag; const auto odom = makeOdometry(pose_far, default_velocity); diff --git a/control/operation_mode_transition_manager/README.md b/control/operation_mode_transition_manager/README.md index 3bbc3a006c73a..5f8c3953db50c 100644 --- a/control/operation_mode_transition_manager/README.md +++ b/control/operation_mode_transition_manager/README.md @@ -38,7 +38,7 @@ For the mode transition: For the transition availability/completion check: -- /control/command/control_cmd [`autoware_auto_control_msgs/msg/AckermannControlCommand`]: vehicle control signal +- /control/command/control_cmd [`autoware_control_msgs/msg/Control`]: vehicle control signal - /localization/kinematic_state [`nav_msgs/msg/Odometry`]: ego vehicle state - /planning/scenario_planning/trajectory [`autoware_auto_planning_msgs/msg/Trajectory`]: planning trajectory - /vehicle/status/control_mode [`autoware_auto_vehicle_msgs/msg/ControlModeReport`]: vehicle control mode (autonomous/manual) diff --git a/control/operation_mode_transition_manager/package.xml b/control/operation_mode_transition_manager/package.xml index dbe9a21c1186a..0eb78949bb3b9 100644 --- a/control/operation_mode_transition_manager/package.xml +++ b/control/operation_mode_transition_manager/package.xml @@ -11,9 +11,9 @@ autoware_cmake rosidl_default_generators - autoware_auto_control_msgs autoware_auto_system_msgs autoware_auto_vehicle_msgs + autoware_control_msgs component_interface_specs component_interface_utils geometry_msgs diff --git a/control/operation_mode_transition_manager/src/state.cpp b/control/operation_mode_transition_manager/src/state.cpp index f8434bbdb081f..c62775cba8625 100644 --- a/control/operation_mode_transition_manager/src/state.cpp +++ b/control/operation_mode_transition_manager/src/state.cpp @@ -34,9 +34,8 @@ AutonomousMode::AutonomousMode(rclcpp::Node * node) { vehicle_info_ = vehicle_info_util::VehicleInfoUtil(*node).getVehicleInfo(); - sub_control_cmd_ = node->create_subscription( - "control_cmd", 1, - [this](const AckermannControlCommand::SharedPtr msg) { control_cmd_ = *msg; }); + sub_control_cmd_ = node->create_subscription( + "control_cmd", 1, [this](const Control::SharedPtr msg) { control_cmd_ = *msg; }); sub_kinematics_ = node->create_subscription( "kinematics", 1, [this](const Odometry::SharedPtr msg) { kinematics_ = *msg; }); @@ -204,7 +203,7 @@ bool AutonomousMode::isModeChangeAvailable() } const auto current_speed = kinematics_.twist.twist.linear.x; - const auto target_control_speed = control_cmd_.longitudinal.speed; + const auto target_control_speed = control_cmd_.longitudinal.velocity; const auto & param = engage_acceptable_param_; if (trajectory_.points.size() < 2) { diff --git a/control/operation_mode_transition_manager/src/state.hpp b/control/operation_mode_transition_manager/src/state.hpp index 9d384857bbe3d..03e6eafff60c8 100644 --- a/control/operation_mode_transition_manager/src/state.hpp +++ b/control/operation_mode_transition_manager/src/state.hpp @@ -21,8 +21,8 @@ #include #include -#include #include +#include #include #include @@ -63,10 +63,10 @@ class AutonomousMode : public ModeChangeBase bool hasDangerAcceleration(); std::pair hasDangerLateralAcceleration(); - using AckermannControlCommand = autoware_auto_control_msgs::msg::AckermannControlCommand; + using Control = autoware_control_msgs::msg::Control; using Odometry = nav_msgs::msg::Odometry; using Trajectory = autoware_auto_planning_msgs::msg::Trajectory; - rclcpp::Subscription::SharedPtr sub_control_cmd_; + rclcpp::Subscription::SharedPtr sub_control_cmd_; rclcpp::Subscription::SharedPtr sub_kinematics_; rclcpp::Subscription::SharedPtr sub_trajectory_; rclcpp::Logger logger_; @@ -77,7 +77,7 @@ class AutonomousMode : public ModeChangeBase double nearest_yaw_deviation_threshold_; // [rad] for finding nearest index EngageAcceptableParam engage_acceptable_param_; StableCheckParam stable_check_param_; - AckermannControlCommand control_cmd_; + Control control_cmd_; Odometry kinematics_; Trajectory trajectory_; vehicle_info_util::VehicleInfo vehicle_info_; diff --git a/control/pid_longitudinal_controller/README.md b/control/pid_longitudinal_controller/README.md index 418b3f7d27220..04633aceff570 100644 --- a/control/pid_longitudinal_controller/README.md +++ b/control/pid_longitudinal_controller/README.md @@ -141,7 +141,7 @@ Set the following from the [controller_node](../trajectory_follower_node/README. Return LongitudinalOutput which contains the following to the controller node -- `autoware_auto_control_msgs/LongitudinalCommand`: command to control the longitudinal motion of the vehicle. It contains the target velocity and target acceleration. +- `autoware_control_msgs/msg/Longitudinal`: command to control the longitudinal motion of the vehicle. It contains the target velocity and target acceleration. - LongitudinalSyncData - velocity convergence(currently not used) diff --git a/control/pid_longitudinal_controller/include/pid_longitudinal_controller/pid_longitudinal_controller.hpp b/control/pid_longitudinal_controller/include/pid_longitudinal_controller/pid_longitudinal_controller.hpp index c2dbc67f011dc..e9d2781f8fe8d 100644 --- a/control/pid_longitudinal_controller/include/pid_longitudinal_controller/pid_longitudinal_controller.hpp +++ b/control/pid_longitudinal_controller/include/pid_longitudinal_controller/pid_longitudinal_controller.hpp @@ -32,9 +32,9 @@ #include #include "autoware_adapi_v1_msgs/msg/operation_mode_state.hpp" -#include "autoware_auto_control_msgs/msg/longitudinal_command.hpp" #include "autoware_auto_planning_msgs/msg/trajectory.hpp" #include "autoware_auto_vehicle_msgs/msg/vehicle_odometry.hpp" +#include "autoware_control_msgs/msg/longitudinal.hpp" #include "geometry_msgs/msg/pose_stamped.hpp" #include "nav_msgs/msg/odometry.hpp" #include "tf2_msgs/msg/tf_message.hpp" @@ -186,7 +186,7 @@ class PidLongitudinalController : public trajectory_follower::LongitudinalContro double m_ego_nearest_yaw_threshold; // buffer of send command - std::vector m_ctrl_cmd_vec; + std::vector m_ctrl_cmd_vec; // for calculating dt std::shared_ptr m_prev_control_time{nullptr}; @@ -279,7 +279,7 @@ class PidLongitudinalController : public trajectory_follower::LongitudinalContro * @param [in] ctrl_cmd calculated control command to control velocity * @param [in] current_vel current velocity of the vehicle */ - autoware_auto_control_msgs::msg::LongitudinalCommand createCtrlCmdMsg( + autoware_control_msgs::msg::Longitudinal createCtrlCmdMsg( const Motion & ctrl_cmd, const double & current_vel); /** diff --git a/control/pid_longitudinal_controller/package.xml b/control/pid_longitudinal_controller/package.xml index 0c2da18f9185c..a31bca2f8a58f 100644 --- a/control/pid_longitudinal_controller/package.xml +++ b/control/pid_longitudinal_controller/package.xml @@ -18,9 +18,9 @@ autoware_cmake autoware_adapi_v1_msgs - autoware_auto_control_msgs autoware_auto_planning_msgs autoware_auto_vehicle_msgs + autoware_control_msgs diagnostic_msgs diagnostic_updater eigen diff --git a/control/pid_longitudinal_controller/src/pid_longitudinal_controller.cpp b/control/pid_longitudinal_controller/src/pid_longitudinal_controller.cpp index 23c4bcc857cd4..d970106ccfed0 100644 --- a/control/pid_longitudinal_controller/src/pid_longitudinal_controller.cpp +++ b/control/pid_longitudinal_controller/src/pid_longitudinal_controller.cpp @@ -681,13 +681,13 @@ PidLongitudinalController::Motion PidLongitudinalController::calcCtrlCmd( } // Do not use nearest_idx here -autoware_auto_control_msgs::msg::LongitudinalCommand PidLongitudinalController::createCtrlCmdMsg( +autoware_control_msgs::msg::Longitudinal PidLongitudinalController::createCtrlCmdMsg( const Motion & ctrl_cmd, const double & current_vel) { // publish control command - autoware_auto_control_msgs::msg::LongitudinalCommand cmd{}; + autoware_control_msgs::msg::Longitudinal cmd{}; cmd.stamp = node_->now(); - cmd.speed = static_cast(ctrl_cmd.vel); + cmd.velocity = static_cast(ctrl_cmd.vel); cmd.acceleration = static_cast(ctrl_cmd.acc); // store current velocity history @@ -784,7 +784,7 @@ void PidLongitudinalController::storeAccelCmd(const double accel) { if (m_control_state == ControlState::DRIVE) { // convert format - autoware_auto_control_msgs::msg::LongitudinalCommand cmd; + autoware_control_msgs::msg::Longitudinal cmd; cmd.stamp = node_->now(); cmd.acceleration = static_cast(accel); diff --git a/control/pure_pursuit/include/pure_pursuit/pure_pursuit_lateral_controller.hpp b/control/pure_pursuit/include/pure_pursuit/pure_pursuit_lateral_controller.hpp index e9e57cf08bcc4..dfd85d84e1995 100644 --- a/control/pure_pursuit/include/pure_pursuit/pure_pursuit_lateral_controller.hpp +++ b/control/pure_pursuit/include/pure_pursuit/pure_pursuit_lateral_controller.hpp @@ -41,8 +41,8 @@ #include #include -#include "autoware_auto_control_msgs/msg/ackermann_lateral_command.hpp" #include "autoware_auto_planning_msgs/msg/trajectory.hpp" +#include "autoware_control_msgs/msg/lateral.hpp" #include "geometry_msgs/msg/pose_stamped.hpp" #include "geometry_msgs/msg/twist_stamped.hpp" #include "nav_msgs/msg/odometry.hpp" @@ -56,9 +56,9 @@ using autoware::motion::control::trajectory_follower::InputData; using autoware::motion::control::trajectory_follower::LateralControllerBase; using autoware::motion::control::trajectory_follower::LateralOutput; -using autoware_auto_control_msgs::msg::AckermannLateralCommand; using autoware_auto_planning_msgs::msg::Trajectory; using autoware_auto_planning_msgs::msg::TrajectoryPoint; +using autoware_control_msgs::msg::Lateral; namespace pure_pursuit { @@ -109,7 +109,7 @@ class PurePursuitLateralController : public LateralControllerBase autoware_auto_planning_msgs::msg::Trajectory trajectory_; nav_msgs::msg::Odometry current_odometry_; autoware_auto_vehicle_msgs::msg::SteeringReport current_steering_; - boost::optional prev_cmd_; + boost::optional prev_cmd_; // Debug Publisher rclcpp::Publisher::SharedPtr pub_debug_marker_; @@ -137,7 +137,7 @@ class PurePursuitLateralController : public LateralControllerBase bool isReady([[maybe_unused]] const InputData & input_data) override; LateralOutput run(const InputData & input_data) override; - AckermannLateralCommand generateCtrlCmdMsg(const double target_curvature); + Lateral generateCtrlCmdMsg(const double target_curvature); // Parameter Param param_{}; @@ -153,14 +153,13 @@ class PurePursuitLateralController : public LateralControllerBase * of vehicle. */ - TrajectoryPoint calcNextPose( - const double ds, TrajectoryPoint & point, AckermannLateralCommand cmd) const; + TrajectoryPoint calcNextPose(const double ds, TrajectoryPoint & point, Lateral cmd) const; boost::optional generatePredictedTrajectory(); - AckermannLateralCommand generateOutputControlCmd(); + Lateral generateOutputControlCmd(); - bool calcIsSteerConverged(const AckermannLateralCommand & cmd); + bool calcIsSteerConverged(const Lateral & cmd); double calcLookaheadDistance( const double lateral_error, const double curvature, const double velocity, const double min_ld, diff --git a/control/pure_pursuit/include/pure_pursuit/pure_pursuit_node.hpp b/control/pure_pursuit/include/pure_pursuit/pure_pursuit_node.hpp index 6d9fca4b852cf..569a52e5612be 100644 --- a/control/pure_pursuit/include/pure_pursuit/pure_pursuit_node.hpp +++ b/control/pure_pursuit/include/pure_pursuit/pure_pursuit_node.hpp @@ -36,8 +36,8 @@ #include #include -#include #include +#include #include #include #include @@ -96,8 +96,7 @@ class PurePursuitNode : public rclcpp::Node geometry_msgs::msg::PoseStamped::ConstSharedPtr current_pose_; // Publisher - rclcpp::Publisher::SharedPtr - pub_ctrl_cmd_; + rclcpp::Publisher::SharedPtr pub_ctrl_cmd_; void publishCommand(const double target_curvature); diff --git a/control/pure_pursuit/package.xml b/control/pure_pursuit/package.xml index 30356b856fbac..34077e5c8ec50 100644 --- a/control/pure_pursuit/package.xml +++ b/control/pure_pursuit/package.xml @@ -13,8 +13,8 @@ ament_cmake_auto autoware_cmake - autoware_auto_control_msgs autoware_auto_planning_msgs + autoware_control_msgs boost geometry_msgs libboost-dev diff --git a/control/pure_pursuit/src/pure_pursuit/pure_pursuit_lateral_controller.cpp b/control/pure_pursuit/src/pure_pursuit/pure_pursuit_lateral_controller.cpp index cb812202d8651..c689706f7cbbd 100644 --- a/control/pure_pursuit/src/pure_pursuit/pure_pursuit_lateral_controller.cpp +++ b/control/pure_pursuit/src/pure_pursuit/pure_pursuit_lateral_controller.cpp @@ -140,7 +140,7 @@ double PurePursuitLateralController::calcLookaheadDistance( } TrajectoryPoint PurePursuitLateralController::calcNextPose( - const double ds, TrajectoryPoint & point, AckermannLateralCommand cmd) const + const double ds, TrajectoryPoint & point, Lateral cmd) const { geometry_msgs::msg::Transform transform; transform.translation = tier4_autoware_utils::createTranslation(ds, 0.0, 0.0); @@ -297,7 +297,7 @@ boost::optional PurePursuitLateralController::generatePredictedTraje predicted_trajectory.points.push_back(p); const auto pp_output = calcTargetCurvature(true, predicted_trajectory.points.at(i).pose); - AckermannLateralCommand tmp_msg; + Lateral tmp_msg; if (pp_output) { tmp_msg = generateCtrlCmdMsg(pp_output->curvature); @@ -314,7 +314,7 @@ boost::optional PurePursuitLateralController::generatePredictedTraje } else { const auto pp_output = calcTargetCurvature(false, predicted_trajectory.points.at(i).pose); - AckermannLateralCommand tmp_msg; + Lateral tmp_msg; if (pp_output) { tmp_msg = generateCtrlCmdMsg(pp_output->curvature); @@ -371,21 +371,21 @@ LateralOutput PurePursuitLateralController::run(const InputData & input_data) return output; } -bool PurePursuitLateralController::calcIsSteerConverged(const AckermannLateralCommand & cmd) +bool PurePursuitLateralController::calcIsSteerConverged(const Lateral & cmd) { return std::abs(cmd.steering_tire_angle - current_steering_.steering_tire_angle) < static_cast(param_.converged_steer_rad_); } -AckermannLateralCommand PurePursuitLateralController::generateOutputControlCmd() +Lateral PurePursuitLateralController::generateOutputControlCmd() { // Generate the control command const auto pp_output = calcTargetCurvature(true, current_odometry_.pose.pose); - AckermannLateralCommand output_cmd; + Lateral output_cmd; if (pp_output) { output_cmd = generateCtrlCmdMsg(pp_output->curvature); - prev_cmd_ = boost::optional(output_cmd); + prev_cmd_ = boost::optional(output_cmd); publishDebugMarker(); } else { RCLCPP_WARN_THROTTLE( @@ -400,12 +400,11 @@ AckermannLateralCommand PurePursuitLateralController::generateOutputControlCmd() return output_cmd; } -AckermannLateralCommand PurePursuitLateralController::generateCtrlCmdMsg( - const double target_curvature) +Lateral PurePursuitLateralController::generateCtrlCmdMsg(const double target_curvature) { const double tmp_steering = planning_utils::convertCurvatureToSteeringAngle(param_.wheel_base, target_curvature); - AckermannLateralCommand cmd; + Lateral cmd; cmd.stamp = node_->get_clock()->now(); cmd.steering_tire_angle = static_cast( std::min(std::max(tmp_steering, -param_.max_steering_angle), param_.max_steering_angle)); diff --git a/control/pure_pursuit/src/pure_pursuit/pure_pursuit_node.cpp b/control/pure_pursuit/src/pure_pursuit/pure_pursuit_node.cpp index 254b83bccbc34..34303669ec09f 100644 --- a/control/pure_pursuit/src/pure_pursuit/pure_pursuit_node.cpp +++ b/control/pure_pursuit/src/pure_pursuit/pure_pursuit_node.cpp @@ -79,8 +79,8 @@ PurePursuitNode::PurePursuitNode(const rclcpp::NodeOptions & node_options) "input/current_odometry", 1, std::bind(&PurePursuitNode::onCurrentOdometry, this, _1)); // Publishers - pub_ctrl_cmd_ = this->create_publisher( - "output/control_raw", 1); + pub_ctrl_cmd_ = + this->create_publisher("output/control_raw", 1); // Debug Publishers pub_debug_marker_ = @@ -150,7 +150,7 @@ void PurePursuitNode::onTimer() void PurePursuitNode::publishCommand(const double target_curvature) { - autoware_auto_control_msgs::msg::AckermannLateralCommand cmd; + autoware_control_msgs::msg::Lateral cmd; cmd.stamp = get_clock()->now(); cmd.steering_tire_angle = planning_utils::convertCurvatureToSteeringAngle(param_.wheel_base, target_curvature); diff --git a/control/shift_decider/README.md b/control/shift_decider/README.md index 820432e4c9e1a..b13ade2103a3c 100644 --- a/control/shift_decider/README.md +++ b/control/shift_decider/README.md @@ -37,9 +37,9 @@ stop ### Input -| Name | Type | Description | -| --------------------- | ---------------------------------------------------------- | ---------------------------- | -| `~/input/control_cmd` | `autoware_auto_control_msgs::msg::AckermannControlCommand` | Control command for vehicle. | +| Name | Type | Description | +| --------------------- | ------------------------------------- | ---------------------------- | +| `~/input/control_cmd` | `autoware_control_msgs::msg::Control` | Control command for vehicle. | ### Output diff --git a/control/shift_decider/include/shift_decider/shift_decider.hpp b/control/shift_decider/include/shift_decider/shift_decider.hpp index d260376e55aa2..b96b4fde1e859 100644 --- a/control/shift_decider/include/shift_decider/shift_decider.hpp +++ b/control/shift_decider/include/shift_decider/shift_decider.hpp @@ -17,10 +17,10 @@ #include -#include #include #include #include +#include #include @@ -31,22 +31,21 @@ class ShiftDecider : public rclcpp::Node private: void onTimer(); - void onControlCmd(autoware_auto_control_msgs::msg::AckermannControlCommand::SharedPtr msg); + void onControlCmd(autoware_control_msgs::msg::Control::SharedPtr msg); void onAutowareState(autoware_auto_system_msgs::msg::AutowareState::SharedPtr msg); void onCurrentGear(autoware_auto_vehicle_msgs::msg::GearReport::SharedPtr msg); void updateCurrentShiftCmd(); void initTimer(double period_s); rclcpp::Publisher::SharedPtr pub_shift_cmd_; - rclcpp::Subscription::SharedPtr - sub_control_cmd_; + rclcpp::Subscription::SharedPtr sub_control_cmd_; rclcpp::Subscription::SharedPtr sub_autoware_state_; rclcpp::Subscription::SharedPtr sub_current_gear_; rclcpp::TimerBase::SharedPtr timer_; - autoware_auto_control_msgs::msg::AckermannControlCommand::SharedPtr control_cmd_; + autoware_control_msgs::msg::Control::SharedPtr control_cmd_; autoware_auto_system_msgs::msg::AutowareState::SharedPtr autoware_state_; autoware_auto_vehicle_msgs::msg::GearCommand shift_cmd_; autoware_auto_vehicle_msgs::msg::GearReport::SharedPtr current_gear_ptr_; diff --git a/control/shift_decider/package.xml b/control/shift_decider/package.xml index b24dbab1786e1..720d7415c02b9 100644 --- a/control/shift_decider/package.xml +++ b/control/shift_decider/package.xml @@ -12,9 +12,9 @@ ament_cmake autoware_cmake - autoware_auto_control_msgs autoware_auto_system_msgs autoware_auto_vehicle_msgs + autoware_control_msgs rclcpp rclcpp_components diff --git a/control/shift_decider/src/shift_decider.cpp b/control/shift_decider/src/shift_decider.cpp index 6f24578bf8d8e..396766b3be94c 100644 --- a/control/shift_decider/src/shift_decider.cpp +++ b/control/shift_decider/src/shift_decider.cpp @@ -34,7 +34,7 @@ ShiftDecider::ShiftDecider(const rclcpp::NodeOptions & node_options) pub_shift_cmd_ = create_publisher("output/gear_cmd", durable_qos); - sub_control_cmd_ = create_subscription( + sub_control_cmd_ = create_subscription( "input/control_cmd", queue_size, std::bind(&ShiftDecider::onControlCmd, this, _1)); sub_autoware_state_ = create_subscription( "input/state", queue_size, std::bind(&ShiftDecider::onAutowareState, this, _1)); @@ -44,8 +44,7 @@ ShiftDecider::ShiftDecider(const rclcpp::NodeOptions & node_options) initTimer(0.1); } -void ShiftDecider::onControlCmd( - autoware_auto_control_msgs::msg::AckermannControlCommand::SharedPtr msg) +void ShiftDecider::onControlCmd(autoware_control_msgs::msg::Control::SharedPtr msg) { control_cmd_ = msg; } @@ -78,9 +77,9 @@ void ShiftDecider::updateCurrentShiftCmd() shift_cmd_.stamp = now(); static constexpr double vel_threshold = 0.01; // to prevent chattering if (autoware_state_->state == AutowareState::DRIVING) { - if (control_cmd_->longitudinal.speed > vel_threshold) { + if (control_cmd_->longitudinal.velocity > vel_threshold) { shift_cmd_.command = GearCommand::DRIVE; - } else if (control_cmd_->longitudinal.speed < -vel_threshold) { + } else if (control_cmd_->longitudinal.velocity < -vel_threshold) { shift_cmd_.command = GearCommand::REVERSE; } else { shift_cmd_.command = current_gear_ptr_->report; diff --git a/control/trajectory_follower_base/include/trajectory_follower_base/lateral_controller_base.hpp b/control/trajectory_follower_base/include/trajectory_follower_base/lateral_controller_base.hpp index 657c981414c32..a70c4c18fedb3 100644 --- a/control/trajectory_follower_base/include/trajectory_follower_base/lateral_controller_base.hpp +++ b/control/trajectory_follower_base/include/trajectory_follower_base/lateral_controller_base.hpp @@ -19,7 +19,7 @@ #include "trajectory_follower_base/input_data.hpp" #include "trajectory_follower_base/sync_data.hpp" -#include "autoware_auto_control_msgs/msg/ackermann_lateral_command.hpp" +#include "autoware_control_msgs/msg/lateral.hpp" #include @@ -27,7 +27,7 @@ namespace autoware::motion::control::trajectory_follower { struct LateralOutput { - autoware_auto_control_msgs::msg::AckermannLateralCommand control_cmd; + autoware_control_msgs::msg::Lateral control_cmd; LateralSyncData sync_data; }; diff --git a/control/trajectory_follower_base/include/trajectory_follower_base/longitudinal_controller_base.hpp b/control/trajectory_follower_base/include/trajectory_follower_base/longitudinal_controller_base.hpp index 0f9c0d57bb5cd..da5381091113f 100644 --- a/control/trajectory_follower_base/include/trajectory_follower_base/longitudinal_controller_base.hpp +++ b/control/trajectory_follower_base/include/trajectory_follower_base/longitudinal_controller_base.hpp @@ -19,7 +19,7 @@ #include "trajectory_follower_base/input_data.hpp" #include "trajectory_follower_base/sync_data.hpp" -#include "autoware_auto_control_msgs/msg/longitudinal_command.hpp" +#include "autoware_control_msgs/msg/longitudinal.hpp" #include @@ -27,7 +27,7 @@ namespace autoware::motion::control::trajectory_follower { struct LongitudinalOutput { - autoware_auto_control_msgs::msg::LongitudinalCommand control_cmd; + autoware_control_msgs::msg::Longitudinal control_cmd; LongitudinalSyncData sync_data; }; class LongitudinalControllerBase diff --git a/control/trajectory_follower_base/package.xml b/control/trajectory_follower_base/package.xml index d896f874a3a20..cedb75a6fe9c2 100644 --- a/control/trajectory_follower_base/package.xml +++ b/control/trajectory_follower_base/package.xml @@ -20,9 +20,9 @@ autoware_cmake autoware_adapi_v1_msgs - autoware_auto_control_msgs autoware_auto_planning_msgs autoware_auto_vehicle_msgs + autoware_control_msgs diagnostic_msgs diagnostic_updater eigen diff --git a/control/trajectory_follower_node/design/simple_trajectory_follower-design.md b/control/trajectory_follower_node/design/simple_trajectory_follower-design.md index 24ffe3166bbe4..7fc644e208ebd 100644 --- a/control/trajectory_follower_node/design/simple_trajectory_follower-design.md +++ b/control/trajectory_follower_node/design/simple_trajectory_follower-design.md @@ -13,7 +13,7 @@ Inputs - `input/reference_trajectory` [autoware_auto_planning_msgs::msg::Trajectory] : reference trajectory to follow. - `input/current_kinematic_state` [nav_msgs::msg::Odometry] : current state of the vehicle (position, velocity, etc). - Output -- `output/control_cmd` [autoware_auto_control_msgs::msg::AckermannControlCommand] : generated control command. +- `output/control_cmd` [autoware_control_msgs::msg::Control] : generated control command. ### Parameters diff --git a/control/trajectory_follower_node/include/trajectory_follower_node/controller_node.hpp b/control/trajectory_follower_node/include/trajectory_follower_node/controller_node.hpp index 4868fa7a6f51d..200577523ebc5 100644 --- a/control/trajectory_follower_node/include/trajectory_follower_node/controller_node.hpp +++ b/control/trajectory_follower_node/include/trajectory_follower_node/controller_node.hpp @@ -27,10 +27,10 @@ #include #include -#include "autoware_auto_control_msgs/msg/ackermann_control_command.hpp" -#include "autoware_auto_control_msgs/msg/longitudinal_command.hpp" #include "autoware_auto_planning_msgs/msg/trajectory.hpp" #include "autoware_auto_vehicle_msgs/msg/vehicle_odometry.hpp" +#include "autoware_control_msgs/msg/control.hpp" +#include "autoware_control_msgs/msg/longitudinal.hpp" #include "geometry_msgs/msg/accel_stamped.hpp" #include "geometry_msgs/msg/accel_with_covariance_stamped.hpp" #include "geometry_msgs/msg/pose_stamped.hpp" @@ -74,8 +74,7 @@ class TRAJECTORY_FOLLOWER_PUBLIC Controller : public rclcpp::Node rclcpp::Subscription::SharedPtr sub_steering_; rclcpp::Subscription::SharedPtr sub_accel_; rclcpp::Subscription::SharedPtr sub_operation_mode_; - rclcpp::Publisher::SharedPtr - control_cmd_pub_; + rclcpp::Publisher::SharedPtr control_cmd_pub_; rclcpp::Publisher::SharedPtr debug_marker_pub_; autoware_auto_planning_msgs::msg::Trajectory::SharedPtr current_trajectory_ptr_; diff --git a/control/trajectory_follower_node/include/trajectory_follower_node/simple_trajectory_follower.hpp b/control/trajectory_follower_node/include/trajectory_follower_node/simple_trajectory_follower.hpp index e748bdf25d419..0add930b409b5 100644 --- a/control/trajectory_follower_node/include/trajectory_follower_node/simple_trajectory_follower.hpp +++ b/control/trajectory_follower_node/include/trajectory_follower_node/simple_trajectory_follower.hpp @@ -17,9 +17,9 @@ #include -#include #include #include +#include #include #include #include @@ -28,9 +28,9 @@ namespace simple_trajectory_follower { -using autoware_auto_control_msgs::msg::AckermannControlCommand; using autoware_auto_planning_msgs::msg::Trajectory; using autoware_auto_planning_msgs::msg::TrajectoryPoint; +using autoware_control_msgs::msg::Control; using geometry_msgs::msg::Pose; using geometry_msgs::msg::Twist; using nav_msgs::msg::Odometry; @@ -44,7 +44,7 @@ class SimpleTrajectoryFollower : public rclcpp::Node private: rclcpp::Subscription::SharedPtr sub_kinematics_; rclcpp::Subscription::SharedPtr sub_trajectory_; - rclcpp::Publisher::SharedPtr pub_cmd_; + rclcpp::Publisher::SharedPtr pub_cmd_; rclcpp::TimerBase::SharedPtr timer_; Trajectory::SharedPtr trajectory_; diff --git a/control/trajectory_follower_node/package.xml b/control/trajectory_follower_node/package.xml index 8673634e24058..efb630e07fadf 100644 --- a/control/trajectory_follower_node/package.xml +++ b/control/trajectory_follower_node/package.xml @@ -20,10 +20,10 @@ autoware_cmake autoware_adapi_v1_msgs - autoware_auto_control_msgs autoware_auto_planning_msgs autoware_auto_system_msgs autoware_auto_vehicle_msgs + autoware_control_msgs motion_utils mpc_lateral_controller pid_longitudinal_controller diff --git a/control/trajectory_follower_node/src/controller_node.cpp b/control/trajectory_follower_node/src/controller_node.cpp index 5ee9f7f4f1e71..3124514f2014f 100644 --- a/control/trajectory_follower_node/src/controller_node.cpp +++ b/control/trajectory_follower_node/src/controller_node.cpp @@ -72,7 +72,7 @@ Controller::Controller(const rclcpp::NodeOptions & node_options) : Node("control sub_operation_mode_ = create_subscription( "~/input/current_operation_mode", rclcpp::QoS{1}, [this](const OperationModeState::SharedPtr msg) { current_operation_mode_ptr_ = msg; }); - control_cmd_pub_ = create_publisher( + control_cmd_pub_ = create_publisher( "~/output/control_cmd", rclcpp::QoS{1}.transient_local()); debug_marker_pub_ = create_publisher("~/output/debug_marker", rclcpp::QoS{1}); @@ -213,7 +213,7 @@ void Controller::callbackTimerControl() if (isTimeOut(lon_out, lat_out)) return; // 5. publish control command - autoware_auto_control_msgs::msg::AckermannControlCommand out; + autoware_control_msgs::msg::Control out; out.stamp = this->now(); out.lateral = lat_out.control_cmd; out.longitudinal = lon_out.control_cmd; diff --git a/control/trajectory_follower_node/src/simple_trajectory_follower.cpp b/control/trajectory_follower_node/src/simple_trajectory_follower.cpp index f073574cc229f..6953c703fc136 100644 --- a/control/trajectory_follower_node/src/simple_trajectory_follower.cpp +++ b/control/trajectory_follower_node/src/simple_trajectory_follower.cpp @@ -29,7 +29,7 @@ using tier4_autoware_utils::calcYawDeviation; SimpleTrajectoryFollower::SimpleTrajectoryFollower(const rclcpp::NodeOptions & options) : Node("simple_trajectory_follower", options) { - pub_cmd_ = create_publisher("output/control_cmd", 1); + pub_cmd_ = create_publisher("output/control_cmd", 1); sub_kinematics_ = create_subscription( "input/kinematics", 1, [this](const Odometry::SharedPtr msg) { odometry_ = msg; }); @@ -54,11 +54,12 @@ void SimpleTrajectoryFollower::onTimer() updateClosest(); - AckermannControlCommand cmd; + Control cmd; cmd.stamp = cmd.lateral.stamp = cmd.longitudinal.stamp = get_clock()->now(); cmd.lateral.steering_tire_angle = static_cast(calcSteerCmd()); - cmd.longitudinal.speed = use_external_target_vel_ ? static_cast(external_target_vel_) - : closest_traj_point_.longitudinal_velocity_mps; + cmd.longitudinal.velocity = use_external_target_vel_ + ? static_cast(external_target_vel_) + : closest_traj_point_.longitudinal_velocity_mps; cmd.longitudinal.acceleration = static_cast(calcAccCmd()); pub_cmd_->publish(cmd); } diff --git a/control/trajectory_follower_node/test/test_controller_node.cpp b/control/trajectory_follower_node/test/test_controller_node.cpp index 6be3625501590..484d532a9d925 100644 --- a/control/trajectory_follower_node/test/test_controller_node.cpp +++ b/control/trajectory_follower_node/test/test_controller_node.cpp @@ -21,10 +21,10 @@ #include "trajectory_follower_test_utils.hpp" #include "autoware_adapi_v1_msgs/msg/operation_mode_state.hpp" -#include "autoware_auto_control_msgs/msg/ackermann_lateral_command.hpp" #include "autoware_auto_planning_msgs/msg/trajectory.hpp" #include "autoware_auto_vehicle_msgs/msg/steering_report.hpp" #include "autoware_auto_vehicle_msgs/msg/vehicle_odometry.hpp" +#include "autoware_control_msgs/msg/lateral.hpp" #include "geometry_msgs/msg/accel_with_covariance_stamped.hpp" #include "geometry_msgs/msg/pose.hpp" #include "geometry_msgs/msg/pose_stamped.hpp" @@ -35,7 +35,7 @@ #include using Controller = autoware::motion::control::trajectory_follower_node::Controller; -using AckermannControlCommand = autoware_auto_control_msgs::msg::AckermannControlCommand; +using Control = autoware_control_msgs::msg::Control; using Trajectory = autoware_auto_planning_msgs::msg::Trajectory; using TrajectoryPoint = autoware_auto_planning_msgs::msg::TrajectoryPoint; using VehicleOdometry = nav_msgs::msg::Odometry; @@ -98,7 +98,7 @@ class ControllerTester FakeNodeFixture * fnf; std::shared_ptr node; - AckermannControlCommand::SharedPtr cmd_msg; + Control::SharedPtr cmd_msg; bool received_control_command = false; void publish_default_odom() @@ -180,13 +180,11 @@ class ControllerTester rclcpp::Publisher::SharedPtr operation_mode_pub = fnf->create_publisher("controller/input/current_operation_mode"); - rclcpp::Subscription::SharedPtr cmd_sub = - fnf->create_subscription( - "controller/output/control_cmd", *fnf->get_fake_node(), - [this](const AckermannControlCommand::SharedPtr msg) { - cmd_msg = msg; - received_control_command = true; - }); + rclcpp::Subscription::SharedPtr cmd_sub = fnf->create_subscription( + "controller/output/control_cmd", *fnf->get_fake_node(), [this](const Control::SharedPtr msg) { + cmd_msg = msg; + received_control_command = true; + }); std::shared_ptr br = std::make_shared(fnf->get_fake_node()); @@ -369,14 +367,14 @@ TEST_F(FakeNodeFixture, longitudinal_keep_velocity) test_utils::waitForMessage(tester.node, this, tester.received_control_command); ASSERT_TRUE(tester.received_control_command); - EXPECT_DOUBLE_EQ(tester.cmd_msg->longitudinal.speed, 1.0); + EXPECT_DOUBLE_EQ(tester.cmd_msg->longitudinal.velocity, 1.0); EXPECT_DOUBLE_EQ(tester.cmd_msg->longitudinal.acceleration, 0.0); // Generate another control message tester.traj_pub->publish(traj_msg); test_utils::waitForMessage(tester.node, this, tester.received_control_command); ASSERT_TRUE(tester.received_control_command); - EXPECT_DOUBLE_EQ(tester.cmd_msg->longitudinal.speed, 1.0); + EXPECT_DOUBLE_EQ(tester.cmd_msg->longitudinal.velocity, 1.0); EXPECT_DOUBLE_EQ(tester.cmd_msg->longitudinal.acceleration, 0.0); } @@ -406,14 +404,14 @@ TEST_F(FakeNodeFixture, longitudinal_slow_down) test_utils::waitForMessage(tester.node, this, tester.received_control_command); ASSERT_TRUE(tester.received_control_command); - EXPECT_LT(tester.cmd_msg->longitudinal.speed, static_cast(odom_vx)); + EXPECT_LT(tester.cmd_msg->longitudinal.velocity, static_cast(odom_vx)); EXPECT_LT(tester.cmd_msg->longitudinal.acceleration, 0.0f); // Generate another control message tester.traj_pub->publish(traj); test_utils::waitForMessage(tester.node, this, tester.received_control_command); ASSERT_TRUE(tester.received_control_command); - EXPECT_LT(tester.cmd_msg->longitudinal.speed, static_cast(odom_vx)); + EXPECT_LT(tester.cmd_msg->longitudinal.velocity, static_cast(odom_vx)); EXPECT_LT(tester.cmd_msg->longitudinal.acceleration, 0.0f); } @@ -443,14 +441,14 @@ TEST_F(FakeNodeFixture, longitudinal_accelerate) test_utils::waitForMessage(tester.node, this, tester.received_control_command); ASSERT_TRUE(tester.received_control_command); - EXPECT_GT(tester.cmd_msg->longitudinal.speed, static_cast(odom_vx)); + EXPECT_GT(tester.cmd_msg->longitudinal.velocity, static_cast(odom_vx)); EXPECT_GT(tester.cmd_msg->longitudinal.acceleration, 0.0f); // Generate another control message tester.traj_pub->publish(traj); test_utils::waitForMessage(tester.node, this, tester.received_control_command); ASSERT_TRUE(tester.received_control_command); - EXPECT_GT(tester.cmd_msg->longitudinal.speed, static_cast(odom_vx)); + EXPECT_GT(tester.cmd_msg->longitudinal.velocity, static_cast(odom_vx)); EXPECT_GT(tester.cmd_msg->longitudinal.acceleration, 0.0f); } @@ -477,7 +475,7 @@ TEST_F(FakeNodeFixture, longitudinal_stopped) test_utils::waitForMessage(tester.node, this, tester.received_control_command); ASSERT_TRUE(tester.received_control_command); - EXPECT_DOUBLE_EQ(tester.cmd_msg->longitudinal.speed, 0.0f); + EXPECT_DOUBLE_EQ(tester.cmd_msg->longitudinal.velocity, 0.0f); EXPECT_LT( tester.cmd_msg->longitudinal.acceleration, 0.0f); // when stopped negative acceleration to brake @@ -507,7 +505,7 @@ TEST_F(FakeNodeFixture, longitudinal_reverse) test_utils::waitForMessage(tester.node, this, tester.received_control_command); ASSERT_TRUE(tester.received_control_command); - EXPECT_LT(tester.cmd_msg->longitudinal.speed, 0.0f); + EXPECT_LT(tester.cmd_msg->longitudinal.velocity, 0.0f); EXPECT_GT(tester.cmd_msg->longitudinal.acceleration, 0.0f); } @@ -535,7 +533,7 @@ TEST_F(FakeNodeFixture, longitudinal_emergency) ASSERT_TRUE(tester.received_control_command); // Emergencies (e.g., far from trajectory) produces braking command (0 vel, negative accel) - EXPECT_DOUBLE_EQ(tester.cmd_msg->longitudinal.speed, 0.0f); + EXPECT_DOUBLE_EQ(tester.cmd_msg->longitudinal.velocity, 0.0f); EXPECT_LT(tester.cmd_msg->longitudinal.acceleration, 0.0f); } @@ -566,7 +564,7 @@ TEST_F(FakeNodeFixture, longitudinal_not_check_steer_converged) ASSERT_TRUE(tester.received_control_command); // Not keep stopped state when the lateral control is not converged. - EXPECT_DOUBLE_EQ(tester.cmd_msg->longitudinal.speed, 1.0f); + EXPECT_DOUBLE_EQ(tester.cmd_msg->longitudinal.velocity, 1.0f); } TEST_F(FakeNodeFixture, longitudinal_check_steer_converged) @@ -597,5 +595,5 @@ TEST_F(FakeNodeFixture, longitudinal_check_steer_converged) ASSERT_TRUE(tester.received_control_command); // Keep stopped state when the lateral control is not converged. - EXPECT_DOUBLE_EQ(tester.cmd_msg->longitudinal.speed, 0.0f); + EXPECT_DOUBLE_EQ(tester.cmd_msg->longitudinal.velocity, 0.0f); } diff --git a/control/trajectory_follower_node/test/test_lateral_controller_node.cpp b/control/trajectory_follower_node/test/test_lateral_controller_node.cpp index 0c0a1f84ab1c2..0bb8405afbc6f 100644 --- a/control/trajectory_follower_node/test/test_lateral_controller_node.cpp +++ b/control/trajectory_follower_node/test/test_lateral_controller_node.cpp @@ -21,10 +21,10 @@ #include -#include "autoware_auto_control_msgs/msg/ackermann_lateral_command.hpp" #include "autoware_auto_planning_msgs/msg/trajectory.hpp" #include "autoware_auto_vehicle_msgs/msg/steering_report.hpp" #include "autoware_auto_vehicle_msgs/msg/vehicle_odometry.hpp" +#include "autoware_control_msgs/msg/lateral.hpp" #include "geometry_msgs/msg/pose.hpp" #include "geometry_msgs/msg/pose_stamped.hpp" #include "geometry_msgs/msg/transform_stamped.hpp" @@ -34,7 +34,7 @@ #include using LateralController = autoware::motion::control::trajectory_follower_node::LateralController; -using LateralCommand = autoware_auto_control_msgs::msg::AckermannLateralCommand; +using LateralCommand = autoware_control_msgs::msg::Lateral; using Trajectory = autoware_auto_planning_msgs::msg::Trajectory; using TrajectoryPoint = autoware_auto_planning_msgs::msg::TrajectoryPoint; using VehicleOdometry = nav_msgs::msg::Odometry; diff --git a/control/trajectory_follower_node/test/test_longitudinal_controller_node.cpp b/control/trajectory_follower_node/test/test_longitudinal_controller_node.cpp index badafd5f2fd15..7ae1491c87bc3 100644 --- a/control/trajectory_follower_node/test/test_longitudinal_controller_node.cpp +++ b/control/trajectory_follower_node/test/test_longitudinal_controller_node.cpp @@ -21,9 +21,9 @@ #include -#include "autoware_auto_control_msgs/msg/longitudinal_command.hpp" #include "autoware_auto_planning_msgs/msg/trajectory.hpp" #include "autoware_auto_vehicle_msgs/msg/vehicle_odometry.hpp" +#include "autoware_control_msgs/msg/longitudinal.hpp" #include "geometry_msgs/msg/pose.hpp" #include "geometry_msgs/msg/pose_stamped.hpp" #include "geometry_msgs/msg/transform_stamped.hpp" @@ -33,7 +33,7 @@ using LongitudinalController = autoware::motion::control::trajectory_follower_node::LongitudinalController; -using LongitudinalCommand = autoware_auto_control_msgs::msg::LongitudinalCommand; +using Longitudinal = autoware_control_msgs::msg::Longitudinal; using Trajectory = autoware_auto_planning_msgs::msg::Trajectory; using TrajectoryPoint = autoware_auto_planning_msgs::msg::TrajectoryPoint; using VehicleOdometry = nav_msgs::msg::Odometry; @@ -65,7 +65,7 @@ std::shared_ptr makeLongitudinalNode() TEST_F(FakeNodeFixture, longitudinal_keep_velocity) { // Data to test - LongitudinalCommand::SharedPtr cmd_msg; + Longitudinal::SharedPtr cmd_msg; bool received_longitudinal_command = false; // Node @@ -76,13 +76,12 @@ TEST_F(FakeNodeFixture, longitudinal_keep_velocity) this->create_publisher("longitudinal_controller/input/current_odometry"); rclcpp::Publisher::SharedPtr traj_pub = this->create_publisher("longitudinal_controller/input/current_trajectory"); - rclcpp::Subscription::SharedPtr cmd_sub = - this->create_subscription( - "longitudinal_controller/output/control_cmd", *this->get_fake_node(), - [&cmd_msg, &received_longitudinal_command](const LongitudinalCommand::SharedPtr msg) { - cmd_msg = msg; - received_longitudinal_command = true; - }); + rclcpp::Subscription::SharedPtr cmd_sub = this->create_subscription( + "longitudinal_controller/output/control_cmd", *this->get_fake_node(), + [&cmd_msg, &received_longitudinal_command](const Longitudinal::SharedPtr msg) { + cmd_msg = msg; + received_longitudinal_command = true; + }); std::shared_ptr br = std::make_shared(this->get_fake_node()); @@ -122,7 +121,7 @@ TEST_F(FakeNodeFixture, longitudinal_keep_velocity) test_utils::waitForMessage(node, this, received_longitudinal_command); ASSERT_TRUE(received_longitudinal_command); - EXPECT_DOUBLE_EQ(cmd_msg->speed, 1.0); + EXPECT_DOUBLE_EQ(cmd_msg->velocity, 1.0); EXPECT_DOUBLE_EQ(cmd_msg->acceleration, 0.0); // Generate another control message @@ -130,14 +129,14 @@ TEST_F(FakeNodeFixture, longitudinal_keep_velocity) traj_pub->publish(traj); test_utils::waitForMessage(node, this, received_longitudinal_command); ASSERT_TRUE(received_longitudinal_command); - EXPECT_DOUBLE_EQ(cmd_msg->speed, 1.0); + EXPECT_DOUBLE_EQ(cmd_msg->velocity, 1.0); EXPECT_DOUBLE_EQ(cmd_msg->acceleration, 0.0); } TEST_F(FakeNodeFixture, longitudinal_slow_down) { // Data to test - LongitudinalCommand::SharedPtr cmd_msg; + Longitudinal::SharedPtr cmd_msg; bool received_longitudinal_command = false; // Node @@ -148,13 +147,12 @@ TEST_F(FakeNodeFixture, longitudinal_slow_down) this->create_publisher("longitudinal_controller/input/current_odometry"); rclcpp::Publisher::SharedPtr traj_pub = this->create_publisher("longitudinal_controller/input/current_trajectory"); - rclcpp::Subscription::SharedPtr cmd_sub = - this->create_subscription( - "longitudinal_controller/output/control_cmd", *this->get_fake_node(), - [&cmd_msg, &received_longitudinal_command](const LongitudinalCommand::SharedPtr msg) { - cmd_msg = msg; - received_longitudinal_command = true; - }); + rclcpp::Subscription::SharedPtr cmd_sub = this->create_subscription( + "longitudinal_controller/output/control_cmd", *this->get_fake_node(), + [&cmd_msg, &received_longitudinal_command](const Longitudinal::SharedPtr msg) { + cmd_msg = msg; + received_longitudinal_command = true; + }); std::shared_ptr br = std::make_shared(this->get_fake_node()); @@ -194,7 +192,7 @@ TEST_F(FakeNodeFixture, longitudinal_slow_down) test_utils::waitForMessage(node, this, received_longitudinal_command); ASSERT_TRUE(received_longitudinal_command); - EXPECT_LT(cmd_msg->speed, static_cast(odom_msg.twist.twist.linear.x)); + EXPECT_LT(cmd_msg->velocity, static_cast(odom_msg.twist.twist.linear.x)); EXPECT_LT(cmd_msg->acceleration, 0.0f); // Generate another control message @@ -202,14 +200,14 @@ TEST_F(FakeNodeFixture, longitudinal_slow_down) traj_pub->publish(traj); test_utils::waitForMessage(node, this, received_longitudinal_command); ASSERT_TRUE(received_longitudinal_command); - EXPECT_LT(cmd_msg->speed, static_cast(odom_msg.twist.twist.linear.x)); + EXPECT_LT(cmd_msg->velocity, static_cast(odom_msg.twist.twist.linear.x)); EXPECT_LT(cmd_msg->acceleration, 0.0f); } TEST_F(FakeNodeFixture, longitudinal_accelerate) { // Data to test - LongitudinalCommand::SharedPtr cmd_msg; + Longitudinal::SharedPtr cmd_msg; bool received_longitudinal_command = false; // Node @@ -220,13 +218,12 @@ TEST_F(FakeNodeFixture, longitudinal_accelerate) this->create_publisher("longitudinal_controller/input/current_odometry"); rclcpp::Publisher::SharedPtr traj_pub = this->create_publisher("longitudinal_controller/input/current_trajectory"); - rclcpp::Subscription::SharedPtr cmd_sub = - this->create_subscription( - "longitudinal_controller/output/control_cmd", *this->get_fake_node(), - [&cmd_msg, &received_longitudinal_command](const LongitudinalCommand::SharedPtr msg) { - cmd_msg = msg; - received_longitudinal_command = true; - }); + rclcpp::Subscription::SharedPtr cmd_sub = this->create_subscription( + "longitudinal_controller/output/control_cmd", *this->get_fake_node(), + [&cmd_msg, &received_longitudinal_command](const Longitudinal::SharedPtr msg) { + cmd_msg = msg; + received_longitudinal_command = true; + }); std::shared_ptr br = std::make_shared(this->get_fake_node()); @@ -266,7 +263,7 @@ TEST_F(FakeNodeFixture, longitudinal_accelerate) test_utils::waitForMessage(node, this, received_longitudinal_command); ASSERT_TRUE(received_longitudinal_command); - EXPECT_GT(cmd_msg->speed, static_cast(odom_msg.twist.twist.linear.x)); + EXPECT_GT(cmd_msg->velocity, static_cast(odom_msg.twist.twist.linear.x)); EXPECT_GT(cmd_msg->acceleration, 0.0f); // Generate another control message @@ -274,14 +271,14 @@ TEST_F(FakeNodeFixture, longitudinal_accelerate) traj_pub->publish(traj); test_utils::waitForMessage(node, this, received_longitudinal_command); ASSERT_TRUE(received_longitudinal_command); - EXPECT_GT(cmd_msg->speed, static_cast(odom_msg.twist.twist.linear.x)); + EXPECT_GT(cmd_msg->velocity, static_cast(odom_msg.twist.twist.linear.x)); EXPECT_GT(cmd_msg->acceleration, 0.0f); } TEST_F(FakeNodeFixture, longitudinal_stopped) { // Data to test - LongitudinalCommand::SharedPtr cmd_msg; + Longitudinal::SharedPtr cmd_msg; bool received_longitudinal_command = false; // Node @@ -292,13 +289,12 @@ TEST_F(FakeNodeFixture, longitudinal_stopped) this->create_publisher("longitudinal_controller/input/current_odometry"); rclcpp::Publisher::SharedPtr traj_pub = this->create_publisher("longitudinal_controller/input/current_trajectory"); - rclcpp::Subscription::SharedPtr cmd_sub = - this->create_subscription( - "longitudinal_controller/output/control_cmd", *this->get_fake_node(), - [&cmd_msg, &received_longitudinal_command](const LongitudinalCommand::SharedPtr msg) { - cmd_msg = msg; - received_longitudinal_command = true; - }); + rclcpp::Subscription::SharedPtr cmd_sub = this->create_subscription( + "longitudinal_controller/output/control_cmd", *this->get_fake_node(), + [&cmd_msg, &received_longitudinal_command](const Longitudinal::SharedPtr msg) { + cmd_msg = msg; + received_longitudinal_command = true; + }); std::shared_ptr br = std::make_shared(this->get_fake_node()); @@ -338,14 +334,14 @@ TEST_F(FakeNodeFixture, longitudinal_stopped) test_utils::waitForMessage(node, this, received_longitudinal_command); ASSERT_TRUE(received_longitudinal_command); - EXPECT_DOUBLE_EQ(cmd_msg->speed, 0.0f); + EXPECT_DOUBLE_EQ(cmd_msg->velocity, 0.0f); EXPECT_LT(cmd_msg->acceleration, 0.0f); // when stopped negative acceleration to brake } TEST_F(FakeNodeFixture, longitudinal_reverse) { // Data to test - LongitudinalCommand::SharedPtr cmd_msg; + Longitudinal::SharedPtr cmd_msg; bool received_longitudinal_command = false; // Node @@ -356,13 +352,12 @@ TEST_F(FakeNodeFixture, longitudinal_reverse) this->create_publisher("longitudinal_controller/input/current_odometry"); rclcpp::Publisher::SharedPtr traj_pub = this->create_publisher("longitudinal_controller/input/current_trajectory"); - rclcpp::Subscription::SharedPtr cmd_sub = - this->create_subscription( - "longitudinal_controller/output/control_cmd", *this->get_fake_node(), - [&cmd_msg, &received_longitudinal_command](const LongitudinalCommand::SharedPtr msg) { - cmd_msg = msg; - received_longitudinal_command = true; - }); + rclcpp::Subscription::SharedPtr cmd_sub = this->create_subscription( + "longitudinal_controller/output/control_cmd", *this->get_fake_node(), + [&cmd_msg, &received_longitudinal_command](const Longitudinal::SharedPtr msg) { + cmd_msg = msg; + received_longitudinal_command = true; + }); std::shared_ptr br = std::make_shared(this->get_fake_node()); @@ -402,14 +397,14 @@ TEST_F(FakeNodeFixture, longitudinal_reverse) test_utils::waitForMessage(node, this, received_longitudinal_command); ASSERT_TRUE(received_longitudinal_command); - EXPECT_LT(cmd_msg->speed, 0.0f); + EXPECT_LT(cmd_msg->velocity, 0.0f); EXPECT_GT(cmd_msg->acceleration, 0.0f); } TEST_F(FakeNodeFixture, longitudinal_emergency) { // Data to test - LongitudinalCommand::SharedPtr cmd_msg; + Longitudinal::SharedPtr cmd_msg; bool received_longitudinal_command = false; // Node @@ -420,13 +415,12 @@ TEST_F(FakeNodeFixture, longitudinal_emergency) this->create_publisher("longitudinal_controller/input/current_odometry"); rclcpp::Publisher::SharedPtr traj_pub = this->create_publisher("longitudinal_controller/input/current_trajectory"); - rclcpp::Subscription::SharedPtr cmd_sub = - this->create_subscription( - "longitudinal_controller/output/control_cmd", *this->get_fake_node(), - [&cmd_msg, &received_longitudinal_command](const LongitudinalCommand::SharedPtr msg) { - cmd_msg = msg; - received_longitudinal_command = true; - }); + rclcpp::Subscription::SharedPtr cmd_sub = this->create_subscription( + "longitudinal_controller/output/control_cmd", *this->get_fake_node(), + [&cmd_msg, &received_longitudinal_command](const Longitudinal::SharedPtr msg) { + cmd_msg = msg; + received_longitudinal_command = true; + }); std::shared_ptr br = std::make_shared(this->get_fake_node()); @@ -467,6 +461,6 @@ TEST_F(FakeNodeFixture, longitudinal_emergency) ASSERT_TRUE(received_longitudinal_command); // Emergencies (e.g., far from trajectory) produces braking command (0 vel, negative accel) - EXPECT_DOUBLE_EQ(cmd_msg->speed, 0.0f); + EXPECT_DOUBLE_EQ(cmd_msg->velocity, 0.0f); EXPECT_LT(cmd_msg->acceleration, 0.0f); } diff --git a/control/vehicle_cmd_gate/README.md b/control/vehicle_cmd_gate/README.md index a8ac8c46ae782..b0e1d776f68b5 100644 --- a/control/vehicle_cmd_gate/README.md +++ b/control/vehicle_cmd_gate/README.md @@ -8,38 +8,38 @@ ### Input -| Name | Type | Description | -| ------------------------------------------- | ---------------------------------------------------------- | -------------------------------------------------------------------- | -| `~/input/steering` | `autoware_auto_vehicle_msgs::msg::SteeringReport` | steering status | -| `~/input/auto/control_cmd` | `autoware_auto_control_msgs::msg::AckermannControlCommand` | command for lateral and longitudinal velocity from planning module | -| `~/input/auto/turn_indicators_cmd` | `autoware_auto_vehicle_msgs::msg::TurnIndicatorsCommand` | turn indicators command from planning module | -| `~/input/auto/hazard_lights_cmd` | `autoware_auto_vehicle_msgs::msg::HazardLightsCommand` | hazard lights command from planning module | -| `~/input/auto/gear_cmd` | `autoware_auto_vehicle_msgs::msg::GearCommand` | gear command from planning module | -| `~/input/external/control_cmd` | `autoware_auto_control_msgs::msg::AckermannControlCommand` | command for lateral and longitudinal velocity from external | -| `~/input/external/turn_indicators_cmd` | `autoware_auto_vehicle_msgs::msg::TurnIndicatorsCommand` | turn indicators command from external | -| `~/input/external/hazard_lights_cmd` | `autoware_auto_vehicle_msgs::msg::HazardLightsCommand` | hazard lights command from external | -| `~/input/external/gear_cmd` | `autoware_auto_vehicle_msgs::msg::GearCommand` | gear command from external | -| `~/input/external_emergency_stop_heartbeat` | `tier4_external_api_msgs::msg::Heartbeat` | heartbeat | -| `~/input/gate_mode` | `tier4_control_msgs::msg::GateMode` | gate mode (AUTO or EXTERNAL) | -| `~/input/emergency/control_cmd` | `autoware_auto_control_msgs::msg::AckermannControlCommand` | command for lateral and longitudinal velocity from emergency handler | -| `~/input/emergency/hazard_lights_cmd` | `autoware_auto_vehicle_msgs::msg::HazardLightsCommand` | hazard lights command from emergency handler | -| `~/input/emergency/gear_cmd` | `autoware_auto_vehicle_msgs::msg::GearCommand` | gear command from emergency handler | -| `~/input/engage` | `autoware_auto_vehicle_msgs::msg::Engage` | engage signal | -| `~/input/operation_mode` | `autoware_adapi_v1_msgs::msg::OperationModeState` | operation mode of Autoware | +| Name | Type | Description | +| ------------------------------------------- | -------------------------------------------------------- | -------------------------------------------------------------------- | +| `~/input/steering` | `autoware_auto_vehicle_msgs::msg::SteeringReport` | steering status | +| `~/input/auto/control_cmd` | `autoware_control_msgs::msg::Control` | command for lateral and longitudinal velocity from planning module | +| `~/input/auto/turn_indicators_cmd` | `autoware_auto_vehicle_msgs::msg::TurnIndicatorsCommand` | turn indicators command from planning module | +| `~/input/auto/hazard_lights_cmd` | `autoware_auto_vehicle_msgs::msg::HazardLightsCommand` | hazard lights command from planning module | +| `~/input/auto/gear_cmd` | `autoware_auto_vehicle_msgs::msg::GearCommand` | gear command from planning module | +| `~/input/external/control_cmd` | `autoware_control_msgs::msg::Control` | command for lateral and longitudinal velocity from external | +| `~/input/external/turn_indicators_cmd` | `autoware_auto_vehicle_msgs::msg::TurnIndicatorsCommand` | turn indicators command from external | +| `~/input/external/hazard_lights_cmd` | `autoware_auto_vehicle_msgs::msg::HazardLightsCommand` | hazard lights command from external | +| `~/input/external/gear_cmd` | `autoware_auto_vehicle_msgs::msg::GearCommand` | gear command from external | +| `~/input/external_emergency_stop_heartbeat` | `tier4_external_api_msgs::msg::Heartbeat` | heartbeat | +| `~/input/gate_mode` | `tier4_control_msgs::msg::GateMode` | gate mode (AUTO or EXTERNAL) | +| `~/input/emergency/control_cmd` | `autoware_control_msgs::msg::Control` | command for lateral and longitudinal velocity from emergency handler | +| `~/input/emergency/hazard_lights_cmd` | `autoware_auto_vehicle_msgs::msg::HazardLightsCommand` | hazard lights command from emergency handler | +| `~/input/emergency/gear_cmd` | `autoware_auto_vehicle_msgs::msg::GearCommand` | gear command from emergency handler | +| `~/input/engage` | `autoware_auto_vehicle_msgs::msg::Engage` | engage signal | +| `~/input/operation_mode` | `autoware_adapi_v1_msgs::msg::OperationModeState` | operation mode of Autoware | ### Output -| Name | Type | Description | -| -------------------------------------- | ---------------------------------------------------------- | -------------------------------------------------------- | -| `~/output/vehicle_cmd_emergency` | `autoware_auto_system_msgs::msg::EmergencyState` | emergency state which was originally in vehicle command | -| `~/output/command/control_cmd` | `autoware_auto_control_msgs::msg::AckermannControlCommand` | command for lateral and longitudinal velocity to vehicle | -| `~/output/command/turn_indicators_cmd` | `autoware_auto_vehicle_msgs::msg::TurnIndicatorsCommand` | turn indicators command to vehicle | -| `~/output/command/hazard_lights_cmd` | `autoware_auto_vehicle_msgs::msg::HazardLightsCommand` | hazard lights command to vehicle | -| `~/output/command/gear_cmd` | `autoware_auto_vehicle_msgs::msg::GearCommand` | gear command to vehicle | -| `~/output/gate_mode` | `tier4_control_msgs::msg::GateMode` | gate mode (AUTO or EXTERNAL) | -| `~/output/engage` | `autoware_auto_vehicle_msgs::msg::Engage` | engage signal | -| `~/output/external_emergency` | `tier4_external_api_msgs::msg::Emergency` | external emergency signal | -| `~/output/operation_mode` | `tier4_system_msgs::msg::OperationMode` | current operation mode of the vehicle_cmd_gate | +| Name | Type | Description | +| -------------------------------------- | -------------------------------------------------------- | -------------------------------------------------------- | +| `~/output/vehicle_cmd_emergency` | `autoware_auto_system_msgs::msg::EmergencyState` | emergency state which was originally in vehicle command | +| `~/output/command/control_cmd` | `autoware_control_msgs::msg::Control` | command for lateral and longitudinal velocity to vehicle | +| `~/output/command/turn_indicators_cmd` | `autoware_auto_vehicle_msgs::msg::TurnIndicatorsCommand` | turn indicators command to vehicle | +| `~/output/command/hazard_lights_cmd` | `autoware_auto_vehicle_msgs::msg::HazardLightsCommand` | hazard lights command to vehicle | +| `~/output/command/gear_cmd` | `autoware_auto_vehicle_msgs::msg::GearCommand` | gear command to vehicle | +| `~/output/gate_mode` | `tier4_control_msgs::msg::GateMode` | gate mode (AUTO or EXTERNAL) | +| `~/output/engage` | `autoware_auto_vehicle_msgs::msg::Engage` | engage signal | +| `~/output/external_emergency` | `tier4_external_api_msgs::msg::Emergency` | external emergency signal | +| `~/output/operation_mode` | `tier4_system_msgs::msg::OperationMode` | current operation mode of the vehicle_cmd_gate | ## Parameters diff --git a/control/vehicle_cmd_gate/package.xml b/control/vehicle_cmd_gate/package.xml index df1de9f370cb6..2dd3b3ce666cc 100644 --- a/control/vehicle_cmd_gate/package.xml +++ b/control/vehicle_cmd_gate/package.xml @@ -14,9 +14,9 @@ autoware_cmake autoware_adapi_v1_msgs - autoware_auto_control_msgs autoware_auto_system_msgs autoware_auto_vehicle_msgs + autoware_control_msgs component_interface_specs component_interface_utils diagnostic_updater diff --git a/control/vehicle_cmd_gate/script/delays_checker.py b/control/vehicle_cmd_gate/script/delays_checker.py index 564b7c7ac94e5..76adac6aa81a3 100644 --- a/control/vehicle_cmd_gate/script/delays_checker.py +++ b/control/vehicle_cmd_gate/script/delays_checker.py @@ -14,8 +14,8 @@ # See the License for the specific language governing permissions and # limitations under the License. -from autoware_auto_control_msgs.msg import AckermannControlCommand from autoware_auto_vehicle_msgs.msg import Engage +from autoware_control_msgs.msg import Control from geometry_msgs.msg import AccelWithCovarianceStamped from nav_msgs.msg import Odometry import rclpy @@ -68,13 +68,13 @@ def __init__(self): ) self.sub_engage = self.create_subscription(Engage, engage_topic, self.CallBackEngage, 1) self.sub_in_gate_cmd = self.create_subscription( - AckermannControlCommand, + Control, in_gate_cmd_topic, self.CallBackInCmd, 1, ) self.sub_out_gate_cmd = self.create_subscription( - AckermannControlCommand, + Control, out_gate_cmd_topic, self.CallBackOutCmd, 1, diff --git a/control/vehicle_cmd_gate/src/adapi_pause_interface.cpp b/control/vehicle_cmd_gate/src/adapi_pause_interface.cpp index afc86f0b4fb20..f4cf28d337a09 100644 --- a/control/vehicle_cmd_gate/src/adapi_pause_interface.cpp +++ b/control/vehicle_cmd_gate/src/adapi_pause_interface.cpp @@ -53,9 +53,9 @@ void AdapiPauseInterface::publish() } } -void AdapiPauseInterface::update(const AckermannControlCommand & control) +void AdapiPauseInterface::update(const Control & control) { - is_start_requested_ = eps < std::abs(control.longitudinal.speed); + is_start_requested_ = eps < std::abs(control.longitudinal.velocity); } void AdapiPauseInterface::on_pause( diff --git a/control/vehicle_cmd_gate/src/adapi_pause_interface.hpp b/control/vehicle_cmd_gate/src/adapi_pause_interface.hpp index c4294ee5f643d..1d5f05e98975e 100644 --- a/control/vehicle_cmd_gate/src/adapi_pause_interface.hpp +++ b/control/vehicle_cmd_gate/src/adapi_pause_interface.hpp @@ -19,7 +19,7 @@ #include #include -#include +#include namespace vehicle_cmd_gate { @@ -28,7 +28,7 @@ class AdapiPauseInterface { private: static constexpr double eps = 1e-3; - using AckermannControlCommand = autoware_auto_control_msgs::msg::AckermannControlCommand; + using Control = autoware_control_msgs::msg::Control; using SetPause = control_interface::SetPause; using IsPaused = control_interface::IsPaused; using IsStartRequested = control_interface::IsStartRequested; @@ -37,7 +37,7 @@ class AdapiPauseInterface explicit AdapiPauseInterface(rclcpp::Node * node); bool is_paused(); void publish(); - void update(const AckermannControlCommand & control); + void update(const Control & control); private: bool is_paused_; diff --git a/control/vehicle_cmd_gate/src/moderate_stop_interface.hpp b/control/vehicle_cmd_gate/src/moderate_stop_interface.hpp index 7ef30b4926b7e..8d5164ed887a5 100644 --- a/control/vehicle_cmd_gate/src/moderate_stop_interface.hpp +++ b/control/vehicle_cmd_gate/src/moderate_stop_interface.hpp @@ -19,7 +19,7 @@ #include #include -#include +#include #include #include diff --git a/control/vehicle_cmd_gate/src/vehicle_cmd_filter.cpp b/control/vehicle_cmd_gate/src/vehicle_cmd_filter.cpp index 49735da876d9a..6cb6194bc01fa 100644 --- a/control/vehicle_cmd_gate/src/vehicle_cmd_filter.cpp +++ b/control/vehicle_cmd_gate/src/vehicle_cmd_filter.cpp @@ -24,43 +24,41 @@ VehicleCmdFilter::VehicleCmdFilter() { } -void VehicleCmdFilter::limitLongitudinalWithVel(AckermannControlCommand & input) const +void VehicleCmdFilter::limitLongitudinalWithVel(Control & input) const { - input.longitudinal.speed = std::max( - std::min(static_cast(input.longitudinal.speed), param_.vel_lim), -param_.vel_lim); + input.longitudinal.velocity = std::max( + std::min(static_cast(input.longitudinal.velocity), param_.vel_lim), -param_.vel_lim); } -void VehicleCmdFilter::limitLongitudinalWithAcc( - const double dt, AckermannControlCommand & input) const +void VehicleCmdFilter::limitLongitudinalWithAcc(const double dt, Control & input) const { input.longitudinal.acceleration = std::max( std::min(static_cast(input.longitudinal.acceleration), param_.lon_acc_lim), -param_.lon_acc_lim); - input.longitudinal.speed = - limitDiff(input.longitudinal.speed, prev_cmd_.longitudinal.speed, param_.lon_acc_lim * dt); + input.longitudinal.velocity = limitDiff( + input.longitudinal.velocity, prev_cmd_.longitudinal.velocity, param_.lon_acc_lim * dt); } void VehicleCmdFilter::VehicleCmdFilter::limitLongitudinalWithJerk( - const double dt, AckermannControlCommand & input) const + const double dt, Control & input) const { input.longitudinal.acceleration = limitDiff( input.longitudinal.acceleration, prev_cmd_.longitudinal.acceleration, param_.lon_jerk_lim * dt); } void VehicleCmdFilter::limitLateralWithLatAcc( - [[maybe_unused]] const double dt, AckermannControlCommand & input) const + [[maybe_unused]] const double dt, Control & input) const { double latacc = calcLatAcc(input); if (std::fabs(latacc) > param_.lat_acc_lim) { - double v_sq = - std::max(static_cast(input.longitudinal.speed * input.longitudinal.speed), 0.001); + double v_sq = std::max( + static_cast(input.longitudinal.velocity * input.longitudinal.velocity), 0.001); double steer_lim = std::atan(param_.lat_acc_lim * param_.wheel_base / v_sq); input.lateral.steering_tire_angle = latacc > 0.0 ? steer_lim : -steer_lim; } } -void VehicleCmdFilter::limitLateralWithLatJerk( - const double dt, AckermannControlCommand & input) const +void VehicleCmdFilter::limitLateralWithLatJerk(const double dt, Control & input) const { double curr_latacc = calcLatAcc(input); double prev_latacc = calcLatAcc(prev_cmd_); @@ -69,14 +67,15 @@ void VehicleCmdFilter::limitLateralWithLatJerk( const double latacc_min = prev_latacc - param_.lat_jerk_lim * dt; if (curr_latacc > latacc_max) { - input.lateral.steering_tire_angle = calcSteerFromLatacc(input.longitudinal.speed, latacc_max); + input.lateral.steering_tire_angle = + calcSteerFromLatacc(input.longitudinal.velocity, latacc_max); } else if (curr_latacc < latacc_min) { - input.lateral.steering_tire_angle = calcSteerFromLatacc(input.longitudinal.speed, latacc_min); + input.lateral.steering_tire_angle = + calcSteerFromLatacc(input.longitudinal.velocity, latacc_min); } } -void VehicleCmdFilter::limitActualSteerDiff( - const double current_steer_angle, AckermannControlCommand & input) const +void VehicleCmdFilter::limitActualSteerDiff(const double current_steer_angle, Control & input) const { auto ds = input.lateral.steering_tire_angle - current_steer_angle; ds = std::clamp(ds, -param_.actual_steer_diff_lim, param_.actual_steer_diff_lim); @@ -84,7 +83,7 @@ void VehicleCmdFilter::limitActualSteerDiff( } void VehicleCmdFilter::filterAll( - const double dt, const double current_steer_angle, AckermannControlCommand & cmd) const + const double dt, const double current_steer_angle, Control & cmd) const { limitLongitudinalWithJerk(dt, cmd); limitLongitudinalWithAcc(dt, cmd); @@ -101,9 +100,9 @@ double VehicleCmdFilter::calcSteerFromLatacc(const double v, const double latacc return std::atan(latacc * param_.wheel_base / v_sq); } -double VehicleCmdFilter::calcLatAcc(const AckermannControlCommand & cmd) const +double VehicleCmdFilter::calcLatAcc(const Control & cmd) const { - double v = cmd.longitudinal.speed; + double v = cmd.longitudinal.velocity; return v * v * std::tan(cmd.lateral.steering_tire_angle) / param_.wheel_base; } diff --git a/control/vehicle_cmd_gate/src/vehicle_cmd_filter.hpp b/control/vehicle_cmd_gate/src/vehicle_cmd_filter.hpp index e6b60687f6b65..d4884c1f914c1 100644 --- a/control/vehicle_cmd_gate/src/vehicle_cmd_filter.hpp +++ b/control/vehicle_cmd_gate/src/vehicle_cmd_filter.hpp @@ -17,11 +17,11 @@ #include -#include +#include namespace vehicle_cmd_gate { -using autoware_auto_control_msgs::msg::AckermannControlCommand; +using autoware_control_msgs::msg::Control; struct VehicleCmdFilterParam { @@ -47,23 +47,21 @@ class VehicleCmdFilter void setLatJerkLim(double v) { param_.lat_jerk_lim = v; } void setActualSteerDiffLim(double v) { param_.actual_steer_diff_lim = v; } void setParam(const VehicleCmdFilterParam & p) { param_ = p; } - void setPrevCmd(const AckermannControlCommand & v) { prev_cmd_ = v; } + void setPrevCmd(const Control & v) { prev_cmd_ = v; } - void limitLongitudinalWithVel(AckermannControlCommand & input) const; - void limitLongitudinalWithAcc(const double dt, AckermannControlCommand & input) const; - void limitLongitudinalWithJerk(const double dt, AckermannControlCommand & input) const; - void limitLateralWithLatAcc(const double dt, AckermannControlCommand & input) const; - void limitLateralWithLatJerk(const double dt, AckermannControlCommand & input) const; - void limitActualSteerDiff( - const double current_steer_angle, AckermannControlCommand & input) const; - void filterAll( - const double dt, const double current_steer_angle, AckermannControlCommand & input) const; + void limitLongitudinalWithVel(Control & input) const; + void limitLongitudinalWithAcc(const double dt, Control & input) const; + void limitLongitudinalWithJerk(const double dt, Control & input) const; + void limitLateralWithLatAcc(const double dt, Control & input) const; + void limitLateralWithLatJerk(const double dt, Control & input) const; + void limitActualSteerDiff(const double current_steer_angle, Control & input) const; + void filterAll(const double dt, const double current_steer_angle, Control & input) const; private: VehicleCmdFilterParam param_; - AckermannControlCommand prev_cmd_; + Control prev_cmd_; - double calcLatAcc(const AckermannControlCommand & cmd) const; + double calcLatAcc(const Control & cmd) const; double calcSteerFromLatacc(const double v, const double latacc) const; double limitDiff(const double curr, const double prev, const double diff_lim) const; }; diff --git a/control/vehicle_cmd_gate/src/vehicle_cmd_gate.cpp b/control/vehicle_cmd_gate/src/vehicle_cmd_gate.cpp index 42e54d20416d8..5c73aee9e2502 100644 --- a/control/vehicle_cmd_gate/src/vehicle_cmd_gate.cpp +++ b/control/vehicle_cmd_gate/src/vehicle_cmd_gate.cpp @@ -57,7 +57,7 @@ VehicleCmdGate::VehicleCmdGate(const rclcpp::NodeOptions & node_options) // Publisher vehicle_cmd_emergency_pub_ = create_publisher("output/vehicle_cmd_emergency", durable_qos); - control_cmd_pub_ = create_publisher("output/control_cmd", durable_qos); + control_cmd_pub_ = create_publisher("output/control_cmd", durable_qos); gear_cmd_pub_ = create_publisher("output/gear_cmd", durable_qos); turn_indicator_cmd_pub_ = create_publisher("output/turn_indicators_cmd", durable_qos); @@ -93,7 +93,7 @@ VehicleCmdGate::VehicleCmdGate(const rclcpp::NodeOptions & node_options) "input/mrm_state", 1, std::bind(&VehicleCmdGate::onMrmState, this, _1)); // Subscriber for auto - auto_control_cmd_sub_ = create_subscription( + auto_control_cmd_sub_ = create_subscription( "input/auto/control_cmd", 1, std::bind(&VehicleCmdGate::onAutoCtrlCmd, this, _1)); auto_turn_indicator_cmd_sub_ = create_subscription( @@ -109,7 +109,7 @@ VehicleCmdGate::VehicleCmdGate(const rclcpp::NodeOptions & node_options) [this](GearCommand::ConstSharedPtr msg) { auto_commands_.gear = *msg; }); // Subscriber for external - remote_control_cmd_sub_ = create_subscription( + remote_control_cmd_sub_ = create_subscription( "input/external/control_cmd", 1, std::bind(&VehicleCmdGate::onRemoteCtrlCmd, this, _1)); remote_turn_indicator_cmd_sub_ = create_subscription( @@ -125,7 +125,7 @@ VehicleCmdGate::VehicleCmdGate(const rclcpp::NodeOptions & node_options) [this](GearCommand::ConstSharedPtr msg) { remote_commands_.gear = *msg; }); // Subscriber for emergency - emergency_control_cmd_sub_ = create_subscription( + emergency_control_cmd_sub_ = create_subscription( "input/emergency/control_cmd", 1, std::bind(&VehicleCmdGate::onEmergencyCtrlCmd, this, _1)); emergency_hazard_light_cmd_sub_ = create_subscription( @@ -251,7 +251,7 @@ bool VehicleCmdGate::isDataReady() } // for auto -void VehicleCmdGate::onAutoCtrlCmd(AckermannControlCommand::ConstSharedPtr msg) +void VehicleCmdGate::onAutoCtrlCmd(Control::ConstSharedPtr msg) { auto_commands_.control = *msg; @@ -261,7 +261,7 @@ void VehicleCmdGate::onAutoCtrlCmd(AckermannControlCommand::ConstSharedPtr msg) } // for remote -void VehicleCmdGate::onRemoteCtrlCmd(AckermannControlCommand::ConstSharedPtr msg) +void VehicleCmdGate::onRemoteCtrlCmd(Control::ConstSharedPtr msg) { remote_commands_.control = *msg; @@ -271,7 +271,7 @@ void VehicleCmdGate::onRemoteCtrlCmd(AckermannControlCommand::ConstSharedPtr msg } // for emergency -void VehicleCmdGate::onEmergencyCtrlCmd(AckermannControlCommand::ConstSharedPtr msg) +void VehicleCmdGate::onEmergencyCtrlCmd(Control::ConstSharedPtr msg) { emergency_commands_.control = *msg; @@ -386,7 +386,7 @@ void VehicleCmdGate::publishControlCommands(const Commands & commands) } if (moderate_stop_interface_->is_stop_requested()) { // if stop requested, stop the vehicle - filtered_commands.control.longitudinal.speed = 0.0; + filtered_commands.control.longitudinal.velocity = 0.0; filtered_commands.control.longitudinal.acceleration = moderate_stop_service_acceleration_; } @@ -406,7 +406,7 @@ void VehicleCmdGate::publishControlCommands(const Commands & commands) // Check pause. Place this check after all other checks as it needs the final output. adapi_pause_->update(filtered_commands.control); if (adapi_pause_->is_paused()) { - filtered_commands.control.longitudinal.speed = 0.0; + filtered_commands.control.longitudinal.velocity = 0.0; filtered_commands.control.longitudinal.acceleration = stop_hold_acceleration_; } @@ -433,7 +433,7 @@ void VehicleCmdGate::publishEmergencyStopControlCommands() const auto stamp = this->now(); // ControlCommand - AckermannControlCommand control_cmd; + Control control_cmd; control_cmd.stamp = stamp; control_cmd = createEmergencyStopControlCmd(); @@ -490,9 +490,9 @@ void VehicleCmdGate::publishStatus() moderate_stop_interface_->publish(); } -AckermannControlCommand VehicleCmdGate::filterControlCommand(const AckermannControlCommand & in) +Control VehicleCmdGate::filterControlCommand(const Control & in) { - AckermannControlCommand out = in; + Control out = in; const double dt = getDt(); const auto mode = current_operation_mode_; const auto current_status_cmd = getActualStatusAsCommand(); @@ -532,31 +532,31 @@ AckermannControlCommand VehicleCmdGate::filterControlCommand(const AckermannCont return out; } -AckermannControlCommand VehicleCmdGate::createStopControlCmd() const +Control VehicleCmdGate::createStopControlCmd() const { - AckermannControlCommand cmd; + Control cmd; const auto t = this->now(); cmd.stamp = t; cmd.lateral.stamp = t; cmd.longitudinal.stamp = t; cmd.lateral.steering_tire_angle = current_steer_; cmd.lateral.steering_tire_rotation_rate = 0.0; - cmd.longitudinal.speed = 0.0; + cmd.longitudinal.velocity = 0.0; cmd.longitudinal.acceleration = stop_hold_acceleration_; return cmd; } -AckermannControlCommand VehicleCmdGate::createEmergencyStopControlCmd() const +Control VehicleCmdGate::createEmergencyStopControlCmd() const { - AckermannControlCommand cmd; + Control cmd; const auto t = this->now(); cmd.stamp = t; cmd.lateral.stamp = t; cmd.longitudinal.stamp = t; cmd.lateral.steering_tire_angle = prev_control_cmd_.lateral.steering_tire_angle; cmd.lateral.steering_tire_rotation_rate = prev_control_cmd_.lateral.steering_tire_rotation_rate; - cmd.longitudinal.speed = 0.0; + cmd.longitudinal.velocity = 0.0; cmd.longitudinal.acceleration = emergency_acceleration_; return cmd; @@ -615,13 +615,13 @@ double VehicleCmdGate::getDt() return dt; } -AckermannControlCommand VehicleCmdGate::getActualStatusAsCommand() +Control VehicleCmdGate::getActualStatusAsCommand() { - AckermannControlCommand status; + Control status; status.stamp = status.lateral.stamp = status.longitudinal.stamp = this->now(); status.lateral.steering_tire_angle = current_steer_; status.lateral.steering_tire_rotation_rate = 0.0; - status.longitudinal.speed = current_kinematics_.twist.twist.linear.x; + status.longitudinal.velocity = current_kinematics_.twist.twist.linear.x; status.longitudinal.acceleration = current_acceleration_; return status; } diff --git a/control/vehicle_cmd_gate/src/vehicle_cmd_gate.hpp b/control/vehicle_cmd_gate/src/vehicle_cmd_gate.hpp index 17bfe99d45251..999eaf54f6a1a 100644 --- a/control/vehicle_cmd_gate/src/vehicle_cmd_gate.hpp +++ b/control/vehicle_cmd_gate/src/vehicle_cmd_gate.hpp @@ -26,12 +26,12 @@ #include #include -#include #include #include #include #include #include +#include #include #include #include @@ -51,11 +51,11 @@ namespace vehicle_cmd_gate using autoware_adapi_v1_msgs::msg::MrmState; using autoware_adapi_v1_msgs::msg::OperationModeState; -using autoware_auto_control_msgs::msg::AckermannControlCommand; using autoware_auto_vehicle_msgs::msg::GearCommand; using autoware_auto_vehicle_msgs::msg::HazardLightsCommand; using autoware_auto_vehicle_msgs::msg::SteeringReport; using autoware_auto_vehicle_msgs::msg::TurnIndicatorsCommand; +using autoware_control_msgs::msg::Control; using geometry_msgs::msg::AccelWithCovarianceStamped; using std_srvs::srv::Trigger; using tier4_control_msgs::msg::GateMode; @@ -74,7 +74,7 @@ using EngageSrv = tier4_external_api_msgs::srv::Engage; using motion_utils::VehicleStopChecker; struct Commands { - AckermannControlCommand control; + Control control; TurnIndicatorsCommand turn_indicator; HazardLightsCommand hazard_light; GearCommand gear; @@ -92,7 +92,7 @@ class VehicleCmdGate : public rclcpp::Node private: // Publisher rclcpp::Publisher::SharedPtr vehicle_cmd_emergency_pub_; - rclcpp::Publisher::SharedPtr control_cmd_pub_; + rclcpp::Publisher::SharedPtr control_cmd_pub_; rclcpp::Publisher::SharedPtr gear_cmd_pub_; rclcpp::Publisher::SharedPtr turn_indicator_cmd_pub_; rclcpp::Publisher::SharedPtr hazard_light_cmd_pub_; @@ -135,26 +135,26 @@ class VehicleCmdGate : public rclcpp::Node // Subscriber for auto Commands auto_commands_; - rclcpp::Subscription::SharedPtr auto_control_cmd_sub_; + rclcpp::Subscription::SharedPtr auto_control_cmd_sub_; rclcpp::Subscription::SharedPtr auto_turn_indicator_cmd_sub_; rclcpp::Subscription::SharedPtr auto_hazard_light_cmd_sub_; rclcpp::Subscription::SharedPtr auto_gear_cmd_sub_; - void onAutoCtrlCmd(AckermannControlCommand::ConstSharedPtr msg); + void onAutoCtrlCmd(Control::ConstSharedPtr msg); // Subscription for external Commands remote_commands_; - rclcpp::Subscription::SharedPtr remote_control_cmd_sub_; + rclcpp::Subscription::SharedPtr remote_control_cmd_sub_; rclcpp::Subscription::SharedPtr remote_turn_indicator_cmd_sub_; rclcpp::Subscription::SharedPtr remote_hazard_light_cmd_sub_; rclcpp::Subscription::SharedPtr remote_gear_cmd_sub_; - void onRemoteCtrlCmd(AckermannControlCommand::ConstSharedPtr msg); + void onRemoteCtrlCmd(Control::ConstSharedPtr msg); // Subscription for emergency Commands emergency_commands_; - rclcpp::Subscription::SharedPtr emergency_control_cmd_sub_; + rclcpp::Subscription::SharedPtr emergency_control_cmd_sub_; rclcpp::Subscription::SharedPtr emergency_hazard_light_cmd_sub_; rclcpp::Subscription::SharedPtr emergency_gear_cmd_sub_; - void onEmergencyCtrlCmd(AckermannControlCommand::ConstSharedPtr msg); + void onEmergencyCtrlCmd(Control::ConstSharedPtr msg); // Parameter bool use_emergency_handling_; @@ -203,16 +203,16 @@ class VehicleCmdGate : public rclcpp::Node void checkExternalEmergencyStop(diagnostic_updater::DiagnosticStatusWrapper & stat); // Algorithm - AckermannControlCommand prev_control_cmd_; - AckermannControlCommand createStopControlCmd() const; - AckermannControlCommand createEmergencyStopControlCmd() const; + Control prev_control_cmd_; + Control createStopControlCmd() const; + Control createEmergencyStopControlCmd() const; std::shared_ptr prev_time_; double getDt(); - AckermannControlCommand getActualStatusAsCommand(); + Control getActualStatusAsCommand(); VehicleCmdFilter filter_; - AckermannControlCommand filterControlCommand(const AckermannControlCommand & msg); + Control filterControlCommand(const Control & msg); // filtering on transition OperationModeState current_operation_mode_; diff --git a/control/vehicle_cmd_gate/test/src/test_vehicle_cmd_filter.cpp b/control/vehicle_cmd_gate/test/src/test_vehicle_cmd_filter.cpp index 5fbfec237b047..8e254d2f83bdc 100644 --- a/control/vehicle_cmd_gate/test/src/test_vehicle_cmd_filter.cpp +++ b/control/vehicle_cmd_gate/test/src/test_vehicle_cmd_filter.cpp @@ -24,7 +24,7 @@ #define ASSERT_LT_NEAR(x, y) ASSERT_LT(x, y + THRESHOLD) #define ASSERT_GT_NEAR(x, y) ASSERT_GT(x, y - THRESHOLD) -using autoware_auto_control_msgs::msg::AckermannControlCommand; +using autoware_control_msgs::msg::Control; constexpr double NOMINAL_INTERVAL = 1.0; @@ -40,25 +40,25 @@ void setFilterParams( f.setWheelBase(wheelbase); } -AckermannControlCommand genCmd(double s, double sr, double v, double a) +Control genCmd(double s, double sr, double v, double a) { - AckermannControlCommand cmd; + Control cmd; cmd.lateral.steering_tire_angle = s; cmd.lateral.steering_tire_rotation_rate = sr; - cmd.longitudinal.speed = v; + cmd.longitudinal.velocity = v; cmd.longitudinal.acceleration = a; return cmd; } -double calcLatAcc(const AckermannControlCommand & cmd, const double wheelbase) +double calcLatAcc(const Control & cmd, const double wheelbase) { - double v = cmd.longitudinal.speed; + double v = cmd.longitudinal.velocity; return v * v * std::tan(cmd.lateral.steering_tire_angle) / wheelbase; } void test_all( double V_LIM, double A_LIM, double J_LIM, double LAT_A_LIM, double LAT_J_LIM, - const AckermannControlCommand & prev_cmd, const AckermannControlCommand & raw_cmd) + const Control & prev_cmd, const Control & raw_cmd) { const double WHEELBASE = 3.0; const double DT = 0.1; // [s] @@ -73,11 +73,11 @@ void test_all( filter.limitLongitudinalWithVel(filtered_cmd); // check if the filtered value does not exceed the limit. - ASSERT_LT_NEAR(filtered_cmd.longitudinal.speed, V_LIM); + ASSERT_LT_NEAR(filtered_cmd.longitudinal.velocity, V_LIM); // check if the undesired filter is not applied. - if (std::abs(raw_cmd.longitudinal.speed) < V_LIM) { - ASSERT_NEAR(filtered_cmd.longitudinal.speed, raw_cmd.longitudinal.speed, THRESHOLD); + if (std::abs(raw_cmd.longitudinal.velocity) < V_LIM) { + ASSERT_NEAR(filtered_cmd.longitudinal.velocity, raw_cmd.longitudinal.velocity, THRESHOLD); } } @@ -99,14 +99,14 @@ void test_all( } // check if the filtered value does not exceed the limit. - const double v_max = prev_cmd.longitudinal.speed + A_LIM * DT; - const double v_min = prev_cmd.longitudinal.speed - A_LIM * DT; - ASSERT_LT_NEAR(filtered_cmd.longitudinal.speed, v_max); - ASSERT_GT_NEAR(filtered_cmd.longitudinal.speed, v_min); + const double v_max = prev_cmd.longitudinal.velocity + A_LIM * DT; + const double v_min = prev_cmd.longitudinal.velocity - A_LIM * DT; + ASSERT_LT_NEAR(filtered_cmd.longitudinal.velocity, v_max); + ASSERT_GT_NEAR(filtered_cmd.longitudinal.velocity, v_min); // check if the undesired filter is not applied. - if (v_min < raw_cmd.longitudinal.speed && raw_cmd.longitudinal.speed < v_max) { - ASSERT_NEAR(filtered_cmd.longitudinal.speed, raw_cmd.longitudinal.speed, THRESHOLD); + if (v_min < raw_cmd.longitudinal.velocity && raw_cmd.longitudinal.velocity < v_max) { + ASSERT_NEAR(filtered_cmd.longitudinal.velocity, raw_cmd.longitudinal.velocity, THRESHOLD); } } @@ -174,10 +174,10 @@ TEST(VehicleCmdFilter, VehicleCmdFilter) const std::vector lat_a_arr = {0.01, 1.0, 100.0}; const std::vector lat_j_arr = {0.01, 1.0, 100.0}; - const std::vector prev_cmd_arr = { + const std::vector prev_cmd_arr = { genCmd(0.0, 0.0, 0.0, 0.0), genCmd(1.0, 1.0, 1.0, 1.0)}; - const std::vector raw_cmd_arr = { + const std::vector raw_cmd_arr = { genCmd(1.0, 1.0, 1.0, 1.0), genCmd(10.0, -1.0, -1.0, -1.0)}; for (const auto & v : v_arr) { diff --git a/planning/planning_debug_tools/scripts/closest_velocity_checker.py b/planning/planning_debug_tools/scripts/closest_velocity_checker.py index 4123766a947b0..1577d334429d5 100755 --- a/planning/planning_debug_tools/scripts/closest_velocity_checker.py +++ b/planning/planning_debug_tools/scripts/closest_velocity_checker.py @@ -16,12 +16,12 @@ import time -from autoware_auto_control_msgs.msg import AckermannControlCommand from autoware_auto_planning_msgs.msg import Path from autoware_auto_planning_msgs.msg import PathWithLaneId from autoware_auto_planning_msgs.msg import Trajectory from autoware_auto_vehicle_msgs.msg import Engage from autoware_auto_vehicle_msgs.msg import VelocityReport +from autoware_control_msgs.msg import Control from geometry_msgs.msg import Pose from nav_msgs.msg import Odometry import numpy as np @@ -104,13 +104,13 @@ def __init__(self): # control commands self.sub6 = self.create_subscription( - AckermannControlCommand, + Control, "/control/trajectory_follower/control_cmd", self.CallBackControlCmd, 1, ) self.sub7 = self.create_subscription( - AckermannControlCommand, "/control/command/control_cmd", self.CallBackVehicleCmd, 1 + Control, "/control/command/control_cmd", self.CallBackVehicleCmd, 1 ) # others related to velocity @@ -274,13 +274,13 @@ def CallBackScenarioTrajectory(self, msg): def CallBackControlCmd(self, msg): # self.get_logger().info('CONTROL_CMD called') - self.data_arr[CONTROL_CMD] = msg.longitudinal.speed + self.data_arr[CONTROL_CMD] = msg.longitudinal.velocity self.data_arr[CONTROL_CMD_ACC] = msg.longitudinal.acceleration return def CallBackVehicleCmd(self, msg): # self.get_logger().info('VEHICLE_CMD called') - self.data_arr[VEHICLE_CMD] = msg.longitudinal.speed + self.data_arr[VEHICLE_CMD] = msg.longitudinal.velocity self.data_arr[VEHICLE_CMD_ACC] = msg.longitudinal.acceleration return diff --git a/planning/planning_test_utils/include/planning_interface_test_manager/planning_interface_test_manager.hpp b/planning/planning_test_utils/include/planning_interface_test_manager/planning_interface_test_manager.hpp index ddc304c99ea6a..79bb13534cb91 100644 --- a/planning/planning_test_utils/include/planning_interface_test_manager/planning_interface_test_manager.hpp +++ b/planning/planning_test_utils/include/planning_interface_test_manager/planning_interface_test_manager.hpp @@ -35,7 +35,6 @@ #include #include -#include #include #include #include @@ -43,6 +42,7 @@ #include #include #include +#include #include #include #include diff --git a/planning/planning_test_utils/package.xml b/planning/planning_test_utils/package.xml index 72662f7ee7b76..7893da239bca7 100644 --- a/planning/planning_test_utils/package.xml +++ b/planning/planning_test_utils/package.xml @@ -13,11 +13,11 @@ ament_cmake_auto autoware_cmake - autoware_auto_control_msgs autoware_auto_mapping_msgs autoware_auto_perception_msgs autoware_auto_planning_msgs autoware_auto_vehicle_msgs + autoware_control_msgs autoware_planning_msgs component_interface_specs component_interface_utils diff --git a/simulator/simple_planning_simulator/design/simple_planning_simulator-design.md b/simulator/simple_planning_simulator/design/simple_planning_simulator-design.md index d70aca794dc07..0695b9a1f9efd 100644 --- a/simulator/simple_planning_simulator/design/simple_planning_simulator-design.md +++ b/simulator/simple_planning_simulator/design/simple_planning_simulator-design.md @@ -18,8 +18,8 @@ The purpose of this simulator is for the integration test of planning and contro ### input - input/initialpose [`geometry_msgs/msg/PoseWithCovarianceStamped`] : for initial pose -- input/ackermann_control_command [`autoware_auto_msgs/msg/AckermannControlCommand`] : target command to drive a vehicle -- input/manual_ackermann_control_command [`autoware_auto_msgs/msg/AckermannControlCommand`] : manual target command to drive a vehicle (used when control_mode_request = Manual) +- input/ackermann_control_command [`autoware_control_msgs/msg/Control`] : target command to drive a vehicle +- input/manual_ackermann_control_command [`autoware_control_msgs/msg/Control`] : manual target command to drive a vehicle (used when control_mode_request = Manual) - input/gear_command [`autoware_auto_vehicle_msgs/msg/GearCommand`] : target gear command. - input/manual_gear_command [`autoware_auto_vehicle_msgs/msg/GearCommand`] : target gear command (used when control_mode_request = Manual) - input/turn_indicators_command [`autoware_auto_vehicle_msgs/msg/TurnIndicatorsCommand`] : target turn indicator command 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 742f87411b7d8..30265fb743c16 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 @@ -20,7 +20,6 @@ #include "simple_planning_simulator/visibility_control.hpp" #include "tier4_api_utils/tier4_api_utils.hpp" -#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" @@ -36,6 +35,7 @@ #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 "autoware_control_msgs/msg/control.hpp" #include "geometry_msgs/msg/accel_with_covariance_stamped.hpp" #include "geometry_msgs/msg/pose.hpp" #include "geometry_msgs/msg/pose_stamped.hpp" @@ -60,7 +60,6 @@ namespace simulation namespace simple_planning_simulator { -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; @@ -75,6 +74,7 @@ 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 autoware_control_msgs::msg::Control; using geometry_msgs::msg::AccelWithCovarianceStamped; using geometry_msgs::msg::Pose; using geometry_msgs::msg::PoseStamped; @@ -141,8 +141,8 @@ class PLANNING_SIMULATOR_PUBLIC SimplePlanningSimulator : public rclcpp::Node rclcpp::Subscription::SharedPtr sub_turn_indicators_cmd_; rclcpp::Subscription::SharedPtr sub_hazard_lights_cmd_; rclcpp::Subscription::SharedPtr sub_vehicle_cmd_; - rclcpp::Subscription::SharedPtr sub_ackermann_cmd_; - rclcpp::Subscription::SharedPtr sub_manual_ackermann_cmd_; + rclcpp::Subscription::SharedPtr sub_ackermann_cmd_; + rclcpp::Subscription::SharedPtr sub_manual_ackermann_cmd_; rclcpp::Subscription::SharedPtr sub_init_pose_; rclcpp::Subscription::SharedPtr sub_init_twist_; rclcpp::Subscription::SharedPtr sub_trajectory_; @@ -170,8 +170,8 @@ class PLANNING_SIMULATOR_PUBLIC SimplePlanningSimulator : public rclcpp::Node VelocityReport current_velocity_; Odometry current_odometry_; SteeringReport current_steer_; - AckermannControlCommand current_ackermann_cmd_; - AckermannControlCommand current_manual_ackermann_cmd_; + Control current_ackermann_cmd_; + Control current_manual_ackermann_cmd_; GearCommand current_gear_cmd_; GearCommand current_manual_gear_cmd_; TurnIndicatorsCommand::ConstSharedPtr current_turn_indicators_cmd_ptr_; @@ -214,7 +214,7 @@ class PLANNING_SIMULATOR_PUBLIC SimplePlanningSimulator : public rclcpp::Node /** * @brief set input steering, velocity, and acceleration of the vehicle model */ - void set_input(const AckermannControlCommand & cmd); + void set_input(const Control & cmd); /** * @brief set current_vehicle_state_ with received message diff --git a/simulator/simple_planning_simulator/package.xml b/simulator/simple_planning_simulator/package.xml index c83433d35bb1b..362a9e654ab45 100644 --- a/simulator/simple_planning_simulator/package.xml +++ b/simulator/simple_planning_simulator/package.xml @@ -11,10 +11,10 @@ ament_cmake_auto autoware_cmake - autoware_auto_control_msgs autoware_auto_planning_msgs autoware_auto_tf2 autoware_auto_vehicle_msgs + autoware_control_msgs geometry_msgs nav_msgs rclcpp 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 cfc58cdaeed63..e3f57d667a788 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 @@ -89,12 +89,12 @@ SimplePlanningSimulator::SimplePlanningSimulator(const rclcpp::NodeOptions & opt "input/initialpose", QoS{1}, std::bind(&SimplePlanningSimulator::on_initialpose, this, _1)); sub_init_twist_ = create_subscription( "input/initialtwist", QoS{1}, std::bind(&SimplePlanningSimulator::on_initialtwist, this, _1)); - sub_ackermann_cmd_ = create_subscription( + sub_ackermann_cmd_ = create_subscription( "input/ackermann_control_command", QoS{1}, - [this](const AckermannControlCommand::SharedPtr msg) { current_ackermann_cmd_ = *msg; }); - sub_manual_ackermann_cmd_ = create_subscription( + [this](const Control::SharedPtr msg) { current_ackermann_cmd_ = *msg; }); + sub_manual_ackermann_cmd_ = create_subscription( "input/manual_ackermann_control_command", QoS{1}, - [this](const AckermannControlCommand::SharedPtr msg) { current_manual_ackermann_cmd_ = *msg; }); + [this](const Control::SharedPtr msg) { current_manual_ackermann_cmd_ = *msg; }); sub_gear_cmd_ = create_subscription( "input/gear_command", QoS{1}, [this](const GearCommand::SharedPtr msg) { current_gear_cmd_ = *msg; }); @@ -344,10 +344,10 @@ void SimplePlanningSimulator::on_set_pose( response->status = tier4_api_utils::response_success(); } -void SimplePlanningSimulator::set_input(const AckermannControlCommand & cmd) +void SimplePlanningSimulator::set_input(const Control & cmd) { const auto steer = cmd.lateral.steering_tire_angle; - const auto vel = cmd.longitudinal.speed; + const auto vel = cmd.longitudinal.velocity; const auto accel = cmd.longitudinal.acceleration; using autoware_auto_vehicle_msgs::msg::GearCommand; diff --git a/simulator/simple_planning_simulator/test/test_simple_planning_simulator.cpp b/simulator/simple_planning_simulator/test/test_simple_planning_simulator.cpp index e57a3683b97a9..3be0edcb05bf2 100644 --- a/simulator/simple_planning_simulator/test/test_simple_planning_simulator.cpp +++ b/simulator/simple_planning_simulator/test/test_simple_planning_simulator.cpp @@ -24,8 +24,8 @@ #include -using autoware_auto_control_msgs::msg::AckermannControlCommand; using autoware_auto_vehicle_msgs::msg::GearCommand; +using autoware_control_msgs::msg::Control; using geometry_msgs::msg::PoseWithCovarianceStamped; using nav_msgs::msg::Odometry; @@ -51,14 +51,14 @@ class PubSubNode : public rclcpp::Node "output/odometry", rclcpp::QoS{1}, [this](const Odometry::SharedPtr msg) { current_odom_ = msg; }); pub_ackermann_command_ = - create_publisher("input/ackermann_control_command", rclcpp::QoS{1}); + create_publisher("input/ackermann_control_command", rclcpp::QoS{1}); pub_initialpose_ = create_publisher("input/initialpose", rclcpp::QoS{1}); pub_gear_cmd_ = create_publisher("input/gear_command", rclcpp::QoS{1}); } rclcpp::Subscription::SharedPtr current_odom_sub_; - rclcpp::Publisher::SharedPtr pub_ackermann_command_; + rclcpp::Publisher::SharedPtr pub_ackermann_command_; rclcpp::Publisher::SharedPtr pub_gear_cmd_; rclcpp::Publisher::SharedPtr pub_initialpose_; @@ -66,7 +66,7 @@ class PubSubNode : public rclcpp::Node }; /** - * @brief Generate an AckermannControlCommand message + * @brief Generate an Control message * @param [in] t timestamp * @param [in] steer [rad] steering * @param [in] steer_rate [rad/s] steering rotation rate @@ -74,17 +74,17 @@ class PubSubNode : public rclcpp::Node * @param [in] acc [m/s²] acceleration * @param [in] jerk [m/s3] jerk */ -AckermannControlCommand cmdGen( +Control cmdGen( const builtin_interfaces::msg::Time & t, double steer, double steer_rate, double vel, double acc, double jerk) { - AckermannControlCommand cmd; + Control cmd; cmd.stamp = t; cmd.lateral.stamp = t; cmd.lateral.steering_tire_angle = steer; cmd.lateral.steering_tire_rotation_rate = steer_rate; cmd.longitudinal.stamp = t; - cmd.longitudinal.speed = vel; + cmd.longitudinal.velocity = vel; cmd.longitudinal.acceleration = acc; cmd.longitudinal.jerk = jerk; return cmd; @@ -125,8 +125,7 @@ void sendGear( * @param [in] pub_sub_node pointer to the node used for communication */ void sendCommand( - const AckermannControlCommand & cmd, rclcpp::Node::SharedPtr sim_node, - std::shared_ptr pub_sub_node) + const Control & cmd, rclcpp::Node::SharedPtr sim_node, std::shared_ptr pub_sub_node) { for (int i = 0; i < 150; ++i) { pub_sub_node->pub_ackermann_command_->publish(cmd); diff --git a/system/component_state_monitor/config/topics.yaml b/system/component_state_monitor/config/topics.yaml index c00203dbb6073..2d47d3cd62dd3 100644 --- a/system/component_state_monitor/config/topics.yaml +++ b/system/component_state_monitor/config/topics.yaml @@ -108,7 +108,7 @@ args: node_name_suffix: trajectory_follower_control_cmd topic: /control/trajectory_follower/control_cmd - topic_type: autoware_auto_control_msgs/msg/AckermannControlCommand + topic_type: autoware_control_msgs/msg/Control best_effort: false transient_local: false warn_rate: 5.0 @@ -121,7 +121,7 @@ args: node_name_suffix: control_command_control_cmd topic: /control/command/control_cmd - topic_type: autoware_auto_control_msgs/msg/AckermannControlCommand + topic_type: autoware_control_msgs/msg/Control best_effort: false transient_local: false warn_rate: 5.0 @@ -160,7 +160,7 @@ args: node_name_suffix: system_emergency_control_cmd topic: /system/emergency/control_cmd - topic_type: autoware_auto_control_msgs/msg/AckermannControlCommand + topic_type: autoware_control_msgs/msg/Control best_effort: false transient_local: false warn_rate: 5.0 diff --git a/system/emergency_handler/README.md b/system/emergency_handler/README.md index 92aa248733db1..f137819f2a913 100644 --- a/system/emergency_handler/README.md +++ b/system/emergency_handler/README.md @@ -14,14 +14,14 @@ Emergency Handler is a node to select proper MRM from from system failure state ### Input -| Name | Type | Description | -| ----------------------------------------- | ---------------------------------------------------------- | ----------------------------------------------------------------------------- | -| `/system/emergency/hazard_status` | `autoware_auto_system_msgs::msg::HazardStatusStamped` | Used to select proper MRM from system failure state contained in HazardStatus | -| `/control/vehicle_cmd` | `autoware_auto_control_msgs::msg::AckermannControlCommand` | Used as reference when generate Emergency Control Command | -| `/localization/kinematic_state` | `nav_msgs::msg::Odometry` | Used to decide whether vehicle is stopped or not | -| `/vehicle/status/control_mode` | `autoware_auto_vehicle_msgs::msg::ControlModeReport` | Used to check vehicle mode: autonomous or manual | -| `/system/api/mrm/comfortable_stop/status` | `tier4_system_msgs::msg::MrmBehaviorStatus` | Used to check if MRM comfortable stop operation is available | -| `/system/api/mrm/emergency_stop/status` | `tier4_system_msgs::msg::MrmBehaviorStatus` | Used to check if MRM emergency stop operation is available | +| Name | Type | Description | +| ----------------------------------------- | ----------------------------------------------------- | ----------------------------------------------------------------------------- | +| `/system/emergency/hazard_status` | `autoware_auto_system_msgs::msg::HazardStatusStamped` | Used to select proper MRM from system failure state contained in HazardStatus | +| `/control/vehicle_cmd` | `autoware_control_msgs::msg::Control` | Used as reference when generate Emergency Control Command | +| `/localization/kinematic_state` | `nav_msgs::msg::Odometry` | Used to decide whether vehicle is stopped or not | +| `/vehicle/status/control_mode` | `autoware_auto_vehicle_msgs::msg::ControlModeReport` | Used to check vehicle mode: autonomous or manual | +| `/system/api/mrm/comfortable_stop/status` | `tier4_system_msgs::msg::MrmBehaviorStatus` | Used to check if MRM comfortable stop operation is available | +| `/system/api/mrm/emergency_stop/status` | `tier4_system_msgs::msg::MrmBehaviorStatus` | Used to check if MRM emergency stop operation is available | ### Output diff --git a/system/emergency_handler/include/emergency_handler/emergency_handler_core.hpp b/system/emergency_handler/include/emergency_handler/emergency_handler_core.hpp index 6d83c6781fad7..73429923e172c 100644 --- a/system/emergency_handler/include/emergency_handler/emergency_handler_core.hpp +++ b/system/emergency_handler/include/emergency_handler/emergency_handler_core.hpp @@ -21,11 +21,11 @@ // Autoware #include -#include #include #include #include #include +#include #include #include @@ -61,8 +61,7 @@ class EmergencyHandler : public rclcpp::Node // Subscribers rclcpp::Subscription::SharedPtr sub_hazard_status_stamped_; - rclcpp::Subscription::SharedPtr - sub_prev_control_command_; + rclcpp::Subscription::SharedPtr sub_prev_control_command_; rclcpp::Subscription::SharedPtr sub_odom_; rclcpp::Subscription::SharedPtr sub_control_mode_; @@ -72,7 +71,7 @@ class EmergencyHandler : public rclcpp::Node sub_mrm_emergency_stop_status_; autoware_auto_system_msgs::msg::HazardStatusStamped::ConstSharedPtr hazard_status_stamped_; - autoware_auto_control_msgs::msg::AckermannControlCommand::ConstSharedPtr prev_control_command_; + autoware_control_msgs::msg::Control::ConstSharedPtr prev_control_command_; nav_msgs::msg::Odometry::ConstSharedPtr odom_; autoware_auto_vehicle_msgs::msg::ControlModeReport::ConstSharedPtr control_mode_; tier4_system_msgs::msg::MrmBehaviorStatus::ConstSharedPtr mrm_comfortable_stop_status_; @@ -80,8 +79,7 @@ class EmergencyHandler : public rclcpp::Node void onHazardStatusStamped( const autoware_auto_system_msgs::msg::HazardStatusStamped::ConstSharedPtr msg); - void onPrevControlCommand( - const autoware_auto_control_msgs::msg::AckermannControlCommand::ConstSharedPtr msg); + void onPrevControlCommand(const autoware_control_msgs::msg::Control::ConstSharedPtr msg); void onOdometry(const nav_msgs::msg::Odometry::ConstSharedPtr msg); void onControlMode(const autoware_auto_vehicle_msgs::msg::ControlModeReport::ConstSharedPtr msg); void onMrmComfortableStopStatus( @@ -90,8 +88,7 @@ class EmergencyHandler : public rclcpp::Node const tier4_system_msgs::msg::MrmBehaviorStatus::ConstSharedPtr msg); // Publisher - rclcpp::Publisher::SharedPtr - pub_control_command_; + rclcpp::Publisher::SharedPtr pub_control_command_; // rclcpp::Publisher::SharedPtr pub_shift_; // rclcpp::Publisher::SharedPtr pub_turn_signal_; diff --git a/system/emergency_handler/package.xml b/system/emergency_handler/package.xml index eb87a63503988..733738c7dc28d 100644 --- a/system/emergency_handler/package.xml +++ b/system/emergency_handler/package.xml @@ -11,9 +11,9 @@ autoware_cmake autoware_adapi_v1_msgs - autoware_auto_control_msgs autoware_auto_system_msgs autoware_auto_vehicle_msgs + autoware_control_msgs nav_msgs rclcpp std_msgs diff --git a/system/emergency_handler/src/emergency_handler/emergency_handler_core.cpp b/system/emergency_handler/src/emergency_handler/emergency_handler_core.cpp index 405359e2020ff..d11eec1f7b472 100644 --- a/system/emergency_handler/src/emergency_handler/emergency_handler_core.cpp +++ b/system/emergency_handler/src/emergency_handler/emergency_handler_core.cpp @@ -36,10 +36,9 @@ EmergencyHandler::EmergencyHandler() : Node("emergency_handler") create_subscription( "~/input/hazard_status", rclcpp::QoS{1}, std::bind(&EmergencyHandler::onHazardStatusStamped, this, _1)); - sub_prev_control_command_ = - create_subscription( - "~/input/prev_control_command", rclcpp::QoS{1}, - std::bind(&EmergencyHandler::onPrevControlCommand, this, _1)); + sub_prev_control_command_ = create_subscription( + "~/input/prev_control_command", rclcpp::QoS{1}, + std::bind(&EmergencyHandler::onPrevControlCommand, this, _1)); sub_odom_ = create_subscription( "~/input/odometry", rclcpp::QoS{1}, std::bind(&EmergencyHandler::onOdometry, this, _1)); // subscribe control mode @@ -53,7 +52,7 @@ EmergencyHandler::EmergencyHandler() : Node("emergency_handler") std::bind(&EmergencyHandler::onMrmEmergencyStopStatus, this, _1)); // Publisher - pub_control_command_ = create_publisher( + pub_control_command_ = create_publisher( "~/output/control_command", rclcpp::QoS{1}); pub_hazard_cmd_ = create_publisher( "~/output/hazard", rclcpp::QoS{1}); @@ -77,8 +76,8 @@ EmergencyHandler::EmergencyHandler() : Node("emergency_handler") // Initialize odom_ = std::make_shared(); control_mode_ = std::make_shared(); - prev_control_command_ = autoware_auto_control_msgs::msg::AckermannControlCommand::ConstSharedPtr( - new autoware_auto_control_msgs::msg::AckermannControlCommand); + prev_control_command_ = + autoware_control_msgs::msg::Control::ConstSharedPtr(new autoware_control_msgs::msg::Control); mrm_comfortable_stop_status_ = std::make_shared(); mrm_emergency_stop_status_ = std::make_shared(); @@ -100,12 +99,11 @@ void EmergencyHandler::onHazardStatusStamped( } void EmergencyHandler::onPrevControlCommand( - const autoware_auto_control_msgs::msg::AckermannControlCommand::ConstSharedPtr msg) + const autoware_control_msgs::msg::Control::ConstSharedPtr msg) { - auto control_command = new autoware_auto_control_msgs::msg::AckermannControlCommand(*msg); + auto control_command = new autoware_control_msgs::msg::Control(*msg); control_command->stamp = msg->stamp; - prev_control_command_ = - autoware_auto_control_msgs::msg::AckermannControlCommand::ConstSharedPtr(control_command); + prev_control_command_ = autoware_control_msgs::msg::Control::ConstSharedPtr(control_command); } void EmergencyHandler::onOdometry(const nav_msgs::msg::Odometry::ConstSharedPtr msg) diff --git a/system/mrm_emergency_stop_operator/READEME.md b/system/mrm_emergency_stop_operator/READEME.md index 0866202f4352a..203303c0ea748 100644 --- a/system/mrm_emergency_stop_operator/READEME.md +++ b/system/mrm_emergency_stop_operator/READEME.md @@ -10,18 +10,18 @@ MRM emergency stop operator is a node that generates emergency stop commands acc ### Input -| Name | Type | Description | -| ------------------------------------ | ---------------------------------------------------------- | ----------------------------------------------------------------------------------------------------------------------------- | -| `~/input/mrm/emergency_stop/operate` | `tier4_system_msgs::srv::OperateMrm` | MRM execution order | -| `~/input/control/control_cmd` | `autoware_auto_control_msgs::msg::AckermannControlCommand` | Control command output from the last node of the control component. Used for the initial value of the emergency stop command. | -| | | | +| Name | Type | Description | +| ------------------------------------ | ------------------------------------- | ----------------------------------------------------------------------------------------------------------------------------- | +| `~/input/mrm/emergency_stop/operate` | `tier4_system_msgs::srv::OperateMrm` | MRM execution order | +| `~/input/control/control_cmd` | `autoware_control_msgs::msg::Control` | Control command output from the last node of the control component. Used for the initial value of the emergency stop command. | +| | | | ### Output -| Name | Type | Description | -| ----------------------------------------- | ---------------------------------------------------------- | ---------------------- | -| `~/output/mrm/emergency_stop/status` | `tier4_system_msgs::msg::MrmBehaviorStatus` | MRM execution status | -| `~/output/mrm/emergency_stop/control_cmd` | `autoware_auto_control_msgs::msg::AckermannControlCommand` | Emergency stop command | +| Name | Type | Description | +| ----------------------------------------- | ------------------------------------------- | ---------------------- | +| `~/output/mrm/emergency_stop/status` | `tier4_system_msgs::msg::MrmBehaviorStatus` | MRM execution status | +| `~/output/mrm/emergency_stop/control_cmd` | `autoware_control_msgs::msg::Control` | Emergency stop command | ## Parameters diff --git a/system/mrm_emergency_stop_operator/include/mrm_emergency_stop_operator/mrm_emergency_stop_operator_core.hpp b/system/mrm_emergency_stop_operator/include/mrm_emergency_stop_operator/mrm_emergency_stop_operator_core.hpp index 19e41397fe4da..f732ebdc60265 100644 --- a/system/mrm_emergency_stop_operator/include/mrm_emergency_stop_operator/mrm_emergency_stop_operator_core.hpp +++ b/system/mrm_emergency_stop_operator/include/mrm_emergency_stop_operator/mrm_emergency_stop_operator_core.hpp @@ -20,7 +20,7 @@ #include // Autoware -#include +#include #include #include @@ -29,7 +29,7 @@ namespace mrm_emergency_stop_operator { -using autoware_auto_control_msgs::msg::AckermannControlCommand; +using autoware_control_msgs::msg::Control; using tier4_system_msgs::msg::MrmBehaviorStatus; using tier4_system_msgs::srv::OperateMrm; @@ -50,9 +50,9 @@ class MrmEmergencyStopOperator : public rclcpp::Node Parameters params_; // Subscriber - rclcpp::Subscription::SharedPtr sub_control_cmd_; + rclcpp::Subscription::SharedPtr sub_control_cmd_; - void onControlCommand(AckermannControlCommand::ConstSharedPtr msg); + void onControlCommand(Control::ConstSharedPtr msg); // Server rclcpp::Service::SharedPtr service_operation_; @@ -62,10 +62,10 @@ class MrmEmergencyStopOperator : public rclcpp::Node // Publisher rclcpp::Publisher::SharedPtr pub_status_; - rclcpp::Publisher::SharedPtr pub_control_cmd_; + rclcpp::Publisher::SharedPtr pub_control_cmd_; void publishStatus() const; - void publishControlCommand(const AckermannControlCommand & command) const; + void publishControlCommand(const Control & command) const; // Timer rclcpp::TimerBase::SharedPtr timer_; @@ -74,12 +74,11 @@ class MrmEmergencyStopOperator : public rclcpp::Node // States MrmBehaviorStatus status_; - AckermannControlCommand prev_control_cmd_; + Control prev_control_cmd_; bool is_prev_control_cmd_subscribed_; // Algorithm - AckermannControlCommand calcTargetAcceleration( - const AckermannControlCommand & prev_control_cmd) const; + Control calcTargetAcceleration(const Control & prev_control_cmd) const; }; } // namespace mrm_emergency_stop_operator diff --git a/system/mrm_emergency_stop_operator/package.xml b/system/mrm_emergency_stop_operator/package.xml index 54ad68f00537a..c49ef9f2f53d4 100644 --- a/system/mrm_emergency_stop_operator/package.xml +++ b/system/mrm_emergency_stop_operator/package.xml @@ -11,7 +11,7 @@ autoware_cmake autoware_adapi_v1_msgs - autoware_auto_control_msgs + autoware_control_msgs rclcpp rclcpp_components std_msgs diff --git a/system/mrm_emergency_stop_operator/src/mrm_emergency_stop_operator/mrm_emergency_stop_operator_core.cpp b/system/mrm_emergency_stop_operator/src/mrm_emergency_stop_operator/mrm_emergency_stop_operator_core.cpp index 08814dbbd710d..16282c6febd5f 100644 --- a/system/mrm_emergency_stop_operator/src/mrm_emergency_stop_operator/mrm_emergency_stop_operator_core.cpp +++ b/system/mrm_emergency_stop_operator/src/mrm_emergency_stop_operator/mrm_emergency_stop_operator_core.cpp @@ -26,7 +26,7 @@ MrmEmergencyStopOperator::MrmEmergencyStopOperator(const rclcpp::NodeOptions & n params_.target_jerk = declare_parameter("target_jerk", -1.5); // Subscriber - sub_control_cmd_ = create_subscription( + sub_control_cmd_ = create_subscription( "~/input/control/control_cmd", 1, std::bind(&MrmEmergencyStopOperator::onControlCommand, this, std::placeholders::_1)); @@ -38,8 +38,7 @@ MrmEmergencyStopOperator::MrmEmergencyStopOperator(const rclcpp::NodeOptions & n // Publisher pub_status_ = create_publisher("~/output/mrm/emergency_stop/status", 1); - pub_control_cmd_ = - create_publisher("~/output/mrm/emergency_stop/control_cmd", 1); + pub_control_cmd_ = create_publisher("~/output/mrm/emergency_stop/control_cmd", 1); // Timer const auto update_period_ns = rclcpp::Rate(params_.update_rate).period(); @@ -51,7 +50,7 @@ MrmEmergencyStopOperator::MrmEmergencyStopOperator(const rclcpp::NodeOptions & n is_prev_control_cmd_subscribed_ = false; } -void MrmEmergencyStopOperator::onControlCommand(AckermannControlCommand::ConstSharedPtr msg) +void MrmEmergencyStopOperator::onControlCommand(Control::ConstSharedPtr msg) { if (status_.state != MrmBehaviorStatus::OPERATING) { prev_control_cmd_ = *msg; @@ -78,7 +77,7 @@ void MrmEmergencyStopOperator::publishStatus() const pub_status_->publish(status); } -void MrmEmergencyStopOperator::publishControlCommand(const AckermannControlCommand & command) const +void MrmEmergencyStopOperator::publishControlCommand(const Control & command) const { pub_control_cmd_->publish(command); } @@ -95,15 +94,14 @@ void MrmEmergencyStopOperator::onTimer() publishStatus(); } -AckermannControlCommand MrmEmergencyStopOperator::calcTargetAcceleration( - const AckermannControlCommand & prev_control_cmd) const +Control MrmEmergencyStopOperator::calcTargetAcceleration(const Control & prev_control_cmd) const { - auto control_cmd = AckermannControlCommand(); + auto control_cmd = Control(); if (!is_prev_control_cmd_subscribed_) { control_cmd.stamp = this->now(); control_cmd.longitudinal.stamp = this->now(); - control_cmd.longitudinal.speed = 0.0; + control_cmd.longitudinal.velocity = 0.0; control_cmd.longitudinal.acceleration = static_cast(params_.target_acceleration); control_cmd.longitudinal.jerk = 0.0; control_cmd.lateral.stamp = this->now(); @@ -117,8 +115,8 @@ AckermannControlCommand MrmEmergencyStopOperator::calcTargetAcceleration( control_cmd.stamp = this->now(); control_cmd.longitudinal.stamp = this->now(); - control_cmd.longitudinal.speed = static_cast(std::max( - prev_control_cmd.longitudinal.speed + prev_control_cmd.longitudinal.acceleration * dt, 0.0)); + control_cmd.longitudinal.velocity = static_cast(std::max( + prev_control_cmd.longitudinal.velocity + prev_control_cmd.longitudinal.acceleration * dt, 0.0)); control_cmd.longitudinal.acceleration = static_cast(std::max( prev_control_cmd.longitudinal.acceleration + params_.target_jerk * dt, params_.target_acceleration)); diff --git a/tools/simulator_test/simulator_compatibility_test/package.xml b/tools/simulator_test/simulator_compatibility_test/package.xml index ffc679ef2964c..b703c9d23405f 100644 --- a/tools/simulator_test/simulator_compatibility_test/package.xml +++ b/tools/simulator_test/simulator_compatibility_test/package.xml @@ -7,7 +7,6 @@ shpark TODO: License declaration - autoware_auto_control_msgs autoware_auto_geometry_msgs autoware_auto_mapping_msgs autoware_auto_msgs @@ -15,6 +14,7 @@ autoware_auto_planning_msgs autoware_auto_system_msgs autoware_auto_vehicle_msgs + autoware_control_msgs rclpy ament_copyright diff --git a/tools/simulator_test/simulator_compatibility_test/simulator_compatibility_test/publishers/ackermann_control_command.py b/tools/simulator_test/simulator_compatibility_test/simulator_compatibility_test/publishers/ackermann_control_command.py index 9e5b0dfb050ca..ce2c03f15a678 100644 --- a/tools/simulator_test/simulator_compatibility_test/simulator_compatibility_test/publishers/ackermann_control_command.py +++ b/tools/simulator_test/simulator_compatibility_test/simulator_compatibility_test/publishers/ackermann_control_command.py @@ -1,6 +1,6 @@ -from autoware_auto_control_msgs.msg import AckermannControlCommand -from autoware_auto_control_msgs.msg import AckermannLateralCommand -from autoware_auto_control_msgs.msg import LongitudinalCommand +from autoware_control_msgs.msg import Control +from autoware_control_msgs.msg import Lateral +from autoware_control_msgs.msg import Longitudinal import rclpy from rclpy.node import Node from rclpy.qos import QoSDurabilityPolicy @@ -9,7 +9,7 @@ from rclpy.qos import QoSReliabilityPolicy -class PublisherAckermannControlCommand(Node): +class PublisherControl(Node): def __init__(self): super().__init__("ackermann_control_command_publisher") @@ -23,13 +23,13 @@ def __init__(self): durability=QoSDurabilityPolicy.TRANSIENT_LOCAL, ) self.topic = "/control/command/control_cmd" - self.publisher_ = self.create_publisher(AckermannControlCommand, self.topic, QOS_RKL10TL) + self.publisher_ = self.create_publisher(Control, self.topic, QOS_RKL10TL) def publish_msg(self, control_cmd): stamp = self.get_clock().now().to_msg() - msg = AckermannControlCommand() - lateral_cmd = AckermannLateralCommand() - longitudinal_cmd = LongitudinalCommand() + msg = Control() + lateral_cmd = Lateral() + longitudinal_cmd = Longitudinal() lateral_cmd.stamp.sec = stamp.sec lateral_cmd.stamp.nanosec = stamp.nanosec lateral_cmd.steering_tire_angle = control_cmd["lateral"]["steering_tire_angle"] @@ -38,7 +38,7 @@ def publish_msg(self, control_cmd): ] longitudinal_cmd.stamp.sec = stamp.sec longitudinal_cmd.stamp.nanosec = stamp.nanosec - longitudinal_cmd.speed = control_cmd["longitudinal"]["speed"] + longitudinal_cmd.velocity = control_cmd["longitudinal"]["speed"] longitudinal_cmd.acceleration = control_cmd["longitudinal"]["acceleration"] longitudinal_cmd.jerk = control_cmd["longitudinal"]["jerk"] @@ -53,7 +53,7 @@ def publish_msg(self, control_cmd): def main(args=None): rclpy.init(args=args) - node = PublisherAckermannControlCommand() + node = PublisherControl() try: rclpy.spin(node) except KeyboardInterrupt: diff --git a/tools/simulator_test/simulator_compatibility_test/test_base/test_03_longitudinal_command_and_report.py b/tools/simulator_test/simulator_compatibility_test/test_base/test_03_longitudinal_command_and_report.py index d55fa7eb1245f..a8de036d80750 100644 --- a/tools/simulator_test/simulator_compatibility_test/test_base/test_03_longitudinal_command_and_report.py +++ b/tools/simulator_test/simulator_compatibility_test/test_base/test_03_longitudinal_command_and_report.py @@ -1,8 +1,8 @@ import time -from autoware_auto_control_msgs.msg import AckermannControlCommand -from autoware_auto_control_msgs.msg import AckermannLateralCommand -from autoware_auto_control_msgs.msg import LongitudinalCommand +from autoware_control_msgs.msg import Control +from autoware_control_msgs.msg import Lateral +from autoware_control_msgs.msg import Longitudinal import pytest import rclpy from rclpy.executors import MultiThreadedExecutor @@ -39,14 +39,12 @@ def setup_class(cls) -> None: cls.node = rclpy.create_node("test_03_longitudinal_command_and_report_base") cls.sub = cls.node.create_subscription( - AckermannControlCommand, + Control, "/control/command/control_cmd", lambda msg: cls.msgs_rx.append(msg), 10, ) - cls.pub = cls.node.create_publisher( - AckermannControlCommand, "/control/command/control_cmd", QOS_RKL10TL - ) + cls.pub = cls.node.create_publisher(Control, "/control/command/control_cmd", QOS_RKL10TL) cls.sub_velocity_report = SubscriberVelocityReport() cls.executor = MultiThreadedExecutor() cls.executor.add_node(cls.sub_velocity_report) @@ -71,9 +69,9 @@ def init_vehicle(self): def generate_control_msg(self, control_cmd): stamp = self.node.get_clock().now().to_msg() - msg = AckermannControlCommand() - lateral_cmd = AckermannLateralCommand() - longitudinal_cmd = LongitudinalCommand() + msg = Control() + lateral_cmd = Lateral() + longitudinal_cmd = Longitudinal() lateral_cmd.stamp.sec = stamp.sec lateral_cmd.stamp.nanosec = stamp.nanosec lateral_cmd.steering_tire_angle = control_cmd["lateral"]["steering_tire_angle"] @@ -82,7 +80,7 @@ def generate_control_msg(self, control_cmd): ] longitudinal_cmd.stamp.sec = stamp.sec longitudinal_cmd.stamp.nanosec = stamp.nanosec - longitudinal_cmd.speed = control_cmd["longitudinal"]["speed"] + longitudinal_cmd.velocity = control_cmd["longitudinal"]["speed"] longitudinal_cmd.acceleration = control_cmd["longitudinal"]["acceleration"] longitudinal_cmd.jerk = control_cmd["longitudinal"]["jerk"] @@ -101,7 +99,7 @@ def set_speed(self, speed): if len(self.msgs_rx) > 2: break received = self.msgs_rx[-1] - assert received.longitudinal.speed == speed + assert received.longitudinal.velocity == speed self.msgs_rx.clear() def set_acceleration(self, acceleration): diff --git a/tools/simulator_test/simulator_compatibility_test/test_base/test_04_lateral_command_and_report.py b/tools/simulator_test/simulator_compatibility_test/test_base/test_04_lateral_command_and_report.py index 0648ffd2bee6c..70aaf76d73c97 100644 --- a/tools/simulator_test/simulator_compatibility_test/test_base/test_04_lateral_command_and_report.py +++ b/tools/simulator_test/simulator_compatibility_test/test_base/test_04_lateral_command_and_report.py @@ -1,8 +1,8 @@ import time -from autoware_auto_control_msgs.msg import AckermannControlCommand -from autoware_auto_control_msgs.msg import AckermannLateralCommand -from autoware_auto_control_msgs.msg import LongitudinalCommand +from autoware_control_msgs.msg import Control +from autoware_control_msgs.msg import Lateral +from autoware_control_msgs.msg import Longitudinal import pytest import rclpy from rclpy.executors import MultiThreadedExecutor @@ -38,14 +38,12 @@ def setup_class(cls) -> None: } cls.node = rclpy.create_node("test_04_lateral_command_and_report_base") cls.sub = cls.node.create_subscription( - AckermannControlCommand, + Control, "/control/command/control_cmd", lambda msg: cls.msgs_rx.append(msg), 10, ) - cls.pub = cls.node.create_publisher( - AckermannControlCommand, "/control/command/control_cmd", QOS_RKL10TL - ) + cls.pub = cls.node.create_publisher(Control, "/control/command/control_cmd", QOS_RKL10TL) cls.sub_steering_report = SubscriberSteeringReport() cls.executor = MultiThreadedExecutor() cls.executor.add_node(cls.sub_steering_report) @@ -70,9 +68,9 @@ def init_vehicle(self): def generate_control_msg(self, control_cmd): stamp = self.node.get_clock().now().to_msg() - msg = AckermannControlCommand() - lateral_cmd = AckermannLateralCommand() - longitudinal_cmd = LongitudinalCommand() + msg = Control() + lateral_cmd = Lateral() + longitudinal_cmd = Longitudinal() lateral_cmd.stamp.sec = stamp.sec lateral_cmd.stamp.nanosec = stamp.nanosec lateral_cmd.steering_tire_angle = control_cmd["lateral"]["steering_tire_angle"] @@ -81,7 +79,7 @@ def generate_control_msg(self, control_cmd): ] longitudinal_cmd.stamp.sec = stamp.sec longitudinal_cmd.stamp.nanosec = stamp.nanosec - longitudinal_cmd.speed = control_cmd["longitudinal"]["speed"] + longitudinal_cmd.velocity = control_cmd["longitudinal"]["speed"] longitudinal_cmd.acceleration = control_cmd["longitudinal"]["acceleration"] longitudinal_cmd.jerk = control_cmd["longitudinal"]["jerk"] diff --git a/vehicle/external_cmd_converter/README.md b/vehicle/external_cmd_converter/README.md index 3c4181a6f093b..d18b18d180224 100644 --- a/vehicle/external_cmd_converter/README.md +++ b/vehicle/external_cmd_converter/README.md @@ -14,9 +14,9 @@ ## Output topics -| Name | Type | Description | -| ------------------- | -------------------------------------------------------- | ------------------------------------------------------------------ | -| `~/out/control_cmd` | autoware_auto_control_msgs::msg::AckermannControlCommand | ackermann control command converted from selected external command | +| Name | Type | Description | +| ------------------- | ----------------------------------- | ------------------------------------------------------------------ | +| `~/out/control_cmd` | autoware_control_msgs::msg::Control | ackermann control command converted from selected external command | ## Parameters diff --git a/vehicle/external_cmd_converter/include/external_cmd_converter/node.hpp b/vehicle/external_cmd_converter/include/external_cmd_converter/node.hpp index f5a52ad4a6032..6ed4fba1cde4c 100644 --- a/vehicle/external_cmd_converter/include/external_cmd_converter/node.hpp +++ b/vehicle/external_cmd_converter/include/external_cmd_converter/node.hpp @@ -20,8 +20,8 @@ #include #include -#include #include +#include #include #include #include @@ -33,12 +33,12 @@ namespace external_cmd_converter { using GearCommand = autoware_auto_vehicle_msgs::msg::GearCommand; -using AckermannControlCommand = autoware_auto_control_msgs::msg::AckermannControlCommand; +using Control = autoware_control_msgs::msg::Control; using ExternalControlCommand = tier4_external_api_msgs::msg::ControlCommandStamped; using Odometry = nav_msgs::msg::Odometry; using raw_vehicle_cmd_converter::AccelMap; using raw_vehicle_cmd_converter::BrakeMap; -using ControlCommandStamped = autoware_auto_control_msgs::msg::AckermannControlCommand; +using ControlCommandStamped = autoware_control_msgs::msg::Control; using Odometry = nav_msgs::msg::Odometry; class ExternalCmdConverterNode : public rclcpp::Node @@ -48,7 +48,7 @@ class ExternalCmdConverterNode : public rclcpp::Node private: // Publisher - rclcpp::Publisher::SharedPtr pub_cmd_; + rclcpp::Publisher::SharedPtr pub_cmd_; rclcpp::Publisher::SharedPtr pub_current_cmd_; diff --git a/vehicle/external_cmd_converter/package.xml b/vehicle/external_cmd_converter/package.xml index d9301e23e05fe..e2a48e55adc39 100644 --- a/vehicle/external_cmd_converter/package.xml +++ b/vehicle/external_cmd_converter/package.xml @@ -12,8 +12,8 @@ ament_cmake_auto autoware_cmake - autoware_auto_control_msgs autoware_auto_vehicle_msgs + autoware_control_msgs diagnostic_updater geometry_msgs nav_msgs diff --git a/vehicle/external_cmd_converter/src/node.cpp b/vehicle/external_cmd_converter/src/node.cpp index 2a310b16c0841..fe407e3022ed8 100644 --- a/vehicle/external_cmd_converter/src/node.cpp +++ b/vehicle/external_cmd_converter/src/node.cpp @@ -26,7 +26,7 @@ ExternalCmdConverterNode::ExternalCmdConverterNode(const rclcpp::NodeOptions & n { using std::placeholders::_1; - pub_cmd_ = create_publisher("out/control_cmd", rclcpp::QoS{1}); + pub_cmd_ = create_publisher("out/control_cmd", rclcpp::QoS{1}); pub_current_cmd_ = create_publisher("out/latest_external_control_cmd", rclcpp::QoS{1}); sub_velocity_ = create_subscription( @@ -142,11 +142,11 @@ void ExternalCmdConverterNode::onExternalCmd(const ExternalControlCommand::Const } // Publish ControlCommand - autoware_auto_control_msgs::msg::AckermannControlCommand output; + autoware_control_msgs::msg::Control output; output.stamp = cmd_ptr->stamp; output.lateral.steering_tire_angle = cmd_ptr->control.steering_angle; output.lateral.steering_tire_rotation_rate = cmd_ptr->control.steering_angle_velocity; - output.longitudinal.speed = ref_velocity; + output.longitudinal.velocity = ref_velocity; output.longitudinal.acceleration = ref_acceleration; pub_cmd_->publish(output); diff --git a/vehicle/raw_vehicle_cmd_converter/README.md b/vehicle/raw_vehicle_cmd_converter/README.md index 94b0256ab5911..3b14f026333e6 100644 --- a/vehicle/raw_vehicle_cmd_converter/README.md +++ b/vehicle/raw_vehicle_cmd_converter/README.md @@ -4,11 +4,11 @@ ## Input topics -| Name | Type | Description | -| --------------------- | -------------------------------------------------------- | ------------------------------------------------------------------------------------------------------------------ | -| `~/input/control_cmd` | autoware_auto_control_msgs::msg::AckermannControlCommand | target `velocity/acceleration/steering_angle/steering_angle_velocity` is necessary to calculate actuation command. | -| `~/input/steering"` | autoware_auto_vehicle_msgs::SteeringReport | current status of steering used for steering feed back control | -| `~/input/twist` | navigation_msgs::Odometry | twist topic in odometry is used. | +| Name | Type | Description | +| --------------------- | ------------------------------------------ | ------------------------------------------------------------------------------------------------------------------ | +| `~/input/control_cmd` | autoware_control_msgs::msg::Control | target `velocity/acceleration/steering_angle/steering_angle_velocity` is necessary to calculate actuation command. | +| `~/input/steering"` | autoware_auto_vehicle_msgs::SteeringReport | current status of steering used for steering feed back control | +| `~/input/twist` | navigation_msgs::Odometry | twist topic in odometry is used. | ## Output topics diff --git a/vehicle/raw_vehicle_cmd_converter/include/raw_vehicle_cmd_converter/node.hpp b/vehicle/raw_vehicle_cmd_converter/include/raw_vehicle_cmd_converter/node.hpp index 27346e99b60fb..b2e9eb614d402 100644 --- a/vehicle/raw_vehicle_cmd_converter/include/raw_vehicle_cmd_converter/node.hpp +++ b/vehicle/raw_vehicle_cmd_converter/include/raw_vehicle_cmd_converter/node.hpp @@ -22,8 +22,8 @@ #include -#include #include +#include #include #include #include @@ -35,7 +35,7 @@ namespace raw_vehicle_cmd_converter { -using AckermannControlCommand = autoware_auto_control_msgs::msg::AckermannControlCommand; +using Control = autoware_control_msgs::msg::Control; using tier4_debug_msgs::msg::Float32MultiArrayStamped; using tier4_vehicle_msgs::msg::ActuationCommandStamped; using TwistStamped = geometry_msgs::msg::TwistStamped; @@ -76,7 +76,7 @@ class RawVehicleCommandConverterNode : public rclcpp::Node //!< @brief subscriber for current velocity rclcpp::Subscription::SharedPtr sub_velocity_; //!< @brief subscriber for vehicle command - rclcpp::Subscription::SharedPtr sub_control_cmd_; + rclcpp::Subscription::SharedPtr sub_control_cmd_; //!< @brief subscriber for steering rclcpp::Subscription::SharedPtr sub_steering_; @@ -84,7 +84,7 @@ class RawVehicleCommandConverterNode : public rclcpp::Node std::unique_ptr current_twist_ptr_; // [m/s] std::unique_ptr current_steer_ptr_; - AckermannControlCommand::ConstSharedPtr control_cmd_ptr_; + Control::ConstSharedPtr control_cmd_ptr_; AccelMap accel_map_; BrakeMap brake_map_; SteerMap steer_map_; @@ -109,7 +109,7 @@ class RawVehicleCommandConverterNode : public rclcpp::Node double calculateBrakeMap(const double current_velocity, const double desired_acc); double calculateSteer(const double vel, const double steering, const double steer_rate); void onSteering(const Steering::ConstSharedPtr msg); - void onControlCmd(const AckermannControlCommand::ConstSharedPtr msg); + void onControlCmd(const Control::ConstSharedPtr msg); void onVelocity(const Odometry::ConstSharedPtr msg); void publishActuationCmd(); // for debugging diff --git a/vehicle/raw_vehicle_cmd_converter/package.xml b/vehicle/raw_vehicle_cmd_converter/package.xml index d68f4601aab67..1ee9268ac2045 100644 --- a/vehicle/raw_vehicle_cmd_converter/package.xml +++ b/vehicle/raw_vehicle_cmd_converter/package.xml @@ -17,8 +17,8 @@ ament_cmake_auto autoware_cmake - autoware_auto_control_msgs autoware_auto_vehicle_msgs + autoware_control_msgs geometry_msgs interpolation nav_msgs diff --git a/vehicle/raw_vehicle_cmd_converter/src/node.cpp b/vehicle/raw_vehicle_cmd_converter/src/node.cpp index e45439ecb4d41..13ab5d84230db 100644 --- a/vehicle/raw_vehicle_cmd_converter/src/node.cpp +++ b/vehicle/raw_vehicle_cmd_converter/src/node.cpp @@ -76,7 +76,7 @@ RawVehicleCommandConverterNode::RawVehicleCommandConverterNode( steer_pid_.setInitialized(); } pub_actuation_cmd_ = create_publisher("~/output/actuation_cmd", 1); - sub_control_cmd_ = create_subscription( + sub_control_cmd_ = create_subscription( "~/input/control_cmd", 1, std::bind(&RawVehicleCommandConverterNode::onControlCmd, this, _1)); sub_velocity_ = create_subscription( "~/input/odometry", 1, std::bind(&RawVehicleCommandConverterNode::onVelocity, this, _1)); @@ -214,7 +214,7 @@ void RawVehicleCommandConverterNode::onVelocity(const Odometry::ConstSharedPtr m current_twist_ptr_->twist = msg->twist.twist; } -void RawVehicleCommandConverterNode::onControlCmd(const AckermannControlCommand::ConstSharedPtr msg) +void RawVehicleCommandConverterNode::onControlCmd(const Control::ConstSharedPtr msg) { control_cmd_ptr_ = msg; publishActuationCmd();