From e7d79fc50833ab5c3deff6941e0a9f842db36119 Mon Sep 17 00:00:00 2001 From: Yohei MISHINA Date: Wed, 8 Jun 2022 13:51:56 +0900 Subject: [PATCH 1/4] Fix calculate stop dist on utarn trajectory --- .../longitudinal_controller_utils.hpp | 3 ++- control/trajectory_follower/package.xml | 1 + .../src/longitudinal_controller_utils.cpp | 10 +++++++--- .../test/test_longitudinal_controller_utils.cpp | 9 +++++---- .../src/longitudinal_controller_node.cpp | 2 +- .../scripts/closest_velocity_checker.py | 5 ++++- 6 files changed, 20 insertions(+), 10 deletions(-) diff --git a/control/trajectory_follower/include/trajectory_follower/longitudinal_controller_utils.hpp b/control/trajectory_follower/include/trajectory_follower/longitudinal_controller_utils.hpp index 010fba8272cea..9ebb4ff1fb869 100644 --- a/control/trajectory_follower/include/trajectory_follower/longitudinal_controller_utils.hpp +++ b/control/trajectory_follower/include/trajectory_follower/longitudinal_controller_utils.hpp @@ -21,6 +21,7 @@ #include "geometry/common_2d.hpp" #include "motion_common/motion_common.hpp" #include "motion_common/trajectory_common.hpp" +#include "tier4_autoware_utils/trajectory/trajectory.hpp" #include "tf2/utils.h" #include "trajectory_follower/visibility_control.hpp" @@ -61,7 +62,7 @@ TRAJECTORY_FOLLOWER_PUBLIC bool8_t isValidTrajectory(const Trajectory & traj); * @brief calculate distance to stopline from current vehicle position where velocity is 0 */ TRAJECTORY_FOLLOWER_PUBLIC float64_t -calcStopDistance(const Point & current_pos, const Trajectory & traj); +calcStopDistance(const Point & current_pos, const size_t src_idx, const Trajectory & traj); /** * @brief calculate pitch angle from estimated current pose diff --git a/control/trajectory_follower/package.xml b/control/trajectory_follower/package.xml index 840cad74aeb55..6d0e3c158c5b8 100644 --- a/control/trajectory_follower/package.xml +++ b/control/trajectory_follower/package.xml @@ -21,6 +21,7 @@ geometry_msgs interpolation motion_common + tier4_autoware_utils osqp_interface rclcpp rclcpp_components diff --git a/control/trajectory_follower/src/longitudinal_controller_utils.cpp b/control/trajectory_follower/src/longitudinal_controller_utils.cpp index 1f5018f84540d..24d91046c1e90 100644 --- a/control/trajectory_follower/src/longitudinal_controller_utils.cpp +++ b/control/trajectory_follower/src/longitudinal_controller_utils.cpp @@ -62,17 +62,21 @@ bool isValidTrajectory(const Trajectory & traj) return true; } -float64_t calcStopDistance(const Point & current_pos, const Trajectory & traj) +float64_t calcStopDistance(const Point & current_pos, const size_t src_idx, const Trajectory & traj) { const std::experimental::optional stop_idx_opt = trajectory_common::searchZeroVelocityIndex(traj.points); + const float64_t signed_length_src_offset = + tier4_autoware_utils::calcLongitudinalOffsetToSegment(traj.points, src_idx, current_pos); // If no zero velocity point, return the length between current_pose to the end of trajectory. if (!stop_idx_opt) { - return trajectory_common::calcSignedArcLength(traj.points, current_pos, traj.points.size() - 1); + float64_t signed_length_on_traj = trajectory_common::calcSignedArcLength(traj.points, src_idx, traj.points.size() - 1); + return signed_length_on_traj - signed_length_src_offset; } - return trajectory_common::calcSignedArcLength(traj.points, current_pos, *stop_idx_opt); + float64_t signed_length_on_traj = trajectory_common::calcSignedArcLength(traj.points, src_idx, *stop_idx_opt); + return signed_length_on_traj - signed_length_src_offset; } float64_t getPitchByPose(const Quaternion & quaternion_msg) diff --git a/control/trajectory_follower/test/test_longitudinal_controller_utils.cpp b/control/trajectory_follower/test/test_longitudinal_controller_utils.cpp index 437a7882f96d2..f69e4f1a0dd50 100644 --- a/control/trajectory_follower/test/test_longitudinal_controller_utils.cpp +++ b/control/trajectory_follower/test/test_longitudinal_controller_utils.cpp @@ -54,16 +54,17 @@ TEST(TestLongitudinalControllerUtils, calcStopDistance) Point current_pos; current_pos.x = 0.0; current_pos.y = 0.0; + size_t closest_idx = 0; Trajectory traj; // empty trajectory : exception - EXPECT_THROW(longitudinal_utils::calcStopDistance(current_pos, traj), std::invalid_argument); + EXPECT_THROW(longitudinal_utils::calcStopDistance(current_pos, closest_idx, traj), std::invalid_argument); // one point trajectory : exception TrajectoryPoint point; point.pose.position.x = 0.0; point.pose.position.y = 0.0; point.longitudinal_velocity_mps = 0.0; traj.points.push_back(point); - EXPECT_THROW(longitudinal_utils::calcStopDistance(current_pos, traj), std::out_of_range); + EXPECT_THROW(longitudinal_utils::calcStopDistance(current_pos, closest_idx, traj), std::out_of_range); traj.points.clear(); // non stopping trajectory: stop distance = trajectory length point.pose.position.x = 0.0; @@ -78,7 +79,7 @@ TEST(TestLongitudinalControllerUtils, calcStopDistance) point.pose.position.y = 0.0; point.longitudinal_velocity_mps = 1.0; traj.points.push_back(point); - EXPECT_EQ(longitudinal_utils::calcStopDistance(current_pos, traj), 2.0); + EXPECT_EQ(longitudinal_utils::calcStopDistance(current_pos, closest_idx, traj), 2.0); // stopping trajectory: stop distance = length until stopping point point.pose.position.x = 3.0; point.pose.position.y = 0.0; @@ -92,7 +93,7 @@ TEST(TestLongitudinalControllerUtils, calcStopDistance) point.pose.position.y = 0.0; point.longitudinal_velocity_mps = 0.0; traj.points.push_back(point); - EXPECT_EQ(longitudinal_utils::calcStopDistance(current_pos, traj), 3.0); + EXPECT_EQ(longitudinal_utils::calcStopDistance(current_pos, closest_idx, traj), 3.0); } TEST(TestLongitudinalControllerUtils, getPitchByPose) diff --git a/control/trajectory_follower_nodes/src/longitudinal_controller_node.cpp b/control/trajectory_follower_nodes/src/longitudinal_controller_node.cpp index 70930416f8412..d045381ab8539 100644 --- a/control/trajectory_follower_nodes/src/longitudinal_controller_node.cpp +++ b/control/trajectory_follower_nodes/src/longitudinal_controller_node.cpp @@ -425,7 +425,7 @@ LongitudinalController::ControlData LongitudinalController::getControlData( // distance to stopline control_data.stop_dist = trajectory_follower::longitudinal_utils::calcStopDistance( - current_pose.position, *m_trajectory_ptr); + current_pose.position, control_data.nearest_idx, *m_trajectory_ptr); // pitch const float64_t raw_pitch = diff --git a/planning/motion_velocity_smoother/scripts/closest_velocity_checker.py b/planning/motion_velocity_smoother/scripts/closest_velocity_checker.py index c7a02417f36ea..61910ce5eac66 100755 --- a/planning/motion_velocity_smoother/scripts/closest_velocity_checker.py +++ b/planning/motion_velocity_smoother/scripts/closest_velocity_checker.py @@ -251,7 +251,10 @@ def CallBackAvoidTrajectory(self, msg): return def CallBackLaneDriveTrajectory(self, msg): - # self.get_logger().info('OBSTACLE_STOP called') + self.get_logger().info('OBSTACLE_STOP called') + for i in range(0, len(msg.points)): + vel = msg.points[i].longitudinal_velocity_mps + self.get_logger().info(f"long vel: {vel}") closest = self.calcClosestTrajectory(msg) self.data_arr[OBSTACLE_STOP] = msg.points[closest].longitudinal_velocity_mps return From 45370ae8619eaa08733bac2512484c22d252380b Mon Sep 17 00:00:00 2001 From: Yohei MISHINA Date: Thu, 9 Jun 2022 11:22:09 +0900 Subject: [PATCH 2/4] Fix calculate target point in longitudinal controller --- .../longitudinal_controller_utils.hpp | 16 +-- .../src/longitudinal_controller_utils.cpp | 13 ++- .../test_longitudinal_controller_utils.cpp | 103 +++++++++--------- .../longitudinal_controller_node.hpp | 2 +- .../src/longitudinal_controller_node.cpp | 16 +-- .../scripts/closest_velocity_checker.py | 5 +- 6 files changed, 82 insertions(+), 73 deletions(-) diff --git a/control/trajectory_follower/include/trajectory_follower/longitudinal_controller_utils.hpp b/control/trajectory_follower/include/trajectory_follower/longitudinal_controller_utils.hpp index 9ebb4ff1fb869..754a3a4f10390 100644 --- a/control/trajectory_follower/include/trajectory_follower/longitudinal_controller_utils.hpp +++ b/control/trajectory_follower/include/trajectory_follower/longitudinal_controller_utils.hpp @@ -62,7 +62,7 @@ TRAJECTORY_FOLLOWER_PUBLIC bool8_t isValidTrajectory(const Trajectory & traj); * @brief calculate distance to stopline from current vehicle position where velocity is 0 */ TRAJECTORY_FOLLOWER_PUBLIC float64_t -calcStopDistance(const Point & current_pos, const size_t src_idx, const Trajectory & traj); +calcStopDistance(const Pose & current_pose, const Trajectory & traj, const float64_t & max_dist, const float64_t & max_yaw); /** * @brief calculate pitch angle from estimated current pose @@ -108,20 +108,22 @@ lerpOrientation(const Quaternion & o_from, const Quaternion & o_to, const float6 */ template TRAJECTORY_FOLLOWER_PUBLIC TrajectoryPoint -lerpTrajectoryPoint(const T & points, const Point & point) +lerpTrajectoryPoint(const T & points, const Pose & pose, const float64_t & max_dist, const float64_t & max_yaw) { TrajectoryPoint interpolated_point; - - const size_t nearest_seg_idx = trajectory_common::findNearestSegmentIndex(points, point); + auto seg_idx = tier4_autoware_utils::findNearestSegmentIndex(points, pose, max_dist, max_yaw); + if (!seg_idx) { // if not fund idx + seg_idx = tier4_autoware_utils::findNearestSegmentIndex(points, pose); + } const float64_t len_to_interpolated = - trajectory_common::calcLongitudinalOffsetToSegment(points, nearest_seg_idx, point); + tier4_autoware_utils::calcLongitudinalOffsetToSegment(points, *seg_idx, pose.position); const float64_t len_segment = - trajectory_common::calcSignedArcLength(points, nearest_seg_idx, nearest_seg_idx + 1); + trajectory_common::calcSignedArcLength(points, *seg_idx, *seg_idx + 1); const float64_t interpolate_ratio = std::clamp(len_to_interpolated / len_segment, 0.0, 1.0); { - const size_t i = nearest_seg_idx; + const size_t i = *seg_idx; interpolated_point.pose.position.x = motion_common::interpolate( points.at(i).pose.position.x, points.at(i + 1).pose.position.x, interpolate_ratio); diff --git a/control/trajectory_follower/src/longitudinal_controller_utils.cpp b/control/trajectory_follower/src/longitudinal_controller_utils.cpp index 24d91046c1e90..b25300139d1bd 100644 --- a/control/trajectory_follower/src/longitudinal_controller_utils.cpp +++ b/control/trajectory_follower/src/longitudinal_controller_utils.cpp @@ -62,20 +62,25 @@ bool isValidTrajectory(const Trajectory & traj) return true; } -float64_t calcStopDistance(const Point & current_pos, const size_t src_idx, const Trajectory & traj) +float64_t calcStopDistance(const Pose & current_pose, const Trajectory & traj, const float64_t & max_dist, const float64_t & max_yaw) { const std::experimental::optional stop_idx_opt = trajectory_common::searchZeroVelocityIndex(traj.points); + + auto seg_idx = tier4_autoware_utils::findNearestSegmentIndex(traj.points, current_pose, max_dist, max_yaw); + if (!seg_idx) { // if not fund idx + seg_idx = tier4_autoware_utils::findNearestSegmentIndex(traj.points, current_pose); + } const float64_t signed_length_src_offset = - tier4_autoware_utils::calcLongitudinalOffsetToSegment(traj.points, src_idx, current_pos); + tier4_autoware_utils::calcLongitudinalOffsetToSegment(traj.points, *seg_idx, current_pose.position); // If no zero velocity point, return the length between current_pose to the end of trajectory. if (!stop_idx_opt) { - float64_t signed_length_on_traj = trajectory_common::calcSignedArcLength(traj.points, src_idx, traj.points.size() - 1); + float64_t signed_length_on_traj = tier4_autoware_utils::calcSignedArcLength(traj.points, *seg_idx, traj.points.size() - 1); return signed_length_on_traj - signed_length_src_offset; } - float64_t signed_length_on_traj = trajectory_common::calcSignedArcLength(traj.points, src_idx, *stop_idx_opt); + float64_t signed_length_on_traj = tier4_autoware_utils::calcSignedArcLength(traj.points, *seg_idx, *stop_idx_opt); return signed_length_on_traj - signed_length_src_offset; } diff --git a/control/trajectory_follower/test/test_longitudinal_controller_utils.cpp b/control/trajectory_follower/test/test_longitudinal_controller_utils.cpp index f69e4f1a0dd50..c9ed8a45defcb 100644 --- a/control/trajectory_follower/test/test_longitudinal_controller_utils.cpp +++ b/control/trajectory_follower/test/test_longitudinal_controller_utils.cpp @@ -50,21 +50,23 @@ TEST(TestLongitudinalControllerUtils, calcStopDistance) { using autoware_auto_planning_msgs::msg::Trajectory; using autoware_auto_planning_msgs::msg::TrajectoryPoint; - using geometry_msgs::msg::Point; - Point current_pos; - current_pos.x = 0.0; - current_pos.y = 0.0; - size_t closest_idx = 0; + using geometry_msgs::msg::Pose; + Pose current_pose; + current_pose.position.x = 0.0; + current_pose.position.y = 0.0; + current_pose.position.z = 0.0; Trajectory traj; + double max_dist = 3.0; + double max_yaw = 0.7; // empty trajectory : exception - EXPECT_THROW(longitudinal_utils::calcStopDistance(current_pos, closest_idx, traj), std::invalid_argument); + EXPECT_THROW(longitudinal_utils::calcStopDistance(current_pose, traj, max_dist, max_yaw), std::invalid_argument); // one point trajectory : exception TrajectoryPoint point; point.pose.position.x = 0.0; point.pose.position.y = 0.0; point.longitudinal_velocity_mps = 0.0; traj.points.push_back(point); - EXPECT_THROW(longitudinal_utils::calcStopDistance(current_pos, closest_idx, traj), std::out_of_range); + EXPECT_THROW(longitudinal_utils::calcStopDistance(current_pose, traj, max_dist, max_yaw), std::out_of_range); traj.points.clear(); // non stopping trajectory: stop distance = trajectory length point.pose.position.x = 0.0; @@ -79,7 +81,7 @@ TEST(TestLongitudinalControllerUtils, calcStopDistance) point.pose.position.y = 0.0; point.longitudinal_velocity_mps = 1.0; traj.points.push_back(point); - EXPECT_EQ(longitudinal_utils::calcStopDistance(current_pos, closest_idx, traj), 2.0); + EXPECT_EQ(longitudinal_utils::calcStopDistance(current_pose, traj, max_dist, max_yaw), 2.0); // stopping trajectory: stop distance = length until stopping point point.pose.position.x = 3.0; point.pose.position.y = 0.0; @@ -93,7 +95,7 @@ TEST(TestLongitudinalControllerUtils, calcStopDistance) point.pose.position.y = 0.0; point.longitudinal_velocity_mps = 0.0; traj.points.push_back(point); - EXPECT_EQ(longitudinal_utils::calcStopDistance(current_pos, closest_idx, traj), 3.0); + EXPECT_EQ(longitudinal_utils::calcStopDistance(current_pose, traj, max_dist, max_yaw), 3.0); } TEST(TestLongitudinalControllerUtils, getPitchByPose) @@ -320,7 +322,7 @@ TEST(TestLongitudinalControllerUtils, lerpOrientation) TEST(TestLongitudinalControllerUtils, lerpTrajectoryPoint) { using autoware_auto_planning_msgs::msg::TrajectoryPoint; - using geometry_msgs::msg::Point; + using geometry_msgs::msg::Pose; const double abs_err = 1e-15; decltype(autoware_auto_planning_msgs::msg::Trajectory::points) points; TrajectoryPoint p; @@ -345,72 +347,73 @@ TEST(TestLongitudinalControllerUtils, lerpTrajectoryPoint) p.acceleration_mps2 = 40.0; points.push_back(p); TrajectoryPoint result; - Point point; - + Pose pose; + double max_dist = 3.0; + double max_yaw = 0.7; // Points on the trajectory gives back the original trajectory points values - point.x = 0.0; - point.y = 0.0; - result = longitudinal_utils::lerpTrajectoryPoint(points, point); - EXPECT_NEAR(result.pose.position.x, point.x, abs_err); - EXPECT_NEAR(result.pose.position.y, point.y, abs_err); + pose.position.x = 0.0; + pose.position.y = 0.0; + result = longitudinal_utils::lerpTrajectoryPoint(points, pose, max_dist, max_yaw); + EXPECT_NEAR(result.pose.position.x, pose.position.x, abs_err); + EXPECT_NEAR(result.pose.position.y, pose.position.y, abs_err); EXPECT_NEAR(result.longitudinal_velocity_mps, 10.0, abs_err); EXPECT_NEAR(result.acceleration_mps2, 10.0, abs_err); - point.x = 1.0; - point.y = 0.0; - result = longitudinal_utils::lerpTrajectoryPoint(points, point); - EXPECT_NEAR(result.pose.position.x, point.x, abs_err); - EXPECT_NEAR(result.pose.position.y, point.y, abs_err); + pose.position.x = 1.0; + pose.position.y = 0.0; + result = longitudinal_utils::lerpTrajectoryPoint(points, pose, max_dist, max_yaw); + EXPECT_NEAR(result.pose.position.x, pose.position.x, abs_err); + EXPECT_NEAR(result.pose.position.y, pose.position.y, abs_err); EXPECT_NEAR(result.longitudinal_velocity_mps, 20.0, abs_err); EXPECT_NEAR(result.acceleration_mps2, 20.0, abs_err); - point.x = 1.0; - point.y = 1.0; - result = longitudinal_utils::lerpTrajectoryPoint(points, point); - EXPECT_NEAR(result.pose.position.x, point.x, abs_err); - EXPECT_NEAR(result.pose.position.y, point.y, abs_err); + pose.position.x = 1.0; + pose.position.y = 1.0; + result = longitudinal_utils::lerpTrajectoryPoint(points, pose, max_dist, max_yaw); + EXPECT_NEAR(result.pose.position.x, pose.position.x, abs_err); + EXPECT_NEAR(result.pose.position.y, pose.position.y, abs_err); EXPECT_NEAR(result.longitudinal_velocity_mps, 30.0, abs_err); EXPECT_NEAR(result.acceleration_mps2, 30.0, abs_err); - point.x = 2.0; - point.y = 1.0; - result = longitudinal_utils::lerpTrajectoryPoint(points, point); - EXPECT_NEAR(result.pose.position.x, point.x, abs_err); - EXPECT_NEAR(result.pose.position.y, point.y, abs_err); + pose.position.x = 2.0; + pose.position.y = 1.0; + result = longitudinal_utils::lerpTrajectoryPoint(points, pose, max_dist, max_yaw); + EXPECT_NEAR(result.pose.position.x, pose.position.x, abs_err); + EXPECT_NEAR(result.pose.position.y, pose.position.y, abs_err); EXPECT_NEAR(result.longitudinal_velocity_mps, 40.0, abs_err); EXPECT_NEAR(result.acceleration_mps2, 40.0, abs_err); // Interpolate between trajectory points - point.x = 0.5; - point.y = 0.0; - result = longitudinal_utils::lerpTrajectoryPoint(points, point); - EXPECT_NEAR(result.pose.position.x, point.x, abs_err); - EXPECT_NEAR(result.pose.position.y, point.y, abs_err); + pose.position.x = 0.5; + pose.position.y = 0.0; + result = longitudinal_utils::lerpTrajectoryPoint(points, pose, max_dist, max_yaw); + EXPECT_NEAR(result.pose.position.x, pose.position.x, abs_err); + EXPECT_NEAR(result.pose.position.y, pose.position.y, abs_err); EXPECT_NEAR(result.longitudinal_velocity_mps, 15.0, abs_err); EXPECT_NEAR(result.acceleration_mps2, 15.0, abs_err); - point.x = 0.75; - point.y = 0.0; - result = longitudinal_utils::lerpTrajectoryPoint(points, point); + pose.position.x = 0.75; + pose.position.y = 0.0; + result = longitudinal_utils::lerpTrajectoryPoint(points, pose, max_dist, max_yaw); - EXPECT_NEAR(result.pose.position.x, point.x, abs_err); - EXPECT_NEAR(result.pose.position.y, point.y, abs_err); + EXPECT_NEAR(result.pose.position.x, pose.position.x, abs_err); + EXPECT_NEAR(result.pose.position.y, pose.position.y, abs_err); EXPECT_NEAR(result.longitudinal_velocity_mps, 17.5, abs_err); EXPECT_NEAR(result.acceleration_mps2, 17.5, abs_err); // Interpolate away from the trajectory (interpolated point is projected) - point.x = 0.5; - point.y = -1.0; - result = longitudinal_utils::lerpTrajectoryPoint(points, point); - EXPECT_NEAR(result.pose.position.x, point.x, abs_err); + pose.position.x = 0.5; + pose.position.y = -1.0; + result = longitudinal_utils::lerpTrajectoryPoint(points, pose, max_dist, max_yaw); + EXPECT_NEAR(result.pose.position.x, pose.position.x, abs_err); EXPECT_NEAR(result.pose.position.y, 0.0, abs_err); EXPECT_NEAR(result.longitudinal_velocity_mps, 15.0, abs_err); EXPECT_NEAR(result.acceleration_mps2, 15.0, abs_err); // Ambiguous projections: possibility with the lowest index is used - point.x = 0.5; - point.y = 0.5; - result = longitudinal_utils::lerpTrajectoryPoint(points, point); - EXPECT_NEAR(result.pose.position.x, point.x, abs_err); + pose.position.x = 0.5; + pose.position.y = 0.5; + result = longitudinal_utils::lerpTrajectoryPoint(points, pose, max_dist, max_yaw); + EXPECT_NEAR(result.pose.position.x, pose.position.x, abs_err); EXPECT_NEAR(result.pose.position.y, 0.0, abs_err); EXPECT_NEAR(result.longitudinal_velocity_mps, 15.0, abs_err); EXPECT_NEAR(result.acceleration_mps2, 15.0, abs_err); diff --git a/control/trajectory_follower_nodes/include/trajectory_follower_nodes/longitudinal_controller_node.hpp b/control/trajectory_follower_nodes/include/trajectory_follower_nodes/longitudinal_controller_node.hpp index 9832f0dfd6e09..f9825902954f4 100644 --- a/control/trajectory_follower_nodes/include/trajectory_follower_nodes/longitudinal_controller_node.hpp +++ b/control/trajectory_follower_nodes/include/trajectory_follower_nodes/longitudinal_controller_node.hpp @@ -326,7 +326,7 @@ class TRAJECTORY_FOLLOWER_PUBLIC LongitudinalController : public rclcpp::Node */ autoware_auto_planning_msgs::msg::TrajectoryPoint calcInterpolatedTargetValue( const autoware_auto_planning_msgs::msg::Trajectory & traj, - const geometry_msgs::msg::Point & point, const size_t nearest_idx) const; + const geometry_msgs::msg::Pose & pose, const size_t nearest_idx) const; /** * @brief calculate predicted velocity after time delay based on past control commands diff --git a/control/trajectory_follower_nodes/src/longitudinal_controller_node.cpp b/control/trajectory_follower_nodes/src/longitudinal_controller_node.cpp index d045381ab8539..9567e508fa128 100644 --- a/control/trajectory_follower_nodes/src/longitudinal_controller_node.cpp +++ b/control/trajectory_follower_nodes/src/longitudinal_controller_node.cpp @@ -425,7 +425,7 @@ LongitudinalController::ControlData LongitudinalController::getControlData( // distance to stopline control_data.stop_dist = trajectory_follower::longitudinal_utils::calcStopDistance( - current_pose.position, control_data.nearest_idx, *m_trajectory_ptr); + current_pose, *m_trajectory_ptr, max_dist, max_yaw); // pitch const float64_t raw_pitch = @@ -553,7 +553,7 @@ LongitudinalController::Motion LongitudinalController::calcCtrlCmd( const auto target_pose = trajectory_follower::longitudinal_utils::calcPoseAfterTimeDelay( current_pose, m_delay_compensation_time, current_vel); const auto target_interpolated_point = - calcInterpolatedTargetValue(*m_trajectory_ptr, target_pose.position, nearest_idx); + calcInterpolatedTargetValue(*m_trajectory_ptr, target_pose, nearest_idx); target_motion = Motion{ target_interpolated_point.longitudinal_velocity_mps, target_interpolated_point.acceleration_mps2}; @@ -801,7 +801,7 @@ LongitudinalController::Motion LongitudinalController::keepBrakeBeforeStop( autoware_auto_planning_msgs::msg::TrajectoryPoint LongitudinalController::calcInterpolatedTargetValue( const autoware_auto_planning_msgs::msg::Trajectory & traj, - const geometry_msgs::msg::Point & point, const size_t nearest_idx) const + const geometry_msgs::msg::Pose & pose, const size_t nearest_idx) const { if (traj.points.size() == 1) { return traj.points.at(0); @@ -810,18 +810,20 @@ LongitudinalController::calcInterpolatedTargetValue( // If the current position is not within the reference trajectory, enable the edge value. // Else, apply linear interpolation if (nearest_idx == 0) { - if (motion_common::calcSignedArcLength(traj.points, point, 0) > 0) { + if (motion_common::calcSignedArcLength(traj.points, pose.position, 0) > 0) { return traj.points.at(0); } } if (nearest_idx == traj.points.size() - 1) { - if (motion_common::calcSignedArcLength(traj.points, point, traj.points.size() - 1) < 0) { + if (motion_common::calcSignedArcLength(traj.points, pose.position, traj.points.size() - 1) < 0) { return traj.points.at(traj.points.size() - 1); } } // apply linear interpolation - return trajectory_follower::longitudinal_utils::lerpTrajectoryPoint(traj.points, point); + return trajectory_follower::longitudinal_utils::lerpTrajectoryPoint(traj.points, pose, + m_state_transition_params.emergency_state_traj_trans_dev, + m_state_transition_params.emergency_state_traj_rot_dev); } float64_t LongitudinalController::predictedVelocityInTargetPoint( @@ -920,7 +922,7 @@ void LongitudinalController::updateDebugVelAcc( const size_t nearest_idx = control_data.nearest_idx; const auto interpolated_point = - calcInterpolatedTargetValue(*m_trajectory_ptr, current_pose.position, nearest_idx); + calcInterpolatedTargetValue(*m_trajectory_ptr, current_pose, nearest_idx); m_debug_values.setValues(DebugValues::TYPE::CURRENT_VEL, current_vel); m_debug_values.setValues(DebugValues::TYPE::TARGET_VEL, target_motion.vel); diff --git a/planning/motion_velocity_smoother/scripts/closest_velocity_checker.py b/planning/motion_velocity_smoother/scripts/closest_velocity_checker.py index 61910ce5eac66..c7a02417f36ea 100755 --- a/planning/motion_velocity_smoother/scripts/closest_velocity_checker.py +++ b/planning/motion_velocity_smoother/scripts/closest_velocity_checker.py @@ -251,10 +251,7 @@ def CallBackAvoidTrajectory(self, msg): return def CallBackLaneDriveTrajectory(self, msg): - self.get_logger().info('OBSTACLE_STOP called') - for i in range(0, len(msg.points)): - vel = msg.points[i].longitudinal_velocity_mps - self.get_logger().info(f"long vel: {vel}") + # self.get_logger().info('OBSTACLE_STOP called') closest = self.calcClosestTrajectory(msg) self.data_arr[OBSTACLE_STOP] = msg.points[closest].longitudinal_velocity_mps return From 8cd4485f791ccf809e1566a4524ca4b7d0420c08 Mon Sep 17 00:00:00 2001 From: "pre-commit-ci[bot]" <66853113+pre-commit-ci[bot]@users.noreply.github.com> Date: Thu, 9 Jun 2022 03:09:13 +0000 Subject: [PATCH 3/4] ci(pre-commit): autofix --- .../longitudinal_controller_utils.hpp | 11 ++++++----- control/trajectory_follower/package.xml | 2 +- .../src/longitudinal_controller_utils.cpp | 17 +++++++++++------ .../test/test_longitudinal_controller_utils.cpp | 7 +++++-- .../src/longitudinal_controller_node.cpp | 13 +++++++------ 5 files changed, 30 insertions(+), 20 deletions(-) diff --git a/control/trajectory_follower/include/trajectory_follower/longitudinal_controller_utils.hpp b/control/trajectory_follower/include/trajectory_follower/longitudinal_controller_utils.hpp index 754a3a4f10390..4fab5e5513944 100644 --- a/control/trajectory_follower/include/trajectory_follower/longitudinal_controller_utils.hpp +++ b/control/trajectory_follower/include/trajectory_follower/longitudinal_controller_utils.hpp @@ -21,8 +21,8 @@ #include "geometry/common_2d.hpp" #include "motion_common/motion_common.hpp" #include "motion_common/trajectory_common.hpp" -#include "tier4_autoware_utils/trajectory/trajectory.hpp" #include "tf2/utils.h" +#include "tier4_autoware_utils/trajectory/trajectory.hpp" #include "trajectory_follower/visibility_control.hpp" #include // NOLINT @@ -61,8 +61,9 @@ TRAJECTORY_FOLLOWER_PUBLIC bool8_t isValidTrajectory(const Trajectory & traj); /** * @brief calculate distance to stopline from current vehicle position where velocity is 0 */ -TRAJECTORY_FOLLOWER_PUBLIC float64_t -calcStopDistance(const Pose & current_pose, const Trajectory & traj, const float64_t & max_dist, const float64_t & max_yaw); +TRAJECTORY_FOLLOWER_PUBLIC float64_t calcStopDistance( + const Pose & current_pose, const Trajectory & traj, const float64_t & max_dist, + const float64_t & max_yaw); /** * @brief calculate pitch angle from estimated current pose @@ -107,8 +108,8 @@ lerpOrientation(const Quaternion & o_from, const Quaternion & o_to, const float6 * @param [in] point Interpolated point is nearest to this point. */ template -TRAJECTORY_FOLLOWER_PUBLIC TrajectoryPoint -lerpTrajectoryPoint(const T & points, const Pose & pose, const float64_t & max_dist, const float64_t & max_yaw) +TRAJECTORY_FOLLOWER_PUBLIC TrajectoryPoint lerpTrajectoryPoint( + const T & points, const Pose & pose, const float64_t & max_dist, const float64_t & max_yaw) { TrajectoryPoint interpolated_point; auto seg_idx = tier4_autoware_utils::findNearestSegmentIndex(points, pose, max_dist, max_yaw); diff --git a/control/trajectory_follower/package.xml b/control/trajectory_follower/package.xml index 6d0e3c158c5b8..f43ffdb07ed20 100644 --- a/control/trajectory_follower/package.xml +++ b/control/trajectory_follower/package.xml @@ -21,13 +21,13 @@ geometry_msgs interpolation motion_common - tier4_autoware_utils osqp_interface rclcpp rclcpp_components std_msgs tf2 tf2_ros + tier4_autoware_utils ament_cmake_ros ament_lint_auto diff --git a/control/trajectory_follower/src/longitudinal_controller_utils.cpp b/control/trajectory_follower/src/longitudinal_controller_utils.cpp index b25300139d1bd..e18872a9b1d18 100644 --- a/control/trajectory_follower/src/longitudinal_controller_utils.cpp +++ b/control/trajectory_follower/src/longitudinal_controller_utils.cpp @@ -62,25 +62,30 @@ bool isValidTrajectory(const Trajectory & traj) return true; } -float64_t calcStopDistance(const Pose & current_pose, const Trajectory & traj, const float64_t & max_dist, const float64_t & max_yaw) +float64_t calcStopDistance( + const Pose & current_pose, const Trajectory & traj, const float64_t & max_dist, + const float64_t & max_yaw) { const std::experimental::optional stop_idx_opt = trajectory_common::searchZeroVelocityIndex(traj.points); - auto seg_idx = tier4_autoware_utils::findNearestSegmentIndex(traj.points, current_pose, max_dist, max_yaw); + auto seg_idx = + tier4_autoware_utils::findNearestSegmentIndex(traj.points, current_pose, max_dist, max_yaw); if (!seg_idx) { // if not fund idx seg_idx = tier4_autoware_utils::findNearestSegmentIndex(traj.points, current_pose); } - const float64_t signed_length_src_offset = - tier4_autoware_utils::calcLongitudinalOffsetToSegment(traj.points, *seg_idx, current_pose.position); + const float64_t signed_length_src_offset = tier4_autoware_utils::calcLongitudinalOffsetToSegment( + traj.points, *seg_idx, current_pose.position); // If no zero velocity point, return the length between current_pose to the end of trajectory. if (!stop_idx_opt) { - float64_t signed_length_on_traj = tier4_autoware_utils::calcSignedArcLength(traj.points, *seg_idx, traj.points.size() - 1); + float64_t signed_length_on_traj = + tier4_autoware_utils::calcSignedArcLength(traj.points, *seg_idx, traj.points.size() - 1); return signed_length_on_traj - signed_length_src_offset; } - float64_t signed_length_on_traj = tier4_autoware_utils::calcSignedArcLength(traj.points, *seg_idx, *stop_idx_opt); + float64_t signed_length_on_traj = + tier4_autoware_utils::calcSignedArcLength(traj.points, *seg_idx, *stop_idx_opt); return signed_length_on_traj - signed_length_src_offset; } diff --git a/control/trajectory_follower/test/test_longitudinal_controller_utils.cpp b/control/trajectory_follower/test/test_longitudinal_controller_utils.cpp index c9ed8a45defcb..354c483982a8f 100644 --- a/control/trajectory_follower/test/test_longitudinal_controller_utils.cpp +++ b/control/trajectory_follower/test/test_longitudinal_controller_utils.cpp @@ -59,14 +59,17 @@ TEST(TestLongitudinalControllerUtils, calcStopDistance) double max_dist = 3.0; double max_yaw = 0.7; // empty trajectory : exception - EXPECT_THROW(longitudinal_utils::calcStopDistance(current_pose, traj, max_dist, max_yaw), std::invalid_argument); + EXPECT_THROW( + longitudinal_utils::calcStopDistance(current_pose, traj, max_dist, max_yaw), + std::invalid_argument); // one point trajectory : exception TrajectoryPoint point; point.pose.position.x = 0.0; point.pose.position.y = 0.0; point.longitudinal_velocity_mps = 0.0; traj.points.push_back(point); - EXPECT_THROW(longitudinal_utils::calcStopDistance(current_pose, traj, max_dist, max_yaw), std::out_of_range); + EXPECT_THROW( + longitudinal_utils::calcStopDistance(current_pose, traj, max_dist, max_yaw), std::out_of_range); traj.points.clear(); // non stopping trajectory: stop distance = trajectory length point.pose.position.x = 0.0; diff --git a/control/trajectory_follower_nodes/src/longitudinal_controller_node.cpp b/control/trajectory_follower_nodes/src/longitudinal_controller_node.cpp index 9567e508fa128..fb3cda35d8317 100644 --- a/control/trajectory_follower_nodes/src/longitudinal_controller_node.cpp +++ b/control/trajectory_follower_nodes/src/longitudinal_controller_node.cpp @@ -800,8 +800,8 @@ LongitudinalController::Motion LongitudinalController::keepBrakeBeforeStop( autoware_auto_planning_msgs::msg::TrajectoryPoint LongitudinalController::calcInterpolatedTargetValue( - const autoware_auto_planning_msgs::msg::Trajectory & traj, - const geometry_msgs::msg::Pose & pose, const size_t nearest_idx) const + const autoware_auto_planning_msgs::msg::Trajectory & traj, const geometry_msgs::msg::Pose & pose, + const size_t nearest_idx) const { if (traj.points.size() == 1) { return traj.points.at(0); @@ -815,15 +815,16 @@ LongitudinalController::calcInterpolatedTargetValue( } } if (nearest_idx == traj.points.size() - 1) { - if (motion_common::calcSignedArcLength(traj.points, pose.position, traj.points.size() - 1) < 0) { + if ( + motion_common::calcSignedArcLength(traj.points, pose.position, traj.points.size() - 1) < 0) { return traj.points.at(traj.points.size() - 1); } } // apply linear interpolation - return trajectory_follower::longitudinal_utils::lerpTrajectoryPoint(traj.points, pose, - m_state_transition_params.emergency_state_traj_trans_dev, - m_state_transition_params.emergency_state_traj_rot_dev); + return trajectory_follower::longitudinal_utils::lerpTrajectoryPoint( + traj.points, pose, m_state_transition_params.emergency_state_traj_trans_dev, + m_state_transition_params.emergency_state_traj_rot_dev); } float64_t LongitudinalController::predictedVelocityInTargetPoint( From 974004039d6a8aba053086a8b7cfff01da5294d7 Mon Sep 17 00:00:00 2001 From: Yohei MISHINA Date: Fri, 10 Jun 2022 10:06:34 +0900 Subject: [PATCH 4/4] fixed coding style --- .../trajectory_follower/longitudinal_controller_utils.hpp | 6 +++--- .../src/longitudinal_controller_utils.cpp | 4 ++-- 2 files changed, 5 insertions(+), 5 deletions(-) diff --git a/control/trajectory_follower/include/trajectory_follower/longitudinal_controller_utils.hpp b/control/trajectory_follower/include/trajectory_follower/longitudinal_controller_utils.hpp index 4fab5e5513944..6cd729841fe77 100644 --- a/control/trajectory_follower/include/trajectory_follower/longitudinal_controller_utils.hpp +++ b/control/trajectory_follower/include/trajectory_follower/longitudinal_controller_utils.hpp @@ -62,8 +62,8 @@ TRAJECTORY_FOLLOWER_PUBLIC bool8_t isValidTrajectory(const Trajectory & traj); * @brief calculate distance to stopline from current vehicle position where velocity is 0 */ TRAJECTORY_FOLLOWER_PUBLIC float64_t calcStopDistance( - const Pose & current_pose, const Trajectory & traj, const float64_t & max_dist, - const float64_t & max_yaw); + const Pose & current_pose, const Trajectory & traj, const float64_t max_dist, + const float64_t max_yaw); /** * @brief calculate pitch angle from estimated current pose @@ -109,7 +109,7 @@ lerpOrientation(const Quaternion & o_from, const Quaternion & o_to, const float6 */ template TRAJECTORY_FOLLOWER_PUBLIC TrajectoryPoint lerpTrajectoryPoint( - const T & points, const Pose & pose, const float64_t & max_dist, const float64_t & max_yaw) + const T & points, const Pose & pose, const float64_t max_dist, const float64_t max_yaw) { TrajectoryPoint interpolated_point; auto seg_idx = tier4_autoware_utils::findNearestSegmentIndex(points, pose, max_dist, max_yaw); diff --git a/control/trajectory_follower/src/longitudinal_controller_utils.cpp b/control/trajectory_follower/src/longitudinal_controller_utils.cpp index e18872a9b1d18..db9370ba8dabf 100644 --- a/control/trajectory_follower/src/longitudinal_controller_utils.cpp +++ b/control/trajectory_follower/src/longitudinal_controller_utils.cpp @@ -63,8 +63,8 @@ bool isValidTrajectory(const Trajectory & traj) } float64_t calcStopDistance( - const Pose & current_pose, const Trajectory & traj, const float64_t & max_dist, - const float64_t & max_yaw) + const Pose & current_pose, const Trajectory & traj, const float64_t max_dist, + const float64_t max_yaw) { const std::experimental::optional stop_idx_opt = trajectory_common::searchZeroVelocityIndex(traj.points);