From f79cfe505a9a84635fed6bbf1ca91d1a7ffdd3a7 Mon Sep 17 00:00:00 2001 From: Takamasa Horibe Date: Thu, 30 Jun 2022 18:09:44 +0900 Subject: [PATCH 1/4] add simple trajectory follower Signed-off-by: Takamasa Horibe --- .../trajectory_follower_nodes/CMakeLists.txt | 15 +++ .../simple_trajectory_follower.hpp | 66 ++++++++++ .../simple_trajectory_follower.launch.xml | 16 +++ .../src/simple_trajectory_follower.cpp | 113 ++++++++++++++++++ 4 files changed, 210 insertions(+) create mode 100644 control/trajectory_follower_nodes/include/trajectory_follower_nodes/simple_trajectory_follower.hpp create mode 100644 control/trajectory_follower_nodes/launch/simple_trajectory_follower.launch.xml create mode 100644 control/trajectory_follower_nodes/src/simple_trajectory_follower.cpp diff --git a/control/trajectory_follower_nodes/CMakeLists.txt b/control/trajectory_follower_nodes/CMakeLists.txt index fa005c1c0bfda..b56ad707ca32b 100644 --- a/control/trajectory_follower_nodes/CMakeLists.txt +++ b/control/trajectory_follower_nodes/CMakeLists.txt @@ -46,6 +46,20 @@ rclcpp_components_register_node(${LATLON_MUXER_NODE} EXECUTABLE ${LATLON_MUXER_NODE}_exe ) +# simple trajectory follower +set(SIMPLE_TRAJECTORY_FOLLOWER_NODE simple_trajectory_follower) +ament_auto_add_library(${SIMPLE_TRAJECTORY_FOLLOWER_NODE} SHARED + include/trajectory_follower_nodes/simple_trajectory_follower.hpp + src/simple_trajectory_follower.cpp +) + +autoware_set_compile_options(${SIMPLE_TRAJECTORY_FOLLOWER_NODE}) +target_compile_options(${SIMPLE_TRAJECTORY_FOLLOWER_NODE} PRIVATE -Wno-error=old-style-cast) +rclcpp_components_register_node(${SIMPLE_TRAJECTORY_FOLLOWER_NODE} + PLUGIN "simple_trajectory_follower::SimpleTrajectoryFollower" + EXECUTABLE ${SIMPLE_TRAJECTORY_FOLLOWER_NODE}_exe +) + if(BUILD_TESTING) set(TRAJECTORY_FOLLOWER_NODES_TEST test_trajectory_follower_nodes) ament_add_ros_isolated_gtest(${TRAJECTORY_FOLLOWER_NODES_TEST} @@ -72,4 +86,5 @@ endif() ament_auto_package( INSTALL_TO_SHARE param + launch ) diff --git a/control/trajectory_follower_nodes/include/trajectory_follower_nodes/simple_trajectory_follower.hpp b/control/trajectory_follower_nodes/include/trajectory_follower_nodes/simple_trajectory_follower.hpp new file mode 100644 index 0000000000000..27137d1890470 --- /dev/null +++ b/control/trajectory_follower_nodes/include/trajectory_follower_nodes/simple_trajectory_follower.hpp @@ -0,0 +1,66 @@ +// Copyright 2022 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. + +#ifndef TRAJECTORY_FOLLOWER_NODES__SIMPLE_TRAJECTORY_FOLLOWER_HPP +#define TRAJECTORY_FOLLOWER_NODES__SIMPLE_TRAJECTORY_FOLLOWER_HPP + +#include + +#include +#include +#include +#include +#include +#include + +#include + +namespace simple_trajectory_follower +{ +using autoware_auto_control_msgs::msg::AckermannControlCommand; +using autoware_auto_planning_msgs::msg::Trajectory; +using autoware_auto_planning_msgs::msg::TrajectoryPoint; +using nav_msgs::msg::Odometry; +using geometry_msgs::msg::Pose; +using geometry_msgs::msg::Twist; + +class SimpleTrajectoryFollower : public rclcpp::Node +{ +public: + SimpleTrajectoryFollower(const rclcpp::NodeOptions & options); + ~SimpleTrajectoryFollower() = default; + +private: + rclcpp::Subscription::SharedPtr sub_kinematics_; + rclcpp::Subscription::SharedPtr sub_trajectory_; + rclcpp::Publisher::SharedPtr pub_cmd_; + rclcpp::TimerBase::SharedPtr timer_; + + Trajectory::SharedPtr trajectory_; + Odometry::SharedPtr odometry_; + TrajectoryPoint closest_traj_point_; + bool use_external_target_vel_; + double external_target_vel_; + double lateral_deviation_; + + void onTimer(); + bool checkData(); + void updateClosest(); + double calcSteerCmd(); + double calcAccCmd(); +}; + +} // namespace simple_trajectory_follower + +#endif // TRAJECTORY_FOLLOWER_NODES__SIMPLE_TRAJECTORY_FOLLOWER_HPP diff --git a/control/trajectory_follower_nodes/launch/simple_trajectory_follower.launch.xml b/control/trajectory_follower_nodes/launch/simple_trajectory_follower.launch.xml new file mode 100644 index 0000000000000..041ac91c42830 --- /dev/null +++ b/control/trajectory_follower_nodes/launch/simple_trajectory_follower.launch.xml @@ -0,0 +1,16 @@ + + + + + + + + + + + + + + + + diff --git a/control/trajectory_follower_nodes/src/simple_trajectory_follower.cpp b/control/trajectory_follower_nodes/src/simple_trajectory_follower.cpp new file mode 100644 index 0000000000000..590d0c7098d27 --- /dev/null +++ b/control/trajectory_follower_nodes/src/simple_trajectory_follower.cpp @@ -0,0 +1,113 @@ +// Copyright 2022 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 "trajectory_follower_nodes/simple_trajectory_follower.hpp" + +#include + +#include + +namespace simple_trajectory_follower +{ + +using namespace tier4_autoware_utils; + +SimpleTrajectoryFollower::SimpleTrajectoryFollower(const rclcpp::NodeOptions & options) +: Node("simple_trajectory_follower", options) +{ + pub_cmd_ = create_publisher("output/control_cmd", 1); + + sub_kinematics_ = create_subscription( + "input/kinematics", 1, [this](const Odometry::SharedPtr msg) { odometry_ = msg; }); + sub_trajectory_ = create_subscription( + "input/trajectory", 1, [this](const Trajectory::SharedPtr msg) { trajectory_ = msg; }); + + use_external_target_vel_ = declare_parameter("use_external_target_vel", false); + external_target_vel_ = declare_parameter("external_target_vel", 0.0); + lateral_deviation_ = declare_parameter("lateral_deviation", 0.0); + + using namespace std::literals::chrono_literals; + timer_ = rclcpp::create_timer( + this, get_clock(), 30ms, std::bind(&SimpleTrajectoryFollower::onTimer, this)); +} + +void SimpleTrajectoryFollower::onTimer() +{ + if (!checkData()) { + RCLCPP_INFO(get_logger(), "data not ready"); + return; + } + + updateClosest(); + + AckermannControlCommand cmd; + cmd.stamp = cmd.lateral.stamp = cmd.longitudinal.stamp = get_clock()->now(); + cmd.lateral.steering_tire_angle = static_cast(calcSteerCmd()); + cmd.longitudinal.speed = use_external_target_vel_ ? static_cast(external_target_vel_) + : closest_traj_point_.longitudinal_velocity_mps; + cmd.longitudinal.acceleration = static_cast(calcAccCmd()); + pub_cmd_->publish(cmd); +} + +void SimpleTrajectoryFollower::updateClosest() +{ + const auto closest = findNearestIndex(trajectory_->points, odometry_->pose.pose.position); + closest_traj_point_ = trajectory_->points.at(closest); +} + +double SimpleTrajectoryFollower::calcSteerCmd() +{ + const auto lat_err = + calcLateralDeviation(closest_traj_point_.pose, odometry_->pose.pose.position) - + lateral_deviation_; + const auto yaw_err = calcYawDeviation(closest_traj_point_.pose, odometry_->pose.pose); + + // linearized pure_pursuit control + constexpr auto wheel_base = 4.0; + constexpr auto lookahead_time = 3.0; + constexpr auto min_lookahead = 3.0; + const auto lookahead = min_lookahead + lookahead_time * std::abs(odometry_->twist.twist.linear.x); + const auto kp = 2.0 * wheel_base / (lookahead * lookahead); + const auto kd = 2.0 * wheel_base / lookahead; + + constexpr auto steer_lim = 0.6; + + const auto steer = std::clamp(-kp * lat_err - kd * yaw_err, -steer_lim, steer_lim); + // RCLCPP_INFO(get_logger(), "kp = %f, lat_err = %f, kd - %f, yaw_err = %f, steer = %f", kp, + // lat_err, kd, yaw_err, steer); + return steer; +} + +double SimpleTrajectoryFollower::calcAccCmd() +{ + const auto traj_vel = static_cast(closest_traj_point_.longitudinal_velocity_mps); + const auto ego_vel = odometry_->twist.twist.linear.x; + const auto target_vel = use_external_target_vel_ ? external_target_vel_ : traj_vel; + const auto vel_err = ego_vel - target_vel; + + // P feedback + constexpr auto kp = 0.5; + constexpr auto acc_lim = 2.0; + + const auto acc = std::clamp(-kp * vel_err, -acc_lim, acc_lim); + // RCLCPP_INFO(get_logger(), "vel_err = %f, acc = %f", vel_err, acc); + return acc; +} + +bool SimpleTrajectoryFollower::checkData() { return (trajectory_ && odometry_); } + +} // namespace simple_trajectory_follower + +#include +RCLCPP_COMPONENTS_REGISTER_NODE(simple_trajectory_follower::SimpleTrajectoryFollower) From c1e793fe1fc0de15014da46f4bcde350c1fa46dc Mon Sep 17 00:00:00 2001 From: Takamasa Horibe Date: Thu, 30 Jun 2022 18:11:19 +0900 Subject: [PATCH 2/4] fix precommit Signed-off-by: Takamasa Horibe --- .../simple_trajectory_follower.hpp | 42 +++++++++---------- .../src/simple_trajectory_follower.cpp | 10 +++-- 2 files changed, 27 insertions(+), 25 deletions(-) diff --git a/control/trajectory_follower_nodes/include/trajectory_follower_nodes/simple_trajectory_follower.hpp b/control/trajectory_follower_nodes/include/trajectory_follower_nodes/simple_trajectory_follower.hpp index 27137d1890470..a862477f309ab 100644 --- a/control/trajectory_follower_nodes/include/trajectory_follower_nodes/simple_trajectory_follower.hpp +++ b/control/trajectory_follower_nodes/include/trajectory_follower_nodes/simple_trajectory_follower.hpp @@ -12,8 +12,8 @@ // See the License for the specific language governing permissions and // limitations under the License. -#ifndef TRAJECTORY_FOLLOWER_NODES__SIMPLE_TRAJECTORY_FOLLOWER_HPP -#define TRAJECTORY_FOLLOWER_NODES__SIMPLE_TRAJECTORY_FOLLOWER_HPP +#ifndef TRAJECTORY_FOLLOWER_NODES__SIMPLE_TRAJECTORY_FOLLOWER_HPP_ +#define TRAJECTORY_FOLLOWER_NODES__SIMPLE_TRAJECTORY_FOLLOWER_HPP_ #include @@ -31,36 +31,36 @@ namespace simple_trajectory_follower using autoware_auto_control_msgs::msg::AckermannControlCommand; using autoware_auto_planning_msgs::msg::Trajectory; using autoware_auto_planning_msgs::msg::TrajectoryPoint; -using nav_msgs::msg::Odometry; using geometry_msgs::msg::Pose; using geometry_msgs::msg::Twist; +using nav_msgs::msg::Odometry; class SimpleTrajectoryFollower : public rclcpp::Node { public: - SimpleTrajectoryFollower(const rclcpp::NodeOptions & options); - ~SimpleTrajectoryFollower() = default; + explicit SimpleTrajectoryFollower(const rclcpp::NodeOptions & options); + ~SimpleTrajectoryFollower() = default; private: - rclcpp::Subscription::SharedPtr sub_kinematics_; - rclcpp::Subscription::SharedPtr sub_trajectory_; - rclcpp::Publisher::SharedPtr pub_cmd_; - rclcpp::TimerBase::SharedPtr timer_; + rclcpp::Subscription::SharedPtr sub_kinematics_; + rclcpp::Subscription::SharedPtr sub_trajectory_; + rclcpp::Publisher::SharedPtr pub_cmd_; + rclcpp::TimerBase::SharedPtr timer_; - Trajectory::SharedPtr trajectory_; - Odometry::SharedPtr odometry_; - TrajectoryPoint closest_traj_point_; - bool use_external_target_vel_; - double external_target_vel_; - double lateral_deviation_; + Trajectory::SharedPtr trajectory_; + Odometry::SharedPtr odometry_; + TrajectoryPoint closest_traj_point_; + bool use_external_target_vel_; + double external_target_vel_; + double lateral_deviation_; - void onTimer(); - bool checkData(); - void updateClosest(); - double calcSteerCmd(); - double calcAccCmd(); + void onTimer(); + bool checkData(); + void updateClosest(); + double calcSteerCmd(); + double calcAccCmd(); }; } // namespace simple_trajectory_follower -#endif // TRAJECTORY_FOLLOWER_NODES__SIMPLE_TRAJECTORY_FOLLOWER_HPP +#endif // TRAJECTORY_FOLLOWER_NODES__SIMPLE_TRAJECTORY_FOLLOWER_HPP_ diff --git a/control/trajectory_follower_nodes/src/simple_trajectory_follower.cpp b/control/trajectory_follower_nodes/src/simple_trajectory_follower.cpp index 590d0c7098d27..0bf9f22cd7529 100644 --- a/control/trajectory_follower_nodes/src/simple_trajectory_follower.cpp +++ b/control/trajectory_follower_nodes/src/simple_trajectory_follower.cpp @@ -21,7 +21,8 @@ namespace simple_trajectory_follower { -using namespace tier4_autoware_utils; +using tier4_autoware_utils::calcLateralDeviation; +using tier4_autoware_utils::calcYawDeviation; SimpleTrajectoryFollower::SimpleTrajectoryFollower(const rclcpp::NodeOptions & options) : Node("simple_trajectory_follower", options) @@ -84,8 +85,9 @@ double SimpleTrajectoryFollower::calcSteerCmd() constexpr auto steer_lim = 0.6; const auto steer = std::clamp(-kp * lat_err - kd * yaw_err, -steer_lim, steer_lim); - // RCLCPP_INFO(get_logger(), "kp = %f, lat_err = %f, kd - %f, yaw_err = %f, steer = %f", kp, - // lat_err, kd, yaw_err, steer); + RCLCPP_DEBUG( + get_logger(), "kp = %f, lat_err = %f, kd - %f, yaw_err = %f, steer = %f", kp, lat_err, kd, + yaw_err, steer); return steer; } @@ -101,7 +103,7 @@ double SimpleTrajectoryFollower::calcAccCmd() constexpr auto acc_lim = 2.0; const auto acc = std::clamp(-kp * vel_err, -acc_lim, acc_lim); - // RCLCPP_INFO(get_logger(), "vel_err = %f, acc = %f", vel_err, acc); + RCLCPP_DEBUG(get_logger(), "vel_err = %f, acc = %f", vel_err, acc); return acc; } From f173c4b1b62c58158577bfae17a4a937187d42bd Mon Sep 17 00:00:00 2001 From: Takamasa Horibe Date: Fri, 1 Jul 2022 16:28:08 +0900 Subject: [PATCH 3/4] fix build error Signed-off-by: Takamasa Horibe --- control/trajectory_follower_nodes/CMakeLists.txt | 3 ++- .../src/simple_trajectory_follower.cpp | 1 + 2 files changed, 3 insertions(+), 1 deletion(-) diff --git a/control/trajectory_follower_nodes/CMakeLists.txt b/control/trajectory_follower_nodes/CMakeLists.txt index b56ad707ca32b..8d57fb7cc5cfa 100644 --- a/control/trajectory_follower_nodes/CMakeLists.txt +++ b/control/trajectory_follower_nodes/CMakeLists.txt @@ -53,7 +53,8 @@ ament_auto_add_library(${SIMPLE_TRAJECTORY_FOLLOWER_NODE} SHARED src/simple_trajectory_follower.cpp ) -autoware_set_compile_options(${SIMPLE_TRAJECTORY_FOLLOWER_NODE}) +# TODO(simple_trajectory_follower) : RCLCPP_ERROR_THROTTLE() has built-in old-style casts. +# TODO(simple_trajectory_follower) : Temporary workaround until this is fixed. target_compile_options(${SIMPLE_TRAJECTORY_FOLLOWER_NODE} PRIVATE -Wno-error=old-style-cast) rclcpp_components_register_node(${SIMPLE_TRAJECTORY_FOLLOWER_NODE} PLUGIN "simple_trajectory_follower::SimpleTrajectoryFollower" diff --git a/control/trajectory_follower_nodes/src/simple_trajectory_follower.cpp b/control/trajectory_follower_nodes/src/simple_trajectory_follower.cpp index 0bf9f22cd7529..db3c2e246f9ab 100644 --- a/control/trajectory_follower_nodes/src/simple_trajectory_follower.cpp +++ b/control/trajectory_follower_nodes/src/simple_trajectory_follower.cpp @@ -23,6 +23,7 @@ namespace simple_trajectory_follower using tier4_autoware_utils::calcLateralDeviation; using tier4_autoware_utils::calcYawDeviation; +using tier4_autoware_utils::findNearestIndex; SimpleTrajectoryFollower::SimpleTrajectoryFollower(const rclcpp::NodeOptions & options) : Node("simple_trajectory_follower", options) From 59eb76681add9aac2e7697a18298b3139d5e8d77 Mon Sep 17 00:00:00 2001 From: Takamasa Horibe Date: Tue, 5 Jul 2022 17:57:03 +0900 Subject: [PATCH 4/4] add readme, fix launch remap Signed-off-by: Takamasa Horibe --- .../simple_trajectory_follower-design.md | 24 +++++++++++++++++++ .../simple_trajectory_follower.launch.xml | 2 +- 2 files changed, 25 insertions(+), 1 deletion(-) create mode 100644 control/trajectory_follower_nodes/design/simple_trajectory_follower-design.md diff --git a/control/trajectory_follower_nodes/design/simple_trajectory_follower-design.md b/control/trajectory_follower_nodes/design/simple_trajectory_follower-design.md new file mode 100644 index 0000000000000..24ffe3166bbe4 --- /dev/null +++ b/control/trajectory_follower_nodes/design/simple_trajectory_follower-design.md @@ -0,0 +1,24 @@ +# Simple Trajectory Follower + +## Purpose + +Provide a base trajectory follower code that is simple and flexible to use. This node calculates control command based on a reference trajectory and an ego vehicle kinematics. + +## Design + +### Inputs / Outputs + +Inputs + +- `input/reference_trajectory` [autoware_auto_planning_msgs::msg::Trajectory] : reference trajectory to follow. +- `input/current_kinematic_state` [nav_msgs::msg::Odometry] : current state of the vehicle (position, velocity, etc). +- Output +- `output/control_cmd` [autoware_auto_control_msgs::msg::AckermannControlCommand] : generated control command. + +### Parameters + +| Name | Type | Description | Default value | +| :---------------------- | :---- | :----------------------------------------------------------------------------------------------------------------- | :------------ | +| use_external_target_vel | bool | use external target velocity defined by parameter when true, else follow the velocity on target trajectory points. | false | +| external_target_vel | float | target velocity used when `use_external_target_vel` is true. | 0.0 | +| lateral_deviation | float | target lateral deviation when following. | 0.0 | diff --git a/control/trajectory_follower_nodes/launch/simple_trajectory_follower.launch.xml b/control/trajectory_follower_nodes/launch/simple_trajectory_follower.launch.xml index 041ac91c42830..f74fa994086b2 100644 --- a/control/trajectory_follower_nodes/launch/simple_trajectory_follower.launch.xml +++ b/control/trajectory_follower_nodes/launch/simple_trajectory_follower.launch.xml @@ -11,6 +11,6 @@ - +