Add autoware api (#1979)

Signed-off-by: tanaka3 <ttatcoder@outlook.jp>

vehicle/external_cmd_converter/CMakeLists.txt

@@ -0,0 +1,33 @@
+cmake_minimum_required(VERSION 3.5)
+project(external_cmd_converter)
+
+### Compile options
+if(NOT CMAKE_CXX_STANDARD)
+  set(CMAKE_CXX_STANDARD 14)
+  set(CMAKE_CXX_STANDARD_REQUIRED ON)
+  set(CMAKE_CXX_EXTENSIONS OFF)
+endif()
+if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang")
+  add_compile_options(-Wall -Wextra -Wpedantic -Werror)
+endif()
+
+find_package(ament_cmake_auto REQUIRED)
+ament_auto_find_build_dependencies()
+
+ament_auto_add_library(external_cmd_converter SHARED
+  src/node.cpp
+)
+
+rclcpp_components_register_node(external_cmd_converter
+  PLUGIN "external_cmd_converter::ExternalCmdConverterNode"
+  EXECUTABLE external_cmd_converter_node
+)
+
+if(BUILD_TESTING)
+  find_package(ament_lint_auto REQUIRED)
+  ament_lint_auto_find_test_dependencies()
+endif()
+
+ament_auto_package(INSTALL_TO_SHARE
+  launch
+)

vehicle/external_cmd_converter/include/external_cmd_converter/node.hpp

@@ -0,0 +1,102 @@
+// Copyright 2020 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 EXTERNAL_CMD_CONVERTER__NODE_HPP_
+#define EXTERNAL_CMD_CONVERTER__NODE_HPP_
+
+#include <memory>
+#include <string>
+
+#include "diagnostic_updater/diagnostic_updater.hpp"
+#include "rclcpp/rclcpp.hpp"
+
+#include "autoware_external_api_msgs/msg/control_command_stamped.hpp"
+#include "autoware_control_msgs/msg/control_command_stamped.hpp"
+#include "autoware_control_msgs/msg/emergency_mode.hpp"
+#include "autoware_control_msgs/msg/gate_mode.hpp"
+#include "autoware_vehicle_msgs/msg/shift_stamped.hpp"
+#include "autoware_vehicle_msgs/msg/vehicle_command.hpp"
+#include "geometry_msgs/msg/twist_stamped.hpp"
+
+#include "raw_vehicle_cmd_converter/accel_map.hpp"
+#include "raw_vehicle_cmd_converter/brake_map.hpp"
+
+namespace external_cmd_converter
+{
+
+class ExternalCmdConverterNode : public rclcpp::Node
+{
+public:
+  explicit ExternalCmdConverterNode(const rclcpp::NodeOptions & node_options);
+
+private:
+  // Publisher
+  rclcpp::Publisher<autoware_control_msgs::msg::ControlCommandStamped>::SharedPtr pub_cmd_;
+  rclcpp::Publisher<autoware_external_api_msgs::msg::ControlCommandStamped>::SharedPtr
+    pub_current_cmd_;
+
+  // Subscriber
+  rclcpp::Subscription<geometry_msgs::msg::TwistStamped>::SharedPtr sub_velocity_;
+  rclcpp::Subscription<autoware_external_api_msgs::msg::ControlCommandStamped>::SharedPtr
+    sub_control_cmd_;
+  rclcpp::Subscription<autoware_vehicle_msgs::msg::ShiftStamped>::SharedPtr sub_shift_cmd_;
+  rclcpp::Subscription<autoware_control_msgs::msg::GateMode>::SharedPtr sub_gate_mode_;
+  rclcpp::Subscription<autoware_control_msgs::msg::EmergencyMode>::SharedPtr sub_emergency_stop_;
+
+  void onVelocity(const geometry_msgs::msg::TwistStamped::ConstSharedPtr msg);
+  void onExternalCmd(
+    const autoware_external_api_msgs::msg::ControlCommandStamped::ConstSharedPtr cmd_ptr);
+  void onShiftCmd(const autoware_vehicle_msgs::msg::ShiftStamped::ConstSharedPtr msg);
+  void onGateMode(const autoware_control_msgs::msg::GateMode::ConstSharedPtr msg);
+  void onEmergencyStop(const autoware_control_msgs::msg::EmergencyMode::ConstSharedPtr msg);
+
+  std::shared_ptr<double> current_velocity_ptr_;  // [m/s]
+  std::shared_ptr<rclcpp::Time> latest_emergency_stop_received_time_;
+  std::shared_ptr<rclcpp::Time> latest_cmd_received_time_;
+  autoware_vehicle_msgs::msg::ShiftStamped::ConstSharedPtr current_shift_cmd_;
+  autoware_control_msgs::msg::GateMode::ConstSharedPtr current_gate_mode_;
+
+  bool current_emergency_cmd_ = false;
+
+  // Timer
+  void onTimer();
+  rclcpp::TimerBase::SharedPtr rate_check_timer_;
+
+  // Parameter
+  double ref_vel_gain_;  // reference velocity = current velocity + desired acceleration * gain
+  bool wait_for_first_topic_;
+  double control_command_timeout_;
+  double emergency_stop_timeout_;
+
+  // Diagnostics
+  diagnostic_updater::Updater updater_;
+
+  void checkTopicStatus(diagnostic_updater::DiagnosticStatusWrapper & stat);
+  void checkEmergencyStop(diagnostic_updater::DiagnosticStatusWrapper & stat);
+  bool checkEmergencyStopTopicTimeout();
+  bool checkRemoteTopicRate();
+
+  // Algorithm
+  AccelMap accel_map_;
+  BrakeMap brake_map_;
+  bool acc_map_initialized_;
+
+  double calculateAcc(
+    const autoware_external_api_msgs::msg::ControlCommand & cmd, const double vel);
+  double getShiftVelocitySign(const autoware_vehicle_msgs::msg::ShiftStamped & cmd);
+};
+
+}  // namespace external_cmd_converter
+
+#endif  // EXTERNAL_CMD_CONVERTER__NODE_HPP_

vehicle/external_cmd_converter/launch/external_cmd_converter.launch.xml

@@ -0,0 +1,38 @@
+<?xml version="1.0"?>
+<launch>
+  <arg name="in/external_control_cmd" />
+  <arg name="in/shift_cmd" />
+  <arg name="in/emergency_stop" default="/remote/emergency_stop" />
+  <arg name="in/current_gate_mode" default="/control/current_gate_mode" />
+  <arg name="in/twist" default="/localization/twist" />
+
+  <arg name="out/control_cmd" />
+  <arg name="out/latest_external_control_cmd" default="/external/selected/latest_external_control_cmd" />
+
+  <arg name="csv_path_accel_map" default="$(find-pkg-share raw_vehicle_cmd_converter)/data/default/accel_map.csv"/>
+  <arg name="csv_path_brake_map" default="$(find-pkg-share raw_vehicle_cmd_converter)/data/default/brake_map.csv"/>
+
+  <arg name="ref_vel_gain" default="3.0"/>
+  <arg name="wait_for_first_topic" default="true"/>
+  <arg name="control_command_timeout" default="1.0"/>
+  <arg name="emergency_stop_timeout" default="3.0"/>
+
+  <node pkg="external_cmd_converter" exec="external_cmd_converter_node" name="external_cmd_converter" output="screen">
+    <param name="csv_path_accel_map" value="$(var csv_path_accel_map)" />
+    <param name="csv_path_brake_map" value="$(var csv_path_brake_map)" />
+
+    <param name="ref_vel_gain" value="$(var ref_vel_gain)" />
+    <param name="wait_for_first_topic" value="$(var wait_for_first_topic)" />
+    <param name="control_command_timeout" value="$(var control_command_timeout)" />
+    <param name="emergency_stop_timeout" value="$(var emergency_stop_timeout)" />
+
+    <remap from="~/in/external_control_cmd" to="$(var in/external_control_cmd)" />
+    <remap from="~/in/shift_cmd" to="$(var in/shift_cmd)" />
+    <remap from="~/in/emergency_stop" to="$(var in/emergency_stop)" />
+    <remap from="~/in/current_gate_mode" to="$(var in/current_gate_mode)" />
+    <remap from="~/in/twist" to="$(var in/twist)" />
+
+    <remap from="~/out/control_cmd" to="$(var out/control_cmd)" />
+    <remap from="~/out/latest_external_control_cmd" to="$(var out/latest_external_control_cmd)" />
+  </node>
+</launch>

vehicle/external_cmd_converter/package.xml

@@ -0,0 +1,28 @@
+<?xml version="1.0"?>
+<package format="2">
+  <name>external_cmd_converter</name>
+  <version>0.1.0</version>
+  <description>The external_cmd_converter package</description>
+  <maintainer email="horibe.takamasa@gmail.com">Horibe Takamasa</maintainer>
+  <author email="horibe.takamasa@gmail.com">Horibe Takamasa</author>
+  <license>Apache License 2.0</license>
+
+  <buildtool_depend>ament_cmake_auto</buildtool_depend>
+
+  <depend>rclcpp</depend>
+  <depend>rclcpp_components</depend>
+  <depend>std_msgs</depend>
+  <depend>autoware_vehicle_msgs</depend>
+  <depend>autoware_control_msgs</depend>
+  <depend>autoware_external_api_msgs</depend>
+  <depend>geometry_msgs</depend>
+  <depend>raw_vehicle_cmd_converter</depend>
+  <depend>diagnostic_updater</depend>
+
+  <test_depend>ament_lint_auto</test_depend>
+  <test_depend>ament_lint_common</test_depend>
+
+  <export>
+    <build_type>ament_cmake</build_type>
+  </export>
+</package>