feat: add shift decider

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
+)

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.

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_

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>

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>

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)