|
| 1 | +// Copyright 2024 TIER IV, Inc. |
| 2 | +// |
| 3 | +// Licensed under the Apache License, Version 2.0 (the "License"); |
| 4 | +// you may not use this file except in compliance with the License. |
| 5 | +// You may obtain a copy of the License at |
| 6 | +// |
| 7 | +// http://www.apache.org/licenses/LICENSE-2.0 |
| 8 | +// |
| 9 | +// Unless required by applicable law or agreed to in writing, software |
| 10 | +// distributed under the License is distributed on an "AS IS" BASIS, |
| 11 | +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. |
| 12 | +// See the License for the specific language governing permissions and |
| 13 | +// limitations under the License. |
| 14 | + |
| 15 | +#include "simple_pure_pursuit.hpp" |
| 16 | + |
| 17 | +#include <autoware/motion_utils/trajectory/trajectory.hpp> |
| 18 | +#include <autoware_utils/geometry/pose_deviation.hpp> |
| 19 | + |
| 20 | +#include <tf2/utils.h> |
| 21 | + |
| 22 | +#include <algorithm> |
| 23 | + |
| 24 | +namespace autoware::control::simple_pure_pursuit |
| 25 | +{ |
| 26 | +using autoware::motion_utils::findNearestIndex; |
| 27 | + |
| 28 | +SimplePurePursuitNode::SimplePurePursuitNode(const rclcpp::NodeOptions & node_options) |
| 29 | +: Node("simple_pure_pursuit", node_options), |
| 30 | + pub_control_command_( |
| 31 | + create_publisher<autoware_control_msgs::msg::Control>("~/output/control_command", 1)), |
| 32 | + vehicle_info_(autoware::vehicle_info_utils::VehicleInfoUtils(*this).getVehicleInfo()), |
| 33 | + lookahead_gain_(declare_parameter<float>("lookahead_gain")), |
| 34 | + lookahead_min_distance_(declare_parameter<float>("lookahead_min_distance")), |
| 35 | + speed_proportional_gain_(declare_parameter<float>("speed_proportional_gain")), |
| 36 | + use_external_target_vel_(declare_parameter<bool>("use_external_target_vel")), |
| 37 | + external_target_vel_(declare_parameter<float>("external_target_vel")) |
| 38 | +{ |
| 39 | + using namespace std::literals::chrono_literals; |
| 40 | + timer_ = rclcpp::create_timer( |
| 41 | + this, get_clock(), 30ms, std::bind(&SimplePurePursuitNode::on_timer, this)); |
| 42 | +} |
| 43 | + |
| 44 | +void SimplePurePursuitNode::on_timer() |
| 45 | +{ |
| 46 | + // 1. subscribe data |
| 47 | + const auto odom_ptr = odom_sub_.take_data(); |
| 48 | + const auto traj_ptr = traj_sub_.take_data(); |
| 49 | + if (!odom_ptr || !traj_ptr) { |
| 50 | + return; |
| 51 | + } |
| 52 | + |
| 53 | + // 2. extract subscribed data |
| 54 | + const auto odom = *odom_ptr; |
| 55 | + const auto traj = *traj_ptr; |
| 56 | + |
| 57 | + // 3. create control command |
| 58 | + const auto control_command = create_control_command(odom, traj); |
| 59 | + |
| 60 | + // 4. publish control command |
| 61 | + pub_control_command_->publish(control_command); |
| 62 | +} |
| 63 | + |
| 64 | +autoware_control_msgs::msg::Control SimplePurePursuitNode::create_control_command( |
| 65 | + const Odometry & odom, const Trajectory & traj) |
| 66 | +{ |
| 67 | + const size_t closest_traj_point_idx = findNearestIndex(traj.points, odom.pose.pose.position); |
| 68 | + |
| 69 | + // when the ego reaches the goal |
| 70 | + if (closest_traj_point_idx == traj.points.size() - 1 || traj.points.size() <= 5) { |
| 71 | + autoware_control_msgs::msg::Control control_command; |
| 72 | + control_command.stamp = get_clock()->now(); |
| 73 | + control_command.longitudinal.velocity = 0.0; |
| 74 | + control_command.longitudinal.acceleration = -10.0; |
| 75 | + RCLCPP_DEBUG_THROTTLE(get_logger(), *get_clock(), 5000, "reached to the goal"); |
| 76 | + return control_command; |
| 77 | + } |
| 78 | + |
| 79 | + // calculate target longitudinal velocity |
| 80 | + const double target_longitudinal_vel = |
| 81 | + use_external_target_vel_ ? external_target_vel_ |
| 82 | + : traj.points.at(closest_traj_point_idx).longitudinal_velocity_mps; |
| 83 | + |
| 84 | + // calculate control command |
| 85 | + autoware_control_msgs::msg::Control control_command; |
| 86 | + control_command.longitudinal = calc_longitudinal_control(odom, target_longitudinal_vel); |
| 87 | + control_command.lateral = |
| 88 | + calc_lateral_control(odom, traj, target_longitudinal_vel, closest_traj_point_idx); |
| 89 | + |
| 90 | + return control_command; |
| 91 | +} |
| 92 | + |
| 93 | +autoware_control_msgs::msg::Longitudinal SimplePurePursuitNode::calc_longitudinal_control( |
| 94 | + const Odometry & odom, const double target_longitudinal_vel) const |
| 95 | +{ |
| 96 | + const double current_longitudinal_vel = odom.twist.twist.linear.x; |
| 97 | + |
| 98 | + autoware_control_msgs::msg::Longitudinal longitudinal_control_command; |
| 99 | + longitudinal_control_command.velocity = target_longitudinal_vel; |
| 100 | + longitudinal_control_command.acceleration = |
| 101 | + speed_proportional_gain_ * (target_longitudinal_vel - current_longitudinal_vel); |
| 102 | + |
| 103 | + return longitudinal_control_command; |
| 104 | +} |
| 105 | + |
| 106 | +autoware_control_msgs::msg::Lateral SimplePurePursuitNode::calc_lateral_control( |
| 107 | + const Odometry & odom, const Trajectory & traj, const double target_longitudinal_vel, |
| 108 | + const size_t closest_traj_point_idx) const |
| 109 | +{ |
| 110 | + // calculate lookahead distance |
| 111 | + const double lookahead_distance = |
| 112 | + lookahead_gain_ * target_longitudinal_vel + lookahead_min_distance_; |
| 113 | + |
| 114 | + // calculate center coordinate of rear wheel |
| 115 | + const double rear_x = odom.pose.pose.position.x - |
| 116 | + vehicle_info_.wheel_base_m / 2.0 * std::cos(odom.pose.pose.orientation.z); |
| 117 | + const double rear_y = odom.pose.pose.position.y - |
| 118 | + vehicle_info_.wheel_base_m / 2.0 * std::sin(odom.pose.pose.orientation.z); |
| 119 | + |
| 120 | + // search lookahead point |
| 121 | + auto lookahead_point_itr = std::find_if( |
| 122 | + traj.points.begin() + closest_traj_point_idx, traj.points.end(), |
| 123 | + [&](const TrajectoryPoint & point) { |
| 124 | + return std::hypot(point.pose.position.x - rear_x, point.pose.position.y - rear_y) >= |
| 125 | + lookahead_distance; |
| 126 | + }); |
| 127 | + if (lookahead_point_itr == traj.points.end()) { |
| 128 | + lookahead_point_itr = traj.points.end() - 1; |
| 129 | + } |
| 130 | + const double lookahead_point_x = lookahead_point_itr->pose.position.x; |
| 131 | + const double lookahead_point_y = lookahead_point_itr->pose.position.y; |
| 132 | + |
| 133 | + // calculate steering angle |
| 134 | + autoware_control_msgs::msg::Lateral lateral_control_command; |
| 135 | + const double alpha = std::atan2(lookahead_point_y - rear_y, lookahead_point_x - rear_x) - |
| 136 | + tf2::getYaw(odom.pose.pose.orientation); |
| 137 | + lateral_control_command.steering_tire_angle = |
| 138 | + std::atan2(2.0 * vehicle_info_.wheel_base_m * std::sin(alpha), lookahead_distance); |
| 139 | + |
| 140 | + return lateral_control_command; |
| 141 | +} |
| 142 | +} // namespace autoware::control::simple_pure_pursuit |
| 143 | + |
| 144 | +#include <rclcpp_components/register_node_macro.hpp> |
| 145 | +RCLCPP_COMPONENTS_REGISTER_NODE(autoware::control::simple_pure_pursuit::SimplePurePursuitNode) |
0 commit comments