feat: add shift decider (autowarefoundation#29)

* release v0.4.0 * remove ROS1 packages temporarily Signed-off-by: mitsudome-r <ryohsuke.mitsudome@tier4.jp> * add sample ros2 packages Signed-off-by: mitsudome-r <ryohsuke.mitsudome@tier4.jp> * remove ROS1 packages Signed-off-by: mitsudome-r <ryohsuke.mitsudome@tier4.jp> * Revert "remove ROS1 packages temporarily" This reverts commit bcc60bdc82842e2ea4d1e844b4442fa613d80fdf. Signed-off-by: mitsudome-r <ryohsuke.mitsudome@tier4.jp> * add COLCON_IGNORE to ros1 packages Signed-off-by: mitsudome-r <ryohsuke.mitsudome@tier4.jp> * 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 (autowarefoundation#142) * Change includes * Rename files * Adjustments to make things compile * Other packages * Adjust copyright notice on 532 out of 699 source files (autowarefoundation#143) * Use quotes for includes where appropriate (autowarefoundation#144) * Use quotes for includes where appropriate * Fix lint tests * Make tests pass hopefully * Run uncrustify on the entire Pilot.Auto codebase (autowarefoundation#151) * Run uncrustify on the entire Pilot.Auto codebase * Exclude open PRs * Added linters to shift_decider (autowarefoundation#167) * Added linters to shift_decider * Fix dependencies * Use ament_cmake_cppcheck and ament_cmake_cpplint * Fix linting errors * add use_sim-time option (autowarefoundation#454) * Unify Apache-2.0 license name (autowarefoundation#1242) * Make control modules components (autowarefoundation#1262) Signed-off-by: wep21 <border_goldenmarket@yahoo.co.jp> * Remove use_sim_time for set_parameter (autowarefoundation#1260) Signed-off-by: wep21 <border_goldenmarket@yahoo.co.jp> * add sort-package-xml hook in pre-commit (autowarefoundation#1881) * add sort xml hook in pre-commit * change retval to exit_status * rename * add prettier plugin-xml * use early return * add license note * add tier4 license * restore prettier * change license order * move local hooks to public repo * move prettier-xml to pre-commit-hooks-ros * update version for bug-fix * apply pre-commit * Change formatter to clang-format and black (autowarefoundation#2332) * Revert "Temporarily comment out pre-commit hooks" This reverts commit 748e9cdb145ce12f8b520bcbd97f5ff899fc28a3. * Replace ament_lint_common with autoware_lint_common Signed-off-by: Kenji Miyake <kenji.miyake@tier4.jp> * Remove ament_cmake_uncrustify and ament_clang_format Signed-off-by: Kenji Miyake <kenji.miyake@tier4.jp> * Apply Black Signed-off-by: Kenji Miyake <kenji.miyake@tier4.jp> * Apply clang-format Signed-off-by: Kenji Miyake <kenji.miyake@tier4.jp> * Fix build errors Signed-off-by: Kenji Miyake <kenji.miyake@tier4.jp> * Fix for cpplint * Fix include double quotes to angle brackets Signed-off-by: Kenji Miyake <kenji.miyake@tier4.jp> * Apply clang-format Signed-off-by: Kenji Miyake <kenji.miyake@tier4.jp> * Fix build errors Signed-off-by: Kenji Miyake <kenji.miyake@tier4.jp> * Add COLCON_IGNORE (autowarefoundation#500) Signed-off-by: Kenji Miyake <kenji.miyake@tier4.jp> * port shift decider (autowarefoundation#485) * port shift decider * apply clang format * delete colcon ignore * change to ackermann control command * remove VehicleStateReport/VehicleStateCommand/VehicleControlCommand (autowarefoundation#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 (autowarefoundation#577) * add readme to shift decider * better expression Co-authored-by: tkimura4 <tomoya.kimura@tier4.jp> Co-authored-by: taikitanaka3 <taiki.tanaka@tier4.jp> Co-authored-by: tkimura4 <tomoya.kimura@tier4.jp> * Fix topic name (autowarefoundation#673) Co-authored-by: mitsudome-r <ryohsuke.mitsudome@tier4.jp> Co-authored-by: Frederik Beaujean <72439809+fred-apex-ai@users.noreply.github.com> Co-authored-by: Nikolai Morin <nnmmgit@gmail.com> Co-authored-by: Esteve Fernandez <esteve@apache.org> Co-authored-by: tkimura4 <tomoya.kimura@tier4.jp> Co-authored-by: Kazuki Miyahara <kmiya@outlook.com> 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 <taiki.tanaka@tier4.jp> Co-authored-by: Fumiya Watanabe <rej55.g@gmail.com>
badai-nguyen · Dec 1, 2021 · d6c2d47 · d6c2d47
1 parent 4a0d224
commit d6c2d47
Show file tree

Hide file tree

Showing 6 changed files with 244 additions and 0 deletions.
diff --git a/control/shift_decider/CMakeLists.txt 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
@@ -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
@@ -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 <rclcpp/rclcpp.hpp>
+
+#include <autoware_auto_control_msgs/msg/ackermann_control_command.hpp>
+#include <autoware_auto_vehicle_msgs/msg/gear_command.hpp>
+
+#include <memory>
+
+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<autoware_auto_vehicle_msgs::msg::GearCommand>::SharedPtr pub_shift_cmd_;
+  rclcpp::Subscription<autoware_auto_control_msgs::msg::AckermannControlCommand>::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
@@ -0,0 +1,6 @@
+<launch>
+  <node pkg="shift_decider" exec="shift_decider" name="shift_decider" output="screen">
+    <remap from="input/control_cmd" to="/control/trajectory_follower/control_cmd" />
+    <remap from="output/gear_cmd" to="/control/shift_decider/gear_cmd" />
+  </node>
+</launch>
diff --git a/control/shift_decider/package.xml b/control/shift_decider/package.xml
@@ -0,0 +1,25 @@
+<?xml version="1.0"?>
+<package format="2">
+  <name>shift_decider</name>
+  <version>0.1.0</version>
+  <description>The shift_decider package</description>
+  <maintainer email="horibe.takamasa@gmail.com">Takamasa Horibe</maintainer>
+  <author email="horibe.takamasa@gmail.com">Takamasa Horibe</author>
+  <license>Apache License 2.0</license>
+
+  <buildtool_depend>ament_cmake</buildtool_depend>
+
+  <depend>autoware_auto_control_msgs</depend>
+  <depend>autoware_auto_vehicle_msgs</depend>
+  <depend>rclcpp</depend>
+  <depend>rclcpp_components</depend>
+
+  <test_depend>ament_cmake_cppcheck</test_depend>
+  <test_depend>ament_cmake_cpplint</test_depend>
+  <test_depend>ament_lint_auto</test_depend>
+
+  <export>
+    <build_type>ament_cmake</build_type>
+  </export>
+
+</package>
diff --git a/control/shift_decider/src/shift_decider.cpp 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 <rclcpp/timer.hpp>
+
+#include <cstddef>
+#include <functional>
+#include <memory>
+#include <utility>
+
+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<autoware_auto_vehicle_msgs::msg::GearCommand>("output/gear_cmd", durable_qos);
+  sub_control_cmd_ = create_subscription<autoware_auto_control_msgs::msg::AckermannControlCommand>(
+    "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::nanoseconds>(std::chrono::duration<double>(period_s));
+  timer_ = std::make_shared<rclcpp::GenericTimer<decltype(timer_callback)>>(
+    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_macro.hpp>
+RCLCPP_COMPONENTS_REGISTER_NODE(ShiftDecider)