From b901b7c8557cb3bcd471178fb35ce342cd49c122 Mon Sep 17 00:00:00 2001 From: "Takagi, Isamu" <43976882+isamu-takagi@users.noreply.github.com> Date: Fri, 29 Oct 2021 12:10:12 +0900 Subject: [PATCH] Add vehicle status API (#8) * Add vehicle status API * Fix logging * Fix lint --- .../CMakeLists.txt | 2 + .../src/vehicle_status.cpp | 139 ++++++++++++++++++ .../src/vehicle_status.hpp | 55 +++++++ 3 files changed, 196 insertions(+) create mode 100644 autoware_iv_external_api_adaptor/src/vehicle_status.cpp create mode 100644 autoware_iv_external_api_adaptor/src/vehicle_status.hpp diff --git a/autoware_iv_external_api_adaptor/CMakeLists.txt b/autoware_iv_external_api_adaptor/CMakeLists.txt index 9a15e736052..ae0664e345f 100644 --- 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 new file mode 100644 index 00000000000..ab88e3f2150 --- /dev/null +++ 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 +#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().data(External::NONE); + case Internal::LEFT: + return autoware_external_api_msgs::build().data(External::LEFT); + case Internal::RIGHT: + return autoware_external_api_msgs::build().data(External::RIGHT); + case Internal::HAZARD: + return autoware_external_api_msgs::build().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().data(External::NONE); + case Internal::PARKING: + return autoware_external_api_msgs::build().data(External::PARKING); + case Internal::REVERSE: + return autoware_external_api_msgs::build().data(External::REVERSE); + case Internal::NEUTRAL: + return autoware_external_api_msgs::build().data(External::NEUTRAL); + case Internal::DRIVE: + return autoware_external_api_msgs::build().data(External::DRIVE); + case Internal::LOW: + return autoware_external_api_msgs::build().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( + "/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( + "/vehicle/status/twist", rclcpp::QoS(1), + [this](const geometry_msgs::msg::TwistStamped::ConstSharedPtr msg) + { + twist_ = msg; + }); + sub_steering_ = create_subscription( + "/vehicle/status/steering", rclcpp::QoS(1), + [this](const autoware_vehicle_msgs::msg::Steering::ConstSharedPtr msg) + { + steering_ = msg; + }); + sub_turn_signal_ = create_subscription( + "/vehicle/status/turn_signal", rclcpp::QoS(1), + [this](const autoware_vehicle_msgs::msg::TurnSignal::ConstSharedPtr msg) + { + turn_signal_ = msg; + }); + sub_gear_shift_ = create_subscription( + "/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(twist_), "twist"), + std::make_pair(std::static_pointer_cast(steering_), "steering"), + std::make_pair(std::static_pointer_cast(turn_signal_), "turn_signal"), + std::make_pair(std::static_pointer_cast(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 new file mode 100644 index 00000000000..cb19e27a46f --- /dev/null +++ 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::SharedPtr pub_status_; + rclcpp::TimerBase::SharedPtr timer_; + rclcpp::Subscription::SharedPtr sub_twist_; + rclcpp::Subscription::SharedPtr sub_steering_; + rclcpp::Subscription::SharedPtr sub_turn_signal_; + rclcpp::Subscription::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_