feat(trajectory_follower_nodes): add simple trajectory follower

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
 )

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           |

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 <rclcpp/rclcpp.hpp>
+
+#include <autoware_auto_control_msgs/msg/ackermann_control_command.hpp>
+#include <autoware_auto_planning_msgs/msg/trajectory.hpp>
+#include <autoware_auto_planning_msgs/msg/trajectory_point.hpp>
+#include <geometry_msgs/msg/pose.hpp>
+#include <geometry_msgs/msg/twist.hpp>
+#include <nav_msgs/msg/odometry.hpp>
+
+#include <memory>
+
+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<Odometry>::SharedPtr sub_kinematics_;
+  rclcpp::Subscription<Trajectory>::SharedPtr sub_trajectory_;
+  rclcpp::Publisher<AckermannControlCommand>::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_

control/trajectory_follower_nodes/launch/simple_trajectory_follower.launch.xml

@@ -0,0 +1,16 @@
+<launch>
+  <arg name="use_external_target_vel" default="true"/>
+  <arg name="external_target_vel" default="5.0"/>
+  <arg name="lateral_deviation" default="0.0"/>
+
+  <!-- engage_transition_manager -->
+  <node pkg="trajectory_follower_nodes" exec="simple_trajectory_follower_exe" name="simple_trajectory_follower" output="screen">
+    <param name="use_external_target_vel" value="$(var use_external_target_vel)"/>
+    <param name="external_target_vel" value="$(var external_target_vel)"/>
+    <param name="lateral_deviation" value="$(var lateral_deviation)"/>
+
+    <remap from="input/kinematics" to="/localization/kinematic_state"/>
+    <remap from="input/trajectory" to="/planning/scenario_planning/trajectory"/>
+    <remap from="output/control_cmd" to="/vehicle/command/manual_control_cmd"/>
+  </node>
+</launch>

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 <tier4_autoware_utils/tier4_autoware_utils.hpp>
+
+#include <algorithm>
+
+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<AckermannControlCommand>("output/control_cmd", 1);
+
+  sub_kinematics_ = create_subscription<Odometry>(
+    "input/kinematics", 1, [this](const Odometry::SharedPtr msg) { odometry_ = msg; });
+  sub_trajectory_ = create_subscription<Trajectory>(
+    "input/trajectory", 1, [this](const Trajectory::SharedPtr msg) { trajectory_ = msg; });
+
+  use_external_target_vel_ = declare_parameter<bool>("use_external_target_vel", false);
+  external_target_vel_ = declare_parameter<float>("external_target_vel", 0.0);
+  lateral_deviation_ = declare_parameter<float>("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<float>(calcSteerCmd());
+  cmd.longitudinal.speed = use_external_target_vel_ ? static_cast<float>(external_target_vel_)
+                                                    : closest_traj_point_.longitudinal_velocity_mps;
+  cmd.longitudinal.acceleration = static_cast<float>(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<double>(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_macro.hpp>
+RCLCPP_COMPONENTS_REGISTER_NODE(simple_trajectory_follower::SimpleTrajectoryFollower)