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)