diff --git a/vehicle/raw_vehicle_cmd_converter/README.md b/vehicle/raw_vehicle_cmd_converter/README.md index 6ac1ee4666cae..585e5da6c05f6 100644 --- a/vehicle/raw_vehicle_cmd_converter/README.md +++ b/vehicle/raw_vehicle_cmd_converter/README.md @@ -26,11 +26,11 @@ For ease of calibration and adjustments to the lookup table, an auto-calibration ## Input topics -| Name | Type | Description | -| --------------------- | ------------------------------------- | ------------------------------------------------------------------------------------------------------------------ | -| `~/input/control_cmd` | autoware_control_msgs::msg::Control | target `velocity/acceleration/steering_angle/steering_angle_velocity` is necessary to calculate actuation command. | -| `~/input/steering"` | autoware_vehicle_msgs::SteeringReport | current status of steering used for steering feed back control | -| `~/input/twist` | navigation_msgs::Odometry | twist topic in odometry is used. | +| Name | Type | Description | +| --------------------- | ------------------------------------------ | ------------------------------------------------------------------------------------------------------------------ | +| `~/input/control_cmd` | autoware_control_msgs::msg::Control | target `velocity/acceleration/steering_angle/steering_angle_velocity` is necessary to calculate actuation command. | +| `~/input/steering"` | autoware_vehicle_msgs::msg::SteeringReport | current status of steering used for steering feed back control | +| `~/input/odometry` | navigation_msgs::Odometry | twist topic in odometry is used. | ## Output topics diff --git a/vehicle/raw_vehicle_cmd_converter/include/raw_vehicle_cmd_converter/node.hpp b/vehicle/raw_vehicle_cmd_converter/include/raw_vehicle_cmd_converter/node.hpp index d71ff96abade0..7096bb5c51a9c 100644 --- a/vehicle/raw_vehicle_cmd_converter/include/raw_vehicle_cmd_converter/node.hpp +++ b/vehicle/raw_vehicle_cmd_converter/include/raw_vehicle_cmd_converter/node.hpp @@ -20,6 +20,7 @@ #include "raw_vehicle_cmd_converter/pid.hpp" #include "raw_vehicle_cmd_converter/steer_map.hpp" #include "tier4_autoware_utils/ros/logger_level_configure.hpp" +#include "tier4_autoware_utils/ros/polling_subscriber.hpp" #include @@ -74,12 +75,14 @@ class RawVehicleCommandConverterNode : public rclcpp::Node //!< @brief topic publisher for low level vehicle command rclcpp::Publisher::SharedPtr pub_actuation_cmd_; - //!< @brief subscriber for current velocity - rclcpp::Subscription::SharedPtr sub_velocity_; //!< @brief subscriber for vehicle command rclcpp::Subscription::SharedPtr sub_control_cmd_; - //!< @brief subscriber for steering - rclcpp::Subscription::SharedPtr sub_steering_; + // polling subscribers + tier4_autoware_utils::InterProcessPollingSubscriber sub_odometry_{ + this, "~/input/odometry"}; + + tier4_autoware_utils::InterProcessPollingSubscriber sub_steering_{ + this, "~/input/steering"}; rclcpp::TimerBase::SharedPtr timer_; @@ -109,9 +112,9 @@ class RawVehicleCommandConverterNode : public rclcpp::Node const double current_velocity, const double desired_acc, bool & accel_cmd_is_zero); double calculateBrakeMap(const double current_velocity, const double desired_acc); double calculateSteer(const double vel, const double steering, const double steer_rate); - void onSteering(const Steering::ConstSharedPtr msg); + void processSteering(const Steering::ConstSharedPtr msg); void onControlCmd(const Control::ConstSharedPtr msg); - void onVelocity(const Odometry::ConstSharedPtr msg); + void processOdometry(const Odometry::ConstSharedPtr msg); void publishActuationCmd(); // for debugging rclcpp::Publisher::SharedPtr debug_pub_steer_pid_; diff --git a/vehicle/raw_vehicle_cmd_converter/src/node.cpp b/vehicle/raw_vehicle_cmd_converter/src/node.cpp index 91c668f63dfbd..b274ea6b9806c 100644 --- a/vehicle/raw_vehicle_cmd_converter/src/node.cpp +++ b/vehicle/raw_vehicle_cmd_converter/src/node.cpp @@ -78,10 +78,6 @@ RawVehicleCommandConverterNode::RawVehicleCommandConverterNode( pub_actuation_cmd_ = create_publisher("~/output/actuation_cmd", 1); sub_control_cmd_ = create_subscription( "~/input/control_cmd", 1, std::bind(&RawVehicleCommandConverterNode::onControlCmd, this, _1)); - sub_velocity_ = create_subscription( - "~/input/odometry", 1, std::bind(&RawVehicleCommandConverterNode::onVelocity, this, _1)); - sub_steering_ = create_subscription( - "~/input/steering", 1, std::bind(&RawVehicleCommandConverterNode::onSteering, this, _1)); debug_pub_steer_pid_ = create_publisher( "/vehicle/raw_vehicle_cmd_converter/debug/steer_pid", 1); @@ -204,13 +200,19 @@ double RawVehicleCommandConverterNode::calculateBrakeMap( return desired_brake_cmd; } -void RawVehicleCommandConverterNode::onSteering(const Steering::ConstSharedPtr msg) +void RawVehicleCommandConverterNode::processSteering(const Steering::ConstSharedPtr msg) { + if (!msg) { + return; + } current_steer_ptr_ = std::make_unique(msg->steering_tire_angle); } -void RawVehicleCommandConverterNode::onVelocity(const Odometry::ConstSharedPtr msg) +void RawVehicleCommandConverterNode::processOdometry(const Odometry::ConstSharedPtr msg) { + if (!msg) { + return; + } current_twist_ptr_ = std::make_unique(); current_twist_ptr_->header = msg->header; current_twist_ptr_->twist = msg->twist.twist; @@ -218,6 +220,10 @@ void RawVehicleCommandConverterNode::onVelocity(const Odometry::ConstSharedPtr m void RawVehicleCommandConverterNode::onControlCmd(const Control::ConstSharedPtr msg) { + const auto odometry_msg = sub_odometry_.takeData(); + const auto steering_msg = sub_steering_.takeData(); + processOdometry(odometry_msg); + processSteering(steering_msg); control_cmd_ptr_ = msg; publishActuationCmd(); }