From b134508940d3d148d4dd883c5f0f9717ce79b0fc Mon Sep 17 00:00:00 2001 From: NorahXiong Date: Thu, 7 Dec 2023 13:36:56 +0800 Subject: [PATCH 1/5] feat(pacmod_interface): replace autoware_control_msg with autoware_control_msg Signed-off-by: NorahXiong --- pacmod_interface/README.md | 2 +- .../include/pacmod_interface/pacmod_diag_publisher.hpp | 8 ++++---- .../include/pacmod_interface/pacmod_interface.hpp | 8 ++++---- pacmod_interface/package.xml | 2 +- .../src/pacmod_interface/pacmod_diag_publisher.cpp | 4 ++-- .../src/pacmod_interface/pacmod_interface.cpp | 4 ++-- 6 files changed, 14 insertions(+), 14 deletions(-) diff --git a/pacmod_interface/README.md b/pacmod_interface/README.md index 55c9849..e710a41 100644 --- a/pacmod_interface/README.md +++ b/pacmod_interface/README.md @@ -10,7 +10,7 @@ | Name | Type | Description | | -------------------------------------- | -------------------------------------------------------- | ----------------------------------------------------- | - | `/control/command/control_cmd` | autoware_auto_control_msgs::msg::AckermannControlCommand | lateral and longitudinal control command | + | `/control/command/control_cmd` | autoware_control_msgs::msg::Control | lateral and longitudinal control command | | `/control/command/gear_cmd` | autoware_auto_vehicle_msgs::msg::GearCommand | gear command | | `/control/command/turn_indicators_cmd` | autoware_auto_vehicle_msgs::msg::TurnIndicatorsCommand | turn indicators command | | `/control/command/hazard_lights_cmd` | autoware_auto_vehicle_msgs::msg::HazardLightsCommand | hazard lights command | diff --git a/pacmod_interface/include/pacmod_interface/pacmod_diag_publisher.hpp b/pacmod_interface/include/pacmod_interface/pacmod_diag_publisher.hpp index bc6252a..e667e94 100644 --- a/pacmod_interface/include/pacmod_interface/pacmod_diag_publisher.hpp +++ b/pacmod_interface/include/pacmod_interface/pacmod_diag_publisher.hpp @@ -18,7 +18,7 @@ #include #include -#include +#include #include #include #include @@ -42,7 +42,7 @@ #include #include -using autoware_auto_control_msgs::msg::AckermannControlCommand; +using autoware_control_msgs::msg::Control; using geometry_msgs::msg::AccelWithCovarianceStamped; using nav_msgs::msg::Odometry; @@ -76,7 +76,7 @@ class PacmodDiagPublisher : public rclcpp::Node // Acceleration-related Topics rclcpp::Subscription::SharedPtr current_acc_sub_; - rclcpp::Subscription::SharedPtr control_cmd_sub_; + rclcpp::Subscription::SharedPtr control_cmd_sub_; rclcpp::Subscription::SharedPtr odom_sub_; /* ros parameters */ @@ -122,7 +122,7 @@ class PacmodDiagPublisher : public rclcpp::Node void callbackAccel(const AccelWithCovarianceStamped::ConstSharedPtr accel); void callbackOdometry(const Odometry::SharedPtr odom); - void callbackControlCmd(const AckermannControlCommand::ConstSharedPtr control_cmd); + void callbackControlCmd(const Control::ConstSharedPtr control_cmd); /* functions */ void checkPacmodMsgs(diagnostic_updater::DiagnosticStatusWrapper & stat); diff --git a/pacmod_interface/include/pacmod_interface/pacmod_interface.hpp b/pacmod_interface/include/pacmod_interface/pacmod_interface.hpp index e34da0f..2fd7606 100644 --- a/pacmod_interface/include/pacmod_interface/pacmod_interface.hpp +++ b/pacmod_interface/include/pacmod_interface/pacmod_interface.hpp @@ -19,7 +19,7 @@ #include #include -#include +#include #include #include #include @@ -74,7 +74,7 @@ class PacmodInterface : public rclcpp::Node /* subscribers */ // From Autoware - rclcpp::Subscription::SharedPtr + rclcpp::Subscription::SharedPtr control_cmd_sub_; rclcpp::Subscription::SharedPtr gear_cmd_sub_; rclcpp::Subscription::SharedPtr @@ -169,7 +169,7 @@ class PacmodInterface : public rclcpp::Node /* input values */ ActuationCommandStamped::ConstSharedPtr actuation_cmd_ptr_; - autoware_auto_control_msgs::msg::AckermannControlCommand::ConstSharedPtr control_cmd_ptr_; + autoware_control_msgs::msg::Control::ConstSharedPtr control_cmd_ptr_; autoware_auto_vehicle_msgs::msg::TurnIndicatorsCommand::ConstSharedPtr turn_indicators_cmd_ptr_; autoware_auto_vehicle_msgs::msg::HazardLightsCommand::ConstSharedPtr hazard_lights_cmd_ptr_; autoware_auto_vehicle_msgs::msg::GearCommand::ConstSharedPtr gear_cmd_ptr_; @@ -194,7 +194,7 @@ class PacmodInterface : public rclcpp::Node /* callbacks */ void callbackActuationCmd(const ActuationCommandStamped::ConstSharedPtr msg); void callbackControlCmd( - const autoware_auto_control_msgs::msg::AckermannControlCommand::ConstSharedPtr msg); + const autoware_control_msgs::msg::Control::ConstSharedPtr msg); void callbackEmergencyCmd( const tier4_vehicle_msgs::msg::VehicleEmergencyStamped::ConstSharedPtr msg); diff --git a/pacmod_interface/package.xml b/pacmod_interface/package.xml index 7381050..e7530e8 100644 --- a/pacmod_interface/package.xml +++ b/pacmod_interface/package.xml @@ -13,7 +13,7 @@ ament_cmake_auto - autoware_auto_control_msgs + autoware_control_msgs autoware_auto_vehicle_msgs can_msgs diagnostic_updater diff --git a/pacmod_interface/src/pacmod_interface/pacmod_diag_publisher.cpp b/pacmod_interface/src/pacmod_interface/pacmod_diag_publisher.cpp index ec524d4..e376d84 100644 --- a/pacmod_interface/src/pacmod_interface/pacmod_diag_publisher.cpp +++ b/pacmod_interface/src/pacmod_interface/pacmod_diag_publisher.cpp @@ -80,7 +80,7 @@ PacmodDiagPublisher::PacmodDiagPublisher() current_acc_sub_ = create_subscription( "/localization/acceleration", 1, std::bind(&PacmodDiagPublisher::callbackAccel, this, std::placeholders::_1)); - control_cmd_sub_ = create_subscription( + control_cmd_sub_ = create_subscription( "/control/command/control_cmd", 1, std::bind(&PacmodDiagPublisher::callbackControlCmd, this, std::placeholders::_1)); odom_sub_ = create_subscription( @@ -120,7 +120,7 @@ void PacmodDiagPublisher::callbackAccel(const AccelWithCovarianceStamped::ConstS } void PacmodDiagPublisher::callbackControlCmd( - const AckermannControlCommand::ConstSharedPtr control_cmd) + const Control::ConstSharedPtr control_cmd) { addValueToQue( acc_cmd_que_, control_cmd->longitudinal.acceleration, control_cmd->stamp, accel_store_time_); diff --git a/pacmod_interface/src/pacmod_interface/pacmod_interface.cpp b/pacmod_interface/src/pacmod_interface/pacmod_interface.cpp index 698b80c..288b3bd 100644 --- a/pacmod_interface/src/pacmod_interface/pacmod_interface.cpp +++ b/pacmod_interface/src/pacmod_interface/pacmod_interface.cpp @@ -70,7 +70,7 @@ PacmodInterface::PacmodInterface() using std::placeholders::_2; // From autoware - control_cmd_sub_ = create_subscription( + control_cmd_sub_ = create_subscription( "/control/command/control_cmd", 1, std::bind(&PacmodInterface::callbackControlCmd, this, _1)); gear_cmd_sub_ = create_subscription( "/control/command/gear_cmd", 1, std::bind(&PacmodInterface::callbackGearCmd, this, _1)); @@ -187,7 +187,7 @@ void PacmodInterface::callbackEmergencyCmd( } void PacmodInterface::callbackControlCmd( - const autoware_auto_control_msgs::msg::AckermannControlCommand::ConstSharedPtr msg) + const autoware_control_msgs::msg::Control::ConstSharedPtr msg) { control_command_received_time_ = this->now(); control_cmd_ptr_ = msg; From 475b6b2f8e35c8f1b13d3c4141f95dadb4f23e5e Mon Sep 17 00:00:00 2001 From: liu cui Date: Tue, 19 Dec 2023 16:29:37 +0800 Subject: [PATCH 2/5] feat(pacmod_interface): replace autoware_auto_msgs with autoware_msgs Signed-off-by: liu cui --- pacmod_interface/README.md | 22 +++--- .../pacmod_interface/pacmod_interface.hpp | 66 +++++++++--------- .../pacmod_steer_test/pacmod_steer_test.hpp | 28 ++++---- pacmod_interface/package.xml | 2 +- .../src/pacmod_interface/pacmod_interface.cpp | 68 +++++++++---------- .../pacmod_steer_test/pacmod_steer_test.cpp | 12 ++-- 6 files changed, 99 insertions(+), 99 deletions(-) diff --git a/pacmod_interface/README.md b/pacmod_interface/README.md index e710a41..ec77242 100644 --- a/pacmod_interface/README.md +++ b/pacmod_interface/README.md @@ -11,10 +11,10 @@ | Name | Type | Description | | -------------------------------------- | -------------------------------------------------------- | ----------------------------------------------------- | | `/control/command/control_cmd` | autoware_control_msgs::msg::Control | lateral and longitudinal control command | - | `/control/command/gear_cmd` | autoware_auto_vehicle_msgs::msg::GearCommand | gear command | - | `/control/command/turn_indicators_cmd` | autoware_auto_vehicle_msgs::msg::TurnIndicatorsCommand | turn indicators command | - | `/control/command/hazard_lights_cmd` | autoware_auto_vehicle_msgs::msg::HazardLightsCommand | hazard lights command | - | `/vehicle/engage` | autoware_auto_vehicle_msgs::msg::Engage | engage command | + | `/control/command/gear_cmd` | autoware_vehicle_msgs::msg::GearCommand | gear command | + | `/control/command/turn_indicators_cmd` | autoware_vehicle_msgs::msg::TurnIndicatorsCommand | turn indicators command | + | `/control/command/hazard_lights_cmd` | autoware_vehicle_msgs::msg::HazardLightsCommand | hazard lights command | + | `/vehicle/engage` | autoware_vehicle_msgs::msg::Engage | engage command | | `/vehicle/command/actuation_cmd` | tier4_vehicle_msgs::msg::ActuationCommandStamped | actuation (accel/brake pedal, steering wheel) command | | `/control/command/emergency_cmd` | tier4_vehicle_msgs::msg::VehicleEmergencyStamped | emergency command | @@ -47,13 +47,13 @@ | Name | Type | Description | | ---------------------------------------- | ------------------------------------------------------- | ---------------------------------------------------- | - | `/vehicle/status/control_mode` | autoware_auto_vehicle_msgs::msg::ControlModeReport | control mode | - | `/vehicle/status/velocity_status` | autoware_auto_vehicle_msgs::msg::VelocityReport | velocity | - | `/vehicle/status/steering_status` | autoware_auto_vehicle_msgs::msg::SteeringReport | steering wheel angle | - | `/vehicle/status/gear_status` | autoware_auto_vehicle_msgs::msg::GearReport | gear status | - | `/vehicle/status/turn_indicators_status` | autoware_auto_vehicle_msgs::msg::TurnIndicatorsReport | turn indicators status | - | `/vehicle/status/hazard_lights_status` | autoware_auto_vehicle_msgs::msg::HazardLightsReport | hazard lights status | - | `/vehicle/status/actuation_status` | autoware_auto_vehicle_msgs::msg::ActuationStatusStamped | actuation (accel/brake pedal, steering wheel) status | + | `/vehicle/status/control_mode` | autoware_vehicle_msgs::msg::ControlModeReport | control mode | + | `/vehicle/status/velocity_status` | autoware_vehicle_msgs::msg::VelocityReport | velocity | + | `/vehicle/status/steering_status` | autoware_vehicle_msgs::msg::SteeringReport | steering wheel angle | + | `/vehicle/status/gear_status` | autoware_vehicle_msgs::msg::GearReport | gear status | + | `/vehicle/status/turn_indicators_status` | autoware_vehicle_msgs::msg::TurnIndicatorsReport | turn indicators status | + | `/vehicle/status/hazard_lights_status` | autoware_vehicle_msgs::msg::HazardLightsReport | hazard lights status | + | `/vehicle/status/actuation_status` | autoware_vehicle_msgs::msg::ActuationStatusStamped | actuation (accel/brake pedal, steering wheel) status | ## ROS Parameters diff --git a/pacmod_interface/include/pacmod_interface/pacmod_interface.hpp b/pacmod_interface/include/pacmod_interface/pacmod_interface.hpp index 2fd7606..eea2336 100644 --- a/pacmod_interface/include/pacmod_interface/pacmod_interface.hpp +++ b/pacmod_interface/include/pacmod_interface/pacmod_interface.hpp @@ -20,17 +20,17 @@ #include #include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include #include #include #include @@ -62,7 +62,7 @@ class PacmodInterface : public rclcpp::Node using ActuationCommandStamped = tier4_vehicle_msgs::msg::ActuationCommandStamped; using ActuationStatusStamped = tier4_vehicle_msgs::msg::ActuationStatusStamped; using SteeringWheelStatusStamped = tier4_vehicle_msgs::msg::SteeringWheelStatusStamped; - using ControlModeCommand = autoware_auto_vehicle_msgs::srv::ControlModeCommand; + using ControlModeCommand = autoware_vehicle_msgs::srv::ControlModeCommand; PacmodInterface(); private: @@ -76,10 +76,10 @@ class PacmodInterface : public rclcpp::Node // From Autoware rclcpp::Subscription::SharedPtr control_cmd_sub_; - rclcpp::Subscription::SharedPtr gear_cmd_sub_; - rclcpp::Subscription::SharedPtr + rclcpp::Subscription::SharedPtr gear_cmd_sub_; + rclcpp::Subscription::SharedPtr turn_indicators_cmd_sub_; - rclcpp::Subscription::SharedPtr + rclcpp::Subscription::SharedPtr hazard_lights_cmd_sub_; rclcpp::Subscription::SharedPtr actuation_cmd_sub_; rclcpp::Subscription::SharedPtr emergency_sub_; @@ -110,15 +110,15 @@ class PacmodInterface : public rclcpp::Node raw_steer_cmd_pub_; // only for debug // To Autoware - rclcpp::Publisher::SharedPtr + rclcpp::Publisher::SharedPtr control_mode_pub_; - rclcpp::Publisher::SharedPtr vehicle_twist_pub_; - rclcpp::Publisher::SharedPtr + rclcpp::Publisher::SharedPtr vehicle_twist_pub_; + rclcpp::Publisher::SharedPtr steering_status_pub_; - rclcpp::Publisher::SharedPtr gear_status_pub_; - rclcpp::Publisher::SharedPtr + rclcpp::Publisher::SharedPtr gear_status_pub_; + rclcpp::Publisher::SharedPtr turn_indicators_status_pub_; - rclcpp::Publisher::SharedPtr + rclcpp::Publisher::SharedPtr hazard_lights_status_pub_; rclcpp::Publisher::SharedPtr actuation_status_pub_; rclcpp::Publisher::SharedPtr steering_wheel_status_pub_; @@ -170,9 +170,9 @@ class PacmodInterface : public rclcpp::Node /* input values */ ActuationCommandStamped::ConstSharedPtr actuation_cmd_ptr_; autoware_control_msgs::msg::Control::ConstSharedPtr control_cmd_ptr_; - autoware_auto_vehicle_msgs::msg::TurnIndicatorsCommand::ConstSharedPtr turn_indicators_cmd_ptr_; - autoware_auto_vehicle_msgs::msg::HazardLightsCommand::ConstSharedPtr hazard_lights_cmd_ptr_; - autoware_auto_vehicle_msgs::msg::GearCommand::ConstSharedPtr gear_cmd_ptr_; + autoware_vehicle_msgs::msg::TurnIndicatorsCommand::ConstSharedPtr turn_indicators_cmd_ptr_; + autoware_vehicle_msgs::msg::HazardLightsCommand::ConstSharedPtr hazard_lights_cmd_ptr_; + autoware_vehicle_msgs::msg::GearCommand::ConstSharedPtr gear_cmd_ptr_; pacmod3_msgs::msg::SystemRptFloat::ConstSharedPtr steer_wheel_rpt_ptr_; // [rad] pacmod3_msgs::msg::WheelSpeedRpt::ConstSharedPtr wheel_speed_rpt_ptr_; // [m/s] @@ -199,12 +199,12 @@ class PacmodInterface : public rclcpp::Node void callbackEmergencyCmd( const tier4_vehicle_msgs::msg::VehicleEmergencyStamped::ConstSharedPtr msg); - void callbackGearCmd(const autoware_auto_vehicle_msgs::msg::GearCommand::ConstSharedPtr msg); + void callbackGearCmd(const autoware_vehicle_msgs::msg::GearCommand::ConstSharedPtr msg); void callbackTurnIndicatorsCommand( - const autoware_auto_vehicle_msgs::msg::TurnIndicatorsCommand::ConstSharedPtr msg); + const autoware_vehicle_msgs::msg::TurnIndicatorsCommand::ConstSharedPtr msg); void callbackHazardLightsCommand( - const autoware_auto_vehicle_msgs::msg::HazardLightsCommand::ConstSharedPtr msg); - void callbackEngage(const autoware_auto_vehicle_msgs::msg::Engage::ConstSharedPtr msg); + const autoware_vehicle_msgs::msg::HazardLightsCommand::ConstSharedPtr msg); + void callbackEngage(const autoware_vehicle_msgs::msg::Engage::ConstSharedPtr msg); void callbackRearDoor(const pacmod3_msgs::msg::SystemRptInt::ConstSharedPtr rear_door_rpt); void callbackPacmodRpt( const pacmod3_msgs::msg::SystemRptFloat::ConstSharedPtr steer_wheel_rpt, @@ -222,14 +222,14 @@ class PacmodInterface : public rclcpp::Node const pacmod3_msgs::msg::SystemRptInt & shift_rpt); double calculateVariableGearRatio(const double vel, const double steer_wheel); double calcSteerWheelRateCmd(const double gear_ratio); - uint16_t toPacmodShiftCmd(const autoware_auto_vehicle_msgs::msg::GearCommand & gear_cmd); + uint16_t toPacmodShiftCmd(const autoware_vehicle_msgs::msg::GearCommand & gear_cmd); uint16_t getGearCmdForPreventChatter(uint16_t gear_command); uint16_t toPacmodTurnCmd( - const autoware_auto_vehicle_msgs::msg::TurnIndicatorsCommand & turn, - const autoware_auto_vehicle_msgs::msg::HazardLightsCommand & hazard); + const autoware_vehicle_msgs::msg::TurnIndicatorsCommand & turn, + const autoware_vehicle_msgs::msg::HazardLightsCommand & hazard); uint16_t toPacmodTurnCmdWithHazardRecover( - const autoware_auto_vehicle_msgs::msg::TurnIndicatorsCommand & turn, - const autoware_auto_vehicle_msgs::msg::HazardLightsCommand & hazard); + const autoware_vehicle_msgs::msg::TurnIndicatorsCommand & turn, + const autoware_vehicle_msgs::msg::HazardLightsCommand & hazard); std::optional toAutowareShiftReport(const pacmod3_msgs::msg::SystemRptInt & shift); int32_t toAutowareTurnIndicatorsReport(const pacmod3_msgs::msg::SystemRptInt & turn); diff --git a/pacmod_interface/include/pacmod_steer_test/pacmod_steer_test.hpp b/pacmod_interface/include/pacmod_steer_test/pacmod_steer_test.hpp index 294a094..3c51a52 100644 --- a/pacmod_interface/include/pacmod_steer_test/pacmod_steer_test.hpp +++ b/pacmod_interface/include/pacmod_steer_test/pacmod_steer_test.hpp @@ -18,13 +18,13 @@ #include #include -#include -#include -#include -#include -#include -#include -#include +#include +#include +#include +#include +#include +#include +#include #include #include #include @@ -57,7 +57,7 @@ class PacmodSteerTest : public rclcpp::Node /* subscribers */ // From Autoware - rclcpp::Subscription::SharedPtr engage_cmd_sub_; + rclcpp::Subscription::SharedPtr engage_cmd_sub_; // From Pacmod std::unique_ptr> @@ -80,13 +80,13 @@ class PacmodSteerTest : public rclcpp::Node rclcpp::Publisher::SharedPtr turn_cmd_pub_; // output vehicle info - rclcpp::Publisher::SharedPtr + rclcpp::Publisher::SharedPtr control_mode_pub_; - rclcpp::Publisher::SharedPtr vehicle_twist_pub_; - rclcpp::Publisher::SharedPtr + rclcpp::Publisher::SharedPtr vehicle_twist_pub_; + rclcpp::Publisher::SharedPtr steering_status_pub_; - rclcpp::Publisher::SharedPtr shift_status_pub_; - rclcpp::Publisher::SharedPtr + rclcpp::Publisher::SharedPtr shift_status_pub_; + rclcpp::Publisher::SharedPtr turn_signal_status_pub_; vehicle_info_util::VehicleInfo vehicle_info_; @@ -118,7 +118,7 @@ class PacmodSteerTest : public rclcpp::Node bool engage_cmd_ = false; /* callbacks */ - void callbackEngage(const autoware_auto_vehicle_msgs::msg::Engage::ConstSharedPtr msg); + void callbackEngage(const autoware_vehicle_msgs::msg::Engage::ConstSharedPtr msg); void callbackPacmodRpt( const pacmod3_msgs::msg::SystemRptFloat::ConstSharedPtr steer_wheel_rpt, const pacmod3_msgs::msg::WheelSpeedRpt::ConstSharedPtr wheel_speed_rpt, diff --git a/pacmod_interface/package.xml b/pacmod_interface/package.xml index e7530e8..dddff1e 100644 --- a/pacmod_interface/package.xml +++ b/pacmod_interface/package.xml @@ -14,7 +14,7 @@ ament_cmake_auto autoware_control_msgs - autoware_auto_vehicle_msgs + autoware_vehicle_msgs can_msgs diagnostic_updater geometry_msgs diff --git a/pacmod_interface/src/pacmod_interface/pacmod_interface.cpp b/pacmod_interface/src/pacmod_interface/pacmod_interface.cpp index 288b3bd..3de1496 100644 --- a/pacmod_interface/src/pacmod_interface/pacmod_interface.cpp +++ b/pacmod_interface/src/pacmod_interface/pacmod_interface.cpp @@ -72,14 +72,14 @@ PacmodInterface::PacmodInterface() // From autoware control_cmd_sub_ = create_subscription( "/control/command/control_cmd", 1, std::bind(&PacmodInterface::callbackControlCmd, this, _1)); - gear_cmd_sub_ = create_subscription( + gear_cmd_sub_ = create_subscription( "/control/command/gear_cmd", 1, std::bind(&PacmodInterface::callbackGearCmd, this, _1)); turn_indicators_cmd_sub_ = - create_subscription( + create_subscription( "/control/command/turn_indicators_cmd", rclcpp::QoS{1}, std::bind(&PacmodInterface::callbackTurnIndicatorsCommand, this, _1)); hazard_lights_cmd_sub_ = - create_subscription( + create_subscription( "/control/command/hazard_lights_cmd", rclcpp::QoS{1}, std::bind(&PacmodInterface::callbackHazardLightsCommand, this, _1)); @@ -141,18 +141,18 @@ PacmodInterface::PacmodInterface() "/pacmod/raw_steer_cmd", rclcpp::QoS{1}); // only for debug // To Autoware - control_mode_pub_ = create_publisher( + control_mode_pub_ = create_publisher( "/vehicle/status/control_mode", rclcpp::QoS{1}); - vehicle_twist_pub_ = create_publisher( + vehicle_twist_pub_ = create_publisher( "/vehicle/status/velocity_status", rclcpp::QoS{1}); - steering_status_pub_ = create_publisher( + steering_status_pub_ = create_publisher( "/vehicle/status/steering_status", rclcpp::QoS{1}); - gear_status_pub_ = create_publisher( + gear_status_pub_ = create_publisher( "/vehicle/status/gear_status", rclcpp::QoS{1}); turn_indicators_status_pub_ = - create_publisher( + create_publisher( "/vehicle/status/turn_indicators_status", rclcpp::QoS{1}); - hazard_lights_status_pub_ = create_publisher( + hazard_lights_status_pub_ = create_publisher( "/vehicle/status/hazard_lights_status", rclcpp::QoS{1}); actuation_status_pub_ = create_publisher("/vehicle/status/actuation_status", 1); @@ -194,19 +194,19 @@ void PacmodInterface::callbackControlCmd( } void PacmodInterface::callbackGearCmd( - const autoware_auto_vehicle_msgs::msg::GearCommand::ConstSharedPtr msg) + const autoware_vehicle_msgs::msg::GearCommand::ConstSharedPtr msg) { gear_cmd_ptr_ = msg; } void PacmodInterface::callbackTurnIndicatorsCommand( - const autoware_auto_vehicle_msgs::msg::TurnIndicatorsCommand::ConstSharedPtr msg) + const autoware_vehicle_msgs::msg::TurnIndicatorsCommand::ConstSharedPtr msg) { turn_indicators_cmd_ptr_ = msg; } void PacmodInterface::callbackHazardLightsCommand( - const autoware_auto_vehicle_msgs::msg::HazardLightsCommand::ConstSharedPtr msg) + const autoware_vehicle_msgs::msg::HazardLightsCommand::ConstSharedPtr msg) { hazard_lights_cmd_ptr_ = msg; } @@ -290,13 +290,13 @@ void PacmodInterface::callbackPacmodRpt( /* publish vehicle status control_mode */ { - autoware_auto_vehicle_msgs::msg::ControlModeReport control_mode_msg; + autoware_vehicle_msgs::msg::ControlModeReport control_mode_msg; control_mode_msg.stamp = header.stamp; if (global_rpt->enabled && is_pacmod_enabled_) { - control_mode_msg.mode = autoware_auto_vehicle_msgs::msg::ControlModeReport::AUTONOMOUS; + control_mode_msg.mode = autoware_vehicle_msgs::msg::ControlModeReport::AUTONOMOUS; } else { - control_mode_msg.mode = autoware_auto_vehicle_msgs::msg::ControlModeReport::MANUAL; + control_mode_msg.mode = autoware_vehicle_msgs::msg::ControlModeReport::MANUAL; } control_mode_pub_->publish(control_mode_msg); @@ -304,7 +304,7 @@ void PacmodInterface::callbackPacmodRpt( /* publish vehicle status twist */ { - autoware_auto_vehicle_msgs::msg::VelocityReport twist; + autoware_vehicle_msgs::msg::VelocityReport twist; twist.header = header; twist.longitudinal_velocity = current_velocity; // [m/s] twist.heading_rate = current_velocity * std::tan(current_steer) / wheel_base_; // [rad/s] @@ -313,7 +313,7 @@ void PacmodInterface::callbackPacmodRpt( /* publish current shift */ { - autoware_auto_vehicle_msgs::msg::GearReport gear_report_msg; + autoware_vehicle_msgs::msg::GearReport gear_report_msg; gear_report_msg.stamp = header.stamp; const auto opt_gear_report = toAutowareShiftReport(*gear_cmd_rpt_ptr_); if (opt_gear_report) { @@ -324,7 +324,7 @@ void PacmodInterface::callbackPacmodRpt( /* publish current status */ { - autoware_auto_vehicle_msgs::msg::SteeringReport steer_msg; + autoware_vehicle_msgs::msg::SteeringReport steer_msg; steer_msg.stamp = header.stamp; steer_msg.steering_tire_angle = current_steer; steering_status_pub_->publish(steer_msg); @@ -342,12 +342,12 @@ void PacmodInterface::callbackPacmodRpt( /* publish current turn signal */ { - autoware_auto_vehicle_msgs::msg::TurnIndicatorsReport turn_msg; + autoware_vehicle_msgs::msg::TurnIndicatorsReport turn_msg; turn_msg.stamp = header.stamp; turn_msg.report = toAutowareTurnIndicatorsReport(*turn_rpt); turn_indicators_status_pub_->publish(turn_msg); - autoware_auto_vehicle_msgs::msg::HazardLightsReport hazard_msg; + autoware_vehicle_msgs::msg::HazardLightsReport hazard_msg; hazard_msg.stamp = header.stamp; hazard_msg.report = toAutowareHazardLightsReport(*turn_rpt); hazard_lights_status_pub_->publish(hazard_msg); @@ -574,20 +574,20 @@ double PacmodInterface::calculateVariableGearRatio(const double vel, const doubl } uint16_t PacmodInterface::toPacmodShiftCmd( - const autoware_auto_vehicle_msgs::msg::GearCommand & gear_cmd) + const autoware_vehicle_msgs::msg::GearCommand & gear_cmd) { using pacmod3_msgs::msg::SystemCmdInt; - if (gear_cmd.command == autoware_auto_vehicle_msgs::msg::GearCommand::PARK) { + if (gear_cmd.command == autoware_vehicle_msgs::msg::GearCommand::PARK) { return SystemCmdInt::SHIFT_PARK; } - if (gear_cmd.command == autoware_auto_vehicle_msgs::msg::GearCommand::REVERSE) { + if (gear_cmd.command == autoware_vehicle_msgs::msg::GearCommand::REVERSE) { return SystemCmdInt::SHIFT_REVERSE; } - if (gear_cmd.command == autoware_auto_vehicle_msgs::msg::GearCommand::DRIVE) { + if (gear_cmd.command == autoware_vehicle_msgs::msg::GearCommand::DRIVE) { return SystemCmdInt::SHIFT_FORWARD; } - if (gear_cmd.command == autoware_auto_vehicle_msgs::msg::GearCommand::LOW) { + if (gear_cmd.command == autoware_vehicle_msgs::msg::GearCommand::LOW) { return SystemCmdInt::SHIFT_LOW; } @@ -627,7 +627,7 @@ uint16_t PacmodInterface::getGearCmdForPreventChatter(uint16_t gear_command) std::optional PacmodInterface::toAutowareShiftReport( const pacmod3_msgs::msg::SystemRptInt & shift) { - using autoware_auto_vehicle_msgs::msg::GearReport; + using autoware_vehicle_msgs::msg::GearReport; using pacmod3_msgs::msg::SystemRptInt; if (shift.output == SystemRptInt::SHIFT_PARK) { @@ -646,11 +646,11 @@ std::optional PacmodInterface::toAutowareShiftReport( } uint16_t PacmodInterface::toPacmodTurnCmd( - const autoware_auto_vehicle_msgs::msg::TurnIndicatorsCommand & turn, - const autoware_auto_vehicle_msgs::msg::HazardLightsCommand & hazard) + const autoware_vehicle_msgs::msg::TurnIndicatorsCommand & turn, + const autoware_vehicle_msgs::msg::HazardLightsCommand & hazard) { - using autoware_auto_vehicle_msgs::msg::HazardLightsCommand; - using autoware_auto_vehicle_msgs::msg::TurnIndicatorsCommand; + using autoware_vehicle_msgs::msg::HazardLightsCommand; + using autoware_vehicle_msgs::msg::TurnIndicatorsCommand; using pacmod3_msgs::msg::SystemCmdInt; // NOTE: hazard lights command has a highest priority here. @@ -667,8 +667,8 @@ uint16_t PacmodInterface::toPacmodTurnCmd( } uint16_t PacmodInterface::toPacmodTurnCmdWithHazardRecover( - const autoware_auto_vehicle_msgs::msg::TurnIndicatorsCommand & turn, - const autoware_auto_vehicle_msgs::msg::HazardLightsCommand & hazard) + const autoware_vehicle_msgs::msg::TurnIndicatorsCommand & turn, + const autoware_vehicle_msgs::msg::HazardLightsCommand & hazard) { using pacmod3_msgs::msg::SystemRptInt; @@ -711,7 +711,7 @@ uint16_t PacmodInterface::toPacmodTurnCmdWithHazardRecover( int32_t PacmodInterface::toAutowareTurnIndicatorsReport( const pacmod3_msgs::msg::SystemRptInt & turn) { - using autoware_auto_vehicle_msgs::msg::TurnIndicatorsReport; + using autoware_vehicle_msgs::msg::TurnIndicatorsReport; using pacmod3_msgs::msg::SystemRptInt; if (turn.output == SystemRptInt::TURN_RIGHT) { @@ -727,7 +727,7 @@ int32_t PacmodInterface::toAutowareTurnIndicatorsReport( int32_t PacmodInterface::toAutowareHazardLightsReport( const pacmod3_msgs::msg::SystemRptInt & hazard) { - using autoware_auto_vehicle_msgs::msg::HazardLightsReport; + using autoware_vehicle_msgs::msg::HazardLightsReport; using pacmod3_msgs::msg::SystemRptInt; if (hazard.output == SystemRptInt::TURN_HAZARDS) { diff --git a/pacmod_interface/src/pacmod_steer_test/pacmod_steer_test.cpp b/pacmod_interface/src/pacmod_steer_test/pacmod_steer_test.cpp index 0b35e65..45ec306 100644 --- a/pacmod_interface/src/pacmod_steer_test/pacmod_steer_test.cpp +++ b/pacmod_interface/src/pacmod_steer_test/pacmod_steer_test.cpp @@ -55,7 +55,7 @@ PacmodSteerTest::PacmodSteerTest() using std::placeholders::_1; // Engage - engage_cmd_sub_ = create_subscription( + engage_cmd_sub_ = create_subscription( "/vehicle/engage", rclcpp::QoS{1}, std::bind(&PacmodSteerTest::callbackEngage, this, _1)); // From pacmod steer_wheel_rpt_sub_ = @@ -99,9 +99,9 @@ PacmodSteerTest::PacmodSteerTest() create_publisher("/pacmod/turn_cmd", rclcpp::QoS{1}); // To Autoware - vehicle_twist_pub_ = create_publisher( + vehicle_twist_pub_ = create_publisher( "/vehicle/status/velocity_status", rclcpp::QoS{1}); - steering_status_pub_ = create_publisher( + steering_status_pub_ = create_publisher( "/vehicle/status/steering_status", rclcpp::QoS{1}); // Timer auto timer_callback = std::bind(&PacmodSteerTest::publishCommands, this); @@ -114,7 +114,7 @@ PacmodSteerTest::PacmodSteerTest() } void PacmodSteerTest::callbackEngage( - const autoware_auto_vehicle_msgs::msg::Engage::ConstSharedPtr msg) + const autoware_vehicle_msgs::msg::Engage::ConstSharedPtr msg) { engage_cmd_ = msg->engage; is_clear_override_needed_ = true; @@ -164,7 +164,7 @@ void PacmodSteerTest::callbackPacmodRpt( /* publish vehicle status twist */ { - autoware_auto_vehicle_msgs::msg::VelocityReport velocity_report; + autoware_vehicle_msgs::msg::VelocityReport velocity_report; velocity_report.header = header; velocity_report.longitudinal_velocity = current_velocity; // [m/s] velocity_report.heading_rate = @@ -174,7 +174,7 @@ void PacmodSteerTest::callbackPacmodRpt( /* publish current steering angle */ { - autoware_auto_vehicle_msgs::msg::SteeringReport steer_msg; + autoware_vehicle_msgs::msg::SteeringReport steer_msg; steer_msg.stamp = header.stamp; steer_msg.steering_tire_angle = current_steer; steering_status_pub_->publish(steer_msg); From 0ff60a36147080d0fbdff2bf7367abdafb3cb9f4 Mon Sep 17 00:00:00 2001 From: mitsudome-r Date: Thu, 25 Apr 2024 17:24:52 +0900 Subject: [PATCH 3/5] fix: remove unused header Signed-off-by: mitsudome-r --- pacmod_interface/include/pacmod_steer_test/pacmod_steer_test.hpp | 1 - 1 file changed, 1 deletion(-) diff --git a/pacmod_interface/include/pacmod_steer_test/pacmod_steer_test.hpp b/pacmod_interface/include/pacmod_steer_test/pacmod_steer_test.hpp index 3c51a52..0b9ffcb 100644 --- a/pacmod_interface/include/pacmod_steer_test/pacmod_steer_test.hpp +++ b/pacmod_interface/include/pacmod_steer_test/pacmod_steer_test.hpp @@ -23,7 +23,6 @@ #include #include #include -#include #include #include #include From 67add861010d3e255b439c0a7bed3862d62d6eaf Mon Sep 17 00:00:00 2001 From: "pre-commit-ci[bot]" <66853113+pre-commit-ci[bot]@users.noreply.github.com> Date: Tue, 4 Jun 2024 16:10:13 +0000 Subject: [PATCH 4/5] style(pre-commit): autofix --- pacmod_interface/README.md | 22 +++++++++---------- .../pacmod_interface/pacmod_interface.hpp | 12 ++++------ .../pacmod_steer_test/pacmod_steer_test.hpp | 6 ++--- .../pacmod_diag_publisher.cpp | 3 +-- .../src/pacmod_interface/pacmod_interface.cpp | 22 ++++++++----------- .../pacmod_steer_test/pacmod_steer_test.cpp | 3 +-- 6 files changed, 28 insertions(+), 40 deletions(-) diff --git a/pacmod_interface/README.md b/pacmod_interface/README.md index ec77242..53b894f 100644 --- a/pacmod_interface/README.md +++ b/pacmod_interface/README.md @@ -8,15 +8,15 @@ - From Autoware - | Name | Type | Description | - | -------------------------------------- | -------------------------------------------------------- | ----------------------------------------------------- | - | `/control/command/control_cmd` | autoware_control_msgs::msg::Control | lateral and longitudinal control command | - | `/control/command/gear_cmd` | autoware_vehicle_msgs::msg::GearCommand | gear command | - | `/control/command/turn_indicators_cmd` | autoware_vehicle_msgs::msg::TurnIndicatorsCommand | turn indicators command | - | `/control/command/hazard_lights_cmd` | autoware_vehicle_msgs::msg::HazardLightsCommand | hazard lights command | - | `/vehicle/engage` | autoware_vehicle_msgs::msg::Engage | engage command | - | `/vehicle/command/actuation_cmd` | tier4_vehicle_msgs::msg::ActuationCommandStamped | actuation (accel/brake pedal, steering wheel) command | - | `/control/command/emergency_cmd` | tier4_vehicle_msgs::msg::VehicleEmergencyStamped | emergency command | + | Name | Type | Description | + | -------------------------------------- | ------------------------------------------------- | ----------------------------------------------------- | + | `/control/command/control_cmd` | autoware_control_msgs::msg::Control | lateral and longitudinal control command | + | `/control/command/gear_cmd` | autoware_vehicle_msgs::msg::GearCommand | gear command | + | `/control/command/turn_indicators_cmd` | autoware_vehicle_msgs::msg::TurnIndicatorsCommand | turn indicators command | + | `/control/command/hazard_lights_cmd` | autoware_vehicle_msgs::msg::HazardLightsCommand | hazard lights command | + | `/vehicle/engage` | autoware_vehicle_msgs::msg::Engage | engage command | + | `/vehicle/command/actuation_cmd` | tier4_vehicle_msgs::msg::ActuationCommandStamped | actuation (accel/brake pedal, steering wheel) command | + | `/control/command/emergency_cmd` | tier4_vehicle_msgs::msg::VehicleEmergencyStamped | emergency command | - From Pacmod @@ -45,8 +45,8 @@ - To Autoware - | Name | Type | Description | - | ---------------------------------------- | ------------------------------------------------------- | ---------------------------------------------------- | + | Name | Type | Description | + | ---------------------------------------- | -------------------------------------------------- | ---------------------------------------------------- | | `/vehicle/status/control_mode` | autoware_vehicle_msgs::msg::ControlModeReport | control mode | | `/vehicle/status/velocity_status` | autoware_vehicle_msgs::msg::VelocityReport | velocity | | `/vehicle/status/steering_status` | autoware_vehicle_msgs::msg::SteeringReport | steering wheel angle | diff --git a/pacmod_interface/include/pacmod_interface/pacmod_interface.hpp b/pacmod_interface/include/pacmod_interface/pacmod_interface.hpp index 1a7997e..69699cb 100644 --- a/pacmod_interface/include/pacmod_interface/pacmod_interface.hpp +++ b/pacmod_interface/include/pacmod_interface/pacmod_interface.hpp @@ -64,8 +64,7 @@ class PacmodInterface : public rclcpp::Node private: /* subscribers */ // From Autoware - rclcpp::Subscription::SharedPtr - control_cmd_sub_; + rclcpp::Subscription::SharedPtr control_cmd_sub_; rclcpp::Subscription::SharedPtr gear_cmd_sub_; rclcpp::Subscription::SharedPtr turn_indicators_cmd_sub_; @@ -97,11 +96,9 @@ class PacmodInterface : public rclcpp::Node raw_steer_cmd_pub_; // only for debug // To Autoware - rclcpp::Publisher::SharedPtr - control_mode_pub_; + rclcpp::Publisher::SharedPtr control_mode_pub_; rclcpp::Publisher::SharedPtr vehicle_twist_pub_; - rclcpp::Publisher::SharedPtr - steering_status_pub_; + rclcpp::Publisher::SharedPtr steering_status_pub_; rclcpp::Publisher::SharedPtr gear_status_pub_; rclcpp::Publisher::SharedPtr turn_indicators_status_pub_; @@ -188,8 +185,7 @@ class PacmodInterface : public rclcpp::Node /* callbacks */ void callbackActuationCmd(const ActuationCommandStamped::ConstSharedPtr msg); - void callbackControlCmd( - const autoware_control_msgs::msg::Control::ConstSharedPtr msg); + void callbackControlCmd(const autoware_control_msgs::msg::Control::ConstSharedPtr msg); void callbackEmergencyCmd( const tier4_vehicle_msgs::msg::VehicleEmergencyStamped::ConstSharedPtr msg); diff --git a/pacmod_interface/include/pacmod_steer_test/pacmod_steer_test.hpp b/pacmod_interface/include/pacmod_steer_test/pacmod_steer_test.hpp index 0b9ffcb..1579b03 100644 --- a/pacmod_interface/include/pacmod_steer_test/pacmod_steer_test.hpp +++ b/pacmod_interface/include/pacmod_steer_test/pacmod_steer_test.hpp @@ -79,11 +79,9 @@ class PacmodSteerTest : public rclcpp::Node rclcpp::Publisher::SharedPtr turn_cmd_pub_; // output vehicle info - rclcpp::Publisher::SharedPtr - control_mode_pub_; + rclcpp::Publisher::SharedPtr control_mode_pub_; rclcpp::Publisher::SharedPtr vehicle_twist_pub_; - rclcpp::Publisher::SharedPtr - steering_status_pub_; + rclcpp::Publisher::SharedPtr steering_status_pub_; rclcpp::Publisher::SharedPtr shift_status_pub_; rclcpp::Publisher::SharedPtr turn_signal_status_pub_; diff --git a/pacmod_interface/src/pacmod_interface/pacmod_diag_publisher.cpp b/pacmod_interface/src/pacmod_interface/pacmod_diag_publisher.cpp index e376d84..36446b6 100644 --- a/pacmod_interface/src/pacmod_interface/pacmod_diag_publisher.cpp +++ b/pacmod_interface/src/pacmod_interface/pacmod_diag_publisher.cpp @@ -119,8 +119,7 @@ void PacmodDiagPublisher::callbackAccel(const AccelWithCovarianceStamped::ConstS addValueToQue(acc_que_, accel->accel.accel.linear.x, accel->header.stamp, accel_store_time_); } -void PacmodDiagPublisher::callbackControlCmd( - const Control::ConstSharedPtr control_cmd) +void PacmodDiagPublisher::callbackControlCmd(const Control::ConstSharedPtr control_cmd) { addValueToQue( acc_cmd_que_, control_cmd->longitudinal.acceleration, control_cmd->stamp, accel_store_time_); diff --git a/pacmod_interface/src/pacmod_interface/pacmod_interface.cpp b/pacmod_interface/src/pacmod_interface/pacmod_interface.cpp index 8297687..044f155 100644 --- a/pacmod_interface/src/pacmod_interface/pacmod_interface.cpp +++ b/pacmod_interface/src/pacmod_interface/pacmod_interface.cpp @@ -74,14 +74,12 @@ PacmodInterface::PacmodInterface() "/control/command/control_cmd", 1, std::bind(&PacmodInterface::callbackControlCmd, this, _1)); gear_cmd_sub_ = create_subscription( "/control/command/gear_cmd", 1, std::bind(&PacmodInterface::callbackGearCmd, this, _1)); - turn_indicators_cmd_sub_ = - create_subscription( - "/control/command/turn_indicators_cmd", rclcpp::QoS{1}, - std::bind(&PacmodInterface::callbackTurnIndicatorsCommand, this, _1)); - hazard_lights_cmd_sub_ = - create_subscription( - "/control/command/hazard_lights_cmd", rclcpp::QoS{1}, - std::bind(&PacmodInterface::callbackHazardLightsCommand, this, _1)); + turn_indicators_cmd_sub_ = create_subscription( + "/control/command/turn_indicators_cmd", rclcpp::QoS{1}, + std::bind(&PacmodInterface::callbackTurnIndicatorsCommand, this, _1)); + hazard_lights_cmd_sub_ = create_subscription( + "/control/command/hazard_lights_cmd", rclcpp::QoS{1}, + std::bind(&PacmodInterface::callbackHazardLightsCommand, this, _1)); actuation_cmd_sub_ = create_subscription( "/control/command/actuation_cmd", 1, @@ -137,9 +135,8 @@ PacmodInterface::PacmodInterface() "/vehicle/status/steering_status", rclcpp::QoS{1}); gear_status_pub_ = create_publisher( "/vehicle/status/gear_status", rclcpp::QoS{1}); - turn_indicators_status_pub_ = - create_publisher( - "/vehicle/status/turn_indicators_status", rclcpp::QoS{1}); + turn_indicators_status_pub_ = create_publisher( + "/vehicle/status/turn_indicators_status", rclcpp::QoS{1}); hazard_lights_status_pub_ = create_publisher( "/vehicle/status/hazard_lights_status", rclcpp::QoS{1}); actuation_status_pub_ = @@ -601,8 +598,7 @@ double PacmodInterface::calculateVariableGearRatio(const double vel, const doubl 1e-5, vgr_coef_a_ + vgr_coef_b_ * vel * vel - vgr_coef_c_ * std::fabs(steer_wheel)); } -uint16_t PacmodInterface::toPacmodShiftCmd( - const autoware_vehicle_msgs::msg::GearCommand & gear_cmd) +uint16_t PacmodInterface::toPacmodShiftCmd(const autoware_vehicle_msgs::msg::GearCommand & gear_cmd) { using pacmod3_msgs::msg::SystemCmdInt; diff --git a/pacmod_interface/src/pacmod_steer_test/pacmod_steer_test.cpp b/pacmod_interface/src/pacmod_steer_test/pacmod_steer_test.cpp index 45ec306..145e4db 100644 --- a/pacmod_interface/src/pacmod_steer_test/pacmod_steer_test.cpp +++ b/pacmod_interface/src/pacmod_steer_test/pacmod_steer_test.cpp @@ -113,8 +113,7 @@ PacmodSteerTest::PacmodSteerTest() this->get_node_timers_interface()->add_timer(timer_, nullptr); } -void PacmodSteerTest::callbackEngage( - const autoware_vehicle_msgs::msg::Engage::ConstSharedPtr msg) +void PacmodSteerTest::callbackEngage(const autoware_vehicle_msgs::msg::Engage::ConstSharedPtr msg) { engage_cmd_ = msg->engage; is_clear_override_needed_ = true; From 38be3d3b177c199095744004c1b6e802565c7c03 Mon Sep 17 00:00:00 2001 From: Ryohsuke Mitsudome Date: Wed, 5 Jun 2024 01:12:24 +0900 Subject: [PATCH 5/5] feat: remove autoware_auto_msgs from build_depends.repos Signed-off-by: Ryohsuke Mitsudome --- build_depends.repos | 4 ---- 1 file changed, 4 deletions(-) diff --git a/build_depends.repos b/build_depends.repos index f78a283..3c1388b 100644 --- a/build_depends.repos +++ b/build_depends.repos @@ -11,10 +11,6 @@ repositories: type: git url: https://github.com/autowarefoundation/autoware_common.git version: main - core/external/autoware_auto_msgs: - type: git - url: https://github.com/tier4/autoware_auto_msgs.git # TODO(Tier IV): Move to autowarefoundation/autoware_msgs - version: tier4/main universe/autoware: type: git url: https://github.com/autowarefoundation/autoware.universe