Skip to content

Commit

Permalink
feat: add autoware joy controller (autowarefoundation#72)
Browse files Browse the repository at this point in the history 
* release v0.4.0

* Support G29 controller in autoware_joy_controller (autowarefoundation#699)

* Add map for G29 controller

* Add new line at end of file

* Change structure of JoyConverterBase class

* Rename PS4 -> DS4

* Rename controler_type -> joy_type

* Set joy_type by console input

* Change doc

* Remap g29 controller

* Remap AccelPedal -> accel, BrakePedal -> brake

* Remove [autoware_joy_controller] from ROS_INFO

Co-authored-by: Fumiya Watanabe <fumiya.watanabe@tier4.jp>

* Change key map for G29 controller and set deadzone parameter (autowarefoundation#740)

* Add missing dependencies of autoware_joy_controller (autowarefoundation#755)

Signed-off-by: Kenji Miyake <kenji.miyake@tier4.jp>

* remove ROS1 packages temporarily

Signed-off-by: mitsudome-r <ryohsuke.mitsudome@tier4.jp>

* add sample ros2 packages

Signed-off-by: mitsudome-r <ryohsuke.mitsudome@tier4.jp>

* remove ROS1 packages

Signed-off-by: mitsudome-r <ryohsuke.mitsudome@tier4.jp>

* Revert "remove ROS1 packages temporarily"

This reverts commit c98294b0b159fb98cd3091d34a626d06f29fdece.

Signed-off-by: mitsudome-r <ryohsuke.mitsudome@tier4.jp>

* add COLCON_IGNORE to ros1 packages

Signed-off-by: mitsudome-r <ryohsuke.mitsudome@tier4.jp>

* Rename launch files to launch.xml (autowarefoundation#28)

* Rename h files to hpp (autowarefoundation#142)

* Change includes

* Rename files

* Adjustments to make things compile

* Other packages

* Adjust copyright notice on 532 out of 699 source files (autowarefoundation#143)

* Use quotes for includes where appropriate (autowarefoundation#144)

* Use quotes for includes where appropriate

* Fix lint tests

* Make tests pass hopefully

* Port autoware joy controller (autowarefoundation#124)

* Port

Signed-off-by: Servando German Serrano <servando.german.serrano@linaro.org>

* Fixed package.xml

* now() to use node clock

Signed-off-by: Servando German Serrano <servando.german.serrano@linaro.org>

* Fix include

Signed-off-by: Servando German Serrano <servando.german.serrano@linaro.org>

* Clear compilation warnings

Signed-off-by: Servando German Serrano <servando.german.serrano@linaro.org>

* Run uncrustify on the entire Pilot.Auto codebase (autowarefoundation#151)

* Run uncrustify on the entire Pilot.Auto codebase

* Exclude open PRs

* [update to v0.8.0] autoware joy controller (autowarefoundation#251)

* restore filename to original for version update

Signed-off-by: Takamasa Horibe <horibe.takamasa@gmail.com>

* Enable to change sensitivity (autowarefoundation#868)

* Improve remote emergency stop (autowarefoundation#900)

* Apply format

Signed-off-by: Kenji Miyake <kenji.miyake@tier4.jp>

* Rename emergency to system_emergency in vehicle_cmd_gate

Signed-off-by: Kenji Miyake <kenji.miyake@tier4.jp>

* Add emergency stop feature to vehicle_cmd_gate

Signed-off-by: Kenji Miyake <kenji.miyake@tier4.jp>

* Fix frame_id of vehicle_cmd_gate output

Signed-off-by: Kenji Miyake <kenji.miyake@tier4.jp>

* Rename /remote/emergency to /remote/emergency_stop in autoware_joy_controller

Signed-off-by: Kenji Miyake <kenji.miyake@tier4.jp>

* Rename /remote/emergency to /remote/emergency_stop in remote_cmd_converter

Signed-off-by: Kenji Miyake <kenji.miyake@tier4.jp>

* Rename /remote/emergency to /remote/emergency_stop in autoware_api

Signed-off-by: Kenji Miyake <kenji.miyake@tier4.jp>

* Check emergency_stop timeout in remote_cmd_converter

Signed-off-by: Kenji Miyake <kenji.miyake@tier4.jp>

* Ignore timeout = 0.0

Signed-off-by: Kenji Miyake <kenji.miyake@tier4.jp>

* Add config_file to arg

Signed-off-by: Kenji Miyake <kenji.miyake@tier4.jp>

* Rename emergency_stop to external_emergency_stop

Signed-off-by: Kenji Miyake <kenji.miyake@tier4.jp>

* Remove unnecessary lines

Signed-off-by: Kenji Miyake <kenji.miyake@tier4.jp>

* Wait for first heartbeat

Signed-off-by: Kenji Miyake <kenji.miyake@tier4.jp>

* Add clear_emergency_stop service

Signed-off-by: Kenji Miyake <kenji.miyake@tier4.jp>

* Call clear_external_emegency_stop service from autoware_joy_controller

Signed-off-by: Kenji Miyake <kenji.miyake@tier4.jp>

* Rename function

Signed-off-by: Kenji Miyake <kenji.miyake@tier4.jp>

* Revert: Wait for first heartbeat

Signed-off-by: Kenji Miyake <kenji.miyake@tier4.jp>

* Fix console messages

Signed-off-by: Kenji Miyake <kenji.miyake@tier4.jp>

* Move emergency_stop diag to vehicle_cmd_gate

Signed-off-by: Kenji Miyake <kenji.miyake@tier4.jp>

* Add heartbeat to vehicle_cmd_gate

Signed-off-by: Kenji Miyake <kenji.miyake@tier4.jp>

* Revert: Move emergency_stop diag to vehicle_cmd_gate

Signed-off-by: Kenji Miyake <kenji.miyake@tier4.jp>

* patch in real-vehicle

* Apply format

Signed-off-by: Kenji Miyake <kenji.miyake@tier4.jp>

* Change default parameter

Signed-off-by: Kenji Miyake <kenji.miyake@tier4.jp>

Co-authored-by: jpntaxi4943-autoware <proj-jpntaxi@tier4.jp>

* restore file name

Signed-off-by: Takamasa Horibe <horibe.takamasa@gmail.com>

* [tmp] fix build error

Signed-off-by: Takamasa Horibe <horibe.takamasa@gmail.com>

* fix service

Signed-off-by: Takamasa Horibe <horibe.takamasa@gmail.com>

* fix format

Signed-off-by: Takamasa Horibe <horibe.takamasa@gmail.com>

* fix service usage

Signed-off-by: Takamasa Horibe <horibe.takamasa@gmail.com>

* fix launch var

Co-authored-by: Fumiya Watanabe <rej55.g@gmail.com>
Co-authored-by: Kenji Miyake <31987104+kenji-miyake@users.noreply.github.com>
Co-authored-by: jpntaxi4943-autoware <proj-jpntaxi@tier4.jp>

* Rename ROS-related .yaml to .param.yaml (autowarefoundation#352)

* Rename ROS-related .yaml to .param.yaml

Signed-off-by: Kenji Miyake <kenji.miyake@tier4.jp>

* Remove prefix 'default_' of yaml files

Signed-off-by: Kenji Miyake <kenji.miyake@tier4.jp>

* Rename vehicle_info.yaml to vehicle_info.param.yaml

Signed-off-by: Kenji Miyake <kenji.miyake@tier4.jp>

* Rename diagnostic_aggregator's param files

Signed-off-by: Kenji Miyake <kenji.miyake@tier4.jp>

* Fix overlooked parameters

Signed-off-by: Kenji Miyake <kenji.miyake@tier4.jp>

* remove using in global namespace (autowarefoundation#379)

* remove using in global namespace (autowarefoundation#1166)

* remove using in global namespace

* Revert "remove using in global namespace"

This reverts commit 7f120509c9e3a036a38e84883868f6036bca23ad.

* Add package namespace

Signed-off-by: Kenji Miyake <kenji.miyake@tier4.jp>

Co-authored-by: Kenji Miyake <kenji.miyake@tier4.jp>

* [autoware_joy_controller] add lint tests

Signed-off-by: mitsudome-r <ryohsuke.mitsudome@tier4.jp>

Co-authored-by: Kenji Miyake <kenji.miyake@tier4.jp>

* fix namespace (autowarefoundation#414)

* add use_sim-time option (autowarefoundation#454)

* Fix for rolling (autowarefoundation#1226)

* Replace doc by description

Signed-off-by: Kenji Miyake <kenji.miyake@tier4.jp>

* Replace ns by push-ros-namespace

Signed-off-by: Kenji Miyake <kenji.miyake@tier4.jp>

* Make control modules components (autowarefoundation#1262)

Signed-off-by: wep21 <border_goldenmarket@yahoo.co.jp>

* Remove use_sim_time for set_parameter (autowarefoundation#1260)

Signed-off-by: wep21 <border_goldenmarket@yahoo.co.jp>

* Remove autoware_debug_msgs from autoware_joy_controller (autowarefoundation#1303)

Signed-off-by: Kenji Miyake <kenji.miyake@tier4.jp>

* Porting remote cmd selector (autowarefoundation#1286)

* Feature/add remote cmd selector (autowarefoundation#1179)

* Add in/out args of remote_cmd_converter.launch

Signed-off-by: Kenji Miyake <kenji.miyake@tier4.jp>

* Change remote input topic of vehicle_cmd_gate

Signed-off-by: Kenji Miyake <kenji.miyake@tier4.jp>

* Add msgs for remote_cmd_selector

Signed-off-by: Kenji Miyake <kenji.miyake@tier4.jp>

* Add remote_cmd_selector

Signed-off-by: Kenji Miyake <kenji.miyake@tier4.jp>

* Rename remote_cmd_selector to external_cmd_selector

Signed-off-by: Kenji Miyake <kenji.miyake@tier4.jp>

* Remove VehicleCommand support in autoware_joy_controller

Signed-off-by: Kenji Miyake <kenji.miyake@tier4.jp>

* Support external_cmd_source in autoware_joy_controller.launch (autowarefoundation#1194)

Signed-off-by: Kenji Miyake <kenji.miyake@tier4.jp>

* Fix porting miss

Signed-off-by: Kenji Miyake <kenji.miyake@tier4.jp>

* fix missing function

* modify xml format

* fix include guard

* add callback group

* modify remap name

* Revert "modify remap name"

This reverts commit 169cc8d28442825b1d61b0439b9892c913304527.

* change topic name

* use rclcpp_component

* Remove autoware_debug_msgs from autoware_joy_controller

Signed-off-by: Kenji Miyake <kenji.miyake@tier4.jp>

* Change default mode of autoware_joy_controller

Signed-off-by: Kenji Miyake <kenji.miyake@tier4.jp>

Co-authored-by: Kenji Miyake <31987104+kenji-miyake@users.noreply.github.com>
Co-authored-by: Kenji Miyake <kenji.miyake@tier4.jp>

* Ros2/create/external commands (autowarefoundation#1299)

* add remote message

* add remote commands

* fix topic

* remove unnecessary topic

* remove unused topic

* add external cmd instead

* ToExternalComd

* fix topic in joy con

* Fix -Wunused-parameter (autowarefoundation#1836)

* Fix -Wunused-parameter

Signed-off-by: Kenji Miyake <kenji.miyake@tier4.jp>

* Fix mistake

Signed-off-by: Kenji Miyake <kenji.miyake@tier4.jp>

* fix spell

* Fix lint issues

Signed-off-by: Kenji Miyake <kenji.miyake@tier4.jp>

* Ignore flake8 warnings

Signed-off-by: Kenji Miyake <kenji.miyake@tier4.jp>

Co-authored-by: Hiroki OTA <hiroki.ota@tier4.jp>

* Add autoware api (autowarefoundation#1979)

* 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>

* 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

* Add selected external command API (autowarefoundation#2053)

* submit engage with api service from joy controller (autowarefoundation#2320)

* fix engagew with api

* delete unused

* fix for uncrustify

* revive vehicle_engage

* some fix

* revive autoware name

* fix service name

* 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>

* Add COLCON_IGNORE (autowarefoundation#500)

Signed-off-by: Kenji Miyake <kenji.miyake@tier4.jp>

* port autoware joy controller (autowarefoundation#588)

* port autoware joy controller

* fix compile error

* use odometry instead of twist

* update launch

Co-authored-by: Takayuki Murooka <takayuki.murooka@tier4.jp>

* update README.md in autoware_joy_controller (autowarefoundation#593)

* update README.md

* update README.md

* fix typo

* Update control/autoware_joy_controller/README.md

Co-authored-by: taikitanaka3 <65527974+taikitanaka3@users.noreply.github.com>

* update README.md

Co-authored-by: Takayuki Murooka <takayuki.murooka@tier4.jp>
Co-authored-by: taikitanaka3 <65527974+taikitanaka3@users.noreply.github.com>

* fix format

* ci(pre-commit): autofix

Co-authored-by: mitsudome-r <ryohsuke.mitsudome@tier4.jp>
Co-authored-by: Fumiya Watanabe <rej55.g@gmail.com>
Co-authored-by: Fumiya Watanabe <fumiya.watanabe@tier4.jp>
Co-authored-by: Kenji Miyake <31987104+kenji-miyake@users.noreply.github.com>
Co-authored-by: Nikolai Morin <nnmmgit@gmail.com>
Co-authored-by: Servando <43142004+sgermanserrano@users.noreply.github.com>
Co-authored-by: Takamasa Horibe <horibe.takamasa@gmail.com>
Co-authored-by: jpntaxi4943-autoware <proj-jpntaxi@tier4.jp>
Co-authored-by: Ryohsuke Mitsudome <43976834+mitsudome-r@users.noreply.github.com>
Co-authored-by: Kenji Miyake <kenji.miyake@tier4.jp>
Co-authored-by: tkimura4 <tomoya.kimura@tier4.jp>
Co-authored-by: Daisuke Nishimatsu <42202095+wep21@users.noreply.github.com>
Co-authored-by: Keisuke Shima <keisuke.shima@tier4.jp>
Co-authored-by: Hiroki OTA <hiroki.ota@tier4.jp>
Co-authored-by: Takagi, Isamu <43976882+isamu-takagi@users.noreply.github.com>
Co-authored-by: Keisuke Shima <19993104+KeisukeShima@users.noreply.github.com>
Co-authored-by: Takayuki Murooka <takayuki5168@gmail.com>
Co-authored-by: Takayuki Murooka <takayuki.murooka@tier4.jp>
Co-authored-by: pre-commit-ci[bot] <66853113+pre-commit-ci[bot]@users.noreply.github.com>
Co-authored-by: Takeshi Miura <57553950+1222-takeshi@users.noreply.github.com>
  • Loading branch information
@taikitanaka3 @mitsudome-r
@rej55 @kenji-miyake @nnmm @sgermanserrano @TakaHoribe @j4tfwm6z @tkimura4 @wep21 @KeisukeShima @h-ohta @isamu-takagi @takayuki5168 @pre-commit-ci @1222-takeshi
21 people authored Dec 7, 2021
1 parent 70ec2e2 commit d44beb2
Show file tree
Hide file tree
Showing 10 changed files with 1,050 additions and 0 deletions.
32 changes: 32 additions & 0 deletions control/autoware_joy_controller/CMakeLists.txt
View file
Original file line number Diff line number Diff line change
@@ -0,0 +1,32 @@
cmake_minimum_required(VERSION 3.5)
project(autoware_joy_controller)

if(NOT CMAKE_CXX_STANDARD)
set(CMAKE_CXX_STANDARD 14)
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(autoware_joy_controller_node SHARED
src/autoware_joy_controller/autoware_joy_controller_node.cpp
)

rclcpp_components_register_node(autoware_joy_controller_node
PLUGIN "autoware_joy_controller::AutowareJoyControllerNode"
EXECUTABLE autoware_joy_controller
)

if(BUILD_TESTING)
find_package(ament_lint_auto REQUIRED)
ament_lint_auto_find_test_dependencies()
endif()

ament_auto_package(
INSTALL_TO_SHARE
launch
config
)
43 changes: 43 additions & 0 deletions control/autoware_joy_controller/README.md
View file
Original file line number Diff line number Diff line change
@@ -0,0 +1,43 @@
# autoware_joy_controller

## Role

`autoware_joy_controller` is the package to convert a joy msg to autoware commands (e.g. steering wheel, shift, turn signal, engage) for a vehicle.

## Input / Output

### Input topics

| Name | Type | Description |
| ------------------ | ----------------------- | --------------------------------- |
| `~/input/joy` | sensor_msgs::msg::Joy | joy controller command |
| `~/input/odometry` | nav_msgs::msg::Odometry | ego vehicle odometry to get twist |

### Output topics

| Name | Type | Description |
| ----------------------------------- | -------------------------------------------------------- | ---------------------------------------- |
| `~/output/control_command` | autoware_auto_control_msgs::msg::AckermannControlCommand | lateral and longitudinal control command |
| `~/output/external_control_command` | autoware_external_api_msgs::msg::ControlCommandStamped | lateral and longitudinal control command |
| `~/output/shift` | autoware_external_api_msgs::msg::GearShiftStamped | gear command |
| `~/output/turn_signal` | autoware_external_api_msgs::msg::TurnSignalStamped | turn signal command |
| `~/output/gate_mode` | autoware_control_msgs::msg::GateMode | gate mode (Auto or External) |
| `~/output/heartbeat` | autoware_external_api_msgs::msg::Heartbeat | heartbeat |
| `~/output/vehicle_engage` | autoware_auto_vehicle_msgs::msg::Engage | vehicle engage |

## Parameters

| Parameter | Type | Description |
| ------------------------- | ------ | ------------------------------------------------------------------------------------------------------------------ |
| `joy_type` | string | joy controller type (default: DS4) |
| `update_rate` | double | update rate to publish control commands |
| `accel_ratio` | double | ratio to calculate acceleration (commanded acceleration is ratio \* operation) |
| `brake_ratio` | double | ratio to calculate deceleration (commanded acceleration is -ratio \* operation) |
| `steer_ratio` | double | ratio to calculate deceleration (commanded steer is ratio \* operation) |
| `steering_angle_velocity` | double | steering angle velocity for operation |
| `accel_sensitivity` | double | sensitivity to calculate acceleration for external API (commanded acceleration is pow(operation, 1 / sensitivity)) |
| `brake_sensitivity` | double | sensitivity to calculate deceleration for external API (commanded acceleration is pow(operation, 1 / sensitivity)) |
| `velocity_gain` | double | ratio to calculate velocity by acceleration |
| `max_forward_velocity` | double | absolute max velocity to go forward |
| `max_backward_velocity` | double | absolute max velocity to go backward |
| `backward_accel_ratio` | double | ratio to calculate deceleration (commanded acceleration is -ratio \* operation) |
14 changes: 14 additions & 0 deletions control/autoware_joy_controller/config/autoware_joy_controller.param.yaml
View file
Original file line number Diff line number Diff line change
@@ -0,0 +1,14 @@
/**:
ros__parameters:
update_rate: 10.0
accel_ratio: 3.0
brake_ratio: 5.0
steer_ratio: 0.5
steering_angle_velocity: 0.1
accel_sensitivity: 1.0
brake_sensitivity: 1.0
control_command:
velocity_gain: 3.0
max_forward_velocity: 20.0
max_backward_velocity: 3.0
backward_accel_ratio: 1.0
123 changes: 123 additions & 0 deletions control/autoware_joy_controller/include/autoware_joy_controller/autoware_joy_controller.hpp
View file
Original file line number Diff line number Diff line change
@@ -0,0 +1,123 @@
// 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 AUTOWARE_JOY_CONTROLLER__AUTOWARE_JOY_CONTROLLER_HPP_
#define AUTOWARE_JOY_CONTROLLER__AUTOWARE_JOY_CONTROLLER_HPP_

#include "autoware_joy_controller/joy_converter/joy_converter_base.hpp"

#include <rclcpp/rclcpp.hpp>

#include <autoware_auto_control_msgs/msg/ackermann_control_command.hpp>
#include <autoware_auto_vehicle_msgs/msg/engage.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/gear_shift_stamped.hpp>
#include <autoware_external_api_msgs/msg/heartbeat.hpp>
#include <autoware_external_api_msgs/msg/turn_signal_stamped.hpp>
#include <autoware_external_api_msgs/srv/engage.hpp>
#include <autoware_external_api_msgs/srv/set_emergency.hpp>
#include <geometry_msgs/msg/twist_stamped.hpp>
#include <nav_msgs/msg/odometry.hpp>
#include <sensor_msgs/msg/joy.hpp>

#include <algorithm>
#include <memory>
#include <string>

namespace autoware_joy_controller
{
using GearShiftType = autoware_external_api_msgs::msg::GearShift::_data_type;
using TurnSignalType = autoware_external_api_msgs::msg::TurnSignal::_data_type;
using GateModeType = autoware_control_msgs::msg::GateMode::_data_type;

class AutowareJoyControllerNode : public rclcpp::Node
{
public:
explicit AutowareJoyControllerNode(const rclcpp::NodeOptions & node_options);

private:
// Parameter
std::string joy_type_;
double update_rate_;
double accel_ratio_;
double brake_ratio_;
double steer_ratio_;
double steering_angle_velocity_;
double accel_sensitivity_;
double brake_sensitivity_;

// ControlCommand Parameter
double velocity_gain_;
double max_forward_velocity_;
double max_backward_velocity_;
double backward_accel_ratio_;

// CallbackGroups
rclcpp::CallbackGroup::SharedPtr callback_group_subscribers_;
rclcpp::CallbackGroup::SharedPtr callback_group_services_;

// Subscriber
rclcpp::Subscription<sensor_msgs::msg::Joy>::SharedPtr sub_joy_;
rclcpp::Subscription<nav_msgs::msg::Odometry>::SharedPtr sub_odom_;

rclcpp::Time last_joy_received_time_;
std::shared_ptr<const JoyConverterBase> joy_;
geometry_msgs::msg::TwistStamped::ConstSharedPtr twist_;

void onJoy(const sensor_msgs::msg::Joy::ConstSharedPtr msg);
void onOdometry(const nav_msgs::msg::Odometry::ConstSharedPtr msg);

// Publisher
rclcpp::Publisher<autoware_auto_control_msgs::msg::AckermannControlCommand>::SharedPtr
pub_control_command_;
rclcpp::Publisher<autoware_external_api_msgs::msg::ControlCommandStamped>::SharedPtr
pub_external_control_command_;
rclcpp::Publisher<autoware_external_api_msgs::msg::GearShiftStamped>::SharedPtr pub_shift_;
rclcpp::Publisher<autoware_external_api_msgs::msg::TurnSignalStamped>::SharedPtr pub_turn_signal_;
rclcpp::Publisher<autoware_external_api_msgs::msg::Heartbeat>::SharedPtr pub_heartbeat_;
rclcpp::Publisher<autoware_control_msgs::msg::GateMode>::SharedPtr pub_gate_mode_;
rclcpp::Publisher<autoware_auto_vehicle_msgs::msg::Engage>::SharedPtr pub_vehicle_engage_;

void publishControlCommand();
void publishExternalControlCommand();
void publishShift();
void publishTurnSignal();
void publishGateMode();
void publishHeartbeat();
void publishAutowareEngage();
void publishVehicleEngage();
void sendEmergencyRequest(bool emergency);

// Service Client
rclcpp::Client<autoware_external_api_msgs::srv::SetEmergency>::SharedPtr client_emergency_stop_;
rclcpp::Client<autoware_external_api_msgs::srv::Engage>::SharedPtr client_autoware_engage_;

// Previous State
autoware_auto_control_msgs::msg::AckermannControlCommand prev_control_command_;
autoware_external_api_msgs::msg::ControlCommand prev_external_control_command_;
GearShiftType prev_shift_ = autoware_external_api_msgs::msg::GearShift::NONE;
TurnSignalType prev_turn_signal_ = autoware_external_api_msgs::msg::TurnSignal::NONE;
GateModeType prev_gate_mode_ = autoware_control_msgs::msg::GateMode::AUTO;

// Timer
rclcpp::TimerBase::SharedPtr timer_;
void initTimer(double period_s);

bool isDataReady();
void onTimer();
};
} // namespace autoware_joy_controller

#endif // AUTOWARE_JOY_CONTROLLER__AUTOWARE_JOY_CONTROLLER_HPP_
95 changes: 95 additions & 0 deletions ...toware_joy_controller/include/autoware_joy_controller/joy_converter/ds4_joy_converter.hpp
View file
Original file line number Diff line number Diff line change
@@ -0,0 +1,95 @@
// 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 AUTOWARE_JOY_CONTROLLER__JOY_CONVERTER__DS4_JOY_CONVERTER_HPP_
#define AUTOWARE_JOY_CONTROLLER__JOY_CONVERTER__DS4_JOY_CONVERTER_HPP_

#include "autoware_joy_controller/joy_converter/joy_converter_base.hpp"

#include <algorithm>

namespace autoware_joy_controller
{
class DS4JoyConverter : public JoyConverterBase
{
public:
explicit DS4JoyConverter(const sensor_msgs::msg::Joy & j) : j_(j) {}

float accel() const
{
const auto button = static_cast<float>(Cross());
const auto stick = std::max(0.0f, RStickUpDown());
const auto trigger = std::max(0.0f, -RTrigger());
return std::max({button, stick, trigger});
}

float brake() const
{
const auto button = static_cast<float>(Square());
const auto stick = std::max(0.0f, -RStickUpDown());
const auto trigger = std::max(0.0f, -LTrigger());
return std::max({button, stick, trigger});
}

float steer() const { return LStickLeftRight(); }

bool shift_up() const { return CursorUpDown() == 1.0f; }
bool shift_down() const { return CursorUpDown() == -1.0f; }
bool shift_drive() const { return CursorLeftRight() == 1.0f; }
bool shift_reverse() const { return CursorLeftRight() == -1.0f; }

bool turn_signal_left() const { return L1(); }
bool turn_signal_right() const { return R1(); }
bool clear_turn_signal() const { return Share(); }

bool gate_mode() const { return Options(); }

bool emergency_stop() const { return !reverse() && PS(); }
bool clear_emergency_stop() const { return reverse() && PS(); }

bool autoware_engage() const { return !reverse() && Circle(); }
bool autoware_disengage() const { return reverse() && Circle(); }

bool vehicle_engage() const { return !reverse() && Triangle(); }
bool vehicle_disengage() const { return reverse() && Triangle(); }

private:
float LStickLeftRight() const { return j_.axes.at(0); }
float LStickUpDown() const { return j_.axes.at(1); }
float LTrigger() const { return j_.axes.at(2); }
float RStickLeftRight() const { return j_.axes.at(3); }
float RStickUpDown() const { return j_.axes.at(4); }
float RTrigger() const { return j_.axes.at(5); }
float CursorLeftRight() const { return j_.axes.at(6); }
float CursorUpDown() const { return j_.axes.at(7); }

bool Cross() const { return j_.buttons.at(0); }
bool Circle() const { return j_.buttons.at(1); }
bool Triangle() const { return j_.buttons.at(2); }
bool Square() const { return j_.buttons.at(3); }
bool L1() const { return j_.buttons.at(4); }
bool R1() const { return j_.buttons.at(5); }
bool L2() const { return j_.buttons.at(6); }
bool R2() const { return j_.buttons.at(7); }
bool Share() const { return j_.buttons.at(8); }
bool Options() const { return j_.buttons.at(9); }
bool PS() const { return j_.buttons.at(10); }

const sensor_msgs::msg::Joy j_;

bool reverse() const { return Share(); }
};
} // namespace autoware_joy_controller

#endif // AUTOWARE_JOY_CONTROLLER__JOY_CONVERTER__DS4_JOY_CONVERTER_HPP_
Loading

0 comments on commit d44beb2

Please sign in to comment.