From 2ab1adef2ee0bc0cceb8a2d30cbd9f5f92339ee4 Mon Sep 17 00:00:00 2001 From: taikitanaka3 <65527974+taikitanaka3@users.noreply.github.com> Date: Wed, 1 Dec 2021 16:51:28 +0900 Subject: [PATCH] feat: add shift decider (#29) * release v0.4.0 * remove ROS1 packages temporarily Signed-off-by: mitsudome-r * add sample ros2 packages Signed-off-by: mitsudome-r * remove ROS1 packages Signed-off-by: mitsudome-r * Revert "remove ROS1 packages temporarily" This reverts commit bcc60bdc82842e2ea4d1e844b4442fa613d80fdf. Signed-off-by: mitsudome-r * add COLCON_IGNORE to ros1 packages Signed-off-by: mitsudome-r * Port shift decider to ros2 (#7) * Update package.xml and CMakeLists.txt to ros2 Code doesn't compile yet * Code compiles * Update launch file * Add better timer clang-format shift_decider * Rename h files to hpp (#142) * Change includes * Rename files * Adjustments to make things compile * Other packages * Adjust copyright notice on 532 out of 699 source files (#143) * Use quotes for includes where appropriate (#144) * Use quotes for includes where appropriate * Fix lint tests * Make tests pass hopefully * Run uncrustify on the entire Pilot.Auto codebase (#151) * Run uncrustify on the entire Pilot.Auto codebase * Exclude open PRs * Added linters to shift_decider (#167) * Added linters to shift_decider * Fix dependencies * Use ament_cmake_cppcheck and ament_cmake_cpplint * Fix linting errors * add use_sim-time option (#454) * Unify Apache-2.0 license name (#1242) * Make control modules components (#1262) Signed-off-by: wep21 * Remove use_sim_time for set_parameter (#1260) Signed-off-by: wep21 * 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 * 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 * Add COLCON_IGNORE (#500) Signed-off-by: Kenji Miyake * port shift decider (#485) * port shift decider * apply clang format * delete colcon ignore * change to ackermann control command * remove VehicleStateReport/VehicleStateCommand/VehicleControlCommand (#549) * fix autoware_error_monitor * fix state monitor * fix emergency handler(vehicle_state_report) * fix emergency Handler(vehicle_state_command) * fix shift_decider * fix emergency_handler(vehicle_control_command) * fix topic name * fix readme * Update system/autoware_state_monitor/Readme.md Co-authored-by: taikitanaka3 <65527974+taikitanaka3@users.noreply.github.com> * fix format * Update system/autoware_state_monitor/launch/autoware_state_monitor.launch.xml Co-authored-by: taikitanaka3 <65527974+taikitanaka3@users.noreply.github.com> * fix typo Co-authored-by: taikitanaka3 <65527974+taikitanaka3@users.noreply.github.com> * [shift decider] add README.md (#577) * add readme to shift decider * better expression Co-authored-by: tkimura4 Co-authored-by: taikitanaka3 Co-authored-by: tkimura4 * Fix topic name (#673) Co-authored-by: mitsudome-r Co-authored-by: Frederik Beaujean <72439809+fred-apex-ai@users.noreply.github.com> Co-authored-by: Nikolai Morin Co-authored-by: Esteve Fernandez Co-authored-by: tkimura4 Co-authored-by: Kazuki Miyahara Co-authored-by: Daisuke Nishimatsu <42202095+wep21@users.noreply.github.com> Co-authored-by: Keisuke Shima <19993104+KeisukeShima@users.noreply.github.com> Co-authored-by: Kenji Miyake <31987104+kenji-miyake@users.noreply.github.com> Co-authored-by: Sugatyon <32741405+Sugatyon@users.noreply.github.com> Co-authored-by: taikitanaka3 Co-authored-by: Fumiya Watanabe --- control/shift_decider/CMakeLists.txt | 30 +++++++ control/shift_decider/README.md | 56 +++++++++++++ .../include/shift_decider/shift_decider.hpp | 45 ++++++++++ .../launch/shift_decider.launch.xml | 6 ++ control/shift_decider/package.xml | 25 ++++++ control/shift_decider/src/shift_decider.cpp | 82 +++++++++++++++++++ 6 files changed, 244 insertions(+) create mode 100644 control/shift_decider/CMakeLists.txt create mode 100644 control/shift_decider/README.md create mode 100644 control/shift_decider/include/shift_decider/shift_decider.hpp create mode 100644 control/shift_decider/launch/shift_decider.launch.xml create mode 100644 control/shift_decider/package.xml create mode 100644 control/shift_decider/src/shift_decider.cpp diff --git a/control/shift_decider/CMakeLists.txt b/control/shift_decider/CMakeLists.txt new file mode 100644 index 0000000000000..6e2e94098646c --- /dev/null +++ b/control/shift_decider/CMakeLists.txt @@ -0,0 +1,30 @@ +cmake_minimum_required(VERSION 3.5) +project(shift_decider) + +find_package(ament_cmake_auto REQUIRED) +ament_auto_find_build_dependencies() + +if(NOT CMAKE_CXX_STANDARD) + set(CMAKE_CXX_STANDARD 14) + set(CMAKE_CXX_STANDARD_REQUIRED ON) + set(CMAKE_CXX_EXTENSIONS OFF) +endif() + +ament_auto_add_library(shift_decider_node SHARED + src/shift_decider.cpp +) + +rclcpp_components_register_node(shift_decider_node + PLUGIN "ShiftDecider" + EXECUTABLE shift_decider +) + +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/shift_decider/README.md b/control/shift_decider/README.md new file mode 100644 index 0000000000000..893b09adfc676 --- /dev/null +++ b/control/shift_decider/README.md @@ -0,0 +1,56 @@ +# Shift Decider + +## Purpose + +`shift_decider` is a module to decide shift from ackermann control command. + +## Inner-workings / Algorithms + +### Flow chart + +```plantuml +@startuml +skinparam monochrome true + +title update current shift +start +if (absolute target velocity is less than threshold) then (yes) + :set previous shift; +else(no) +if (target velocity is positive) then (yes) + :set shift DRIVE; +else + :set shift REVERSE; +endif +endif + :publish current shift; +note right + publish shift for constant interval +end note +stop +@enduml +``` + +### Algorithms + +## Inputs / Outputs + +### Input + +| Name | Type | Description | +| --------------------- | ---------------------------------------------------------- | ---------------------------- | +| `~/input/control_cmd` | `autoware_auto_control_msgs::msg::AckermannControlCommand` | Control command for vehicle. | + +### Output + +| Name | Type | Description | +| ------------------- | ---------------------------------------------- | ---------------------------------- | +| `~output/shift_cmd` | `autoware_auto_vehicle_msgs::msg::GearCommand` | Gear for drive forward / backward. | + +## Parameters + +none. + +## Assumptions / Known limits + +TBD. diff --git a/control/shift_decider/include/shift_decider/shift_decider.hpp b/control/shift_decider/include/shift_decider/shift_decider.hpp new file mode 100644 index 0000000000000..a9d50d474c55a --- /dev/null +++ b/control/shift_decider/include/shift_decider/shift_decider.hpp @@ -0,0 +1,45 @@ +// Copyright 2020 Tier IV, Inc. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#ifndef SHIFT_DECIDER__SHIFT_DECIDER_HPP_ +#define SHIFT_DECIDER__SHIFT_DECIDER_HPP_ + +#include + +#include +#include + +#include + +class ShiftDecider : public rclcpp::Node +{ +public: + explicit ShiftDecider(const rclcpp::NodeOptions & node_options); + +private: + void onTimer(); + void onControlCmd(autoware_auto_control_msgs::msg::AckermannControlCommand::SharedPtr msg); + void updateCurrentShiftCmd(); + void initTimer(double period_s); + + rclcpp::Publisher::SharedPtr pub_shift_cmd_; + rclcpp::Subscription::SharedPtr + sub_control_cmd_; + rclcpp::TimerBase::SharedPtr timer_; + + autoware_auto_control_msgs::msg::AckermannControlCommand::SharedPtr control_cmd_; + autoware_auto_vehicle_msgs::msg::GearCommand shift_cmd_; +}; + +#endif // SHIFT_DECIDER__SHIFT_DECIDER_HPP_ diff --git a/control/shift_decider/launch/shift_decider.launch.xml b/control/shift_decider/launch/shift_decider.launch.xml new file mode 100644 index 0000000000000..083eaef413ff4 --- /dev/null +++ b/control/shift_decider/launch/shift_decider.launch.xml @@ -0,0 +1,6 @@ + + + + + + diff --git a/control/shift_decider/package.xml b/control/shift_decider/package.xml new file mode 100644 index 0000000000000..a6640725a1597 --- /dev/null +++ b/control/shift_decider/package.xml @@ -0,0 +1,25 @@ + + + shift_decider + 0.1.0 + The shift_decider package + Takamasa Horibe + Takamasa Horibe + Apache License 2.0 + + ament_cmake + + autoware_auto_control_msgs + autoware_auto_vehicle_msgs + rclcpp + rclcpp_components + + ament_cmake_cppcheck + ament_cmake_cpplint + ament_lint_auto + + + ament_cmake + + + diff --git a/control/shift_decider/src/shift_decider.cpp b/control/shift_decider/src/shift_decider.cpp new file mode 100644 index 0000000000000..dc7fe6e8fed61 --- /dev/null +++ b/control/shift_decider/src/shift_decider.cpp @@ -0,0 +1,82 @@ +// 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 "shift_decider/shift_decider.hpp" + +#include + +#include +#include +#include +#include + +ShiftDecider::ShiftDecider(const rclcpp::NodeOptions & node_options) +: Node("shift_decider", node_options) +{ + using std::placeholders::_1; + + static constexpr std::size_t queue_size = 1; + rclcpp::QoS durable_qos(queue_size); + durable_qos.transient_local(); + + pub_shift_cmd_ = + create_publisher("output/gear_cmd", durable_qos); + sub_control_cmd_ = create_subscription( + "input/control_cmd", queue_size, std::bind(&ShiftDecider::onControlCmd, this, _1)); + + initTimer(0.1); +} + +void ShiftDecider::onControlCmd( + autoware_auto_control_msgs::msg::AckermannControlCommand::SharedPtr msg) +{ + control_cmd_ = msg; +} + +void ShiftDecider::onTimer() +{ + if (!control_cmd_) { + return; + } + + updateCurrentShiftCmd(); + pub_shift_cmd_->publish(shift_cmd_); +} + +void ShiftDecider::updateCurrentShiftCmd() +{ + using autoware_auto_vehicle_msgs::msg::GearCommand; + + shift_cmd_.stamp = now(); + static constexpr double vel_threshold = 0.01; // to prevent chattering + if (control_cmd_->longitudinal.speed > vel_threshold) { + shift_cmd_.command = GearCommand::DRIVE; + } else if (control_cmd_->longitudinal.speed < -vel_threshold) { + shift_cmd_.command = GearCommand::REVERSE; + } +} + +void ShiftDecider::initTimer(double period_s) +{ + auto timer_callback = std::bind(&ShiftDecider::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); +} + +#include +RCLCPP_COMPONENTS_REGISTER_NODE(ShiftDecider)