Add vehicle status API (autowarefoundation#8)

* Add vehicle status API * Fix logging * Fix lint
BalajiEdutech · Nov 1, 2021 · b901b7c · b901b7c
1 parent dfb57dd
commit b901b7c
Show file tree

Hide file tree

Showing 3 changed files with 196 additions and 0 deletions.
diff --git a/autoware_iv_external_api_adaptor/CMakeLists.txt b/autoware_iv_external_api_adaptor/CMakeLists.txt
@@ -26,6 +26,7 @@ ament_auto_add_library(${PROJECT_NAME} SHARED
   src/route.cpp
   src/service.cpp
   src/start.cpp
+  src/vehicle_status.cpp
   src/velocity.cpp
   src/version.cpp
 )
@@ -40,6 +41,7 @@ rclcpp_components_register_nodes(${PROJECT_NAME} "external_api::MetadataPackages
 rclcpp_components_register_nodes(${PROJECT_NAME} "external_api::Route")
 rclcpp_components_register_nodes(${PROJECT_NAME} "external_api::Service")
 rclcpp_components_register_nodes(${PROJECT_NAME} "external_api::Start")
+rclcpp_components_register_nodes(${PROJECT_NAME} "external_api::VehicleStatus")
 rclcpp_components_register_nodes(${PROJECT_NAME} "external_api::Velocity")
 rclcpp_components_register_nodes(${PROJECT_NAME} "external_api::Version")
 

diff --git a/autoware_iv_external_api_adaptor/src/vehicle_status.cpp b/autoware_iv_external_api_adaptor/src/vehicle_status.cpp
@@ -0,0 +1,139 @@
+// Copyright 2021 Tier IV, Inc.
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+//     http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+// See the License for the specific language governing permissions and
+// limitations under the License.
+
+#include "vehicle_status.hpp"
+#include <utility>
+#include "autoware_external_api_msgs/msg/turn_signal.hpp"
+#include "autoware_external_api_msgs/msg/gear_shift.hpp"
+
+namespace
+{
+
+autoware_external_api_msgs::msg::TurnSignal convert(
+  const autoware_vehicle_msgs::msg::TurnSignal msg)
+{
+  using External = autoware_external_api_msgs::msg::TurnSignal;
+  using Internal = autoware_vehicle_msgs::msg::TurnSignal;
+
+  switch (msg.data) {
+    case Internal::NONE:
+      return autoware_external_api_msgs::build<External>().data(External::NONE);
+    case Internal::LEFT:
+      return autoware_external_api_msgs::build<External>().data(External::LEFT);
+    case Internal::RIGHT:
+      return autoware_external_api_msgs::build<External>().data(External::RIGHT);
+    case Internal::HAZARD:
+      return autoware_external_api_msgs::build<External>().data(External::HAZARD);
+  }
+  throw std::out_of_range("turn_signal=" + std::to_string(msg.data));
+}
+
+autoware_external_api_msgs::msg::GearShift convert(
+  const autoware_vehicle_msgs::msg::Shift msg)
+{
+  using External = autoware_external_api_msgs::msg::GearShift;
+  using Internal = autoware_vehicle_msgs::msg::Shift;
+
+  switch (msg.data) {
+    case Internal::NONE:
+      return autoware_external_api_msgs::build<External>().data(External::NONE);
+    case Internal::PARKING:
+      return autoware_external_api_msgs::build<External>().data(External::PARKING);
+    case Internal::REVERSE:
+      return autoware_external_api_msgs::build<External>().data(External::REVERSE);
+    case Internal::NEUTRAL:
+      return autoware_external_api_msgs::build<External>().data(External::NEUTRAL);
+    case Internal::DRIVE:
+      return autoware_external_api_msgs::build<External>().data(External::DRIVE);
+    case Internal::LOW:
+      return autoware_external_api_msgs::build<External>().data(External::LOW);
+  }
+  throw std::out_of_range("gear_shift=" + std::to_string(msg.data));
+}
+
+}  // namespace
+
+namespace external_api
+{
+
+VehicleStatus::VehicleStatus(const rclcpp::NodeOptions & options)
+: Node("external_api_vehicle_status", options)
+{
+  using namespace std::literals::chrono_literals;
+
+  pub_status_ = create_publisher<autoware_external_api_msgs::msg::VehicleStatusStamped>(
+    "/api/external/get/vehicle/status", rclcpp::QoS(1));
+  timer_ = rclcpp::create_timer(
+    this, get_clock(), 200ms, std::bind(&VehicleStatus::onTimer, this));
+
+  sub_twist_ = create_subscription<geometry_msgs::msg::TwistStamped>(
+    "/vehicle/status/twist", rclcpp::QoS(1),
+    [this](const geometry_msgs::msg::TwistStamped::ConstSharedPtr msg)
+    {
+      twist_ = msg;
+    });
+  sub_steering_ = create_subscription<autoware_vehicle_msgs::msg::Steering>(
+    "/vehicle/status/steering", rclcpp::QoS(1),
+    [this](const autoware_vehicle_msgs::msg::Steering::ConstSharedPtr msg)
+    {
+      steering_ = msg;
+    });
+  sub_turn_signal_ = create_subscription<autoware_vehicle_msgs::msg::TurnSignal>(
+    "/vehicle/status/turn_signal", rclcpp::QoS(1),
+    [this](const autoware_vehicle_msgs::msg::TurnSignal::ConstSharedPtr msg)
+    {
+      turn_signal_ = msg;
+    });
+  sub_gear_shift_ = create_subscription<autoware_vehicle_msgs::msg::ShiftStamped>(
+    "/vehicle/status/shift", rclcpp::QoS(1),
+    [this](const autoware_vehicle_msgs::msg::ShiftStamped::ConstSharedPtr msg)
+    {
+      gear_shift_ = msg;
+    });
+}
+
+void VehicleStatus::onTimer()
+{
+  const auto subscriptions = {
+    std::make_pair(std::static_pointer_cast<const void>(twist_), "twist"),
+    std::make_pair(std::static_pointer_cast<const void>(steering_), "steering"),
+    std::make_pair(std::static_pointer_cast<const void>(turn_signal_), "turn_signal"),
+    std::make_pair(std::static_pointer_cast<const void>(gear_shift_), "gear_shift")
+  };
+
+  for (const auto & [pointer, topic] : subscriptions) {
+    if (!pointer) {
+      RCLCPP_WARN_THROTTLE(
+        get_logger(), *get_clock(), 5000, "The %s topic is not subscribed", topic);
+      return;
+    }
+  }
+
+  try {
+    autoware_external_api_msgs::msg::VehicleStatusStamped msg;
+    msg.stamp = now();
+    msg.status.twist = twist_->twist;
+    msg.status.steering.data = steering_->data;
+    msg.status.turn_signal = convert(*turn_signal_);
+    msg.status.gear_shift = convert(gear_shift_->shift);
+    pub_status_->publish(msg);
+  } catch (const std::out_of_range & exception) {
+    RCLCPP_ERROR(get_logger(), exception.what());
+  }
+}
+
+}  // namespace external_api
+
+#include "rclcpp_components/register_node_macro.hpp"
+RCLCPP_COMPONENTS_REGISTER_NODE(external_api::VehicleStatus)
diff --git a/autoware_iv_external_api_adaptor/src/vehicle_status.hpp b/autoware_iv_external_api_adaptor/src/vehicle_status.hpp
@@ -0,0 +1,55 @@
+// Copyright 2021 Tier IV, Inc.
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+//     http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+// See the License for the specific language governing permissions and
+// limitations under the License.
+
+#ifndef VEHICLE_STATUS_HPP_
+#define VEHICLE_STATUS_HPP_
+
+#include "rclcpp/rclcpp.hpp"
+#include "autoware_api_utils/autoware_api_utils.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"
+
+namespace external_api
+{
+
+class VehicleStatus : public rclcpp::Node
+{
+public:
+  explicit VehicleStatus(const rclcpp::NodeOptions & options);
+
+private:
+  // ros interface
+  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 callback
+  void onTimer();
+
+  // vehicle status
+  geometry_msgs::msg::TwistStamped::ConstSharedPtr twist_;
+  autoware_vehicle_msgs::msg::Steering::ConstSharedPtr steering_;
+  autoware_vehicle_msgs::msg::TurnSignal::ConstSharedPtr turn_signal_;
+  autoware_vehicle_msgs::msg::ShiftStamped::ConstSharedPtr gear_shift_;
+};
+
+}  // namespace external_api
+
+#endif  // VEHICLE_STATUS_HPP_