Add vehicle command API (autowarefoundation#11)

BalajiEdutech · Nov 1, 2021 · 37eba23 · 37eba23
1 parent b901b7c
commit 37eba23
Show file tree

Hide file tree

Showing 2 changed files with 20 additions and 1 deletion.
diff --git a/autoware_iv_external_api_adaptor/src/vehicle_status.cpp b/autoware_iv_external_api_adaptor/src/vehicle_status.cpp
@@ -101,6 +101,19 @@ VehicleStatus::VehicleStatus(const rclcpp::NodeOptions & options)
     {
       gear_shift_ = msg;
     });
+
+  pub_cmd_ = create_publisher<autoware_external_api_msgs::msg::VehicleCommandStamped>(
+    "/api/external/get/command/selected/vehicle", rclcpp::QoS(1));
+  sub_cmd_ = create_subscription<autoware_vehicle_msgs::msg::VehicleCommand>(
+    "/control/vehicle_cmd", rclcpp::QoS(1),
+    [this](const autoware_vehicle_msgs::msg::VehicleCommand::ConstSharedPtr msg)
+    {
+      autoware_external_api_msgs::msg::VehicleCommandStamped cmd;
+      cmd.stamp = msg->header.stamp;
+      cmd.command.velocity = msg->control.velocity;
+      cmd.command.acceleration = msg->control.acceleration;
+      pub_cmd_->publish(cmd);
+    });
 }
 
 void VehicleStatus::onTimer()

diff --git a/autoware_iv_external_api_adaptor/src/vehicle_status.hpp b/autoware_iv_external_api_adaptor/src/vehicle_status.hpp
@@ -17,11 +17,13 @@
 
 #include "rclcpp/rclcpp.hpp"
 #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_vehicle_msgs/msg/steering.hpp"
 #include "autoware_vehicle_msgs/msg/turn_signal.hpp"
 #include "autoware_vehicle_msgs/msg/shift_stamped.hpp"
+#include "autoware_vehicle_msgs/msg/vehicle_command.hpp"
 
 namespace external_api
 {
@@ -32,14 +34,18 @@ class VehicleStatus : public rclcpp::Node
   explicit VehicleStatus(const rclcpp::NodeOptions & options);
 
 private:
-  // ros interface
+  // ros interface for vehicle status
   rclcpp::Publisher<autoware_external_api_msgs::msg::VehicleStatusStamped>::SharedPtr pub_status_;
   rclcpp::TimerBase::SharedPtr timer_;
   rclcpp::Subscription<geometry_msgs::msg::TwistStamped>::SharedPtr sub_twist_;
   rclcpp::Subscription<autoware_vehicle_msgs::msg::Steering>::SharedPtr sub_steering_;
   rclcpp::Subscription<autoware_vehicle_msgs::msg::TurnSignal>::SharedPtr sub_turn_signal_;
   rclcpp::Subscription<autoware_vehicle_msgs::msg::ShiftStamped>::SharedPtr sub_gear_shift_;
 
+  // ros interface for vehicle command
+  rclcpp::Publisher<autoware_external_api_msgs::msg::VehicleCommandStamped>::SharedPtr pub_cmd_;
+  rclcpp::Subscription<autoware_vehicle_msgs::msg::VehicleCommand>::SharedPtr sub_cmd_;
+
   // ros callback
   void onTimer();