Skip to content

Commit

Permalink
feat: external cmd converter package (autowarefoundation#81)
Browse files Browse the repository at this point in the history 
* Add autoware api (autowarefoundation#1979)

Signed-off-by: tanaka3 <[email protected]>

* Use EmergencyState instead of deprecated EmergencyMode (autowarefoundation#2030)

* Use EmergencyState instead of deprecated EmergencyMode

Signed-off-by: Kenji Miyake <[email protected]>

* Use stamped type

Signed-off-by: Kenji Miyake <[email protected]>
Signed-off-by: tanaka3 <[email protected]>

* 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 <[email protected]>

* Add selected external command API (autowarefoundation#2053)

Signed-off-by: tanaka3 <[email protected]>

* 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 <[email protected]>

* Apply lint

Signed-off-by: wep21 <[email protected]>

* Fix build failure for remote cmd converter

Signed-off-by: wep21 <[email protected]>

* Fix lint

Signed-off-by: wep21 <[email protected]>

* replace to vehicle cmd emergency (autowarefoundation#1710) (autowarefoundation#1717)

* Fix subscriber topic type

Co-authored-by: tkimura4 <[email protected]>

* Fix rclcpp::Time initialization

Co-authored-by: tkimura4 <[email protected]>

Co-authored-by: tkimura4 <[email protected]>
Signed-off-by: tanaka3 <[email protected]>

* 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 <[email protected]>

* Remove ament_cmake_uncrustify and ament_clang_format

Signed-off-by: Kenji Miyake <[email protected]>

* Apply Black

Signed-off-by: Kenji Miyake <[email protected]>

* Apply clang-format

Signed-off-by: Kenji Miyake <[email protected]>

* Fix build errors

Signed-off-by: Kenji Miyake <[email protected]>

* Fix for cpplint

* Fix include double quotes to angle brackets

Signed-off-by: Kenji Miyake <[email protected]>

* Apply clang-format

Signed-off-by: Kenji Miyake <[email protected]>

* Fix build errors

Signed-off-by: Kenji Miyake <[email protected]>
Signed-off-by: tanaka3 <[email protected]>

* Add COLCON_IGNORE (autowarefoundation#500)

Signed-off-by: Kenji Miyake <[email protected]>
Signed-off-by: tanaka3 <[email protected]>

* [external_cmd_converter] apply autoware auto msgs (autowarefoundation#535)

* remove ignore

* apply auto msgs

* modify launch

* topic to shift command

Signed-off-by: tanaka3 <[email protected]>

* move cmd converters to control pkg (autowarefoundation#642)

Signed-off-by: tanaka3 <[email protected]>

* 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 <[email protected]>

* fix shift to gear (autowarefoundation#683)

Signed-off-by: tanaka3 <[email protected]>

* Auto/readme cmd converter (autowarefoundation#692)

* fix format

* add readme external cmd converter

* fix lint

* fiix sentence

Co-authored-by: Kazuki Miyahara <[email protected]>

* fix format

Co-authored-by: Kazuki Miyahara <[email protected]>

* fix sentence

Co-authored-by: Kazuki Miyahara <[email protected]>

* better expression

Co-authored-by: Kazuki Miyahara <[email protected]>

Co-authored-by: taikitanaka3 <[email protected]>
Co-authored-by: Kazuki Miyahara <[email protected]>
Signed-off-by: tanaka3 <[email protected]>

Co-authored-by: Takagi, Isamu <[email protected]>
Co-authored-by: Kenji Miyake <[email protected]>
Co-authored-by: Keisuke Shima <[email protected]>
Co-authored-by: Daisuke Nishimatsu <[email protected]>
Co-authored-by: tkimura4 <[email protected]>
Co-authored-by: taikitanaka3 <[email protected]>
Co-authored-by: Kazuki Miyahara <[email protected]>
Co-authored-by: Takeshi Miura <[email protected]>
  • Loading branch information
@taikitanaka3 @isamu-takagi
@kenji-miyake @KeisukeShima @wep21 @tkimura4 @kmiya @1222-takeshi
9 people authored Dec 3, 2021
1 parent 869e6bc commit 5d8f891
Show file tree
Hide file tree
Showing 7 changed files with 641 additions and 0 deletions.
33 changes: 33 additions & 0 deletions vehicle/external_cmd_converter/CMakeLists.txt
View file
Original file line number Diff line number Diff line change
@@ -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
)
32 changes: 32 additions & 0 deletions vehicle/external_cmd_converter/README.md
View file
Original file line number Diff line number Diff line change
@@ -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.
106 changes: 106 additions & 0 deletions vehicle/external_cmd_converter/include/external_cmd_converter/node.hpp
View file
Original file line number Diff line number Diff line change
@@ -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_
140 changes: 140 additions & 0 deletions vehicle/external_cmd_converter/launch/external_cmd_converter.launch.py
View file
Original file line number Diff line number Diff line change
@@ -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])
41 changes: 41 additions & 0 deletions vehicle/external_cmd_converter/launch/external_cmd_converter.launch.xml
View file
Original file line number Diff line number Diff line change
@@ -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>
31 changes: 31 additions & 0 deletions vehicle/external_cmd_converter/package.xml
View file
Original file line number Diff line number Diff line change
@@ -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="[email protected]">Horibe Takamasa</maintainer>
<author email="[email protected]">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>
Loading

0 comments on commit 5d8f891

Please sign in to comment.