feat: external cmd converter package (autowarefoundation#81)

* Add autoware api (autowarefoundation#1979) Signed-off-by: tanaka3 <ttatcoder@outlook.jp> * Use EmergencyState instead of deprecated EmergencyMode (autowarefoundation#2030) * Use EmergencyState instead of deprecated EmergencyMode Signed-off-by: Kenji Miyake <kenji.miyake@tier4.jp> * Use stamped type Signed-off-by: Kenji Miyake <kenji.miyake@tier4.jp> Signed-off-by: tanaka3 <ttatcoder@outlook.jp> * add sort-package-xml hook in pre-commit (autowarefoundation#1881) * add sort xml hook in pre-commit * change retval to exit_status * rename * add prettier plugin-xml * use early return * add license note * add tier4 license * restore prettier * change license order * move local hooks to public repo * move prettier-xml to pre-commit-hooks-ros * update version for bug-fix * apply pre-commit Signed-off-by: tanaka3 <ttatcoder@outlook.jp> * Add selected external command API (autowarefoundation#2053) Signed-off-by: tanaka3 <ttatcoder@outlook.jp> * Feature/vehicle interface improvements (autowarefoundation#1361) (autowarefoundation#1688) * Feature/vehicle interface improvements (autowarefoundation#1361) * add vehicle msg * add pacmod interface * add eps controller * use each control commands instead of vehicle command * fixed details * fixed speell check * const * fixed brake status * publish cmd when recieving ctrl cmd * fix shift cmd ptr * remove unused function and set proper license * fix names * fix typo for pacmod * remove unnecessary waiting * use flags, limit, multiarray * remove accel brake dependency * fix retrun value * replace eps to steer * cosmetic change for namespace * fix segfo and retval * Use Enum instead of int * remove unused var * add const * rename to calcFFMap * prev time steer calc * add sample csv * add new line Signed-off-by: wep21 <border_goldenmarket@yahoo.co.jp> * Apply lint Signed-off-by: wep21 <border_goldenmarket@yahoo.co.jp> * Fix build failure for remote cmd converter Signed-off-by: wep21 <border_goldenmarket@yahoo.co.jp> * Fix lint Signed-off-by: wep21 <border_goldenmarket@yahoo.co.jp> * replace to vehicle cmd emergency (autowarefoundation#1710) (autowarefoundation#1717) * Fix subscriber topic type Co-authored-by: tkimura4 <tomoya.kimura@tier4.jp> * Fix rclcpp::Time initialization Co-authored-by: tkimura4 <tomoya.kimura@tier4.jp> Co-authored-by: tkimura4 <tomoya.kimura@tier4.jp> Signed-off-by: tanaka3 <ttatcoder@outlook.jp> * Change formatter to clang-format and black (autowarefoundation#2332) * Revert "Temporarily comment out pre-commit hooks" This reverts commit 748e9cdb145ce12f8b520bcbd97f5ff899fc28a3. * Replace ament_lint_common with autoware_lint_common Signed-off-by: Kenji Miyake <kenji.miyake@tier4.jp> * Remove ament_cmake_uncrustify and ament_clang_format Signed-off-by: Kenji Miyake <kenji.miyake@tier4.jp> * Apply Black Signed-off-by: Kenji Miyake <kenji.miyake@tier4.jp> * Apply clang-format Signed-off-by: Kenji Miyake <kenji.miyake@tier4.jp> * Fix build errors Signed-off-by: Kenji Miyake <kenji.miyake@tier4.jp> * Fix for cpplint * Fix include double quotes to angle brackets Signed-off-by: Kenji Miyake <kenji.miyake@tier4.jp> * Apply clang-format Signed-off-by: Kenji Miyake <kenji.miyake@tier4.jp> * Fix build errors Signed-off-by: Kenji Miyake <kenji.miyake@tier4.jp> Signed-off-by: tanaka3 <ttatcoder@outlook.jp> * Add COLCON_IGNORE (autowarefoundation#500) Signed-off-by: Kenji Miyake <kenji.miyake@tier4.jp> Signed-off-by: tanaka3 <ttatcoder@outlook.jp> * [external_cmd_converter] apply autoware auto msgs (autowarefoundation#535) * remove ignore * apply auto msgs * modify launch * topic to shift command Signed-off-by: tanaka3 <ttatcoder@outlook.jp> * move cmd converters to control pkg (autowarefoundation#642) Signed-off-by: tanaka3 <ttatcoder@outlook.jp> * auto/revert cmd converter (autowarefoundation#680) * Revert "move cmd converters to control pkg (autowarefoundation#642)" This reverts commit aed827c4fdfa01cc7bc29e70307d5e2cbc8aa8d3. * fix topic Signed-off-by: tanaka3 <ttatcoder@outlook.jp> * fix shift to gear (autowarefoundation#683) Signed-off-by: tanaka3 <ttatcoder@outlook.jp> * Auto/readme cmd converter (autowarefoundation#692) * fix format * add readme external cmd converter * fix lint * fiix sentence Co-authored-by: Kazuki Miyahara <kmiya@outlook.com> * fix format Co-authored-by: Kazuki Miyahara <kmiya@outlook.com> * fix sentence Co-authored-by: Kazuki Miyahara <kmiya@outlook.com> * better expression Co-authored-by: Kazuki Miyahara <kmiya@outlook.com> Co-authored-by: taikitanaka3 <taiki.tanaka@tier4.jp> Co-authored-by: Kazuki Miyahara <kmiya@outlook.com> Signed-off-by: tanaka3 <ttatcoder@outlook.jp> Co-authored-by: Takagi, Isamu <43976882+isamu-takagi@users.noreply.github.com> Co-authored-by: Kenji Miyake <31987104+kenji-miyake@users.noreply.github.com> Co-authored-by: Keisuke Shima <19993104+KeisukeShima@users.noreply.github.com> Co-authored-by: Daisuke Nishimatsu <42202095+wep21@users.noreply.github.com> Co-authored-by: tkimura4 <tomoya.kimura@tier4.jp> Co-authored-by: taikitanaka3 <taiki.tanaka@tier4.jp> Co-authored-by: Kazuki Miyahara <kmiya@outlook.com> Co-authored-by: Takeshi Miura <57553950+1222-takeshi@users.noreply.github.com>
satoshi-ota · Dec 3, 2021 · 5d8f891 · 5d8f891
1 parent 869e6bc
commit 5d8f891
Show file tree

Hide file tree

Showing 7 changed files with 641 additions and 0 deletions.
diff --git a/vehicle/external_cmd_converter/CMakeLists.txt b/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
+)
diff --git a/vehicle/external_cmd_converter/README.md b/vehicle/external_cmd_converter/README.md
@@ -0,0 +1,32 @@
+# external_cmd_converter
+
+`external_cmd_converter` is a node that converts desired mechanical input to acceleration and velocity by using accel/brake map.
+
+## Input topics
+
+| Name                        | Type                                            | Description                                                                                                       |
+| --------------------------- | ----------------------------------------------- | ----------------------------------------------------------------------------------------------------------------- |
+| `~/in/external_control_cmd` | autoware_external_api_msgs::msg::ControlCommand | target `throttle/brake/steering_angle/steering_angle_velocity` is necessary to calculate desired control command. |
+| `~/input/shift_cmd"`        | autoware_auto_vehicle_msgs::GearCommand         | current command of gear.                                                                                          |
+| `~/input/emergency_stop`    | autoware_external_api_msgs::msg::Heartbeat      | emergency heart beat for external command.                                                                        |
+| `~/input/current_gate_mode` | autoware_control_msgs::msg::GateMode            | topic for gate mode.                                                                                              |
+| `~/input/odometry`          | navigation_msgs::Odometry                       | twist topic in odometry is used.                                                                                  |
+
+## Output topics
+
+| Name                | Type                                                | Description                                                        |
+| ------------------- | --------------------------------------------------- | ------------------------------------------------------------------ |
+| `~/out/control_cmd` | autoware_control_msgs::msg::AckermannControlCommand | ackermann control command converted from selected external command |
+
+## Parameters
+
+| Parameter                 | Type   | Description                                           |
+| ------------------------- | ------ | ----------------------------------------------------- |
+| `timer_rate`              | double | timer's update rate                                   |
+| `wait_for_first_topic`    | double | if time out check is done after receiving first topic |
+| `control_command_timeout` | double | time out check for control command                    |
+| `emergency_stop_timeout`  | double | time out check for emergency stop command             |
+
+## Limitation
+
+tbd.
diff --git a/vehicle/external_cmd_converter/include/external_cmd_converter/node.hpp b/vehicle/external_cmd_converter/include/external_cmd_converter/node.hpp
@@ -0,0 +1,106 @@
+// 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 <diagnostic_updater/diagnostic_updater.hpp>
+#include <raw_vehicle_cmd_converter/accel_map.hpp>
+#include <raw_vehicle_cmd_converter/brake_map.hpp>
+#include <rclcpp/rclcpp.hpp>
+
+#include <autoware_auto_control_msgs/msg/ackermann_control_command.hpp>
+#include <autoware_auto_vehicle_msgs/msg/gear_command.hpp>
+#include <autoware_control_msgs/msg/gate_mode.hpp>
+#include <autoware_external_api_msgs/msg/control_command_stamped.hpp>
+#include <autoware_external_api_msgs/msg/heartbeat.hpp>
+#include <nav_msgs/msg/odometry.hpp>
+
+#include <memory>
+#include <string>
+
+namespace external_cmd_converter
+{
+using GearCommand = autoware_auto_vehicle_msgs::msg::GearCommand;
+using AckermannControlCommand = autoware_auto_control_msgs::msg::AckermannControlCommand;
+using ExternalControlCommand = autoware_external_api_msgs::msg::ControlCommandStamped;
+using Odometry = nav_msgs::msg::Odometry;
+using raw_vehicle_cmd_converter::AccelMap;
+using raw_vehicle_cmd_converter::BrakeMap;
+using ControlCommandStamped = autoware_auto_control_msgs::msg::AckermannControlCommand;
+using Odometry = nav_msgs::msg::Odometry;
+
+class ExternalCmdConverterNode : public rclcpp::Node
+{
+public:
+  explicit ExternalCmdConverterNode(const rclcpp::NodeOptions & node_options);
+
+private:
+  // Publisher
+  rclcpp::Publisher<AckermannControlCommand>::SharedPtr pub_cmd_;
+  rclcpp::Publisher<autoware_external_api_msgs::msg::ControlCommandStamped>::SharedPtr
+    pub_current_cmd_;
+
+  // Subscriber
+  rclcpp::Subscription<Odometry>::SharedPtr sub_velocity_;
+  rclcpp::Subscription<autoware_external_api_msgs::msg::ControlCommandStamped>::SharedPtr
+    sub_control_cmd_;
+  rclcpp::Subscription<GearCommand>::SharedPtr sub_shift_cmd_;
+  rclcpp::Subscription<autoware_control_msgs::msg::GateMode>::SharedPtr sub_gate_mode_;
+  rclcpp::Subscription<autoware_external_api_msgs::msg::Heartbeat>::SharedPtr
+    sub_emergency_stop_heartbeat_;
+
+  void onVelocity(const Odometry::ConstSharedPtr msg);
+  void onExternalCmd(const ExternalControlCommand::ConstSharedPtr cmd_ptr);
+  void onGearCommand(const GearCommand::ConstSharedPtr msg);
+  void onGateMode(const autoware_control_msgs::msg::GateMode::ConstSharedPtr msg);
+  void onEmergencyStopHeartbeat(
+    const autoware_external_api_msgs::msg::Heartbeat::ConstSharedPtr msg);
+
+  std::shared_ptr<double> current_velocity_ptr_;  // [m/s]
+  std::shared_ptr<rclcpp::Time> latest_emergency_stop_heartbeat_received_time_;
+  std::shared_ptr<rclcpp::Time> latest_cmd_received_time_;
+  GearCommand::ConstSharedPtr current_shift_cmd_;
+  autoware_control_msgs::msg::GateMode::ConstSharedPtr current_gate_mode_;
+
+  // 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_{this};
+
+  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 ExternalControlCommand & cmd, const double vel);
+  double getShiftVelocitySign(const GearCommand & cmd);
+};
+
+}  // namespace external_cmd_converter
+
+#endif  // EXTERNAL_CMD_CONVERTER__NODE_HPP_
diff --git a/vehicle/external_cmd_converter/launch/external_cmd_converter.launch.py b/vehicle/external_cmd_converter/launch/external_cmd_converter.launch.py
@@ -0,0 +1,140 @@
+# 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.
+
+from launch import LaunchDescription
+from launch.actions import DeclareLaunchArgument
+from launch.substitutions import LaunchConfiguration
+from launch_ros.actions import LoadComposableNodes
+from launch_ros.descriptions import ComposableNode
+from launch_ros.substitutions import FindPackageShare
+
+
+def _create_mapping_tuple(name):
+    return (name, LaunchConfiguration(name))
+
+
+def generate_launch_description():
+
+    arguments = [
+        # component
+        DeclareLaunchArgument("use_intra_process"),
+        DeclareLaunchArgument("target_container"),
+        # map file
+        DeclareLaunchArgument(
+            "csv_path_accel_map",
+            default_value=[
+                FindPackageShare("raw_vehicle_cmd_converter"),
+                "/data/default/accel_map.csv",
+            ],  # noqa: E501
+            description="csv file path for accel map",
+        ),
+        DeclareLaunchArgument(
+            "csv_path_brake_map",
+            default_value=[
+                FindPackageShare("raw_vehicle_cmd_converter"),
+                "/data/default/brake_map.csv",
+            ],  # noqa: E501
+            description="csv file path for brake map",
+        ),
+        # settings
+        DeclareLaunchArgument(
+            "ref_vel_gain",
+            default_value="3.0",
+            description="gain for external command accel",
+        ),
+        DeclareLaunchArgument(
+            "wait_for_first_topic",
+            default_value="true",
+            description="disable topic disruption detection until subscribing first topics",
+        ),
+        DeclareLaunchArgument(
+            "control_command_timeout",
+            default_value="1.0",
+            description="external control command timeout",
+        ),
+        DeclareLaunchArgument(
+            "emergency_stop_timeout",
+            default_value="3.0",
+            description="emergency stop timeout for external heartbeat",
+        ),
+        # input
+        DeclareLaunchArgument(
+            "in/external_control_cmd",
+            default_value="/external/selected/external_control_cmd",
+        ),
+        DeclareLaunchArgument(
+            "in/shift_cmd",
+            default_value="/external/selected/gear_cmd",
+        ),
+        DeclareLaunchArgument(
+            "in/emergency_stop",
+            default_value="/external/selected/heartbeat",
+        ),
+        DeclareLaunchArgument(
+            "in/current_gate_mode",
+            default_value="/control/current_gate_mode",
+        ),
+        DeclareLaunchArgument(
+            "in/odometry",
+            default_value="/localization/kinematic_state",
+        ),
+        # output
+        DeclareLaunchArgument(
+            "out/control_cmd",
+            default_value="/external/selected/control_cmd",
+        ),
+        DeclareLaunchArgument(
+            "out/latest_external_control_cmd",
+            default_value="/api/external/get/command/selected/control",
+        ),
+    ]
+
+    component = ComposableNode(
+        package="external_cmd_converter",
+        plugin="external_cmd_converter::ExternalCmdConverterNode",
+        name="external_cmd_converter",
+        remappings=[
+            _create_mapping_tuple("in/external_control_cmd"),
+            _create_mapping_tuple("in/shift_cmd"),
+            _create_mapping_tuple("in/emergency_stop"),
+            _create_mapping_tuple("in/current_gate_mode"),
+            _create_mapping_tuple("in/odometry"),
+            _create_mapping_tuple("out/control_cmd"),
+            _create_mapping_tuple("out/latest_external_control_cmd"),
+        ],
+        parameters=[
+            dict(  # noqa: C406 for using helper function
+                [
+                    _create_mapping_tuple("csv_path_accel_map"),
+                    _create_mapping_tuple("csv_path_brake_map"),
+                    _create_mapping_tuple("ref_vel_gain"),
+                    _create_mapping_tuple("wait_for_first_topic"),
+                    _create_mapping_tuple("control_command_timeout"),
+                    _create_mapping_tuple("emergency_stop_timeout"),
+                ]
+            )
+        ],
+        extra_arguments=[
+            {
+                "use_intra_process_comms": LaunchConfiguration("use_intra_process"),
+            }
+        ],
+    )
+
+    loader = LoadComposableNodes(
+        composable_node_descriptions=[component],
+        target_container=LaunchConfiguration("target_container"),
+    )
+
+    return LaunchDescription(arguments + [loader])
diff --git a/vehicle/external_cmd_converter/launch/external_cmd_converter.launch.xml b/vehicle/external_cmd_converter/launch/external_cmd_converter.launch.xml
@@ -0,0 +1,41 @@
+<?xml version="1.0"?>
+<launch>
+
+  <!-- map file -->
+  <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"/>
+
+  <!-- settings -->
+  <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"/>
+
+  <!-- input -->
+  <arg name="in/external_control_cmd" default="/external/selected/external_control_cmd" />
+  <arg name="in/shift_cmd" default="/external/selected/gear_cmd" />
+  <arg name="in/emergency_stop" default="/external/selected/heartbeat" />
+  <arg name="in/current_gate_mode" default="/control/current_gate_mode" />
+  <arg name="in/odometry" default="/localization/kinematic_state" />
+
+  <!-- output -->
+  <arg name="out/control_cmd" default="/external/selected/control_cmd" />
+  <arg name="out/latest_external_control_cmd" default="/api/external/get/command/selected/control" />
+
+  <!-- node -->
+  <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/odometry" to="$(var in/odometry)" />
+    <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>
diff --git a/vehicle/external_cmd_converter/package.xml b/vehicle/external_cmd_converter/package.xml
@@ -0,0 +1,31 @@
+<?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>autoware_auto_control_msgs</depend>
+  <depend>autoware_auto_vehicle_msgs</depend>
+  <depend>autoware_control_msgs</depend>
+  <depend>autoware_external_api_msgs</depend>
+  <depend>autoware_system_msgs</depend>
+  <depend>diagnostic_updater</depend>
+  <depend>geometry_msgs</depend>
+  <depend>nav_msgs</depend>
+  <depend>raw_vehicle_cmd_converter</depend>
+  <depend>rclcpp</depend>
+  <depend>rclcpp_components</depend>
+  <depend>std_msgs</depend>
+
+  <test_depend>ament_lint_auto</test_depend>
+  <test_depend>autoware_lint_common</test_depend>
+
+  <export>
+    <build_type>ament_cmake</build_type>
+  </export>
+</package>