From 55874d21b34b1638aecc3e26be870ece6f4bf2b5 Mon Sep 17 00:00:00 2001 From: mitsudome-r Date: Fri, 18 Sep 2020 15:10:43 +0900 Subject: [PATCH 01/38] release v0.4.0 --- .../autoware_joy_controller/CMakeLists.txt | 55 +++ .../config/autoware_joy_controller.yaml | 10 + .../autoware_joy_controller.h | 184 ++++++++ .../launch/autoware_joy_controller.launch | 40 ++ control/autoware_joy_controller/package.xml | 20 + .../autoware_joy_controller_node.cpp | 410 ++++++++++++++++++ .../src/autoware_joy_controller/main.cpp | 30 ++ 7 files changed, 749 insertions(+) create mode 100644 control/autoware_joy_controller/CMakeLists.txt create mode 100644 control/autoware_joy_controller/config/autoware_joy_controller.yaml create mode 100644 control/autoware_joy_controller/include/autoware_joy_controller/autoware_joy_controller.h create mode 100644 control/autoware_joy_controller/launch/autoware_joy_controller.launch create mode 100644 control/autoware_joy_controller/package.xml create mode 100644 control/autoware_joy_controller/src/autoware_joy_controller/autoware_joy_controller_node.cpp create mode 100644 control/autoware_joy_controller/src/autoware_joy_controller/main.cpp diff --git a/control/autoware_joy_controller/CMakeLists.txt b/control/autoware_joy_controller/CMakeLists.txt new file mode 100644 index 0000000000000..f0d8844959ac0 --- /dev/null +++ b/control/autoware_joy_controller/CMakeLists.txt @@ -0,0 +1,55 @@ +cmake_minimum_required(VERSION 3.0.2) +project(autoware_joy_controller) + +add_compile_options(-std=c++14) + +find_package(catkin REQUIRED COMPONENTS + autoware_control_msgs + autoware_vehicle_msgs + joy + geometry_msgs + sensor_msgs + std_msgs +) + +catkin_package() + +include_directories( + include + ${catkin_INCLUDE_DIRS} +) + +add_executable(autoware_joy_controller + src/autoware_joy_controller/main.cpp + src/autoware_joy_controller/autoware_joy_controller_node.cpp +) + +target_link_libraries(autoware_joy_controller + ${catkin_LIBRARIES} +) + +add_dependencies(autoware_joy_controller + ${catkin_EXPORTED_TARGETS} +) + +# Install executables and/or libraries +install( + TARGETS + autoware_joy_controller + ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} + LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} + RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} +) + +# Install project namespaced headers +install(DIRECTORY include/${PROJECT_NAME}/ + DESTINATION ${CATKIN_PACKAGE_INCLUDE_DESTINATION} +) + +# Install directories +install( + DIRECTORY + launch + config + DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION} +) diff --git a/control/autoware_joy_controller/config/autoware_joy_controller.yaml b/control/autoware_joy_controller/config/autoware_joy_controller.yaml new file mode 100644 index 0000000000000..f7ee5a5c4e10e --- /dev/null +++ b/control/autoware_joy_controller/config/autoware_joy_controller.yaml @@ -0,0 +1,10 @@ +update_rate: 10 +accel_ratio: 3.0 +brake_ratio: 5.0 +steer_ratio: 0.5 +steering_angle_velocity: 0.1 +control_command: + velocity_gain: 3.0 + max_forward_velocity: 20 + max_backward_velocity: 3 + backward_accel_ratio: 1.0 diff --git a/control/autoware_joy_controller/include/autoware_joy_controller/autoware_joy_controller.h b/control/autoware_joy_controller/include/autoware_joy_controller/autoware_joy_controller.h new file mode 100644 index 0000000000000..6ca9d1fca10cd --- /dev/null +++ b/control/autoware_joy_controller/include/autoware_joy_controller/autoware_joy_controller.h @@ -0,0 +1,184 @@ +/* + * Copyright 2020 Tier IV, Inc. All rights reserved. + * + * 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. + */ + +#pragma once + +#include +#include +#include + +#include + +#include +#include +#include +#include +#include +#include +#include +#include + +// tmp +#include +#include + +class JoyConverter +{ +public: + explicit JoyConverter(const sensor_msgs::Joy & j) : j_(j) {} + + const float accel() const + { + const auto button = static_cast(Cross()); + const auto stick = std::max(0.0f, RStickUpDown()); + const auto trigger = std::max(0.0f, -RTrigger()); + return std::max({button, stick, trigger}); + } + + const float brake() const + { + const auto button = static_cast(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; } + bool shift_down() const { return CursorUpDown() == -1; } + bool shift_drive() const { return CursorLeftRight() == 1; } + bool shift_reverse() const { return CursorLeftRight() == -1; } + + 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() const { return !reverse() && PS(); } + bool clear_emergency() 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); } + +private: + bool reverse() const { return Share(); } + + const sensor_msgs::Joy j_; +}; + +using ShiftType = autoware_vehicle_msgs::Shift::_data_type; +using TurnSignalType = autoware_vehicle_msgs::TurnSignal::_data_type; +using GateModeType = autoware_control_msgs::GateMode::_data_type; + +class AutowareJoyControllerNode +{ +public: + AutowareJoyControllerNode(); + +private: + // NodeHandle + ros::NodeHandle nh_{""}; + ros::NodeHandle private_nh_{"~"}; + + // Parameter + double update_rate_; + double accel_ratio_; + double brake_ratio_; + double steer_ratio_; + double steering_angle_velocity_; + + // ControlCommand Parameter + double velocity_gain_; + double max_forward_velocity_; + double max_backward_velocity_; + double backward_accel_ratio_; + + // Subscriber + ros::Subscriber sub_joy_; + ros::Subscriber sub_twist_; + + ros::Time last_joy_received_time_; + std::shared_ptr joy_; + geometry_msgs::TwistStamped::ConstPtr twist_; + + void onJoy(const sensor_msgs::Joy::ConstPtr & msg); + void onTwist(const geometry_msgs::TwistStamped::ConstPtr & msg); + + // Publisher + ros::Publisher pub_control_command_; + ros::Publisher pub_raw_control_command_; + ros::Publisher pub_shift_; + ros::Publisher pub_turn_signal_; + ros::Publisher pub_gate_mode_; + ros::Publisher pub_emergency_; + ros::Publisher pub_autoware_engage_; + ros::Publisher pub_vehicle_engage_; + + void publishControlCommand(); + void publishRawControlCommand(); + void publishShift(); + void publishTurnSignal(); + void publishGateMode(); + void publishEmergency(); + void publishAutowareEngage(); + void publishVehicleEngage(); + + // Previous State + autoware_control_msgs::ControlCommand prev_control_command_; + autoware_vehicle_msgs::RawControlCommand prev_raw_control_command_; + ShiftType prev_shift_ = autoware_vehicle_msgs::Shift::NONE; + TurnSignalType prev_turn_signal_ = autoware_vehicle_msgs::TurnSignal::NONE; + GateModeType prev_gate_mode_ = autoware_control_msgs::GateMode::AUTO; + + // tmp + ros::Publisher pub_vehicle_command_; + ros::Publisher pub_raw_vehicle_command_; + void publishVehicleCommand(); + void publishRawVehicleCommand(); + + // Timer + ros::Timer timer_; + + bool isDataReady(); + void onTimer(const ros::TimerEvent & event); +}; diff --git a/control/autoware_joy_controller/launch/autoware_joy_controller.launch b/control/autoware_joy_controller/launch/autoware_joy_controller.launch new file mode 100644 index 0000000000000..843da5465096a --- /dev/null +++ b/control/autoware_joy_controller/launch/autoware_joy_controller.launch @@ -0,0 +1,40 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/control/autoware_joy_controller/package.xml b/control/autoware_joy_controller/package.xml new file mode 100644 index 0000000000000..0657eb663f726 --- /dev/null +++ b/control/autoware_joy_controller/package.xml @@ -0,0 +1,20 @@ + + + autoware_joy_controller + 0.1.0 + The autoware_joy_controller package + Kenji Miyake + Apache 2 + + catkin + + autoware_control_msgs + autoware_vehicle_msgs + joy + geometry_msgs + sensor_msgs + std_msgs + + + + diff --git a/control/autoware_joy_controller/src/autoware_joy_controller/autoware_joy_controller_node.cpp b/control/autoware_joy_controller/src/autoware_joy_controller/autoware_joy_controller_node.cpp new file mode 100644 index 0000000000000..057274bddf553 --- /dev/null +++ b/control/autoware_joy_controller/src/autoware_joy_controller/autoware_joy_controller_node.cpp @@ -0,0 +1,410 @@ +/* + * Copyright 2020 Tier IV, Inc. All rights reserved. + * + * 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 + +namespace +{ +ShiftType getUpperShift(const ShiftType & shift) +{ + using autoware_vehicle_msgs::Shift; + + if (shift == Shift::NONE) return Shift::PARKING; + if (shift == Shift::PARKING) return Shift::REVERSE; + if (shift == Shift::REVERSE) return Shift::NEUTRAL; + if (shift == Shift::NEUTRAL) return Shift::DRIVE; + if (shift == Shift::DRIVE) return Shift::LOW; + if (shift == Shift::LOW) return Shift::LOW; + + return Shift::NONE; +} + +ShiftType getLowerShift(const ShiftType & shift) +{ + using autoware_vehicle_msgs::Shift; + + if (shift == Shift::NONE) return Shift::PARKING; + if (shift == Shift::PARKING) return Shift::PARKING; + if (shift == Shift::REVERSE) return Shift::PARKING; + if (shift == Shift::NEUTRAL) return Shift::REVERSE; + if (shift == Shift::DRIVE) return Shift::NEUTRAL; + if (shift == Shift::LOW) return Shift::DRIVE; + + return Shift::NONE; +} + +const char * getShiftName(const ShiftType & shift) +{ + using autoware_vehicle_msgs::Shift; + + if (shift == Shift::NONE) return "NONE"; + if (shift == Shift::PARKING) return "PARKING"; + if (shift == Shift::REVERSE) return "REVERSE"; + if (shift == Shift::NEUTRAL) return "NEUTRAL"; + if (shift == Shift::DRIVE) return "DRIVE"; + if (shift == Shift::LOW) return "LOW"; + + return "NOT_SUPPORTED"; +} + +const char * getTurnSignalName(const TurnSignalType & turn_signal) +{ + using autoware_vehicle_msgs::TurnSignal; + + if (turn_signal == TurnSignal::NONE) return "NONE"; + if (turn_signal == TurnSignal::LEFT) return "LEFT"; + if (turn_signal == TurnSignal::RIGHT) return "RIGHT"; + if (turn_signal == TurnSignal::HAZARD) return "HAZARD"; + + return "NOT_SUPPORTED"; +} + +const char * getGateModeName(const GateModeType & gate_mode) +{ + using autoware_control_msgs::GateMode; + + if (gate_mode == GateMode::AUTO) return "AUTO"; + if (gate_mode == GateMode::REMOTE) return "REMOTE"; + + return "NOT_SUPPORTED"; +} + +} // namespace + +void AutowareJoyControllerNode::onJoy(const sensor_msgs::Joy::ConstPtr & msg) +{ + last_joy_received_time_ = msg->header.stamp; + joy_ = std::make_shared(*msg); + + if (joy_->shift_up() || joy_->shift_down() || joy_->shift_drive() || joy_->shift_reverse()) { + publishShift(); + } + + if (joy_->turn_signal_left() || joy_->turn_signal_right() || joy_->clear_turn_signal()) { + publishTurnSignal(); + } + + if (joy_->gate_mode()) { + publishGateMode(); + } + + if (joy_->emergency() || joy_->clear_emergency()) { + publishEmergency(); + } + + if (joy_->autoware_engage() || joy_->autoware_disengage()) { + publishAutowareEngage(); + } + + if (joy_->vehicle_engage() || joy_->vehicle_disengage()) { + publishVehicleEngage(); + } +} + +void AutowareJoyControllerNode::onTwist(const geometry_msgs::TwistStamped::ConstPtr & msg) +{ + twist_ = msg; +} + +bool AutowareJoyControllerNode::isDataReady() +{ + // Joy + { + if (!joy_) { + ROS_WARN_THROTTLE(1.0, "waiting for joy msg..."); + return false; + } + + constexpr auto timeout = 2.0; + const auto time_diff = ros::Time::now() - last_joy_received_time_; + if (time_diff.toSec() > timeout) { + ROS_WARN_THROTTLE(1.0, "joy msg is timeout"); + return false; + } + } + + // Twist + { + if (!twist_) { + ROS_WARN_THROTTLE(1.0, "waiting for twist msg..."); + return false; + } + + constexpr auto timeout = 0.5; + const auto time_diff = ros::Time::now() - twist_->header.stamp; + if (time_diff.toSec() > timeout) { + ROS_WARN_THROTTLE(1.0, "twist msg is timeout"); + return false; + } + } + + return true; +} + +void AutowareJoyControllerNode::onTimer(const ros::TimerEvent & event) +{ + if (!isDataReady()) { + return; + } + + publishControlCommand(); + publishRawControlCommand(); + + // tmp + publishVehicleCommand(); + publishRawVehicleCommand(); +} + +void AutowareJoyControllerNode::publishControlCommand() +{ + autoware_control_msgs::ControlCommandStamped cmd_stamped; + cmd_stamped.header.stamp = ros::Time::now(); + + { + auto & cmd = cmd_stamped.control; + + cmd.steering_angle = steer_ratio_ * joy_->steer(); + cmd.steering_angle_velocity = steering_angle_velocity_; + + if (joy_->accel()) { + cmd.acceleration = accel_ratio_ * joy_->accel(); + cmd.velocity = twist_->twist.linear.x + velocity_gain_ * cmd.acceleration; + cmd.velocity = std::min(cmd.velocity, max_forward_velocity_); + } + + if (joy_->brake()) { + cmd.velocity = 0.0; + cmd.acceleration = -brake_ratio_ * joy_->brake(); + } + + // Backward + if (joy_->accel() && joy_->brake()) { + cmd.acceleration = backward_accel_ratio_ * joy_->accel(); + cmd.velocity = twist_->twist.linear.x - velocity_gain_ * cmd.acceleration; + cmd.velocity = std::max(cmd.velocity, -max_backward_velocity_); + } + } + + pub_control_command_.publish(cmd_stamped); + prev_control_command_ = cmd_stamped.control; +} + +void AutowareJoyControllerNode::publishRawControlCommand() +{ + autoware_vehicle_msgs::RawControlCommandStamped cmd_stamped; + cmd_stamped.header.stamp = ros::Time::now(); + + { + auto & cmd = cmd_stamped.control; + + cmd.steering_angle = steer_ratio_ * joy_->steer(); + cmd.steering_angle_velocity = steering_angle_velocity_; + cmd.throttle = accel_ratio_ * joy_->accel(); + cmd.brake = brake_ratio_ * joy_->brake(); + } + + pub_raw_control_command_.publish(cmd_stamped); + prev_raw_control_command_ = cmd_stamped.control; +} + +void AutowareJoyControllerNode::publishShift() +{ + using autoware_vehicle_msgs::Shift; + + autoware_vehicle_msgs::ShiftStamped shift_stamped; + shift_stamped.header.stamp = ros::Time::now(); + + { + auto & shift = shift_stamped.shift; + if (joy_->shift_up()) { + shift.data = getUpperShift(prev_shift_); + } + + if (joy_->shift_down()) { + shift.data = getLowerShift(prev_shift_); + } + + if (joy_->shift_drive()) { + shift.data = Shift::DRIVE; + } + + if (joy_->shift_reverse()) { + shift.data = Shift::REVERSE; + } + + ROS_INFO("Shift::%s", getShiftName(shift.data)); + } + + pub_shift_.publish(shift_stamped); + prev_shift_ = shift_stamped.shift.data; +} + +void AutowareJoyControllerNode::publishTurnSignal() +{ + using autoware_vehicle_msgs::TurnSignal; + + TurnSignal turn_signal; + turn_signal.header.stamp = ros::Time::now(); + + if (joy_->turn_signal_left() && joy_->turn_signal_right()) { + turn_signal.data = TurnSignal::HAZARD; + } else if (joy_->turn_signal_left()) { + turn_signal.data = TurnSignal::LEFT; + } else if (joy_->turn_signal_right()) { + turn_signal.data = TurnSignal::RIGHT; + } + + if (joy_->clear_turn_signal()) { + turn_signal.data = TurnSignal::NONE; + } + + ROS_INFO("TurnSignal::%s", getTurnSignalName(turn_signal.data)); + + pub_turn_signal_.publish(turn_signal); +} + +void AutowareJoyControllerNode::publishGateMode() +{ + using autoware_control_msgs::GateMode; + + autoware_control_msgs::GateMode gate_mode; + + if (prev_gate_mode_ == GateMode::AUTO) { + gate_mode.data = GateMode::REMOTE; + } + + if (prev_gate_mode_ == GateMode::REMOTE) { + gate_mode.data = GateMode::AUTO; + } + + ROS_INFO("GateMode::%s", getGateModeName(gate_mode.data)); + + pub_gate_mode_.publish(gate_mode); + prev_gate_mode_ = gate_mode.data; +} + +void AutowareJoyControllerNode::publishEmergency() +{ + std_msgs::Bool emergency; + + if (joy_->emergency()) { + emergency.data = true; + ROS_INFO("Emergency"); + } + + if (joy_->clear_emergency()) { + emergency.data = false; + ROS_INFO("Clear Emergency"); + } + + pub_emergency_.publish(emergency); +} + +void AutowareJoyControllerNode::publishAutowareEngage() +{ + std_msgs::Bool engage; + + if (joy_->autoware_engage()) { + engage.data = true; + ROS_INFO("Autoware Engage"); + } + + if (joy_->autoware_disengage()) { + engage.data = false; + ROS_INFO("Autoware Disengage"); + } + + pub_autoware_engage_.publish(engage); +} + +void AutowareJoyControllerNode::publishVehicleEngage() +{ + std_msgs::Bool engage; + + if (joy_->vehicle_engage()) { + engage.data = true; + ROS_INFO("Vehicle Engage"); + } + + if (joy_->vehicle_disengage()) { + engage.data = false; + ROS_INFO("Vehicle Disengage"); + } + + pub_vehicle_engage_.publish(engage); +} + +// tmp +void AutowareJoyControllerNode::publishVehicleCommand() +{ + autoware_vehicle_msgs::VehicleCommand vehicle_cmd; + vehicle_cmd.header.stamp = ros::Time::now(); + vehicle_cmd.control = prev_control_command_; + vehicle_cmd.shift.data = prev_shift_; + + pub_vehicle_command_.publish(vehicle_cmd); +} + +void AutowareJoyControllerNode::publishRawVehicleCommand() +{ + autoware_vehicle_msgs::RawVehicleCommand vehicle_cmd; + vehicle_cmd.header.stamp = ros::Time::now(); + vehicle_cmd.control = prev_raw_control_command_; + vehicle_cmd.shift.data = prev_shift_; + + pub_raw_vehicle_command_.publish(vehicle_cmd); +} + +AutowareJoyControllerNode::AutowareJoyControllerNode() +{ + // Parameter + private_nh_.param("update_rate", update_rate_, 10.0); + private_nh_.param("accel_ratio", accel_ratio_, 3.0); + private_nh_.param("brake_ratio", brake_ratio_, 5.0); + private_nh_.param("steer_ratio", steer_ratio_, 0.5); + private_nh_.param("steering_angle_velocity", steering_angle_velocity_, 0.1); + private_nh_.param("control_command/velocity_gain", velocity_gain_, 3.0); + private_nh_.param("control_command/max_forward_velocity", max_forward_velocity_, 20.0); + private_nh_.param("control_command/max_backward_velocity", max_backward_velocity_, 3.0); + private_nh_.param("control_command/backward_accel_ratio", backward_accel_ratio_, 1.0); + + // Subscriber + sub_joy_ = private_nh_.subscribe("input/joy", 1, &AutowareJoyControllerNode::onJoy, this); + sub_twist_ = private_nh_.subscribe("input/twist", 1, &AutowareJoyControllerNode::onTwist, this); + + // Publisher + pub_control_command_ = private_nh_.advertise( + "output/control_command", 1); + pub_raw_control_command_ = private_nh_.advertise( + "output/raw_control_command", 1); + pub_shift_ = private_nh_.advertise("output/shift", 1); + pub_turn_signal_ = + private_nh_.advertise("output/turn_signal", 1); + pub_gate_mode_ = private_nh_.advertise("output/gate_mode", 1); + pub_emergency_ = private_nh_.advertise("output/emergency", 1); + pub_autoware_engage_ = private_nh_.advertise("output/autoware_engage", 1); + pub_vehicle_engage_ = private_nh_.advertise("output/vehicle_engage", 1); + + // tmp + pub_vehicle_command_ = + private_nh_.advertise("output/vehicle_cmd", 1); + pub_raw_vehicle_command_ = + private_nh_.advertise("output/raw_vehicle_cmd", 1); + + // Timer + timer_ = + private_nh_.createTimer(ros::Rate(update_rate_), &AutowareJoyControllerNode::onTimer, this); +} diff --git a/control/autoware_joy_controller/src/autoware_joy_controller/main.cpp b/control/autoware_joy_controller/src/autoware_joy_controller/main.cpp new file mode 100644 index 0000000000000..2c5e549236f76 --- /dev/null +++ b/control/autoware_joy_controller/src/autoware_joy_controller/main.cpp @@ -0,0 +1,30 @@ +/* + * Copyright 2020 Tier IV, Inc. All rights reserved. + * + * 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 + +#include + +int main(int argc, char * argv[]) +{ + ros::init(argc, argv, "autoware_joy_controller"); + + AutowareJoyControllerNode node; + + ros::spin(); + + return 0; +} From 1e8dd6ece23616999011cc390f2ae7de00999d59 Mon Sep 17 00:00:00 2001 From: Fumiya Watanabe Date: Sun, 19 Jul 2020 17:59:19 +0900 Subject: [PATCH 02/38] Support G29 controller in autoware_joy_controller (#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 --- .../autoware_joy_controller.h | 76 +--------------- .../joy_converter/ds4_joy_converter.h | 89 +++++++++++++++++++ .../joy_converter/g29_joy_converter.h | 85 ++++++++++++++++++ .../joy_converter/joy_converter_base.h | 51 +++++++++++ .../launch/autoware_joy_controller.launch | 3 + .../autoware_joy_controller_node.cpp | 11 ++- 6 files changed, 242 insertions(+), 73 deletions(-) create mode 100644 control/autoware_joy_controller/include/autoware_joy_controller/joy_converter/ds4_joy_converter.h create mode 100644 control/autoware_joy_controller/include/autoware_joy_controller/joy_converter/g29_joy_converter.h create mode 100644 control/autoware_joy_controller/include/autoware_joy_controller/joy_converter/joy_converter_base.h diff --git a/control/autoware_joy_controller/include/autoware_joy_controller/autoware_joy_controller.h b/control/autoware_joy_controller/include/autoware_joy_controller/autoware_joy_controller.h index 6ca9d1fca10cd..35ad7697a264a 100644 --- a/control/autoware_joy_controller/include/autoware_joy_controller/autoware_joy_controller.h +++ b/control/autoware_joy_controller/include/autoware_joy_controller/autoware_joy_controller.h @@ -31,81 +31,12 @@ #include #include +#include + // tmp #include #include -class JoyConverter -{ -public: - explicit JoyConverter(const sensor_msgs::Joy & j) : j_(j) {} - - const float accel() const - { - const auto button = static_cast(Cross()); - const auto stick = std::max(0.0f, RStickUpDown()); - const auto trigger = std::max(0.0f, -RTrigger()); - return std::max({button, stick, trigger}); - } - - const float brake() const - { - const auto button = static_cast(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; } - bool shift_down() const { return CursorUpDown() == -1; } - bool shift_drive() const { return CursorLeftRight() == 1; } - bool shift_reverse() const { return CursorLeftRight() == -1; } - - 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() const { return !reverse() && PS(); } - bool clear_emergency() 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); } - -private: - bool reverse() const { return Share(); } - - const sensor_msgs::Joy j_; -}; - using ShiftType = autoware_vehicle_msgs::Shift::_data_type; using TurnSignalType = autoware_vehicle_msgs::TurnSignal::_data_type; using GateModeType = autoware_control_msgs::GateMode::_data_type; @@ -121,6 +52,7 @@ class AutowareJoyControllerNode ros::NodeHandle private_nh_{"~"}; // Parameter + std::string joy_type_; double update_rate_; double accel_ratio_; double brake_ratio_; @@ -138,7 +70,7 @@ class AutowareJoyControllerNode ros::Subscriber sub_twist_; ros::Time last_joy_received_time_; - std::shared_ptr joy_; + std::shared_ptr joy_; geometry_msgs::TwistStamped::ConstPtr twist_; void onJoy(const sensor_msgs::Joy::ConstPtr & msg); diff --git a/control/autoware_joy_controller/include/autoware_joy_controller/joy_converter/ds4_joy_converter.h b/control/autoware_joy_controller/include/autoware_joy_controller/joy_converter/ds4_joy_converter.h new file mode 100644 index 0000000000000..166623d907551 --- /dev/null +++ b/control/autoware_joy_controller/include/autoware_joy_controller/joy_converter/ds4_joy_converter.h @@ -0,0 +1,89 @@ +/* + * Copyright 2020 Tier IV, Inc. All rights reserved. + * + * 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. + */ + +#pragma once + +#include + +class DS4JoyConverter : public JoyConverterBase +{ +public: + explicit DS4JoyConverter(const sensor_msgs::Joy & j) : j_(j) {} + + const float accel() const + { + const auto button = static_cast(Cross()); + const auto stick = std::max(0.0f, RStickUpDown()); + const auto trigger = std::max(0.0f, -RTrigger()); + return std::max({button, stick, trigger}); + } + + const float brake() const + { + const auto button = static_cast(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() const { return !reverse() && PS(); } + bool clear_emergency() 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::Joy j_; + + bool reverse() const { return Share(); } +}; diff --git a/control/autoware_joy_controller/include/autoware_joy_controller/joy_converter/g29_joy_converter.h b/control/autoware_joy_controller/include/autoware_joy_controller/joy_converter/g29_joy_converter.h new file mode 100644 index 0000000000000..d54a3067097fd --- /dev/null +++ b/control/autoware_joy_controller/include/autoware_joy_controller/joy_converter/g29_joy_converter.h @@ -0,0 +1,85 @@ +/* + * Copyright 2020 Tier IV, Inc. All rights reserved. + * + * 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. + */ + +#pragma once + +#include + +class G29JoyConverter : public JoyConverterBase +{ +public: + explicit G29JoyConverter(const sensor_msgs::Joy & j) : j_(j) {} + + const float accel() const + { + constexpr float eps = 0.0000001; + if(std::fabs(AccelPedal()) < eps) return 0.0f; + return (AccelPedal() + 1.0f) / 2; + } + + const float brake() const + { + constexpr float eps = 0.0000001; + if(std::fabs(BrakePedal()) < eps) return 0.0f; + return (BrakePedal() + 1.0f) / 2; + } + + float steer() const { return Steer(); } + + 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() const { return !reverse() && PS(); } + bool clear_emergency() 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 Steer() const { return j_.axes.at(0); } + float AccelPedal() const { return j_.axes.at(2); } + float BrakePedal() const { return j_.axes.at(3); } + + float CursorLeftRight() const { return j_.axes.at(4); } + float CursorUpDown() const { return j_.axes.at(5); } + + 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(24); } + + const sensor_msgs::Joy j_; + + bool reverse() const { return Share(); } +}; diff --git a/control/autoware_joy_controller/include/autoware_joy_controller/joy_converter/joy_converter_base.h b/control/autoware_joy_controller/include/autoware_joy_controller/joy_converter/joy_converter_base.h new file mode 100644 index 0000000000000..3a2aab8d833ff --- /dev/null +++ b/control/autoware_joy_controller/include/autoware_joy_controller/joy_converter/joy_converter_base.h @@ -0,0 +1,51 @@ +/* + * Copyright 2020 Tier IV, Inc. All rights reserved. + * + * 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. + */ + +#pragma once + +#include + +#include + +class JoyConverterBase +{ +public: + virtual const float accel() const = 0; + + virtual const float brake() const = 0; + + virtual float steer() const = 0; + + virtual bool shift_up() const = 0; + virtual bool shift_down() const = 0; + virtual bool shift_drive() const = 0; + virtual bool shift_reverse() const = 0; + + virtual bool turn_signal_left() const = 0; + virtual bool turn_signal_right() const = 0; + virtual bool clear_turn_signal() const = 0; + + virtual bool gate_mode() const = 0; + + virtual bool emergency() const = 0; + virtual bool clear_emergency() const = 0; + + virtual bool autoware_engage() const = 0; + virtual bool autoware_disengage() const = 0; + + virtual bool vehicle_engage() const = 0; + virtual bool vehicle_disengage() const = 0; +}; diff --git a/control/autoware_joy_controller/launch/autoware_joy_controller.launch b/control/autoware_joy_controller/launch/autoware_joy_controller.launch index 843da5465096a..d582b1ec2fe46 100644 --- a/control/autoware_joy_controller/launch/autoware_joy_controller.launch +++ b/control/autoware_joy_controller/launch/autoware_joy_controller.launch @@ -1,4 +1,6 @@ + + @@ -32,6 +34,7 @@ + diff --git a/control/autoware_joy_controller/src/autoware_joy_controller/autoware_joy_controller_node.cpp b/control/autoware_joy_controller/src/autoware_joy_controller/autoware_joy_controller_node.cpp index 057274bddf553..7a7fe7a0d4de6 100644 --- a/control/autoware_joy_controller/src/autoware_joy_controller/autoware_joy_controller_node.cpp +++ b/control/autoware_joy_controller/src/autoware_joy_controller/autoware_joy_controller_node.cpp @@ -15,6 +15,8 @@ */ #include +#include +#include namespace { @@ -87,7 +89,11 @@ const char * getGateModeName(const GateModeType & gate_mode) void AutowareJoyControllerNode::onJoy(const sensor_msgs::Joy::ConstPtr & msg) { last_joy_received_time_ = msg->header.stamp; - joy_ = std::make_shared(*msg); + if (joy_type_ == "G29") { + joy_ = std::make_shared(*msg); + } else { + joy_ = std::make_shared(*msg); + } if (joy_->shift_up() || joy_->shift_down() || joy_->shift_drive() || joy_->shift_reverse()) { publishShift(); @@ -371,6 +377,7 @@ void AutowareJoyControllerNode::publishRawVehicleCommand() AutowareJoyControllerNode::AutowareJoyControllerNode() { // Parameter + private_nh_.param("joy_type", joy_type_, "DS4"); private_nh_.param("update_rate", update_rate_, 10.0); private_nh_.param("accel_ratio", accel_ratio_, 3.0); private_nh_.param("brake_ratio", brake_ratio_, 5.0); @@ -381,6 +388,8 @@ AutowareJoyControllerNode::AutowareJoyControllerNode() private_nh_.param("control_command/max_backward_velocity", max_backward_velocity_, 3.0); private_nh_.param("control_command/backward_accel_ratio", backward_accel_ratio_, 1.0); + ROS_INFO("Joy type: %s", joy_type_.c_str()); + // Subscriber sub_joy_ = private_nh_.subscribe("input/joy", 1, &AutowareJoyControllerNode::onJoy, this); sub_twist_ = private_nh_.subscribe("input/twist", 1, &AutowareJoyControllerNode::onTwist, this); From 491c0ce2373280014ca81c818c23368f28b86a7d Mon Sep 17 00:00:00 2001 From: Fumiya Watanabe Date: Thu, 6 Aug 2020 16:23:12 +0900 Subject: [PATCH 03/38] Change key map for G29 controller and set deadzone parameter (#740) --- .../joy_converter/g29_joy_converter.h | 8 ++++---- .../launch/autoware_joy_controller.launch | 1 + 2 files changed, 5 insertions(+), 4 deletions(-) diff --git a/control/autoware_joy_controller/include/autoware_joy_controller/joy_converter/g29_joy_converter.h b/control/autoware_joy_controller/include/autoware_joy_controller/joy_converter/g29_joy_converter.h index d54a3067097fd..9861a643fb734 100644 --- a/control/autoware_joy_controller/include/autoware_joy_controller/joy_converter/g29_joy_converter.h +++ b/control/autoware_joy_controller/include/autoware_joy_controller/joy_converter/g29_joy_converter.h @@ -71,10 +71,10 @@ class G29JoyConverter : public JoyConverterBase 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 L1() const { return j_.buttons.at(5); } + bool R1() const { return j_.buttons.at(4); } + bool L2() const { return j_.buttons.at(7); } + bool R2() const { return j_.buttons.at(6); } bool Share() const { return j_.buttons.at(8); } bool Options() const { return j_.buttons.at(9); } bool PS() const { return j_.buttons.at(24); } diff --git a/control/autoware_joy_controller/launch/autoware_joy_controller.launch b/control/autoware_joy_controller/launch/autoware_joy_controller.launch index d582b1ec2fe46..9818e14c57a6d 100644 --- a/control/autoware_joy_controller/launch/autoware_joy_controller.launch +++ b/control/autoware_joy_controller/launch/autoware_joy_controller.launch @@ -39,5 +39,6 @@ + From af3a2310bddbcdc18c0e138bfaf0208d41df26be Mon Sep 17 00:00:00 2001 From: Kenji Miyake <31987104+kenji-miyake@users.noreply.github.com> Date: Wed, 12 Aug 2020 12:30:35 +0900 Subject: [PATCH 04/38] Add missing dependencies of autoware_joy_controller (#755) Signed-off-by: Kenji Miyake --- control/autoware_joy_controller/CMakeLists.txt | 1 + control/autoware_joy_controller/package.xml | 1 + 2 files changed, 2 insertions(+) diff --git a/control/autoware_joy_controller/CMakeLists.txt b/control/autoware_joy_controller/CMakeLists.txt index f0d8844959ac0..5f13f9a27a1d7 100644 --- a/control/autoware_joy_controller/CMakeLists.txt +++ b/control/autoware_joy_controller/CMakeLists.txt @@ -8,6 +8,7 @@ find_package(catkin REQUIRED COMPONENTS autoware_vehicle_msgs joy geometry_msgs + roscpp sensor_msgs std_msgs ) diff --git a/control/autoware_joy_controller/package.xml b/control/autoware_joy_controller/package.xml index 0657eb663f726..620f50dbdd515 100644 --- a/control/autoware_joy_controller/package.xml +++ b/control/autoware_joy_controller/package.xml @@ -12,6 +12,7 @@ autoware_vehicle_msgs joy geometry_msgs + roscpp sensor_msgs std_msgs From 41601eb9692cb716726a2de06bba38f3ec37f9f9 Mon Sep 17 00:00:00 2001 From: mitsudome-r Date: Tue, 29 Sep 2020 19:22:18 +0900 Subject: [PATCH 05/38] remove ROS1 packages temporarily Signed-off-by: mitsudome-r --- .../autoware_joy_controller/CMakeLists.txt | 56 --- .../config/autoware_joy_controller.yaml | 10 - .../autoware_joy_controller.h | 116 ----- .../joy_converter/ds4_joy_converter.h | 89 ---- .../joy_converter/g29_joy_converter.h | 85 ---- .../joy_converter/joy_converter_base.h | 51 --- .../launch/autoware_joy_controller.launch | 44 -- control/autoware_joy_controller/package.xml | 21 - .../autoware_joy_controller_node.cpp | 419 ------------------ .../src/autoware_joy_controller/main.cpp | 30 -- 10 files changed, 921 deletions(-) delete mode 100644 control/autoware_joy_controller/CMakeLists.txt delete mode 100644 control/autoware_joy_controller/config/autoware_joy_controller.yaml delete mode 100644 control/autoware_joy_controller/include/autoware_joy_controller/autoware_joy_controller.h delete mode 100644 control/autoware_joy_controller/include/autoware_joy_controller/joy_converter/ds4_joy_converter.h delete mode 100644 control/autoware_joy_controller/include/autoware_joy_controller/joy_converter/g29_joy_converter.h delete mode 100644 control/autoware_joy_controller/include/autoware_joy_controller/joy_converter/joy_converter_base.h delete mode 100644 control/autoware_joy_controller/launch/autoware_joy_controller.launch delete mode 100644 control/autoware_joy_controller/package.xml delete mode 100644 control/autoware_joy_controller/src/autoware_joy_controller/autoware_joy_controller_node.cpp delete mode 100644 control/autoware_joy_controller/src/autoware_joy_controller/main.cpp diff --git a/control/autoware_joy_controller/CMakeLists.txt b/control/autoware_joy_controller/CMakeLists.txt deleted file mode 100644 index 5f13f9a27a1d7..0000000000000 --- a/control/autoware_joy_controller/CMakeLists.txt +++ /dev/null @@ -1,56 +0,0 @@ -cmake_minimum_required(VERSION 3.0.2) -project(autoware_joy_controller) - -add_compile_options(-std=c++14) - -find_package(catkin REQUIRED COMPONENTS - autoware_control_msgs - autoware_vehicle_msgs - joy - geometry_msgs - roscpp - sensor_msgs - std_msgs -) - -catkin_package() - -include_directories( - include - ${catkin_INCLUDE_DIRS} -) - -add_executable(autoware_joy_controller - src/autoware_joy_controller/main.cpp - src/autoware_joy_controller/autoware_joy_controller_node.cpp -) - -target_link_libraries(autoware_joy_controller - ${catkin_LIBRARIES} -) - -add_dependencies(autoware_joy_controller - ${catkin_EXPORTED_TARGETS} -) - -# Install executables and/or libraries -install( - TARGETS - autoware_joy_controller - ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} - LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} - RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} -) - -# Install project namespaced headers -install(DIRECTORY include/${PROJECT_NAME}/ - DESTINATION ${CATKIN_PACKAGE_INCLUDE_DESTINATION} -) - -# Install directories -install( - DIRECTORY - launch - config - DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION} -) diff --git a/control/autoware_joy_controller/config/autoware_joy_controller.yaml b/control/autoware_joy_controller/config/autoware_joy_controller.yaml deleted file mode 100644 index f7ee5a5c4e10e..0000000000000 --- a/control/autoware_joy_controller/config/autoware_joy_controller.yaml +++ /dev/null @@ -1,10 +0,0 @@ -update_rate: 10 -accel_ratio: 3.0 -brake_ratio: 5.0 -steer_ratio: 0.5 -steering_angle_velocity: 0.1 -control_command: - velocity_gain: 3.0 - max_forward_velocity: 20 - max_backward_velocity: 3 - backward_accel_ratio: 1.0 diff --git a/control/autoware_joy_controller/include/autoware_joy_controller/autoware_joy_controller.h b/control/autoware_joy_controller/include/autoware_joy_controller/autoware_joy_controller.h deleted file mode 100644 index 35ad7697a264a..0000000000000 --- a/control/autoware_joy_controller/include/autoware_joy_controller/autoware_joy_controller.h +++ /dev/null @@ -1,116 +0,0 @@ -/* - * Copyright 2020 Tier IV, Inc. All rights reserved. - * - * 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. - */ - -#pragma once - -#include -#include -#include - -#include - -#include -#include -#include -#include -#include -#include -#include -#include - -#include - -// tmp -#include -#include - -using ShiftType = autoware_vehicle_msgs::Shift::_data_type; -using TurnSignalType = autoware_vehicle_msgs::TurnSignal::_data_type; -using GateModeType = autoware_control_msgs::GateMode::_data_type; - -class AutowareJoyControllerNode -{ -public: - AutowareJoyControllerNode(); - -private: - // NodeHandle - ros::NodeHandle nh_{""}; - ros::NodeHandle private_nh_{"~"}; - - // Parameter - std::string joy_type_; - double update_rate_; - double accel_ratio_; - double brake_ratio_; - double steer_ratio_; - double steering_angle_velocity_; - - // ControlCommand Parameter - double velocity_gain_; - double max_forward_velocity_; - double max_backward_velocity_; - double backward_accel_ratio_; - - // Subscriber - ros::Subscriber sub_joy_; - ros::Subscriber sub_twist_; - - ros::Time last_joy_received_time_; - std::shared_ptr joy_; - geometry_msgs::TwistStamped::ConstPtr twist_; - - void onJoy(const sensor_msgs::Joy::ConstPtr & msg); - void onTwist(const geometry_msgs::TwistStamped::ConstPtr & msg); - - // Publisher - ros::Publisher pub_control_command_; - ros::Publisher pub_raw_control_command_; - ros::Publisher pub_shift_; - ros::Publisher pub_turn_signal_; - ros::Publisher pub_gate_mode_; - ros::Publisher pub_emergency_; - ros::Publisher pub_autoware_engage_; - ros::Publisher pub_vehicle_engage_; - - void publishControlCommand(); - void publishRawControlCommand(); - void publishShift(); - void publishTurnSignal(); - void publishGateMode(); - void publishEmergency(); - void publishAutowareEngage(); - void publishVehicleEngage(); - - // Previous State - autoware_control_msgs::ControlCommand prev_control_command_; - autoware_vehicle_msgs::RawControlCommand prev_raw_control_command_; - ShiftType prev_shift_ = autoware_vehicle_msgs::Shift::NONE; - TurnSignalType prev_turn_signal_ = autoware_vehicle_msgs::TurnSignal::NONE; - GateModeType prev_gate_mode_ = autoware_control_msgs::GateMode::AUTO; - - // tmp - ros::Publisher pub_vehicle_command_; - ros::Publisher pub_raw_vehicle_command_; - void publishVehicleCommand(); - void publishRawVehicleCommand(); - - // Timer - ros::Timer timer_; - - bool isDataReady(); - void onTimer(const ros::TimerEvent & event); -}; diff --git a/control/autoware_joy_controller/include/autoware_joy_controller/joy_converter/ds4_joy_converter.h b/control/autoware_joy_controller/include/autoware_joy_controller/joy_converter/ds4_joy_converter.h deleted file mode 100644 index 166623d907551..0000000000000 --- a/control/autoware_joy_controller/include/autoware_joy_controller/joy_converter/ds4_joy_converter.h +++ /dev/null @@ -1,89 +0,0 @@ -/* - * Copyright 2020 Tier IV, Inc. All rights reserved. - * - * 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. - */ - -#pragma once - -#include - -class DS4JoyConverter : public JoyConverterBase -{ -public: - explicit DS4JoyConverter(const sensor_msgs::Joy & j) : j_(j) {} - - const float accel() const - { - const auto button = static_cast(Cross()); - const auto stick = std::max(0.0f, RStickUpDown()); - const auto trigger = std::max(0.0f, -RTrigger()); - return std::max({button, stick, trigger}); - } - - const float brake() const - { - const auto button = static_cast(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() const { return !reverse() && PS(); } - bool clear_emergency() 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::Joy j_; - - bool reverse() const { return Share(); } -}; diff --git a/control/autoware_joy_controller/include/autoware_joy_controller/joy_converter/g29_joy_converter.h b/control/autoware_joy_controller/include/autoware_joy_controller/joy_converter/g29_joy_converter.h deleted file mode 100644 index 9861a643fb734..0000000000000 --- a/control/autoware_joy_controller/include/autoware_joy_controller/joy_converter/g29_joy_converter.h +++ /dev/null @@ -1,85 +0,0 @@ -/* - * Copyright 2020 Tier IV, Inc. All rights reserved. - * - * 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. - */ - -#pragma once - -#include - -class G29JoyConverter : public JoyConverterBase -{ -public: - explicit G29JoyConverter(const sensor_msgs::Joy & j) : j_(j) {} - - const float accel() const - { - constexpr float eps = 0.0000001; - if(std::fabs(AccelPedal()) < eps) return 0.0f; - return (AccelPedal() + 1.0f) / 2; - } - - const float brake() const - { - constexpr float eps = 0.0000001; - if(std::fabs(BrakePedal()) < eps) return 0.0f; - return (BrakePedal() + 1.0f) / 2; - } - - float steer() const { return Steer(); } - - 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() const { return !reverse() && PS(); } - bool clear_emergency() 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 Steer() const { return j_.axes.at(0); } - float AccelPedal() const { return j_.axes.at(2); } - float BrakePedal() const { return j_.axes.at(3); } - - float CursorLeftRight() const { return j_.axes.at(4); } - float CursorUpDown() const { return j_.axes.at(5); } - - 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(5); } - bool R1() const { return j_.buttons.at(4); } - bool L2() const { return j_.buttons.at(7); } - bool R2() const { return j_.buttons.at(6); } - bool Share() const { return j_.buttons.at(8); } - bool Options() const { return j_.buttons.at(9); } - bool PS() const { return j_.buttons.at(24); } - - const sensor_msgs::Joy j_; - - bool reverse() const { return Share(); } -}; diff --git a/control/autoware_joy_controller/include/autoware_joy_controller/joy_converter/joy_converter_base.h b/control/autoware_joy_controller/include/autoware_joy_controller/joy_converter/joy_converter_base.h deleted file mode 100644 index 3a2aab8d833ff..0000000000000 --- a/control/autoware_joy_controller/include/autoware_joy_controller/joy_converter/joy_converter_base.h +++ /dev/null @@ -1,51 +0,0 @@ -/* - * Copyright 2020 Tier IV, Inc. All rights reserved. - * - * 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. - */ - -#pragma once - -#include - -#include - -class JoyConverterBase -{ -public: - virtual const float accel() const = 0; - - virtual const float brake() const = 0; - - virtual float steer() const = 0; - - virtual bool shift_up() const = 0; - virtual bool shift_down() const = 0; - virtual bool shift_drive() const = 0; - virtual bool shift_reverse() const = 0; - - virtual bool turn_signal_left() const = 0; - virtual bool turn_signal_right() const = 0; - virtual bool clear_turn_signal() const = 0; - - virtual bool gate_mode() const = 0; - - virtual bool emergency() const = 0; - virtual bool clear_emergency() const = 0; - - virtual bool autoware_engage() const = 0; - virtual bool autoware_disengage() const = 0; - - virtual bool vehicle_engage() const = 0; - virtual bool vehicle_disengage() const = 0; -}; diff --git a/control/autoware_joy_controller/launch/autoware_joy_controller.launch b/control/autoware_joy_controller/launch/autoware_joy_controller.launch deleted file mode 100644 index 9818e14c57a6d..0000000000000 --- a/control/autoware_joy_controller/launch/autoware_joy_controller.launch +++ /dev/null @@ -1,44 +0,0 @@ - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - diff --git a/control/autoware_joy_controller/package.xml b/control/autoware_joy_controller/package.xml deleted file mode 100644 index 620f50dbdd515..0000000000000 --- a/control/autoware_joy_controller/package.xml +++ /dev/null @@ -1,21 +0,0 @@ - - - autoware_joy_controller - 0.1.0 - The autoware_joy_controller package - Kenji Miyake - Apache 2 - - catkin - - autoware_control_msgs - autoware_vehicle_msgs - joy - geometry_msgs - roscpp - sensor_msgs - std_msgs - - - - diff --git a/control/autoware_joy_controller/src/autoware_joy_controller/autoware_joy_controller_node.cpp b/control/autoware_joy_controller/src/autoware_joy_controller/autoware_joy_controller_node.cpp deleted file mode 100644 index 7a7fe7a0d4de6..0000000000000 --- a/control/autoware_joy_controller/src/autoware_joy_controller/autoware_joy_controller_node.cpp +++ /dev/null @@ -1,419 +0,0 @@ -/* - * Copyright 2020 Tier IV, Inc. All rights reserved. - * - * 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 -#include -#include - -namespace -{ -ShiftType getUpperShift(const ShiftType & shift) -{ - using autoware_vehicle_msgs::Shift; - - if (shift == Shift::NONE) return Shift::PARKING; - if (shift == Shift::PARKING) return Shift::REVERSE; - if (shift == Shift::REVERSE) return Shift::NEUTRAL; - if (shift == Shift::NEUTRAL) return Shift::DRIVE; - if (shift == Shift::DRIVE) return Shift::LOW; - if (shift == Shift::LOW) return Shift::LOW; - - return Shift::NONE; -} - -ShiftType getLowerShift(const ShiftType & shift) -{ - using autoware_vehicle_msgs::Shift; - - if (shift == Shift::NONE) return Shift::PARKING; - if (shift == Shift::PARKING) return Shift::PARKING; - if (shift == Shift::REVERSE) return Shift::PARKING; - if (shift == Shift::NEUTRAL) return Shift::REVERSE; - if (shift == Shift::DRIVE) return Shift::NEUTRAL; - if (shift == Shift::LOW) return Shift::DRIVE; - - return Shift::NONE; -} - -const char * getShiftName(const ShiftType & shift) -{ - using autoware_vehicle_msgs::Shift; - - if (shift == Shift::NONE) return "NONE"; - if (shift == Shift::PARKING) return "PARKING"; - if (shift == Shift::REVERSE) return "REVERSE"; - if (shift == Shift::NEUTRAL) return "NEUTRAL"; - if (shift == Shift::DRIVE) return "DRIVE"; - if (shift == Shift::LOW) return "LOW"; - - return "NOT_SUPPORTED"; -} - -const char * getTurnSignalName(const TurnSignalType & turn_signal) -{ - using autoware_vehicle_msgs::TurnSignal; - - if (turn_signal == TurnSignal::NONE) return "NONE"; - if (turn_signal == TurnSignal::LEFT) return "LEFT"; - if (turn_signal == TurnSignal::RIGHT) return "RIGHT"; - if (turn_signal == TurnSignal::HAZARD) return "HAZARD"; - - return "NOT_SUPPORTED"; -} - -const char * getGateModeName(const GateModeType & gate_mode) -{ - using autoware_control_msgs::GateMode; - - if (gate_mode == GateMode::AUTO) return "AUTO"; - if (gate_mode == GateMode::REMOTE) return "REMOTE"; - - return "NOT_SUPPORTED"; -} - -} // namespace - -void AutowareJoyControllerNode::onJoy(const sensor_msgs::Joy::ConstPtr & msg) -{ - last_joy_received_time_ = msg->header.stamp; - if (joy_type_ == "G29") { - joy_ = std::make_shared(*msg); - } else { - joy_ = std::make_shared(*msg); - } - - if (joy_->shift_up() || joy_->shift_down() || joy_->shift_drive() || joy_->shift_reverse()) { - publishShift(); - } - - if (joy_->turn_signal_left() || joy_->turn_signal_right() || joy_->clear_turn_signal()) { - publishTurnSignal(); - } - - if (joy_->gate_mode()) { - publishGateMode(); - } - - if (joy_->emergency() || joy_->clear_emergency()) { - publishEmergency(); - } - - if (joy_->autoware_engage() || joy_->autoware_disengage()) { - publishAutowareEngage(); - } - - if (joy_->vehicle_engage() || joy_->vehicle_disengage()) { - publishVehicleEngage(); - } -} - -void AutowareJoyControllerNode::onTwist(const geometry_msgs::TwistStamped::ConstPtr & msg) -{ - twist_ = msg; -} - -bool AutowareJoyControllerNode::isDataReady() -{ - // Joy - { - if (!joy_) { - ROS_WARN_THROTTLE(1.0, "waiting for joy msg..."); - return false; - } - - constexpr auto timeout = 2.0; - const auto time_diff = ros::Time::now() - last_joy_received_time_; - if (time_diff.toSec() > timeout) { - ROS_WARN_THROTTLE(1.0, "joy msg is timeout"); - return false; - } - } - - // Twist - { - if (!twist_) { - ROS_WARN_THROTTLE(1.0, "waiting for twist msg..."); - return false; - } - - constexpr auto timeout = 0.5; - const auto time_diff = ros::Time::now() - twist_->header.stamp; - if (time_diff.toSec() > timeout) { - ROS_WARN_THROTTLE(1.0, "twist msg is timeout"); - return false; - } - } - - return true; -} - -void AutowareJoyControllerNode::onTimer(const ros::TimerEvent & event) -{ - if (!isDataReady()) { - return; - } - - publishControlCommand(); - publishRawControlCommand(); - - // tmp - publishVehicleCommand(); - publishRawVehicleCommand(); -} - -void AutowareJoyControllerNode::publishControlCommand() -{ - autoware_control_msgs::ControlCommandStamped cmd_stamped; - cmd_stamped.header.stamp = ros::Time::now(); - - { - auto & cmd = cmd_stamped.control; - - cmd.steering_angle = steer_ratio_ * joy_->steer(); - cmd.steering_angle_velocity = steering_angle_velocity_; - - if (joy_->accel()) { - cmd.acceleration = accel_ratio_ * joy_->accel(); - cmd.velocity = twist_->twist.linear.x + velocity_gain_ * cmd.acceleration; - cmd.velocity = std::min(cmd.velocity, max_forward_velocity_); - } - - if (joy_->brake()) { - cmd.velocity = 0.0; - cmd.acceleration = -brake_ratio_ * joy_->brake(); - } - - // Backward - if (joy_->accel() && joy_->brake()) { - cmd.acceleration = backward_accel_ratio_ * joy_->accel(); - cmd.velocity = twist_->twist.linear.x - velocity_gain_ * cmd.acceleration; - cmd.velocity = std::max(cmd.velocity, -max_backward_velocity_); - } - } - - pub_control_command_.publish(cmd_stamped); - prev_control_command_ = cmd_stamped.control; -} - -void AutowareJoyControllerNode::publishRawControlCommand() -{ - autoware_vehicle_msgs::RawControlCommandStamped cmd_stamped; - cmd_stamped.header.stamp = ros::Time::now(); - - { - auto & cmd = cmd_stamped.control; - - cmd.steering_angle = steer_ratio_ * joy_->steer(); - cmd.steering_angle_velocity = steering_angle_velocity_; - cmd.throttle = accel_ratio_ * joy_->accel(); - cmd.brake = brake_ratio_ * joy_->brake(); - } - - pub_raw_control_command_.publish(cmd_stamped); - prev_raw_control_command_ = cmd_stamped.control; -} - -void AutowareJoyControllerNode::publishShift() -{ - using autoware_vehicle_msgs::Shift; - - autoware_vehicle_msgs::ShiftStamped shift_stamped; - shift_stamped.header.stamp = ros::Time::now(); - - { - auto & shift = shift_stamped.shift; - if (joy_->shift_up()) { - shift.data = getUpperShift(prev_shift_); - } - - if (joy_->shift_down()) { - shift.data = getLowerShift(prev_shift_); - } - - if (joy_->shift_drive()) { - shift.data = Shift::DRIVE; - } - - if (joy_->shift_reverse()) { - shift.data = Shift::REVERSE; - } - - ROS_INFO("Shift::%s", getShiftName(shift.data)); - } - - pub_shift_.publish(shift_stamped); - prev_shift_ = shift_stamped.shift.data; -} - -void AutowareJoyControllerNode::publishTurnSignal() -{ - using autoware_vehicle_msgs::TurnSignal; - - TurnSignal turn_signal; - turn_signal.header.stamp = ros::Time::now(); - - if (joy_->turn_signal_left() && joy_->turn_signal_right()) { - turn_signal.data = TurnSignal::HAZARD; - } else if (joy_->turn_signal_left()) { - turn_signal.data = TurnSignal::LEFT; - } else if (joy_->turn_signal_right()) { - turn_signal.data = TurnSignal::RIGHT; - } - - if (joy_->clear_turn_signal()) { - turn_signal.data = TurnSignal::NONE; - } - - ROS_INFO("TurnSignal::%s", getTurnSignalName(turn_signal.data)); - - pub_turn_signal_.publish(turn_signal); -} - -void AutowareJoyControllerNode::publishGateMode() -{ - using autoware_control_msgs::GateMode; - - autoware_control_msgs::GateMode gate_mode; - - if (prev_gate_mode_ == GateMode::AUTO) { - gate_mode.data = GateMode::REMOTE; - } - - if (prev_gate_mode_ == GateMode::REMOTE) { - gate_mode.data = GateMode::AUTO; - } - - ROS_INFO("GateMode::%s", getGateModeName(gate_mode.data)); - - pub_gate_mode_.publish(gate_mode); - prev_gate_mode_ = gate_mode.data; -} - -void AutowareJoyControllerNode::publishEmergency() -{ - std_msgs::Bool emergency; - - if (joy_->emergency()) { - emergency.data = true; - ROS_INFO("Emergency"); - } - - if (joy_->clear_emergency()) { - emergency.data = false; - ROS_INFO("Clear Emergency"); - } - - pub_emergency_.publish(emergency); -} - -void AutowareJoyControllerNode::publishAutowareEngage() -{ - std_msgs::Bool engage; - - if (joy_->autoware_engage()) { - engage.data = true; - ROS_INFO("Autoware Engage"); - } - - if (joy_->autoware_disengage()) { - engage.data = false; - ROS_INFO("Autoware Disengage"); - } - - pub_autoware_engage_.publish(engage); -} - -void AutowareJoyControllerNode::publishVehicleEngage() -{ - std_msgs::Bool engage; - - if (joy_->vehicle_engage()) { - engage.data = true; - ROS_INFO("Vehicle Engage"); - } - - if (joy_->vehicle_disengage()) { - engage.data = false; - ROS_INFO("Vehicle Disengage"); - } - - pub_vehicle_engage_.publish(engage); -} - -// tmp -void AutowareJoyControllerNode::publishVehicleCommand() -{ - autoware_vehicle_msgs::VehicleCommand vehicle_cmd; - vehicle_cmd.header.stamp = ros::Time::now(); - vehicle_cmd.control = prev_control_command_; - vehicle_cmd.shift.data = prev_shift_; - - pub_vehicle_command_.publish(vehicle_cmd); -} - -void AutowareJoyControllerNode::publishRawVehicleCommand() -{ - autoware_vehicle_msgs::RawVehicleCommand vehicle_cmd; - vehicle_cmd.header.stamp = ros::Time::now(); - vehicle_cmd.control = prev_raw_control_command_; - vehicle_cmd.shift.data = prev_shift_; - - pub_raw_vehicle_command_.publish(vehicle_cmd); -} - -AutowareJoyControllerNode::AutowareJoyControllerNode() -{ - // Parameter - private_nh_.param("joy_type", joy_type_, "DS4"); - private_nh_.param("update_rate", update_rate_, 10.0); - private_nh_.param("accel_ratio", accel_ratio_, 3.0); - private_nh_.param("brake_ratio", brake_ratio_, 5.0); - private_nh_.param("steer_ratio", steer_ratio_, 0.5); - private_nh_.param("steering_angle_velocity", steering_angle_velocity_, 0.1); - private_nh_.param("control_command/velocity_gain", velocity_gain_, 3.0); - private_nh_.param("control_command/max_forward_velocity", max_forward_velocity_, 20.0); - private_nh_.param("control_command/max_backward_velocity", max_backward_velocity_, 3.0); - private_nh_.param("control_command/backward_accel_ratio", backward_accel_ratio_, 1.0); - - ROS_INFO("Joy type: %s", joy_type_.c_str()); - - // Subscriber - sub_joy_ = private_nh_.subscribe("input/joy", 1, &AutowareJoyControllerNode::onJoy, this); - sub_twist_ = private_nh_.subscribe("input/twist", 1, &AutowareJoyControllerNode::onTwist, this); - - // Publisher - pub_control_command_ = private_nh_.advertise( - "output/control_command", 1); - pub_raw_control_command_ = private_nh_.advertise( - "output/raw_control_command", 1); - pub_shift_ = private_nh_.advertise("output/shift", 1); - pub_turn_signal_ = - private_nh_.advertise("output/turn_signal", 1); - pub_gate_mode_ = private_nh_.advertise("output/gate_mode", 1); - pub_emergency_ = private_nh_.advertise("output/emergency", 1); - pub_autoware_engage_ = private_nh_.advertise("output/autoware_engage", 1); - pub_vehicle_engage_ = private_nh_.advertise("output/vehicle_engage", 1); - - // tmp - pub_vehicle_command_ = - private_nh_.advertise("output/vehicle_cmd", 1); - pub_raw_vehicle_command_ = - private_nh_.advertise("output/raw_vehicle_cmd", 1); - - // Timer - timer_ = - private_nh_.createTimer(ros::Rate(update_rate_), &AutowareJoyControllerNode::onTimer, this); -} diff --git a/control/autoware_joy_controller/src/autoware_joy_controller/main.cpp b/control/autoware_joy_controller/src/autoware_joy_controller/main.cpp deleted file mode 100644 index 2c5e549236f76..0000000000000 --- a/control/autoware_joy_controller/src/autoware_joy_controller/main.cpp +++ /dev/null @@ -1,30 +0,0 @@ -/* - * Copyright 2020 Tier IV, Inc. All rights reserved. - * - * 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 - -#include - -int main(int argc, char * argv[]) -{ - ros::init(argc, argv, "autoware_joy_controller"); - - AutowareJoyControllerNode node; - - ros::spin(); - - return 0; -} From b657d55829db5fec91c1848fb04c6c2d67c4b19f Mon Sep 17 00:00:00 2001 From: mitsudome-r Date: Tue, 29 Sep 2020 19:22:55 +0900 Subject: [PATCH 06/38] add sample ros2 packages Signed-off-by: mitsudome-r --- .../autoware_joy_controller/CMakeLists.txt | 56 +++ control/autoware_joy_controller/COLCON_IGNORE | 0 .../config/autoware_joy_controller.yaml | 10 + .../autoware_joy_controller.h | 116 +++++ .../joy_converter/ds4_joy_converter.h | 89 ++++ .../joy_converter/g29_joy_converter.h | 85 ++++ .../joy_converter/joy_converter_base.h | 51 +++ .../launch/autoware_joy_controller.launch | 44 ++ control/autoware_joy_controller/package.xml | 21 + .../autoware_joy_controller_node.cpp | 419 ++++++++++++++++++ .../src/autoware_joy_controller/main.cpp | 30 ++ 11 files changed, 921 insertions(+) create mode 100644 control/autoware_joy_controller/CMakeLists.txt create mode 100644 control/autoware_joy_controller/COLCON_IGNORE create mode 100644 control/autoware_joy_controller/config/autoware_joy_controller.yaml create mode 100644 control/autoware_joy_controller/include/autoware_joy_controller/autoware_joy_controller.h create mode 100644 control/autoware_joy_controller/include/autoware_joy_controller/joy_converter/ds4_joy_converter.h create mode 100644 control/autoware_joy_controller/include/autoware_joy_controller/joy_converter/g29_joy_converter.h create mode 100644 control/autoware_joy_controller/include/autoware_joy_controller/joy_converter/joy_converter_base.h create mode 100644 control/autoware_joy_controller/launch/autoware_joy_controller.launch create mode 100644 control/autoware_joy_controller/package.xml create mode 100644 control/autoware_joy_controller/src/autoware_joy_controller/autoware_joy_controller_node.cpp create mode 100644 control/autoware_joy_controller/src/autoware_joy_controller/main.cpp diff --git a/control/autoware_joy_controller/CMakeLists.txt b/control/autoware_joy_controller/CMakeLists.txt new file mode 100644 index 0000000000000..5f13f9a27a1d7 --- /dev/null +++ b/control/autoware_joy_controller/CMakeLists.txt @@ -0,0 +1,56 @@ +cmake_minimum_required(VERSION 3.0.2) +project(autoware_joy_controller) + +add_compile_options(-std=c++14) + +find_package(catkin REQUIRED COMPONENTS + autoware_control_msgs + autoware_vehicle_msgs + joy + geometry_msgs + roscpp + sensor_msgs + std_msgs +) + +catkin_package() + +include_directories( + include + ${catkin_INCLUDE_DIRS} +) + +add_executable(autoware_joy_controller + src/autoware_joy_controller/main.cpp + src/autoware_joy_controller/autoware_joy_controller_node.cpp +) + +target_link_libraries(autoware_joy_controller + ${catkin_LIBRARIES} +) + +add_dependencies(autoware_joy_controller + ${catkin_EXPORTED_TARGETS} +) + +# Install executables and/or libraries +install( + TARGETS + autoware_joy_controller + ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} + LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} + RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} +) + +# Install project namespaced headers +install(DIRECTORY include/${PROJECT_NAME}/ + DESTINATION ${CATKIN_PACKAGE_INCLUDE_DESTINATION} +) + +# Install directories +install( + DIRECTORY + launch + config + DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION} +) diff --git a/control/autoware_joy_controller/COLCON_IGNORE b/control/autoware_joy_controller/COLCON_IGNORE new file mode 100644 index 0000000000000..e69de29bb2d1d diff --git a/control/autoware_joy_controller/config/autoware_joy_controller.yaml b/control/autoware_joy_controller/config/autoware_joy_controller.yaml new file mode 100644 index 0000000000000..f7ee5a5c4e10e --- /dev/null +++ b/control/autoware_joy_controller/config/autoware_joy_controller.yaml @@ -0,0 +1,10 @@ +update_rate: 10 +accel_ratio: 3.0 +brake_ratio: 5.0 +steer_ratio: 0.5 +steering_angle_velocity: 0.1 +control_command: + velocity_gain: 3.0 + max_forward_velocity: 20 + max_backward_velocity: 3 + backward_accel_ratio: 1.0 diff --git a/control/autoware_joy_controller/include/autoware_joy_controller/autoware_joy_controller.h b/control/autoware_joy_controller/include/autoware_joy_controller/autoware_joy_controller.h new file mode 100644 index 0000000000000..8aef6ccfd32f1 --- /dev/null +++ b/control/autoware_joy_controller/include/autoware_joy_controller/autoware_joy_controller.h @@ -0,0 +1,116 @@ +/* + * Copyright 2020 Tier IV, Inc. All rights reserved. + * + * 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. + */ + +#pragma once + +#include +#include +#include + +#include + +#include +#include +#include +#include +#include +#include +#include +#include + +#include + +// tmp +#include +#include + +using ShiftType = autoware_vehicle_msgs::Shift::_data_type; +using TurnSignalType = autoware_vehicle_msgs::TurnSignal::_data_type; +using GateModeType = autoware_control_msgs::GateMode::_data_type; + +class AutowareJoyControllerNode +{ +public: + AutowareJoyControllerNode(); + +private: + // NodeHandle + ros::NodeHandle nh_{""}; + ros::NodeHandle private_nh_{"~"}; + + // Parameter + std::string joy_type_; + double update_rate_; + double accel_ratio_; + double brake_ratio_; + double steer_ratio_; + double steering_angle_velocity_; + + // ControlCommand Parameter + double velocity_gain_; + double max_forward_velocity_; + double max_backward_velocity_; + double backward_accel_ratio_; + + // Subscriber + ros::Subscriber sub_joy_; + ros::Subscriber sub_twist_; + + ros::Time last_joy_received_time_; + std::shared_ptr joy_; + geometry_msgs::TwistStamped::ConstPtr twist_; + + void onJoy(const sensor_msgs::Joy::ConstPtr & msg); + void onTwist(const geometry_msgs::TwistStamped::ConstPtr & msg); + + // Publisher + ros::Publisher pub_control_command_; + ros::Publisher pub_raw_control_command_; + ros::Publisher pub_shift_; + ros::Publisher pub_turn_signal_; + ros::Publisher pub_gate_mode_; + ros::Publisher pub_emergency_; + ros::Publisher pub_autoware_engage_; + ros::Publisher pub_vehicle_engage_; + + void publishControlCommand(); + void publishRawControlCommand(); + void publishShift(); + void publishTurnSignal(); + void publishGateMode(); + void publishEmergency(); + void publishAutowareEngage(); + void publishVehicleEngage(); + + // Previous State + autoware_control_msgs::ControlCommand prev_control_command_; + autoware_vehicle_msgs::RawControlCommand prev_raw_control_command_; + ShiftType prev_shift_ = autoware_vehicle_msgs::Shift::NONE; + TurnSignalType prev_turn_signal_ = autoware_vehicle_msgs::TurnSignal::NONE; + GateModeType prev_gate_mode_ = autoware_control_msgs::GateMode::AUTO; + + // tmp + ros::Publisher pub_vehicle_command_; + ros::Publisher pub_raw_vehicle_command_; + void publishVehicleCommand(); + void publishRawVehicleCommand(); + + // Timer + ros::Timer timer_; + + bool isDataReady(); + void onTimer(const ros::TimerEvent & event); +}; diff --git a/control/autoware_joy_controller/include/autoware_joy_controller/joy_converter/ds4_joy_converter.h b/control/autoware_joy_controller/include/autoware_joy_controller/joy_converter/ds4_joy_converter.h new file mode 100644 index 0000000000000..166623d907551 --- /dev/null +++ b/control/autoware_joy_controller/include/autoware_joy_controller/joy_converter/ds4_joy_converter.h @@ -0,0 +1,89 @@ +/* + * Copyright 2020 Tier IV, Inc. All rights reserved. + * + * 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. + */ + +#pragma once + +#include + +class DS4JoyConverter : public JoyConverterBase +{ +public: + explicit DS4JoyConverter(const sensor_msgs::Joy & j) : j_(j) {} + + const float accel() const + { + const auto button = static_cast(Cross()); + const auto stick = std::max(0.0f, RStickUpDown()); + const auto trigger = std::max(0.0f, -RTrigger()); + return std::max({button, stick, trigger}); + } + + const float brake() const + { + const auto button = static_cast(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() const { return !reverse() && PS(); } + bool clear_emergency() 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::Joy j_; + + bool reverse() const { return Share(); } +}; diff --git a/control/autoware_joy_controller/include/autoware_joy_controller/joy_converter/g29_joy_converter.h b/control/autoware_joy_controller/include/autoware_joy_controller/joy_converter/g29_joy_converter.h new file mode 100644 index 0000000000000..9861a643fb734 --- /dev/null +++ b/control/autoware_joy_controller/include/autoware_joy_controller/joy_converter/g29_joy_converter.h @@ -0,0 +1,85 @@ +/* + * Copyright 2020 Tier IV, Inc. All rights reserved. + * + * 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. + */ + +#pragma once + +#include + +class G29JoyConverter : public JoyConverterBase +{ +public: + explicit G29JoyConverter(const sensor_msgs::Joy & j) : j_(j) {} + + const float accel() const + { + constexpr float eps = 0.0000001; + if(std::fabs(AccelPedal()) < eps) return 0.0f; + return (AccelPedal() + 1.0f) / 2; + } + + const float brake() const + { + constexpr float eps = 0.0000001; + if(std::fabs(BrakePedal()) < eps) return 0.0f; + return (BrakePedal() + 1.0f) / 2; + } + + float steer() const { return Steer(); } + + 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() const { return !reverse() && PS(); } + bool clear_emergency() 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 Steer() const { return j_.axes.at(0); } + float AccelPedal() const { return j_.axes.at(2); } + float BrakePedal() const { return j_.axes.at(3); } + + float CursorLeftRight() const { return j_.axes.at(4); } + float CursorUpDown() const { return j_.axes.at(5); } + + 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(5); } + bool R1() const { return j_.buttons.at(4); } + bool L2() const { return j_.buttons.at(7); } + bool R2() const { return j_.buttons.at(6); } + bool Share() const { return j_.buttons.at(8); } + bool Options() const { return j_.buttons.at(9); } + bool PS() const { return j_.buttons.at(24); } + + const sensor_msgs::Joy j_; + + bool reverse() const { return Share(); } +}; diff --git a/control/autoware_joy_controller/include/autoware_joy_controller/joy_converter/joy_converter_base.h b/control/autoware_joy_controller/include/autoware_joy_controller/joy_converter/joy_converter_base.h new file mode 100644 index 0000000000000..3a2aab8d833ff --- /dev/null +++ b/control/autoware_joy_controller/include/autoware_joy_controller/joy_converter/joy_converter_base.h @@ -0,0 +1,51 @@ +/* + * Copyright 2020 Tier IV, Inc. All rights reserved. + * + * 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. + */ + +#pragma once + +#include + +#include + +class JoyConverterBase +{ +public: + virtual const float accel() const = 0; + + virtual const float brake() const = 0; + + virtual float steer() const = 0; + + virtual bool shift_up() const = 0; + virtual bool shift_down() const = 0; + virtual bool shift_drive() const = 0; + virtual bool shift_reverse() const = 0; + + virtual bool turn_signal_left() const = 0; + virtual bool turn_signal_right() const = 0; + virtual bool clear_turn_signal() const = 0; + + virtual bool gate_mode() const = 0; + + virtual bool emergency() const = 0; + virtual bool clear_emergency() const = 0; + + virtual bool autoware_engage() const = 0; + virtual bool autoware_disengage() const = 0; + + virtual bool vehicle_engage() const = 0; + virtual bool vehicle_disengage() const = 0; +}; diff --git a/control/autoware_joy_controller/launch/autoware_joy_controller.launch b/control/autoware_joy_controller/launch/autoware_joy_controller.launch new file mode 100644 index 0000000000000..9818e14c57a6d --- /dev/null +++ b/control/autoware_joy_controller/launch/autoware_joy_controller.launch @@ -0,0 +1,44 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/control/autoware_joy_controller/package.xml b/control/autoware_joy_controller/package.xml new file mode 100644 index 0000000000000..620f50dbdd515 --- /dev/null +++ b/control/autoware_joy_controller/package.xml @@ -0,0 +1,21 @@ + + + autoware_joy_controller + 0.1.0 + The autoware_joy_controller package + Kenji Miyake + Apache 2 + + catkin + + autoware_control_msgs + autoware_vehicle_msgs + joy + geometry_msgs + roscpp + sensor_msgs + std_msgs + + + + diff --git a/control/autoware_joy_controller/src/autoware_joy_controller/autoware_joy_controller_node.cpp b/control/autoware_joy_controller/src/autoware_joy_controller/autoware_joy_controller_node.cpp new file mode 100644 index 0000000000000..7a7fe7a0d4de6 --- /dev/null +++ b/control/autoware_joy_controller/src/autoware_joy_controller/autoware_joy_controller_node.cpp @@ -0,0 +1,419 @@ +/* + * Copyright 2020 Tier IV, Inc. All rights reserved. + * + * 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 +#include +#include + +namespace +{ +ShiftType getUpperShift(const ShiftType & shift) +{ + using autoware_vehicle_msgs::Shift; + + if (shift == Shift::NONE) return Shift::PARKING; + if (shift == Shift::PARKING) return Shift::REVERSE; + if (shift == Shift::REVERSE) return Shift::NEUTRAL; + if (shift == Shift::NEUTRAL) return Shift::DRIVE; + if (shift == Shift::DRIVE) return Shift::LOW; + if (shift == Shift::LOW) return Shift::LOW; + + return Shift::NONE; +} + +ShiftType getLowerShift(const ShiftType & shift) +{ + using autoware_vehicle_msgs::Shift; + + if (shift == Shift::NONE) return Shift::PARKING; + if (shift == Shift::PARKING) return Shift::PARKING; + if (shift == Shift::REVERSE) return Shift::PARKING; + if (shift == Shift::NEUTRAL) return Shift::REVERSE; + if (shift == Shift::DRIVE) return Shift::NEUTRAL; + if (shift == Shift::LOW) return Shift::DRIVE; + + return Shift::NONE; +} + +const char * getShiftName(const ShiftType & shift) +{ + using autoware_vehicle_msgs::Shift; + + if (shift == Shift::NONE) return "NONE"; + if (shift == Shift::PARKING) return "PARKING"; + if (shift == Shift::REVERSE) return "REVERSE"; + if (shift == Shift::NEUTRAL) return "NEUTRAL"; + if (shift == Shift::DRIVE) return "DRIVE"; + if (shift == Shift::LOW) return "LOW"; + + return "NOT_SUPPORTED"; +} + +const char * getTurnSignalName(const TurnSignalType & turn_signal) +{ + using autoware_vehicle_msgs::TurnSignal; + + if (turn_signal == TurnSignal::NONE) return "NONE"; + if (turn_signal == TurnSignal::LEFT) return "LEFT"; + if (turn_signal == TurnSignal::RIGHT) return "RIGHT"; + if (turn_signal == TurnSignal::HAZARD) return "HAZARD"; + + return "NOT_SUPPORTED"; +} + +const char * getGateModeName(const GateModeType & gate_mode) +{ + using autoware_control_msgs::GateMode; + + if (gate_mode == GateMode::AUTO) return "AUTO"; + if (gate_mode == GateMode::REMOTE) return "REMOTE"; + + return "NOT_SUPPORTED"; +} + +} // namespace + +void AutowareJoyControllerNode::onJoy(const sensor_msgs::Joy::ConstPtr & msg) +{ + last_joy_received_time_ = msg->header.stamp; + if (joy_type_ == "G29") { + joy_ = std::make_shared(*msg); + } else { + joy_ = std::make_shared(*msg); + } + + if (joy_->shift_up() || joy_->shift_down() || joy_->shift_drive() || joy_->shift_reverse()) { + publishShift(); + } + + if (joy_->turn_signal_left() || joy_->turn_signal_right() || joy_->clear_turn_signal()) { + publishTurnSignal(); + } + + if (joy_->gate_mode()) { + publishGateMode(); + } + + if (joy_->emergency() || joy_->clear_emergency()) { + publishEmergency(); + } + + if (joy_->autoware_engage() || joy_->autoware_disengage()) { + publishAutowareEngage(); + } + + if (joy_->vehicle_engage() || joy_->vehicle_disengage()) { + publishVehicleEngage(); + } +} + +void AutowareJoyControllerNode::onTwist(const geometry_msgs::TwistStamped::ConstPtr & msg) +{ + twist_ = msg; +} + +bool AutowareJoyControllerNode::isDataReady() +{ + // Joy + { + if (!joy_) { + ROS_WARN_THROTTLE(1.0, "waiting for joy msg..."); + return false; + } + + constexpr auto timeout = 2.0; + const auto time_diff = ros::Time::now() - last_joy_received_time_; + if (time_diff.toSec() > timeout) { + ROS_WARN_THROTTLE(1.0, "joy msg is timeout"); + return false; + } + } + + // Twist + { + if (!twist_) { + ROS_WARN_THROTTLE(1.0, "waiting for twist msg..."); + return false; + } + + constexpr auto timeout = 0.5; + const auto time_diff = ros::Time::now() - twist_->header.stamp; + if (time_diff.toSec() > timeout) { + ROS_WARN_THROTTLE(1.0, "twist msg is timeout"); + return false; + } + } + + return true; +} + +void AutowareJoyControllerNode::onTimer(const ros::TimerEvent & event) +{ + if (!isDataReady()) { + return; + } + + publishControlCommand(); + publishRawControlCommand(); + + // tmp + publishVehicleCommand(); + publishRawVehicleCommand(); +} + +void AutowareJoyControllerNode::publishControlCommand() +{ + autoware_control_msgs::ControlCommandStamped cmd_stamped; + cmd_stamped.header.stamp = ros::Time::now(); + + { + auto & cmd = cmd_stamped.control; + + cmd.steering_angle = steer_ratio_ * joy_->steer(); + cmd.steering_angle_velocity = steering_angle_velocity_; + + if (joy_->accel()) { + cmd.acceleration = accel_ratio_ * joy_->accel(); + cmd.velocity = twist_->twist.linear.x + velocity_gain_ * cmd.acceleration; + cmd.velocity = std::min(cmd.velocity, max_forward_velocity_); + } + + if (joy_->brake()) { + cmd.velocity = 0.0; + cmd.acceleration = -brake_ratio_ * joy_->brake(); + } + + // Backward + if (joy_->accel() && joy_->brake()) { + cmd.acceleration = backward_accel_ratio_ * joy_->accel(); + cmd.velocity = twist_->twist.linear.x - velocity_gain_ * cmd.acceleration; + cmd.velocity = std::max(cmd.velocity, -max_backward_velocity_); + } + } + + pub_control_command_.publish(cmd_stamped); + prev_control_command_ = cmd_stamped.control; +} + +void AutowareJoyControllerNode::publishRawControlCommand() +{ + autoware_vehicle_msgs::RawControlCommandStamped cmd_stamped; + cmd_stamped.header.stamp = ros::Time::now(); + + { + auto & cmd = cmd_stamped.control; + + cmd.steering_angle = steer_ratio_ * joy_->steer(); + cmd.steering_angle_velocity = steering_angle_velocity_; + cmd.throttle = accel_ratio_ * joy_->accel(); + cmd.brake = brake_ratio_ * joy_->brake(); + } + + pub_raw_control_command_.publish(cmd_stamped); + prev_raw_control_command_ = cmd_stamped.control; +} + +void AutowareJoyControllerNode::publishShift() +{ + using autoware_vehicle_msgs::Shift; + + autoware_vehicle_msgs::ShiftStamped shift_stamped; + shift_stamped.header.stamp = ros::Time::now(); + + { + auto & shift = shift_stamped.shift; + if (joy_->shift_up()) { + shift.data = getUpperShift(prev_shift_); + } + + if (joy_->shift_down()) { + shift.data = getLowerShift(prev_shift_); + } + + if (joy_->shift_drive()) { + shift.data = Shift::DRIVE; + } + + if (joy_->shift_reverse()) { + shift.data = Shift::REVERSE; + } + + ROS_INFO("Shift::%s", getShiftName(shift.data)); + } + + pub_shift_.publish(shift_stamped); + prev_shift_ = shift_stamped.shift.data; +} + +void AutowareJoyControllerNode::publishTurnSignal() +{ + using autoware_vehicle_msgs::TurnSignal; + + TurnSignal turn_signal; + turn_signal.header.stamp = ros::Time::now(); + + if (joy_->turn_signal_left() && joy_->turn_signal_right()) { + turn_signal.data = TurnSignal::HAZARD; + } else if (joy_->turn_signal_left()) { + turn_signal.data = TurnSignal::LEFT; + } else if (joy_->turn_signal_right()) { + turn_signal.data = TurnSignal::RIGHT; + } + + if (joy_->clear_turn_signal()) { + turn_signal.data = TurnSignal::NONE; + } + + ROS_INFO("TurnSignal::%s", getTurnSignalName(turn_signal.data)); + + pub_turn_signal_.publish(turn_signal); +} + +void AutowareJoyControllerNode::publishGateMode() +{ + using autoware_control_msgs::GateMode; + + autoware_control_msgs::GateMode gate_mode; + + if (prev_gate_mode_ == GateMode::AUTO) { + gate_mode.data = GateMode::REMOTE; + } + + if (prev_gate_mode_ == GateMode::REMOTE) { + gate_mode.data = GateMode::AUTO; + } + + ROS_INFO("GateMode::%s", getGateModeName(gate_mode.data)); + + pub_gate_mode_.publish(gate_mode); + prev_gate_mode_ = gate_mode.data; +} + +void AutowareJoyControllerNode::publishEmergency() +{ + std_msgs::Bool emergency; + + if (joy_->emergency()) { + emergency.data = true; + ROS_INFO("Emergency"); + } + + if (joy_->clear_emergency()) { + emergency.data = false; + ROS_INFO("Clear Emergency"); + } + + pub_emergency_.publish(emergency); +} + +void AutowareJoyControllerNode::publishAutowareEngage() +{ + std_msgs::Bool engage; + + if (joy_->autoware_engage()) { + engage.data = true; + ROS_INFO("Autoware Engage"); + } + + if (joy_->autoware_disengage()) { + engage.data = false; + ROS_INFO("Autoware Disengage"); + } + + pub_autoware_engage_.publish(engage); +} + +void AutowareJoyControllerNode::publishVehicleEngage() +{ + std_msgs::Bool engage; + + if (joy_->vehicle_engage()) { + engage.data = true; + ROS_INFO("Vehicle Engage"); + } + + if (joy_->vehicle_disengage()) { + engage.data = false; + ROS_INFO("Vehicle Disengage"); + } + + pub_vehicle_engage_.publish(engage); +} + +// tmp +void AutowareJoyControllerNode::publishVehicleCommand() +{ + autoware_vehicle_msgs::VehicleCommand vehicle_cmd; + vehicle_cmd.header.stamp = ros::Time::now(); + vehicle_cmd.control = prev_control_command_; + vehicle_cmd.shift.data = prev_shift_; + + pub_vehicle_command_.publish(vehicle_cmd); +} + +void AutowareJoyControllerNode::publishRawVehicleCommand() +{ + autoware_vehicle_msgs::RawVehicleCommand vehicle_cmd; + vehicle_cmd.header.stamp = ros::Time::now(); + vehicle_cmd.control = prev_raw_control_command_; + vehicle_cmd.shift.data = prev_shift_; + + pub_raw_vehicle_command_.publish(vehicle_cmd); +} + +AutowareJoyControllerNode::AutowareJoyControllerNode() +{ + // Parameter + private_nh_.param("joy_type", joy_type_, "DS4"); + private_nh_.param("update_rate", update_rate_, 10.0); + private_nh_.param("accel_ratio", accel_ratio_, 3.0); + private_nh_.param("brake_ratio", brake_ratio_, 5.0); + private_nh_.param("steer_ratio", steer_ratio_, 0.5); + private_nh_.param("steering_angle_velocity", steering_angle_velocity_, 0.1); + private_nh_.param("control_command/velocity_gain", velocity_gain_, 3.0); + private_nh_.param("control_command/max_forward_velocity", max_forward_velocity_, 20.0); + private_nh_.param("control_command/max_backward_velocity", max_backward_velocity_, 3.0); + private_nh_.param("control_command/backward_accel_ratio", backward_accel_ratio_, 1.0); + + ROS_INFO("Joy type: %s", joy_type_.c_str()); + + // Subscriber + sub_joy_ = private_nh_.subscribe("input/joy", 1, &AutowareJoyControllerNode::onJoy, this); + sub_twist_ = private_nh_.subscribe("input/twist", 1, &AutowareJoyControllerNode::onTwist, this); + + // Publisher + pub_control_command_ = private_nh_.advertise( + "output/control_command", 1); + pub_raw_control_command_ = private_nh_.advertise( + "output/raw_control_command", 1); + pub_shift_ = private_nh_.advertise("output/shift", 1); + pub_turn_signal_ = + private_nh_.advertise("output/turn_signal", 1); + pub_gate_mode_ = private_nh_.advertise("output/gate_mode", 1); + pub_emergency_ = private_nh_.advertise("output/emergency", 1); + pub_autoware_engage_ = private_nh_.advertise("output/autoware_engage", 1); + pub_vehicle_engage_ = private_nh_.advertise("output/vehicle_engage", 1); + + // tmp + pub_vehicle_command_ = + private_nh_.advertise("output/vehicle_cmd", 1); + pub_raw_vehicle_command_ = + private_nh_.advertise("output/raw_vehicle_cmd", 1); + + // Timer + timer_ = + private_nh_.createTimer(ros::Rate(update_rate_), &AutowareJoyControllerNode::onTimer, this); +} diff --git a/control/autoware_joy_controller/src/autoware_joy_controller/main.cpp b/control/autoware_joy_controller/src/autoware_joy_controller/main.cpp new file mode 100644 index 0000000000000..1617ed436068e --- /dev/null +++ b/control/autoware_joy_controller/src/autoware_joy_controller/main.cpp @@ -0,0 +1,30 @@ +/* + * Copyright 2020 Tier IV, Inc. All rights reserved. + * + * 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 + +#include + +int main(int argc, char * argv[]) +{ + ros::init(argc, argv, "autoware_joy_controller"); + + AutowareJoyControllerNode node; + + ros::spin(); + + return 0; +} From f98dee7573da44d0ec7591305793dd1ee86d283b Mon Sep 17 00:00:00 2001 From: mitsudome-r Date: Tue, 6 Oct 2020 15:33:58 +0900 Subject: [PATCH 07/38] remove ROS1 packages Signed-off-by: mitsudome-r --- .../autoware_joy_controller/CMakeLists.txt | 56 --- control/autoware_joy_controller/COLCON_IGNORE | 0 .../config/autoware_joy_controller.yaml | 10 - .../autoware_joy_controller.h | 116 ----- .../joy_converter/ds4_joy_converter.h | 89 ---- .../joy_converter/g29_joy_converter.h | 85 ---- .../joy_converter/joy_converter_base.h | 51 --- .../launch/autoware_joy_controller.launch | 44 -- control/autoware_joy_controller/package.xml | 21 - .../autoware_joy_controller_node.cpp | 419 ------------------ .../src/autoware_joy_controller/main.cpp | 30 -- 11 files changed, 921 deletions(-) delete mode 100644 control/autoware_joy_controller/CMakeLists.txt delete mode 100644 control/autoware_joy_controller/COLCON_IGNORE delete mode 100644 control/autoware_joy_controller/config/autoware_joy_controller.yaml delete mode 100644 control/autoware_joy_controller/include/autoware_joy_controller/autoware_joy_controller.h delete mode 100644 control/autoware_joy_controller/include/autoware_joy_controller/joy_converter/ds4_joy_converter.h delete mode 100644 control/autoware_joy_controller/include/autoware_joy_controller/joy_converter/g29_joy_converter.h delete mode 100644 control/autoware_joy_controller/include/autoware_joy_controller/joy_converter/joy_converter_base.h delete mode 100644 control/autoware_joy_controller/launch/autoware_joy_controller.launch delete mode 100644 control/autoware_joy_controller/package.xml delete mode 100644 control/autoware_joy_controller/src/autoware_joy_controller/autoware_joy_controller_node.cpp delete mode 100644 control/autoware_joy_controller/src/autoware_joy_controller/main.cpp diff --git a/control/autoware_joy_controller/CMakeLists.txt b/control/autoware_joy_controller/CMakeLists.txt deleted file mode 100644 index 5f13f9a27a1d7..0000000000000 --- a/control/autoware_joy_controller/CMakeLists.txt +++ /dev/null @@ -1,56 +0,0 @@ -cmake_minimum_required(VERSION 3.0.2) -project(autoware_joy_controller) - -add_compile_options(-std=c++14) - -find_package(catkin REQUIRED COMPONENTS - autoware_control_msgs - autoware_vehicle_msgs - joy - geometry_msgs - roscpp - sensor_msgs - std_msgs -) - -catkin_package() - -include_directories( - include - ${catkin_INCLUDE_DIRS} -) - -add_executable(autoware_joy_controller - src/autoware_joy_controller/main.cpp - src/autoware_joy_controller/autoware_joy_controller_node.cpp -) - -target_link_libraries(autoware_joy_controller - ${catkin_LIBRARIES} -) - -add_dependencies(autoware_joy_controller - ${catkin_EXPORTED_TARGETS} -) - -# Install executables and/or libraries -install( - TARGETS - autoware_joy_controller - ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} - LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} - RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} -) - -# Install project namespaced headers -install(DIRECTORY include/${PROJECT_NAME}/ - DESTINATION ${CATKIN_PACKAGE_INCLUDE_DESTINATION} -) - -# Install directories -install( - DIRECTORY - launch - config - DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION} -) diff --git a/control/autoware_joy_controller/COLCON_IGNORE b/control/autoware_joy_controller/COLCON_IGNORE deleted file mode 100644 index e69de29bb2d1d..0000000000000 diff --git a/control/autoware_joy_controller/config/autoware_joy_controller.yaml b/control/autoware_joy_controller/config/autoware_joy_controller.yaml deleted file mode 100644 index f7ee5a5c4e10e..0000000000000 --- a/control/autoware_joy_controller/config/autoware_joy_controller.yaml +++ /dev/null @@ -1,10 +0,0 @@ -update_rate: 10 -accel_ratio: 3.0 -brake_ratio: 5.0 -steer_ratio: 0.5 -steering_angle_velocity: 0.1 -control_command: - velocity_gain: 3.0 - max_forward_velocity: 20 - max_backward_velocity: 3 - backward_accel_ratio: 1.0 diff --git a/control/autoware_joy_controller/include/autoware_joy_controller/autoware_joy_controller.h b/control/autoware_joy_controller/include/autoware_joy_controller/autoware_joy_controller.h deleted file mode 100644 index 8aef6ccfd32f1..0000000000000 --- a/control/autoware_joy_controller/include/autoware_joy_controller/autoware_joy_controller.h +++ /dev/null @@ -1,116 +0,0 @@ -/* - * Copyright 2020 Tier IV, Inc. All rights reserved. - * - * 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. - */ - -#pragma once - -#include -#include -#include - -#include - -#include -#include -#include -#include -#include -#include -#include -#include - -#include - -// tmp -#include -#include - -using ShiftType = autoware_vehicle_msgs::Shift::_data_type; -using TurnSignalType = autoware_vehicle_msgs::TurnSignal::_data_type; -using GateModeType = autoware_control_msgs::GateMode::_data_type; - -class AutowareJoyControllerNode -{ -public: - AutowareJoyControllerNode(); - -private: - // NodeHandle - ros::NodeHandle nh_{""}; - ros::NodeHandle private_nh_{"~"}; - - // Parameter - std::string joy_type_; - double update_rate_; - double accel_ratio_; - double brake_ratio_; - double steer_ratio_; - double steering_angle_velocity_; - - // ControlCommand Parameter - double velocity_gain_; - double max_forward_velocity_; - double max_backward_velocity_; - double backward_accel_ratio_; - - // Subscriber - ros::Subscriber sub_joy_; - ros::Subscriber sub_twist_; - - ros::Time last_joy_received_time_; - std::shared_ptr joy_; - geometry_msgs::TwistStamped::ConstPtr twist_; - - void onJoy(const sensor_msgs::Joy::ConstPtr & msg); - void onTwist(const geometry_msgs::TwistStamped::ConstPtr & msg); - - // Publisher - ros::Publisher pub_control_command_; - ros::Publisher pub_raw_control_command_; - ros::Publisher pub_shift_; - ros::Publisher pub_turn_signal_; - ros::Publisher pub_gate_mode_; - ros::Publisher pub_emergency_; - ros::Publisher pub_autoware_engage_; - ros::Publisher pub_vehicle_engage_; - - void publishControlCommand(); - void publishRawControlCommand(); - void publishShift(); - void publishTurnSignal(); - void publishGateMode(); - void publishEmergency(); - void publishAutowareEngage(); - void publishVehicleEngage(); - - // Previous State - autoware_control_msgs::ControlCommand prev_control_command_; - autoware_vehicle_msgs::RawControlCommand prev_raw_control_command_; - ShiftType prev_shift_ = autoware_vehicle_msgs::Shift::NONE; - TurnSignalType prev_turn_signal_ = autoware_vehicle_msgs::TurnSignal::NONE; - GateModeType prev_gate_mode_ = autoware_control_msgs::GateMode::AUTO; - - // tmp - ros::Publisher pub_vehicle_command_; - ros::Publisher pub_raw_vehicle_command_; - void publishVehicleCommand(); - void publishRawVehicleCommand(); - - // Timer - ros::Timer timer_; - - bool isDataReady(); - void onTimer(const ros::TimerEvent & event); -}; diff --git a/control/autoware_joy_controller/include/autoware_joy_controller/joy_converter/ds4_joy_converter.h b/control/autoware_joy_controller/include/autoware_joy_controller/joy_converter/ds4_joy_converter.h deleted file mode 100644 index 166623d907551..0000000000000 --- a/control/autoware_joy_controller/include/autoware_joy_controller/joy_converter/ds4_joy_converter.h +++ /dev/null @@ -1,89 +0,0 @@ -/* - * Copyright 2020 Tier IV, Inc. All rights reserved. - * - * 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. - */ - -#pragma once - -#include - -class DS4JoyConverter : public JoyConverterBase -{ -public: - explicit DS4JoyConverter(const sensor_msgs::Joy & j) : j_(j) {} - - const float accel() const - { - const auto button = static_cast(Cross()); - const auto stick = std::max(0.0f, RStickUpDown()); - const auto trigger = std::max(0.0f, -RTrigger()); - return std::max({button, stick, trigger}); - } - - const float brake() const - { - const auto button = static_cast(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() const { return !reverse() && PS(); } - bool clear_emergency() 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::Joy j_; - - bool reverse() const { return Share(); } -}; diff --git a/control/autoware_joy_controller/include/autoware_joy_controller/joy_converter/g29_joy_converter.h b/control/autoware_joy_controller/include/autoware_joy_controller/joy_converter/g29_joy_converter.h deleted file mode 100644 index 9861a643fb734..0000000000000 --- a/control/autoware_joy_controller/include/autoware_joy_controller/joy_converter/g29_joy_converter.h +++ /dev/null @@ -1,85 +0,0 @@ -/* - * Copyright 2020 Tier IV, Inc. All rights reserved. - * - * 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. - */ - -#pragma once - -#include - -class G29JoyConverter : public JoyConverterBase -{ -public: - explicit G29JoyConverter(const sensor_msgs::Joy & j) : j_(j) {} - - const float accel() const - { - constexpr float eps = 0.0000001; - if(std::fabs(AccelPedal()) < eps) return 0.0f; - return (AccelPedal() + 1.0f) / 2; - } - - const float brake() const - { - constexpr float eps = 0.0000001; - if(std::fabs(BrakePedal()) < eps) return 0.0f; - return (BrakePedal() + 1.0f) / 2; - } - - float steer() const { return Steer(); } - - 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() const { return !reverse() && PS(); } - bool clear_emergency() 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 Steer() const { return j_.axes.at(0); } - float AccelPedal() const { return j_.axes.at(2); } - float BrakePedal() const { return j_.axes.at(3); } - - float CursorLeftRight() const { return j_.axes.at(4); } - float CursorUpDown() const { return j_.axes.at(5); } - - 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(5); } - bool R1() const { return j_.buttons.at(4); } - bool L2() const { return j_.buttons.at(7); } - bool R2() const { return j_.buttons.at(6); } - bool Share() const { return j_.buttons.at(8); } - bool Options() const { return j_.buttons.at(9); } - bool PS() const { return j_.buttons.at(24); } - - const sensor_msgs::Joy j_; - - bool reverse() const { return Share(); } -}; diff --git a/control/autoware_joy_controller/include/autoware_joy_controller/joy_converter/joy_converter_base.h b/control/autoware_joy_controller/include/autoware_joy_controller/joy_converter/joy_converter_base.h deleted file mode 100644 index 3a2aab8d833ff..0000000000000 --- a/control/autoware_joy_controller/include/autoware_joy_controller/joy_converter/joy_converter_base.h +++ /dev/null @@ -1,51 +0,0 @@ -/* - * Copyright 2020 Tier IV, Inc. All rights reserved. - * - * 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. - */ - -#pragma once - -#include - -#include - -class JoyConverterBase -{ -public: - virtual const float accel() const = 0; - - virtual const float brake() const = 0; - - virtual float steer() const = 0; - - virtual bool shift_up() const = 0; - virtual bool shift_down() const = 0; - virtual bool shift_drive() const = 0; - virtual bool shift_reverse() const = 0; - - virtual bool turn_signal_left() const = 0; - virtual bool turn_signal_right() const = 0; - virtual bool clear_turn_signal() const = 0; - - virtual bool gate_mode() const = 0; - - virtual bool emergency() const = 0; - virtual bool clear_emergency() const = 0; - - virtual bool autoware_engage() const = 0; - virtual bool autoware_disengage() const = 0; - - virtual bool vehicle_engage() const = 0; - virtual bool vehicle_disengage() const = 0; -}; diff --git a/control/autoware_joy_controller/launch/autoware_joy_controller.launch b/control/autoware_joy_controller/launch/autoware_joy_controller.launch deleted file mode 100644 index 9818e14c57a6d..0000000000000 --- a/control/autoware_joy_controller/launch/autoware_joy_controller.launch +++ /dev/null @@ -1,44 +0,0 @@ - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - diff --git a/control/autoware_joy_controller/package.xml b/control/autoware_joy_controller/package.xml deleted file mode 100644 index 620f50dbdd515..0000000000000 --- a/control/autoware_joy_controller/package.xml +++ /dev/null @@ -1,21 +0,0 @@ - - - autoware_joy_controller - 0.1.0 - The autoware_joy_controller package - Kenji Miyake - Apache 2 - - catkin - - autoware_control_msgs - autoware_vehicle_msgs - joy - geometry_msgs - roscpp - sensor_msgs - std_msgs - - - - diff --git a/control/autoware_joy_controller/src/autoware_joy_controller/autoware_joy_controller_node.cpp b/control/autoware_joy_controller/src/autoware_joy_controller/autoware_joy_controller_node.cpp deleted file mode 100644 index 7a7fe7a0d4de6..0000000000000 --- a/control/autoware_joy_controller/src/autoware_joy_controller/autoware_joy_controller_node.cpp +++ /dev/null @@ -1,419 +0,0 @@ -/* - * Copyright 2020 Tier IV, Inc. All rights reserved. - * - * 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 -#include -#include - -namespace -{ -ShiftType getUpperShift(const ShiftType & shift) -{ - using autoware_vehicle_msgs::Shift; - - if (shift == Shift::NONE) return Shift::PARKING; - if (shift == Shift::PARKING) return Shift::REVERSE; - if (shift == Shift::REVERSE) return Shift::NEUTRAL; - if (shift == Shift::NEUTRAL) return Shift::DRIVE; - if (shift == Shift::DRIVE) return Shift::LOW; - if (shift == Shift::LOW) return Shift::LOW; - - return Shift::NONE; -} - -ShiftType getLowerShift(const ShiftType & shift) -{ - using autoware_vehicle_msgs::Shift; - - if (shift == Shift::NONE) return Shift::PARKING; - if (shift == Shift::PARKING) return Shift::PARKING; - if (shift == Shift::REVERSE) return Shift::PARKING; - if (shift == Shift::NEUTRAL) return Shift::REVERSE; - if (shift == Shift::DRIVE) return Shift::NEUTRAL; - if (shift == Shift::LOW) return Shift::DRIVE; - - return Shift::NONE; -} - -const char * getShiftName(const ShiftType & shift) -{ - using autoware_vehicle_msgs::Shift; - - if (shift == Shift::NONE) return "NONE"; - if (shift == Shift::PARKING) return "PARKING"; - if (shift == Shift::REVERSE) return "REVERSE"; - if (shift == Shift::NEUTRAL) return "NEUTRAL"; - if (shift == Shift::DRIVE) return "DRIVE"; - if (shift == Shift::LOW) return "LOW"; - - return "NOT_SUPPORTED"; -} - -const char * getTurnSignalName(const TurnSignalType & turn_signal) -{ - using autoware_vehicle_msgs::TurnSignal; - - if (turn_signal == TurnSignal::NONE) return "NONE"; - if (turn_signal == TurnSignal::LEFT) return "LEFT"; - if (turn_signal == TurnSignal::RIGHT) return "RIGHT"; - if (turn_signal == TurnSignal::HAZARD) return "HAZARD"; - - return "NOT_SUPPORTED"; -} - -const char * getGateModeName(const GateModeType & gate_mode) -{ - using autoware_control_msgs::GateMode; - - if (gate_mode == GateMode::AUTO) return "AUTO"; - if (gate_mode == GateMode::REMOTE) return "REMOTE"; - - return "NOT_SUPPORTED"; -} - -} // namespace - -void AutowareJoyControllerNode::onJoy(const sensor_msgs::Joy::ConstPtr & msg) -{ - last_joy_received_time_ = msg->header.stamp; - if (joy_type_ == "G29") { - joy_ = std::make_shared(*msg); - } else { - joy_ = std::make_shared(*msg); - } - - if (joy_->shift_up() || joy_->shift_down() || joy_->shift_drive() || joy_->shift_reverse()) { - publishShift(); - } - - if (joy_->turn_signal_left() || joy_->turn_signal_right() || joy_->clear_turn_signal()) { - publishTurnSignal(); - } - - if (joy_->gate_mode()) { - publishGateMode(); - } - - if (joy_->emergency() || joy_->clear_emergency()) { - publishEmergency(); - } - - if (joy_->autoware_engage() || joy_->autoware_disengage()) { - publishAutowareEngage(); - } - - if (joy_->vehicle_engage() || joy_->vehicle_disengage()) { - publishVehicleEngage(); - } -} - -void AutowareJoyControllerNode::onTwist(const geometry_msgs::TwistStamped::ConstPtr & msg) -{ - twist_ = msg; -} - -bool AutowareJoyControllerNode::isDataReady() -{ - // Joy - { - if (!joy_) { - ROS_WARN_THROTTLE(1.0, "waiting for joy msg..."); - return false; - } - - constexpr auto timeout = 2.0; - const auto time_diff = ros::Time::now() - last_joy_received_time_; - if (time_diff.toSec() > timeout) { - ROS_WARN_THROTTLE(1.0, "joy msg is timeout"); - return false; - } - } - - // Twist - { - if (!twist_) { - ROS_WARN_THROTTLE(1.0, "waiting for twist msg..."); - return false; - } - - constexpr auto timeout = 0.5; - const auto time_diff = ros::Time::now() - twist_->header.stamp; - if (time_diff.toSec() > timeout) { - ROS_WARN_THROTTLE(1.0, "twist msg is timeout"); - return false; - } - } - - return true; -} - -void AutowareJoyControllerNode::onTimer(const ros::TimerEvent & event) -{ - if (!isDataReady()) { - return; - } - - publishControlCommand(); - publishRawControlCommand(); - - // tmp - publishVehicleCommand(); - publishRawVehicleCommand(); -} - -void AutowareJoyControllerNode::publishControlCommand() -{ - autoware_control_msgs::ControlCommandStamped cmd_stamped; - cmd_stamped.header.stamp = ros::Time::now(); - - { - auto & cmd = cmd_stamped.control; - - cmd.steering_angle = steer_ratio_ * joy_->steer(); - cmd.steering_angle_velocity = steering_angle_velocity_; - - if (joy_->accel()) { - cmd.acceleration = accel_ratio_ * joy_->accel(); - cmd.velocity = twist_->twist.linear.x + velocity_gain_ * cmd.acceleration; - cmd.velocity = std::min(cmd.velocity, max_forward_velocity_); - } - - if (joy_->brake()) { - cmd.velocity = 0.0; - cmd.acceleration = -brake_ratio_ * joy_->brake(); - } - - // Backward - if (joy_->accel() && joy_->brake()) { - cmd.acceleration = backward_accel_ratio_ * joy_->accel(); - cmd.velocity = twist_->twist.linear.x - velocity_gain_ * cmd.acceleration; - cmd.velocity = std::max(cmd.velocity, -max_backward_velocity_); - } - } - - pub_control_command_.publish(cmd_stamped); - prev_control_command_ = cmd_stamped.control; -} - -void AutowareJoyControllerNode::publishRawControlCommand() -{ - autoware_vehicle_msgs::RawControlCommandStamped cmd_stamped; - cmd_stamped.header.stamp = ros::Time::now(); - - { - auto & cmd = cmd_stamped.control; - - cmd.steering_angle = steer_ratio_ * joy_->steer(); - cmd.steering_angle_velocity = steering_angle_velocity_; - cmd.throttle = accel_ratio_ * joy_->accel(); - cmd.brake = brake_ratio_ * joy_->brake(); - } - - pub_raw_control_command_.publish(cmd_stamped); - prev_raw_control_command_ = cmd_stamped.control; -} - -void AutowareJoyControllerNode::publishShift() -{ - using autoware_vehicle_msgs::Shift; - - autoware_vehicle_msgs::ShiftStamped shift_stamped; - shift_stamped.header.stamp = ros::Time::now(); - - { - auto & shift = shift_stamped.shift; - if (joy_->shift_up()) { - shift.data = getUpperShift(prev_shift_); - } - - if (joy_->shift_down()) { - shift.data = getLowerShift(prev_shift_); - } - - if (joy_->shift_drive()) { - shift.data = Shift::DRIVE; - } - - if (joy_->shift_reverse()) { - shift.data = Shift::REVERSE; - } - - ROS_INFO("Shift::%s", getShiftName(shift.data)); - } - - pub_shift_.publish(shift_stamped); - prev_shift_ = shift_stamped.shift.data; -} - -void AutowareJoyControllerNode::publishTurnSignal() -{ - using autoware_vehicle_msgs::TurnSignal; - - TurnSignal turn_signal; - turn_signal.header.stamp = ros::Time::now(); - - if (joy_->turn_signal_left() && joy_->turn_signal_right()) { - turn_signal.data = TurnSignal::HAZARD; - } else if (joy_->turn_signal_left()) { - turn_signal.data = TurnSignal::LEFT; - } else if (joy_->turn_signal_right()) { - turn_signal.data = TurnSignal::RIGHT; - } - - if (joy_->clear_turn_signal()) { - turn_signal.data = TurnSignal::NONE; - } - - ROS_INFO("TurnSignal::%s", getTurnSignalName(turn_signal.data)); - - pub_turn_signal_.publish(turn_signal); -} - -void AutowareJoyControllerNode::publishGateMode() -{ - using autoware_control_msgs::GateMode; - - autoware_control_msgs::GateMode gate_mode; - - if (prev_gate_mode_ == GateMode::AUTO) { - gate_mode.data = GateMode::REMOTE; - } - - if (prev_gate_mode_ == GateMode::REMOTE) { - gate_mode.data = GateMode::AUTO; - } - - ROS_INFO("GateMode::%s", getGateModeName(gate_mode.data)); - - pub_gate_mode_.publish(gate_mode); - prev_gate_mode_ = gate_mode.data; -} - -void AutowareJoyControllerNode::publishEmergency() -{ - std_msgs::Bool emergency; - - if (joy_->emergency()) { - emergency.data = true; - ROS_INFO("Emergency"); - } - - if (joy_->clear_emergency()) { - emergency.data = false; - ROS_INFO("Clear Emergency"); - } - - pub_emergency_.publish(emergency); -} - -void AutowareJoyControllerNode::publishAutowareEngage() -{ - std_msgs::Bool engage; - - if (joy_->autoware_engage()) { - engage.data = true; - ROS_INFO("Autoware Engage"); - } - - if (joy_->autoware_disengage()) { - engage.data = false; - ROS_INFO("Autoware Disengage"); - } - - pub_autoware_engage_.publish(engage); -} - -void AutowareJoyControllerNode::publishVehicleEngage() -{ - std_msgs::Bool engage; - - if (joy_->vehicle_engage()) { - engage.data = true; - ROS_INFO("Vehicle Engage"); - } - - if (joy_->vehicle_disengage()) { - engage.data = false; - ROS_INFO("Vehicle Disengage"); - } - - pub_vehicle_engage_.publish(engage); -} - -// tmp -void AutowareJoyControllerNode::publishVehicleCommand() -{ - autoware_vehicle_msgs::VehicleCommand vehicle_cmd; - vehicle_cmd.header.stamp = ros::Time::now(); - vehicle_cmd.control = prev_control_command_; - vehicle_cmd.shift.data = prev_shift_; - - pub_vehicle_command_.publish(vehicle_cmd); -} - -void AutowareJoyControllerNode::publishRawVehicleCommand() -{ - autoware_vehicle_msgs::RawVehicleCommand vehicle_cmd; - vehicle_cmd.header.stamp = ros::Time::now(); - vehicle_cmd.control = prev_raw_control_command_; - vehicle_cmd.shift.data = prev_shift_; - - pub_raw_vehicle_command_.publish(vehicle_cmd); -} - -AutowareJoyControllerNode::AutowareJoyControllerNode() -{ - // Parameter - private_nh_.param("joy_type", joy_type_, "DS4"); - private_nh_.param("update_rate", update_rate_, 10.0); - private_nh_.param("accel_ratio", accel_ratio_, 3.0); - private_nh_.param("brake_ratio", brake_ratio_, 5.0); - private_nh_.param("steer_ratio", steer_ratio_, 0.5); - private_nh_.param("steering_angle_velocity", steering_angle_velocity_, 0.1); - private_nh_.param("control_command/velocity_gain", velocity_gain_, 3.0); - private_nh_.param("control_command/max_forward_velocity", max_forward_velocity_, 20.0); - private_nh_.param("control_command/max_backward_velocity", max_backward_velocity_, 3.0); - private_nh_.param("control_command/backward_accel_ratio", backward_accel_ratio_, 1.0); - - ROS_INFO("Joy type: %s", joy_type_.c_str()); - - // Subscriber - sub_joy_ = private_nh_.subscribe("input/joy", 1, &AutowareJoyControllerNode::onJoy, this); - sub_twist_ = private_nh_.subscribe("input/twist", 1, &AutowareJoyControllerNode::onTwist, this); - - // Publisher - pub_control_command_ = private_nh_.advertise( - "output/control_command", 1); - pub_raw_control_command_ = private_nh_.advertise( - "output/raw_control_command", 1); - pub_shift_ = private_nh_.advertise("output/shift", 1); - pub_turn_signal_ = - private_nh_.advertise("output/turn_signal", 1); - pub_gate_mode_ = private_nh_.advertise("output/gate_mode", 1); - pub_emergency_ = private_nh_.advertise("output/emergency", 1); - pub_autoware_engage_ = private_nh_.advertise("output/autoware_engage", 1); - pub_vehicle_engage_ = private_nh_.advertise("output/vehicle_engage", 1); - - // tmp - pub_vehicle_command_ = - private_nh_.advertise("output/vehicle_cmd", 1); - pub_raw_vehicle_command_ = - private_nh_.advertise("output/raw_vehicle_cmd", 1); - - // Timer - timer_ = - private_nh_.createTimer(ros::Rate(update_rate_), &AutowareJoyControllerNode::onTimer, this); -} diff --git a/control/autoware_joy_controller/src/autoware_joy_controller/main.cpp b/control/autoware_joy_controller/src/autoware_joy_controller/main.cpp deleted file mode 100644 index 1617ed436068e..0000000000000 --- a/control/autoware_joy_controller/src/autoware_joy_controller/main.cpp +++ /dev/null @@ -1,30 +0,0 @@ -/* - * Copyright 2020 Tier IV, Inc. All rights reserved. - * - * 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 - -#include - -int main(int argc, char * argv[]) -{ - ros::init(argc, argv, "autoware_joy_controller"); - - AutowareJoyControllerNode node; - - ros::spin(); - - return 0; -} From 0d65b02fba3eb57566f3bd7d9fec4f5ab48162f1 Mon Sep 17 00:00:00 2001 From: mitsudome-r Date: Thu, 8 Oct 2020 17:24:31 +0900 Subject: [PATCH 08/38] Revert "remove ROS1 packages temporarily" This reverts commit c98294b0b159fb98cd3091d34a626d06f29fdece. Signed-off-by: mitsudome-r --- .../autoware_joy_controller/CMakeLists.txt | 56 +++ .../config/autoware_joy_controller.yaml | 10 + .../autoware_joy_controller.h | 116 +++++ .../joy_converter/ds4_joy_converter.h | 89 ++++ .../joy_converter/g29_joy_converter.h | 85 ++++ .../joy_converter/joy_converter_base.h | 51 +++ .../launch/autoware_joy_controller.launch | 44 ++ control/autoware_joy_controller/package.xml | 21 + .../autoware_joy_controller_node.cpp | 419 ++++++++++++++++++ .../src/autoware_joy_controller/main.cpp | 30 ++ 10 files changed, 921 insertions(+) create mode 100644 control/autoware_joy_controller/CMakeLists.txt create mode 100644 control/autoware_joy_controller/config/autoware_joy_controller.yaml create mode 100644 control/autoware_joy_controller/include/autoware_joy_controller/autoware_joy_controller.h create mode 100644 control/autoware_joy_controller/include/autoware_joy_controller/joy_converter/ds4_joy_converter.h create mode 100644 control/autoware_joy_controller/include/autoware_joy_controller/joy_converter/g29_joy_converter.h create mode 100644 control/autoware_joy_controller/include/autoware_joy_controller/joy_converter/joy_converter_base.h create mode 100644 control/autoware_joy_controller/launch/autoware_joy_controller.launch create mode 100644 control/autoware_joy_controller/package.xml create mode 100644 control/autoware_joy_controller/src/autoware_joy_controller/autoware_joy_controller_node.cpp create mode 100644 control/autoware_joy_controller/src/autoware_joy_controller/main.cpp diff --git a/control/autoware_joy_controller/CMakeLists.txt b/control/autoware_joy_controller/CMakeLists.txt new file mode 100644 index 0000000000000..5f13f9a27a1d7 --- /dev/null +++ b/control/autoware_joy_controller/CMakeLists.txt @@ -0,0 +1,56 @@ +cmake_minimum_required(VERSION 3.0.2) +project(autoware_joy_controller) + +add_compile_options(-std=c++14) + +find_package(catkin REQUIRED COMPONENTS + autoware_control_msgs + autoware_vehicle_msgs + joy + geometry_msgs + roscpp + sensor_msgs + std_msgs +) + +catkin_package() + +include_directories( + include + ${catkin_INCLUDE_DIRS} +) + +add_executable(autoware_joy_controller + src/autoware_joy_controller/main.cpp + src/autoware_joy_controller/autoware_joy_controller_node.cpp +) + +target_link_libraries(autoware_joy_controller + ${catkin_LIBRARIES} +) + +add_dependencies(autoware_joy_controller + ${catkin_EXPORTED_TARGETS} +) + +# Install executables and/or libraries +install( + TARGETS + autoware_joy_controller + ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} + LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} + RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} +) + +# Install project namespaced headers +install(DIRECTORY include/${PROJECT_NAME}/ + DESTINATION ${CATKIN_PACKAGE_INCLUDE_DESTINATION} +) + +# Install directories +install( + DIRECTORY + launch + config + DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION} +) diff --git a/control/autoware_joy_controller/config/autoware_joy_controller.yaml b/control/autoware_joy_controller/config/autoware_joy_controller.yaml new file mode 100644 index 0000000000000..f7ee5a5c4e10e --- /dev/null +++ b/control/autoware_joy_controller/config/autoware_joy_controller.yaml @@ -0,0 +1,10 @@ +update_rate: 10 +accel_ratio: 3.0 +brake_ratio: 5.0 +steer_ratio: 0.5 +steering_angle_velocity: 0.1 +control_command: + velocity_gain: 3.0 + max_forward_velocity: 20 + max_backward_velocity: 3 + backward_accel_ratio: 1.0 diff --git a/control/autoware_joy_controller/include/autoware_joy_controller/autoware_joy_controller.h b/control/autoware_joy_controller/include/autoware_joy_controller/autoware_joy_controller.h new file mode 100644 index 0000000000000..35ad7697a264a --- /dev/null +++ b/control/autoware_joy_controller/include/autoware_joy_controller/autoware_joy_controller.h @@ -0,0 +1,116 @@ +/* + * Copyright 2020 Tier IV, Inc. All rights reserved. + * + * 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. + */ + +#pragma once + +#include +#include +#include + +#include + +#include +#include +#include +#include +#include +#include +#include +#include + +#include + +// tmp +#include +#include + +using ShiftType = autoware_vehicle_msgs::Shift::_data_type; +using TurnSignalType = autoware_vehicle_msgs::TurnSignal::_data_type; +using GateModeType = autoware_control_msgs::GateMode::_data_type; + +class AutowareJoyControllerNode +{ +public: + AutowareJoyControllerNode(); + +private: + // NodeHandle + ros::NodeHandle nh_{""}; + ros::NodeHandle private_nh_{"~"}; + + // Parameter + std::string joy_type_; + double update_rate_; + double accel_ratio_; + double brake_ratio_; + double steer_ratio_; + double steering_angle_velocity_; + + // ControlCommand Parameter + double velocity_gain_; + double max_forward_velocity_; + double max_backward_velocity_; + double backward_accel_ratio_; + + // Subscriber + ros::Subscriber sub_joy_; + ros::Subscriber sub_twist_; + + ros::Time last_joy_received_time_; + std::shared_ptr joy_; + geometry_msgs::TwistStamped::ConstPtr twist_; + + void onJoy(const sensor_msgs::Joy::ConstPtr & msg); + void onTwist(const geometry_msgs::TwistStamped::ConstPtr & msg); + + // Publisher + ros::Publisher pub_control_command_; + ros::Publisher pub_raw_control_command_; + ros::Publisher pub_shift_; + ros::Publisher pub_turn_signal_; + ros::Publisher pub_gate_mode_; + ros::Publisher pub_emergency_; + ros::Publisher pub_autoware_engage_; + ros::Publisher pub_vehicle_engage_; + + void publishControlCommand(); + void publishRawControlCommand(); + void publishShift(); + void publishTurnSignal(); + void publishGateMode(); + void publishEmergency(); + void publishAutowareEngage(); + void publishVehicleEngage(); + + // Previous State + autoware_control_msgs::ControlCommand prev_control_command_; + autoware_vehicle_msgs::RawControlCommand prev_raw_control_command_; + ShiftType prev_shift_ = autoware_vehicle_msgs::Shift::NONE; + TurnSignalType prev_turn_signal_ = autoware_vehicle_msgs::TurnSignal::NONE; + GateModeType prev_gate_mode_ = autoware_control_msgs::GateMode::AUTO; + + // tmp + ros::Publisher pub_vehicle_command_; + ros::Publisher pub_raw_vehicle_command_; + void publishVehicleCommand(); + void publishRawVehicleCommand(); + + // Timer + ros::Timer timer_; + + bool isDataReady(); + void onTimer(const ros::TimerEvent & event); +}; diff --git a/control/autoware_joy_controller/include/autoware_joy_controller/joy_converter/ds4_joy_converter.h b/control/autoware_joy_controller/include/autoware_joy_controller/joy_converter/ds4_joy_converter.h new file mode 100644 index 0000000000000..166623d907551 --- /dev/null +++ b/control/autoware_joy_controller/include/autoware_joy_controller/joy_converter/ds4_joy_converter.h @@ -0,0 +1,89 @@ +/* + * Copyright 2020 Tier IV, Inc. All rights reserved. + * + * 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. + */ + +#pragma once + +#include + +class DS4JoyConverter : public JoyConverterBase +{ +public: + explicit DS4JoyConverter(const sensor_msgs::Joy & j) : j_(j) {} + + const float accel() const + { + const auto button = static_cast(Cross()); + const auto stick = std::max(0.0f, RStickUpDown()); + const auto trigger = std::max(0.0f, -RTrigger()); + return std::max({button, stick, trigger}); + } + + const float brake() const + { + const auto button = static_cast(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() const { return !reverse() && PS(); } + bool clear_emergency() 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::Joy j_; + + bool reverse() const { return Share(); } +}; diff --git a/control/autoware_joy_controller/include/autoware_joy_controller/joy_converter/g29_joy_converter.h b/control/autoware_joy_controller/include/autoware_joy_controller/joy_converter/g29_joy_converter.h new file mode 100644 index 0000000000000..9861a643fb734 --- /dev/null +++ b/control/autoware_joy_controller/include/autoware_joy_controller/joy_converter/g29_joy_converter.h @@ -0,0 +1,85 @@ +/* + * Copyright 2020 Tier IV, Inc. All rights reserved. + * + * 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. + */ + +#pragma once + +#include + +class G29JoyConverter : public JoyConverterBase +{ +public: + explicit G29JoyConverter(const sensor_msgs::Joy & j) : j_(j) {} + + const float accel() const + { + constexpr float eps = 0.0000001; + if(std::fabs(AccelPedal()) < eps) return 0.0f; + return (AccelPedal() + 1.0f) / 2; + } + + const float brake() const + { + constexpr float eps = 0.0000001; + if(std::fabs(BrakePedal()) < eps) return 0.0f; + return (BrakePedal() + 1.0f) / 2; + } + + float steer() const { return Steer(); } + + 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() const { return !reverse() && PS(); } + bool clear_emergency() 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 Steer() const { return j_.axes.at(0); } + float AccelPedal() const { return j_.axes.at(2); } + float BrakePedal() const { return j_.axes.at(3); } + + float CursorLeftRight() const { return j_.axes.at(4); } + float CursorUpDown() const { return j_.axes.at(5); } + + 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(5); } + bool R1() const { return j_.buttons.at(4); } + bool L2() const { return j_.buttons.at(7); } + bool R2() const { return j_.buttons.at(6); } + bool Share() const { return j_.buttons.at(8); } + bool Options() const { return j_.buttons.at(9); } + bool PS() const { return j_.buttons.at(24); } + + const sensor_msgs::Joy j_; + + bool reverse() const { return Share(); } +}; diff --git a/control/autoware_joy_controller/include/autoware_joy_controller/joy_converter/joy_converter_base.h b/control/autoware_joy_controller/include/autoware_joy_controller/joy_converter/joy_converter_base.h new file mode 100644 index 0000000000000..3a2aab8d833ff --- /dev/null +++ b/control/autoware_joy_controller/include/autoware_joy_controller/joy_converter/joy_converter_base.h @@ -0,0 +1,51 @@ +/* + * Copyright 2020 Tier IV, Inc. All rights reserved. + * + * 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. + */ + +#pragma once + +#include + +#include + +class JoyConverterBase +{ +public: + virtual const float accel() const = 0; + + virtual const float brake() const = 0; + + virtual float steer() const = 0; + + virtual bool shift_up() const = 0; + virtual bool shift_down() const = 0; + virtual bool shift_drive() const = 0; + virtual bool shift_reverse() const = 0; + + virtual bool turn_signal_left() const = 0; + virtual bool turn_signal_right() const = 0; + virtual bool clear_turn_signal() const = 0; + + virtual bool gate_mode() const = 0; + + virtual bool emergency() const = 0; + virtual bool clear_emergency() const = 0; + + virtual bool autoware_engage() const = 0; + virtual bool autoware_disengage() const = 0; + + virtual bool vehicle_engage() const = 0; + virtual bool vehicle_disengage() const = 0; +}; diff --git a/control/autoware_joy_controller/launch/autoware_joy_controller.launch b/control/autoware_joy_controller/launch/autoware_joy_controller.launch new file mode 100644 index 0000000000000..9818e14c57a6d --- /dev/null +++ b/control/autoware_joy_controller/launch/autoware_joy_controller.launch @@ -0,0 +1,44 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/control/autoware_joy_controller/package.xml b/control/autoware_joy_controller/package.xml new file mode 100644 index 0000000000000..620f50dbdd515 --- /dev/null +++ b/control/autoware_joy_controller/package.xml @@ -0,0 +1,21 @@ + + + autoware_joy_controller + 0.1.0 + The autoware_joy_controller package + Kenji Miyake + Apache 2 + + catkin + + autoware_control_msgs + autoware_vehicle_msgs + joy + geometry_msgs + roscpp + sensor_msgs + std_msgs + + + + diff --git a/control/autoware_joy_controller/src/autoware_joy_controller/autoware_joy_controller_node.cpp b/control/autoware_joy_controller/src/autoware_joy_controller/autoware_joy_controller_node.cpp new file mode 100644 index 0000000000000..7a7fe7a0d4de6 --- /dev/null +++ b/control/autoware_joy_controller/src/autoware_joy_controller/autoware_joy_controller_node.cpp @@ -0,0 +1,419 @@ +/* + * Copyright 2020 Tier IV, Inc. All rights reserved. + * + * 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 +#include +#include + +namespace +{ +ShiftType getUpperShift(const ShiftType & shift) +{ + using autoware_vehicle_msgs::Shift; + + if (shift == Shift::NONE) return Shift::PARKING; + if (shift == Shift::PARKING) return Shift::REVERSE; + if (shift == Shift::REVERSE) return Shift::NEUTRAL; + if (shift == Shift::NEUTRAL) return Shift::DRIVE; + if (shift == Shift::DRIVE) return Shift::LOW; + if (shift == Shift::LOW) return Shift::LOW; + + return Shift::NONE; +} + +ShiftType getLowerShift(const ShiftType & shift) +{ + using autoware_vehicle_msgs::Shift; + + if (shift == Shift::NONE) return Shift::PARKING; + if (shift == Shift::PARKING) return Shift::PARKING; + if (shift == Shift::REVERSE) return Shift::PARKING; + if (shift == Shift::NEUTRAL) return Shift::REVERSE; + if (shift == Shift::DRIVE) return Shift::NEUTRAL; + if (shift == Shift::LOW) return Shift::DRIVE; + + return Shift::NONE; +} + +const char * getShiftName(const ShiftType & shift) +{ + using autoware_vehicle_msgs::Shift; + + if (shift == Shift::NONE) return "NONE"; + if (shift == Shift::PARKING) return "PARKING"; + if (shift == Shift::REVERSE) return "REVERSE"; + if (shift == Shift::NEUTRAL) return "NEUTRAL"; + if (shift == Shift::DRIVE) return "DRIVE"; + if (shift == Shift::LOW) return "LOW"; + + return "NOT_SUPPORTED"; +} + +const char * getTurnSignalName(const TurnSignalType & turn_signal) +{ + using autoware_vehicle_msgs::TurnSignal; + + if (turn_signal == TurnSignal::NONE) return "NONE"; + if (turn_signal == TurnSignal::LEFT) return "LEFT"; + if (turn_signal == TurnSignal::RIGHT) return "RIGHT"; + if (turn_signal == TurnSignal::HAZARD) return "HAZARD"; + + return "NOT_SUPPORTED"; +} + +const char * getGateModeName(const GateModeType & gate_mode) +{ + using autoware_control_msgs::GateMode; + + if (gate_mode == GateMode::AUTO) return "AUTO"; + if (gate_mode == GateMode::REMOTE) return "REMOTE"; + + return "NOT_SUPPORTED"; +} + +} // namespace + +void AutowareJoyControllerNode::onJoy(const sensor_msgs::Joy::ConstPtr & msg) +{ + last_joy_received_time_ = msg->header.stamp; + if (joy_type_ == "G29") { + joy_ = std::make_shared(*msg); + } else { + joy_ = std::make_shared(*msg); + } + + if (joy_->shift_up() || joy_->shift_down() || joy_->shift_drive() || joy_->shift_reverse()) { + publishShift(); + } + + if (joy_->turn_signal_left() || joy_->turn_signal_right() || joy_->clear_turn_signal()) { + publishTurnSignal(); + } + + if (joy_->gate_mode()) { + publishGateMode(); + } + + if (joy_->emergency() || joy_->clear_emergency()) { + publishEmergency(); + } + + if (joy_->autoware_engage() || joy_->autoware_disengage()) { + publishAutowareEngage(); + } + + if (joy_->vehicle_engage() || joy_->vehicle_disengage()) { + publishVehicleEngage(); + } +} + +void AutowareJoyControllerNode::onTwist(const geometry_msgs::TwistStamped::ConstPtr & msg) +{ + twist_ = msg; +} + +bool AutowareJoyControllerNode::isDataReady() +{ + // Joy + { + if (!joy_) { + ROS_WARN_THROTTLE(1.0, "waiting for joy msg..."); + return false; + } + + constexpr auto timeout = 2.0; + const auto time_diff = ros::Time::now() - last_joy_received_time_; + if (time_diff.toSec() > timeout) { + ROS_WARN_THROTTLE(1.0, "joy msg is timeout"); + return false; + } + } + + // Twist + { + if (!twist_) { + ROS_WARN_THROTTLE(1.0, "waiting for twist msg..."); + return false; + } + + constexpr auto timeout = 0.5; + const auto time_diff = ros::Time::now() - twist_->header.stamp; + if (time_diff.toSec() > timeout) { + ROS_WARN_THROTTLE(1.0, "twist msg is timeout"); + return false; + } + } + + return true; +} + +void AutowareJoyControllerNode::onTimer(const ros::TimerEvent & event) +{ + if (!isDataReady()) { + return; + } + + publishControlCommand(); + publishRawControlCommand(); + + // tmp + publishVehicleCommand(); + publishRawVehicleCommand(); +} + +void AutowareJoyControllerNode::publishControlCommand() +{ + autoware_control_msgs::ControlCommandStamped cmd_stamped; + cmd_stamped.header.stamp = ros::Time::now(); + + { + auto & cmd = cmd_stamped.control; + + cmd.steering_angle = steer_ratio_ * joy_->steer(); + cmd.steering_angle_velocity = steering_angle_velocity_; + + if (joy_->accel()) { + cmd.acceleration = accel_ratio_ * joy_->accel(); + cmd.velocity = twist_->twist.linear.x + velocity_gain_ * cmd.acceleration; + cmd.velocity = std::min(cmd.velocity, max_forward_velocity_); + } + + if (joy_->brake()) { + cmd.velocity = 0.0; + cmd.acceleration = -brake_ratio_ * joy_->brake(); + } + + // Backward + if (joy_->accel() && joy_->brake()) { + cmd.acceleration = backward_accel_ratio_ * joy_->accel(); + cmd.velocity = twist_->twist.linear.x - velocity_gain_ * cmd.acceleration; + cmd.velocity = std::max(cmd.velocity, -max_backward_velocity_); + } + } + + pub_control_command_.publish(cmd_stamped); + prev_control_command_ = cmd_stamped.control; +} + +void AutowareJoyControllerNode::publishRawControlCommand() +{ + autoware_vehicle_msgs::RawControlCommandStamped cmd_stamped; + cmd_stamped.header.stamp = ros::Time::now(); + + { + auto & cmd = cmd_stamped.control; + + cmd.steering_angle = steer_ratio_ * joy_->steer(); + cmd.steering_angle_velocity = steering_angle_velocity_; + cmd.throttle = accel_ratio_ * joy_->accel(); + cmd.brake = brake_ratio_ * joy_->brake(); + } + + pub_raw_control_command_.publish(cmd_stamped); + prev_raw_control_command_ = cmd_stamped.control; +} + +void AutowareJoyControllerNode::publishShift() +{ + using autoware_vehicle_msgs::Shift; + + autoware_vehicle_msgs::ShiftStamped shift_stamped; + shift_stamped.header.stamp = ros::Time::now(); + + { + auto & shift = shift_stamped.shift; + if (joy_->shift_up()) { + shift.data = getUpperShift(prev_shift_); + } + + if (joy_->shift_down()) { + shift.data = getLowerShift(prev_shift_); + } + + if (joy_->shift_drive()) { + shift.data = Shift::DRIVE; + } + + if (joy_->shift_reverse()) { + shift.data = Shift::REVERSE; + } + + ROS_INFO("Shift::%s", getShiftName(shift.data)); + } + + pub_shift_.publish(shift_stamped); + prev_shift_ = shift_stamped.shift.data; +} + +void AutowareJoyControllerNode::publishTurnSignal() +{ + using autoware_vehicle_msgs::TurnSignal; + + TurnSignal turn_signal; + turn_signal.header.stamp = ros::Time::now(); + + if (joy_->turn_signal_left() && joy_->turn_signal_right()) { + turn_signal.data = TurnSignal::HAZARD; + } else if (joy_->turn_signal_left()) { + turn_signal.data = TurnSignal::LEFT; + } else if (joy_->turn_signal_right()) { + turn_signal.data = TurnSignal::RIGHT; + } + + if (joy_->clear_turn_signal()) { + turn_signal.data = TurnSignal::NONE; + } + + ROS_INFO("TurnSignal::%s", getTurnSignalName(turn_signal.data)); + + pub_turn_signal_.publish(turn_signal); +} + +void AutowareJoyControllerNode::publishGateMode() +{ + using autoware_control_msgs::GateMode; + + autoware_control_msgs::GateMode gate_mode; + + if (prev_gate_mode_ == GateMode::AUTO) { + gate_mode.data = GateMode::REMOTE; + } + + if (prev_gate_mode_ == GateMode::REMOTE) { + gate_mode.data = GateMode::AUTO; + } + + ROS_INFO("GateMode::%s", getGateModeName(gate_mode.data)); + + pub_gate_mode_.publish(gate_mode); + prev_gate_mode_ = gate_mode.data; +} + +void AutowareJoyControllerNode::publishEmergency() +{ + std_msgs::Bool emergency; + + if (joy_->emergency()) { + emergency.data = true; + ROS_INFO("Emergency"); + } + + if (joy_->clear_emergency()) { + emergency.data = false; + ROS_INFO("Clear Emergency"); + } + + pub_emergency_.publish(emergency); +} + +void AutowareJoyControllerNode::publishAutowareEngage() +{ + std_msgs::Bool engage; + + if (joy_->autoware_engage()) { + engage.data = true; + ROS_INFO("Autoware Engage"); + } + + if (joy_->autoware_disengage()) { + engage.data = false; + ROS_INFO("Autoware Disengage"); + } + + pub_autoware_engage_.publish(engage); +} + +void AutowareJoyControllerNode::publishVehicleEngage() +{ + std_msgs::Bool engage; + + if (joy_->vehicle_engage()) { + engage.data = true; + ROS_INFO("Vehicle Engage"); + } + + if (joy_->vehicle_disengage()) { + engage.data = false; + ROS_INFO("Vehicle Disengage"); + } + + pub_vehicle_engage_.publish(engage); +} + +// tmp +void AutowareJoyControllerNode::publishVehicleCommand() +{ + autoware_vehicle_msgs::VehicleCommand vehicle_cmd; + vehicle_cmd.header.stamp = ros::Time::now(); + vehicle_cmd.control = prev_control_command_; + vehicle_cmd.shift.data = prev_shift_; + + pub_vehicle_command_.publish(vehicle_cmd); +} + +void AutowareJoyControllerNode::publishRawVehicleCommand() +{ + autoware_vehicle_msgs::RawVehicleCommand vehicle_cmd; + vehicle_cmd.header.stamp = ros::Time::now(); + vehicle_cmd.control = prev_raw_control_command_; + vehicle_cmd.shift.data = prev_shift_; + + pub_raw_vehicle_command_.publish(vehicle_cmd); +} + +AutowareJoyControllerNode::AutowareJoyControllerNode() +{ + // Parameter + private_nh_.param("joy_type", joy_type_, "DS4"); + private_nh_.param("update_rate", update_rate_, 10.0); + private_nh_.param("accel_ratio", accel_ratio_, 3.0); + private_nh_.param("brake_ratio", brake_ratio_, 5.0); + private_nh_.param("steer_ratio", steer_ratio_, 0.5); + private_nh_.param("steering_angle_velocity", steering_angle_velocity_, 0.1); + private_nh_.param("control_command/velocity_gain", velocity_gain_, 3.0); + private_nh_.param("control_command/max_forward_velocity", max_forward_velocity_, 20.0); + private_nh_.param("control_command/max_backward_velocity", max_backward_velocity_, 3.0); + private_nh_.param("control_command/backward_accel_ratio", backward_accel_ratio_, 1.0); + + ROS_INFO("Joy type: %s", joy_type_.c_str()); + + // Subscriber + sub_joy_ = private_nh_.subscribe("input/joy", 1, &AutowareJoyControllerNode::onJoy, this); + sub_twist_ = private_nh_.subscribe("input/twist", 1, &AutowareJoyControllerNode::onTwist, this); + + // Publisher + pub_control_command_ = private_nh_.advertise( + "output/control_command", 1); + pub_raw_control_command_ = private_nh_.advertise( + "output/raw_control_command", 1); + pub_shift_ = private_nh_.advertise("output/shift", 1); + pub_turn_signal_ = + private_nh_.advertise("output/turn_signal", 1); + pub_gate_mode_ = private_nh_.advertise("output/gate_mode", 1); + pub_emergency_ = private_nh_.advertise("output/emergency", 1); + pub_autoware_engage_ = private_nh_.advertise("output/autoware_engage", 1); + pub_vehicle_engage_ = private_nh_.advertise("output/vehicle_engage", 1); + + // tmp + pub_vehicle_command_ = + private_nh_.advertise("output/vehicle_cmd", 1); + pub_raw_vehicle_command_ = + private_nh_.advertise("output/raw_vehicle_cmd", 1); + + // Timer + timer_ = + private_nh_.createTimer(ros::Rate(update_rate_), &AutowareJoyControllerNode::onTimer, this); +} diff --git a/control/autoware_joy_controller/src/autoware_joy_controller/main.cpp b/control/autoware_joy_controller/src/autoware_joy_controller/main.cpp new file mode 100644 index 0000000000000..2c5e549236f76 --- /dev/null +++ b/control/autoware_joy_controller/src/autoware_joy_controller/main.cpp @@ -0,0 +1,30 @@ +/* + * Copyright 2020 Tier IV, Inc. All rights reserved. + * + * 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 + +#include + +int main(int argc, char * argv[]) +{ + ros::init(argc, argv, "autoware_joy_controller"); + + AutowareJoyControllerNode node; + + ros::spin(); + + return 0; +} From 6c94e2ccfceaf65cac97c124d004c429e8d6a7d1 Mon Sep 17 00:00:00 2001 From: mitsudome-r Date: Thu, 8 Oct 2020 17:27:14 +0900 Subject: [PATCH 09/38] add COLCON_IGNORE to ros1 packages Signed-off-by: mitsudome-r --- control/autoware_joy_controller/COLCON_IGNORE | 0 1 file changed, 0 insertions(+), 0 deletions(-) create mode 100644 control/autoware_joy_controller/COLCON_IGNORE diff --git a/control/autoware_joy_controller/COLCON_IGNORE b/control/autoware_joy_controller/COLCON_IGNORE new file mode 100644 index 0000000000000..e69de29bb2d1d From fa8b02985bf6d91f038c5155b9c788e301a2062f Mon Sep 17 00:00:00 2001 From: Nikolai Morin Date: Thu, 15 Oct 2020 14:28:52 +0200 Subject: [PATCH 10/38] Rename launch files to launch.xml (#28) --- ...e_joy_controller.launch => autoware_joy_controller.launch.xml} | 0 1 file changed, 0 insertions(+), 0 deletions(-) rename control/autoware_joy_controller/launch/{autoware_joy_controller.launch => autoware_joy_controller.launch.xml} (100%) diff --git a/control/autoware_joy_controller/launch/autoware_joy_controller.launch b/control/autoware_joy_controller/launch/autoware_joy_controller.launch.xml similarity index 100% rename from control/autoware_joy_controller/launch/autoware_joy_controller.launch rename to control/autoware_joy_controller/launch/autoware_joy_controller.launch.xml From 1929d9f2403493155580bd06d6575b5903072094 Mon Sep 17 00:00:00 2001 From: Nikolai Morin Date: Thu, 3 Dec 2020 14:26:39 +0100 Subject: [PATCH 11/38] Rename h files to hpp (#142) * Change includes * Rename files * Adjustments to make things compile * Other packages --- ...utoware_joy_controller.h => autoware_joy_controller.hpp} | 2 +- .../{ds4_joy_converter.h => ds4_joy_converter.hpp} | 2 +- .../{g29_joy_converter.h => g29_joy_converter.hpp} | 2 +- .../{joy_converter_base.h => joy_converter_base.hpp} | 0 .../autoware_joy_controller_node.cpp | 6 +++--- .../src/autoware_joy_controller/main.cpp | 2 +- 6 files changed, 7 insertions(+), 7 deletions(-) rename control/autoware_joy_controller/include/autoware_joy_controller/{autoware_joy_controller.h => autoware_joy_controller.hpp} (99%) rename control/autoware_joy_controller/include/autoware_joy_controller/joy_converter/{ds4_joy_converter.h => ds4_joy_converter.hpp} (99%) rename control/autoware_joy_controller/include/autoware_joy_controller/joy_converter/{g29_joy_converter.h => g29_joy_converter.hpp} (99%) rename control/autoware_joy_controller/include/autoware_joy_controller/joy_converter/{joy_converter_base.h => joy_converter_base.hpp} (100%) diff --git a/control/autoware_joy_controller/include/autoware_joy_controller/autoware_joy_controller.h b/control/autoware_joy_controller/include/autoware_joy_controller/autoware_joy_controller.hpp similarity index 99% rename from control/autoware_joy_controller/include/autoware_joy_controller/autoware_joy_controller.h rename to control/autoware_joy_controller/include/autoware_joy_controller/autoware_joy_controller.hpp index 35ad7697a264a..602c9357ec807 100644 --- a/control/autoware_joy_controller/include/autoware_joy_controller/autoware_joy_controller.h +++ b/control/autoware_joy_controller/include/autoware_joy_controller/autoware_joy_controller.hpp @@ -31,7 +31,7 @@ #include #include -#include +#include // tmp #include diff --git a/control/autoware_joy_controller/include/autoware_joy_controller/joy_converter/ds4_joy_converter.h b/control/autoware_joy_controller/include/autoware_joy_controller/joy_converter/ds4_joy_converter.hpp similarity index 99% rename from control/autoware_joy_controller/include/autoware_joy_controller/joy_converter/ds4_joy_converter.h rename to control/autoware_joy_controller/include/autoware_joy_controller/joy_converter/ds4_joy_converter.hpp index 166623d907551..7e57e2d1bde25 100644 --- a/control/autoware_joy_controller/include/autoware_joy_controller/joy_converter/ds4_joy_converter.h +++ b/control/autoware_joy_controller/include/autoware_joy_controller/joy_converter/ds4_joy_converter.hpp @@ -16,7 +16,7 @@ #pragma once -#include +#include class DS4JoyConverter : public JoyConverterBase { diff --git a/control/autoware_joy_controller/include/autoware_joy_controller/joy_converter/g29_joy_converter.h b/control/autoware_joy_controller/include/autoware_joy_controller/joy_converter/g29_joy_converter.hpp similarity index 99% rename from control/autoware_joy_controller/include/autoware_joy_controller/joy_converter/g29_joy_converter.h rename to control/autoware_joy_controller/include/autoware_joy_controller/joy_converter/g29_joy_converter.hpp index 9861a643fb734..6631b731e4245 100644 --- a/control/autoware_joy_controller/include/autoware_joy_controller/joy_converter/g29_joy_converter.h +++ b/control/autoware_joy_controller/include/autoware_joy_controller/joy_converter/g29_joy_converter.hpp @@ -16,7 +16,7 @@ #pragma once -#include +#include class G29JoyConverter : public JoyConverterBase { diff --git a/control/autoware_joy_controller/include/autoware_joy_controller/joy_converter/joy_converter_base.h b/control/autoware_joy_controller/include/autoware_joy_controller/joy_converter/joy_converter_base.hpp similarity index 100% rename from control/autoware_joy_controller/include/autoware_joy_controller/joy_converter/joy_converter_base.h rename to control/autoware_joy_controller/include/autoware_joy_controller/joy_converter/joy_converter_base.hpp diff --git a/control/autoware_joy_controller/src/autoware_joy_controller/autoware_joy_controller_node.cpp b/control/autoware_joy_controller/src/autoware_joy_controller/autoware_joy_controller_node.cpp index 7a7fe7a0d4de6..0374966c6dfe9 100644 --- a/control/autoware_joy_controller/src/autoware_joy_controller/autoware_joy_controller_node.cpp +++ b/control/autoware_joy_controller/src/autoware_joy_controller/autoware_joy_controller_node.cpp @@ -14,9 +14,9 @@ * limitations under the License. */ -#include -#include -#include +#include +#include +#include namespace { diff --git a/control/autoware_joy_controller/src/autoware_joy_controller/main.cpp b/control/autoware_joy_controller/src/autoware_joy_controller/main.cpp index 2c5e549236f76..160ef9ccc271f 100644 --- a/control/autoware_joy_controller/src/autoware_joy_controller/main.cpp +++ b/control/autoware_joy_controller/src/autoware_joy_controller/main.cpp @@ -16,7 +16,7 @@ #include -#include +#include int main(int argc, char * argv[]) { From 443a14725414f6081f2315358fc22f8b62e43373 Mon Sep 17 00:00:00 2001 From: Nikolai Morin Date: Thu, 3 Dec 2020 17:21:16 +0100 Subject: [PATCH 12/38] Adjust copyright notice on 532 out of 699 source files (#143) --- .../autoware_joy_controller.hpp | 28 +++++++++---------- .../joy_converter/ds4_joy_converter.hpp | 28 +++++++++---------- .../joy_converter/g29_joy_converter.hpp | 28 +++++++++---------- .../joy_converter/joy_converter_base.hpp | 28 +++++++++---------- .../autoware_joy_controller_node.cpp | 28 +++++++++---------- .../src/autoware_joy_controller/main.cpp | 28 +++++++++---------- 6 files changed, 78 insertions(+), 90 deletions(-) diff --git a/control/autoware_joy_controller/include/autoware_joy_controller/autoware_joy_controller.hpp b/control/autoware_joy_controller/include/autoware_joy_controller/autoware_joy_controller.hpp index 602c9357ec807..3148421d6d86b 100644 --- a/control/autoware_joy_controller/include/autoware_joy_controller/autoware_joy_controller.hpp +++ b/control/autoware_joy_controller/include/autoware_joy_controller/autoware_joy_controller.hpp @@ -1,18 +1,16 @@ -/* - * Copyright 2020 Tier IV, Inc. All rights reserved. - * - * 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. - */ +// 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. #pragma once diff --git a/control/autoware_joy_controller/include/autoware_joy_controller/joy_converter/ds4_joy_converter.hpp b/control/autoware_joy_controller/include/autoware_joy_controller/joy_converter/ds4_joy_converter.hpp index 7e57e2d1bde25..98d77d967b592 100644 --- a/control/autoware_joy_controller/include/autoware_joy_controller/joy_converter/ds4_joy_converter.hpp +++ b/control/autoware_joy_controller/include/autoware_joy_controller/joy_converter/ds4_joy_converter.hpp @@ -1,18 +1,16 @@ -/* - * Copyright 2020 Tier IV, Inc. All rights reserved. - * - * 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. - */ +// 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. #pragma once diff --git a/control/autoware_joy_controller/include/autoware_joy_controller/joy_converter/g29_joy_converter.hpp b/control/autoware_joy_controller/include/autoware_joy_controller/joy_converter/g29_joy_converter.hpp index 6631b731e4245..d44e9cc3da9e9 100644 --- a/control/autoware_joy_controller/include/autoware_joy_controller/joy_converter/g29_joy_converter.hpp +++ b/control/autoware_joy_controller/include/autoware_joy_controller/joy_converter/g29_joy_converter.hpp @@ -1,18 +1,16 @@ -/* - * Copyright 2020 Tier IV, Inc. All rights reserved. - * - * 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. - */ +// 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. #pragma once diff --git a/control/autoware_joy_controller/include/autoware_joy_controller/joy_converter/joy_converter_base.hpp b/control/autoware_joy_controller/include/autoware_joy_controller/joy_converter/joy_converter_base.hpp index 3a2aab8d833ff..ac908c97d292e 100644 --- a/control/autoware_joy_controller/include/autoware_joy_controller/joy_converter/joy_converter_base.hpp +++ b/control/autoware_joy_controller/include/autoware_joy_controller/joy_converter/joy_converter_base.hpp @@ -1,18 +1,16 @@ -/* - * Copyright 2020 Tier IV, Inc. All rights reserved. - * - * 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. - */ +// 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. #pragma once diff --git a/control/autoware_joy_controller/src/autoware_joy_controller/autoware_joy_controller_node.cpp b/control/autoware_joy_controller/src/autoware_joy_controller/autoware_joy_controller_node.cpp index 0374966c6dfe9..c81354f5cf504 100644 --- a/control/autoware_joy_controller/src/autoware_joy_controller/autoware_joy_controller_node.cpp +++ b/control/autoware_joy_controller/src/autoware_joy_controller/autoware_joy_controller_node.cpp @@ -1,18 +1,16 @@ -/* - * Copyright 2020 Tier IV, Inc. All rights reserved. - * - * 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. - */ +// 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. #include #include diff --git a/control/autoware_joy_controller/src/autoware_joy_controller/main.cpp b/control/autoware_joy_controller/src/autoware_joy_controller/main.cpp index 160ef9ccc271f..98b2e1ec70e57 100644 --- a/control/autoware_joy_controller/src/autoware_joy_controller/main.cpp +++ b/control/autoware_joy_controller/src/autoware_joy_controller/main.cpp @@ -1,18 +1,16 @@ -/* - * Copyright 2020 Tier IV, Inc. All rights reserved. - * - * 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. - */ +// 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. #include From 44e70c4ccd2048df67e22e964584cd4995fe7797 Mon Sep 17 00:00:00 2001 From: Nikolai Morin Date: Mon, 7 Dec 2020 09:52:36 +0100 Subject: [PATCH 13/38] Use quotes for includes where appropriate (#144) * Use quotes for includes where appropriate * Fix lint tests * Make tests pass hopefully --- .../autoware_joy_controller.hpp | 24 +++++++++---------- .../joy_converter/ds4_joy_converter.hpp | 2 +- .../joy_converter/g29_joy_converter.hpp | 2 +- .../joy_converter/joy_converter_base.hpp | 2 +- .../autoware_joy_controller_node.cpp | 6 ++--- .../src/autoware_joy_controller/main.cpp | 4 ++-- 6 files changed, 20 insertions(+), 20 deletions(-) diff --git a/control/autoware_joy_controller/include/autoware_joy_controller/autoware_joy_controller.hpp b/control/autoware_joy_controller/include/autoware_joy_controller/autoware_joy_controller.hpp index 3148421d6d86b..96c7280ee6540 100644 --- a/control/autoware_joy_controller/include/autoware_joy_controller/autoware_joy_controller.hpp +++ b/control/autoware_joy_controller/include/autoware_joy_controller/autoware_joy_controller.hpp @@ -18,22 +18,22 @@ #include #include -#include +#include "ros/ros.h" -#include -#include -#include -#include -#include -#include -#include -#include +#include "autoware_control_msgs/ControlCommandStamped.h" +#include "autoware_control_msgs/GateMode.h" +#include "autoware_vehicle_msgs/RawControlCommandStamped.h" +#include "autoware_vehicle_msgs/ShiftStamped.h" +#include "autoware_vehicle_msgs/TurnSignal.h" +#include "geometry_msgs/TwistStamped.h" +#include "sensor_msgs/Joy.h" +#include "std_msgs/Bool.h" -#include +#include "autoware_joy_controller/joy_converter/joy_converter_base.hpp" // tmp -#include -#include +#include "autoware_vehicle_msgs/RawVehicleCommand.h" +#include "autoware_vehicle_msgs/VehicleCommand.h" using ShiftType = autoware_vehicle_msgs::Shift::_data_type; using TurnSignalType = autoware_vehicle_msgs::TurnSignal::_data_type; diff --git a/control/autoware_joy_controller/include/autoware_joy_controller/joy_converter/ds4_joy_converter.hpp b/control/autoware_joy_controller/include/autoware_joy_controller/joy_converter/ds4_joy_converter.hpp index 98d77d967b592..093b6b290faab 100644 --- a/control/autoware_joy_controller/include/autoware_joy_controller/joy_converter/ds4_joy_converter.hpp +++ b/control/autoware_joy_controller/include/autoware_joy_controller/joy_converter/ds4_joy_converter.hpp @@ -14,7 +14,7 @@ #pragma once -#include +#include "autoware_joy_controller/joy_converter/joy_converter_base.hpp" class DS4JoyConverter : public JoyConverterBase { diff --git a/control/autoware_joy_controller/include/autoware_joy_controller/joy_converter/g29_joy_converter.hpp b/control/autoware_joy_controller/include/autoware_joy_controller/joy_converter/g29_joy_converter.hpp index d44e9cc3da9e9..9a43d2ecba865 100644 --- a/control/autoware_joy_controller/include/autoware_joy_controller/joy_converter/g29_joy_converter.hpp +++ b/control/autoware_joy_controller/include/autoware_joy_controller/joy_converter/g29_joy_converter.hpp @@ -14,7 +14,7 @@ #pragma once -#include +#include "autoware_joy_controller/joy_converter/joy_converter_base.hpp" class G29JoyConverter : public JoyConverterBase { diff --git a/control/autoware_joy_controller/include/autoware_joy_controller/joy_converter/joy_converter_base.hpp b/control/autoware_joy_controller/include/autoware_joy_controller/joy_converter/joy_converter_base.hpp index ac908c97d292e..d745dfdb85496 100644 --- a/control/autoware_joy_controller/include/autoware_joy_controller/joy_converter/joy_converter_base.hpp +++ b/control/autoware_joy_controller/include/autoware_joy_controller/joy_converter/joy_converter_base.hpp @@ -16,7 +16,7 @@ #include -#include +#include "sensor_msgs/Joy.h" class JoyConverterBase { diff --git a/control/autoware_joy_controller/src/autoware_joy_controller/autoware_joy_controller_node.cpp b/control/autoware_joy_controller/src/autoware_joy_controller/autoware_joy_controller_node.cpp index c81354f5cf504..ccc969f1ba164 100644 --- a/control/autoware_joy_controller/src/autoware_joy_controller/autoware_joy_controller_node.cpp +++ b/control/autoware_joy_controller/src/autoware_joy_controller/autoware_joy_controller_node.cpp @@ -12,9 +12,9 @@ // See the License for the specific language governing permissions and // limitations under the License. -#include -#include -#include +#include "autoware_joy_controller/autoware_joy_controller.hpp" +#include "autoware_joy_controller/joy_converter/g29_joy_converter.hpp" +#include "autoware_joy_controller/joy_converter/ds4_joy_converter.hpp" namespace { diff --git a/control/autoware_joy_controller/src/autoware_joy_controller/main.cpp b/control/autoware_joy_controller/src/autoware_joy_controller/main.cpp index 98b2e1ec70e57..c41a8aaf6a3cb 100644 --- a/control/autoware_joy_controller/src/autoware_joy_controller/main.cpp +++ b/control/autoware_joy_controller/src/autoware_joy_controller/main.cpp @@ -12,9 +12,9 @@ // See the License for the specific language governing permissions and // limitations under the License. -#include +#include "ros/ros.h" -#include +#include "autoware_joy_controller/autoware_joy_controller.hpp" int main(int argc, char * argv[]) { From 75ede3bc6c5d8a05b3dde52f442fbac1af4d9c3c Mon Sep 17 00:00:00 2001 From: Servando <43142004+sgermanserrano@users.noreply.github.com> Date: Tue, 8 Dec 2020 02:15:13 +0000 Subject: [PATCH 14/38] Port autoware joy controller (#124) * Port Signed-off-by: Servando German Serrano * Fixed package.xml * now() to use node clock Signed-off-by: Servando German Serrano * Fix include Signed-off-by: Servando German Serrano * Clear compilation warnings Signed-off-by: Servando German Serrano --- .../autoware_joy_controller/CMakeLists.txt | 61 ++---- control/autoware_joy_controller/COLCON_IGNORE | 0 .../config/autoware_joy_controller.yaml | 22 ++- .../autoware_joy_controller.hpp | 86 ++++----- .../joy_converter/ds4_joy_converter.hpp | 8 +- .../joy_converter/g29_joy_converter.hpp | 8 +- .../joy_converter/joy_converter_base.hpp | 6 +- .../launch/autoware_joy_controller.launch.xml | 42 ++--- control/autoware_joy_controller/package.xml | 9 +- .../autoware_joy_controller_node.cpp | 177 ++++++++++-------- .../src/autoware_joy_controller/main.cpp | 10 +- 11 files changed, 205 insertions(+), 224 deletions(-) delete mode 100644 control/autoware_joy_controller/COLCON_IGNORE diff --git a/control/autoware_joy_controller/CMakeLists.txt b/control/autoware_joy_controller/CMakeLists.txt index 5f13f9a27a1d7..553c840738536 100644 --- a/control/autoware_joy_controller/CMakeLists.txt +++ b/control/autoware_joy_controller/CMakeLists.txt @@ -1,56 +1,23 @@ -cmake_minimum_required(VERSION 3.0.2) +cmake_minimum_required(VERSION 3.5) project(autoware_joy_controller) -add_compile_options(-std=c++14) +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 -Wno-unused-parameter) +endif() -find_package(catkin REQUIRED COMPONENTS - autoware_control_msgs - autoware_vehicle_msgs - joy - geometry_msgs - roscpp - sensor_msgs - std_msgs -) - -catkin_package() - -include_directories( - include - ${catkin_INCLUDE_DIRS} -) +find_package(ament_cmake_auto REQUIRED) +ament_auto_find_build_dependencies() -add_executable(autoware_joy_controller +ament_auto_add_executable(autoware_joy_controller src/autoware_joy_controller/main.cpp src/autoware_joy_controller/autoware_joy_controller_node.cpp ) -target_link_libraries(autoware_joy_controller - ${catkin_LIBRARIES} -) - -add_dependencies(autoware_joy_controller - ${catkin_EXPORTED_TARGETS} -) - -# Install executables and/or libraries -install( - TARGETS - autoware_joy_controller - ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} - LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} - RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} -) - -# Install project namespaced headers -install(DIRECTORY include/${PROJECT_NAME}/ - DESTINATION ${CATKIN_PACKAGE_INCLUDE_DESTINATION} -) - -# Install directories -install( - DIRECTORY - launch - config - DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION} +ament_auto_package( + INSTALL_TO_SHARE + launch + config ) diff --git a/control/autoware_joy_controller/COLCON_IGNORE b/control/autoware_joy_controller/COLCON_IGNORE deleted file mode 100644 index e69de29bb2d1d..0000000000000 diff --git a/control/autoware_joy_controller/config/autoware_joy_controller.yaml b/control/autoware_joy_controller/config/autoware_joy_controller.yaml index f7ee5a5c4e10e..8e2b0564f6a17 100644 --- a/control/autoware_joy_controller/config/autoware_joy_controller.yaml +++ b/control/autoware_joy_controller/config/autoware_joy_controller.yaml @@ -1,10 +1,12 @@ -update_rate: 10 -accel_ratio: 3.0 -brake_ratio: 5.0 -steer_ratio: 0.5 -steering_angle_velocity: 0.1 -control_command: - velocity_gain: 3.0 - max_forward_velocity: 20 - max_backward_velocity: 3 - backward_accel_ratio: 1.0 +/**: + ros__parameters: + update_rate: 10.0 + accel_ratio: 3.0 + brake_ratio: 5.0 + steer_ratio: 0.5 + steering_angle_velocity: 0.1 + control_command: + velocity_gain: 3.0 + max_forward_velocity: 20.0 + max_backward_velocity: 3.0 + backward_accel_ratio: 1.0 diff --git a/control/autoware_joy_controller/include/autoware_joy_controller/autoware_joy_controller.hpp b/control/autoware_joy_controller/include/autoware_joy_controller/autoware_joy_controller.hpp index 96c7280ee6540..6da5498e3b97e 100644 --- a/control/autoware_joy_controller/include/autoware_joy_controller/autoware_joy_controller.hpp +++ b/control/autoware_joy_controller/include/autoware_joy_controller/autoware_joy_controller.hpp @@ -18,37 +18,30 @@ #include #include -#include "ros/ros.h" - -#include "autoware_control_msgs/ControlCommandStamped.h" -#include "autoware_control_msgs/GateMode.h" -#include "autoware_vehicle_msgs/RawControlCommandStamped.h" -#include "autoware_vehicle_msgs/ShiftStamped.h" -#include "autoware_vehicle_msgs/TurnSignal.h" -#include "geometry_msgs/TwistStamped.h" -#include "sensor_msgs/Joy.h" -#include "std_msgs/Bool.h" - +#include "rclcpp/rclcpp.hpp" + +#include "autoware_control_msgs/msg/control_command_stamped.hpp" +#include "autoware_control_msgs/msg/gate_mode.hpp" +#include "autoware_debug_msgs/msg/bool_stamped.hpp" +#include "autoware_vehicle_msgs/msg/raw_control_command_stamped.hpp" +#include "autoware_vehicle_msgs/msg/shift_stamped.hpp" +#include "autoware_vehicle_msgs/msg/turn_signal.hpp" +#include "geometry_msgs/msg/twist_stamped.hpp" +#include "sensor_msgs/msg/joy.hpp" #include "autoware_joy_controller/joy_converter/joy_converter_base.hpp" +#include "autoware_vehicle_msgs/msg/raw_vehicle_command.hpp" +#include "autoware_vehicle_msgs/msg/vehicle_command.hpp" -// tmp -#include "autoware_vehicle_msgs/RawVehicleCommand.h" -#include "autoware_vehicle_msgs/VehicleCommand.h" +using ShiftType = autoware_vehicle_msgs::msg::Shift::_data_type; +using TurnSignalType = autoware_vehicle_msgs::msg::TurnSignal::_data_type; +using GateModeType = autoware_control_msgs::msg::GateMode::_data_type; -using ShiftType = autoware_vehicle_msgs::Shift::_data_type; -using TurnSignalType = autoware_vehicle_msgs::TurnSignal::_data_type; -using GateModeType = autoware_control_msgs::GateMode::_data_type; - -class AutowareJoyControllerNode +class AutowareJoyControllerNode : public rclcpp::Node { public: AutowareJoyControllerNode(); private: - // NodeHandle - ros::NodeHandle nh_{""}; - ros::NodeHandle private_nh_{"~"}; - // Parameter std::string joy_type_; double update_rate_; @@ -64,25 +57,25 @@ class AutowareJoyControllerNode double backward_accel_ratio_; // Subscriber - ros::Subscriber sub_joy_; - ros::Subscriber sub_twist_; + rclcpp::Subscription::SharedPtr sub_joy_; + rclcpp::Subscription::SharedPtr sub_twist_; - ros::Time last_joy_received_time_; + rclcpp::Time last_joy_received_time_; std::shared_ptr joy_; - geometry_msgs::TwistStamped::ConstPtr twist_; + geometry_msgs::msg::TwistStamped::ConstSharedPtr twist_; - void onJoy(const sensor_msgs::Joy::ConstPtr & msg); - void onTwist(const geometry_msgs::TwistStamped::ConstPtr & msg); + void onJoy(const sensor_msgs::msg::Joy::ConstSharedPtr msg); + void onTwist(const geometry_msgs::msg::TwistStamped::ConstSharedPtr msg); // Publisher - ros::Publisher pub_control_command_; - ros::Publisher pub_raw_control_command_; - ros::Publisher pub_shift_; - ros::Publisher pub_turn_signal_; - ros::Publisher pub_gate_mode_; - ros::Publisher pub_emergency_; - ros::Publisher pub_autoware_engage_; - ros::Publisher pub_vehicle_engage_; + rclcpp::Publisher::SharedPtr pub_control_command_; + rclcpp::Publisher::SharedPtr pub_raw_control_command_; + rclcpp::Publisher::SharedPtr pub_shift_; + rclcpp::Publisher::SharedPtr pub_turn_signal_; + rclcpp::Publisher::SharedPtr pub_gate_mode_; + rclcpp::Publisher::SharedPtr pub_emergency_; + rclcpp::Publisher::SharedPtr pub_autoware_engage_; + rclcpp::Publisher::SharedPtr pub_vehicle_engage_; void publishControlCommand(); void publishRawControlCommand(); @@ -94,21 +87,22 @@ class AutowareJoyControllerNode void publishVehicleEngage(); // Previous State - autoware_control_msgs::ControlCommand prev_control_command_; - autoware_vehicle_msgs::RawControlCommand prev_raw_control_command_; - ShiftType prev_shift_ = autoware_vehicle_msgs::Shift::NONE; - TurnSignalType prev_turn_signal_ = autoware_vehicle_msgs::TurnSignal::NONE; - GateModeType prev_gate_mode_ = autoware_control_msgs::GateMode::AUTO; + autoware_control_msgs::msg::ControlCommand prev_control_command_; + autoware_vehicle_msgs::msg::RawControlCommand prev_raw_control_command_; + ShiftType prev_shift_ = autoware_vehicle_msgs::msg::Shift::NONE; + TurnSignalType prev_turn_signal_ = autoware_vehicle_msgs::msg::TurnSignal::NONE; + GateModeType prev_gate_mode_ = autoware_control_msgs::msg::GateMode::AUTO; // tmp - ros::Publisher pub_vehicle_command_; - ros::Publisher pub_raw_vehicle_command_; + rclcpp::Publisher::SharedPtr pub_vehicle_command_; + rclcpp::Publisher::SharedPtr pub_raw_vehicle_command_; void publishVehicleCommand(); void publishRawVehicleCommand(); // Timer - ros::Timer timer_; + rclcpp::TimerBase::SharedPtr timer_; + void initTimer(double period_s); bool isDataReady(); - void onTimer(const ros::TimerEvent & event); + void onTimer(); }; diff --git a/control/autoware_joy_controller/include/autoware_joy_controller/joy_converter/ds4_joy_converter.hpp b/control/autoware_joy_controller/include/autoware_joy_controller/joy_converter/ds4_joy_converter.hpp index 093b6b290faab..e3971db73d77b 100644 --- a/control/autoware_joy_controller/include/autoware_joy_controller/joy_converter/ds4_joy_converter.hpp +++ b/control/autoware_joy_controller/include/autoware_joy_controller/joy_converter/ds4_joy_converter.hpp @@ -19,9 +19,9 @@ class DS4JoyConverter : public JoyConverterBase { public: - explicit DS4JoyConverter(const sensor_msgs::Joy & j) : j_(j) {} + explicit DS4JoyConverter(const sensor_msgs::msg::Joy & j) : j_(j) {} - const float accel() const + float accel() const { const auto button = static_cast(Cross()); const auto stick = std::max(0.0f, RStickUpDown()); @@ -29,7 +29,7 @@ class DS4JoyConverter : public JoyConverterBase return std::max({button, stick, trigger}); } - const float brake() const + float brake() const { const auto button = static_cast(Square()); const auto stick = std::max(0.0f, -RStickUpDown()); @@ -81,7 +81,7 @@ class DS4JoyConverter : public JoyConverterBase bool Options() const { return j_.buttons.at(9); } bool PS() const { return j_.buttons.at(10); } - const sensor_msgs::Joy j_; + const sensor_msgs::msg::Joy j_; bool reverse() const { return Share(); } }; diff --git a/control/autoware_joy_controller/include/autoware_joy_controller/joy_converter/g29_joy_converter.hpp b/control/autoware_joy_controller/include/autoware_joy_controller/joy_converter/g29_joy_converter.hpp index 9a43d2ecba865..c9ab723a47f4c 100644 --- a/control/autoware_joy_controller/include/autoware_joy_controller/joy_converter/g29_joy_converter.hpp +++ b/control/autoware_joy_controller/include/autoware_joy_controller/joy_converter/g29_joy_converter.hpp @@ -19,16 +19,16 @@ class G29JoyConverter : public JoyConverterBase { public: - explicit G29JoyConverter(const sensor_msgs::Joy & j) : j_(j) {} + explicit G29JoyConverter(const sensor_msgs::msg::Joy & j) : j_(j) {} - const float accel() const + float accel() const { constexpr float eps = 0.0000001; if(std::fabs(AccelPedal()) < eps) return 0.0f; return (AccelPedal() + 1.0f) / 2; } - const float brake() const + float brake() const { constexpr float eps = 0.0000001; if(std::fabs(BrakePedal()) < eps) return 0.0f; @@ -77,7 +77,7 @@ class G29JoyConverter : public JoyConverterBase bool Options() const { return j_.buttons.at(9); } bool PS() const { return j_.buttons.at(24); } - const sensor_msgs::Joy j_; + const sensor_msgs::msg::Joy j_; bool reverse() const { return Share(); } }; diff --git a/control/autoware_joy_controller/include/autoware_joy_controller/joy_converter/joy_converter_base.hpp b/control/autoware_joy_controller/include/autoware_joy_controller/joy_converter/joy_converter_base.hpp index d745dfdb85496..a2774527f3fc8 100644 --- a/control/autoware_joy_controller/include/autoware_joy_controller/joy_converter/joy_converter_base.hpp +++ b/control/autoware_joy_controller/include/autoware_joy_controller/joy_converter/joy_converter_base.hpp @@ -16,14 +16,14 @@ #include -#include "sensor_msgs/Joy.h" +#include "sensor_msgs/msg/joy.hpp" class JoyConverterBase { public: - virtual const float accel() const = 0; + virtual float accel() const = 0; - virtual const float brake() const = 0; + virtual float brake() const = 0; virtual float steer() const = 0; diff --git a/control/autoware_joy_controller/launch/autoware_joy_controller.launch.xml b/control/autoware_joy_controller/launch/autoware_joy_controller.launch.xml index 9818e14c57a6d..06a730249c1e0 100644 --- a/control/autoware_joy_controller/launch/autoware_joy_controller.launch.xml +++ b/control/autoware_joy_controller/launch/autoware_joy_controller.launch.xml @@ -13,32 +13,32 @@ - - - - - - - - - - - - - - - + + + + + + + + + + + + + + + - - + + - - + + - - + + diff --git a/control/autoware_joy_controller/package.xml b/control/autoware_joy_controller/package.xml index 620f50dbdd515..a6c7cd0a9331c 100644 --- a/control/autoware_joy_controller/package.xml +++ b/control/autoware_joy_controller/package.xml @@ -4,18 +4,19 @@ 0.1.0 The autoware_joy_controller package Kenji Miyake - Apache 2 + Apache License 2.0 - catkin + ament_cmake_auto autoware_control_msgs autoware_vehicle_msgs + autoware_debug_msgs joy geometry_msgs - roscpp + rclcpp sensor_msgs - std_msgs + ament_cmake diff --git a/control/autoware_joy_controller/src/autoware_joy_controller/autoware_joy_controller_node.cpp b/control/autoware_joy_controller/src/autoware_joy_controller/autoware_joy_controller_node.cpp index ccc969f1ba164..97d20ca010c47 100644 --- a/control/autoware_joy_controller/src/autoware_joy_controller/autoware_joy_controller_node.cpp +++ b/control/autoware_joy_controller/src/autoware_joy_controller/autoware_joy_controller_node.cpp @@ -20,7 +20,7 @@ namespace { ShiftType getUpperShift(const ShiftType & shift) { - using autoware_vehicle_msgs::Shift; + using autoware_vehicle_msgs::msg::Shift; if (shift == Shift::NONE) return Shift::PARKING; if (shift == Shift::PARKING) return Shift::REVERSE; @@ -34,7 +34,7 @@ ShiftType getUpperShift(const ShiftType & shift) ShiftType getLowerShift(const ShiftType & shift) { - using autoware_vehicle_msgs::Shift; + using autoware_vehicle_msgs::msg::Shift; if (shift == Shift::NONE) return Shift::PARKING; if (shift == Shift::PARKING) return Shift::PARKING; @@ -48,7 +48,7 @@ ShiftType getLowerShift(const ShiftType & shift) const char * getShiftName(const ShiftType & shift) { - using autoware_vehicle_msgs::Shift; + using autoware_vehicle_msgs::msg::Shift; if (shift == Shift::NONE) return "NONE"; if (shift == Shift::PARKING) return "PARKING"; @@ -62,7 +62,7 @@ const char * getShiftName(const ShiftType & shift) const char * getTurnSignalName(const TurnSignalType & turn_signal) { - using autoware_vehicle_msgs::TurnSignal; + using autoware_vehicle_msgs::msg::TurnSignal; if (turn_signal == TurnSignal::NONE) return "NONE"; if (turn_signal == TurnSignal::LEFT) return "LEFT"; @@ -74,7 +74,7 @@ const char * getTurnSignalName(const TurnSignalType & turn_signal) const char * getGateModeName(const GateModeType & gate_mode) { - using autoware_control_msgs::GateMode; + using autoware_control_msgs::msg::GateMode; if (gate_mode == GateMode::AUTO) return "AUTO"; if (gate_mode == GateMode::REMOTE) return "REMOTE"; @@ -84,7 +84,7 @@ const char * getGateModeName(const GateModeType & gate_mode) } // namespace -void AutowareJoyControllerNode::onJoy(const sensor_msgs::Joy::ConstPtr & msg) +void AutowareJoyControllerNode::onJoy(const sensor_msgs::msg::Joy::ConstSharedPtr msg) { last_joy_received_time_ = msg->header.stamp; if (joy_type_ == "G29") { @@ -118,7 +118,7 @@ void AutowareJoyControllerNode::onJoy(const sensor_msgs::Joy::ConstPtr & msg) } } -void AutowareJoyControllerNode::onTwist(const geometry_msgs::TwistStamped::ConstPtr & msg) +void AutowareJoyControllerNode::onTwist(const geometry_msgs::msg::TwistStamped::ConstSharedPtr msg) { twist_ = msg; } @@ -128,14 +128,16 @@ bool AutowareJoyControllerNode::isDataReady() // Joy { if (!joy_) { - ROS_WARN_THROTTLE(1.0, "waiting for joy msg..."); + RCLCPP_WARN_THROTTLE(get_logger(), *get_clock(), std::chrono::milliseconds(1000).count(), + "waiting for joy msg..."); return false; } constexpr auto timeout = 2.0; - const auto time_diff = ros::Time::now() - last_joy_received_time_; - if (time_diff.toSec() > timeout) { - ROS_WARN_THROTTLE(1.0, "joy msg is timeout"); + const auto time_diff = this->now() - last_joy_received_time_; + if (time_diff.seconds() > timeout) { + RCLCPP_WARN_THROTTLE(get_logger(), *get_clock(), std::chrono::milliseconds(1000).count(), + "joy msg is timeout"); return false; } } @@ -143,14 +145,16 @@ bool AutowareJoyControllerNode::isDataReady() // Twist { if (!twist_) { - ROS_WARN_THROTTLE(1.0, "waiting for twist msg..."); + RCLCPP_WARN_THROTTLE(get_logger(), *get_clock(), std::chrono::milliseconds(1000).count(), + "waiting for twist msg..."); return false; } constexpr auto timeout = 0.5; - const auto time_diff = ros::Time::now() - twist_->header.stamp; - if (time_diff.toSec() > timeout) { - ROS_WARN_THROTTLE(1.0, "twist msg is timeout"); + const auto time_diff = this->now() - twist_->header.stamp; + if (time_diff.seconds() > timeout) { + RCLCPP_WARN_THROTTLE(get_logger(), *get_clock(), std::chrono::milliseconds(1000).count(), + "twist msg is timeout"); return false; } } @@ -158,7 +162,7 @@ bool AutowareJoyControllerNode::isDataReady() return true; } -void AutowareJoyControllerNode::onTimer(const ros::TimerEvent & event) +void AutowareJoyControllerNode::onTimer() { if (!isDataReady()) { return; @@ -174,8 +178,8 @@ void AutowareJoyControllerNode::onTimer(const ros::TimerEvent & event) void AutowareJoyControllerNode::publishControlCommand() { - autoware_control_msgs::ControlCommandStamped cmd_stamped; - cmd_stamped.header.stamp = ros::Time::now(); + autoware_control_msgs::msg::ControlCommandStamped cmd_stamped; + cmd_stamped.header.stamp = this->now(); { auto & cmd = cmd_stamped.control; @@ -202,14 +206,14 @@ void AutowareJoyControllerNode::publishControlCommand() } } - pub_control_command_.publish(cmd_stamped); + pub_control_command_->publish(cmd_stamped); prev_control_command_ = cmd_stamped.control; } void AutowareJoyControllerNode::publishRawControlCommand() { - autoware_vehicle_msgs::RawControlCommandStamped cmd_stamped; - cmd_stamped.header.stamp = ros::Time::now(); + autoware_vehicle_msgs::msg::RawControlCommandStamped cmd_stamped; + cmd_stamped.header.stamp = this->now(); { auto & cmd = cmd_stamped.control; @@ -220,16 +224,16 @@ void AutowareJoyControllerNode::publishRawControlCommand() cmd.brake = brake_ratio_ * joy_->brake(); } - pub_raw_control_command_.publish(cmd_stamped); + pub_raw_control_command_->publish(cmd_stamped); prev_raw_control_command_ = cmd_stamped.control; } void AutowareJoyControllerNode::publishShift() { - using autoware_vehicle_msgs::Shift; + using autoware_vehicle_msgs::msg::Shift; - autoware_vehicle_msgs::ShiftStamped shift_stamped; - shift_stamped.header.stamp = ros::Time::now(); + autoware_vehicle_msgs::msg::ShiftStamped shift_stamped; + shift_stamped.header.stamp = this->now(); { auto & shift = shift_stamped.shift; @@ -249,19 +253,19 @@ void AutowareJoyControllerNode::publishShift() shift.data = Shift::REVERSE; } - ROS_INFO("Shift::%s", getShiftName(shift.data)); + RCLCPP_INFO(get_logger(), "Shift::%s", getShiftName(shift.data)); } - pub_shift_.publish(shift_stamped); + pub_shift_->publish(shift_stamped); prev_shift_ = shift_stamped.shift.data; } void AutowareJoyControllerNode::publishTurnSignal() { - using autoware_vehicle_msgs::TurnSignal; + using autoware_vehicle_msgs::msg::TurnSignal; TurnSignal turn_signal; - turn_signal.header.stamp = ros::Time::now(); + turn_signal.header.stamp = this->now(); if (joy_->turn_signal_left() && joy_->turn_signal_right()) { turn_signal.data = TurnSignal::HAZARD; @@ -275,16 +279,16 @@ void AutowareJoyControllerNode::publishTurnSignal() turn_signal.data = TurnSignal::NONE; } - ROS_INFO("TurnSignal::%s", getTurnSignalName(turn_signal.data)); + RCLCPP_INFO(get_logger(), "TurnSignal::%s", getTurnSignalName(turn_signal.data)); - pub_turn_signal_.publish(turn_signal); + pub_turn_signal_->publish(turn_signal); } void AutowareJoyControllerNode::publishGateMode() { - using autoware_control_msgs::GateMode; + using autoware_control_msgs::msg::GateMode; - autoware_control_msgs::GateMode gate_mode; + autoware_control_msgs::msg::GateMode gate_mode; if (prev_gate_mode_ == GateMode::AUTO) { gate_mode.data = GateMode::REMOTE; @@ -294,124 +298,139 @@ void AutowareJoyControllerNode::publishGateMode() gate_mode.data = GateMode::AUTO; } - ROS_INFO("GateMode::%s", getGateModeName(gate_mode.data)); + RCLCPP_INFO(get_logger(), "GateMode::%s", getGateModeName(gate_mode.data)); - pub_gate_mode_.publish(gate_mode); + pub_gate_mode_->publish(gate_mode); prev_gate_mode_ = gate_mode.data; } void AutowareJoyControllerNode::publishEmergency() { - std_msgs::Bool emergency; + autoware_debug_msgs::msg::BoolStamped emergency; if (joy_->emergency()) { emergency.data = true; - ROS_INFO("Emergency"); + RCLCPP_INFO(get_logger(), "Emergency"); } if (joy_->clear_emergency()) { emergency.data = false; - ROS_INFO("Clear Emergency"); + RCLCPP_INFO(get_logger(), "Clear Emergency"); } - pub_emergency_.publish(emergency); + pub_emergency_->publish(emergency); } void AutowareJoyControllerNode::publishAutowareEngage() { - std_msgs::Bool engage; + autoware_debug_msgs::msg::BoolStamped engage; if (joy_->autoware_engage()) { engage.data = true; - ROS_INFO("Autoware Engage"); + RCLCPP_INFO(get_logger(), "Autoware Engage"); } if (joy_->autoware_disengage()) { engage.data = false; - ROS_INFO("Autoware Disengage"); + RCLCPP_INFO(get_logger(), "Autoware Disengage"); } - pub_autoware_engage_.publish(engage); + pub_autoware_engage_->publish(engage); } void AutowareJoyControllerNode::publishVehicleEngage() { - std_msgs::Bool engage; + autoware_debug_msgs::msg::BoolStamped engage; if (joy_->vehicle_engage()) { engage.data = true; - ROS_INFO("Vehicle Engage"); + RCLCPP_INFO(get_logger(), "Vehicle Engage"); } if (joy_->vehicle_disengage()) { engage.data = false; - ROS_INFO("Vehicle Disengage"); + RCLCPP_INFO(get_logger(), "Vehicle Disengage"); } - pub_vehicle_engage_.publish(engage); + pub_vehicle_engage_->publish(engage); } // tmp void AutowareJoyControllerNode::publishVehicleCommand() { - autoware_vehicle_msgs::VehicleCommand vehicle_cmd; - vehicle_cmd.header.stamp = ros::Time::now(); + autoware_vehicle_msgs::msg::VehicleCommand vehicle_cmd; + vehicle_cmd.header.stamp = this->now(); vehicle_cmd.control = prev_control_command_; vehicle_cmd.shift.data = prev_shift_; - pub_vehicle_command_.publish(vehicle_cmd); + pub_vehicle_command_->publish(vehicle_cmd); } void AutowareJoyControllerNode::publishRawVehicleCommand() { - autoware_vehicle_msgs::RawVehicleCommand vehicle_cmd; - vehicle_cmd.header.stamp = ros::Time::now(); + autoware_vehicle_msgs::msg::RawVehicleCommand vehicle_cmd; + vehicle_cmd.header.stamp = this->now(); vehicle_cmd.control = prev_raw_control_command_; vehicle_cmd.shift.data = prev_shift_; - pub_raw_vehicle_command_.publish(vehicle_cmd); + pub_raw_vehicle_command_->publish(vehicle_cmd); +} + +void AutowareJoyControllerNode::initTimer(double period_s) +{ + auto timer_callback = std::bind(&AutowareJoyControllerNode::onTimer, this); + const auto period_ns = + std::chrono::duration_cast(std::chrono::duration(period_s)); + timer_ = std::make_shared>( + this->get_clock(), period_ns, std::move(timer_callback), + this->get_node_base_interface()->get_context()); + this->get_node_timers_interface()->add_timer(timer_, nullptr); } AutowareJoyControllerNode::AutowareJoyControllerNode() +: Node("autoware_joy_controller") { // Parameter - private_nh_.param("joy_type", joy_type_, "DS4"); - private_nh_.param("update_rate", update_rate_, 10.0); - private_nh_.param("accel_ratio", accel_ratio_, 3.0); - private_nh_.param("brake_ratio", brake_ratio_, 5.0); - private_nh_.param("steer_ratio", steer_ratio_, 0.5); - private_nh_.param("steering_angle_velocity", steering_angle_velocity_, 0.1); - private_nh_.param("control_command/velocity_gain", velocity_gain_, 3.0); - private_nh_.param("control_command/max_forward_velocity", max_forward_velocity_, 20.0); - private_nh_.param("control_command/max_backward_velocity", max_backward_velocity_, 3.0); - private_nh_.param("control_command/backward_accel_ratio", backward_accel_ratio_, 1.0); - - ROS_INFO("Joy type: %s", joy_type_.c_str()); + joy_type_ = declare_parameter("joy_type", std::string("DS4")); + update_rate_ = declare_parameter("update_rate", 10.0); + accel_ratio_ = declare_parameter("accel_ratio", 3.0); + brake_ratio_ = declare_parameter("brake_ratio", 5.0); + steer_ratio_ = declare_parameter("steer_ratio", 0.5); + steering_angle_velocity_ = declare_parameter("steering_angle_velocity", 0.1); + velocity_gain_ = declare_parameter("control_command/velocity_gain", 3.0); + max_forward_velocity_ = declare_parameter("control_command/max_forward_velocity", 20.0); + max_backward_velocity_ = declare_parameter("control_command/max_backward_velocity", 3.0); + backward_accel_ratio_ = declare_parameter("control_command/backward_accel_ratio", 1.0); + + RCLCPP_INFO(get_logger(), "Joy type: %s", joy_type_.c_str()); // Subscriber - sub_joy_ = private_nh_.subscribe("input/joy", 1, &AutowareJoyControllerNode::onJoy, this); - sub_twist_ = private_nh_.subscribe("input/twist", 1, &AutowareJoyControllerNode::onTwist, this); + sub_joy_ = this->create_subscription( + "input/joy", 1, + std::bind(&AutowareJoyControllerNode::onJoy, this, std::placeholders::_1)); + sub_twist_ = this->create_subscription( + "input/twist", 1, + std::bind(&AutowareJoyControllerNode::onTwist, this, std::placeholders::_1)); // Publisher - pub_control_command_ = private_nh_.advertise( + pub_control_command_ = this->create_publisher( "output/control_command", 1); - pub_raw_control_command_ = private_nh_.advertise( + pub_raw_control_command_ = this->create_publisher( "output/raw_control_command", 1); - pub_shift_ = private_nh_.advertise("output/shift", 1); + pub_shift_ = this->create_publisher("output/shift", 1); pub_turn_signal_ = - private_nh_.advertise("output/turn_signal", 1); - pub_gate_mode_ = private_nh_.advertise("output/gate_mode", 1); - pub_emergency_ = private_nh_.advertise("output/emergency", 1); - pub_autoware_engage_ = private_nh_.advertise("output/autoware_engage", 1); - pub_vehicle_engage_ = private_nh_.advertise("output/vehicle_engage", 1); + this->create_publisher("output/turn_signal", 1); + pub_gate_mode_ = this->create_publisher("output/gate_mode", 1); + pub_emergency_ = this->create_publisher("output/emergency", 1); + pub_autoware_engage_ = this->create_publisher("output/autoware_engage", 1); + pub_vehicle_engage_ = this->create_publisher("output/vehicle_engage", 1); // tmp pub_vehicle_command_ = - private_nh_.advertise("output/vehicle_cmd", 1); + this->create_publisher("output/vehicle_cmd", 1); pub_raw_vehicle_command_ = - private_nh_.advertise("output/raw_vehicle_cmd", 1); + this->create_publisher("output/raw_vehicle_cmd", 1); // Timer - timer_ = - private_nh_.createTimer(ros::Rate(update_rate_), &AutowareJoyControllerNode::onTimer, this); + initTimer(1.0/update_rate_); } diff --git a/control/autoware_joy_controller/src/autoware_joy_controller/main.cpp b/control/autoware_joy_controller/src/autoware_joy_controller/main.cpp index c41a8aaf6a3cb..0f61e1f59adfd 100644 --- a/control/autoware_joy_controller/src/autoware_joy_controller/main.cpp +++ b/control/autoware_joy_controller/src/autoware_joy_controller/main.cpp @@ -12,17 +12,15 @@ // See the License for the specific language governing permissions and // limitations under the License. -#include "ros/ros.h" +#include "rclcpp/rclcpp.hpp" #include "autoware_joy_controller/autoware_joy_controller.hpp" int main(int argc, char * argv[]) { - ros::init(argc, argv, "autoware_joy_controller"); - - AutowareJoyControllerNode node; - - ros::spin(); + rclcpp::init(argc, argv); + rclcpp::spin(std::make_shared()); + rclcpp::shutdown(); return 0; } From bf98f2f067dcbed56da388172746ad27644b1d60 Mon Sep 17 00:00:00 2001 From: Nikolai Morin Date: Tue, 8 Dec 2020 15:10:45 +0100 Subject: [PATCH 15/38] Run uncrustify on the entire Pilot.Auto codebase (#151) * Run uncrustify on the entire Pilot.Auto codebase * Exclude open PRs --- .../autoware_joy_controller.hpp | 9 +- .../joy_converter/ds4_joy_converter.hpp | 75 ++++++++-------- .../joy_converter/g29_joy_converter.hpp | 77 ++++++++-------- .../autoware_joy_controller_node.cpp | 87 +++++++++++-------- 4 files changed, 133 insertions(+), 115 deletions(-) diff --git a/control/autoware_joy_controller/include/autoware_joy_controller/autoware_joy_controller.hpp b/control/autoware_joy_controller/include/autoware_joy_controller/autoware_joy_controller.hpp index 6da5498e3b97e..6d1c8b6741281 100644 --- a/control/autoware_joy_controller/include/autoware_joy_controller/autoware_joy_controller.hpp +++ b/control/autoware_joy_controller/include/autoware_joy_controller/autoware_joy_controller.hpp @@ -68,8 +68,10 @@ class AutowareJoyControllerNode : public rclcpp::Node void onTwist(const geometry_msgs::msg::TwistStamped::ConstSharedPtr msg); // Publisher - rclcpp::Publisher::SharedPtr pub_control_command_; - rclcpp::Publisher::SharedPtr pub_raw_control_command_; + rclcpp::Publisher::SharedPtr + pub_control_command_; + rclcpp::Publisher::SharedPtr + pub_raw_control_command_; rclcpp::Publisher::SharedPtr pub_shift_; rclcpp::Publisher::SharedPtr pub_turn_signal_; rclcpp::Publisher::SharedPtr pub_gate_mode_; @@ -95,7 +97,8 @@ class AutowareJoyControllerNode : public rclcpp::Node // tmp rclcpp::Publisher::SharedPtr pub_vehicle_command_; - rclcpp::Publisher::SharedPtr pub_raw_vehicle_command_; + rclcpp::Publisher::SharedPtr + pub_raw_vehicle_command_; void publishVehicleCommand(); void publishRawVehicleCommand(); diff --git a/control/autoware_joy_controller/include/autoware_joy_controller/joy_converter/ds4_joy_converter.hpp b/control/autoware_joy_controller/include/autoware_joy_controller/joy_converter/ds4_joy_converter.hpp index e3971db73d77b..6d38d9dd61d03 100644 --- a/control/autoware_joy_controller/include/autoware_joy_controller/joy_converter/ds4_joy_converter.hpp +++ b/control/autoware_joy_controller/include/autoware_joy_controller/joy_converter/ds4_joy_converter.hpp @@ -19,7 +19,8 @@ class DS4JoyConverter : public JoyConverterBase { public: - explicit DS4JoyConverter(const sensor_msgs::msg::Joy & j) : j_(j) {} + explicit DS4JoyConverter(const sensor_msgs::msg::Joy & j) + : j_(j) {} float accel() const { @@ -37,51 +38,51 @@ class DS4JoyConverter : public JoyConverterBase return std::max({button, stick, trigger}); } - float steer() const { return LStickLeftRight(); } + 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 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 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 gate_mode() const {return Options();} - bool emergency() const { return !reverse() && PS(); } - bool clear_emergency() const { return reverse() && PS(); } + bool emergency() const {return !reverse() && PS();} + bool clear_emergency() const {return reverse() && PS();} - bool autoware_engage() const { return !reverse() && Circle(); } - bool autoware_disengage() const { return reverse() && Circle(); } + 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(); } + 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); } + 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(); } + bool reverse() const {return Share();} }; diff --git a/control/autoware_joy_controller/include/autoware_joy_controller/joy_converter/g29_joy_converter.hpp b/control/autoware_joy_controller/include/autoware_joy_controller/joy_converter/g29_joy_converter.hpp index c9ab723a47f4c..2dca0d6c6e5ae 100644 --- a/control/autoware_joy_controller/include/autoware_joy_controller/joy_converter/g29_joy_converter.hpp +++ b/control/autoware_joy_controller/include/autoware_joy_controller/joy_converter/g29_joy_converter.hpp @@ -19,65 +19,66 @@ class G29JoyConverter : public JoyConverterBase { public: - explicit G29JoyConverter(const sensor_msgs::msg::Joy & j) : j_(j) {} + explicit G29JoyConverter(const sensor_msgs::msg::Joy & j) + : j_(j) {} float accel() const { constexpr float eps = 0.0000001; - if(std::fabs(AccelPedal()) < eps) return 0.0f; + if (std::fabs(AccelPedal()) < eps) {return 0.0f;} return (AccelPedal() + 1.0f) / 2; } float brake() const - { + { constexpr float eps = 0.0000001; - if(std::fabs(BrakePedal()) < eps) return 0.0f; + if (std::fabs(BrakePedal()) < eps) {return 0.0f;} return (BrakePedal() + 1.0f) / 2; } - float steer() const { return Steer(); } + float steer() const {return Steer();} - 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 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 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 gate_mode() const {return Options();} - bool emergency() const { return !reverse() && PS(); } - bool clear_emergency() const { return reverse() && PS(); } + bool emergency() const {return !reverse() && PS();} + bool clear_emergency() const {return reverse() && PS();} - bool autoware_engage() const { return !reverse() && Circle(); } - bool autoware_disengage() const { return reverse() && Circle(); } + 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(); } + bool vehicle_engage() const {return !reverse() && Triangle();} + bool vehicle_disengage() const {return reverse() && Triangle();} private: - float Steer() const { return j_.axes.at(0); } - float AccelPedal() const { return j_.axes.at(2); } - float BrakePedal() const { return j_.axes.at(3); } - - float CursorLeftRight() const { return j_.axes.at(4); } - float CursorUpDown() const { return j_.axes.at(5); } - - 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(5); } - bool R1() const { return j_.buttons.at(4); } - bool L2() const { return j_.buttons.at(7); } - bool R2() const { return j_.buttons.at(6); } - bool Share() const { return j_.buttons.at(8); } - bool Options() const { return j_.buttons.at(9); } - bool PS() const { return j_.buttons.at(24); } + float Steer() const {return j_.axes.at(0);} + float AccelPedal() const {return j_.axes.at(2);} + float BrakePedal() const {return j_.axes.at(3);} + + float CursorLeftRight() const {return j_.axes.at(4);} + float CursorUpDown() const {return j_.axes.at(5);} + + 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(5);} + bool R1() const {return j_.buttons.at(4);} + bool L2() const {return j_.buttons.at(7);} + bool R2() const {return j_.buttons.at(6);} + bool Share() const {return j_.buttons.at(8);} + bool Options() const {return j_.buttons.at(9);} + bool PS() const {return j_.buttons.at(24);} const sensor_msgs::msg::Joy j_; - bool reverse() const { return Share(); } + bool reverse() const {return Share();} }; diff --git a/control/autoware_joy_controller/src/autoware_joy_controller/autoware_joy_controller_node.cpp b/control/autoware_joy_controller/src/autoware_joy_controller/autoware_joy_controller_node.cpp index 97d20ca010c47..9b4e0124fba7c 100644 --- a/control/autoware_joy_controller/src/autoware_joy_controller/autoware_joy_controller_node.cpp +++ b/control/autoware_joy_controller/src/autoware_joy_controller/autoware_joy_controller_node.cpp @@ -22,12 +22,12 @@ ShiftType getUpperShift(const ShiftType & shift) { using autoware_vehicle_msgs::msg::Shift; - if (shift == Shift::NONE) return Shift::PARKING; - if (shift == Shift::PARKING) return Shift::REVERSE; - if (shift == Shift::REVERSE) return Shift::NEUTRAL; - if (shift == Shift::NEUTRAL) return Shift::DRIVE; - if (shift == Shift::DRIVE) return Shift::LOW; - if (shift == Shift::LOW) return Shift::LOW; + if (shift == Shift::NONE) {return Shift::PARKING;} + if (shift == Shift::PARKING) {return Shift::REVERSE;} + if (shift == Shift::REVERSE) {return Shift::NEUTRAL;} + if (shift == Shift::NEUTRAL) {return Shift::DRIVE;} + if (shift == Shift::DRIVE) {return Shift::LOW;} + if (shift == Shift::LOW) {return Shift::LOW;} return Shift::NONE; } @@ -36,12 +36,12 @@ ShiftType getLowerShift(const ShiftType & shift) { using autoware_vehicle_msgs::msg::Shift; - if (shift == Shift::NONE) return Shift::PARKING; - if (shift == Shift::PARKING) return Shift::PARKING; - if (shift == Shift::REVERSE) return Shift::PARKING; - if (shift == Shift::NEUTRAL) return Shift::REVERSE; - if (shift == Shift::DRIVE) return Shift::NEUTRAL; - if (shift == Shift::LOW) return Shift::DRIVE; + if (shift == Shift::NONE) {return Shift::PARKING;} + if (shift == Shift::PARKING) {return Shift::PARKING;} + if (shift == Shift::REVERSE) {return Shift::PARKING;} + if (shift == Shift::NEUTRAL) {return Shift::REVERSE;} + if (shift == Shift::DRIVE) {return Shift::NEUTRAL;} + if (shift == Shift::LOW) {return Shift::DRIVE;} return Shift::NONE; } @@ -50,12 +50,12 @@ const char * getShiftName(const ShiftType & shift) { using autoware_vehicle_msgs::msg::Shift; - if (shift == Shift::NONE) return "NONE"; - if (shift == Shift::PARKING) return "PARKING"; - if (shift == Shift::REVERSE) return "REVERSE"; - if (shift == Shift::NEUTRAL) return "NEUTRAL"; - if (shift == Shift::DRIVE) return "DRIVE"; - if (shift == Shift::LOW) return "LOW"; + if (shift == Shift::NONE) {return "NONE";} + if (shift == Shift::PARKING) {return "PARKING";} + if (shift == Shift::REVERSE) {return "REVERSE";} + if (shift == Shift::NEUTRAL) {return "NEUTRAL";} + if (shift == Shift::DRIVE) {return "DRIVE";} + if (shift == Shift::LOW) {return "LOW";} return "NOT_SUPPORTED"; } @@ -64,10 +64,10 @@ const char * getTurnSignalName(const TurnSignalType & turn_signal) { using autoware_vehicle_msgs::msg::TurnSignal; - if (turn_signal == TurnSignal::NONE) return "NONE"; - if (turn_signal == TurnSignal::LEFT) return "LEFT"; - if (turn_signal == TurnSignal::RIGHT) return "RIGHT"; - if (turn_signal == TurnSignal::HAZARD) return "HAZARD"; + if (turn_signal == TurnSignal::NONE) {return "NONE";} + if (turn_signal == TurnSignal::LEFT) {return "LEFT";} + if (turn_signal == TurnSignal::RIGHT) {return "RIGHT";} + if (turn_signal == TurnSignal::HAZARD) {return "HAZARD";} return "NOT_SUPPORTED"; } @@ -76,8 +76,8 @@ const char * getGateModeName(const GateModeType & gate_mode) { using autoware_control_msgs::msg::GateMode; - if (gate_mode == GateMode::AUTO) return "AUTO"; - if (gate_mode == GateMode::REMOTE) return "REMOTE"; + if (gate_mode == GateMode::AUTO) {return "AUTO";} + if (gate_mode == GateMode::REMOTE) {return "REMOTE";} return "NOT_SUPPORTED"; } @@ -128,7 +128,8 @@ bool AutowareJoyControllerNode::isDataReady() // Joy { if (!joy_) { - RCLCPP_WARN_THROTTLE(get_logger(), *get_clock(), std::chrono::milliseconds(1000).count(), + RCLCPP_WARN_THROTTLE( + get_logger(), *get_clock(), std::chrono::milliseconds(1000).count(), "waiting for joy msg..."); return false; } @@ -136,7 +137,8 @@ bool AutowareJoyControllerNode::isDataReady() constexpr auto timeout = 2.0; const auto time_diff = this->now() - last_joy_received_time_; if (time_diff.seconds() > timeout) { - RCLCPP_WARN_THROTTLE(get_logger(), *get_clock(), std::chrono::milliseconds(1000).count(), + RCLCPP_WARN_THROTTLE( + get_logger(), *get_clock(), std::chrono::milliseconds(1000).count(), "joy msg is timeout"); return false; } @@ -145,7 +147,8 @@ bool AutowareJoyControllerNode::isDataReady() // Twist { if (!twist_) { - RCLCPP_WARN_THROTTLE(get_logger(), *get_clock(), std::chrono::milliseconds(1000).count(), + RCLCPP_WARN_THROTTLE( + get_logger(), *get_clock(), std::chrono::milliseconds(1000).count(), "waiting for twist msg..."); return false; } @@ -153,7 +156,8 @@ bool AutowareJoyControllerNode::isDataReady() constexpr auto timeout = 0.5; const auto time_diff = this->now() - twist_->header.stamp; if (time_diff.seconds() > timeout) { - RCLCPP_WARN_THROTTLE(get_logger(), *get_clock(), std::chrono::milliseconds(1000).count(), + RCLCPP_WARN_THROTTLE( + get_logger(), *get_clock(), std::chrono::milliseconds(1000).count(), "twist msg is timeout"); return false; } @@ -406,31 +410,40 @@ AutowareJoyControllerNode::AutowareJoyControllerNode() // Subscriber sub_joy_ = this->create_subscription( - "input/joy", 1, + "input/joy", 1, std::bind(&AutowareJoyControllerNode::onJoy, this, std::placeholders::_1)); sub_twist_ = this->create_subscription( - "input/twist", 1, + "input/twist", 1, std::bind(&AutowareJoyControllerNode::onTwist, this, std::placeholders::_1)); // Publisher pub_control_command_ = this->create_publisher( "output/control_command", 1); - pub_raw_control_command_ = this->create_publisher( + pub_raw_control_command_ = + this->create_publisher( "output/raw_control_command", 1); pub_shift_ = this->create_publisher("output/shift", 1); pub_turn_signal_ = this->create_publisher("output/turn_signal", 1); - pub_gate_mode_ = this->create_publisher("output/gate_mode", 1); - pub_emergency_ = this->create_publisher("output/emergency", 1); - pub_autoware_engage_ = this->create_publisher("output/autoware_engage", 1); - pub_vehicle_engage_ = this->create_publisher("output/vehicle_engage", 1); + pub_gate_mode_ = this->create_publisher( + "output/gate_mode", + 1); + pub_emergency_ = this->create_publisher( + "output/emergency", + 1); + pub_autoware_engage_ = this->create_publisher( + "output/autoware_engage", 1); + pub_vehicle_engage_ = this->create_publisher( + "output/vehicle_engage", 1); // tmp pub_vehicle_command_ = this->create_publisher("output/vehicle_cmd", 1); pub_raw_vehicle_command_ = - this->create_publisher("output/raw_vehicle_cmd", 1); + this->create_publisher( + "output/raw_vehicle_cmd", + 1); // Timer - initTimer(1.0/update_rate_); + initTimer(1.0 / update_rate_); } From 21266f19f222de19d7e23e6f8172146d85c004d9 Mon Sep 17 00:00:00 2001 From: Takamasa Horibe Date: Mon, 8 Feb 2021 17:22:35 +0900 Subject: [PATCH 16/38] [update to v0.8.0] autoware joy controller (#251) * restore filename to original for version update Signed-off-by: Takamasa Horibe * Enable to change sensitivity (#868) * Improve remote emergency stop (#900) * Apply format Signed-off-by: Kenji Miyake * Rename emergency to system_emergency in vehicle_cmd_gate Signed-off-by: Kenji Miyake * Add emergency stop feature to vehicle_cmd_gate Signed-off-by: Kenji Miyake * Fix frame_id of vehicle_cmd_gate output Signed-off-by: Kenji Miyake * Rename /remote/emergency to /remote/emergency_stop in autoware_joy_controller Signed-off-by: Kenji Miyake * Rename /remote/emergency to /remote/emergency_stop in remote_cmd_converter Signed-off-by: Kenji Miyake * Rename /remote/emergency to /remote/emergency_stop in autoware_api Signed-off-by: Kenji Miyake * Check emergency_stop timeout in remote_cmd_converter Signed-off-by: Kenji Miyake * Ignore timeout = 0.0 Signed-off-by: Kenji Miyake * Add config_file to arg Signed-off-by: Kenji Miyake * Rename emergency_stop to external_emergency_stop Signed-off-by: Kenji Miyake * Remove unnecessary lines Signed-off-by: Kenji Miyake * Wait for first heartbeat Signed-off-by: Kenji Miyake * Add clear_emergency_stop service Signed-off-by: Kenji Miyake * Call clear_external_emegency_stop service from autoware_joy_controller Signed-off-by: Kenji Miyake * Rename function Signed-off-by: Kenji Miyake * Revert: Wait for first heartbeat Signed-off-by: Kenji Miyake * Fix console messages Signed-off-by: Kenji Miyake * Move emergency_stop diag to vehicle_cmd_gate Signed-off-by: Kenji Miyake * Add heartbeat to vehicle_cmd_gate Signed-off-by: Kenji Miyake * Revert: Move emergency_stop diag to vehicle_cmd_gate Signed-off-by: Kenji Miyake * patch in real-vehicle * Apply format Signed-off-by: Kenji Miyake * Change default parameter Signed-off-by: Kenji Miyake Co-authored-by: jpntaxi4943-autoware * restore file name Signed-off-by: Takamasa Horibe * [tmp] fix build error Signed-off-by: Takamasa Horibe * fix service Signed-off-by: Takamasa Horibe * fix format Signed-off-by: Takamasa Horibe * fix service usage Signed-off-by: Takamasa Horibe * fix launch var Co-authored-by: Fumiya Watanabe Co-authored-by: Kenji Miyake <31987104+kenji-miyake@users.noreply.github.com> Co-authored-by: jpntaxi4943-autoware --- .../config/autoware_joy_controller.yaml | 2 + .../autoware_joy_controller.hpp | 13 ++++- .../joy_converter/ds4_joy_converter.hpp | 4 +- .../joy_converter/g29_joy_converter.hpp | 4 +- .../joy_converter/joy_converter_base.hpp | 4 +- .../launch/autoware_joy_controller.launch.xml | 6 +- control/autoware_joy_controller/package.xml | 1 + .../autoware_joy_controller_node.cpp | 57 ++++++++++++++----- 8 files changed, 68 insertions(+), 23 deletions(-) diff --git a/control/autoware_joy_controller/config/autoware_joy_controller.yaml b/control/autoware_joy_controller/config/autoware_joy_controller.yaml index 8e2b0564f6a17..8796e026b0e69 100644 --- a/control/autoware_joy_controller/config/autoware_joy_controller.yaml +++ b/control/autoware_joy_controller/config/autoware_joy_controller.yaml @@ -5,6 +5,8 @@ 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 diff --git a/control/autoware_joy_controller/include/autoware_joy_controller/autoware_joy_controller.hpp b/control/autoware_joy_controller/include/autoware_joy_controller/autoware_joy_controller.hpp index 6d1c8b6741281..97ac08d1d6695 100644 --- a/control/autoware_joy_controller/include/autoware_joy_controller/autoware_joy_controller.hpp +++ b/control/autoware_joy_controller/include/autoware_joy_controller/autoware_joy_controller.hpp @@ -32,6 +32,9 @@ #include "autoware_vehicle_msgs/msg/raw_vehicle_command.hpp" #include "autoware_vehicle_msgs/msg/vehicle_command.hpp" +#include "std_srvs/srv/trigger.hpp" + + using ShiftType = autoware_vehicle_msgs::msg::Shift::_data_type; using TurnSignalType = autoware_vehicle_msgs::msg::TurnSignal::_data_type; using GateModeType = autoware_control_msgs::msg::GateMode::_data_type; @@ -49,6 +52,8 @@ class AutowareJoyControllerNode : public rclcpp::Node double brake_ratio_; double steer_ratio_; double steering_angle_velocity_; + double accel_sensitivity_; + double brake_sensitivity_; // ControlCommand Parameter double velocity_gain_; @@ -75,7 +80,7 @@ class AutowareJoyControllerNode : public rclcpp::Node rclcpp::Publisher::SharedPtr pub_shift_; rclcpp::Publisher::SharedPtr pub_turn_signal_; rclcpp::Publisher::SharedPtr pub_gate_mode_; - rclcpp::Publisher::SharedPtr pub_emergency_; + rclcpp::Publisher::SharedPtr pub_emergency_stop_; rclcpp::Publisher::SharedPtr pub_autoware_engage_; rclcpp::Publisher::SharedPtr pub_vehicle_engage_; @@ -84,9 +89,13 @@ class AutowareJoyControllerNode : public rclcpp::Node void publishShift(); void publishTurnSignal(); void publishGateMode(); - void publishEmergency(); + void publishEmergencyStop(); void publishAutowareEngage(); void publishVehicleEngage(); + void clearEmergencyResponse(rclcpp::Client::SharedFuture result); + + // Service Client + rclcpp::Client::SharedPtr client_clear_emergency_stop_; // Previous State autoware_control_msgs::msg::ControlCommand prev_control_command_; diff --git a/control/autoware_joy_controller/include/autoware_joy_controller/joy_converter/ds4_joy_converter.hpp b/control/autoware_joy_controller/include/autoware_joy_controller/joy_converter/ds4_joy_converter.hpp index 6d38d9dd61d03..2fe15e2531087 100644 --- a/control/autoware_joy_controller/include/autoware_joy_controller/joy_converter/ds4_joy_converter.hpp +++ b/control/autoware_joy_controller/include/autoware_joy_controller/joy_converter/ds4_joy_converter.hpp @@ -51,8 +51,8 @@ class DS4JoyConverter : public JoyConverterBase bool gate_mode() const {return Options();} - bool emergency() const {return !reverse() && PS();} - bool clear_emergency() const {return reverse() && PS();} + 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();} diff --git a/control/autoware_joy_controller/include/autoware_joy_controller/joy_converter/g29_joy_converter.hpp b/control/autoware_joy_controller/include/autoware_joy_controller/joy_converter/g29_joy_converter.hpp index 2dca0d6c6e5ae..82e5ddce9cb1e 100644 --- a/control/autoware_joy_controller/include/autoware_joy_controller/joy_converter/g29_joy_converter.hpp +++ b/control/autoware_joy_controller/include/autoware_joy_controller/joy_converter/g29_joy_converter.hpp @@ -49,8 +49,8 @@ class G29JoyConverter : public JoyConverterBase bool gate_mode() const {return Options();} - bool emergency() const {return !reverse() && PS();} - bool clear_emergency() const {return reverse() && PS();} + 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();} diff --git a/control/autoware_joy_controller/include/autoware_joy_controller/joy_converter/joy_converter_base.hpp b/control/autoware_joy_controller/include/autoware_joy_controller/joy_converter/joy_converter_base.hpp index a2774527f3fc8..fb9e0c37f7c9e 100644 --- a/control/autoware_joy_controller/include/autoware_joy_controller/joy_converter/joy_converter_base.hpp +++ b/control/autoware_joy_controller/include/autoware_joy_controller/joy_converter/joy_converter_base.hpp @@ -38,8 +38,8 @@ class JoyConverterBase virtual bool gate_mode() const = 0; - virtual bool emergency() const = 0; - virtual bool clear_emergency() const = 0; + virtual bool emergency_stop() const = 0; + virtual bool clear_emergency_stop() const = 0; virtual bool autoware_engage() const = 0; virtual bool autoware_disengage() const = 0; diff --git a/control/autoware_joy_controller/launch/autoware_joy_controller.launch.xml b/control/autoware_joy_controller/launch/autoware_joy_controller.launch.xml index 06a730249c1e0..6a6b3098e454c 100644 --- a/control/autoware_joy_controller/launch/autoware_joy_controller.launch.xml +++ b/control/autoware_joy_controller/launch/autoware_joy_controller.launch.xml @@ -9,7 +9,7 @@ - + @@ -24,7 +24,7 @@ - + @@ -33,6 +33,8 @@ + + diff --git a/control/autoware_joy_controller/package.xml b/control/autoware_joy_controller/package.xml index a6c7cd0a9331c..d938227e8c9da 100644 --- a/control/autoware_joy_controller/package.xml +++ b/control/autoware_joy_controller/package.xml @@ -15,6 +15,7 @@ geometry_msgs rclcpp sensor_msgs + std_srvs ament_cmake diff --git a/control/autoware_joy_controller/src/autoware_joy_controller/autoware_joy_controller_node.cpp b/control/autoware_joy_controller/src/autoware_joy_controller/autoware_joy_controller_node.cpp index 9b4e0124fba7c..e0941883e639a 100644 --- a/control/autoware_joy_controller/src/autoware_joy_controller/autoware_joy_controller_node.cpp +++ b/control/autoware_joy_controller/src/autoware_joy_controller/autoware_joy_controller_node.cpp @@ -82,6 +82,12 @@ const char * getGateModeName(const GateModeType & gate_mode) return "NOT_SUPPORTED"; } +double calcMapping(const double input, const double sensitivity) +{ + const double exponent = 1.0 / (std::max(0.001, std::min(1.0, sensitivity))); + return std::pow(input, exponent); +} + } // namespace void AutowareJoyControllerNode::onJoy(const sensor_msgs::msg::Joy::ConstSharedPtr msg) @@ -105,10 +111,6 @@ void AutowareJoyControllerNode::onJoy(const sensor_msgs::msg::Joy::ConstSharedPt publishGateMode(); } - if (joy_->emergency() || joy_->clear_emergency()) { - publishEmergency(); - } - if (joy_->autoware_engage() || joy_->autoware_disengage()) { publishAutowareEngage(); } @@ -174,6 +176,7 @@ void AutowareJoyControllerNode::onTimer() publishControlCommand(); publishRawControlCommand(); + publishEmergencyStop(); // tmp publishVehicleCommand(); @@ -224,8 +227,9 @@ void AutowareJoyControllerNode::publishRawControlCommand() cmd.steering_angle = steer_ratio_ * joy_->steer(); cmd.steering_angle_velocity = steering_angle_velocity_; - cmd.throttle = accel_ratio_ * joy_->accel(); - cmd.brake = brake_ratio_ * joy_->brake(); + cmd.throttle = + accel_ratio_ * calcMapping(static_cast(joy_->accel()), accel_sensitivity_); + cmd.brake = brake_ratio_ * calcMapping(static_cast(joy_->brake()), brake_sensitivity_); } pub_raw_control_command_->publish(cmd_stamped); @@ -308,21 +312,35 @@ void AutowareJoyControllerNode::publishGateMode() prev_gate_mode_ = gate_mode.data; } -void AutowareJoyControllerNode::publishEmergency() +void AutowareJoyControllerNode::publishEmergencyStop() { autoware_debug_msgs::msg::BoolStamped emergency; - if (joy_->emergency()) { + if (joy_->emergency_stop()) { emergency.data = true; RCLCPP_INFO(get_logger(), "Emergency"); } - if (joy_->clear_emergency()) { + if (joy_->clear_emergency_stop()) { emergency.data = false; - RCLCPP_INFO(get_logger(), "Clear Emergency"); + + auto request = std::make_shared(); + auto result = client_clear_emergency_stop_->async_send_request( + request, + std::bind(&AutowareJoyControllerNode::clearEmergencyResponse, this, std::placeholders::_1)); } - pub_emergency_->publish(emergency); + pub_emergency_stop_->publish(emergency); +} + +void AutowareJoyControllerNode::clearEmergencyResponse( + rclcpp::Client::SharedFuture result) +{ + if (result.get()->success) { + RCLCPP_INFO(get_logger(), "Clear Emergency Stop"); + } else { + RCLCPP_WARN(get_logger(), "failed to clear emergency stop: %s", result.get()->message.c_str()); + } } void AutowareJoyControllerNode::publishAutowareEngage() @@ -401,6 +419,8 @@ AutowareJoyControllerNode::AutowareJoyControllerNode() brake_ratio_ = declare_parameter("brake_ratio", 5.0); steer_ratio_ = declare_parameter("steer_ratio", 0.5); steering_angle_velocity_ = declare_parameter("steering_angle_velocity", 0.1); + accel_sensitivity_ = declare_parameter("accel_sensitivity", 1.0); + brake_sensitivity_ = declare_parameter("brake_sensitivity", 1.0); velocity_gain_ = declare_parameter("control_command/velocity_gain", 3.0); max_forward_velocity_ = declare_parameter("control_command/max_forward_velocity", 20.0); max_backward_velocity_ = declare_parameter("control_command/max_backward_velocity", 3.0); @@ -428,8 +448,8 @@ AutowareJoyControllerNode::AutowareJoyControllerNode() pub_gate_mode_ = this->create_publisher( "output/gate_mode", 1); - pub_emergency_ = this->create_publisher( - "output/emergency", + pub_emergency_stop_ = this->create_publisher( + "output/emergency_stop", 1); pub_autoware_engage_ = this->create_publisher( "output/autoware_engage", 1); @@ -444,6 +464,17 @@ AutowareJoyControllerNode::AutowareJoyControllerNode() "output/raw_vehicle_cmd", 1); + // Service Client + client_clear_emergency_stop_ = this->create_client("service/clear_emergency_stop"); + while (!client_clear_emergency_stop_->wait_for_service(std::chrono::seconds(1))) { + if (!rclcpp::ok()) { + RCLCPP_ERROR(get_logger(), "Interrupted while waiting for service."); + rclcpp::shutdown(); + return; + } + RCLCPP_INFO(get_logger(), "Waiting for clear_emergency_stop service connection..."); + } + // Timer initTimer(1.0 / update_rate_); } From 449f4ba22aae2ef0cd4d70ad87bb7e42cb51ac85 Mon Sep 17 00:00:00 2001 From: Kenji Miyake <31987104+kenji-miyake@users.noreply.github.com> Date: Thu, 25 Feb 2021 02:29:39 +0900 Subject: [PATCH 17/38] Rename ROS-related .yaml to .param.yaml (#352) * Rename ROS-related .yaml to .param.yaml Signed-off-by: Kenji Miyake * Remove prefix 'default_' of yaml files Signed-off-by: Kenji Miyake * Rename vehicle_info.yaml to vehicle_info.param.yaml Signed-off-by: Kenji Miyake * Rename diagnostic_aggregator's param files Signed-off-by: Kenji Miyake * Fix overlooked parameters Signed-off-by: Kenji Miyake --- ...e_joy_controller.yaml => autoware_joy_controller.param.yaml} | 0 .../launch/autoware_joy_controller.launch.xml | 2 +- 2 files changed, 1 insertion(+), 1 deletion(-) rename control/autoware_joy_controller/config/{autoware_joy_controller.yaml => autoware_joy_controller.param.yaml} (100%) diff --git a/control/autoware_joy_controller/config/autoware_joy_controller.yaml b/control/autoware_joy_controller/config/autoware_joy_controller.param.yaml similarity index 100% rename from control/autoware_joy_controller/config/autoware_joy_controller.yaml rename to control/autoware_joy_controller/config/autoware_joy_controller.param.yaml diff --git a/control/autoware_joy_controller/launch/autoware_joy_controller.launch.xml b/control/autoware_joy_controller/launch/autoware_joy_controller.launch.xml index 6a6b3098e454c..7e7063a1e5866 100644 --- a/control/autoware_joy_controller/launch/autoware_joy_controller.launch.xml +++ b/control/autoware_joy_controller/launch/autoware_joy_controller.launch.xml @@ -13,7 +13,7 @@ - + From 9c16639ff4fd7802e2a043ababa58cdad26dc2d1 Mon Sep 17 00:00:00 2001 From: Ryohsuke Mitsudome <43976834+mitsudome-r@users.noreply.github.com> Date: Mon, 1 Mar 2021 02:07:21 +0900 Subject: [PATCH 18/38] remove using in global namespace (#379) * remove using in global namespace (#1166) * remove using in global namespace * Revert "remove using in global namespace" This reverts commit 7f120509c9e3a036a38e84883868f6036bca23ad. * Add package namespace Signed-off-by: Kenji Miyake Co-authored-by: Kenji Miyake * [autoware_joy_controller] add lint tests Signed-off-by: mitsudome-r Co-authored-by: Kenji Miyake --- control/autoware_joy_controller/CMakeLists.txt | 7 ++++++- .../autoware_joy_controller.hpp | 9 +++++++-- .../joy_converter/ds4_joy_converter.hpp | 9 ++++++++- .../joy_converter/g29_joy_converter.hpp | 8 +++++++- .../joy_converter/joy_converter_base.hpp | 8 +++++++- control/autoware_joy_controller/package.xml | 3 +++ .../autoware_joy_controller_node.cpp | 15 ++++++++++++++- .../src/autoware_joy_controller/main.cpp | 3 ++- 8 files changed, 54 insertions(+), 8 deletions(-) diff --git a/control/autoware_joy_controller/CMakeLists.txt b/control/autoware_joy_controller/CMakeLists.txt index 553c840738536..c04d0f4041c4f 100644 --- a/control/autoware_joy_controller/CMakeLists.txt +++ b/control/autoware_joy_controller/CMakeLists.txt @@ -2,7 +2,7 @@ cmake_minimum_required(VERSION 3.5) project(autoware_joy_controller) if(NOT CMAKE_CXX_STANDARD) - set(CMAKE_CXX_STANDARD 14) + set(CMAKE_CXX_STANDARD 14) endif() if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang") add_compile_options(-Wall -Wextra -Wpedantic -Wno-unused-parameter) @@ -16,6 +16,11 @@ ament_auto_add_executable(autoware_joy_controller src/autoware_joy_controller/autoware_joy_controller_node.cpp ) +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/control/autoware_joy_controller/include/autoware_joy_controller/autoware_joy_controller.hpp b/control/autoware_joy_controller/include/autoware_joy_controller/autoware_joy_controller.hpp index 97ac08d1d6695..ade672e339f90 100644 --- a/control/autoware_joy_controller/include/autoware_joy_controller/autoware_joy_controller.hpp +++ b/control/autoware_joy_controller/include/autoware_joy_controller/autoware_joy_controller.hpp @@ -12,7 +12,8 @@ // See the License for the specific language governing permissions and // limitations under the License. -#pragma once +#ifndef AUTOWARE_JOY_CONTROLLER__AUTOWARE_JOY_CONTROLLER_HPP_ +#define AUTOWARE_JOY_CONTROLLER__AUTOWARE_JOY_CONTROLLER_HPP_ #include #include @@ -34,7 +35,8 @@ #include "std_srvs/srv/trigger.hpp" - +namespace autoware_joy_controller +{ using ShiftType = autoware_vehicle_msgs::msg::Shift::_data_type; using TurnSignalType = autoware_vehicle_msgs::msg::TurnSignal::_data_type; using GateModeType = autoware_control_msgs::msg::GateMode::_data_type; @@ -118,3 +120,6 @@ class AutowareJoyControllerNode : public rclcpp::Node bool isDataReady(); void onTimer(); }; +} // namespace autoware_joy_controller + +#endif // AUTOWARE_JOY_CONTROLLER__AUTOWARE_JOY_CONTROLLER_HPP_ diff --git a/control/autoware_joy_controller/include/autoware_joy_controller/joy_converter/ds4_joy_converter.hpp b/control/autoware_joy_controller/include/autoware_joy_controller/joy_converter/ds4_joy_converter.hpp index 2fe15e2531087..fd2a8d4079dae 100644 --- a/control/autoware_joy_controller/include/autoware_joy_controller/joy_converter/ds4_joy_converter.hpp +++ b/control/autoware_joy_controller/include/autoware_joy_controller/joy_converter/ds4_joy_converter.hpp @@ -12,10 +12,14 @@ // See the License for the specific language governing permissions and // limitations under the License. -#pragma once +#ifndef AUTOWARE_JOY_CONTROLLER__JOY_CONVERTER__DS4_JOY_CONVERTER_HPP_ +#define AUTOWARE_JOY_CONTROLLER__JOY_CONVERTER__DS4_JOY_CONVERTER_HPP_ +#include #include "autoware_joy_controller/joy_converter/joy_converter_base.hpp" +namespace autoware_joy_controller +{ class DS4JoyConverter : public JoyConverterBase { public: @@ -86,3 +90,6 @@ class DS4JoyConverter : public JoyConverterBase bool reverse() const {return Share();} }; +} // namespace autoware_joy_controller + +#endif // AUTOWARE_JOY_CONTROLLER__JOY_CONVERTER__DS4_JOY_CONVERTER_HPP_ diff --git a/control/autoware_joy_controller/include/autoware_joy_controller/joy_converter/g29_joy_converter.hpp b/control/autoware_joy_controller/include/autoware_joy_controller/joy_converter/g29_joy_converter.hpp index 82e5ddce9cb1e..4eb617b152342 100644 --- a/control/autoware_joy_controller/include/autoware_joy_controller/joy_converter/g29_joy_converter.hpp +++ b/control/autoware_joy_controller/include/autoware_joy_controller/joy_converter/g29_joy_converter.hpp @@ -12,10 +12,13 @@ // See the License for the specific language governing permissions and // limitations under the License. -#pragma once +#ifndef AUTOWARE_JOY_CONTROLLER__JOY_CONVERTER__G29_JOY_CONVERTER_HPP_ +#define AUTOWARE_JOY_CONTROLLER__JOY_CONVERTER__G29_JOY_CONVERTER_HPP_ #include "autoware_joy_controller/joy_converter/joy_converter_base.hpp" +namespace autoware_joy_controller +{ class G29JoyConverter : public JoyConverterBase { public: @@ -82,3 +85,6 @@ class G29JoyConverter : public JoyConverterBase bool reverse() const {return Share();} }; +} // namespace autoware_joy_controller + +#endif // AUTOWARE_JOY_CONTROLLER__JOY_CONVERTER__G29_JOY_CONVERTER_HPP_ diff --git a/control/autoware_joy_controller/include/autoware_joy_controller/joy_converter/joy_converter_base.hpp b/control/autoware_joy_controller/include/autoware_joy_controller/joy_converter/joy_converter_base.hpp index fb9e0c37f7c9e..4931db61f6d5a 100644 --- a/control/autoware_joy_controller/include/autoware_joy_controller/joy_converter/joy_converter_base.hpp +++ b/control/autoware_joy_controller/include/autoware_joy_controller/joy_converter/joy_converter_base.hpp @@ -12,12 +12,15 @@ // See the License for the specific language governing permissions and // limitations under the License. -#pragma once +#ifndef AUTOWARE_JOY_CONTROLLER__JOY_CONVERTER__JOY_CONVERTER_BASE_HPP_ +#define AUTOWARE_JOY_CONTROLLER__JOY_CONVERTER__JOY_CONVERTER_BASE_HPP_ #include #include "sensor_msgs/msg/joy.hpp" +namespace autoware_joy_controller +{ class JoyConverterBase { public: @@ -47,3 +50,6 @@ class JoyConverterBase virtual bool vehicle_engage() const = 0; virtual bool vehicle_disengage() const = 0; }; +} // namespace autoware_joy_controller + +#endif // AUTOWARE_JOY_CONTROLLER__JOY_CONVERTER__JOY_CONVERTER_BASE_HPP_ diff --git a/control/autoware_joy_controller/package.xml b/control/autoware_joy_controller/package.xml index d938227e8c9da..e5a859c33d77f 100644 --- a/control/autoware_joy_controller/package.xml +++ b/control/autoware_joy_controller/package.xml @@ -17,6 +17,9 @@ sensor_msgs std_srvs + ament_lint_auto + ament_lint_common + ament_cmake diff --git a/control/autoware_joy_controller/src/autoware_joy_controller/autoware_joy_controller_node.cpp b/control/autoware_joy_controller/src/autoware_joy_controller/autoware_joy_controller_node.cpp index e0941883e639a..edc7709a73171 100644 --- a/control/autoware_joy_controller/src/autoware_joy_controller/autoware_joy_controller_node.cpp +++ b/control/autoware_joy_controller/src/autoware_joy_controller/autoware_joy_controller_node.cpp @@ -12,12 +12,21 @@ // See the License for the specific language governing permissions and // limitations under the License. +#include +#include +#include +#include + #include "autoware_joy_controller/autoware_joy_controller.hpp" #include "autoware_joy_controller/joy_converter/g29_joy_converter.hpp" #include "autoware_joy_controller/joy_converter/ds4_joy_converter.hpp" namespace { +using autoware_joy_controller::GateModeType; +using autoware_joy_controller::ShiftType; +using autoware_joy_controller::TurnSignalType; + ShiftType getUpperShift(const ShiftType & shift) { using autoware_vehicle_msgs::msg::Shift; @@ -90,6 +99,8 @@ double calcMapping(const double input, const double sensitivity) } // namespace +namespace autoware_joy_controller +{ void AutowareJoyControllerNode::onJoy(const sensor_msgs::msg::Joy::ConstSharedPtr msg) { last_joy_received_time_ = msg->header.stamp; @@ -465,7 +476,8 @@ AutowareJoyControllerNode::AutowareJoyControllerNode() 1); // Service Client - client_clear_emergency_stop_ = this->create_client("service/clear_emergency_stop"); + client_clear_emergency_stop_ = this->create_client( + "service/clear_emergency_stop"); while (!client_clear_emergency_stop_->wait_for_service(std::chrono::seconds(1))) { if (!rclcpp::ok()) { RCLCPP_ERROR(get_logger(), "Interrupted while waiting for service."); @@ -478,3 +490,4 @@ AutowareJoyControllerNode::AutowareJoyControllerNode() // Timer initTimer(1.0 / update_rate_); } +} // namespace autoware_joy_controller diff --git a/control/autoware_joy_controller/src/autoware_joy_controller/main.cpp b/control/autoware_joy_controller/src/autoware_joy_controller/main.cpp index 0f61e1f59adfd..5dcece709a323 100644 --- a/control/autoware_joy_controller/src/autoware_joy_controller/main.cpp +++ b/control/autoware_joy_controller/src/autoware_joy_controller/main.cpp @@ -12,6 +12,7 @@ // See the License for the specific language governing permissions and // limitations under the License. +#include #include "rclcpp/rclcpp.hpp" #include "autoware_joy_controller/autoware_joy_controller.hpp" @@ -19,7 +20,7 @@ int main(int argc, char * argv[]) { rclcpp::init(argc, argv); - rclcpp::spin(std::make_shared()); + rclcpp::spin(std::make_shared()); rclcpp::shutdown(); return 0; From e4ab6a114bdd7acff238775e8ab273099175307b Mon Sep 17 00:00:00 2001 From: tkimura4 Date: Wed, 10 Mar 2021 13:51:28 +0900 Subject: [PATCH 19/38] fix namespace (#414) --- .../autoware_joy_controller_node.cpp | 8 ++++---- 1 file changed, 4 insertions(+), 4 deletions(-) diff --git a/control/autoware_joy_controller/src/autoware_joy_controller/autoware_joy_controller_node.cpp b/control/autoware_joy_controller/src/autoware_joy_controller/autoware_joy_controller_node.cpp index edc7709a73171..ee34c21c120e6 100644 --- a/control/autoware_joy_controller/src/autoware_joy_controller/autoware_joy_controller_node.cpp +++ b/control/autoware_joy_controller/src/autoware_joy_controller/autoware_joy_controller_node.cpp @@ -432,10 +432,10 @@ AutowareJoyControllerNode::AutowareJoyControllerNode() steering_angle_velocity_ = declare_parameter("steering_angle_velocity", 0.1); accel_sensitivity_ = declare_parameter("accel_sensitivity", 1.0); brake_sensitivity_ = declare_parameter("brake_sensitivity", 1.0); - velocity_gain_ = declare_parameter("control_command/velocity_gain", 3.0); - max_forward_velocity_ = declare_parameter("control_command/max_forward_velocity", 20.0); - max_backward_velocity_ = declare_parameter("control_command/max_backward_velocity", 3.0); - backward_accel_ratio_ = declare_parameter("control_command/backward_accel_ratio", 1.0); + velocity_gain_ = declare_parameter("control_command.velocity_gain", 3.0); + max_forward_velocity_ = declare_parameter("control_command.max_forward_velocity", 20.0); + max_backward_velocity_ = declare_parameter("control_command.max_backward_velocity", 3.0); + backward_accel_ratio_ = declare_parameter("control_command.backward_accel_ratio", 1.0); RCLCPP_INFO(get_logger(), "Joy type: %s", joy_type_.c_str()); From 226c5a2c31c4ececb86ed3818b22a2d929ba62d6 Mon Sep 17 00:00:00 2001 From: tkimura4 Date: Fri, 26 Mar 2021 09:47:00 +0900 Subject: [PATCH 20/38] add use_sim-time option (#454) --- .../launch/autoware_joy_controller.launch.xml | 2 ++ 1 file changed, 2 insertions(+) diff --git a/control/autoware_joy_controller/launch/autoware_joy_controller.launch.xml b/control/autoware_joy_controller/launch/autoware_joy_controller.launch.xml index 7e7063a1e5866..0d0ce1da970eb 100644 --- a/control/autoware_joy_controller/launch/autoware_joy_controller.launch.xml +++ b/control/autoware_joy_controller/launch/autoware_joy_controller.launch.xml @@ -16,6 +16,7 @@ + @@ -40,6 +41,7 @@ + From 59a39b1481523ab30fecbc7bfe2de1dfc79d005f Mon Sep 17 00:00:00 2001 From: Kenji Miyake <31987104+kenji-miyake@users.noreply.github.com> Date: Thu, 8 Apr 2021 15:10:43 +0900 Subject: [PATCH 21/38] Fix for rolling (#1226) * Replace doc by description Signed-off-by: Kenji Miyake * Replace ns by push-ros-namespace Signed-off-by: Kenji Miyake --- .../launch/autoware_joy_controller.launch.xml | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/control/autoware_joy_controller/launch/autoware_joy_controller.launch.xml b/control/autoware_joy_controller/launch/autoware_joy_controller.launch.xml index 0d0ce1da970eb..b835f8b8e8de2 100644 --- a/control/autoware_joy_controller/launch/autoware_joy_controller.launch.xml +++ b/control/autoware_joy_controller/launch/autoware_joy_controller.launch.xml @@ -1,5 +1,5 @@ - + From df5bf04f5d4ddf92432c630320b99d18b2db3fc3 Mon Sep 17 00:00:00 2001 From: Daisuke Nishimatsu <42202095+wep21@users.noreply.github.com> Date: Wed, 21 Apr 2021 11:28:52 +0900 Subject: [PATCH 22/38] Make control modules components (#1262) Signed-off-by: wep21 --- .../autoware_joy_controller/CMakeLists.txt | 8 ++++-- .../autoware_joy_controller.hpp | 2 +- control/autoware_joy_controller/package.xml | 1 + .../autoware_joy_controller_node.cpp | 7 +++-- .../src/autoware_joy_controller/main.cpp | 27 ------------------- 5 files changed, 13 insertions(+), 32 deletions(-) delete mode 100644 control/autoware_joy_controller/src/autoware_joy_controller/main.cpp diff --git a/control/autoware_joy_controller/CMakeLists.txt b/control/autoware_joy_controller/CMakeLists.txt index c04d0f4041c4f..227fb72531c4f 100644 --- a/control/autoware_joy_controller/CMakeLists.txt +++ b/control/autoware_joy_controller/CMakeLists.txt @@ -11,11 +11,15 @@ endif() find_package(ament_cmake_auto REQUIRED) ament_auto_find_build_dependencies() -ament_auto_add_executable(autoware_joy_controller - src/autoware_joy_controller/main.cpp +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() diff --git a/control/autoware_joy_controller/include/autoware_joy_controller/autoware_joy_controller.hpp b/control/autoware_joy_controller/include/autoware_joy_controller/autoware_joy_controller.hpp index ade672e339f90..8a701ba887344 100644 --- a/control/autoware_joy_controller/include/autoware_joy_controller/autoware_joy_controller.hpp +++ b/control/autoware_joy_controller/include/autoware_joy_controller/autoware_joy_controller.hpp @@ -44,7 +44,7 @@ using GateModeType = autoware_control_msgs::msg::GateMode::_data_type; class AutowareJoyControllerNode : public rclcpp::Node { public: - AutowareJoyControllerNode(); + explicit AutowareJoyControllerNode(const rclcpp::NodeOptions & node_options); private: // Parameter diff --git a/control/autoware_joy_controller/package.xml b/control/autoware_joy_controller/package.xml index e5a859c33d77f..8f643abca5f6c 100644 --- a/control/autoware_joy_controller/package.xml +++ b/control/autoware_joy_controller/package.xml @@ -14,6 +14,7 @@ joy geometry_msgs rclcpp + rclcpp_components sensor_msgs std_srvs diff --git a/control/autoware_joy_controller/src/autoware_joy_controller/autoware_joy_controller_node.cpp b/control/autoware_joy_controller/src/autoware_joy_controller/autoware_joy_controller_node.cpp index ee34c21c120e6..568df69ad2aec 100644 --- a/control/autoware_joy_controller/src/autoware_joy_controller/autoware_joy_controller_node.cpp +++ b/control/autoware_joy_controller/src/autoware_joy_controller/autoware_joy_controller_node.cpp @@ -420,8 +420,8 @@ void AutowareJoyControllerNode::initTimer(double period_s) this->get_node_timers_interface()->add_timer(timer_, nullptr); } -AutowareJoyControllerNode::AutowareJoyControllerNode() -: Node("autoware_joy_controller") +AutowareJoyControllerNode::AutowareJoyControllerNode(const rclcpp::NodeOptions & node_options) +: Node("autoware_joy_controller", node_options) { // Parameter joy_type_ = declare_parameter("joy_type", std::string("DS4")); @@ -491,3 +491,6 @@ AutowareJoyControllerNode::AutowareJoyControllerNode() initTimer(1.0 / update_rate_); } } // namespace autoware_joy_controller + +#include "rclcpp_components/register_node_macro.hpp" +RCLCPP_COMPONENTS_REGISTER_NODE(autoware_joy_controller::AutowareJoyControllerNode) diff --git a/control/autoware_joy_controller/src/autoware_joy_controller/main.cpp b/control/autoware_joy_controller/src/autoware_joy_controller/main.cpp deleted file mode 100644 index 5dcece709a323..0000000000000 --- a/control/autoware_joy_controller/src/autoware_joy_controller/main.cpp +++ /dev/null @@ -1,27 +0,0 @@ -// 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. - -#include - -#include "rclcpp/rclcpp.hpp" -#include "autoware_joy_controller/autoware_joy_controller.hpp" - -int main(int argc, char * argv[]) -{ - rclcpp::init(argc, argv); - rclcpp::spin(std::make_shared()); - rclcpp::shutdown(); - - return 0; -} From 9e5b51fd1dcfd0b2a6a01804bf70747f2532e134 Mon Sep 17 00:00:00 2001 From: Daisuke Nishimatsu <42202095+wep21@users.noreply.github.com> Date: Mon, 26 Apr 2021 15:29:13 +0900 Subject: [PATCH 23/38] Remove use_sim_time for set_parameter (#1260) Signed-off-by: wep21 --- .../launch/autoware_joy_controller.launch.xml | 2 -- 1 file changed, 2 deletions(-) diff --git a/control/autoware_joy_controller/launch/autoware_joy_controller.launch.xml b/control/autoware_joy_controller/launch/autoware_joy_controller.launch.xml index b835f8b8e8de2..456ef123fc919 100644 --- a/control/autoware_joy_controller/launch/autoware_joy_controller.launch.xml +++ b/control/autoware_joy_controller/launch/autoware_joy_controller.launch.xml @@ -16,7 +16,6 @@ - @@ -41,7 +40,6 @@ - From 1919d0ce9ff8d41ccd9e75ad85a367975de16bb2 Mon Sep 17 00:00:00 2001 From: Kenji Miyake <31987104+kenji-miyake@users.noreply.github.com> Date: Sat, 8 May 2021 00:30:57 +0900 Subject: [PATCH 24/38] Remove autoware_debug_msgs from autoware_joy_controller (#1303) Signed-off-by: Kenji Miyake --- .../autoware_joy_controller.hpp | 9 +++---- control/autoware_joy_controller/package.xml | 1 - .../autoware_joy_controller_node.cpp | 24 +++++++++---------- 3 files changed, 17 insertions(+), 17 deletions(-) diff --git a/control/autoware_joy_controller/include/autoware_joy_controller/autoware_joy_controller.hpp b/control/autoware_joy_controller/include/autoware_joy_controller/autoware_joy_controller.hpp index 8a701ba887344..2c762a5d7973e 100644 --- a/control/autoware_joy_controller/include/autoware_joy_controller/autoware_joy_controller.hpp +++ b/control/autoware_joy_controller/include/autoware_joy_controller/autoware_joy_controller.hpp @@ -23,10 +23,11 @@ #include "autoware_control_msgs/msg/control_command_stamped.hpp" #include "autoware_control_msgs/msg/gate_mode.hpp" -#include "autoware_debug_msgs/msg/bool_stamped.hpp" +#include "autoware_control_msgs/msg/emergency_mode.hpp" #include "autoware_vehicle_msgs/msg/raw_control_command_stamped.hpp" #include "autoware_vehicle_msgs/msg/shift_stamped.hpp" #include "autoware_vehicle_msgs/msg/turn_signal.hpp" +#include "autoware_vehicle_msgs/msg/engage.hpp" #include "geometry_msgs/msg/twist_stamped.hpp" #include "sensor_msgs/msg/joy.hpp" #include "autoware_joy_controller/joy_converter/joy_converter_base.hpp" @@ -82,9 +83,9 @@ class AutowareJoyControllerNode : public rclcpp::Node rclcpp::Publisher::SharedPtr pub_shift_; rclcpp::Publisher::SharedPtr pub_turn_signal_; rclcpp::Publisher::SharedPtr pub_gate_mode_; - rclcpp::Publisher::SharedPtr pub_emergency_stop_; - rclcpp::Publisher::SharedPtr pub_autoware_engage_; - rclcpp::Publisher::SharedPtr pub_vehicle_engage_; + rclcpp::Publisher::SharedPtr pub_emergency_stop_; + rclcpp::Publisher::SharedPtr pub_autoware_engage_; + rclcpp::Publisher::SharedPtr pub_vehicle_engage_; void publishControlCommand(); void publishRawControlCommand(); diff --git a/control/autoware_joy_controller/package.xml b/control/autoware_joy_controller/package.xml index 8f643abca5f6c..169394c777298 100644 --- a/control/autoware_joy_controller/package.xml +++ b/control/autoware_joy_controller/package.xml @@ -10,7 +10,6 @@ autoware_control_msgs autoware_vehicle_msgs - autoware_debug_msgs joy geometry_msgs rclcpp diff --git a/control/autoware_joy_controller/src/autoware_joy_controller/autoware_joy_controller_node.cpp b/control/autoware_joy_controller/src/autoware_joy_controller/autoware_joy_controller_node.cpp index 568df69ad2aec..8da68327890de 100644 --- a/control/autoware_joy_controller/src/autoware_joy_controller/autoware_joy_controller_node.cpp +++ b/control/autoware_joy_controller/src/autoware_joy_controller/autoware_joy_controller_node.cpp @@ -325,15 +325,15 @@ void AutowareJoyControllerNode::publishGateMode() void AutowareJoyControllerNode::publishEmergencyStop() { - autoware_debug_msgs::msg::BoolStamped emergency; + autoware_control_msgs::msg::EmergencyMode emergency; if (joy_->emergency_stop()) { - emergency.data = true; + emergency.is_emergency = true; RCLCPP_INFO(get_logger(), "Emergency"); } if (joy_->clear_emergency_stop()) { - emergency.data = false; + emergency.is_emergency = false; auto request = std::make_shared(); auto result = client_clear_emergency_stop_->async_send_request( @@ -356,15 +356,15 @@ void AutowareJoyControllerNode::clearEmergencyResponse( void AutowareJoyControllerNode::publishAutowareEngage() { - autoware_debug_msgs::msg::BoolStamped engage; + autoware_vehicle_msgs::msg::Engage engage; if (joy_->autoware_engage()) { - engage.data = true; + engage.engage = true; RCLCPP_INFO(get_logger(), "Autoware Engage"); } if (joy_->autoware_disengage()) { - engage.data = false; + engage.engage = false; RCLCPP_INFO(get_logger(), "Autoware Disengage"); } @@ -373,15 +373,15 @@ void AutowareJoyControllerNode::publishAutowareEngage() void AutowareJoyControllerNode::publishVehicleEngage() { - autoware_debug_msgs::msg::BoolStamped engage; + autoware_vehicle_msgs::msg::Engage engage; if (joy_->vehicle_engage()) { - engage.data = true; + engage.engage = true; RCLCPP_INFO(get_logger(), "Vehicle Engage"); } if (joy_->vehicle_disengage()) { - engage.data = false; + engage.engage = false; RCLCPP_INFO(get_logger(), "Vehicle Disengage"); } @@ -459,12 +459,12 @@ AutowareJoyControllerNode::AutowareJoyControllerNode(const rclcpp::NodeOptions & pub_gate_mode_ = this->create_publisher( "output/gate_mode", 1); - pub_emergency_stop_ = this->create_publisher( + pub_emergency_stop_ = this->create_publisher( "output/emergency_stop", 1); - pub_autoware_engage_ = this->create_publisher( + pub_autoware_engage_ = this->create_publisher( "output/autoware_engage", 1); - pub_vehicle_engage_ = this->create_publisher( + pub_vehicle_engage_ = this->create_publisher( "output/vehicle_engage", 1); // tmp From cb2108749533c3f48c963b2d7615014fd4e6dec0 Mon Sep 17 00:00:00 2001 From: Keisuke Shima Date: Sat, 8 May 2021 03:03:14 +0900 Subject: [PATCH 25/38] Porting remote cmd selector (#1286) * Feature/add remote cmd selector (#1179) * Add in/out args of remote_cmd_converter.launch Signed-off-by: Kenji Miyake * Change remote input topic of vehicle_cmd_gate Signed-off-by: Kenji Miyake * Add msgs for remote_cmd_selector Signed-off-by: Kenji Miyake * Add remote_cmd_selector Signed-off-by: Kenji Miyake * Rename remote_cmd_selector to external_cmd_selector Signed-off-by: Kenji Miyake * Remove VehicleCommand support in autoware_joy_controller Signed-off-by: Kenji Miyake * Support external_cmd_source in autoware_joy_controller.launch (#1194) Signed-off-by: Kenji Miyake * Fix porting miss Signed-off-by: Kenji Miyake * 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 * Change default mode of autoware_joy_controller Signed-off-by: Kenji Miyake Co-authored-by: Kenji Miyake <31987104+kenji-miyake@users.noreply.github.com> Co-authored-by: Kenji Miyake --- .../autoware_joy_controller.hpp | 11 ++--- .../launch/autoware_joy_controller.launch.xml | 13 ++--- .../autoware_joy_controller_node.cpp | 47 +++++-------------- 3 files changed, 20 insertions(+), 51 deletions(-) diff --git a/control/autoware_joy_controller/include/autoware_joy_controller/autoware_joy_controller.hpp b/control/autoware_joy_controller/include/autoware_joy_controller/autoware_joy_controller.hpp index 2c762a5d7973e..a9ed9debb3e79 100644 --- a/control/autoware_joy_controller/include/autoware_joy_controller/autoware_joy_controller.hpp +++ b/control/autoware_joy_controller/include/autoware_joy_controller/autoware_joy_controller.hpp @@ -64,6 +64,10 @@ class AutowareJoyControllerNode : public rclcpp::Node double max_backward_velocity_; double backward_accel_ratio_; + // CallbackGroups + rclcpp::CallbackGroup::SharedPtr callback_group_subscribers_; + rclcpp::CallbackGroup::SharedPtr callback_group_services_; + // Subscriber rclcpp::Subscription::SharedPtr sub_joy_; rclcpp::Subscription::SharedPtr sub_twist_; @@ -107,13 +111,6 @@ class AutowareJoyControllerNode : public rclcpp::Node TurnSignalType prev_turn_signal_ = autoware_vehicle_msgs::msg::TurnSignal::NONE; GateModeType prev_gate_mode_ = autoware_control_msgs::msg::GateMode::AUTO; - // tmp - rclcpp::Publisher::SharedPtr pub_vehicle_command_; - rclcpp::Publisher::SharedPtr - pub_raw_vehicle_command_; - void publishVehicleCommand(); - void publishRawVehicleCommand(); - // Timer rclcpp::TimerBase::SharedPtr timer_; void initTimer(double period_s); diff --git a/control/autoware_joy_controller/launch/autoware_joy_controller.launch.xml b/control/autoware_joy_controller/launch/autoware_joy_controller.launch.xml index 456ef123fc919..18f1676b68bb9 100644 --- a/control/autoware_joy_controller/launch/autoware_joy_controller.launch.xml +++ b/control/autoware_joy_controller/launch/autoware_joy_controller.launch.xml @@ -1,13 +1,14 @@ + - - - - + + + + @@ -29,10 +30,6 @@ - - - - diff --git a/control/autoware_joy_controller/src/autoware_joy_controller/autoware_joy_controller_node.cpp b/control/autoware_joy_controller/src/autoware_joy_controller/autoware_joy_controller_node.cpp index 8da68327890de..5e5118490489e 100644 --- a/control/autoware_joy_controller/src/autoware_joy_controller/autoware_joy_controller_node.cpp +++ b/control/autoware_joy_controller/src/autoware_joy_controller/autoware_joy_controller_node.cpp @@ -188,10 +188,6 @@ void AutowareJoyControllerNode::onTimer() publishControlCommand(); publishRawControlCommand(); publishEmergencyStop(); - - // tmp - publishVehicleCommand(); - publishRawVehicleCommand(); } void AutowareJoyControllerNode::publishControlCommand() @@ -388,27 +384,6 @@ void AutowareJoyControllerNode::publishVehicleEngage() pub_vehicle_engage_->publish(engage); } -// tmp -void AutowareJoyControllerNode::publishVehicleCommand() -{ - autoware_vehicle_msgs::msg::VehicleCommand vehicle_cmd; - vehicle_cmd.header.stamp = this->now(); - vehicle_cmd.control = prev_control_command_; - vehicle_cmd.shift.data = prev_shift_; - - pub_vehicle_command_->publish(vehicle_cmd); -} - -void AutowareJoyControllerNode::publishRawVehicleCommand() -{ - autoware_vehicle_msgs::msg::RawVehicleCommand vehicle_cmd; - vehicle_cmd.header.stamp = this->now(); - vehicle_cmd.control = prev_raw_control_command_; - vehicle_cmd.shift.data = prev_shift_; - - pub_raw_vehicle_command_->publish(vehicle_cmd); -} - void AutowareJoyControllerNode::initTimer(double period_s) { auto timer_callback = std::bind(&AutowareJoyControllerNode::onTimer, this); @@ -439,13 +414,21 @@ AutowareJoyControllerNode::AutowareJoyControllerNode(const rclcpp::NodeOptions & RCLCPP_INFO(get_logger(), "Joy type: %s", joy_type_.c_str()); + // Callback Groups + callback_group_subscribers_ = this->create_callback_group( + rclcpp::CallbackGroupType::MutuallyExclusive); + callback_group_services_ = this->create_callback_group( + rclcpp::CallbackGroupType::MutuallyExclusive); + auto subscriber_option = rclcpp::SubscriptionOptions(); + subscriber_option.callback_group = callback_group_subscribers_; + // Subscriber sub_joy_ = this->create_subscription( "input/joy", 1, - std::bind(&AutowareJoyControllerNode::onJoy, this, std::placeholders::_1)); + std::bind(&AutowareJoyControllerNode::onJoy, this, std::placeholders::_1), subscriber_option); sub_twist_ = this->create_subscription( "input/twist", 1, - std::bind(&AutowareJoyControllerNode::onTwist, this, std::placeholders::_1)); + std::bind(&AutowareJoyControllerNode::onTwist, this, std::placeholders::_1), subscriber_option); // Publisher pub_control_command_ = this->create_publisher( @@ -467,17 +450,9 @@ AutowareJoyControllerNode::AutowareJoyControllerNode(const rclcpp::NodeOptions & pub_vehicle_engage_ = this->create_publisher( "output/vehicle_engage", 1); - // tmp - pub_vehicle_command_ = - this->create_publisher("output/vehicle_cmd", 1); - pub_raw_vehicle_command_ = - this->create_publisher( - "output/raw_vehicle_cmd", - 1); - // Service Client client_clear_emergency_stop_ = this->create_client( - "service/clear_emergency_stop"); + "service/clear_emergency_stop", rmw_qos_profile_services_default, callback_group_services_); while (!client_clear_emergency_stop_->wait_for_service(std::chrono::seconds(1))) { if (!rclcpp::ok()) { RCLCPP_ERROR(get_logger(), "Interrupted while waiting for service."); From 01531d6a5d970265eda38e38ed87e4fe90101b39 Mon Sep 17 00:00:00 2001 From: taikitanaka3 <65527974+taikitanaka3@users.noreply.github.com> Date: Mon, 31 May 2021 17:34:07 +0900 Subject: [PATCH 26/38] Ros2/create/external commands (#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 --- .../autoware_joy_controller.hpp | 11 +++++------ .../launch/autoware_joy_controller.launch.xml | 4 ++-- .../autoware_joy_controller_node.cpp | 16 ++++++++-------- 3 files changed, 15 insertions(+), 16 deletions(-) diff --git a/control/autoware_joy_controller/include/autoware_joy_controller/autoware_joy_controller.hpp b/control/autoware_joy_controller/include/autoware_joy_controller/autoware_joy_controller.hpp index a9ed9debb3e79..33ad359106928 100644 --- a/control/autoware_joy_controller/include/autoware_joy_controller/autoware_joy_controller.hpp +++ b/control/autoware_joy_controller/include/autoware_joy_controller/autoware_joy_controller.hpp @@ -23,15 +23,14 @@ #include "autoware_control_msgs/msg/control_command_stamped.hpp" #include "autoware_control_msgs/msg/gate_mode.hpp" +#include "autoware_vehicle_msgs/msg/external_control_command_stamped.hpp" #include "autoware_control_msgs/msg/emergency_mode.hpp" -#include "autoware_vehicle_msgs/msg/raw_control_command_stamped.hpp" #include "autoware_vehicle_msgs/msg/shift_stamped.hpp" #include "autoware_vehicle_msgs/msg/turn_signal.hpp" #include "autoware_vehicle_msgs/msg/engage.hpp" #include "geometry_msgs/msg/twist_stamped.hpp" #include "sensor_msgs/msg/joy.hpp" #include "autoware_joy_controller/joy_converter/joy_converter_base.hpp" -#include "autoware_vehicle_msgs/msg/raw_vehicle_command.hpp" #include "autoware_vehicle_msgs/msg/vehicle_command.hpp" #include "std_srvs/srv/trigger.hpp" @@ -82,8 +81,8 @@ class AutowareJoyControllerNode : public rclcpp::Node // Publisher rclcpp::Publisher::SharedPtr pub_control_command_; - rclcpp::Publisher::SharedPtr - pub_raw_control_command_; + rclcpp::Publisher::SharedPtr + pub_external_control_command_; rclcpp::Publisher::SharedPtr pub_shift_; rclcpp::Publisher::SharedPtr pub_turn_signal_; rclcpp::Publisher::SharedPtr pub_gate_mode_; @@ -92,7 +91,7 @@ class AutowareJoyControllerNode : public rclcpp::Node rclcpp::Publisher::SharedPtr pub_vehicle_engage_; void publishControlCommand(); - void publishRawControlCommand(); + void publishExternalControlCommand(); void publishShift(); void publishTurnSignal(); void publishGateMode(); @@ -106,7 +105,7 @@ class AutowareJoyControllerNode : public rclcpp::Node // Previous State autoware_control_msgs::msg::ControlCommand prev_control_command_; - autoware_vehicle_msgs::msg::RawControlCommand prev_raw_control_command_; + autoware_vehicle_msgs::msg::ExternalControlCommand prev_external_control_command_; ShiftType prev_shift_ = autoware_vehicle_msgs::msg::Shift::NONE; TurnSignalType prev_turn_signal_ = autoware_vehicle_msgs::msg::TurnSignal::NONE; GateModeType prev_gate_mode_ = autoware_control_msgs::msg::GateMode::AUTO; diff --git a/control/autoware_joy_controller/launch/autoware_joy_controller.launch.xml b/control/autoware_joy_controller/launch/autoware_joy_controller.launch.xml index 18f1676b68bb9..e9d5a798809e3 100644 --- a/control/autoware_joy_controller/launch/autoware_joy_controller.launch.xml +++ b/control/autoware_joy_controller/launch/autoware_joy_controller.launch.xml @@ -6,7 +6,7 @@ - + @@ -21,7 +21,7 @@ - + diff --git a/control/autoware_joy_controller/src/autoware_joy_controller/autoware_joy_controller_node.cpp b/control/autoware_joy_controller/src/autoware_joy_controller/autoware_joy_controller_node.cpp index 5e5118490489e..6c53d380257f5 100644 --- a/control/autoware_joy_controller/src/autoware_joy_controller/autoware_joy_controller_node.cpp +++ b/control/autoware_joy_controller/src/autoware_joy_controller/autoware_joy_controller_node.cpp @@ -186,7 +186,7 @@ void AutowareJoyControllerNode::onTimer() } publishControlCommand(); - publishRawControlCommand(); + publishExternalControlCommand(); publishEmergencyStop(); } @@ -224,9 +224,9 @@ void AutowareJoyControllerNode::publishControlCommand() prev_control_command_ = cmd_stamped.control; } -void AutowareJoyControllerNode::publishRawControlCommand() +void AutowareJoyControllerNode::publishExternalControlCommand() { - autoware_vehicle_msgs::msg::RawControlCommandStamped cmd_stamped; + autoware_vehicle_msgs::msg::ExternalControlCommandStamped cmd_stamped; cmd_stamped.header.stamp = this->now(); { @@ -239,8 +239,8 @@ void AutowareJoyControllerNode::publishRawControlCommand() cmd.brake = brake_ratio_ * calcMapping(static_cast(joy_->brake()), brake_sensitivity_); } - pub_raw_control_command_->publish(cmd_stamped); - prev_raw_control_command_ = cmd_stamped.control; + pub_external_control_command_->publish(cmd_stamped); + prev_external_control_command_ = cmd_stamped.control; } void AutowareJoyControllerNode::publishShift() @@ -433,9 +433,9 @@ AutowareJoyControllerNode::AutowareJoyControllerNode(const rclcpp::NodeOptions & // Publisher pub_control_command_ = this->create_publisher( "output/control_command", 1); - pub_raw_control_command_ = - this->create_publisher( - "output/raw_control_command", 1); + pub_external_control_command_ = + this->create_publisher( + "output/external_control_command", 1); pub_shift_ = this->create_publisher("output/shift", 1); pub_turn_signal_ = this->create_publisher("output/turn_signal", 1); From 39a0365f481808b22bd0c8e83e8dddc6487c816d Mon Sep 17 00:00:00 2001 From: Kenji Miyake <31987104+kenji-miyake@users.noreply.github.com> Date: Sun, 15 Aug 2021 07:28:48 +0900 Subject: [PATCH 27/38] Fix -Wunused-parameter (#1836) * Fix -Wunused-parameter Signed-off-by: Kenji Miyake * Fix mistake Signed-off-by: Kenji Miyake * fix spell * Fix lint issues Signed-off-by: Kenji Miyake * Ignore flake8 warnings Signed-off-by: Kenji Miyake Co-authored-by: Hiroki OTA --- control/autoware_joy_controller/CMakeLists.txt | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/control/autoware_joy_controller/CMakeLists.txt b/control/autoware_joy_controller/CMakeLists.txt index 227fb72531c4f..32d679b3d5d22 100644 --- a/control/autoware_joy_controller/CMakeLists.txt +++ b/control/autoware_joy_controller/CMakeLists.txt @@ -5,7 +5,7 @@ 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 -Wno-unused-parameter) + add_compile_options(-Wall -Wextra -Wpedantic) endif() find_package(ament_cmake_auto REQUIRED) From 9d8381aa60daa8d91a936f771d9951ee3cb6695e Mon Sep 17 00:00:00 2001 From: "Takagi, Isamu" <43976882+isamu-takagi@users.noreply.github.com> Date: Tue, 31 Aug 2021 17:01:55 +0900 Subject: [PATCH 28/38] Add autoware api (#1979) --- .../autoware_joy_controller/CMakeLists.txt | 2 +- .../autoware_joy_controller.hpp | 35 ++- .../launch/autoware_joy_controller.launch.xml | 8 +- control/autoware_joy_controller/package.xml | 2 + .../autoware_joy_controller_node.cpp | 216 +++++++++--------- 5 files changed, 127 insertions(+), 136 deletions(-) diff --git a/control/autoware_joy_controller/CMakeLists.txt b/control/autoware_joy_controller/CMakeLists.txt index 32d679b3d5d22..55bd83ac16158 100644 --- a/control/autoware_joy_controller/CMakeLists.txt +++ b/control/autoware_joy_controller/CMakeLists.txt @@ -5,7 +5,7 @@ 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) + add_compile_options(-Wall -Wextra -Wpedantic -Werror) endif() find_package(ament_cmake_auto REQUIRED) diff --git a/control/autoware_joy_controller/include/autoware_joy_controller/autoware_joy_controller.hpp b/control/autoware_joy_controller/include/autoware_joy_controller/autoware_joy_controller.hpp index 33ad359106928..733e4f7b03da4 100644 --- a/control/autoware_joy_controller/include/autoware_joy_controller/autoware_joy_controller.hpp +++ b/control/autoware_joy_controller/include/autoware_joy_controller/autoware_joy_controller.hpp @@ -21,24 +21,23 @@ #include "rclcpp/rclcpp.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/turn_signal_stamped.hpp" +#include "autoware_external_api_msgs/msg/heartbeat.hpp" +#include "autoware_external_api_msgs/srv/set_emergency.hpp" #include "autoware_control_msgs/msg/control_command_stamped.hpp" #include "autoware_control_msgs/msg/gate_mode.hpp" -#include "autoware_vehicle_msgs/msg/external_control_command_stamped.hpp" -#include "autoware_control_msgs/msg/emergency_mode.hpp" -#include "autoware_vehicle_msgs/msg/shift_stamped.hpp" -#include "autoware_vehicle_msgs/msg/turn_signal.hpp" #include "autoware_vehicle_msgs/msg/engage.hpp" #include "geometry_msgs/msg/twist_stamped.hpp" #include "sensor_msgs/msg/joy.hpp" #include "autoware_joy_controller/joy_converter/joy_converter_base.hpp" #include "autoware_vehicle_msgs/msg/vehicle_command.hpp" -#include "std_srvs/srv/trigger.hpp" - namespace autoware_joy_controller { -using ShiftType = autoware_vehicle_msgs::msg::Shift::_data_type; -using TurnSignalType = autoware_vehicle_msgs::msg::TurnSignal::_data_type; +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 @@ -81,12 +80,12 @@ class AutowareJoyControllerNode : public rclcpp::Node // Publisher rclcpp::Publisher::SharedPtr pub_control_command_; - rclcpp::Publisher::SharedPtr + rclcpp::Publisher::SharedPtr pub_external_control_command_; - rclcpp::Publisher::SharedPtr pub_shift_; - rclcpp::Publisher::SharedPtr pub_turn_signal_; + rclcpp::Publisher::SharedPtr pub_shift_; + rclcpp::Publisher::SharedPtr pub_turn_signal_; + rclcpp::Publisher::SharedPtr pub_heartbeat_; rclcpp::Publisher::SharedPtr pub_gate_mode_; - rclcpp::Publisher::SharedPtr pub_emergency_stop_; rclcpp::Publisher::SharedPtr pub_autoware_engage_; rclcpp::Publisher::SharedPtr pub_vehicle_engage_; @@ -95,19 +94,19 @@ class AutowareJoyControllerNode : public rclcpp::Node void publishShift(); void publishTurnSignal(); void publishGateMode(); - void publishEmergencyStop(); + void publishHeartbeat(); void publishAutowareEngage(); void publishVehicleEngage(); - void clearEmergencyResponse(rclcpp::Client::SharedFuture result); + void sendEmergencyRequest(bool emergency); // Service Client - rclcpp::Client::SharedPtr client_clear_emergency_stop_; + rclcpp::Client::SharedPtr client_emergency_stop_; // Previous State autoware_control_msgs::msg::ControlCommand prev_control_command_; - autoware_vehicle_msgs::msg::ExternalControlCommand prev_external_control_command_; - ShiftType prev_shift_ = autoware_vehicle_msgs::msg::Shift::NONE; - TurnSignalType prev_turn_signal_ = autoware_vehicle_msgs::msg::TurnSignal::NONE; + 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 diff --git a/control/autoware_joy_controller/launch/autoware_joy_controller.launch.xml b/control/autoware_joy_controller/launch/autoware_joy_controller.launch.xml index e9d5a798809e3..7d01e3d655c77 100644 --- a/control/autoware_joy_controller/launch/autoware_joy_controller.launch.xml +++ b/control/autoware_joy_controller/launch/autoware_joy_controller.launch.xml @@ -9,8 +9,8 @@ - - + + @@ -25,12 +25,12 @@ - + - + diff --git a/control/autoware_joy_controller/package.xml b/control/autoware_joy_controller/package.xml index 169394c777298..525a74a4add6d 100644 --- a/control/autoware_joy_controller/package.xml +++ b/control/autoware_joy_controller/package.xml @@ -8,6 +8,8 @@ ament_cmake_auto + autoware_api_utils + autoware_external_api_msgs autoware_control_msgs autoware_vehicle_msgs joy diff --git a/control/autoware_joy_controller/src/autoware_joy_controller/autoware_joy_controller_node.cpp b/control/autoware_joy_controller/src/autoware_joy_controller/autoware_joy_controller_node.cpp index 6c53d380257f5..7ca11be3bdb9d 100644 --- a/control/autoware_joy_controller/src/autoware_joy_controller/autoware_joy_controller_node.cpp +++ b/control/autoware_joy_controller/src/autoware_joy_controller/autoware_joy_controller_node.cpp @@ -20,59 +20,55 @@ #include "autoware_joy_controller/autoware_joy_controller.hpp" #include "autoware_joy_controller/joy_converter/g29_joy_converter.hpp" #include "autoware_joy_controller/joy_converter/ds4_joy_converter.hpp" +#include "autoware_api_utils/autoware_api_utils.hpp" namespace { using autoware_joy_controller::GateModeType; -using autoware_joy_controller::ShiftType; +using autoware_joy_controller::GearShiftType; using autoware_joy_controller::TurnSignalType; +using GearShift = autoware_external_api_msgs::msg::GearShift; +using TurnSignal = autoware_external_api_msgs::msg::TurnSignal; +using GateMode = autoware_control_msgs::msg::GateMode; -ShiftType getUpperShift(const ShiftType & shift) +GearShiftType getUpperShift(const GearShiftType & shift) { - using autoware_vehicle_msgs::msg::Shift; - - if (shift == Shift::NONE) {return Shift::PARKING;} - if (shift == Shift::PARKING) {return Shift::REVERSE;} - if (shift == Shift::REVERSE) {return Shift::NEUTRAL;} - if (shift == Shift::NEUTRAL) {return Shift::DRIVE;} - if (shift == Shift::DRIVE) {return Shift::LOW;} - if (shift == Shift::LOW) {return Shift::LOW;} - - return Shift::NONE; + if (shift == GearShift::NONE) {return GearShift::PARKING;} + if (shift == GearShift::PARKING) {return GearShift::REVERSE;} + if (shift == GearShift::REVERSE) {return GearShift::NEUTRAL;} + if (shift == GearShift::NEUTRAL) {return GearShift::DRIVE;} + if (shift == GearShift::DRIVE) {return GearShift::LOW;} + if (shift == GearShift::LOW) {return GearShift::LOW;} + + return GearShift::NONE; } -ShiftType getLowerShift(const ShiftType & shift) +GearShiftType getLowerShift(const GearShiftType & shift) { - using autoware_vehicle_msgs::msg::Shift; - - if (shift == Shift::NONE) {return Shift::PARKING;} - if (shift == Shift::PARKING) {return Shift::PARKING;} - if (shift == Shift::REVERSE) {return Shift::PARKING;} - if (shift == Shift::NEUTRAL) {return Shift::REVERSE;} - if (shift == Shift::DRIVE) {return Shift::NEUTRAL;} - if (shift == Shift::LOW) {return Shift::DRIVE;} - - return Shift::NONE; + if (shift == GearShift::NONE) {return GearShift::PARKING;} + if (shift == GearShift::PARKING) {return GearShift::PARKING;} + if (shift == GearShift::REVERSE) {return GearShift::PARKING;} + if (shift == GearShift::NEUTRAL) {return GearShift::REVERSE;} + if (shift == GearShift::DRIVE) {return GearShift::NEUTRAL;} + if (shift == GearShift::LOW) {return GearShift::DRIVE;} + + return GearShift::NONE; } -const char * getShiftName(const ShiftType & shift) +const char * getShiftName(const GearShiftType & shift) { - using autoware_vehicle_msgs::msg::Shift; - - if (shift == Shift::NONE) {return "NONE";} - if (shift == Shift::PARKING) {return "PARKING";} - if (shift == Shift::REVERSE) {return "REVERSE";} - if (shift == Shift::NEUTRAL) {return "NEUTRAL";} - if (shift == Shift::DRIVE) {return "DRIVE";} - if (shift == Shift::LOW) {return "LOW";} + if (shift == GearShift::NONE) {return "NONE";} + if (shift == GearShift::PARKING) {return "PARKING";} + if (shift == GearShift::REVERSE) {return "REVERSE";} + if (shift == GearShift::NEUTRAL) {return "NEUTRAL";} + if (shift == GearShift::DRIVE) {return "DRIVE";} + if (shift == GearShift::LOW) {return "LOW";} return "NOT_SUPPORTED"; } const char * getTurnSignalName(const TurnSignalType & turn_signal) { - using autoware_vehicle_msgs::msg::TurnSignal; - if (turn_signal == TurnSignal::NONE) {return "NONE";} if (turn_signal == TurnSignal::LEFT) {return "LEFT";} if (turn_signal == TurnSignal::RIGHT) {return "RIGHT";} @@ -86,7 +82,7 @@ const char * getGateModeName(const GateModeType & gate_mode) using autoware_control_msgs::msg::GateMode; if (gate_mode == GateMode::AUTO) {return "AUTO";} - if (gate_mode == GateMode::REMOTE) {return "REMOTE";} + if (gate_mode == GateMode::EXTERNAL) {return "EXTERNAL";} return "NOT_SUPPORTED"; } @@ -129,6 +125,14 @@ void AutowareJoyControllerNode::onJoy(const sensor_msgs::msg::Joy::ConstSharedPt if (joy_->vehicle_engage() || joy_->vehicle_disengage()) { publishVehicleEngage(); } + + if (joy_->emergency_stop()) { + sendEmergencyRequest(true); + } + + if (joy_->clear_emergency_stop()) { + sendEmergencyRequest(false); + } } void AutowareJoyControllerNode::onTwist(const geometry_msgs::msg::TwistStamped::ConstSharedPtr msg) @@ -187,14 +191,13 @@ void AutowareJoyControllerNode::onTimer() publishControlCommand(); publishExternalControlCommand(); - publishEmergencyStop(); + publishHeartbeat(); } void AutowareJoyControllerNode::publishControlCommand() { autoware_control_msgs::msg::ControlCommandStamped cmd_stamped; cmd_stamped.header.stamp = this->now(); - { auto & cmd = cmd_stamped.control; @@ -226,9 +229,8 @@ void AutowareJoyControllerNode::publishControlCommand() void AutowareJoyControllerNode::publishExternalControlCommand() { - autoware_vehicle_msgs::msg::ExternalControlCommandStamped cmd_stamped; - cmd_stamped.header.stamp = this->now(); - + autoware_external_api_msgs::msg::ControlCommandStamped cmd_stamped; + cmd_stamped.stamp = this->now(); { auto & cmd = cmd_stamped.control; @@ -245,71 +247,62 @@ void AutowareJoyControllerNode::publishExternalControlCommand() void AutowareJoyControllerNode::publishShift() { - using autoware_vehicle_msgs::msg::Shift; + autoware_external_api_msgs::msg::GearShiftStamped gear_shift; + gear_shift.stamp = this->now(); - autoware_vehicle_msgs::msg::ShiftStamped shift_stamped; - shift_stamped.header.stamp = this->now(); - - { - auto & shift = shift_stamped.shift; - if (joy_->shift_up()) { - shift.data = getUpperShift(prev_shift_); - } - - if (joy_->shift_down()) { - shift.data = getLowerShift(prev_shift_); - } + if (joy_->shift_up()) { + gear_shift.gear_shift.data = getUpperShift(prev_shift_); + } - if (joy_->shift_drive()) { - shift.data = Shift::DRIVE; - } + if (joy_->shift_down()) { + gear_shift.gear_shift.data = getLowerShift(prev_shift_); + } - if (joy_->shift_reverse()) { - shift.data = Shift::REVERSE; - } + if (joy_->shift_drive()) { + gear_shift.gear_shift.data = GearShift::DRIVE; + } - RCLCPP_INFO(get_logger(), "Shift::%s", getShiftName(shift.data)); + if (joy_->shift_reverse()) { + gear_shift.gear_shift.data = GearShift::REVERSE; } - pub_shift_->publish(shift_stamped); - prev_shift_ = shift_stamped.shift.data; + RCLCPP_INFO(get_logger(), "GearShift::%s", getShiftName(gear_shift.gear_shift.data)); + + pub_shift_->publish(gear_shift); + prev_shift_ = gear_shift.gear_shift.data; } void AutowareJoyControllerNode::publishTurnSignal() { - using autoware_vehicle_msgs::msg::TurnSignal; - - TurnSignal turn_signal; - turn_signal.header.stamp = this->now(); + autoware_external_api_msgs::msg::TurnSignalStamped turn_signal; + turn_signal.stamp = this->now(); if (joy_->turn_signal_left() && joy_->turn_signal_right()) { - turn_signal.data = TurnSignal::HAZARD; + turn_signal.turn_signal.data = TurnSignal::HAZARD; } else if (joy_->turn_signal_left()) { - turn_signal.data = TurnSignal::LEFT; + turn_signal.turn_signal.data = TurnSignal::LEFT; } else if (joy_->turn_signal_right()) { - turn_signal.data = TurnSignal::RIGHT; + turn_signal.turn_signal.data = TurnSignal::RIGHT; } if (joy_->clear_turn_signal()) { - turn_signal.data = TurnSignal::NONE; + turn_signal.turn_signal.data = TurnSignal::NONE; } - RCLCPP_INFO(get_logger(), "TurnSignal::%s", getTurnSignalName(turn_signal.data)); + RCLCPP_INFO(get_logger(), "TurnSignal::%s", getTurnSignalName(turn_signal.turn_signal.data)); pub_turn_signal_->publish(turn_signal); } void AutowareJoyControllerNode::publishGateMode() { - using autoware_control_msgs::msg::GateMode; - autoware_control_msgs::msg::GateMode gate_mode; if (prev_gate_mode_ == GateMode::AUTO) { - gate_mode.data = GateMode::REMOTE; + gate_mode.data = GateMode::EXTERNAL; } - if (prev_gate_mode_ == GateMode::REMOTE) { + if (prev_gate_mode_ == GateMode::EXTERNAL) { gate_mode.data = GateMode::AUTO; } @@ -319,35 +312,33 @@ void AutowareJoyControllerNode::publishGateMode() prev_gate_mode_ = gate_mode.data; } -void AutowareJoyControllerNode::publishEmergencyStop() +void AutowareJoyControllerNode::publishHeartbeat() { - autoware_control_msgs::msg::EmergencyMode emergency; - - if (joy_->emergency_stop()) { - emergency.is_emergency = true; - RCLCPP_INFO(get_logger(), "Emergency"); - } - - if (joy_->clear_emergency_stop()) { - emergency.is_emergency = false; - - auto request = std::make_shared(); - auto result = client_clear_emergency_stop_->async_send_request( - request, - std::bind(&AutowareJoyControllerNode::clearEmergencyResponse, this, std::placeholders::_1)); - } - - pub_emergency_stop_->publish(emergency); + autoware_external_api_msgs::msg::Heartbeat heartbeat; + heartbeat.stamp = this->now(); + pub_heartbeat_->publish(heartbeat); } -void AutowareJoyControllerNode::clearEmergencyResponse( - rclcpp::Client::SharedFuture result) +void AutowareJoyControllerNode::sendEmergencyRequest(bool emergency) { - if (result.get()->success) { - RCLCPP_INFO(get_logger(), "Clear Emergency Stop"); - } else { - RCLCPP_WARN(get_logger(), "failed to clear emergency stop: %s", result.get()->message.c_str()); - } + RCLCPP_INFO(get_logger(), "%s emergency stop", emergency ? "Set" : "Clear"); + + auto request = std::make_shared(); + request->emergency = emergency; + + client_emergency_stop_->async_send_request( + request, + [this, emergency] + (rclcpp::Client::SharedFuture result) + { + auto response = result.get(); + if (autoware_api_utils::is_success(response->status)) { + RCLCPP_INFO(get_logger(), "service succeeded"); + } else { + RCLCPP_WARN(get_logger(), "service failed: %s", response->status.message.c_str()); + } + } + ); } void AutowareJoyControllerNode::publishAutowareEngage() @@ -434,32 +425,31 @@ AutowareJoyControllerNode::AutowareJoyControllerNode(const rclcpp::NodeOptions & pub_control_command_ = this->create_publisher( "output/control_command", 1); pub_external_control_command_ = - this->create_publisher( + this->create_publisher( "output/external_control_command", 1); - pub_shift_ = this->create_publisher("output/shift", 1); - pub_turn_signal_ = - this->create_publisher("output/turn_signal", 1); + pub_shift_ = this->create_publisher( + "output/shift", 1); + pub_turn_signal_ = this->create_publisher( + "output/turn_signal", 1); pub_gate_mode_ = this->create_publisher( - "output/gate_mode", - 1); - pub_emergency_stop_ = this->create_publisher( - "output/emergency_stop", - 1); + "output/gate_mode", 1); + pub_heartbeat_ = this->create_publisher( + "output/heartbeat", 1); pub_autoware_engage_ = this->create_publisher( "output/autoware_engage", 1); pub_vehicle_engage_ = this->create_publisher( "output/vehicle_engage", 1); // Service Client - client_clear_emergency_stop_ = this->create_client( - "service/clear_emergency_stop", rmw_qos_profile_services_default, callback_group_services_); - while (!client_clear_emergency_stop_->wait_for_service(std::chrono::seconds(1))) { + client_emergency_stop_ = this->create_client( + "service/emergency_stop", rmw_qos_profile_services_default, callback_group_services_); + while (!client_emergency_stop_->wait_for_service(std::chrono::seconds(1))) { if (!rclcpp::ok()) { RCLCPP_ERROR(get_logger(), "Interrupted while waiting for service."); rclcpp::shutdown(); return; } - RCLCPP_INFO(get_logger(), "Waiting for clear_emergency_stop service connection..."); + RCLCPP_INFO(get_logger(), "Waiting for emergency_stop service connection..."); } // Timer From 4368ea0fb43b18089aeee42595ffeddcef2361f7 Mon Sep 17 00:00:00 2001 From: Kenji Miyake <31987104+kenji-miyake@users.noreply.github.com> Date: Wed, 8 Sep 2021 18:28:23 +0900 Subject: [PATCH 29/38] Use EmergencyState instead of deprecated EmergencyMode (#2030) * Use EmergencyState instead of deprecated EmergencyMode Signed-off-by: Kenji Miyake * Use stamped type Signed-off-by: Kenji Miyake --- .../launch/autoware_joy_controller.launch.xml | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/control/autoware_joy_controller/launch/autoware_joy_controller.launch.xml b/control/autoware_joy_controller/launch/autoware_joy_controller.launch.xml index 7d01e3d655c77..593a2d337c2a5 100644 --- a/control/autoware_joy_controller/launch/autoware_joy_controller.launch.xml +++ b/control/autoware_joy_controller/launch/autoware_joy_controller.launch.xml @@ -9,7 +9,7 @@ - + @@ -25,7 +25,7 @@ - + From 509df2e9e9cc6a943eb23d925499c21a57eff0fe Mon Sep 17 00:00:00 2001 From: Keisuke Shima <19993104+KeisukeShima@users.noreply.github.com> Date: Thu, 9 Sep 2021 18:44:51 +0900 Subject: [PATCH 30/38] add sort-package-xml hook in pre-commit (#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 --- control/autoware_joy_controller/package.xml | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/control/autoware_joy_controller/package.xml b/control/autoware_joy_controller/package.xml index 525a74a4add6d..21650224749e0 100644 --- a/control/autoware_joy_controller/package.xml +++ b/control/autoware_joy_controller/package.xml @@ -9,11 +9,11 @@ ament_cmake_auto autoware_api_utils - autoware_external_api_msgs autoware_control_msgs + autoware_external_api_msgs autoware_vehicle_msgs - joy geometry_msgs + joy rclcpp rclcpp_components sensor_msgs From f75215298bceeee7a549c17fd6240d551d1e6e8a Mon Sep 17 00:00:00 2001 From: "Takagi, Isamu" <43976882+isamu-takagi@users.noreply.github.com> Date: Fri, 10 Sep 2021 13:29:18 +0900 Subject: [PATCH 31/38] Add selected external command API (#2053) --- .../launch/autoware_joy_controller.launch.xml | 8 ++++---- 1 file changed, 4 insertions(+), 4 deletions(-) diff --git a/control/autoware_joy_controller/launch/autoware_joy_controller.launch.xml b/control/autoware_joy_controller/launch/autoware_joy_controller.launch.xml index 593a2d337c2a5..98893ddc8c260 100644 --- a/control/autoware_joy_controller/launch/autoware_joy_controller.launch.xml +++ b/control/autoware_joy_controller/launch/autoware_joy_controller.launch.xml @@ -6,10 +6,10 @@ - - - - + + + + From e6857cb71ea7742b11230c982e06bbb5f6550b31 Mon Sep 17 00:00:00 2001 From: Hiroki OTA Date: Fri, 29 Oct 2021 22:44:31 +0900 Subject: [PATCH 32/38] submit engage with api service from joy controller (#2320) * fix engagew with api * delete unused * fix for uncrustify * revive vehicle_engage * some fix * revive autoware name * fix service name --- .../autoware_joy_controller.hpp | 3 ++- .../launch/autoware_joy_controller.launch.xml | 4 +-- .../autoware_joy_controller_node.cpp | 25 +++++++++++++------ 3 files changed, 21 insertions(+), 11 deletions(-) diff --git a/control/autoware_joy_controller/include/autoware_joy_controller/autoware_joy_controller.hpp b/control/autoware_joy_controller/include/autoware_joy_controller/autoware_joy_controller.hpp index 733e4f7b03da4..da606a550c5ff 100644 --- a/control/autoware_joy_controller/include/autoware_joy_controller/autoware_joy_controller.hpp +++ b/control/autoware_joy_controller/include/autoware_joy_controller/autoware_joy_controller.hpp @@ -25,6 +25,7 @@ #include "autoware_external_api_msgs/msg/gear_shift_stamped.hpp" #include "autoware_external_api_msgs/msg/turn_signal_stamped.hpp" #include "autoware_external_api_msgs/msg/heartbeat.hpp" +#include "autoware_external_api_msgs/srv/engage.hpp" #include "autoware_external_api_msgs/srv/set_emergency.hpp" #include "autoware_control_msgs/msg/control_command_stamped.hpp" #include "autoware_control_msgs/msg/gate_mode.hpp" @@ -86,7 +87,6 @@ class AutowareJoyControllerNode : public rclcpp::Node rclcpp::Publisher::SharedPtr pub_turn_signal_; rclcpp::Publisher::SharedPtr pub_heartbeat_; rclcpp::Publisher::SharedPtr pub_gate_mode_; - rclcpp::Publisher::SharedPtr pub_autoware_engage_; rclcpp::Publisher::SharedPtr pub_vehicle_engage_; void publishControlCommand(); @@ -101,6 +101,7 @@ class AutowareJoyControllerNode : public rclcpp::Node // Service Client rclcpp::Client::SharedPtr client_emergency_stop_; + rclcpp::Client::SharedPtr client_autoware_engage_; // Previous State autoware_control_msgs::msg::ControlCommand prev_control_command_; diff --git a/control/autoware_joy_controller/launch/autoware_joy_controller.launch.xml b/control/autoware_joy_controller/launch/autoware_joy_controller.launch.xml index 98893ddc8c260..c7e66ce545df2 100644 --- a/control/autoware_joy_controller/launch/autoware_joy_controller.launch.xml +++ b/control/autoware_joy_controller/launch/autoware_joy_controller.launch.xml @@ -11,7 +11,6 @@ - @@ -27,10 +26,9 @@ - - + diff --git a/control/autoware_joy_controller/src/autoware_joy_controller/autoware_joy_controller_node.cpp b/control/autoware_joy_controller/src/autoware_joy_controller/autoware_joy_controller_node.cpp index 7ca11be3bdb9d..0a521e83a2745 100644 --- a/control/autoware_joy_controller/src/autoware_joy_controller/autoware_joy_controller_node.cpp +++ b/control/autoware_joy_controller/src/autoware_joy_controller/autoware_joy_controller_node.cpp @@ -343,19 +343,29 @@ void AutowareJoyControllerNode::sendEmergencyRequest(bool emergency) void AutowareJoyControllerNode::publishAutowareEngage() { - autoware_vehicle_msgs::msg::Engage engage; - + auto req = std::make_shared(); if (joy_->autoware_engage()) { - engage.engage = true; + req->engage = true; RCLCPP_INFO(get_logger(), "Autoware Engage"); } if (joy_->autoware_disengage()) { - engage.engage = false; + req->engage = false; RCLCPP_INFO(get_logger(), "Autoware Disengage"); } - pub_autoware_engage_->publish(engage); + if (!client_autoware_engage_->service_is_ready()) { + RCLCPP_WARN(get_logger(), "%s is unavailable", client_autoware_engage_->get_service_name()); + return; + } + + client_autoware_engage_->async_send_request( + req, + [this](rclcpp::Client::SharedFuture result) { + RCLCPP_INFO( + get_logger(), "%s: %d, %s", client_autoware_engage_->get_service_name(), + result.get()->status.code, result.get()->status.message.c_str()); + }); } void AutowareJoyControllerNode::publishVehicleEngage() @@ -435,8 +445,6 @@ AutowareJoyControllerNode::AutowareJoyControllerNode(const rclcpp::NodeOptions & "output/gate_mode", 1); pub_heartbeat_ = this->create_publisher( "output/heartbeat", 1); - pub_autoware_engage_ = this->create_publisher( - "output/autoware_engage", 1); pub_vehicle_engage_ = this->create_publisher( "output/vehicle_engage", 1); @@ -452,6 +460,9 @@ AutowareJoyControllerNode::AutowareJoyControllerNode(const rclcpp::NodeOptions & RCLCPP_INFO(get_logger(), "Waiting for emergency_stop service connection..."); } + client_autoware_engage_ = this->create_client( + "service/autoware_engage", rmw_qos_profile_services_default); + // Timer initTimer(1.0 / update_rate_); } From 2ffed42f3d186d359860ad8b09bae76ee76e2736 Mon Sep 17 00:00:00 2001 From: Kenji Miyake <31987104+kenji-miyake@users.noreply.github.com> Date: Tue, 2 Nov 2021 21:49:19 +0900 Subject: [PATCH 33/38] Change formatter to clang-format and black (#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 * Remove ament_cmake_uncrustify and ament_clang_format Signed-off-by: Kenji Miyake * Apply Black Signed-off-by: Kenji Miyake * Apply clang-format Signed-off-by: Kenji Miyake * Fix build errors Signed-off-by: Kenji Miyake * Fix for cpplint * Fix include double quotes to angle brackets Signed-off-by: Kenji Miyake * Apply clang-format Signed-off-by: Kenji Miyake * Fix build errors Signed-off-by: Kenji Miyake --- .../autoware_joy_controller.hpp | 33 ++-- .../joy_converter/ds4_joy_converter.hpp | 78 ++++----- .../joy_converter/g29_joy_converter.hpp | 79 ++++----- .../joy_converter/joy_converter_base.hpp | 4 +- control/autoware_joy_controller/package.xml | 2 +- .../autoware_joy_controller_node.cpp | 158 +++++++++++------- 6 files changed, 201 insertions(+), 153 deletions(-) diff --git a/control/autoware_joy_controller/include/autoware_joy_controller/autoware_joy_controller.hpp b/control/autoware_joy_controller/include/autoware_joy_controller/autoware_joy_controller.hpp index da606a550c5ff..c3836e04799f2 100644 --- a/control/autoware_joy_controller/include/autoware_joy_controller/autoware_joy_controller.hpp +++ b/control/autoware_joy_controller/include/autoware_joy_controller/autoware_joy_controller.hpp @@ -15,26 +15,27 @@ #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 + +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + #include #include #include -#include "rclcpp/rclcpp.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/turn_signal_stamped.hpp" -#include "autoware_external_api_msgs/msg/heartbeat.hpp" -#include "autoware_external_api_msgs/srv/engage.hpp" -#include "autoware_external_api_msgs/srv/set_emergency.hpp" -#include "autoware_control_msgs/msg/control_command_stamped.hpp" -#include "autoware_control_msgs/msg/gate_mode.hpp" -#include "autoware_vehicle_msgs/msg/engage.hpp" -#include "geometry_msgs/msg/twist_stamped.hpp" -#include "sensor_msgs/msg/joy.hpp" -#include "autoware_joy_controller/joy_converter/joy_converter_base.hpp" -#include "autoware_vehicle_msgs/msg/vehicle_command.hpp" - namespace autoware_joy_controller { using GearShiftType = autoware_external_api_msgs::msg::GearShift::_data_type; diff --git a/control/autoware_joy_controller/include/autoware_joy_controller/joy_converter/ds4_joy_converter.hpp b/control/autoware_joy_controller/include/autoware_joy_controller/joy_converter/ds4_joy_converter.hpp index fd2a8d4079dae..0a35696808d94 100644 --- a/control/autoware_joy_controller/include/autoware_joy_controller/joy_converter/ds4_joy_converter.hpp +++ b/control/autoware_joy_controller/include/autoware_joy_controller/joy_converter/ds4_joy_converter.hpp @@ -15,16 +15,16 @@ #ifndef AUTOWARE_JOY_CONTROLLER__JOY_CONVERTER__DS4_JOY_CONVERTER_HPP_ #define AUTOWARE_JOY_CONTROLLER__JOY_CONVERTER__DS4_JOY_CONVERTER_HPP_ -#include #include "autoware_joy_controller/joy_converter/joy_converter_base.hpp" +#include + namespace autoware_joy_controller { class DS4JoyConverter : public JoyConverterBase { public: - explicit DS4JoyConverter(const sensor_msgs::msg::Joy & j) - : j_(j) {} + explicit DS4JoyConverter(const sensor_msgs::msg::Joy & j) : j_(j) {} float accel() const { @@ -42,53 +42,53 @@ class DS4JoyConverter : public JoyConverterBase return std::max({button, stick, trigger}); } - float steer() const {return LStickLeftRight();} + 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 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 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 gate_mode() const { return Options(); } - bool emergency_stop() const {return !reverse() && PS();} - bool clear_emergency_stop() const {return reverse() && PS();} + 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 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();} + 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);} + 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();} + bool reverse() const { return Share(); } }; } // namespace autoware_joy_controller diff --git a/control/autoware_joy_controller/include/autoware_joy_controller/joy_converter/g29_joy_converter.hpp b/control/autoware_joy_controller/include/autoware_joy_controller/joy_converter/g29_joy_converter.hpp index 4eb617b152342..22039cdf6f95f 100644 --- a/control/autoware_joy_controller/include/autoware_joy_controller/joy_converter/g29_joy_converter.hpp +++ b/control/autoware_joy_controller/include/autoware_joy_controller/joy_converter/g29_joy_converter.hpp @@ -22,68 +22,71 @@ namespace autoware_joy_controller class G29JoyConverter : public JoyConverterBase { public: - explicit G29JoyConverter(const sensor_msgs::msg::Joy & j) - : j_(j) {} + explicit G29JoyConverter(const sensor_msgs::msg::Joy & j) : j_(j) {} float accel() const { constexpr float eps = 0.0000001; - if (std::fabs(AccelPedal()) < eps) {return 0.0f;} + if (std::fabs(AccelPedal()) < eps) { + return 0.0f; + } return (AccelPedal() + 1.0f) / 2; } float brake() const { constexpr float eps = 0.0000001; - if (std::fabs(BrakePedal()) < eps) {return 0.0f;} + if (std::fabs(BrakePedal()) < eps) { + return 0.0f; + } return (BrakePedal() + 1.0f) / 2; } - float steer() const {return Steer();} + float steer() const { return Steer(); } - 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 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 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 gate_mode() const { return Options(); } - bool emergency_stop() const {return !reverse() && PS();} - bool clear_emergency_stop() const {return reverse() && PS();} + 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 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();} + bool vehicle_engage() const { return !reverse() && Triangle(); } + bool vehicle_disengage() const { return reverse() && Triangle(); } private: - float Steer() const {return j_.axes.at(0);} - float AccelPedal() const {return j_.axes.at(2);} - float BrakePedal() const {return j_.axes.at(3);} - - float CursorLeftRight() const {return j_.axes.at(4);} - float CursorUpDown() const {return j_.axes.at(5);} - - 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(5);} - bool R1() const {return j_.buttons.at(4);} - bool L2() const {return j_.buttons.at(7);} - bool R2() const {return j_.buttons.at(6);} - bool Share() const {return j_.buttons.at(8);} - bool Options() const {return j_.buttons.at(9);} - bool PS() const {return j_.buttons.at(24);} + float Steer() const { return j_.axes.at(0); } + float AccelPedal() const { return j_.axes.at(2); } + float BrakePedal() const { return j_.axes.at(3); } + + float CursorLeftRight() const { return j_.axes.at(4); } + float CursorUpDown() const { return j_.axes.at(5); } + + 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(5); } + bool R1() const { return j_.buttons.at(4); } + bool L2() const { return j_.buttons.at(7); } + bool R2() const { return j_.buttons.at(6); } + bool Share() const { return j_.buttons.at(8); } + bool Options() const { return j_.buttons.at(9); } + bool PS() const { return j_.buttons.at(24); } const sensor_msgs::msg::Joy j_; - bool reverse() const {return Share();} + bool reverse() const { return Share(); } }; } // namespace autoware_joy_controller diff --git a/control/autoware_joy_controller/include/autoware_joy_controller/joy_converter/joy_converter_base.hpp b/control/autoware_joy_controller/include/autoware_joy_controller/joy_converter/joy_converter_base.hpp index 4931db61f6d5a..3da69c915e21a 100644 --- a/control/autoware_joy_controller/include/autoware_joy_controller/joy_converter/joy_converter_base.hpp +++ b/control/autoware_joy_controller/include/autoware_joy_controller/joy_converter/joy_converter_base.hpp @@ -15,9 +15,9 @@ #ifndef AUTOWARE_JOY_CONTROLLER__JOY_CONVERTER__JOY_CONVERTER_BASE_HPP_ #define AUTOWARE_JOY_CONTROLLER__JOY_CONVERTER__JOY_CONVERTER_BASE_HPP_ -#include +#include -#include "sensor_msgs/msg/joy.hpp" +#include namespace autoware_joy_controller { diff --git a/control/autoware_joy_controller/package.xml b/control/autoware_joy_controller/package.xml index 21650224749e0..c5aae048e4356 100644 --- a/control/autoware_joy_controller/package.xml +++ b/control/autoware_joy_controller/package.xml @@ -20,7 +20,7 @@ std_srvs ament_lint_auto - ament_lint_common + autoware_lint_common ament_cmake diff --git a/control/autoware_joy_controller/src/autoware_joy_controller/autoware_joy_controller_node.cpp b/control/autoware_joy_controller/src/autoware_joy_controller/autoware_joy_controller_node.cpp index 0a521e83a2745..df0951dde46a1 100644 --- a/control/autoware_joy_controller/src/autoware_joy_controller/autoware_joy_controller_node.cpp +++ b/control/autoware_joy_controller/src/autoware_joy_controller/autoware_joy_controller_node.cpp @@ -12,16 +12,17 @@ // See the License for the specific language governing permissions and // limitations under the License. +#include "autoware_joy_controller/autoware_joy_controller.hpp" +#include "autoware_joy_controller/joy_converter/ds4_joy_converter.hpp" +#include "autoware_joy_controller/joy_converter/g29_joy_converter.hpp" + +#include + #include #include #include #include -#include "autoware_joy_controller/autoware_joy_controller.hpp" -#include "autoware_joy_controller/joy_converter/g29_joy_converter.hpp" -#include "autoware_joy_controller/joy_converter/ds4_joy_converter.hpp" -#include "autoware_api_utils/autoware_api_utils.hpp" - namespace { using autoware_joy_controller::GateModeType; @@ -33,46 +34,90 @@ using GateMode = autoware_control_msgs::msg::GateMode; GearShiftType getUpperShift(const GearShiftType & shift) { - if (shift == GearShift::NONE) {return GearShift::PARKING;} - if (shift == GearShift::PARKING) {return GearShift::REVERSE;} - if (shift == GearShift::REVERSE) {return GearShift::NEUTRAL;} - if (shift == GearShift::NEUTRAL) {return GearShift::DRIVE;} - if (shift == GearShift::DRIVE) {return GearShift::LOW;} - if (shift == GearShift::LOW) {return GearShift::LOW;} + if (shift == GearShift::NONE) { + return GearShift::PARKING; + } + if (shift == GearShift::PARKING) { + return GearShift::REVERSE; + } + if (shift == GearShift::REVERSE) { + return GearShift::NEUTRAL; + } + if (shift == GearShift::NEUTRAL) { + return GearShift::DRIVE; + } + if (shift == GearShift::DRIVE) { + return GearShift::LOW; + } + if (shift == GearShift::LOW) { + return GearShift::LOW; + } return GearShift::NONE; } GearShiftType getLowerShift(const GearShiftType & shift) { - if (shift == GearShift::NONE) {return GearShift::PARKING;} - if (shift == GearShift::PARKING) {return GearShift::PARKING;} - if (shift == GearShift::REVERSE) {return GearShift::PARKING;} - if (shift == GearShift::NEUTRAL) {return GearShift::REVERSE;} - if (shift == GearShift::DRIVE) {return GearShift::NEUTRAL;} - if (shift == GearShift::LOW) {return GearShift::DRIVE;} + if (shift == GearShift::NONE) { + return GearShift::PARKING; + } + if (shift == GearShift::PARKING) { + return GearShift::PARKING; + } + if (shift == GearShift::REVERSE) { + return GearShift::PARKING; + } + if (shift == GearShift::NEUTRAL) { + return GearShift::REVERSE; + } + if (shift == GearShift::DRIVE) { + return GearShift::NEUTRAL; + } + if (shift == GearShift::LOW) { + return GearShift::DRIVE; + } return GearShift::NONE; } const char * getShiftName(const GearShiftType & shift) { - if (shift == GearShift::NONE) {return "NONE";} - if (shift == GearShift::PARKING) {return "PARKING";} - if (shift == GearShift::REVERSE) {return "REVERSE";} - if (shift == GearShift::NEUTRAL) {return "NEUTRAL";} - if (shift == GearShift::DRIVE) {return "DRIVE";} - if (shift == GearShift::LOW) {return "LOW";} + if (shift == GearShift::NONE) { + return "NONE"; + } + if (shift == GearShift::PARKING) { + return "PARKING"; + } + if (shift == GearShift::REVERSE) { + return "REVERSE"; + } + if (shift == GearShift::NEUTRAL) { + return "NEUTRAL"; + } + if (shift == GearShift::DRIVE) { + return "DRIVE"; + } + if (shift == GearShift::LOW) { + return "LOW"; + } return "NOT_SUPPORTED"; } const char * getTurnSignalName(const TurnSignalType & turn_signal) { - if (turn_signal == TurnSignal::NONE) {return "NONE";} - if (turn_signal == TurnSignal::LEFT) {return "LEFT";} - if (turn_signal == TurnSignal::RIGHT) {return "RIGHT";} - if (turn_signal == TurnSignal::HAZARD) {return "HAZARD";} + if (turn_signal == TurnSignal::NONE) { + return "NONE"; + } + if (turn_signal == TurnSignal::LEFT) { + return "LEFT"; + } + if (turn_signal == TurnSignal::RIGHT) { + return "RIGHT"; + } + if (turn_signal == TurnSignal::HAZARD) { + return "HAZARD"; + } return "NOT_SUPPORTED"; } @@ -81,8 +126,12 @@ const char * getGateModeName(const GateModeType & gate_mode) { using autoware_control_msgs::msg::GateMode; - if (gate_mode == GateMode::AUTO) {return "AUTO";} - if (gate_mode == GateMode::EXTERNAL) {return "EXTERNAL";} + if (gate_mode == GateMode::AUTO) { + return "AUTO"; + } + if (gate_mode == GateMode::EXTERNAL) { + return "EXTERNAL"; + } return "NOT_SUPPORTED"; } @@ -155,8 +204,7 @@ bool AutowareJoyControllerNode::isDataReady() const auto time_diff = this->now() - last_joy_received_time_; if (time_diff.seconds() > timeout) { RCLCPP_WARN_THROTTLE( - get_logger(), *get_clock(), std::chrono::milliseconds(1000).count(), - "joy msg is timeout"); + get_logger(), *get_clock(), std::chrono::milliseconds(1000).count(), "joy msg is timeout"); return false; } } @@ -327,18 +375,15 @@ void AutowareJoyControllerNode::sendEmergencyRequest(bool emergency) request->emergency = emergency; client_emergency_stop_->async_send_request( - request, - [this, emergency] - (rclcpp::Client::SharedFuture result) - { + request, [this, emergency]( + rclcpp::Client::SharedFuture result) { auto response = result.get(); if (autoware_api_utils::is_success(response->status)) { RCLCPP_INFO(get_logger(), "service succeeded"); } else { RCLCPP_WARN(get_logger(), "service failed: %s", response->status.message.c_str()); } - } - ); + }); } void AutowareJoyControllerNode::publishAutowareEngage() @@ -360,8 +405,7 @@ void AutowareJoyControllerNode::publishAutowareEngage() } client_autoware_engage_->async_send_request( - req, - [this](rclcpp::Client::SharedFuture result) { + req, [this](rclcpp::Client::SharedFuture result) { RCLCPP_INFO( get_logger(), "%s: %d, %s", client_autoware_engage_->get_service_name(), result.get()->status.code, result.get()->status.message.c_str()); @@ -416,37 +460,37 @@ AutowareJoyControllerNode::AutowareJoyControllerNode(const rclcpp::NodeOptions & RCLCPP_INFO(get_logger(), "Joy type: %s", joy_type_.c_str()); // Callback Groups - callback_group_subscribers_ = this->create_callback_group( - rclcpp::CallbackGroupType::MutuallyExclusive); - callback_group_services_ = this->create_callback_group( - rclcpp::CallbackGroupType::MutuallyExclusive); + callback_group_subscribers_ = + this->create_callback_group(rclcpp::CallbackGroupType::MutuallyExclusive); + callback_group_services_ = + this->create_callback_group(rclcpp::CallbackGroupType::MutuallyExclusive); auto subscriber_option = rclcpp::SubscriptionOptions(); subscriber_option.callback_group = callback_group_subscribers_; // Subscriber sub_joy_ = this->create_subscription( - "input/joy", 1, - std::bind(&AutowareJoyControllerNode::onJoy, this, std::placeholders::_1), subscriber_option); + "input/joy", 1, std::bind(&AutowareJoyControllerNode::onJoy, this, std::placeholders::_1), + subscriber_option); sub_twist_ = this->create_subscription( - "input/twist", 1, - std::bind(&AutowareJoyControllerNode::onTwist, this, std::placeholders::_1), subscriber_option); + "input/twist", 1, std::bind(&AutowareJoyControllerNode::onTwist, this, std::placeholders::_1), + subscriber_option); // Publisher pub_control_command_ = this->create_publisher( "output/control_command", 1); pub_external_control_command_ = this->create_publisher( - "output/external_control_command", 1); - pub_shift_ = this->create_publisher( - "output/shift", 1); + "output/external_control_command", 1); + pub_shift_ = + this->create_publisher("output/shift", 1); pub_turn_signal_ = this->create_publisher( "output/turn_signal", 1); - pub_gate_mode_ = this->create_publisher( - "output/gate_mode", 1); - pub_heartbeat_ = this->create_publisher( - "output/heartbeat", 1); - pub_vehicle_engage_ = this->create_publisher( - "output/vehicle_engage", 1); + pub_gate_mode_ = + this->create_publisher("output/gate_mode", 1); + pub_heartbeat_ = + this->create_publisher("output/heartbeat", 1); + pub_vehicle_engage_ = + this->create_publisher("output/vehicle_engage", 1); // Service Client client_emergency_stop_ = this->create_client( @@ -468,5 +512,5 @@ AutowareJoyControllerNode::AutowareJoyControllerNode(const rclcpp::NodeOptions & } } // namespace autoware_joy_controller -#include "rclcpp_components/register_node_macro.hpp" +#include RCLCPP_COMPONENTS_REGISTER_NODE(autoware_joy_controller::AutowareJoyControllerNode) From e52a75c5341dacb851a098964c4f873bb3bd796e Mon Sep 17 00:00:00 2001 From: Kenji Miyake <31987104+kenji-miyake@users.noreply.github.com> Date: Thu, 4 Nov 2021 19:32:37 +0900 Subject: [PATCH 34/38] Add COLCON_IGNORE (#500) Signed-off-by: Kenji Miyake --- control/autoware_joy_controller/COLCON_IGNORE | 0 1 file changed, 0 insertions(+), 0 deletions(-) create mode 100644 control/autoware_joy_controller/COLCON_IGNORE diff --git a/control/autoware_joy_controller/COLCON_IGNORE b/control/autoware_joy_controller/COLCON_IGNORE new file mode 100644 index 0000000000000..e69de29bb2d1d From 39d2387784bf37623645e30f5e4558aecdba2888 Mon Sep 17 00:00:00 2001 From: Takayuki Murooka Date: Thu, 11 Nov 2021 15:28:32 +0900 Subject: [PATCH 35/38] port autoware joy controller (#588) * port autoware joy controller * fix compile error * use odometry instead of twist * update launch Co-authored-by: Takayuki Murooka --- control/autoware_joy_controller/COLCON_IGNORE | 0 .../autoware_joy_controller.hpp | 16 +++--- .../launch/autoware_joy_controller.launch.xml | 4 +- control/autoware_joy_controller/package.xml | 4 +- .../autoware_joy_controller_node.cpp | 56 +++++++++++-------- 5 files changed, 45 insertions(+), 35 deletions(-) delete mode 100644 control/autoware_joy_controller/COLCON_IGNORE diff --git a/control/autoware_joy_controller/COLCON_IGNORE b/control/autoware_joy_controller/COLCON_IGNORE deleted file mode 100644 index e69de29bb2d1d..0000000000000 diff --git a/control/autoware_joy_controller/include/autoware_joy_controller/autoware_joy_controller.hpp b/control/autoware_joy_controller/include/autoware_joy_controller/autoware_joy_controller.hpp index c3836e04799f2..23ee25f4127ff 100644 --- a/control/autoware_joy_controller/include/autoware_joy_controller/autoware_joy_controller.hpp +++ b/control/autoware_joy_controller/include/autoware_joy_controller/autoware_joy_controller.hpp @@ -19,7 +19,8 @@ #include -#include +#include +#include #include #include #include @@ -27,9 +28,8 @@ #include #include #include -#include -#include #include +#include #include #include @@ -70,17 +70,17 @@ class AutowareJoyControllerNode : public rclcpp::Node // Subscriber rclcpp::Subscription::SharedPtr sub_joy_; - rclcpp::Subscription::SharedPtr sub_twist_; + rclcpp::Subscription::SharedPtr sub_odom_; rclcpp::Time last_joy_received_time_; std::shared_ptr joy_; geometry_msgs::msg::TwistStamped::ConstSharedPtr twist_; void onJoy(const sensor_msgs::msg::Joy::ConstSharedPtr msg); - void onTwist(const geometry_msgs::msg::TwistStamped::ConstSharedPtr msg); + void onOdometry(const nav_msgs::msg::Odometry::ConstSharedPtr msg); // Publisher - rclcpp::Publisher::SharedPtr + rclcpp::Publisher::SharedPtr pub_control_command_; rclcpp::Publisher::SharedPtr pub_external_control_command_; @@ -88,7 +88,7 @@ class AutowareJoyControllerNode : public rclcpp::Node rclcpp::Publisher::SharedPtr pub_turn_signal_; rclcpp::Publisher::SharedPtr pub_heartbeat_; rclcpp::Publisher::SharedPtr pub_gate_mode_; - rclcpp::Publisher::SharedPtr pub_vehicle_engage_; + rclcpp::Publisher::SharedPtr pub_vehicle_engage_; void publishControlCommand(); void publishExternalControlCommand(); @@ -105,7 +105,7 @@ class AutowareJoyControllerNode : public rclcpp::Node rclcpp::Client::SharedPtr client_autoware_engage_; // Previous State - autoware_control_msgs::msg::ControlCommand prev_control_command_; + 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; diff --git a/control/autoware_joy_controller/launch/autoware_joy_controller.launch.xml b/control/autoware_joy_controller/launch/autoware_joy_controller.launch.xml index c7e66ce545df2..62dc933a3dfbf 100644 --- a/control/autoware_joy_controller/launch/autoware_joy_controller.launch.xml +++ b/control/autoware_joy_controller/launch/autoware_joy_controller.launch.xml @@ -3,7 +3,7 @@ - + @@ -17,7 +17,7 @@ - + diff --git a/control/autoware_joy_controller/package.xml b/control/autoware_joy_controller/package.xml index c5aae048e4356..566ee4477afea 100644 --- a/control/autoware_joy_controller/package.xml +++ b/control/autoware_joy_controller/package.xml @@ -9,15 +9,17 @@ ament_cmake_auto autoware_api_utils + autoware_auto_control_msgs + autoware_auto_vehicle_msgs autoware_control_msgs autoware_external_api_msgs - autoware_vehicle_msgs geometry_msgs joy rclcpp rclcpp_components sensor_msgs std_srvs +nav_msgs ament_lint_auto autoware_lint_common diff --git a/control/autoware_joy_controller/src/autoware_joy_controller/autoware_joy_controller_node.cpp b/control/autoware_joy_controller/src/autoware_joy_controller/autoware_joy_controller_node.cpp index df0951dde46a1..feca8f12c5dea 100644 --- a/control/autoware_joy_controller/src/autoware_joy_controller/autoware_joy_controller_node.cpp +++ b/control/autoware_joy_controller/src/autoware_joy_controller/autoware_joy_controller_node.cpp @@ -184,9 +184,13 @@ void AutowareJoyControllerNode::onJoy(const sensor_msgs::msg::Joy::ConstSharedPt } } -void AutowareJoyControllerNode::onTwist(const geometry_msgs::msg::TwistStamped::ConstSharedPtr msg) +void AutowareJoyControllerNode::onOdometry(const nav_msgs::msg::Odometry::ConstSharedPtr msg) { - twist_ = msg; + auto twist = std::make_shared(); + twist->header = msg->header; + twist->twist = msg->twist.twist; + + twist_ = twist; } bool AutowareJoyControllerNode::isDataReady() @@ -244,35 +248,37 @@ void AutowareJoyControllerNode::onTimer() void AutowareJoyControllerNode::publishControlCommand() { - autoware_control_msgs::msg::ControlCommandStamped cmd_stamped; - cmd_stamped.header.stamp = this->now(); + autoware_auto_control_msgs::msg::AckermannControlCommand cmd; + cmd.stamp = this->now(); { - auto & cmd = cmd_stamped.control; - - cmd.steering_angle = steer_ratio_ * joy_->steer(); - cmd.steering_angle_velocity = steering_angle_velocity_; + cmd.lateral.steering_tire_angle = steer_ratio_ * joy_->steer(); + cmd.lateral.steering_tire_rotation_rate = steering_angle_velocity_; if (joy_->accel()) { - cmd.acceleration = accel_ratio_ * joy_->accel(); - cmd.velocity = twist_->twist.linear.x + velocity_gain_ * cmd.acceleration; - cmd.velocity = std::min(cmd.velocity, max_forward_velocity_); + cmd.longitudinal.acceleration = accel_ratio_ * joy_->accel(); + cmd.longitudinal.speed = + twist_->twist.linear.x + velocity_gain_ * cmd.longitudinal.acceleration; + cmd.longitudinal.speed = + std::min(cmd.longitudinal.speed, static_cast(max_forward_velocity_)); } if (joy_->brake()) { - cmd.velocity = 0.0; - cmd.acceleration = -brake_ratio_ * joy_->brake(); + cmd.longitudinal.speed = 0.0; + cmd.longitudinal.acceleration = -brake_ratio_ * joy_->brake(); } // Backward if (joy_->accel() && joy_->brake()) { - cmd.acceleration = backward_accel_ratio_ * joy_->accel(); - cmd.velocity = twist_->twist.linear.x - velocity_gain_ * cmd.acceleration; - cmd.velocity = std::max(cmd.velocity, -max_backward_velocity_); + cmd.longitudinal.acceleration = backward_accel_ratio_ * joy_->accel(); + cmd.longitudinal.speed = + twist_->twist.linear.x - velocity_gain_ * cmd.longitudinal.acceleration; + cmd.longitudinal.speed = + std::max(cmd.longitudinal.speed, static_cast(-max_backward_velocity_)); } } - pub_control_command_->publish(cmd_stamped); - prev_control_command_ = cmd_stamped.control; + pub_control_command_->publish(cmd); + prev_control_command_ = cmd; } void AutowareJoyControllerNode::publishExternalControlCommand() @@ -414,7 +420,7 @@ void AutowareJoyControllerNode::publishAutowareEngage() void AutowareJoyControllerNode::publishVehicleEngage() { - autoware_vehicle_msgs::msg::Engage engage; + autoware_auto_vehicle_msgs::msg::Engage engage; if (joy_->vehicle_engage()) { engage.engage = true; @@ -471,13 +477,15 @@ AutowareJoyControllerNode::AutowareJoyControllerNode(const rclcpp::NodeOptions & sub_joy_ = this->create_subscription( "input/joy", 1, std::bind(&AutowareJoyControllerNode::onJoy, this, std::placeholders::_1), subscriber_option); - sub_twist_ = this->create_subscription( - "input/twist", 1, std::bind(&AutowareJoyControllerNode::onTwist, this, std::placeholders::_1), + sub_odom_ = this->create_subscription( + "input/odometry", 1, + std::bind(&AutowareJoyControllerNode::onOdometry, this, std::placeholders::_1), subscriber_option); // Publisher - pub_control_command_ = this->create_publisher( - "output/control_command", 1); + pub_control_command_ = + this->create_publisher( + "output/control_command", 1); pub_external_control_command_ = this->create_publisher( "output/external_control_command", 1); @@ -490,7 +498,7 @@ AutowareJoyControllerNode::AutowareJoyControllerNode(const rclcpp::NodeOptions & pub_heartbeat_ = this->create_publisher("output/heartbeat", 1); pub_vehicle_engage_ = - this->create_publisher("output/vehicle_engage", 1); + this->create_publisher("output/vehicle_engage", 1); // Service Client client_emergency_stop_ = this->create_client( From 824c008ec0074a3f0f2379f89cf3a5be4c0ef888 Mon Sep 17 00:00:00 2001 From: Takayuki Murooka Date: Thu, 11 Nov 2021 18:05:48 +0900 Subject: [PATCH 36/38] update README.md in autoware_joy_controller (#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 Co-authored-by: taikitanaka3 <65527974+taikitanaka3@users.noreply.github.com> --- control/autoware_joy_controller/README.md | 43 +++++++++++++++++++++++ 1 file changed, 43 insertions(+) create mode 100644 control/autoware_joy_controller/README.md diff --git a/control/autoware_joy_controller/README.md b/control/autoware_joy_controller/README.md new file mode 100644 index 0000000000000..3ff48c3cc3f82 --- /dev/null +++ b/control/autoware_joy_controller/README.md @@ -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) | From c5669099c64211cdb77ade1871c481d705445d5c Mon Sep 17 00:00:00 2001 From: tanaka3 Date: Tue, 30 Nov 2021 19:40:34 +0900 Subject: [PATCH 37/38] fix format --- control/autoware_joy_controller/package.xml | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/control/autoware_joy_controller/package.xml b/control/autoware_joy_controller/package.xml index 566ee4477afea..3279995c4d7bf 100644 --- a/control/autoware_joy_controller/package.xml +++ b/control/autoware_joy_controller/package.xml @@ -19,7 +19,7 @@ rclcpp_components sensor_msgs std_srvs -nav_msgs + nav_msgs ament_lint_auto autoware_lint_common From f1e2eb87f5dc42cbe78ade85ab5d30182971bb66 Mon Sep 17 00:00:00 2001 From: "pre-commit-ci[bot]" <66853113+pre-commit-ci[bot]@users.noreply.github.com> Date: Wed, 1 Dec 2021 08:02:36 +0000 Subject: [PATCH 38/38] ci(pre-commit): autofix --- control/autoware_joy_controller/package.xml | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/control/autoware_joy_controller/package.xml b/control/autoware_joy_controller/package.xml index 3279995c4d7bf..1839dbab645fa 100644 --- a/control/autoware_joy_controller/package.xml +++ b/control/autoware_joy_controller/package.xml @@ -15,11 +15,11 @@ autoware_external_api_msgs geometry_msgs joy + nav_msgs rclcpp rclcpp_components sensor_msgs std_srvs - nav_msgs ament_lint_auto autoware_lint_common