From 7b50687402f4accaf0cb232ed1ea96835cf8105c Mon Sep 17 00:00:00 2001 From: Ryohsuke Mitsudome Date: Mon, 3 Jun 2024 23:43:15 +0900 Subject: [PATCH] feat!: replace autoware_auto_msgs with autoware_msgs for control modules Signed-off-by: Ryohsuke Mitsudome Co-authored-by: Cynthia Liu Co-authored-by: NorahXiong Co-authored-by: beginningfan --- .../autonomous_emergency_braking/node.hpp | 12 ++-- .../autonomous_emergency_braking/package.xml | 7 ++- .../control_performance_analysis/README.md | 14 ++--- .../control_performance_analysis_core.hpp | 18 +++--- .../control_performance_analysis_node.hpp | 25 ++++---- .../control_performance_analysis/package.xml | 6 +- .../src/control_performance_analysis_core.cpp | 4 +- .../src/control_performance_analysis_node.cpp | 7 +-- control/control_validator/README.md | 10 +-- .../control_validator/control_validator.hpp | 6 +- .../control_validator/debug_marker.hpp | 5 +- .../include/control_validator/utils.hpp | 6 +- control/control_validator/package.xml | 2 +- .../control_validator/src/debug_marker.cpp | 2 +- control/external_cmd_selector/README.md | 18 +++--- .../external_cmd_selector_node.hpp | 12 ++-- control/external_cmd_selector/package.xml | 2 +- control/joy_controller/README.md | 18 +++--- .../include/joy_controller/joy_controller.hpp | 11 ++-- control/joy_controller/package.xml | 4 +- .../joy_controller/joy_controller_node.cpp | 23 ++++--- control/lane_departure_checker/README.md | 6 +- .../lane_departure_checker.hpp | 12 ++-- .../lane_departure_checker_node.hpp | 10 +-- control/lane_departure_checker/package.xml | 3 +- .../lane_departure_checker.cpp | 4 +- .../lane_departure_checker_node.cpp | 4 +- control/mpc_lateral_controller/README.md | 6 +- .../include/mpc_lateral_controller/mpc.hpp | 19 +++--- .../mpc_lateral_controller.hpp | 27 ++++---- .../mpc_lateral_controller/mpc_trajectory.hpp | 6 +- .../mpc_lateral_controller/mpc_utils.hpp | 8 +-- .../steering_predictor.hpp | 6 +- control/mpc_lateral_controller/package.xml | 6 +- control/mpc_lateral_controller/src/mpc.cpp | 7 +-- .../src/mpc_lateral_controller.cpp | 17 +++--- .../src/steering_predictor.cpp | 2 +- .../mpc_lateral_controller/test/test_mpc.cpp | 34 +++++------ .../test/test_mpc_utils.cpp | 8 +-- control/obstacle_collision_checker/README.md | 14 ++--- .../obstacle_collision_checker.hpp | 18 +++--- .../obstacle_collision_checker_node.hpp | 14 ++--- .../obstacle_collision_checker/package.xml | 2 +- .../obstacle_collision_checker.cpp | 18 +++--- .../obstacle_collision_checker_node.cpp | 8 +-- .../test/test_obstacle_collision_checker.cpp | 4 +- .../README.md | 12 ++-- .../package.xml | 5 +- .../src/compatibility.hpp | 4 +- .../src/data.hpp | 8 +-- .../src/state.cpp | 16 +++-- .../src/state.hpp | 16 ++--- control/pid_longitudinal_controller/README.md | 4 +- .../longitudinal_controller_utils.hpp | 8 +-- .../pid_longitudinal_controller.hpp | 19 +++--- .../pid_longitudinal_controller/package.xml | 6 +- .../src/longitudinal_controller_utils.cpp | 2 +- .../src/pid_longitudinal_controller.cpp | 16 +++-- .../test_longitudinal_controller_utils.cpp | 24 ++++---- control/predicted_path_checker/README.md | 16 ++--- .../collision_checker.hpp | 8 +-- .../predicted_path_checker_node.hpp | 22 +++---- .../include/predicted_path_checker/utils.hpp | 16 ++--- control/predicted_path_checker/package.xml | 4 +- .../predicted_path_checker_node.cpp | 8 +-- .../src/predicted_path_checker_node/utils.cpp | 18 +++--- control/pure_pursuit/README.md | 6 +- .../pure_pursuit_lateral_controller.hpp | 34 +++++------ .../pure_pursuit/pure_pursuit_node.hpp | 15 +++-- .../pure_pursuit/util/planning_utils.hpp | 6 +- control/pure_pursuit/package.xml | 4 +- .../pure_pursuit_lateral_controller.cpp | 27 ++++---- .../src/pure_pursuit/pure_pursuit_node.cpp | 14 ++--- .../src/pure_pursuit_core/planning_utils.cpp | 4 +- control/shift_decider/README.md | 12 ++-- .../include/shift_decider/shift_decider.hpp | 34 +++++------ control/shift_decider/package.xml | 6 +- control/shift_decider/src/shift_decider.cpp | 23 ++++--- .../smart_mpc_trajectory_follower/package.xml | 8 +-- .../scripts/pympc_trajectory_follower.py | 14 ++--- .../training_and_data_check/rosbag2.bash | 8 +-- .../trajectory_follower_base/input_data.hpp | 8 +-- .../lateral_controller_base.hpp | 4 +- .../longitudinal_controller_base.hpp | 4 +- control/trajectory_follower_base/package.xml | 6 +- control/trajectory_follower_node/README.md | 8 +-- .../simple_trajectory_follower-design.md | 4 +- .../controller_node.hpp | 22 +++---- .../simple_trajectory_follower.hpp | 14 ++--- control/trajectory_follower_node/package.xml | 7 +-- .../src/controller_node.cpp | 12 ++-- .../src/simple_trajectory_follower.cpp | 9 +-- .../test/test_controller_node.cpp | 53 ++++++++-------- control/vehicle_cmd_gate/README.md | 58 +++++++++--------- control/vehicle_cmd_gate/package.xml | 5 +- .../vehicle_cmd_gate/script/delays_checker.py | 8 +-- .../src/adapi_pause_interface.cpp | 4 +- .../src/adapi_pause_interface.hpp | 6 +- .../src/moderate_stop_interface.hpp | 2 - .../src/vehicle_cmd_filter.cpp | 39 ++++++------ .../src/vehicle_cmd_filter.hpp | 36 ++++++----- .../vehicle_cmd_gate/src/vehicle_cmd_gate.cpp | 54 ++++++++-------- .../vehicle_cmd_gate/src/vehicle_cmd_gate.hpp | 54 ++++++++-------- .../test_filter_in_vehicle_cmd_gate_node.cpp | 44 +++++++------ .../test/src/test_vehicle_cmd_filter.cpp | 61 +++++++++---------- 105 files changed, 703 insertions(+), 741 deletions(-) diff --git a/control/autonomous_emergency_braking/include/autonomous_emergency_braking/node.hpp b/control/autonomous_emergency_braking/include/autonomous_emergency_braking/node.hpp index 27f04603d6a31..81ab2ecff790b 100644 --- a/control/autonomous_emergency_braking/include/autonomous_emergency_braking/node.hpp +++ b/control/autonomous_emergency_braking/include/autonomous_emergency_braking/node.hpp @@ -23,9 +23,9 @@ #include #include -#include -#include -#include +#include +#include +#include #include #include #include @@ -52,9 +52,9 @@ namespace autoware::motion::control::autonomous_emergency_braking { -using autoware_auto_planning_msgs::msg::Trajectory; -using autoware_auto_system_msgs::msg::AutowareState; -using autoware_auto_vehicle_msgs::msg::VelocityReport; +using autoware_planning_msgs::msg::Trajectory; +using autoware_system_msgs::msg::AutowareState; +using autoware_vehicle_msgs::msg::VelocityReport; using nav_msgs::msg::Odometry; using sensor_msgs::msg::Imu; using sensor_msgs::msg::PointCloud2; diff --git a/control/autonomous_emergency_braking/package.xml b/control/autonomous_emergency_braking/package.xml index 68c070a86dd97..bf27e6b7e1575 100644 --- a/control/autonomous_emergency_braking/package.xml +++ b/control/autonomous_emergency_braking/package.xml @@ -14,9 +14,10 @@ ament_cmake autoware_cmake - autoware_auto_control_msgs - autoware_auto_system_msgs - autoware_auto_vehicle_msgs + autoware_control_msgs + autoware_planning_msgs + autoware_system_msgs + autoware_vehicle_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..3895b3cc13465 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_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_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 6bf8fb1c34f5e..6ecf453bf7ab3 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 #include #include @@ -39,9 +39,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 autoware_planning_msgs::msg::Trajectory; +using autoware_vehicle_msgs::msg::SteeringReport; using control_performance_analysis::msg::DrivingMonitorStamped; using control_performance_analysis::msg::Error; using control_performance_analysis::msg::ErrorStamped; @@ -73,7 +73,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); @@ -100,10 +100,10 @@ class ControlPerformanceAnalysisCore Params p_; // Variables Received Outside - std::shared_ptr current_trajectory_ptr_; + 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..73d48db6400a5 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 #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 autoware_planning_msgs::msg::Trajectory; +using autoware_vehicle_msgs::msg::SteeringReport; 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..a26f4164a177b 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 + autoware_planning_msgs + autoware_vehicle_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 f4ed125956bfa..51c818205e047 100644 --- a/control/control_performance_analysis/src/control_performance_analysis_core.cpp +++ b/control/control_performance_analysis/src/control_performance_analysis_core.cpp @@ -75,9 +75,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 768aed321b4f0..693194e8b70a6 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/control_validator/README.md b/control/control_validator/README.md index 3d78721a0a040..9c4a9be0732a5 100644 --- a/control/control_validator/README.md +++ b/control/control_validator/README.md @@ -20,11 +20,11 @@ Other features are to be implemented. The `control_validator` takes in the following inputs: -| Name | Type | Description | -| ------------------------------ | ------------------------------------- | ------------------------------------------------------------------------------ | -| `~/input/kinematics` | nav_msgs/Odometry | ego pose and twist | -| `~/input/reference_trajectory` | autoware_auto_control_msgs/Trajectory | reference trajectory which is outputted from planning module to to be followed | -| `~/input/predicted_trajectory` | autoware_auto_control_msgs/Trajectory | predicted trajectory which is outputted from control module | +| Name | Type | Description | +| ------------------------------ | --------------------------------- | ------------------------------------------------------------------------------ | +| `~/input/kinematics` | nav_msgs/Odometry | ego pose and twist | +| `~/input/reference_trajectory` | autoware_planning_msgs/Trajectory | reference trajectory which is outputted from planning module to to be followed | +| `~/input/predicted_trajectory` | autoware_planning_msgs/Trajectory | predicted trajectory which is outputted from control module | ### Outputs diff --git a/control/control_validator/include/control_validator/control_validator.hpp b/control/control_validator/include/control_validator/control_validator.hpp index eba9bf700ba08..e048ef03bf11a 100644 --- a/control/control_validator/include/control_validator/control_validator.hpp +++ b/control/control_validator/include/control_validator/control_validator.hpp @@ -22,7 +22,7 @@ #include #include -#include +#include #include #include @@ -31,8 +31,8 @@ namespace control_validator { -using autoware_auto_planning_msgs::msg::Trajectory; -using autoware_auto_planning_msgs::msg::TrajectoryPoint; +using autoware_planning_msgs::msg::Trajectory; +using autoware_planning_msgs::msg::TrajectoryPoint; using control_validator::msg::ControlValidatorStatus; using diagnostic_updater::DiagnosticStatusWrapper; using diagnostic_updater::Updater; diff --git a/control/control_validator/include/control_validator/debug_marker.hpp b/control/control_validator/include/control_validator/debug_marker.hpp index 794912e085949..2d3a209cbd7da 100644 --- a/control/control_validator/include/control_validator/debug_marker.hpp +++ b/control/control_validator/include/control_validator/debug_marker.hpp @@ -17,7 +17,7 @@ #include -#include +#include #include #include @@ -32,8 +32,7 @@ class ControlValidatorDebugMarkerPublisher explicit ControlValidatorDebugMarkerPublisher(rclcpp::Node * node); void pushPoseMarker( - const autoware_auto_planning_msgs::msg::TrajectoryPoint & p, const std::string & ns, - int id = 0); + const autoware_planning_msgs::msg::TrajectoryPoint & p, const std::string & ns, int id = 0); void pushPoseMarker(const geometry_msgs::msg::Pose & pose, const std::string & ns, int id = 0); void pushVirtualWall(const geometry_msgs::msg::Pose & pose); void pushWarningMsg(const geometry_msgs::msg::Pose & pose, const std::string & msg); diff --git a/control/control_validator/include/control_validator/utils.hpp b/control/control_validator/include/control_validator/utils.hpp index 8033ac9442960..edf97aaf5f510 100644 --- a/control/control_validator/include/control_validator/utils.hpp +++ b/control/control_validator/include/control_validator/utils.hpp @@ -18,7 +18,7 @@ #include #include -#include +#include #include #include @@ -26,8 +26,8 @@ namespace control_validator { -using autoware_auto_planning_msgs::msg::Trajectory; -using autoware_auto_planning_msgs::msg::TrajectoryPoint; +using autoware_planning_msgs::msg::Trajectory; +using autoware_planning_msgs::msg::TrajectoryPoint; using geometry_msgs::msg::Pose; using motion_utils::convertToTrajectory; using motion_utils::convertToTrajectoryPointArray; diff --git a/control/control_validator/package.xml b/control/control_validator/package.xml index faf254708ddbb..2f4400d6ebc57 100644 --- a/control/control_validator/package.xml +++ b/control/control_validator/package.xml @@ -20,7 +20,7 @@ autoware_cmake rosidl_default_generators - autoware_auto_planning_msgs + autoware_planning_msgs diagnostic_updater geometry_msgs motion_utils diff --git a/control/control_validator/src/debug_marker.cpp b/control/control_validator/src/debug_marker.cpp index 1b92aee0bb416..2dbdc558b305b 100644 --- a/control/control_validator/src/debug_marker.cpp +++ b/control/control_validator/src/debug_marker.cpp @@ -40,7 +40,7 @@ void ControlValidatorDebugMarkerPublisher::clearMarkers() } void ControlValidatorDebugMarkerPublisher::pushPoseMarker( - const autoware_auto_planning_msgs::msg::TrajectoryPoint & p, const std::string & ns, int id) + const autoware_planning_msgs::msg::TrajectoryPoint & p, const std::string & ns, int id) { pushPoseMarker(p.pose, ns, id); } diff --git a/control/external_cmd_selector/README.md b/control/external_cmd_selector/README.md index 93ded5713a25b..ca0f4f0954dbf 100644 --- a/control/external_cmd_selector/README.md +++ b/control/external_cmd_selector/README.md @@ -23,12 +23,12 @@ The current mode is set via service, `remote` is remotely operated, `local` is t ### Output topics -| Name | Type | Description | -| ------------------------------------------------------ | ------------------------------------------------------ | ----------------------------------------------- | -| `/control/external_cmd_selector/current_selector_mode` | TBD | Current selected mode, remote or local. | -| `/diagnostics` | diagnostic_msgs::msg::DiagnosticArray | Check if node is active or not. | -| `/external/selected/external_control_cmd` | TBD | Pass through control command with current mode. | -| `/external/selected/gear_cmd` | autoware_auto_vehicle_msgs::msg::GearCommand | Pass through gear command with current mode. | -| `/external/selected/hazard_lights_cmd` | autoware_auto_vehicle_msgs::msg::HazardLightsCommand | Pass through hazard light with current mode. | -| `/external/selected/heartbeat` | TBD | Pass through heartbeat with current mode. | -| `/external/selected/turn_indicators_cmd` | autoware_auto_vehicle_msgs::msg::TurnIndicatorsCommand | Pass through turn indicator with current mode. | +| Name | Type | Description | +| ------------------------------------------------------ | ------------------------------------------------- | ----------------------------------------------- | +| `/control/external_cmd_selector/current_selector_mode` | TBD | Current selected mode, remote or local. | +| `/diagnostics` | diagnostic_msgs::msg::DiagnosticArray | Check if node is active or not. | +| `/external/selected/external_control_cmd` | TBD | Pass through control command with current mode. | +| `/external/selected/gear_cmd` | autoware_vehicle_msgs::msg::GearCommand | Pass through gear command with current mode. | +| `/external/selected/hazard_lights_cmd` | autoware_vehicle_msgs::msg::HazardLightsCommand | Pass through hazard light with current mode. | +| `/external/selected/heartbeat` | TBD | Pass through heartbeat with current mode. | +| `/external/selected/turn_indicators_cmd` | autoware_vehicle_msgs::msg::TurnIndicatorsCommand | Pass through turn indicator with current mode. | diff --git a/control/external_cmd_selector/include/external_cmd_selector/external_cmd_selector_node.hpp b/control/external_cmd_selector/include/external_cmd_selector/external_cmd_selector_node.hpp index f2803e897ba3e..6a4fb897b57bc 100644 --- a/control/external_cmd_selector/include/external_cmd_selector/external_cmd_selector_node.hpp +++ b/control/external_cmd_selector/include/external_cmd_selector/external_cmd_selector_node.hpp @@ -19,9 +19,9 @@ #include #include -#include -#include -#include +#include +#include +#include #include #include #include @@ -39,9 +39,9 @@ class ExternalCmdSelector : public rclcpp::Node private: using CommandSourceSelect = tier4_control_msgs::srv::ExternalCommandSelect; using CommandSourceMode = tier4_control_msgs::msg::ExternalCommandSelectorMode; - using InternalGearShift = autoware_auto_vehicle_msgs::msg::GearCommand; - using InternalTurnSignal = autoware_auto_vehicle_msgs::msg::TurnIndicatorsCommand; - using InternalHazardSignal = autoware_auto_vehicle_msgs::msg::HazardLightsCommand; + using InternalGearShift = autoware_vehicle_msgs::msg::GearCommand; + using InternalTurnSignal = autoware_vehicle_msgs::msg::TurnIndicatorsCommand; + using InternalHazardSignal = autoware_vehicle_msgs::msg::HazardLightsCommand; using InternalHeartbeat = tier4_external_api_msgs::msg::Heartbeat; using ExternalControlCommand = tier4_external_api_msgs::msg::ControlCommandStamped; using ExternalGearShift = tier4_external_api_msgs::msg::GearShiftStamped; diff --git a/control/external_cmd_selector/package.xml b/control/external_cmd_selector/package.xml index 4d8cc5a385494..b3520839e3b51 100644 --- a/control/external_cmd_selector/package.xml +++ b/control/external_cmd_selector/package.xml @@ -16,7 +16,7 @@ ament_cmake_auto autoware_cmake - autoware_auto_vehicle_msgs + autoware_vehicle_msgs diagnostic_updater geometry_msgs rclcpp diff --git a/control/joy_controller/README.md b/control/joy_controller/README.md index 4674ebdadb51d..73e3644df49ac 100644 --- a/control/joy_controller/README.md +++ b/control/joy_controller/README.md @@ -30,15 +30,15 @@ ros2 launch joy_controller joy_controller.launch.xml config_file:=/path/to/your/ ### 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_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 22064f9cefaad..e88f7bcb3904e 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 #include @@ -81,15 +81,14 @@ 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_; rclcpp::Publisher::SharedPtr pub_turn_signal_; rclcpp::Publisher::SharedPtr pub_heartbeat_; rclcpp::Publisher::SharedPtr pub_gate_mode_; - rclcpp::Publisher::SharedPtr pub_vehicle_engage_; + rclcpp::Publisher::SharedPtr pub_vehicle_engage_; void publishControlCommand(); void publishExternalControlCommand(); @@ -106,7 +105,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..f7a5ed805b8ad 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 + autoware_vehicle_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 5eec438032410..39198825f9c4e 100644 --- a/control/joy_controller/src/joy_controller/joy_controller_node.cpp +++ b/control/joy_controller/src/joy_controller/joy_controller_node.cpp @@ -254,7 +254,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(); @@ -262,24 +262,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_)); } } @@ -426,7 +426,7 @@ void AutowareJoyControllerNode::publishAutowareEngage() void AutowareJoyControllerNode::publishVehicleEngage() { - autoware_auto_vehicle_msgs::msg::Engage engage; + autoware_vehicle_msgs::msg::Engage engage; if (joy_->vehicle_engage()) { engage.engage = true; @@ -492,8 +492,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); @@ -505,7 +504,7 @@ AutowareJoyControllerNode::AutowareJoyControllerNode(const rclcpp::NodeOptions & pub_heartbeat_ = this->create_publisher("output/heartbeat", 1); pub_vehicle_engage_ = - this->create_publisher("output/vehicle_engage", 1); + this->create_publisher("output/vehicle_engage", 1); // Service Client client_emergency_stop_ = this->create_client( diff --git a/control/lane_departure_checker/README.md b/control/lane_departure_checker/README.md index 4a6592a103f2f..6eb62dcb2c23a 100644 --- a/control/lane_departure_checker/README.md +++ b/control/lane_departure_checker/README.md @@ -49,10 +49,10 @@ This package includes the following features: ### Input - /localization/kinematic_state [`nav_msgs::msg::Odometry`] -- /map/vector_map [`autoware_auto_mapping_msgs::msg::HADMapBin`] +- /map/vector_map [`autoware_map_msgs::msg::LaneletMapBin`] - /planning/mission_planning/route [`autoware_planning_msgs::msg::LaneletRoute`] -- /planning/scenario_planning/trajectory [`autoware_auto_planning_msgs::msg::Trajectory`] -- /control/trajectory_follower/predicted_trajectory [`autoware_auto_planning_msgs::msg::Trajectory`] +- /planning/scenario_planning/trajectory [`autoware_planning_msgs::msg::Trajectory`] +- /control/trajectory_follower/predicted_trajectory [`autoware_planning_msgs::msg::Trajectory`] ### Output diff --git a/control/lane_departure_checker/include/lane_departure_checker/lane_departure_checker.hpp b/control/lane_departure_checker/include/lane_departure_checker/lane_departure_checker.hpp index b95c0a4b26e5c..967cb65c8efa8 100644 --- a/control/lane_departure_checker/include/lane_departure_checker/lane_departure_checker.hpp +++ b/control/lane_departure_checker/include/lane_departure_checker/lane_departure_checker.hpp @@ -20,14 +20,14 @@ #include #include -#include -#include -#include #include +#include +#include #include #include #include #include +#include #include #include @@ -47,13 +47,13 @@ namespace lane_departure_checker { -using autoware_auto_planning_msgs::msg::PathWithLaneId; -using autoware_auto_planning_msgs::msg::Trajectory; -using autoware_auto_planning_msgs::msg::TrajectoryPoint; using autoware_planning_msgs::msg::LaneletRoute; +using autoware_planning_msgs::msg::Trajectory; +using autoware_planning_msgs::msg::TrajectoryPoint; using tier4_autoware_utils::LinearRing2d; using tier4_autoware_utils::PoseDeviation; using tier4_autoware_utils::Segment2d; +using tier4_planning_msgs::msg::PathWithLaneId; using TrajectoryPoints = std::vector; typedef boost::geometry::index::rtree> SegmentRtree; diff --git a/control/lane_departure_checker/include/lane_departure_checker/lane_departure_checker_node.hpp b/control/lane_departure_checker/include/lane_departure_checker/lane_departure_checker_node.hpp index dd05ab226f6b7..0f0e15d0a4f62 100644 --- a/control/lane_departure_checker/include/lane_departure_checker/lane_departure_checker_node.hpp +++ b/control/lane_departure_checker/include/lane_departure_checker/lane_departure_checker_node.hpp @@ -22,9 +22,9 @@ #include #include -#include -#include +#include #include +#include #include #include #include @@ -42,7 +42,7 @@ namespace lane_departure_checker { -using autoware_auto_mapping_msgs::msg::HADMapBin; +using autoware_map_msgs::msg::LaneletMapBin; struct NodeParam { @@ -67,7 +67,7 @@ class LaneDepartureCheckerNode : public rclcpp::Node private: // Subscriber rclcpp::Subscription::SharedPtr sub_odom_; - rclcpp::Subscription::SharedPtr sub_lanelet_map_bin_; + rclcpp::Subscription::SharedPtr sub_lanelet_map_bin_; rclcpp::Subscription::SharedPtr sub_route_; rclcpp::Subscription::SharedPtr sub_reference_trajectory_; rclcpp::Subscription::SharedPtr sub_predicted_trajectory_; @@ -87,7 +87,7 @@ class LaneDepartureCheckerNode : public rclcpp::Node // Callback void onOdometry(const nav_msgs::msg::Odometry::ConstSharedPtr msg); - void onLaneletMapBin(const HADMapBin::ConstSharedPtr msg); + void onLaneletMapBin(const LaneletMapBin::ConstSharedPtr msg); void onRoute(const LaneletRoute::ConstSharedPtr msg); void onReferenceTrajectory(const Trajectory::ConstSharedPtr msg); void onPredictedTrajectory(const Trajectory::ConstSharedPtr msg); diff --git a/control/lane_departure_checker/package.xml b/control/lane_departure_checker/package.xml index 1059e86cadc6d..73955613d21a9 100644 --- a/control/lane_departure_checker/package.xml +++ b/control/lane_departure_checker/package.xml @@ -11,8 +11,7 @@ ament_cmake_auto autoware_cmake - autoware_auto_mapping_msgs - autoware_auto_planning_msgs + autoware_map_msgs autoware_planning_msgs boost diagnostic_updater diff --git a/control/lane_departure_checker/src/lane_departure_checker_node/lane_departure_checker.cpp b/control/lane_departure_checker/src/lane_departure_checker_node/lane_departure_checker.cpp index cd3fd67a3ab78..eb5d674705aaa 100644 --- a/control/lane_departure_checker/src/lane_departure_checker_node/lane_departure_checker.cpp +++ b/control/lane_departure_checker/src/lane_departure_checker_node/lane_departure_checker.cpp @@ -39,8 +39,8 @@ using tier4_autoware_utils::Point2d; namespace { -using autoware_auto_planning_msgs::msg::Trajectory; -using autoware_auto_planning_msgs::msg::TrajectoryPoint; +using autoware_planning_msgs::msg::Trajectory; +using autoware_planning_msgs::msg::TrajectoryPoint; using TrajectoryPoints = std::vector; using geometry_msgs::msg::Point; diff --git a/control/lane_departure_checker/src/lane_departure_checker_node/lane_departure_checker_node.cpp b/control/lane_departure_checker/src/lane_departure_checker_node/lane_departure_checker_node.cpp index 4d6c86990c399..c3fd1d314e371 100644 --- a/control/lane_departure_checker/src/lane_departure_checker_node/lane_departure_checker_node.cpp +++ b/control/lane_departure_checker/src/lane_departure_checker_node/lane_departure_checker_node.cpp @@ -172,7 +172,7 @@ LaneDepartureCheckerNode::LaneDepartureCheckerNode(const rclcpp::NodeOptions & o // Subscriber sub_odom_ = this->create_subscription( "~/input/odometry", 1, std::bind(&LaneDepartureCheckerNode::onOdometry, this, _1)); - sub_lanelet_map_bin_ = this->create_subscription( + sub_lanelet_map_bin_ = this->create_subscription( "~/input/lanelet_map_bin", rclcpp::QoS{1}.transient_local(), std::bind(&LaneDepartureCheckerNode::onLaneletMapBin, this, _1)); sub_route_ = this->create_subscription( @@ -206,7 +206,7 @@ void LaneDepartureCheckerNode::onOdometry(const nav_msgs::msg::Odometry::ConstSh current_odom_ = msg; } -void LaneDepartureCheckerNode::onLaneletMapBin(const HADMapBin::ConstSharedPtr msg) +void LaneDepartureCheckerNode::onLaneletMapBin(const LaneletMapBin::ConstSharedPtr msg) { lanelet_map_ = std::make_shared(); lanelet::utils::conversion::fromBinMsg(*msg, lanelet_map_, &traffic_rules_, &routing_graph_); diff --git a/control/mpc_lateral_controller/README.md b/control/mpc_lateral_controller/README.md index 6bfba5818bd06..3ad116929b50c 100644 --- a/control/mpc_lateral_controller/README.md +++ b/control/mpc_lateral_controller/README.md @@ -63,15 +63,15 @@ The tracking is not accurate if the first point of the reference trajectory is a Set the following from the [controller_node](../trajectory_follower_node/README.md) -- `autoware_auto_planning_msgs/Trajectory` : reference trajectory to follow. +- `autoware_planning_msgs/Trajectory` : reference trajectory to follow. - `nav_msgs/Odometry`: current odometry -- `autoware_auto_vehicle_msgs/SteeringReport`: current steering +- `autoware_vehicle_msgs/SteeringReport`: current steering ### Outputs Return LateralOutput which contains the following to the controller node -- `autoware_auto_control_msgs/AckermannLateralCommand` +- `autoware_control_msgs/Lateral` - LateralSyncData - steer angle convergence 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 2e83c5ab847c4..91b803dea36fa 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 "autoware_planning_msgs/msg/trajectory.hpp" +#include "autoware_vehicle_msgs/msg/steering_report.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 autoware_planning_msgs::msg::Trajectory; +using autoware_vehicle_msgs::msg::SteeringReport; using geometry_msgs::msg::Pose; using nav_msgs::msg::Odometry; using tier4_debug_msgs::msg::Float32MultiArrayStamped; @@ -378,7 +378,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; /** @@ -441,9 +441,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 92b01247105c6..eb1d75f9508b3 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,9 @@ #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 "autoware_planning_msgs/msg/trajectory.hpp" +#include "autoware_vehicle_msgs/msg/steering_report.hpp" #include "geometry_msgs/msg/pose.hpp" #include "geometry_msgs/msg/pose_stamped.hpp" #include "nav_msgs/msg/odometry.hpp" @@ -42,9 +41,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 autoware_planning_msgs::msg::Trajectory; +using autoware_vehicle_msgs::msg::SteeringReport; using nav_msgs::msg::Odometry; using tier4_debug_msgs::msg::Float32MultiArrayStamped; using tier4_debug_msgs::msg::Float32Stamped; @@ -97,10 +96,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(); @@ -118,7 +117,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; @@ -200,7 +199,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. @@ -218,13 +217,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. @@ -250,7 +249,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/mpc_trajectory.hpp b/control/mpc_lateral_controller/include/mpc_lateral_controller/mpc_trajectory.hpp index 7e1c7ebdf1348..eb624f39ae76b 100644 --- a/control/mpc_lateral_controller/include/mpc_lateral_controller/mpc_trajectory.hpp +++ b/control/mpc_lateral_controller/include/mpc_lateral_controller/mpc_trajectory.hpp @@ -109,11 +109,11 @@ class MPCTrajectory return points; } - std::vector toTrajectoryPoints() const + std::vector toTrajectoryPoints() const { - std::vector points; + std::vector points; for (size_t i = 0; i < x.size(); ++i) { - autoware_auto_planning_msgs::msg::TrajectoryPoint point; + autoware_planning_msgs::msg::TrajectoryPoint point; point.pose.position.x = x.at(i); point.pose.position.y = y.at(i); point.pose.position.z = z.at(i); diff --git a/control/mpc_lateral_controller/include/mpc_lateral_controller/mpc_utils.hpp b/control/mpc_lateral_controller/include/mpc_lateral_controller/mpc_utils.hpp index 9062ff1ea34e3..819d7fb89b8a7 100644 --- a/control/mpc_lateral_controller/include/mpc_lateral_controller/mpc_utils.hpp +++ b/control/mpc_lateral_controller/include/mpc_lateral_controller/mpc_utils.hpp @@ -28,8 +28,8 @@ #include "mpc_lateral_controller/mpc_trajectory.hpp" -#include "autoware_auto_planning_msgs/msg/trajectory.hpp" -#include "autoware_auto_planning_msgs/msg/trajectory_point.hpp" +#include "autoware_planning_msgs/msg/trajectory.hpp" +#include "autoware_planning_msgs/msg/trajectory_point.hpp" #include "geometry_msgs/msg/pose_stamped.hpp" #include "geometry_msgs/msg/twist_stamped.hpp" @@ -43,8 +43,8 @@ namespace autoware::motion::control::mpc_lateral_controller namespace MPCUtils { -using autoware_auto_planning_msgs::msg::Trajectory; -using autoware_auto_planning_msgs::msg::TrajectoryPoint; +using autoware_planning_msgs::msg::Trajectory; +using autoware_planning_msgs::msg::TrajectoryPoint; using geometry_msgs::msg::Pose; /** 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..000bddc65aa1f 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 + autoware_planning_msgs + autoware_vehicle_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 373de2e0bd911..177c1e0854bfb 100644 --- a/control/mpc_lateral_controller/src/mpc.cpp +++ b/control/mpc_lateral_controller/src/mpc.cpp @@ -36,9 +36,8 @@ MPC::MPC(rclcpp::Node & node) } 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. @@ -117,7 +116,7 @@ bool MPC::calculateMPC( 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 882150ffc1644..85d33a5e9f1c0 100644 --- a/control/mpc_lateral_controller/src/mpc_lateral_controller.cpp +++ b/control/mpc_lateral_controller/src/mpc_lateral_controller.cpp @@ -239,7 +239,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; @@ -309,7 +309,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. @@ -381,17 +381,17 @@ void MpcLateralController::setTrajectory( } } -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; @@ -429,8 +429,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 = clock_->now(); @@ -456,7 +455,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 = clock_->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 ba145b5dd146c..7644fb28b0788 100644 --- a/control/mpc_lateral_controller/test/test_mpc.cpp +++ b/control/mpc_lateral_controller/test/test_mpc.cpp @@ -21,10 +21,10 @@ #include "mpc_lateral_controller/vehicle_model/vehicle_model_bicycle_kinematics_no_delay.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_planning_msgs/msg/trajectory_point.hpp" -#include "autoware_auto_vehicle_msgs/msg/steering_report.hpp" +#include "autoware_control_msgs/msg/lateral.hpp" +#include "autoware_planning_msgs/msg/trajectory.hpp" +#include "autoware_planning_msgs/msg/trajectory_point.hpp" +#include "autoware_vehicle_msgs/msg/steering_report.hpp" #include "geometry_msgs/msg/pose.hpp" #include "tier4_debug_msgs/msg/float32_multi_array_stamped.hpp" @@ -41,10 +41,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 autoware_planning_msgs::msg::Trajectory; +using autoware_planning_msgs::msg::TrajectoryPoint; +using autoware_vehicle_msgs::msg::SteeringReport; using geometry_msgs::msg::Pose; using geometry_msgs::msg::PoseStamped; using tier4_debug_msgs::msg::Float32MultiArrayStamped; @@ -206,7 +206,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); @@ -238,7 +238,7 @@ TEST_F(MPCTest, InitializeAndCalculateRightTurn) mpc->setReferenceTrajectory(dummy_right_turn_trajectory, trajectory_param, current_kinematics); // Calculate MPC - AckermannLateralCommand ctrl_cmd; + Lateral ctrl_cmd; Trajectory pred_traj; Float32MultiArrayStamped diag; const auto odom = makeOdometry(pose_zero, default_velocity); @@ -265,7 +265,7 @@ TEST_F(MPCTest, OsqpCalculate) 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); @@ -293,7 +293,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); @@ -323,7 +323,7 @@ TEST_F(MPCTest, KinematicsNoDelayCalculate) const auto current_kinematics = makeOdometry(dummy_straight_trajectory.points.front().pose, 0.0); mpc->setReferenceTrajectory(dummy_straight_trajectory, trajectory_param, current_kinematics); // Calculate MPC - AckermannLateralCommand ctrl_cmd; + Lateral ctrl_cmd; Trajectory pred_traj; Float32MultiArrayStamped diag; const auto odom = makeOdometry(pose_zero, default_velocity); @@ -354,7 +354,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); @@ -379,7 +379,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); @@ -403,7 +403,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); @@ -443,7 +443,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/mpc_lateral_controller/test/test_mpc_utils.cpp b/control/mpc_lateral_controller/test/test_mpc_utils.cpp index 75a7208074b90..51cc7d55e4560 100644 --- a/control/mpc_lateral_controller/test/test_mpc_utils.cpp +++ b/control/mpc_lateral_controller/test/test_mpc_utils.cpp @@ -16,8 +16,8 @@ #include "mpc_lateral_controller/mpc_trajectory.hpp" #include "mpc_lateral_controller/mpc_utils.hpp" -#include "autoware_auto_planning_msgs/msg/trajectory.hpp" -#include "autoware_auto_planning_msgs/msg/trajectory_point.hpp" +#include "autoware_planning_msgs/msg/trajectory.hpp" +#include "autoware_planning_msgs/msg/trajectory_point.hpp" #include #include @@ -25,8 +25,8 @@ namespace { namespace MPCUtils = autoware::motion::control::mpc_lateral_controller::MPCUtils; -using autoware_auto_planning_msgs::msg::Trajectory; -using autoware_auto_planning_msgs::msg::TrajectoryPoint; +using autoware_planning_msgs::msg::Trajectory; +using autoware_planning_msgs::msg::TrajectoryPoint; TrajectoryPoint makePoint(const double x, const double y, const float vx) { diff --git a/control/obstacle_collision_checker/README.md b/control/obstacle_collision_checker/README.md index 5478db1f5a135..25b57fe6f4653 100644 --- a/control/obstacle_collision_checker/README.md +++ b/control/obstacle_collision_checker/README.md @@ -57,13 +57,13 @@ If any collision is found on predicted path, this module sets `ERROR` level as d ### Input -| Name | Type | Description | -| ---------------------------------------------- | ---------------------------------------------- | ------------------------------------------------------------------ | -| `~/input/trajectory` | `autoware_auto_planning_msgs::msg::Trajectory` | Reference trajectory | -| `~/input/trajectory` | `autoware_auto_planning_msgs::msg::Trajectory` | Predicted trajectory | -| `/perception/obstacle_segmentation/pointcloud` | `sensor_msgs::msg::PointCloud2` | Pointcloud of obstacles which the ego-vehicle should stop or avoid | -| `/tf` | `tf2_msgs::msg::TFMessage` | TF | -| `/tf_static` | `tf2_msgs::msg::TFMessage` | TF static | +| Name | Type | Description | +| ---------------------------------------------- | ----------------------------------------- | ------------------------------------------------------------------ | +| `~/input/trajectory` | `autoware_planning_msgs::msg::Trajectory` | Reference trajectory | +| `~/input/trajectory` | `autoware_planning_msgs::msg::Trajectory` | Predicted trajectory | +| `/perception/obstacle_segmentation/pointcloud` | `sensor_msgs::msg::PointCloud2` | Pointcloud of obstacles which the ego-vehicle should stop or avoid | +| `/tf` | `tf2_msgs::msg::TFMessage` | TF | +| `/tf_static` | `tf2_msgs::msg::TFMessage` | TF static | ### Output diff --git a/control/obstacle_collision_checker/include/obstacle_collision_checker/obstacle_collision_checker.hpp b/control/obstacle_collision_checker/include/obstacle_collision_checker/obstacle_collision_checker.hpp index 4ab26c31f9910..a65d818056bd7 100644 --- a/control/obstacle_collision_checker/include/obstacle_collision_checker/obstacle_collision_checker.hpp +++ b/control/obstacle_collision_checker/include/obstacle_collision_checker/obstacle_collision_checker.hpp @@ -18,7 +18,7 @@ #include #include -#include +#include #include #include #include @@ -52,15 +52,15 @@ struct Input geometry_msgs::msg::Twist::ConstSharedPtr current_twist; sensor_msgs::msg::PointCloud2::ConstSharedPtr obstacle_pointcloud; geometry_msgs::msg::TransformStamped::ConstSharedPtr obstacle_transform; - autoware_auto_planning_msgs::msg::Trajectory::ConstSharedPtr reference_trajectory; - autoware_auto_planning_msgs::msg::Trajectory::ConstSharedPtr predicted_trajectory; + autoware_planning_msgs::msg::Trajectory::ConstSharedPtr reference_trajectory; + autoware_planning_msgs::msg::Trajectory::ConstSharedPtr predicted_trajectory; }; struct Output { std::map processing_time_map; bool will_collide; - autoware_auto_planning_msgs::msg::Trajectory resampled_trajectory; + autoware_planning_msgs::msg::Trajectory resampled_trajectory; std::vector vehicle_footprints; std::vector vehicle_passing_areas; }; @@ -78,14 +78,14 @@ class ObstacleCollisionChecker vehicle_info_util::VehicleInfo vehicle_info_; //! This function assumes the input trajectory is sampled dense enough - static autoware_auto_planning_msgs::msg::Trajectory resampleTrajectory( - const autoware_auto_planning_msgs::msg::Trajectory & trajectory, const double interval); + static autoware_planning_msgs::msg::Trajectory resampleTrajectory( + const autoware_planning_msgs::msg::Trajectory & trajectory, const double interval); - static autoware_auto_planning_msgs::msg::Trajectory cutTrajectory( - const autoware_auto_planning_msgs::msg::Trajectory & trajectory, const double length); + static autoware_planning_msgs::msg::Trajectory cutTrajectory( + const autoware_planning_msgs::msg::Trajectory & trajectory, const double length); static std::vector createVehicleFootprints( - const autoware_auto_planning_msgs::msg::Trajectory & trajectory, const Param & param, + const autoware_planning_msgs::msg::Trajectory & trajectory, const Param & param, const vehicle_info_util::VehicleInfo & vehicle_info); static std::vector createVehiclePassingAreas( diff --git a/control/obstacle_collision_checker/include/obstacle_collision_checker/obstacle_collision_checker_node.hpp b/control/obstacle_collision_checker/include/obstacle_collision_checker/obstacle_collision_checker_node.hpp index 12b1e51a1cf3c..9d79a0facac95 100644 --- a/control/obstacle_collision_checker/include/obstacle_collision_checker/obstacle_collision_checker_node.hpp +++ b/control/obstacle_collision_checker/include/obstacle_collision_checker/obstacle_collision_checker_node.hpp @@ -25,7 +25,7 @@ #include #include -#include +#include #include #include #include @@ -51,9 +51,9 @@ class ObstacleCollisionCheckerNode : public rclcpp::Node std::shared_ptr self_pose_listener_; std::shared_ptr transform_listener_; rclcpp::Subscription::SharedPtr sub_obstacle_pointcloud_; - rclcpp::Subscription::SharedPtr + rclcpp::Subscription::SharedPtr sub_reference_trajectory_; - rclcpp::Subscription::SharedPtr + rclcpp::Subscription::SharedPtr sub_predicted_trajectory_; rclcpp::Subscription::SharedPtr sub_odom_; @@ -62,13 +62,13 @@ class ObstacleCollisionCheckerNode : public rclcpp::Node geometry_msgs::msg::Twist::ConstSharedPtr current_twist_; sensor_msgs::msg::PointCloud2::ConstSharedPtr obstacle_pointcloud_; geometry_msgs::msg::TransformStamped::ConstSharedPtr obstacle_transform_; - autoware_auto_planning_msgs::msg::Trajectory::ConstSharedPtr reference_trajectory_; - autoware_auto_planning_msgs::msg::Trajectory::ConstSharedPtr predicted_trajectory_; + autoware_planning_msgs::msg::Trajectory::ConstSharedPtr reference_trajectory_; + autoware_planning_msgs::msg::Trajectory::ConstSharedPtr predicted_trajectory_; // Callback void onObstaclePointcloud(const sensor_msgs::msg::PointCloud2::SharedPtr msg); - void onReferenceTrajectory(const autoware_auto_planning_msgs::msg::Trajectory::SharedPtr msg); - void onPredictedTrajectory(const autoware_auto_planning_msgs::msg::Trajectory::SharedPtr msg); + void onReferenceTrajectory(const autoware_planning_msgs::msg::Trajectory::SharedPtr msg); + void onPredictedTrajectory(const autoware_planning_msgs::msg::Trajectory::SharedPtr msg); void onOdom(const nav_msgs::msg::Odometry::SharedPtr msg); // Publisher diff --git a/control/obstacle_collision_checker/package.xml b/control/obstacle_collision_checker/package.xml index 46b0b18191e81..ef7560755a122 100644 --- a/control/obstacle_collision_checker/package.xml +++ b/control/obstacle_collision_checker/package.xml @@ -18,7 +18,7 @@ ament_cmake autoware_cmake - autoware_auto_planning_msgs + autoware_planning_msgs boost diagnostic_updater eigen diff --git a/control/obstacle_collision_checker/src/obstacle_collision_checker_node/obstacle_collision_checker.cpp b/control/obstacle_collision_checker/src/obstacle_collision_checker_node/obstacle_collision_checker.cpp index 8b89913ef35c8..6887be4cedd77 100644 --- a/control/obstacle_collision_checker/src/obstacle_collision_checker_node/obstacle_collision_checker.cpp +++ b/control/obstacle_collision_checker/src/obstacle_collision_checker_node/obstacle_collision_checker.cpp @@ -54,7 +54,7 @@ pcl::PointCloud getTransformedPointCloud( pcl::PointCloud filterPointCloudByTrajectory( const pcl::PointCloud & pointcloud, - const autoware_auto_planning_msgs::msg::Trajectory & trajectory, const double radius) + const autoware_planning_msgs::msg::Trajectory & trajectory, const double radius) { pcl::PointCloud filtered_pointcloud; for (const auto & point : pointcloud.points) { @@ -121,10 +121,10 @@ Output ObstacleCollisionChecker::update(const Input & input) return output; } -autoware_auto_planning_msgs::msg::Trajectory ObstacleCollisionChecker::resampleTrajectory( - const autoware_auto_planning_msgs::msg::Trajectory & trajectory, const double interval) +autoware_planning_msgs::msg::Trajectory ObstacleCollisionChecker::resampleTrajectory( + const autoware_planning_msgs::msg::Trajectory & trajectory, const double interval) { - autoware_auto_planning_msgs::msg::Trajectory resampled; + autoware_planning_msgs::msg::Trajectory resampled; resampled.header = trajectory.header; resampled.points.push_back(trajectory.points.front()); @@ -143,10 +143,10 @@ autoware_auto_planning_msgs::msg::Trajectory ObstacleCollisionChecker::resampleT return resampled; } -autoware_auto_planning_msgs::msg::Trajectory ObstacleCollisionChecker::cutTrajectory( - const autoware_auto_planning_msgs::msg::Trajectory & trajectory, const double length) +autoware_planning_msgs::msg::Trajectory ObstacleCollisionChecker::cutTrajectory( + const autoware_planning_msgs::msg::Trajectory & trajectory, const double length) { - autoware_auto_planning_msgs::msg::Trajectory cut; + autoware_planning_msgs::msg::Trajectory cut; cut.header = trajectory.header; double total_length = 0.0; @@ -169,7 +169,7 @@ autoware_auto_planning_msgs::msg::Trajectory ObstacleCollisionChecker::cutTrajec if (remain_distance <= points_distance) { const Eigen::Vector3d p_interpolated = p1 + remain_distance * (p2 - p1).normalized(); - autoware_auto_planning_msgs::msg::TrajectoryPoint p; + autoware_planning_msgs::msg::TrajectoryPoint p; p.pose.position.x = p_interpolated.x(); p.pose.position.y = p_interpolated.y(); p.pose.position.z = p_interpolated.z(); @@ -187,7 +187,7 @@ autoware_auto_planning_msgs::msg::Trajectory ObstacleCollisionChecker::cutTrajec } std::vector ObstacleCollisionChecker::createVehicleFootprints( - const autoware_auto_planning_msgs::msg::Trajectory & trajectory, const Param & param, + const autoware_planning_msgs::msg::Trajectory & trajectory, const Param & param, const vehicle_info_util::VehicleInfo & vehicle_info) { // Create vehicle footprint in base_link coordinate diff --git a/control/obstacle_collision_checker/src/obstacle_collision_checker_node/obstacle_collision_checker_node.cpp b/control/obstacle_collision_checker/src/obstacle_collision_checker_node/obstacle_collision_checker_node.cpp index bf3da0fe32627..cfde4ee849728 100644 --- a/control/obstacle_collision_checker/src/obstacle_collision_checker_node/obstacle_collision_checker_node.cpp +++ b/control/obstacle_collision_checker/src/obstacle_collision_checker_node/obstacle_collision_checker_node.cpp @@ -76,10 +76,10 @@ ObstacleCollisionCheckerNode::ObstacleCollisionCheckerNode(const rclcpp::NodeOpt sub_obstacle_pointcloud_ = create_subscription( "input/obstacle_pointcloud", rclcpp::SensorDataQoS(), std::bind(&ObstacleCollisionCheckerNode::onObstaclePointcloud, this, _1)); - sub_reference_trajectory_ = create_subscription( + sub_reference_trajectory_ = create_subscription( "input/reference_trajectory", 1, std::bind(&ObstacleCollisionCheckerNode::onReferenceTrajectory, this, _1)); - sub_predicted_trajectory_ = create_subscription( + sub_predicted_trajectory_ = create_subscription( "input/predicted_trajectory", 1, std::bind(&ObstacleCollisionCheckerNode::onPredictedTrajectory, this, _1)); sub_odom_ = create_subscription( @@ -109,13 +109,13 @@ void ObstacleCollisionCheckerNode::onObstaclePointcloud( } void ObstacleCollisionCheckerNode::onReferenceTrajectory( - const autoware_auto_planning_msgs::msg::Trajectory::SharedPtr msg) + const autoware_planning_msgs::msg::Trajectory::SharedPtr msg) { reference_trajectory_ = msg; } void ObstacleCollisionCheckerNode::onPredictedTrajectory( - const autoware_auto_planning_msgs::msg::Trajectory::SharedPtr msg) + const autoware_planning_msgs::msg::Trajectory::SharedPtr msg) { predicted_trajectory_ = msg; } diff --git a/control/obstacle_collision_checker/test/test_obstacle_collision_checker.cpp b/control/obstacle_collision_checker/test/test_obstacle_collision_checker.cpp index e851cca85c11f..705bff754d3d9 100644 --- a/control/obstacle_collision_checker/test/test_obstacle_collision_checker.cpp +++ b/control/obstacle_collision_checker/test/test_obstacle_collision_checker.cpp @@ -18,9 +18,9 @@ TEST(test_obstacle_collision_checker, filterPointCloudByTrajectory) { pcl::PointCloud pcl; - autoware_auto_planning_msgs::msg::Trajectory trajectory; + autoware_planning_msgs::msg::Trajectory trajectory; pcl::PointXYZ pcl_point; - autoware_auto_planning_msgs::msg::TrajectoryPoint traj_point; + autoware_planning_msgs::msg::TrajectoryPoint traj_point; pcl_point.y = 0.0; traj_point.pose.position.y = 0.99; for (double x = 0.0; x < 10.0; x += 1.0) { diff --git a/control/operation_mode_transition_manager/README.md b/control/operation_mode_transition_manager/README.md index ea0688e88d9f1..008d092565ba4 100644 --- a/control/operation_mode_transition_manager/README.md +++ b/control/operation_mode_transition_manager/README.md @@ -58,15 +58,15 @@ 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) +- /planning/scenario_planning/trajectory [`autoware_planning_msgs/msg/Trajectory`]: planning trajectory +- /vehicle/status/control_mode [`autoware_vehicle_msgs/msg/ControlModeReport`]: vehicle control mode (autonomous/manual) - /control/vehicle_cmd_gate/operation_mode [`autoware_adapi_v1_msgs/msg/OperationModeState`]: the operation mode in the `vehicle_cmd_gate`. (To be removed) For the backward compatibility (to be removed): -- /api/autoware/get/engage [`autoware_auto_vehicle_msgs/msg/Engage`] +- /api/autoware/get/engage [`autoware_vehicle_msgs/msg/Engage`] - /control/current_gate_mode [`tier4_control_msgs/msg/GateMode`] - /control/external_cmd_selector/current_selector_mode [`tier4_control_msgs/msg/ExternalCommandSelectorMode`] @@ -76,9 +76,9 @@ For the backward compatibility (to be removed): - /control/operation_mode_transition_manager/debug_info [`operation_mode_transition_manager/msg/OperationModeTransitionManagerDebug`]: detailed information about the operation mode transition - /control/gate_mode_cmd [`tier4_control_msgs/msg/GateMode`]: to change the `vehicle_cmd_gate` state to use its features (to be removed) -- /autoware/engage [`autoware_auto_vehicle_msgs/msg/Engage`]: +- /autoware/engage [`autoware_vehicle_msgs/msg/Engage`]: -- /control/control_mode_request [`autoware_auto_vehicle_msgs/srv/ControlModeCommand`]: to change the vehicle control mode (autonomous/manual) +- /control/control_mode_request [`autoware_vehicle_msgs/srv/ControlModeCommand`]: to change the vehicle control mode (autonomous/manual) - /control/external_cmd_selector/select_external_command [`tier4_control_msgs/srv/ExternalCommandSelect`]: ## Parameters diff --git a/control/operation_mode_transition_manager/package.xml b/control/operation_mode_transition_manager/package.xml index dbe9a21c1186a..99439aa7e3ab7 100644 --- a/control/operation_mode_transition_manager/package.xml +++ b/control/operation_mode_transition_manager/package.xml @@ -11,9 +11,8 @@ autoware_cmake rosidl_default_generators - autoware_auto_control_msgs - autoware_auto_system_msgs - autoware_auto_vehicle_msgs + autoware_control_msgs + autoware_vehicle_msgs component_interface_specs component_interface_utils geometry_msgs diff --git a/control/operation_mode_transition_manager/src/compatibility.hpp b/control/operation_mode_transition_manager/src/compatibility.hpp index a69b15e69960c..d9bde7cb22872 100644 --- a/control/operation_mode_transition_manager/src/compatibility.hpp +++ b/control/operation_mode_transition_manager/src/compatibility.hpp @@ -17,7 +17,7 @@ #include "data.hpp" -#include +#include #include #include #include @@ -33,7 +33,7 @@ class Compatibility std::optional get_mode() const; private: - using AutowareEngage = autoware_auto_vehicle_msgs::msg::Engage; + using AutowareEngage = autoware_vehicle_msgs::msg::Engage; using GateMode = tier4_control_msgs::msg::GateMode; using SelectorModeMsg = tier4_control_msgs::msg::ExternalCommandSelectorMode; using SelectorModeSrv = tier4_control_msgs::srv::ExternalCommandSelect; diff --git a/control/operation_mode_transition_manager/src/data.hpp b/control/operation_mode_transition_manager/src/data.hpp index 9b822936a0252..8ea9f8d2b99ee 100644 --- a/control/operation_mode_transition_manager/src/data.hpp +++ b/control/operation_mode_transition_manager/src/data.hpp @@ -19,8 +19,8 @@ #include #include -#include -#include +#include +#include #include #include @@ -34,8 +34,8 @@ using ServiceResponse = autoware_adapi_v1_msgs::srv::ChangeOperationMode::Respon using OperationModeState = autoware_adapi_v1_msgs::msg::OperationModeState; using OperationModeValue = OperationModeState::_mode_type; using ChangeOperationMode = tier4_system_msgs::srv::ChangeOperationMode; -using ControlModeCommand = autoware_auto_vehicle_msgs::srv::ControlModeCommand; -using ControlModeReport = autoware_auto_vehicle_msgs::msg::ControlModeReport; +using ControlModeCommand = autoware_vehicle_msgs::srv::ControlModeCommand; +using ControlModeReport = autoware_vehicle_msgs::msg::ControlModeReport; enum class OperationMode { STOP, AUTONOMOUS, LOCAL, REMOTE }; diff --git a/control/operation_mode_transition_manager/src/state.cpp b/control/operation_mode_transition_manager/src/state.cpp index d57cb8c78c740..635ead2430677 100644 --- a/control/operation_mode_transition_manager/src/state.cpp +++ b/control/operation_mode_transition_manager/src/state.cpp @@ -35,13 +35,11 @@ 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_trajectory_follower_control_cmd_ = node->create_subscription( - "trajectory_follower_control_cmd", 1, [this](const AckermannControlCommand::SharedPtr msg) { - trajectory_follower_control_cmd_ = *msg; - }); + sub_control_cmd_ = node->create_subscription( + "control_cmd", 1, [this](const Control::SharedPtr msg) { control_cmd_ = *msg; }); + sub_trajectory_follower_control_cmd_ = node->create_subscription( + "trajectory_follower_control_cmd", 1, + [this](const Control::SharedPtr msg) { trajectory_follower_control_cmd_ = *msg; }); sub_kinematics_ = node->create_subscription( "kinematics", 1, [this](const Odometry::SharedPtr msg) { kinematics_ = *msg; }); @@ -220,7 +218,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 (!enable_engage_on_driving_ && std::fabs(current_speed) > 1.0e-2) { @@ -267,7 +265,7 @@ bool AutonomousMode::isModeChangeAvailable() // No engagement if the vehicle is moving but the target speed is zero. const bool is_stop_cmd_indicated = std::abs(target_control_speed) < 0.01 || - std::abs(trajectory_follower_control_cmd_.longitudinal.speed) < 0.01; + std::abs(trajectory_follower_control_cmd_.longitudinal.velocity) < 0.01; const bool stop_ok = !(std::abs(current_speed) > 0.1 && is_stop_cmd_indicated); // No engagement if the large acceleration is commanded. diff --git a/control/operation_mode_transition_manager/src/state.hpp b/control/operation_mode_transition_manager/src/state.hpp index e10d64e367f8d..e5abd4285ad5f 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 #include @@ -63,11 +63,11 @@ 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_trajectory_follower_control_cmd_; + using Trajectory = autoware_planning_msgs::msg::Trajectory; + rclcpp::Subscription::SharedPtr sub_control_cmd_; + rclcpp::Subscription::SharedPtr sub_trajectory_follower_control_cmd_; rclcpp::Subscription::SharedPtr sub_kinematics_; rclcpp::Subscription::SharedPtr sub_trajectory_; rclcpp::Logger logger_; @@ -79,8 +79,8 @@ 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_; - AckermannControlCommand trajectory_follower_control_cmd_; + Control control_cmd_; + Control trajectory_follower_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 57eab2d87f18e..4e20cb27fe66a 100644 --- a/control/pid_longitudinal_controller/README.md +++ b/control/pid_longitudinal_controller/README.md @@ -143,14 +143,14 @@ There are two sources of the slope information, which can be switched by a param Set the following from the [controller_node](../trajectory_follower_node/README.md) -- `autoware_auto_planning_msgs/Trajectory` : reference trajectory to follow. +- `autoware_planning_msgs/Trajectory` : reference trajectory to follow. - `nav_msgs/Odometry`: current odometry ### Output 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/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/longitudinal_controller_utils.hpp b/control/pid_longitudinal_controller/include/pid_longitudinal_controller/longitudinal_controller_utils.hpp index ac4fec8dacb7d..4ea844a140f4f 100644 --- a/control/pid_longitudinal_controller/include/pid_longitudinal_controller/longitudinal_controller_utils.hpp +++ b/control/pid_longitudinal_controller/include/pid_longitudinal_controller/longitudinal_controller_utils.hpp @@ -25,7 +25,7 @@ #include #include // NOLINT -#include "autoware_auto_planning_msgs/msg/trajectory.hpp" +#include "autoware_planning_msgs/msg/trajectory.hpp" #include "geometry_msgs/msg/pose.hpp" #include @@ -37,8 +37,8 @@ namespace autoware::motion::control::pid_longitudinal_controller namespace longitudinal_utils { -using autoware_auto_planning_msgs::msg::Trajectory; -using autoware_auto_planning_msgs::msg::TrajectoryPoint; +using autoware_planning_msgs::msg::Trajectory; +using autoware_planning_msgs::msg::TrajectoryPoint; using geometry_msgs::msg::Point; using geometry_msgs::msg::Pose; using geometry_msgs::msg::Quaternion; @@ -150,7 +150,7 @@ double applyDiffLimitFilter( */ geometry_msgs::msg::Pose findTrajectoryPoseAfterDistance( const size_t src_idx, const double distance, - const autoware_auto_planning_msgs::msg::Trajectory & trajectory); + const autoware_planning_msgs::msg::Trajectory & trajectory); } // namespace longitudinal_utils } // namespace autoware::motion::control::pid_longitudinal_controller 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 967b7c40fbd10..dee2e580e6911 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 @@ -33,9 +33,8 @@ #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 "autoware_planning_msgs/msg/trajectory.hpp" #include "geometry_msgs/msg/pose_stamped.hpp" #include "nav_msgs/msg/odometry.hpp" #include "tf2_msgs/msg/tf_message.hpp" @@ -88,7 +87,7 @@ class PidLongitudinalController : public trajectory_follower::LongitudinalContro struct ControlData { bool is_far_from_trajectory{false}; - autoware_auto_planning_msgs::msg::Trajectory interpolated_traj{}; + autoware_planning_msgs::msg::Trajectory interpolated_traj{}; size_t nearest_idx{0}; // nearest_idx = 0 when nearest_idx is not found with findNearestIdx size_t target_idx{0}; StateAfterDelay state_after_delay{0.0, 0.0, 0.0}; @@ -113,7 +112,7 @@ class PidLongitudinalController : public trajectory_follower::LongitudinalContro // pointers for ros topic nav_msgs::msg::Odometry m_current_kinematic_state; geometry_msgs::msg::AccelWithCovarianceStamped m_current_accel; - autoware_auto_planning_msgs::msg::Trajectory m_trajectory; + autoware_planning_msgs::msg::Trajectory m_trajectory; OperationModeState m_current_operation_mode; // vehicle info @@ -218,7 +217,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}; @@ -270,7 +269,7 @@ class PidLongitudinalController : public trajectory_follower::LongitudinalContro * @brief set reference trajectory with received message * @param [in] msg trajectory message */ - void setTrajectory(const autoware_auto_planning_msgs::msg::Trajectory & msg); + void setTrajectory(const autoware_planning_msgs::msg::Trajectory & msg); bool isReady(const trajectory_follower::InputData & input_data) override; @@ -309,7 +308,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); /** @@ -371,9 +370,9 @@ class PidLongitudinalController : public trajectory_follower::LongitudinalContro * @param [in] point vehicle position * @param [in] nearest_idx index of the trajectory point nearest to the vehicle position */ - std::pair + std::pair calcInterpolatedTrajPointAndSegment( - const autoware_auto_planning_msgs::msg::Trajectory & traj, + const autoware_planning_msgs::msg::Trajectory & traj, const geometry_msgs::msg::Pose & pose) const; /** diff --git a/control/pid_longitudinal_controller/package.xml b/control/pid_longitudinal_controller/package.xml index 959aca689816a..82b297a69fee6 100644 --- a/control/pid_longitudinal_controller/package.xml +++ b/control/pid_longitudinal_controller/package.xml @@ -19,9 +19,9 @@ autoware_cmake autoware_adapi_v1_msgs - autoware_auto_control_msgs - autoware_auto_planning_msgs - autoware_auto_vehicle_msgs + autoware_control_msgs + autoware_planning_msgs + autoware_vehicle_msgs diagnostic_msgs diagnostic_updater eigen diff --git a/control/pid_longitudinal_controller/src/longitudinal_controller_utils.cpp b/control/pid_longitudinal_controller/src/longitudinal_controller_utils.cpp index 28cd6e1824859..4e9ef52a4969e 100644 --- a/control/pid_longitudinal_controller/src/longitudinal_controller_utils.cpp +++ b/control/pid_longitudinal_controller/src/longitudinal_controller_utils.cpp @@ -161,7 +161,7 @@ double applyDiffLimitFilter( geometry_msgs::msg::Pose findTrajectoryPoseAfterDistance( const size_t src_idx, const double distance, - const autoware_auto_planning_msgs::msg::Trajectory & trajectory) + const autoware_planning_msgs::msg::Trajectory & trajectory) { double remain_dist = distance; geometry_msgs::msg::Pose p = trajectory.points.back().pose; diff --git a/control/pid_longitudinal_controller/src/pid_longitudinal_controller.cpp b/control/pid_longitudinal_controller/src/pid_longitudinal_controller.cpp index cc1c0c6383707..9a8223c610f9a 100644 --- a/control/pid_longitudinal_controller/src/pid_longitudinal_controller.cpp +++ b/control/pid_longitudinal_controller/src/pid_longitudinal_controller.cpp @@ -232,8 +232,7 @@ void PidLongitudinalController::setCurrentOperationMode(const OperationModeState m_current_operation_mode = msg; } -void PidLongitudinalController::setTrajectory( - const autoware_auto_planning_msgs::msg::Trajectory & msg) +void PidLongitudinalController::setTrajectory(const autoware_planning_msgs::msg::Trajectory & msg) { if (!longitudinal_utils::isValidTrajectory(msg)) { RCLCPP_ERROR_THROTTLE(logger_, *clock_, 3000, "received invalid trajectory. ignore."); @@ -820,13 +819,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 = clock_->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 @@ -926,7 +925,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 = clock_->now(); cmd.acceleration = static_cast(accel); @@ -994,10 +993,9 @@ PidLongitudinalController::Motion PidLongitudinalController::keepBrakeBeforeStop return output_motion; } -std::pair +std::pair PidLongitudinalController::calcInterpolatedTrajPointAndSegment( - const autoware_auto_planning_msgs::msg::Trajectory & traj, - const geometry_msgs::msg::Pose & pose) const + const autoware_planning_msgs::msg::Trajectory & traj, const geometry_msgs::msg::Pose & pose) const { if (traj.points.size() == 1) { return std::make_pair(traj.points.at(0), 0); diff --git a/control/pid_longitudinal_controller/test/test_longitudinal_controller_utils.cpp b/control/pid_longitudinal_controller/test/test_longitudinal_controller_utils.cpp index 02bcce8c91c4b..bd79d7a8c3ac3 100644 --- a/control/pid_longitudinal_controller/test/test_longitudinal_controller_utils.cpp +++ b/control/pid_longitudinal_controller/test/test_longitudinal_controller_utils.cpp @@ -18,8 +18,8 @@ #include "pid_longitudinal_controller/longitudinal_controller_utils.hpp" #include "tf2/LinearMath/Quaternion.h" -#include "autoware_auto_planning_msgs/msg/trajectory.hpp" -#include "autoware_auto_planning_msgs/msg/trajectory_point.hpp" +#include "autoware_planning_msgs/msg/trajectory.hpp" +#include "autoware_planning_msgs/msg/trajectory_point.hpp" #include "geometry_msgs/msg/point.hpp" #include "geometry_msgs/msg/pose.hpp" #include "geometry_msgs/msg/quaternion.hpp" @@ -37,8 +37,8 @@ namespace longitudinal_utils = TEST(TestLongitudinalControllerUtils, isValidTrajectory) { - using autoware_auto_planning_msgs::msg::Trajectory; - using autoware_auto_planning_msgs::msg::TrajectoryPoint; + using autoware_planning_msgs::msg::Trajectory; + using autoware_planning_msgs::msg::TrajectoryPoint; Trajectory traj; TrajectoryPoint point; EXPECT_FALSE(longitudinal_utils::isValidTrajectory(traj)); @@ -51,8 +51,8 @@ TEST(TestLongitudinalControllerUtils, isValidTrajectory) TEST(TestLongitudinalControllerUtils, calcStopDistance) { - using autoware_auto_planning_msgs::msg::Trajectory; - using autoware_auto_planning_msgs::msg::TrajectoryPoint; + using autoware_planning_msgs::msg::Trajectory; + using autoware_planning_msgs::msg::TrajectoryPoint; using geometry_msgs::msg::Pose; Pose current_pose; current_pose.position.x = 0.0; @@ -115,8 +115,8 @@ TEST(TestLongitudinalControllerUtils, getPitchByPose) TEST(TestLongitudinalControllerUtils, getPitchByTraj) { - using autoware_auto_planning_msgs::msg::Trajectory; - using autoware_auto_planning_msgs::msg::TrajectoryPoint; + using autoware_planning_msgs::msg::Trajectory; + using autoware_planning_msgs::msg::TrajectoryPoint; const double wheel_base = 0.9; /** * Trajectory: @@ -346,10 +346,10 @@ TEST(TestLongitudinalControllerUtils, lerpOrientation) TEST(TestLongitudinalControllerUtils, lerpTrajectoryPoint) { - using autoware_auto_planning_msgs::msg::TrajectoryPoint; + using autoware_planning_msgs::msg::TrajectoryPoint; using geometry_msgs::msg::Pose; const double abs_err = 1e-15; - decltype(autoware_auto_planning_msgs::msg::Trajectory::points) points; + decltype(autoware_planning_msgs::msg::Trajectory::points) points; TrajectoryPoint p; p.pose.position.x = 0.0; p.pose.position.y = 0.0; @@ -505,8 +505,8 @@ TEST(TestLongitudinalControllerUtils, applyDiffLimitFilter) TEST(TestLongitudinalControllerUtils, findTrajectoryPoseAfterDistance) { - using autoware_auto_planning_msgs::msg::Trajectory; - using autoware_auto_planning_msgs::msg::TrajectoryPoint; + using autoware_planning_msgs::msg::Trajectory; + using autoware_planning_msgs::msg::TrajectoryPoint; using geometry_msgs::msg::Pose; const double abs_err = 1e-5; Trajectory traj; diff --git a/control/predicted_path_checker/README.md b/control/predicted_path_checker/README.md index 24e4b91ef441b..a968a285928f6 100644 --- a/control/predicted_path_checker/README.md +++ b/control/predicted_path_checker/README.md @@ -55,14 +55,14 @@ make the vehicle stop. ## Inputs -| Name | Type | Description | -| ------------------------------------- | ----------------------------------------------------- | --------------------------------------------------- | -| `~/input/reference_trajectory` | `autoware_auto_planning_msgs::msg::Trajectory` | Reference trajectory | -| `~/input/predicted_trajectory` | `autoware_auto_planning_msgs::msg::Trajectory` | Predicted trajectory | -| `~/input/objects` | `autoware_auto_perception_msgs::msg::PredictedObject` | Dynamic objects in the environment | -| `~/input/odometry` | `nav_msgs::msg::Odometry` | Odometry message of vehicle to get current velocity | -| `~/input/current_accel` | `geometry_msgs::msg::AccelWithCovarianceStamped` | Current acceleration | -| `/control/vehicle_cmd_gate/is_paused` | `tier4_control_msgs::msg::IsPaused` | Current pause state of the vehicle | +| Name | Type | Description | +| ------------------------------------- | ------------------------------------------------ | --------------------------------------------------- | +| `~/input/reference_trajectory` | `autoware_planning_msgs::msg::Trajectory` | Reference trajectory | +| `~/input/predicted_trajectory` | `autoware_planning_msgs::msg::Trajectory` | Predicted trajectory | +| `~/input/objects` | `autoware_perception_msgs::msg::PredictedObject` | Dynamic objects in the environment | +| `~/input/odometry` | `nav_msgs::msg::Odometry` | Odometry message of vehicle to get current velocity | +| `~/input/current_accel` | `geometry_msgs::msg::AccelWithCovarianceStamped` | Current acceleration | +| `/control/vehicle_cmd_gate/is_paused` | `tier4_control_msgs::msg::IsPaused` | Current pause state of the vehicle | ## Outputs diff --git a/control/predicted_path_checker/include/predicted_path_checker/collision_checker.hpp b/control/predicted_path_checker/include/predicted_path_checker/collision_checker.hpp index 1d7791955b576..8410e7c78f723 100644 --- a/control/predicted_path_checker/include/predicted_path_checker/collision_checker.hpp +++ b/control/predicted_path_checker/include/predicted_path_checker/collision_checker.hpp @@ -42,11 +42,11 @@ namespace autoware::motion::control::predicted_path_checker { -using autoware_auto_planning_msgs::msg::Trajectory; -using autoware_auto_planning_msgs::msg::TrajectoryPoint; +using autoware_planning_msgs::msg::Trajectory; +using autoware_planning_msgs::msg::TrajectoryPoint; using TrajectoryPoints = std::vector; -using autoware_auto_perception_msgs::msg::PredictedObject; -using autoware_auto_perception_msgs::msg::PredictedObjects; +using autoware_perception_msgs::msg::PredictedObject; +using autoware_perception_msgs::msg::PredictedObjects; using geometry_msgs::msg::Pose; using geometry_msgs::msg::TransformStamped; using tier4_autoware_utils::Point2d; diff --git a/control/predicted_path_checker/include/predicted_path_checker/predicted_path_checker_node.hpp b/control/predicted_path_checker/include/predicted_path_checker/predicted_path_checker_node.hpp index 3ce5728521141..23696887c7051 100644 --- a/control/predicted_path_checker/include/predicted_path_checker/predicted_path_checker_node.hpp +++ b/control/predicted_path_checker/include/predicted_path_checker/predicted_path_checker_node.hpp @@ -27,8 +27,8 @@ #include #include -#include -#include +#include +#include #include #include #include @@ -47,10 +47,10 @@ namespace autoware::motion::control::predicted_path_checker { -using autoware_auto_planning_msgs::msg::Trajectory; -using autoware_auto_planning_msgs::msg::TrajectoryPoint; +using autoware_planning_msgs::msg::Trajectory; +using autoware_planning_msgs::msg::TrajectoryPoint; using TrajectoryPoints = std::vector; -using autoware_auto_perception_msgs::msg::PredictedObjects; +using autoware_perception_msgs::msg::PredictedObjects; using geometry_msgs::msg::Pose; using geometry_msgs::msg::TransformStamped; using tier4_autoware_utils::Point2d; @@ -90,9 +90,9 @@ class PredictedPathCheckerNode : public rclcpp::Node // Subscriber std::shared_ptr self_pose_listener_; rclcpp::Subscription::SharedPtr sub_dynamic_objects_; - rclcpp::Subscription::SharedPtr + rclcpp::Subscription::SharedPtr sub_reference_trajectory_; - rclcpp::Subscription::SharedPtr + rclcpp::Subscription::SharedPtr sub_predicted_trajectory_; rclcpp::Subscription::SharedPtr sub_odom_; rclcpp::Subscription::SharedPtr sub_accel_; @@ -106,8 +106,8 @@ class PredictedPathCheckerNode : public rclcpp::Node geometry_msgs::msg::Twist::ConstSharedPtr current_twist_; geometry_msgs::msg::AccelWithCovarianceStamped::SharedPtr current_accel_; PredictedObjects::ConstSharedPtr object_ptr_{nullptr}; - autoware_auto_planning_msgs::msg::Trajectory::ConstSharedPtr reference_trajectory_; - autoware_auto_planning_msgs::msg::Trajectory::ConstSharedPtr predicted_trajectory_; + autoware_planning_msgs::msg::Trajectory::ConstSharedPtr reference_trajectory_; + autoware_planning_msgs::msg::Trajectory::ConstSharedPtr predicted_trajectory_; control_interface::IsStopped::Message::ConstSharedPtr is_stopped_ptr_{nullptr}; // Core @@ -122,8 +122,8 @@ class PredictedPathCheckerNode : public rclcpp::Node // Callback void onDynamicObjects(PredictedObjects::ConstSharedPtr msg); - void onReferenceTrajectory(const autoware_auto_planning_msgs::msg::Trajectory::SharedPtr msg); - void onPredictedTrajectory(const autoware_auto_planning_msgs::msg::Trajectory::SharedPtr msg); + void onReferenceTrajectory(const autoware_planning_msgs::msg::Trajectory::SharedPtr msg); + void onPredictedTrajectory(const autoware_planning_msgs::msg::Trajectory::SharedPtr msg); void onOdom(const nav_msgs::msg::Odometry::SharedPtr msg); void onAccel(const geometry_msgs::msg::AccelWithCovarianceStamped::SharedPtr msg); void onIsStopped(const control_interface::IsStopped::Message::ConstSharedPtr msg); diff --git a/control/predicted_path_checker/include/predicted_path_checker/utils.hpp b/control/predicted_path_checker/include/predicted_path_checker/utils.hpp index 75e90e624a17e..957800ad04305 100644 --- a/control/predicted_path_checker/include/predicted_path_checker/utils.hpp +++ b/control/predicted_path_checker/include/predicted_path_checker/utils.hpp @@ -20,8 +20,8 @@ #include #include -#include -#include +#include +#include #include #include #include @@ -39,9 +39,9 @@ namespace utils { -using autoware_auto_perception_msgs::msg::PredictedObject; -using autoware_auto_perception_msgs::msg::PredictedObjects; -using autoware_auto_planning_msgs::msg::TrajectoryPoint; +using autoware_perception_msgs::msg::PredictedObject; +using autoware_perception_msgs::msg::PredictedObjects; +using autoware_planning_msgs::msg::TrajectoryPoint; using geometry_msgs::msg::Point; using geometry_msgs::msg::Pose; using geometry_msgs::msg::TransformStamped; @@ -85,14 +85,14 @@ Polygon2d convertBoundingBoxObjectToGeometryPolygon( const double & base_to_width); Polygon2d convertCylindricalObjectToGeometryPolygon( - const Pose & current_pose, const autoware_auto_perception_msgs::msg::Shape & obj_shape); + const Pose & current_pose, const autoware_perception_msgs::msg::Shape & obj_shape); Polygon2d convertPolygonObjectToGeometryPolygon( - const Pose & current_pose, const autoware_auto_perception_msgs::msg::Shape & obj_shape); + const Pose & current_pose, const autoware_perception_msgs::msg::Shape & obj_shape); Polygon2d convertObjToPolygon(const PredictedObject & obj); -double calcObstacleMaxLength(const autoware_auto_perception_msgs::msg::Shape & shape); +double calcObstacleMaxLength(const autoware_perception_msgs::msg::Shape & shape); void getCurrentObjectPose( PredictedObject & predicted_object, const rclcpp::Time & obj_base_time, diff --git a/control/predicted_path_checker/package.xml b/control/predicted_path_checker/package.xml index 35f971907696a..bca65302dba55 100644 --- a/control/predicted_path_checker/package.xml +++ b/control/predicted_path_checker/package.xml @@ -12,8 +12,8 @@ ament_cmake autoware_cmake - autoware_auto_perception_msgs - autoware_auto_planning_msgs + autoware_perception_msgs + autoware_planning_msgs boost component_interface_specs component_interface_utils diff --git a/control/predicted_path_checker/src/predicted_path_checker_node/predicted_path_checker_node.cpp b/control/predicted_path_checker/src/predicted_path_checker_node/predicted_path_checker_node.cpp index bba069442bee7..28ea21f639a0e 100644 --- a/control/predicted_path_checker/src/predicted_path_checker_node/predicted_path_checker_node.cpp +++ b/control/predicted_path_checker/src/predicted_path_checker_node/predicted_path_checker_node.cpp @@ -72,10 +72,10 @@ PredictedPathCheckerNode::PredictedPathCheckerNode(const rclcpp::NodeOptions & n sub_dynamic_objects_ = create_subscription( "~/input/objects", rclcpp::SensorDataQoS(), std::bind(&PredictedPathCheckerNode::onDynamicObjects, this, _1)); - sub_reference_trajectory_ = create_subscription( + sub_reference_trajectory_ = create_subscription( "~/input/reference_trajectory", 1, std::bind(&PredictedPathCheckerNode::onReferenceTrajectory, this, _1)); - sub_predicted_trajectory_ = create_subscription( + sub_predicted_trajectory_ = create_subscription( "~/input/predicted_trajectory", 1, std::bind(&PredictedPathCheckerNode::onPredictedTrajectory, this, _1)); sub_odom_ = create_subscription( @@ -109,13 +109,13 @@ void PredictedPathCheckerNode::onDynamicObjects(const PredictedObjects::ConstSha } void PredictedPathCheckerNode::onReferenceTrajectory( - const autoware_auto_planning_msgs::msg::Trajectory::SharedPtr msg) + const autoware_planning_msgs::msg::Trajectory::SharedPtr msg) { reference_trajectory_ = msg; } void PredictedPathCheckerNode::onPredictedTrajectory( - const autoware_auto_planning_msgs::msg::Trajectory::SharedPtr msg) + const autoware_planning_msgs::msg::Trajectory::SharedPtr msg) { predicted_trajectory_ = msg; } diff --git a/control/predicted_path_checker/src/predicted_path_checker_node/utils.cpp b/control/predicted_path_checker/src/predicted_path_checker_node/utils.cpp index 9cb095e908d41..f1c76ce6eef8f 100644 --- a/control/predicted_path_checker/src/predicted_path_checker_node/utils.cpp +++ b/control/predicted_path_checker/src/predicted_path_checker_node/utils.cpp @@ -298,7 +298,7 @@ Polygon2d convertBoundingBoxObjectToGeometryPolygon( } Polygon2d convertCylindricalObjectToGeometryPolygon( - const Pose & current_pose, const autoware_auto_perception_msgs::msg::Shape & obj_shape) + const Pose & current_pose, const autoware_perception_msgs::msg::Shape & obj_shape) { Polygon2d object_polygon; @@ -320,7 +320,7 @@ Polygon2d convertCylindricalObjectToGeometryPolygon( } Polygon2d convertPolygonObjectToGeometryPolygon( - const Pose & current_pose, const autoware_auto_perception_msgs::msg::Shape & obj_shape) + const Pose & current_pose, const autoware_perception_msgs::msg::Shape & obj_shape) { Polygon2d object_polygon; tf2::Transform tf_map2obj; @@ -350,15 +350,15 @@ void appendPointToPolygon(Polygon2d & polygon, const geometry_msgs::msg::Point & Polygon2d convertObjToPolygon(const PredictedObject & obj) { Polygon2d object_polygon{}; - if (obj.shape.type == autoware_auto_perception_msgs::msg::Shape::CYLINDER) { + if (obj.shape.type == autoware_perception_msgs::msg::Shape::CYLINDER) { object_polygon = utils::convertCylindricalObjectToGeometryPolygon( obj.kinematics.initial_pose_with_covariance.pose, obj.shape); - } else if (obj.shape.type == autoware_auto_perception_msgs::msg::Shape::BOUNDING_BOX) { + } else if (obj.shape.type == autoware_perception_msgs::msg::Shape::BOUNDING_BOX) { const double & length_m = obj.shape.dimensions.x / 2; const double & width_m = obj.shape.dimensions.y / 2; object_polygon = utils::convertBoundingBoxObjectToGeometryPolygon( obj.kinematics.initial_pose_with_covariance.pose, length_m, length_m, width_m); - } else if (obj.shape.type == autoware_auto_perception_msgs::msg::Shape::POLYGON) { + } else if (obj.shape.type == autoware_perception_msgs::msg::Shape::POLYGON) { object_polygon = utils::convertPolygonObjectToGeometryPolygon( obj.kinematics.initial_pose_with_covariance.pose, obj.shape); } else { @@ -377,13 +377,13 @@ bool isFrontObstacle(const Pose & ego_pose, const geometry_msgs::msg::Point & ob return base_pose_vec.dot(obstacle_vec) >= 0; } -double calcObstacleMaxLength(const autoware_auto_perception_msgs::msg::Shape & shape) +double calcObstacleMaxLength(const autoware_perception_msgs::msg::Shape & shape) { - if (shape.type == autoware_auto_perception_msgs::msg::Shape::BOUNDING_BOX) { + if (shape.type == autoware_perception_msgs::msg::Shape::BOUNDING_BOX) { return std::hypot(shape.dimensions.x / 2.0, shape.dimensions.y / 2.0); - } else if (shape.type == autoware_auto_perception_msgs::msg::Shape::CYLINDER) { + } else if (shape.type == autoware_perception_msgs::msg::Shape::CYLINDER) { return shape.dimensions.x / 2.0; - } else if (shape.type == autoware_auto_perception_msgs::msg::Shape::POLYGON) { + } else if (shape.type == autoware_perception_msgs::msg::Shape::POLYGON) { double max_length_to_point = 0.0; for (const auto rel_point : shape.footprint.points) { const double length_to_point = std::hypot(rel_point.x, rel_point.y); diff --git a/control/pure_pursuit/README.md b/control/pure_pursuit/README.md index 48e2a13ef664d..55c847fe88d22 100644 --- a/control/pure_pursuit/README.md +++ b/control/pure_pursuit/README.md @@ -6,14 +6,14 @@ The Pure Pursuit Controller module calculates the steering angle for tracking a Set the following from the [controller_node](../trajectory_follower_node/README.md) -- `autoware_auto_planning_msgs/Trajectory` : reference trajectory to follow. +- `autoware_planning_msgs/Trajectory` : reference trajectory to follow. - `nav_msgs/Odometry`: current ego pose and velocity information ## Outputs Return LateralOutput which contains the following to the controller node -- `autoware_auto_control_msgs/AckermannLateralCommand`: target steering angle +- `autoware_control_msgs/Lateral`: target steering angle - LateralSyncData - steer angle convergence -- `autoware_auto_planning_msgs/Trajectory`: predicted path for ego vehicle +- `autoware_planning_msgs/Trajectory`: predicted path for ego vehicle 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 ca0d77140b195..5b7b466dcb4dd 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 "autoware_planning_msgs/msg/trajectory.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; +using autoware_planning_msgs::msg::Trajectory; +using autoware_planning_msgs::msg::TrajectoryPoint; namespace pure_pursuit { @@ -107,20 +107,19 @@ class PurePursuitLateralController : public LateralControllerBase rclcpp::Clock::SharedPtr clock_; rclcpp::Logger logger_; std::vector output_tp_array_; - autoware_auto_planning_msgs::msg::Trajectory::SharedPtr trajectory_resampled_; - autoware_auto_planning_msgs::msg::Trajectory trajectory_; + autoware_planning_msgs::msg::Trajectory::SharedPtr trajectory_resampled_; + autoware_planning_msgs::msg::Trajectory trajectory_; nav_msgs::msg::Odometry current_odometry_; - autoware_auto_vehicle_msgs::msg::SteeringReport current_steering_; - boost::optional prev_cmd_; + autoware_vehicle_msgs::msg::SteeringReport current_steering_; + boost::optional prev_cmd_; // Debug Publisher rclcpp::Publisher::SharedPtr pub_debug_marker_; rclcpp::Publisher::SharedPtr pub_debug_values_; // Predicted Trajectory publish - rclcpp::Publisher::SharedPtr - pub_predicted_trajectory_; + rclcpp::Publisher::SharedPtr pub_predicted_trajectory_; - void onTrajectory(const autoware_auto_planning_msgs::msg::Trajectory::ConstSharedPtr msg); + void onTrajectory(const autoware_planning_msgs::msg::Trajectory::ConstSharedPtr msg); void onCurrentOdometry(const nav_msgs::msg::Odometry::ConstSharedPtr msg); @@ -139,7 +138,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_{}; @@ -155,14 +154,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, @@ -170,7 +168,7 @@ class PurePursuitLateralController : public LateralControllerBase double calcCurvature(const size_t closest_idx); - void averageFilterTrajectory(autoware_auto_planning_msgs::msg::Trajectory & u); + void averageFilterTrajectory(autoware_planning_msgs::msg::Trajectory & u); // Debug mutable DebugData debug_data_; 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..a5ae31133be8e 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 #include @@ -79,15 +79,15 @@ class PurePursuitNode : public rclcpp::Node private: // Subscriber tier4_autoware_utils::SelfPoseListener self_pose_listener_{this}; - rclcpp::Subscription::SharedPtr sub_trajectory_; + rclcpp::Subscription::SharedPtr sub_trajectory_; rclcpp::Subscription::SharedPtr sub_current_odometry_; - autoware_auto_planning_msgs::msg::Trajectory::ConstSharedPtr trajectory_; + autoware_planning_msgs::msg::Trajectory::ConstSharedPtr trajectory_; nav_msgs::msg::Odometry::ConstSharedPtr current_odometry_; bool isDataReady(); - void onTrajectory(const autoware_auto_planning_msgs::msg::Trajectory::ConstSharedPtr msg); + void onTrajectory(const autoware_planning_msgs::msg::Trajectory::ConstSharedPtr msg); void onCurrentOdometry(const nav_msgs::msg::Odometry::ConstSharedPtr msg); // TF @@ -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); @@ -117,7 +116,7 @@ class PurePursuitNode : public rclcpp::Node std::unique_ptr pure_pursuit_; boost::optional calcTargetCurvature(); - boost::optional calcTargetPoint() const; + boost::optional calcTargetPoint() const; // Debug mutable DebugData debug_data_; diff --git a/control/pure_pursuit/include/pure_pursuit/util/planning_utils.hpp b/control/pure_pursuit/include/pure_pursuit/util/planning_utils.hpp index a5b1e17ed983f..b2d6e13c585f4 100644 --- a/control/pure_pursuit/include/pure_pursuit/util/planning_utils.hpp +++ b/control/pure_pursuit/include/pure_pursuit/util/planning_utils.hpp @@ -23,7 +23,7 @@ #include #include -#include +#include #include #include @@ -51,7 +51,7 @@ namespace planning_utils { constexpr double ERROR = 1e-6; double calcArcLengthFromWayPoint( - const autoware_auto_planning_msgs::msg::Trajectory & input_path, const size_t src_idx, + const autoware_planning_msgs::msg::Trajectory & input_path, const size_t src_idx, const size_t dst_idx); double calcCurvature( const geometry_msgs::msg::Point & target, const geometry_msgs::msg::Pose & curr_pose); @@ -66,7 +66,7 @@ double calcRadius( double convertCurvatureToSteeringAngle(double wheel_base, double kappa); std::vector extractPoses( - const autoware_auto_planning_msgs::msg::Trajectory & motions); + const autoware_planning_msgs::msg::Trajectory & motions); std::pair findClosestIdxWithDistAngThr( const std::vector & poses, diff --git a/control/pure_pursuit/package.xml b/control/pure_pursuit/package.xml index 30356b856fbac..b4820aee5cec4 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 + autoware_planning_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 f0c49b07e675a..a8232cce5d08d 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 @@ -95,7 +95,7 @@ PurePursuitLateralController::PurePursuitLateralController(rclcpp::Node & node) "~/debug/ld_outputs", rclcpp::QoS{1}); // Publish predicted trajectory - pub_predicted_trajectory_ = node.create_publisher( + pub_predicted_trajectory_ = node.create_publisher( "~/output/predicted_trajectory", 1); } @@ -138,7 +138,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); @@ -164,7 +164,7 @@ void PurePursuitLateralController::setResampledTrajectory() out_arclength.push_back(s); } trajectory_resampled_ = - std::make_shared(motion_utils::resampleTrajectory( + std::make_shared(motion_utils::resampleTrajectory( motion_utils::convertToTrajectory(input_tp_array), out_arclength)); trajectory_resampled_->points.back() = trajectory_.points.back(); trajectory_resampled_->header = trajectory_.header; @@ -215,14 +215,14 @@ double PurePursuitLateralController::calcCurvature(const size_t closest_idx) } void PurePursuitLateralController::averageFilterTrajectory( - autoware_auto_planning_msgs::msg::Trajectory & u) + autoware_planning_msgs::msg::Trajectory & u) { if (static_cast(u.points.size()) <= 2 * param_.path_filter_moving_ave_num) { RCLCPP_ERROR(logger_, "Cannot smooth path! Trajectory size is too low!"); return; } - autoware_auto_planning_msgs::msg::Trajectory filtered_trajectory(u); + autoware_planning_msgs::msg::Trajectory filtered_trajectory(u); for (int64_t i = 0; i < static_cast(u.points.size()); ++i) { TrajectoryPoint tmp{}; @@ -295,7 +295,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); @@ -310,7 +310,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); @@ -365,21 +365,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( @@ -393,12 +393,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 = 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..a4b491930df1e 100644 --- a/control/pure_pursuit/src/pure_pursuit/pure_pursuit_node.cpp +++ b/control/pure_pursuit/src/pure_pursuit/pure_pursuit_node.cpp @@ -73,14 +73,14 @@ PurePursuitNode::PurePursuitNode(const rclcpp::NodeOptions & node_options) // Subscribers using std::placeholders::_1; - sub_trajectory_ = this->create_subscription( + sub_trajectory_ = this->create_subscription( "input/reference_trajectory", 1, std::bind(&PurePursuitNode::onTrajectory, this, _1)); sub_current_odometry_ = this->create_subscription( "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_ = @@ -124,7 +124,7 @@ void PurePursuitNode::onCurrentOdometry(const nav_msgs::msg::Odometry::ConstShar } void PurePursuitNode::onTrajectory( - const autoware_auto_planning_msgs::msg::Trajectory::ConstSharedPtr msg) + const autoware_planning_msgs::msg::Trajectory::ConstSharedPtr msg) { trajectory_ = msg; } @@ -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); @@ -211,8 +211,8 @@ boost::optional PurePursuitNode::calcTargetCurvature() return kappa; } -boost::optional -PurePursuitNode::calcTargetPoint() const +boost::optional PurePursuitNode::calcTargetPoint() + const { const auto closest_idx_result = planning_utils::findClosestIdxWithDistAngThr( planning_utils::extractPoses(*trajectory_), current_pose_->pose, 3.0, M_PI_4); diff --git a/control/pure_pursuit/src/pure_pursuit_core/planning_utils.cpp b/control/pure_pursuit/src/pure_pursuit_core/planning_utils.cpp index 2597d7087ad61..b3a846157638a 100644 --- a/control/pure_pursuit/src/pure_pursuit_core/planning_utils.cpp +++ b/control/pure_pursuit/src/pure_pursuit_core/planning_utils.cpp @@ -23,7 +23,7 @@ namespace pure_pursuit namespace planning_utils { double calcArcLengthFromWayPoint( - const autoware_auto_planning_msgs::msg::Trajectory & input_path, const size_t src_idx, + const autoware_planning_msgs::msg::Trajectory & input_path, const size_t src_idx, const size_t dst_idx) { double length = 0; @@ -101,7 +101,7 @@ double convertCurvatureToSteeringAngle(double wheel_base, double kappa) } std::vector extractPoses( - const autoware_auto_planning_msgs::msg::Trajectory & trajectory) + const autoware_planning_msgs::msg::Trajectory & trajectory) { std::vector poses; diff --git a/control/shift_decider/README.md b/control/shift_decider/README.md index 820432e4c9e1a..c9fdc20696f0e 100644 --- a/control/shift_decider/README.md +++ b/control/shift_decider/README.md @@ -37,15 +37,15 @@ 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 -| Name | Type | Description | -| ------------------ | ---------------------------------------------- | ---------------------------------- | -| `~output/gear_cmd` | `autoware_auto_vehicle_msgs::msg::GearCommand` | Gear for drive forward / backward. | +| Name | Type | Description | +| ------------------ | ----------------------------------------- | ---------------------------------- | +| `~output/gear_cmd` | `autoware_vehicle_msgs::msg::GearCommand` | Gear for drive forward / backward. | ## Parameters diff --git a/control/shift_decider/include/shift_decider/shift_decider.hpp b/control/shift_decider/include/shift_decider/shift_decider.hpp index a6b9938e404f6..b11a0f40625af 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 +#include +#include #include @@ -31,26 +31,24 @@ class ShiftDecider : public rclcpp::Node private: void onTimer(); - void onControlCmd(autoware_auto_control_msgs::msg::AckermannControlCommand::SharedPtr msg); - void onAutowareState(autoware_auto_system_msgs::msg::AutowareState::SharedPtr msg); - void onCurrentGear(autoware_auto_vehicle_msgs::msg::GearReport::SharedPtr msg); + void onControlCmd(autoware_control_msgs::msg::Control::SharedPtr msg); + void onAutowareState(autoware_system_msgs::msg::AutowareState::SharedPtr msg); + void onCurrentGear(autoware_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_autoware_state_; - rclcpp::Subscription::SharedPtr sub_current_gear_; + rclcpp::Publisher::SharedPtr pub_shift_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_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_; - uint8_t prev_shift_command = autoware_auto_vehicle_msgs::msg::GearCommand::PARK; + autoware_control_msgs::msg::Control::SharedPtr control_cmd_; + autoware_system_msgs::msg::AutowareState::SharedPtr autoware_state_; + autoware_vehicle_msgs::msg::GearCommand shift_cmd_; + autoware_vehicle_msgs::msg::GearReport::SharedPtr current_gear_ptr_; + uint8_t prev_shift_command = autoware_vehicle_msgs::msg::GearCommand::PARK; bool park_on_goal_; }; diff --git a/control/shift_decider/package.xml b/control/shift_decider/package.xml index b24dbab1786e1..885e780c90bcc 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 + autoware_system_msgs + autoware_vehicle_msgs rclcpp rclcpp_components diff --git a/control/shift_decider/src/shift_decider.cpp b/control/shift_decider/src/shift_decider.cpp index 0e47cc7378f27..f003513060a34 100644 --- a/control/shift_decider/src/shift_decider.cpp +++ b/control/shift_decider/src/shift_decider.cpp @@ -33,29 +33,28 @@ ShiftDecider::ShiftDecider(const rclcpp::NodeOptions & node_options) park_on_goal_ = declare_parameter("park_on_goal"); pub_shift_cmd_ = - create_publisher("output/gear_cmd", durable_qos); - sub_control_cmd_ = create_subscription( + create_publisher("output/gear_cmd", durable_qos); + sub_control_cmd_ = create_subscription( "input/control_cmd", queue_size, std::bind(&ShiftDecider::onControlCmd, this, _1)); - sub_autoware_state_ = create_subscription( + sub_autoware_state_ = create_subscription( "input/state", queue_size, std::bind(&ShiftDecider::onAutowareState, this, _1)); - sub_current_gear_ = create_subscription( + sub_current_gear_ = create_subscription( "input/current_gear", queue_size, std::bind(&ShiftDecider::onCurrentGear, this, _1)); 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; } -void ShiftDecider::onAutowareState(autoware_auto_system_msgs::msg::AutowareState::SharedPtr msg) +void ShiftDecider::onAutowareState(autoware_system_msgs::msg::AutowareState::SharedPtr msg) { autoware_state_ = msg; } -void ShiftDecider::onCurrentGear(autoware_auto_vehicle_msgs::msg::GearReport::SharedPtr msg) +void ShiftDecider::onCurrentGear(autoware_vehicle_msgs::msg::GearReport::SharedPtr msg) { current_gear_ptr_ = msg; } @@ -72,15 +71,15 @@ void ShiftDecider::onTimer() void ShiftDecider::updateCurrentShiftCmd() { - using autoware_auto_system_msgs::msg::AutowareState; - using autoware_auto_vehicle_msgs::msg::GearCommand; + using autoware_system_msgs::msg::AutowareState; + using autoware_vehicle_msgs::msg::GearCommand; 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 = prev_shift_command; diff --git a/control/smart_mpc_trajectory_follower/package.xml b/control/smart_mpc_trajectory_follower/package.xml index 70bd15f97d609..6cdd689c8bd5a 100644 --- a/control/smart_mpc_trajectory_follower/package.xml +++ b/control/smart_mpc_trajectory_follower/package.xml @@ -17,10 +17,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 + autoware_planning_msgs + autoware_system_msgs + autoware_vehicle_msgs motion_utils pybind11_vendor python3-scipy diff --git a/control/smart_mpc_trajectory_follower/smart_mpc_trajectory_follower/scripts/pympc_trajectory_follower.py b/control/smart_mpc_trajectory_follower/smart_mpc_trajectory_follower/scripts/pympc_trajectory_follower.py index 409b6bf594c9e..992a5fd53a10c 100755 --- a/control/smart_mpc_trajectory_follower/smart_mpc_trajectory_follower/scripts/pympc_trajectory_follower.py +++ b/control/smart_mpc_trajectory_follower/smart_mpc_trajectory_follower/scripts/pympc_trajectory_follower.py @@ -19,10 +19,10 @@ import time from autoware_adapi_v1_msgs.msg import OperationModeState -from autoware_auto_control_msgs.msg import AckermannControlCommand -from autoware_auto_planning_msgs.msg import Trajectory -from autoware_auto_planning_msgs.msg import TrajectoryPoint -from autoware_auto_vehicle_msgs.msg import SteeringReport +from autoware_control_msgs.msg import Control +from autoware_planning_msgs.msg import Trajectory +from autoware_planning_msgs.msg import TrajectoryPoint +from autoware_vehicle_msgs.msg import SteeringReport from builtin_interfaces.msg import Duration from geometry_msgs.msg import AccelWithCovarianceStamped from geometry_msgs.msg import PoseStamped @@ -128,7 +128,7 @@ def __init__(self): self.sub_reload_mpc_param_trigger_ self.sub_control_command_control_cmd_ = self.create_subscription( - AckermannControlCommand, + Control, "/control/command/control_cmd", self.onControlCommandControlCmd, 3, @@ -136,7 +136,7 @@ def __init__(self): self.sub_control_command_control_cmd_ self.control_cmd_pub_ = self.create_publisher( - AckermannControlCommand, + Control, "/control/trajectory_follower/control_cmd", 1, ) @@ -699,7 +699,7 @@ def control(self): else: steer_cmd = 0.0 - cmd_msg = AckermannControlCommand() + cmd_msg = Control() cmd_msg.stamp = cmd_msg.lateral.stamp = cmd_msg.longitudinal.stamp = ( self.get_clock().now().to_msg() ) diff --git a/control/smart_mpc_trajectory_follower/smart_mpc_trajectory_follower/training_and_data_check/rosbag2.bash b/control/smart_mpc_trajectory_follower/smart_mpc_trajectory_follower/training_and_data_check/rosbag2.bash index 524dfe5a169df..d2687b352a17d 100644 --- a/control/smart_mpc_trajectory_follower/smart_mpc_trajectory_follower/training_and_data_check/rosbag2.bash +++ b/control/smart_mpc_trajectory_follower/smart_mpc_trajectory_follower/training_and_data_check/rosbag2.bash @@ -2,16 +2,16 @@ gnome-terminal -- bash -c 'ros2 topic echo /localization/kinematic_state nav_msgs/msg/Odometry --csv --qos-history keep_all --qos-reliability reliable > kinematic_state.csv' gnome-terminal -- bash -c 'ros2 topic echo /localization/acceleration geometry_msgs/msg/AccelWithCovarianceStamped --csv --qos-history keep_all --qos-reliability reliable > acceleration.csv' -gnome-terminal -- bash -c 'ros2 topic echo /vehicle/status/steering_status autoware_auto_vehicle_msgs/msg/SteeringReport --csv --qos-history keep_all --qos-reliability reliable > steering_status.csv' +gnome-terminal -- bash -c 'ros2 topic echo /vehicle/status/steering_status autoware_vehicle_msgs/msg/SteeringReport --csv --qos-history keep_all --qos-reliability reliable > steering_status.csv' -gnome-terminal -- bash -c 'ros2 topic echo /control/command/control_cmd autoware_auto_control_msgs/msg/AckermannControlCommand --csv --qos-history keep_all --qos-reliability reliable > control_cmd.csv' -gnome-terminal -- bash -c 'ros2 topic echo /control/trajectory_follower/control_cmd autoware_auto_control_msgs/msg/AckermannControlCommand --csv --qos-history keep_all --qos-reliability reliable > control_cmd_orig.csv' +gnome-terminal -- bash -c 'ros2 topic echo /control/command/control_cmd autoware_control_msgs/msg/Control --csv --qos-history keep_all --qos-reliability reliable > control_cmd.csv' +gnome-terminal -- bash -c 'ros2 topic echo /control/trajectory_follower/control_cmd autoware_control_msgs/msg/Control --csv --qos-history keep_all --qos-reliability reliable > control_cmd_orig.csv' gnome-terminal -- bash -c 'ros2 topic echo /control/trajectory_follower/lane_departure_checker_node/debug/deviation/lateral tier4_debug_msgs/msg/Float64Stamped --csv --qos-history keep_all --qos-reliability reliable > lateral_error.csv' gnome-terminal -- bash -c 'ros2 topic echo /control/trajectory_follower/lane_departure_checker_node/debug/deviation/yaw tier4_debug_msgs/msg/Float64Stamped --csv --qos-history keep_all --qos-reliability reliable > yaw_error.csv' gnome-terminal -- bash -c 'ros2 topic echo /system/operation_mode/state autoware_adapi_v1_msgs/msg/OperationModeState --csv --qos-history keep_all --qos-reliability reliable > system_operation_mode_state.csv' -gnome-terminal -- bash -c 'ros2 topic echo /vehicle/status/control_mode autoware_auto_vehicle_msgs/msg/ControlModeReport --csv --qos-history keep_all --qos-reliability reliable > vehicle_status_control_mode.csv' +gnome-terminal -- bash -c 'ros2 topic echo /vehicle/status/control_mode autoware_vehicle_msgs/msg/ControlModeReport --csv --qos-history keep_all --qos-reliability reliable > vehicle_status_control_mode.csv' gnome-terminal -- bash -c 'ros2 topic echo /sensing/imu/imu_data sensor_msgs/msg/Imu --csv --qos-history keep_all --qos-reliability reliable > imu.csv' gnome-terminal -- bash -c 'ros2 topic echo /debug_mpc_x_des tier4_debug_msgs/msg/Float32MultiArrayStamped --csv --qos-history keep_all --qos-reliability reliable > debug_mpc_x_des.csv' diff --git a/control/trajectory_follower_base/include/trajectory_follower_base/input_data.hpp b/control/trajectory_follower_base/include/trajectory_follower_base/input_data.hpp index 52a82526a9548..665a8604214dd 100644 --- a/control/trajectory_follower_base/include/trajectory_follower_base/input_data.hpp +++ b/control/trajectory_follower_base/include/trajectory_follower_base/input_data.hpp @@ -16,8 +16,8 @@ #define TRAJECTORY_FOLLOWER_BASE__INPUT_DATA_HPP_ #include "autoware_adapi_v1_msgs/msg/operation_mode_state.hpp" -#include "autoware_auto_planning_msgs/msg/trajectory.hpp" -#include "autoware_auto_vehicle_msgs/msg/steering_report.hpp" +#include "autoware_planning_msgs/msg/trajectory.hpp" +#include "autoware_vehicle_msgs/msg/steering_report.hpp" #include "geometry_msgs/msg/accel_with_covariance_stamped.hpp" #include "nav_msgs/msg/odometry.hpp" @@ -25,9 +25,9 @@ namespace autoware::motion::control::trajectory_follower { struct InputData { - autoware_auto_planning_msgs::msg::Trajectory current_trajectory; + autoware_planning_msgs::msg::Trajectory current_trajectory; nav_msgs::msg::Odometry current_odometry; - autoware_auto_vehicle_msgs::msg::SteeringReport current_steering; + autoware_vehicle_msgs::msg::SteeringReport current_steering; geometry_msgs::msg::AccelWithCovarianceStamped current_accel; autoware_adapi_v1_msgs::msg::OperationModeState current_operation_mode; }; 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..6f2e9c3e8ebc2 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 + autoware_planning_msgs + autoware_vehicle_msgs diagnostic_msgs diagnostic_updater eigen diff --git a/control/trajectory_follower_node/README.md b/control/trajectory_follower_node/README.md index c3ea3ddf6b7e8..3496284efd670 100644 --- a/control/trajectory_follower_node/README.md +++ b/control/trajectory_follower_node/README.md @@ -129,19 +129,19 @@ Giving the longitudinal controller information about steer convergence allows it #### Inputs -- `autoware_auto_planning_msgs/Trajectory` : reference trajectory to follow. +- `autoware_planning_msgs/Trajectory` : reference trajectory to follow. - `nav_msgs/Odometry`: current odometry -- `autoware_auto_vehicle_msgs/SteeringReport` current steering +- `autoware_vehicle_msgs/SteeringReport` current steering #### Outputs -- `autoware_auto_control_msgs/AckermannControlCommand`: message containing both lateral and longitudinal commands. +- `autoware_control_msgs/Control`: message containing both lateral and longitudinal commands. #### Parameter - `ctrl_period`: control commands publishing period - `timeout_thr_sec`: duration in second after which input messages are discarded. - - Each time the node receives lateral and longitudinal commands from each controller, it publishes an `AckermannControlCommand` if the following two conditions are met. + - Each time the node receives lateral and longitudinal commands from each controller, it publishes an `Control` if the following two conditions are met. 1. Both commands have been received. 2. The last received commands are not older than defined by `timeout_thr_sec`. - `lateral_controller_mode`: `mpc` or `pure_pursuit` 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..4c1ee1b422484 100644 --- a/control/trajectory_follower_node/design/simple_trajectory_follower-design.md +++ b/control/trajectory_follower_node/design/simple_trajectory_follower-design.md @@ -10,10 +10,10 @@ Provide a base trajectory follower code that is simple and flexible to use. This Inputs -- `input/reference_trajectory` [autoware_auto_planning_msgs::msg::Trajectory] : reference trajectory to follow. +- `input/reference_trajectory` [autoware_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 be4fe885ae8c1..8e9752ba19f66 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 @@ -30,10 +30,9 @@ #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 "autoware_planning_msgs/msg/trajectory.hpp" #include "geometry_msgs/msg/accel_stamped.hpp" #include "geometry_msgs/msg/accel_with_covariance_stamped.hpp" #include "geometry_msgs/msg/pose_stamped.hpp" @@ -76,20 +75,19 @@ class TRAJECTORY_FOLLOWER_PUBLIC Controller : public rclcpp::Node std::shared_ptr longitudinal_controller_; std::shared_ptr lateral_controller_; - rclcpp::Subscription::SharedPtr sub_ref_path_; + rclcpp::Subscription::SharedPtr sub_ref_path_; rclcpp::Subscription::SharedPtr sub_odometry_; - rclcpp::Subscription::SharedPtr sub_steering_; + 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 pub_processing_time_lat_ms_; rclcpp::Publisher::SharedPtr pub_processing_time_lon_ms_; rclcpp::Publisher::SharedPtr debug_marker_pub_; - autoware_auto_planning_msgs::msg::Trajectory::SharedPtr current_trajectory_ptr_; + autoware_planning_msgs::msg::Trajectory::SharedPtr current_trajectory_ptr_; nav_msgs::msg::Odometry::SharedPtr current_odometry_ptr_; - autoware_auto_vehicle_msgs::msg::SteeringReport::SharedPtr current_steering_ptr_; + autoware_vehicle_msgs::msg::SteeringReport::SharedPtr current_steering_ptr_; geometry_msgs::msg::AccelWithCovarianceStamped::SharedPtr current_accel_ptr_; OperationModeState::SharedPtr current_operation_mode_ptr_; @@ -108,9 +106,9 @@ class TRAJECTORY_FOLLOWER_PUBLIC Controller : public rclcpp::Node */ boost::optional createInputData(rclcpp::Clock & clock) const; void callbackTimerControl(); - void onTrajectory(const autoware_auto_planning_msgs::msg::Trajectory::SharedPtr); + void onTrajectory(const autoware_planning_msgs::msg::Trajectory::SharedPtr); void onOdometry(const nav_msgs::msg::Odometry::SharedPtr msg); - void onSteering(const autoware_auto_vehicle_msgs::msg::SteeringReport::SharedPtr msg); + void onSteering(const autoware_vehicle_msgs::msg::SteeringReport::SharedPtr msg); void onAccel(const geometry_msgs::msg::AccelWithCovarianceStamped::SharedPtr msg); bool isTimeOut(const LongitudinalOutput & lon_out, const LateralOutput & lat_out); LateralControllerMode getLateralControllerMode(const std::string & algorithm_name) const; 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..e744243969cab 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 #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 autoware_planning_msgs::msg::Trajectory; +using autoware_planning_msgs::msg::TrajectoryPoint; 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 36b4d0108de78..65446dfb3dd01 100644 --- a/control/trajectory_follower_node/package.xml +++ b/control/trajectory_follower_node/package.xml @@ -20,10 +20,9 @@ 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 + autoware_planning_msgs + autoware_vehicle_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 801f42ad9a470..6fe63ca07de6f 100644 --- a/control/trajectory_follower_node/src/controller_node.cpp +++ b/control/trajectory_follower_node/src/controller_node.cpp @@ -62,9 +62,9 @@ Controller::Controller(const rclcpp::NodeOptions & node_options) : Node("control throw std::domain_error("[LongitudinalController] invalid algorithm"); } - sub_ref_path_ = create_subscription( + sub_ref_path_ = create_subscription( "~/input/reference_trajectory", rclcpp::QoS{1}, std::bind(&Controller::onTrajectory, this, _1)); - sub_steering_ = create_subscription( + sub_steering_ = create_subscription( "~/input/current_steering", rclcpp::QoS{1}, std::bind(&Controller::onSteering, this, _1)); sub_odometry_ = create_subscription( "~/input/current_odometry", rclcpp::QoS{1}, std::bind(&Controller::onOdometry, this, _1)); @@ -73,7 +73,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()); pub_processing_time_lat_ms_ = create_publisher("~/lateral/debug/processing_time_ms", 1); @@ -112,7 +112,7 @@ Controller::LongitudinalControllerMode Controller::getLongitudinalControllerMode return LongitudinalControllerMode::INVALID; } -void Controller::onTrajectory(const autoware_auto_planning_msgs::msg::Trajectory::SharedPtr msg) +void Controller::onTrajectory(const autoware_planning_msgs::msg::Trajectory::SharedPtr msg) { current_trajectory_ptr_ = msg; } @@ -122,7 +122,7 @@ void Controller::onOdometry(const nav_msgs::msg::Odometry::SharedPtr msg) current_odometry_ptr_ = msg; } -void Controller::onSteering(const autoware_auto_vehicle_msgs::msg::SteeringReport::SharedPtr msg) +void Controller::onSteering(const autoware_vehicle_msgs::msg::SteeringReport::SharedPtr msg) { current_steering_ptr_ = msg; } @@ -227,7 +227,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 5e9488bf2ca93..7c27eaed41380 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 1aa4035e98d51..9bdf625226134 100644 --- a/control/trajectory_follower_node/test/test_controller_node.cpp +++ b/control/trajectory_follower_node/test/test_controller_node.cpp @@ -21,10 +21,9 @@ #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/control.hpp" +#include "autoware_planning_msgs/msg/trajectory.hpp" +#include "autoware_vehicle_msgs/msg/steering_report.hpp" #include "geometry_msgs/msg/accel_with_covariance_stamped.hpp" #include "geometry_msgs/msg/pose.hpp" #include "geometry_msgs/msg/pose_stamped.hpp" @@ -35,11 +34,11 @@ #include using Controller = autoware::motion::control::trajectory_follower_node::Controller; -using AckermannControlCommand = autoware_auto_control_msgs::msg::AckermannControlCommand; -using Trajectory = autoware_auto_planning_msgs::msg::Trajectory; -using TrajectoryPoint = autoware_auto_planning_msgs::msg::TrajectoryPoint; +using Control = autoware_control_msgs::msg::Control; +using Trajectory = autoware_planning_msgs::msg::Trajectory; +using TrajectoryPoint = autoware_planning_msgs::msg::TrajectoryPoint; using VehicleOdometry = nav_msgs::msg::Odometry; -using SteeringReport = autoware_auto_vehicle_msgs::msg::SteeringReport; +using SteeringReport = autoware_vehicle_msgs::msg::SteeringReport; using autoware_adapi_v1_msgs::msg::OperationModeState; using geometry_msgs::msg::AccelWithCovarianceStamped; @@ -96,7 +95,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() @@ -178,13 +177,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()); @@ -255,7 +252,7 @@ TEST_F(FakeNodeFixture, straight_trajectory) // following conditions will pass even if the MPC solution does not converge EXPECT_EQ(tester.cmd_msg->lateral.steering_tire_angle, 0.0f); EXPECT_EQ(tester.cmd_msg->lateral.steering_tire_rotation_rate, 0.0f); - EXPECT_GT(tester.cmd_msg->longitudinal.speed, 0.0f); + EXPECT_GT(tester.cmd_msg->longitudinal.velocity, 0.0f); EXPECT_GT(rclcpp::Time(tester.cmd_msg->stamp), rclcpp::Time(traj_msg.header.stamp)); } @@ -369,14 +366,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 +403,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 +440,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 +474,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 +504,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 +532,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 +563,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 +594,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/vehicle_cmd_gate/README.md b/control/vehicle_cmd_gate/README.md index 6ec2da84a7b6c..2d6f5c5f949af 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_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_vehicle_msgs::msg::TurnIndicatorsCommand` | turn indicators command from planning module | +| `~/input/auto/hazard_lights_cmd` | `autoware_vehicle_msgs::msg::HazardLightsCommand` | hazard lights command from planning module | +| `~/input/auto/gear_cmd` | `autoware_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_vehicle_msgs::msg::TurnIndicatorsCommand` | turn indicators command from external | +| `~/input/external/hazard_lights_cmd` | `autoware_vehicle_msgs::msg::HazardLightsCommand` | hazard lights command from external | +| `~/input/external/gear_cmd` | `autoware_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_vehicle_msgs::msg::HazardLightsCommand` | hazard lights command from emergency handler | +| `~/input/emergency/gear_cmd` | `autoware_vehicle_msgs::msg::GearCommand` | gear command from emergency handler | +| `~/input/engage` | `autoware_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` | `tier4_vehicle_msgs::msg::VehicleEmergencyStamped` | 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_vehicle_msgs::msg::TurnIndicatorsCommand` | turn indicators command to vehicle | +| `~/output/command/hazard_lights_cmd` | `autoware_vehicle_msgs::msg::HazardLightsCommand` | hazard lights command to vehicle | +| `~/output/command/gear_cmd` | `autoware_vehicle_msgs::msg::GearCommand` | gear command to vehicle | +| `~/output/gate_mode` | `tier4_control_msgs::msg::GateMode` | gate mode (AUTO or EXTERNAL) | +| `~/output/engage` | `autoware_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 22ae9da6d222e..e01d566c37df9 100644 --- a/control/vehicle_cmd_gate/package.xml +++ b/control/vehicle_cmd_gate/package.xml @@ -16,9 +16,8 @@ rosidl_default_generators autoware_adapi_v1_msgs - autoware_auto_control_msgs - autoware_auto_system_msgs - autoware_auto_vehicle_msgs + autoware_control_msgs + autoware_vehicle_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..a67c794877a92 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 autoware_vehicle_msgs.msg import Engage 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..b643afc212f61 100644 --- a/control/vehicle_cmd_gate/src/moderate_stop_interface.hpp +++ b/control/vehicle_cmd_gate/src/moderate_stop_interface.hpp @@ -19,8 +19,6 @@ #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 b289365b46b3b..bd9955e773020 100644 --- a/control/vehicle_cmd_gate/src/vehicle_cmd_filter.cpp +++ b/control/vehicle_cmd_gate/src/vehicle_cmd_filter.cpp @@ -95,24 +95,23 @@ VehicleCmdFilterParam VehicleCmdFilter::getParam() const return param_; } -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 { const auto lon_acc_lim = getLonAccLim(); input.longitudinal.acceleration = std::max( std::min(static_cast(input.longitudinal.acceleration), lon_acc_lim), -lon_acc_lim); - input.longitudinal.speed = - limitDiff(input.longitudinal.speed, prev_cmd_.longitudinal.speed, lon_acc_lim * dt); + input.longitudinal.velocity = + limitDiff(input.longitudinal.velocity, prev_cmd_.longitudinal.velocity, lon_acc_lim * dt); } void VehicleCmdFilter::VehicleCmdFilter::limitLongitudinalWithJerk( - const double dt, AckermannControlCommand & input) const + const double dt, Control & input) const { const auto lon_jerk_lim = getLonJerkLim(); input.longitudinal.acceleration = limitDiff( @@ -124,7 +123,7 @@ void VehicleCmdFilter::VehicleCmdFilter::limitLongitudinalWithJerk( // Use ego vehicle speed (not speed command) for the lateral acceleration calculation, otherwise the // filtered steering angle oscillates if the input velocity oscillates. void VehicleCmdFilter::limitLateralWithLatAcc( - [[maybe_unused]] const double dt, AckermannControlCommand & input) const + [[maybe_unused]] const double dt, Control & input) const { const auto lat_acc_lim = getLatAccLim(); @@ -138,8 +137,7 @@ void VehicleCmdFilter::limitLateralWithLatAcc( // Use ego vehicle speed (not speed command) for the lateral acceleration calculation, otherwise the // filtered steering angle oscillates if the input velocity oscillates. -void VehicleCmdFilter::limitLateralWithLatJerk( - const double dt, AckermannControlCommand & input) const +void VehicleCmdFilter::limitLateralWithLatJerk(const double dt, Control & input) const { double curr_latacc = calcLatAcc(input, current_speed_); double prev_latacc = calcLatAcc(prev_cmd_, current_speed_); @@ -156,8 +154,7 @@ void VehicleCmdFilter::limitLateralWithLatJerk( } } -void VehicleCmdFilter::limitActualSteerDiff( - const double current_steer_angle, AckermannControlCommand & input) const +void VehicleCmdFilter::limitActualSteerDiff(const double current_steer_angle, Control & input) const { const auto actual_steer_diff_lim = getSteerDiffLim(); @@ -166,7 +163,7 @@ void VehicleCmdFilter::limitActualSteerDiff( input.lateral.steering_tire_angle = current_steer_angle + ds; } -void VehicleCmdFilter::limitLateralSteer(AckermannControlCommand & input) const +void VehicleCmdFilter::limitLateralSteer(Control & input) const { const float steer_limit = getSteerLim(); @@ -185,7 +182,7 @@ void VehicleCmdFilter::limitLateralSteer(AckermannControlCommand & input) const } } -void VehicleCmdFilter::limitLateralSteerRate(const double dt, AckermannControlCommand & input) const +void VehicleCmdFilter::limitLateralSteerRate(const double dt, Control & input) const { const float steer_rate_limit = getSteerRateLim(); @@ -201,7 +198,7 @@ void VehicleCmdFilter::limitLateralSteerRate(const double dt, AckermannControlCo } void VehicleCmdFilter::filterAll( - const double dt, const double current_steer_angle, AckermannControlCommand & cmd, + const double dt, const double current_steer_angle, Control & cmd, IsFilterActivated & is_activated) const { const auto cmd_orig = cmd; @@ -219,14 +216,14 @@ void VehicleCmdFilter::filterAll( } IsFilterActivated VehicleCmdFilter::checkIsActivated( - const AckermannControlCommand & c1, const AckermannControlCommand & c2, const double tol) + const Control & c1, const Control & c2, const double tol) { IsFilterActivated msg; msg.is_activated_on_steering = std::abs(c1.lateral.steering_tire_angle - c2.lateral.steering_tire_angle) > tol; msg.is_activated_on_steering_rate = std::abs(c1.lateral.steering_tire_rotation_rate - c2.lateral.steering_tire_rotation_rate) > tol; - msg.is_activated_on_speed = std::abs(c1.longitudinal.speed - c2.longitudinal.speed) > tol; + msg.is_activated_on_speed = std::abs(c1.longitudinal.velocity - c2.longitudinal.velocity) > tol; msg.is_activated_on_acceleration = std::abs(c1.longitudinal.acceleration - c2.longitudinal.acceleration) > tol; msg.is_activated_on_jerk = std::abs(c1.longitudinal.jerk - c2.longitudinal.jerk) > tol; @@ -244,13 +241,13 @@ 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; } -double VehicleCmdFilter::calcLatAcc(const AckermannControlCommand & cmd, const double v) const +double VehicleCmdFilter::calcLatAcc(const Control & cmd, const double v) const { 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 e71fb405beda1..d9b8383a1de51 100644 --- a/control/vehicle_cmd_gate/src/vehicle_cmd_filter.hpp +++ b/control/vehicle_cmd_gate/src/vehicle_cmd_filter.hpp @@ -18,13 +18,13 @@ #include #include -#include +#include #include namespace vehicle_cmd_gate { -using autoware_auto_control_msgs::msg::AckermannControlCommand; +using autoware_control_msgs::msg::Control; using vehicle_cmd_gate::msg::IsFilterActivated; using LimitArray = std::vector; @@ -59,35 +59,33 @@ class VehicleCmdFilter void setCurrentSpeed(double v) { current_speed_ = v; } void setParam(const VehicleCmdFilterParam & p); VehicleCmdFilterParam getParam() const; - 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 limitLateralSteer(AckermannControlCommand & input) const; - void limitLateralSteerRate(const double dt, 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 limitLateralSteer(Control & input) const; + void limitLateralSteerRate(const double dt, Control & input) const; void filterAll( - const double dt, const double current_steer_angle, AckermannControlCommand & input, + const double dt, const double current_steer_angle, Control & input, IsFilterActivated & is_activated) const; static IsFilterActivated checkIsActivated( - const AckermannControlCommand & c1, const AckermannControlCommand & c2, - const double tol = 1.0e-3); + const Control & c1, const Control & c2, const double tol = 1.0e-3); - AckermannControlCommand getPrevCmd() { return prev_cmd_; } + Control getPrevCmd() { return prev_cmd_; } private: VehicleCmdFilterParam param_; - AckermannControlCommand prev_cmd_; + Control prev_cmd_; double current_speed_ = 0.0; bool setParameterWithValidation(const VehicleCmdFilterParam & p); - double calcLatAcc(const AckermannControlCommand & cmd) const; - double calcLatAcc(const AckermannControlCommand & cmd, const double v) const; + double calcLatAcc(const Control & cmd) const; + double calcLatAcc(const Control & cmd, const double v) 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 ffd452db0337c..cc9d7813083f4 100644 --- a/control/vehicle_cmd_gate/src/vehicle_cmd_gate.cpp +++ b/control/vehicle_cmd_gate/src/vehicle_cmd_gate.cpp @@ -62,7 +62,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); @@ -108,7 +108,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( @@ -124,7 +124,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( @@ -140,7 +140,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( @@ -354,7 +354,7 @@ bool VehicleCmdGate::isDataReady() } // for auto -void VehicleCmdGate::onAutoCtrlCmd(AckermannControlCommand::ConstSharedPtr msg) +void VehicleCmdGate::onAutoCtrlCmd(Control::ConstSharedPtr msg) { auto_commands_.control = *msg; @@ -364,7 +364,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; @@ -374,7 +374,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; @@ -489,7 +489,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_; } @@ -543,7 +543,7 @@ void VehicleCmdGate::publishEmergencyStopControlCommands() const auto stamp = this->now(); // ControlCommand - AckermannControlCommand control_cmd; + Control control_cmd; control_cmd.stamp = stamp; control_cmd = createEmergencyStopControlCmd(); @@ -600,9 +600,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(); @@ -629,10 +629,10 @@ AckermannControlCommand VehicleCmdGate::filterControlCommand(const AckermannCont prev_cmd.longitudinal.acceleration = std::max(prev_cmd.longitudinal.acceleration, current_status_cmd.longitudinal.acceleration); // consider reverse driving - prev_cmd.longitudinal.speed = - std::fabs(prev_cmd.longitudinal.speed) > std::fabs(current_status_cmd.longitudinal.speed) - ? prev_cmd.longitudinal.speed - : current_status_cmd.longitudinal.speed; + prev_cmd.longitudinal.velocity = std::fabs(prev_cmd.longitudinal.velocity) > + std::fabs(current_status_cmd.longitudinal.velocity) + ? prev_cmd.longitudinal.velocity + : current_status_cmd.longitudinal.velocity; filter_.setPrevCmd(prev_cmd); } filter_.filterAll(dt, current_steer_, out, is_filter_activated); @@ -663,42 +663,42 @@ 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; } -LongitudinalCommand VehicleCmdGate::createLongitudinalStopControlCmd() const +Longitudinal VehicleCmdGate::createLongitudinalStopControlCmd() const { - LongitudinalCommand cmd; + Longitudinal cmd; const auto t = this->now(); cmd.stamp = t; - cmd.speed = 0.0; + cmd.velocity = 0.0; cmd.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; @@ -757,13 +757,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 85bc183361b94..f45280d9d2e48 100644 --- a/control/vehicle_cmd_gate/src/vehicle_cmd_gate.hpp +++ b/control/vehicle_cmd_gate/src/vehicle_cmd_gate.hpp @@ -29,12 +29,12 @@ #include #include -#include -#include -#include -#include -#include -#include +#include +#include +#include +#include +#include +#include #include #include #include @@ -56,12 +56,12 @@ 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_control_msgs::msg::LongitudinalCommand; -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 autoware_control_msgs::msg::Longitudinal; +using autoware_vehicle_msgs::msg::GearCommand; +using autoware_vehicle_msgs::msg::HazardLightsCommand; +using autoware_vehicle_msgs::msg::SteeringReport; +using autoware_vehicle_msgs::msg::TurnIndicatorsCommand; using geometry_msgs::msg::AccelWithCovarianceStamped; using std_srvs::srv::Trigger; using tier4_control_msgs::msg::GateMode; @@ -77,13 +77,13 @@ using visualization_msgs::msg::MarkerArray; using diagnostic_msgs::msg::DiagnosticStatus; using nav_msgs::msg::Odometry; -using EngageMsg = autoware_auto_vehicle_msgs::msg::Engage; +using EngageMsg = autoware_vehicle_msgs::msg::Engage; 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; @@ -101,7 +101,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_; @@ -153,26 +153,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_; @@ -224,17 +224,17 @@ class VehicleCmdGate : public rclcpp::Node void checkExternalEmergencyStop(diagnostic_updater::DiagnosticStatusWrapper & stat); // Algorithm - AckermannControlCommand prev_control_cmd_; - AckermannControlCommand createStopControlCmd() const; - LongitudinalCommand createLongitudinalStopControlCmd() const; - AckermannControlCommand createEmergencyStopControlCmd() const; + Control prev_control_cmd_; + Control createStopControlCmd() const; + Longitudinal createLongitudinalStopControlCmd() 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_filter_in_vehicle_cmd_gate_node.cpp b/control/vehicle_cmd_gate/test/src/test_filter_in_vehicle_cmd_gate_node.cpp index 949ca46d27dea..d51db90c8a260 100644 --- a/control/vehicle_cmd_gate/test/src/test_filter_in_vehicle_cmd_gate_node.cpp +++ b/control/vehicle_cmd_gate/test/src/test_filter_in_vehicle_cmd_gate_node.cpp @@ -58,26 +58,25 @@ using vehicle_cmd_gate::VehicleCmdGate; 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 autoware_vehicle_msgs::msg::GearCommand; +using autoware_vehicle_msgs::msg::HazardLightsCommand; +using autoware_vehicle_msgs::msg::SteeringReport; +using autoware_vehicle_msgs::msg::TurnIndicatorsCommand; using geometry_msgs::msg::AccelWithCovarianceStamped; using nav_msgs::msg::Odometry; using tier4_control_msgs::msg::GateMode; using tier4_external_api_msgs::msg::Emergency; using tier4_external_api_msgs::msg::Heartbeat; -using EngageMsg = autoware_auto_vehicle_msgs::msg::Engage; +using EngageMsg = autoware_vehicle_msgs::msg::Engage; class PubSubNode : public rclcpp::Node { public: PubSubNode() : Node{"test_vehicle_cmd_gate_filter_pubsub"} { - sub_cmd_ = create_subscription( - "output/control_cmd", rclcpp::QoS{1}, - [this](const AckermannControlCommand::ConstSharedPtr msg) { + sub_cmd_ = create_subscription( + "output/control_cmd", rclcpp::QoS{1}, [this](const Control::ConstSharedPtr msg) { cmd_history_.push_back(msg); cmd_received_times_.push_back(now()); // check the effectiveness of the filter for last x elements in the queue @@ -97,8 +96,7 @@ class PubSubNode : public rclcpp::Node pub_operation_mode_ = create_publisher("input/operation_mode", qos); pub_mrm_state_ = create_publisher("input/mrm_state", qos); - pub_auto_control_cmd_ = - create_publisher("input/auto/control_cmd", qos); + pub_auto_control_cmd_ = create_publisher("input/auto/control_cmd", qos); pub_auto_turn_indicator_cmd_ = create_publisher("input/auto/turn_indicators_cmd", qos); pub_auto_hazard_light_cmd_ = @@ -106,7 +104,7 @@ class PubSubNode : public rclcpp::Node pub_auto_gear_cmd_ = create_publisher("input/auto/gear_cmd", qos); } - rclcpp::Subscription::SharedPtr sub_cmd_; + rclcpp::Subscription::SharedPtr sub_cmd_; rclcpp::Publisher::SharedPtr pub_external_emergency_stop_heartbeat_; rclcpp::Publisher::SharedPtr pub_engage_; @@ -116,13 +114,13 @@ class PubSubNode : public rclcpp::Node rclcpp::Publisher::SharedPtr pub_steer_; rclcpp::Publisher::SharedPtr pub_operation_mode_; rclcpp::Publisher::SharedPtr pub_mrm_state_; - rclcpp::Publisher::SharedPtr pub_auto_control_cmd_; + rclcpp::Publisher::SharedPtr pub_auto_control_cmd_; rclcpp::Publisher::SharedPtr pub_auto_turn_indicator_cmd_; rclcpp::Publisher::SharedPtr pub_auto_hazard_light_cmd_; rclcpp::Publisher::SharedPtr pub_auto_gear_cmd_; - std::vector cmd_history_; - std::vector raw_cmd_history_; + std::vector cmd_history_; + std::vector raw_cmd_history_; std::vector cmd_received_times_; // publish except for the control_cmd @@ -152,7 +150,7 @@ class PubSubNode : public rclcpp::Node msg.twist.twist.linear.x = 0.0; if (!cmd_history_.empty()) { // ego moves as commanded. msg.twist.twist.linear.x = - cmd_history_.back()->longitudinal.speed; // ego moves as commanded. + cmd_history_.back()->longitudinal.velocity; // ego moves as commanded. } pub_odom_->publish(msg); } @@ -209,11 +207,11 @@ class PubSubNode : public rclcpp::Node } } - void publishControlCommand(AckermannControlCommand msg) + void publishControlCommand(Control msg) { msg.stamp = now(); pub_auto_control_cmd_->publish(msg); - raw_cmd_history_.push_back(std::make_shared(msg)); + raw_cmd_history_.push_back(std::make_shared(msg)); } void checkFilter(size_t last_x) @@ -249,7 +247,7 @@ class PubSubNode : public rclcpp::Node // TODO(Horibe): prev_lat_acc should use the previous velocity, but use the current velocity // since the current filtering logic uses the current velocity. // when it's fixed, should be like this: - // prev_lat_acc = calculate_lateral_acceleration(cmd_start->longitudinal.speed, + // prev_lat_acc = calculate_lateral_acceleration(cmd_start->longitudinal.velocity, // cmd_start->lateral.steering_tire_angle, wheelbase); prev_tire_angle = cmd_start->lateral.steering_tire_angle; } @@ -261,7 +259,7 @@ class PubSubNode : public rclcpp::Node ASSERT_GT(dt, 0.0) << "Invalid dt. Time must be strictly positive."; - lon_vel = cmd->longitudinal.speed; + lon_vel = cmd->longitudinal.velocity; const auto lon_acc = cmd->longitudinal.acceleration; const auto lon_jerk = (lon_acc - prev_lon_acc) / dt; @@ -341,7 +339,7 @@ class ControlCmdGenerator // generate ControlCommand with sin wave format. // TODO(Horibe): implement steering_rate and jerk command. - AckermannControlCommand calcSinWaveCommand(bool reset_clock = false) + Control calcSinWaveCommand(bool reset_clock = false) { if (reset_clock) { start_time_ = Clock::now(); @@ -355,9 +353,9 @@ class ControlCmdGenerator return amp * std::sin(2.0 * M_PI * freq * dt_s + bias); }; - AckermannControlCommand cmd; + Control cmd; cmd.lateral.steering_tire_angle = sinWave(p_.steering.max, p_.steering.freq, p_.steering.bias); - cmd.longitudinal.speed = + cmd.longitudinal.velocity = sinWave(p_.velocity.max, p_.velocity.freq, p_.velocity.bias) + p_.velocity.max; cmd.longitudinal.acceleration = sinWave(p_.acceleration.max, p_.acceleration.freq, p_.acceleration.bias); 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 b8200490dd1d5..5fbab1c5cfb4f 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; using vehicle_cmd_gate::LimitArray; constexpr double NOMINAL_INTERVAL = 1.0; @@ -49,38 +49,37 @@ void setFilterParams( f.setParam(p); } -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; } // calc from ego velocity -double calcLatAcc(const AckermannControlCommand & cmd, const double wheelbase, const double ego_v) +double calcLatAcc(const Control & cmd, const double wheelbase, const double ego_v) { return ego_v * ego_v * std::tan(cmd.lateral.steering_tire_angle) / wheelbase; } // calc from command velocity -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; } // calc from command velocity double calcLatJerk( - const AckermannControlCommand & cmd, const AckermannControlCommand & prev_cmd, - const double wheelbase, const double dt) + const Control & cmd, const Control & prev_cmd, const double wheelbase, const double dt) { - const auto prev_v = prev_cmd.longitudinal.speed; + const auto prev_v = prev_cmd.longitudinal.velocity; const auto prev = prev_v * prev_v * std::tan(prev_cmd.lateral.steering_tire_angle) / wheelbase; - const auto curr_v = cmd.longitudinal.speed; + const auto curr_v = cmd.longitudinal.velocity; const auto curr = curr_v * curr_v * std::tan(cmd.lateral.steering_tire_angle) / wheelbase; return (curr - prev) / dt; @@ -88,8 +87,8 @@ double calcLatJerk( // calc from ego velocity double calcLatJerk( - const AckermannControlCommand & cmd, const AckermannControlCommand & prev_cmd, - const double wheelbase, const double dt, const double ego_v) + const Control & cmd, const Control & prev_cmd, const double wheelbase, const double dt, + const double ego_v) { const auto prev = ego_v * ego_v * std::tan(prev_cmd.lateral.steering_tire_angle) / wheelbase; @@ -100,8 +99,8 @@ double calcLatJerk( void test_1d_limit( double ego_v, double V_LIM, double A_LIM, double J_LIM, double LAT_A_LIM, double LAT_J_LIM, - double STEER_DIFF, double STEER_LIM, double STEER_RATE_LIM, - const AckermannControlCommand & prev_cmd, const AckermannControlCommand & raw_cmd) + double STEER_DIFF, double STEER_LIM, double STEER_RATE_LIM, const Control & prev_cmd, + const Control & raw_cmd) { const double WHEELBASE = 3.0; const double DT = 0.1; // [s] @@ -119,11 +118,11 @@ void test_1d_limit( 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); } } @@ -145,14 +144,14 @@ void test_1d_limit( } // 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); } } @@ -241,10 +240,10 @@ TEST(VehicleCmdFilter, VehicleCmdFilter) const std::vector steer_rate_lim_arr = {0.01, 1.0, 100.0}; const std::vector ego_v_arr = {0.0, 0.1, 1.0, 3.0, 15.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) { @@ -294,10 +293,10 @@ TEST(VehicleCmdFilter, VehicleCmdFilterInterpolate) const auto DT = 0.033; const auto orig_cmd = []() { - AckermannControlCommand cmd; + Control cmd; cmd.lateral.steering_tire_angle = 0.5; cmd.lateral.steering_tire_rotation_rate = 0.5; - cmd.longitudinal.speed = 30.0; + cmd.longitudinal.velocity = 30.0; cmd.longitudinal.acceleration = 10.0; cmd.longitudinal.jerk = 10.0; return cmd; @@ -353,7 +352,7 @@ TEST(VehicleCmdFilter, VehicleCmdFilterInterpolate) // vel lim { set_speed_and_reset_prev(0.0); - EXPECT_NEAR(_limitLongitudinalWithVel(orig_cmd).longitudinal.speed, 20.0, ep); + EXPECT_NEAR(_limitLongitudinalWithVel(orig_cmd).longitudinal.velocity, 20.0, ep); } // steer angle lim @@ -389,7 +388,7 @@ TEST(VehicleCmdFilter, VehicleCmdFilterInterpolate) const auto calcSteerRateFromAngle = [&](const auto & cmd) { return (cmd.steering_tire_angle - 0.0) / DT; }; - autoware_auto_control_msgs::msg::AckermannLateralCommand filtered; + autoware_control_msgs::msg::Lateral filtered; set_speed_and_reset_prev(0.0); filtered = _limitSteerRate(orig_cmd).lateral; @@ -521,7 +520,7 @@ TEST(VehicleCmdFilter, VehicleCmdFilterInterpolate) // p.reference_speed_points = std::vector{2.0, 4.0, 10.0}; // p.lat_jerk_lim = std::vector{0.9, 0.7, 0.1}; const auto _calcLatJerk = [&](const auto & cmd, const double ego_v) { - return calcLatJerk(cmd, AckermannControlCommand{}, WHEELBASE, DT, ego_v); + return calcLatJerk(cmd, Control{}, WHEELBASE, DT, ego_v); }; { // since the lateral acceleration and jerk is 0 when the velocity is 0, the target value is 0