From cd2040c92052039d4efb2a57ed51d77890e146dc Mon Sep 17 00:00:00 2001 From: "Takagi, Isamu" <43976882+isamu-takagi@users.noreply.github.com> Date: Wed, 24 Nov 2021 19:27:55 +0900 Subject: [PATCH] Fix vehicle status (#1) --- .../src/vehicle_status.cpp | 24 ++++++++++--------- .../src/vehicle_status.hpp | 6 ++--- 2 files changed, 16 insertions(+), 14 deletions(-) diff --git a/autoware_iv_external_api_adaptor/src/vehicle_status.cpp b/autoware_iv_external_api_adaptor/src/vehicle_status.cpp index 10ee8ad524f..7699e7518d2 100644 --- a/autoware_iv_external_api_adaptor/src/vehicle_status.cpp +++ b/autoware_iv_external_api_adaptor/src/vehicle_status.cpp @@ -80,32 +80,32 @@ VehicleStatus::VehicleStatus(const rclcpp::NodeOptions & options) timer_ = rclcpp::create_timer( this, get_clock(), 200ms, std::bind(&VehicleStatus::onTimer, this)); - sub_twist_ = create_subscription( - "/vehicle/status/twist", rclcpp::QoS(1), - [this](const geometry_msgs::msg::TwistStamped::ConstSharedPtr msg) + sub_velocity_ = create_subscription( + "/vehicle/status/velocity_status", rclcpp::QoS(1), + [this](const autoware_auto_vehicle_msgs::msg::VelocityReport::ConstSharedPtr msg) { - twist_ = msg; + velocity_ = msg; }); sub_steering_ = create_subscription( - "/vehicle/status/steering", rclcpp::QoS(1), + "/vehicle/status/steering_status", rclcpp::QoS(1), [this](const autoware_auto_vehicle_msgs::msg::SteeringReport::ConstSharedPtr msg) { steering_ = msg; }); sub_turn_indicators_ = create_subscription( - "/vehicle/status/turn_indicators", rclcpp::QoS(1), + "/vehicle/status/turn_indicators_status", rclcpp::QoS(1), [this](const autoware_auto_vehicle_msgs::msg::TurnIndicatorsReport::ConstSharedPtr msg) { turn_indicators_ = msg; }); sub_hazard_lights_ = create_subscription( - "/vehicle/status/hazard_lights", rclcpp::QoS(1), + "/vehicle/status/hazard_lights_status", rclcpp::QoS(1), [this](const autoware_auto_vehicle_msgs::msg::HazardLightsReport::ConstSharedPtr msg) { hazard_lights_ = msg; }); sub_gear_shift_ = create_subscription( - "/vehicle/status/gear", rclcpp::QoS(1), + "/vehicle/status/gear_status", rclcpp::QoS(1), [this](const autoware_auto_vehicle_msgs::msg::GearReport::ConstSharedPtr msg) { gear_shift_ = msg; @@ -114,7 +114,7 @@ VehicleStatus::VehicleStatus(const rclcpp::NodeOptions & options) pub_cmd_ = create_publisher( "/api/external/get/command/selected/vehicle", rclcpp::QoS(1)); sub_cmd_ = create_subscription( - "/control/command/ackermann_control_command", rclcpp::QoS(1), + "/control/command/control_cmd", rclcpp::QoS(1), [this](const autoware_auto_control_msgs::msg::AckermannControlCommand::ConstSharedPtr msg) { autoware_external_api_msgs::msg::VehicleCommandStamped cmd; @@ -128,7 +128,7 @@ VehicleStatus::VehicleStatus(const rclcpp::NodeOptions & options) void VehicleStatus::onTimer() { const auto subscriptions = { - std::make_pair(std::static_pointer_cast(twist_), "twist"), + std::make_pair(std::static_pointer_cast(velocity_), "velocity"), std::make_pair(std::static_pointer_cast(steering_), "steering"), std::make_pair(std::static_pointer_cast(turn_indicators_), "turn_indicators"), std::make_pair(std::static_pointer_cast(hazard_lights_), "hazard_lights_"), @@ -147,7 +147,9 @@ void VehicleStatus::onTimer() try { autoware_external_api_msgs::msg::VehicleStatusStamped msg; msg.stamp = now(); - msg.status.twist = twist_->twist; + msg.status.twist.linear.x = velocity_->longitudinal_velocity; + msg.status.twist.linear.y = velocity_->lateral_velocity; + msg.status.twist.angular.z = velocity_->heading_rate; msg.status.steering.data = convert(*steering_).data; msg.status.turn_signal = convert(convert(*turn_indicators_, *hazard_lights_)); msg.status.gear_shift = convert(convert(*gear_shift_).shift); diff --git a/autoware_iv_external_api_adaptor/src/vehicle_status.hpp b/autoware_iv_external_api_adaptor/src/vehicle_status.hpp index 9d8725b82fc..26601ebefe4 100644 --- a/autoware_iv_external_api_adaptor/src/vehicle_status.hpp +++ b/autoware_iv_external_api_adaptor/src/vehicle_status.hpp @@ -19,12 +19,12 @@ #include "autoware_api_utils/autoware_api_utils.hpp" #include "autoware_external_api_msgs/msg/vehicle_command_stamped.hpp" #include "autoware_external_api_msgs/msg/vehicle_status_stamped.hpp" -#include "geometry_msgs/msg/twist_stamped.hpp" #include "autoware_auto_control_msgs/msg/ackermann_control_command.hpp" #include "autoware_auto_vehicle_msgs/msg/hazard_lights_report.hpp" #include "autoware_auto_vehicle_msgs/msg/turn_indicators_report.hpp" #include "autoware_auto_vehicle_msgs/msg/steering_report.hpp" #include "autoware_auto_vehicle_msgs/msg/gear_report.hpp" +#include "autoware_auto_vehicle_msgs/msg/velocity_report.hpp" namespace external_api { @@ -38,7 +38,7 @@ class VehicleStatus : public rclcpp::Node // ros interface for vehicle status rclcpp::Publisher::SharedPtr pub_status_; rclcpp::TimerBase::SharedPtr timer_; - rclcpp::Subscription::SharedPtr sub_twist_; + rclcpp::Subscription::SharedPtr sub_velocity_; rclcpp::Subscription::SharedPtr sub_steering_; rclcpp::Subscription::SharedPtr sub_turn_indicators_; rclcpp::Subscription::SharedPtr sub_hazard_lights_; @@ -52,7 +52,7 @@ class VehicleStatus : public rclcpp::Node void onTimer(); // vehicle status - geometry_msgs::msg::TwistStamped::ConstSharedPtr twist_; + autoware_auto_vehicle_msgs::msg::VelocityReport::ConstSharedPtr velocity_; autoware_auto_vehicle_msgs::msg::SteeringReport::ConstSharedPtr steering_; autoware_auto_vehicle_msgs::msg::TurnIndicatorsReport::ConstSharedPtr turn_indicators_; autoware_auto_vehicle_msgs::msg::HazardLightsReport::ConstSharedPtr hazard_lights_;