diff --git a/control/trajectory_follower_nodes/CMakeLists.txt b/control/trajectory_follower_nodes/CMakeLists.txt index fa005c1c0bfda..8d57fb7cc5cfa 100644 --- a/control/trajectory_follower_nodes/CMakeLists.txt +++ b/control/trajectory_follower_nodes/CMakeLists.txt @@ -46,6 +46,21 @@ 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 +) + +# 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" + 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 +87,5 @@ endif() ament_auto_package( INSTALL_TO_SHARE param + launch ) 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/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..a862477f309ab --- /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 geometry_msgs::msg::Pose; +using geometry_msgs::msg::Twist; +using nav_msgs::msg::Odometry; + +class SimpleTrajectoryFollower : public rclcpp::Node +{ +public: + 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_; + + 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..f74fa994086b2 --- /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..db3c2e246f9ab --- /dev/null +++ b/control/trajectory_follower_nodes/src/simple_trajectory_follower.cpp @@ -0,0 +1,116 @@ +// 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 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) +{ + 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_DEBUG( + 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_DEBUG(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)