diff --git a/control/mpc_lateral_controller/param/lateral_controller_defaults.param.yaml b/control/mpc_lateral_controller/param/lateral_controller_defaults.param.yaml index 182458d532c8a..c0382ca2ccb6b 100644 --- a/control/mpc_lateral_controller/param/lateral_controller_defaults.param.yaml +++ b/control/mpc_lateral_controller/param/lateral_controller_defaults.param.yaml @@ -1,58 +1,58 @@ /**: ros__parameters: # -- system -- - traj_resample_dist: 0.1 # path resampling interval [m] - use_steer_prediction: false # flag for using steer prediction (do not use steer measurement) - admissible_position_error: 5.0 # stop mpc calculation when error is larger than the following value - admissible_yaw_error_rad: 1.57 # stop mpc calculation when error is larger than the following value + traj_resample_dist: 0.1 # path resampling interval [m] + use_steer_prediction: false # flag for using steer prediction (do not use steer measurement) + admissible_position_error: 5.0 # stop mpc calculation when error is larger than the following value + admissible_yaw_error_rad: 1.57 # stop mpc calculation when error is larger than the following value # -- path smoothing -- - enable_path_smoothing: false # flag for path smoothing + enable_path_smoothing: false # flag for path smoothing path_filter_moving_ave_num: 25 # param of moving average filter for path smoothing - curvature_smoothing_num_traj: 15 # point-to-point index distance used in curvature calculation (for trajectory): curvature is calculated from three points p(i-num), p(i), p(i+num) - curvature_smoothing_num_ref_steer: 15 # point-to-point index distance used in curvature calculation (for steer command reference): curvature is calculated from three points p(i-num), p(i), p(i+num) + curvature_smoothing_num_traj: 15 # point-to-point index distance used in curvature calculation (for trajectory): curvature is calculated from three points p(i-num), p(i), p(i+num) + curvature_smoothing_num_ref_steer: 15 # point-to-point index distance used in curvature calculation (for steer command reference): curvature is calculated from three points p(i-num), p(i), p(i+num) # -- trajectory extending -- - extend_trajectory_for_end_yaw_control: true # flag of trajectory extending for terminal yaw control + extend_trajectory_for_end_yaw_control: false # flag of trajectory extending for terminal yaw control # -- mpc optimization -- - qp_solver_type: "osqp" # optimization solver option (unconstraint_fast or osqp) - mpc_prediction_horizon: 50 # prediction horizon step - mpc_prediction_dt: 0.1 # prediction horizon period [s] - mpc_weight_lat_error: 0.1 # lateral error weight in matrix Q - mpc_weight_heading_error: 0.0 # heading error weight in matrix Q - mpc_weight_heading_error_squared_vel: 0.3 # heading error * velocity weight in matrix Q - mpc_weight_steering_input: 1.0 # steering error weight in matrix R - mpc_weight_steering_input_squared_vel: 0.25 # steering error * velocity weight in matrix R - mpc_weight_lat_jerk: 0.0 # lateral jerk weight in matrix R - mpc_weight_steer_rate: 0.0 # steering rate weight in matrix R - mpc_weight_steer_acc: 0.000001 # steering angular acceleration weight in matrix R - mpc_low_curvature_weight_lat_error: 0.1 # lateral error weight in matrix Q in low curvature point - mpc_low_curvature_weight_heading_error: 0.0 # heading error weight in matrix Q in low curvature point - mpc_low_curvature_weight_heading_error_squared_vel: 0.3 # heading error * velocity weight in matrix Q in low curvature point - mpc_low_curvature_weight_steering_input: 1.0 # steering error weight in matrix R in low curvature point - mpc_low_curvature_weight_steering_input_squared_vel: 0.25 # steering error * velocity weight in matrix R in low curvature point - mpc_low_curvature_weight_lat_jerk: 0.0 # lateral jerk weight in matrix R in low curvature point - mpc_low_curvature_weight_steer_rate: 0.0 # steering rate weight in matrix R in low curvature point - mpc_low_curvature_weight_steer_acc: 0.000001 # steering angular acceleration weight in matrix R in low curvature point - mpc_low_curvature_thresh_curvature: 0.0 # threshold of curvature to use "low_curvature" parameter (recommend: 0.01~0.03) - mpc_weight_terminal_lat_error: 1.0 # terminal lateral error weight in matrix Q to improve mpc stability - mpc_weight_terminal_heading_error: 0.1 # terminal heading error weight in matrix Q to improve mpc stability - mpc_zero_ff_steer_deg: 0.5 # threshold that feed-forward angle becomes zero - mpc_acceleration_limit: 2.0 # limit on the vehicle's acceleration - mpc_velocity_time_constant: 0.3 # time constant used for velocity smoothing - mpc_min_prediction_length: 5.0 # minimum prediction length + qp_solver_type: "osqp" # optimization solver option (unconstraint_fast or osqp) + mpc_prediction_horizon: 50 # prediction horizon step + mpc_prediction_dt: 0.1 # prediction horizon period [s] + mpc_weight_lat_error: 1.0 # lateral error weight in matrix Q + mpc_weight_heading_error: 0.0 # heading error weight in matrix Q + mpc_weight_heading_error_squared_vel: 0.3 # heading error * velocity weight in matrix Q + mpc_weight_steering_input: 1.0 # steering error weight in matrix R + mpc_weight_steering_input_squared_vel: 0.25 # steering error * velocity weight in matrix R + mpc_weight_lat_jerk: 0.1 # lateral jerk weight in matrix R + mpc_weight_steer_rate: 0.0 # steering rate weight in matrix R + mpc_weight_steer_acc: 0.000001 # steering angular acceleration weight in matrix R + mpc_low_curvature_weight_lat_error: 0.1 # lateral error weight in matrix Q in low curvature point + mpc_low_curvature_weight_heading_error: 0.0 # heading error weight in matrix Q in low curvature point + mpc_low_curvature_weight_heading_error_squared_vel: 0.3 # heading error * velocity weight in matrix Q in low curvature point + mpc_low_curvature_weight_steering_input: 1.0 # steering error weight in matrix R in low curvature point + mpc_low_curvature_weight_steering_input_squared_vel: 0.25 # steering error * velocity weight in matrix R in low curvature point + mpc_low_curvature_weight_lat_jerk: 0.0 # lateral jerk weight in matrix R in low curvature point + mpc_low_curvature_weight_steer_rate: 0.0 # steering rate weight in matrix R in low curvature point + mpc_low_curvature_weight_steer_acc: 0.000001 # steering angular acceleration weight in matrix R in low curvature point + mpc_low_curvature_thresh_curvature: 0.0 # threshold of curvature to use "low_curvature" parameter (recommend: 0.01~0.03) + mpc_weight_terminal_lat_error: 1.0 # terminal lateral error weight in matrix Q to improve mpc stability + mpc_weight_terminal_heading_error: 0.1 # terminal heading error weight in matrix Q to improve mpc stability + mpc_zero_ff_steer_deg: 0.5 # threshold that feed-forward angle becomes zero + mpc_acceleration_limit: 2.0 # limit on the vehicle's acceleration + mpc_velocity_time_constant: 0.3 # time constant used for velocity smoothing + mpc_min_prediction_length: 5.0 # minimum prediction length # -- vehicle model -- vehicle_model_type: "kinematics" # vehicle model type for mpc prediction. option is kinematics, kinematics_no_delay, and dynamics - input_delay: 0.24 # steering input delay time for delay compensation - vehicle_model_steer_tau: 0.3 # steering dynamics time constant (1d approximation) [s] - steer_rate_lim_dps_list_by_curvature: [10.0, 20.0, 30.0] # steering angle rate limit list depending on curvature [deg/s] + input_delay: 0.24 # steering input delay time for delay compensation + vehicle_model_steer_tau: 0.3 # steering dynamics time constant (1d approximation) [s] + steer_rate_lim_dps_list_by_curvature: [40.0, 50.0, 60.0] # steering angle rate limit list depending on curvature [deg/s] curvature_list_for_steer_rate_lim: [0.001, 0.002, 0.01] # curvature list for steering angle rate limit interpolation in ascending order [/m] - steer_rate_lim_dps_list_by_velocity: [40.0, 30.0, 20.0] # steering angle rate limit list depending on velocity [deg/s] + steer_rate_lim_dps_list_by_velocity: [60.0, 50.0, 40.0] # steering angle rate limit list depending on velocity [deg/s] velocity_list_for_steer_rate_lim: [10.0, 15.0, 20.0] # velocity list for steering angle rate limit interpolation in ascending order [m/s] - acceleration_limit: 2.0 # acceleration limit for trajectory velocity modification [m/ss] - velocity_time_constant: 0.3 # velocity dynamics time constant for trajectory velocity modification [s] + acceleration_limit: 2.0 # acceleration limit for trajectory velocity modification [m/ss] + velocity_time_constant: 0.3 # velocity dynamics time constant for trajectory velocity modification [s] # -- lowpass filter for noise reduction -- steering_lpf_cutoff_hz: 3.0 # cutoff frequency of lowpass filter for steering command [Hz] @@ -65,7 +65,7 @@ keep_steer_control_until_converged: true new_traj_duration_time: 1.0 new_traj_end_dist: 0.3 - mpc_converged_threshold_rps: 0.01 # threshold of mpc convergence check [rad/s] + mpc_converged_threshold_rps: 0.01 # threshold of mpc convergence check [rad/s] # steer offset steering_offset: diff --git a/control/pid_longitudinal_controller/param/longitudinal_controller_defaults.param.yaml b/control/pid_longitudinal_controller/param/longitudinal_controller_defaults.param.yaml index e95428096c17c..ad6217663fbae 100644 --- a/control/pid_longitudinal_controller/param/longitudinal_controller_defaults.param.yaml +++ b/control/pid_longitudinal_controller/param/longitudinal_controller_defaults.param.yaml @@ -5,19 +5,19 @@ enable_smooth_stop: true enable_overshoot_emergency: true enable_large_tracking_error_emergency: true - enable_slope_compensation: false + enable_slope_compensation: true enable_keep_stopped_until_steer_convergence: true # state transition drive_state_stop_dist: 0.5 drive_state_offset_stop_dist: 1.0 - stopping_state_stop_dist: 0.49 + stopping_state_stop_dist: 0.5 stopped_state_entry_duration_time: 0.1 - stopped_state_entry_vel: 0.1 + stopped_state_entry_vel: 0.01 stopped_state_entry_acc: 0.1 emergency_state_overshoot_stop_dist: 1.5 emergency_state_traj_trans_dev: 3.0 - emergency_state_traj_rot_dev: 0.7 + emergency_state_traj_rot_dev: 0.7854 # drive state kp: 1.0 @@ -40,7 +40,7 @@ # smooth stop state smooth_stop_max_strong_acc: -0.5 - smooth_stop_min_strong_acc: -1.0 + smooth_stop_min_strong_acc: -0.8 smooth_stop_weak_acc: -0.3 smooth_stop_weak_stop_acc: -0.8 smooth_stop_strong_stop_acc: -3.4 diff --git a/control/trajectory_follower_node/test/test_controller_node.cpp b/control/trajectory_follower_node/test/test_controller_node.cpp index 6be3625501590..1aa4035e98d51 100644 --- a/control/trajectory_follower_node/test/test_controller_node.cpp +++ b/control/trajectory_follower_node/test/test_controller_node.cpp @@ -56,8 +56,6 @@ rclcpp::NodeOptions makeNodeOptions(const bool enable_keep_stopped_until_steer_c const auto lateral_share_dir = ament_index_cpp::get_package_share_directory("mpc_lateral_controller"); rclcpp::NodeOptions node_options; - node_options.append_parameter_override("ctrl_period", 0.03); - node_options.append_parameter_override("timeout_thr_sec", 0.5); node_options.append_parameter_override("lateral_controller_mode", "mpc"); node_options.append_parameter_override("longitudinal_controller_mode", "pid"); node_options.append_parameter_override( @@ -254,8 +252,10 @@ TEST_F(FakeNodeFixture, straight_trajectory) test_utils::waitForMessage(tester.node, this, tester.received_control_command); ASSERT_TRUE(tester.received_control_command); + // following conditions will pass even if the MPC solution does not converge EXPECT_EQ(tester.cmd_msg->lateral.steering_tire_angle, 0.0f); EXPECT_EQ(tester.cmd_msg->lateral.steering_tire_rotation_rate, 0.0f); + EXPECT_GT(tester.cmd_msg->longitudinal.speed, 0.0f); EXPECT_GT(rclcpp::Time(tester.cmd_msg->stamp), rclcpp::Time(traj_msg.header.stamp)); } diff --git a/control/trajectory_follower_node/test/test_lateral_controller_node.cpp b/control/trajectory_follower_node/test/test_lateral_controller_node.cpp deleted file mode 100644 index 0c0a1f84ab1c2..0000000000000 --- a/control/trajectory_follower_node/test/test_lateral_controller_node.cpp +++ /dev/null @@ -1,436 +0,0 @@ -// Copyright 2021 The Autoware Foundation -// -// 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 "ament_index_cpp/get_package_share_directory.hpp" -#include "fake_test_node/fake_test_node.hpp" -#include "gtest/gtest.h" -#include "rclcpp/rclcpp.hpp" -#include "rclcpp/time.hpp" -#include "trajectory_follower_test_utils.hpp" - -#include - -#include "autoware_auto_control_msgs/msg/ackermann_lateral_command.hpp" -#include "autoware_auto_planning_msgs/msg/trajectory.hpp" -#include "autoware_auto_vehicle_msgs/msg/steering_report.hpp" -#include "autoware_auto_vehicle_msgs/msg/vehicle_odometry.hpp" -#include "geometry_msgs/msg/pose.hpp" -#include "geometry_msgs/msg/pose_stamped.hpp" -#include "geometry_msgs/msg/transform_stamped.hpp" -#include "nav_msgs/msg/odometry.hpp" - -#include -#include - -using LateralController = autoware::motion::control::trajectory_follower_node::LateralController; -using LateralCommand = autoware_auto_control_msgs::msg::AckermannLateralCommand; -using Trajectory = autoware_auto_planning_msgs::msg::Trajectory; -using TrajectoryPoint = autoware_auto_planning_msgs::msg::TrajectoryPoint; -using VehicleOdometry = nav_msgs::msg::Odometry; -using SteeringReport = autoware_auto_vehicle_msgs::msg::SteeringReport; - -using FakeNodeFixture = autoware::tools::testing::FakeTestNode; - -const rclcpp::Duration one_second(1, 0); - -std::shared_ptr makeLateralNode() -{ - // Pass default parameter file to the node - const auto share_dir = ament_index_cpp::get_package_share_directory("trajectory_follower_node"); - rclcpp::NodeOptions node_options; - node_options.arguments( - {"--ros-args", "--params-file", share_dir + "/param/lateral_controller_defaults.param.yaml", - "--params-file", share_dir + "/param/test_vehicle_info.param.yaml", "--params-file", - share_dir + "/param/test_nearest_search.param.yaml"}); - std::shared_ptr node = std::make_shared(node_options); - - // Enable all logging in the node - auto ret = - rcutils_logging_set_logger_level(node->get_logger().get_name(), RCUTILS_LOG_SEVERITY_DEBUG); - if (ret != RCUTILS_RET_OK) { - std::cout << "Failed to set logging severity to DEBUG\n"; - } - return node; -} - -TEST_F(FakeNodeFixture, no_input) -{ - // Data to test - LateralCommand::SharedPtr cmd_msg; - bool received_lateral_command = false; - // Node - std::shared_ptr node = makeLateralNode(); - // Publisher/Subscribers - rclcpp::Publisher::SharedPtr traj_pub = - this->create_publisher("lateral_controller/input/reference_trajectory"); - rclcpp::Publisher::SharedPtr odom_pub = - this->create_publisher("lateral_controller/input/current_odometry"); - rclcpp::Publisher::SharedPtr steer_pub = - this->create_publisher("lateral_controller/input/current_steering"); - rclcpp::Subscription::SharedPtr cmd_sub = - this->create_subscription( - "lateral_controller/output/control_cmd", *this->get_fake_node(), - [&cmd_msg, &received_lateral_command](const LateralCommand::SharedPtr msg) { - cmd_msg = msg; - received_lateral_command = true; - }); - std::shared_ptr br = - std::make_shared(this->get_fake_node()); - - // No published data: expect a stopped command - test_utils::waitForMessage( - node, this, received_lateral_command, std::chrono::seconds{1LL}, false); - ASSERT_FALSE(received_lateral_command); -} - -TEST_F(FakeNodeFixture, empty_trajectory) -{ - // Data to test - LateralCommand::SharedPtr cmd_msg; - bool received_lateral_command = false; - // Node - std::shared_ptr node = makeLateralNode(); - // Publisher/Subscribers - rclcpp::Publisher::SharedPtr traj_pub = - this->create_publisher("lateral_controller/input/reference_trajectory"); - rclcpp::Publisher::SharedPtr odom_pub = - this->create_publisher("lateral_controller/input/current_odometry"); - rclcpp::Publisher::SharedPtr steer_pub = - this->create_publisher("lateral_controller/input/current_steering"); - rclcpp::Subscription::SharedPtr cmd_sub = - this->create_subscription( - "lateral_controller/output/control_cmd", *this->get_fake_node(), - [&cmd_msg, &received_lateral_command](const LateralCommand::SharedPtr msg) { - cmd_msg = msg; - received_lateral_command = true; - }); - std::shared_ptr br = - std::make_shared(this->get_fake_node()); - - // Dummy transform: ego is at (0.0, 0.0) in map frame - geometry_msgs::msg::TransformStamped transform = test_utils::getDummyTransform(); - transform.header.stamp = node->now(); - br->sendTransform(transform); - - // Spin for transform to be published - test_utils::spinWhile(node); - - // Empty trajectory: expect a stopped command - Trajectory traj_msg; - traj_msg.header.stamp = node->now(); - traj_msg.header.frame_id = "map"; - VehicleOdometry odom_msg; - SteeringReport steer_msg; - traj_msg.header.stamp = node->now(); - odom_msg.header.stamp = node->now(); - odom_msg.twist.twist.linear.x = 0.0; - steer_msg.stamp = node->now(); - steer_msg.steering_tire_angle = 0.0; - traj_pub->publish(traj_msg); - odom_pub->publish(odom_msg); - steer_pub->publish(steer_msg); - - test_utils::waitForMessage( - node, this, received_lateral_command, std::chrono::seconds{1LL}, false); - ASSERT_FALSE(received_lateral_command); -} - -TEST_F(FakeNodeFixture, straight_trajectory) -{ - // Data to test - LateralCommand::SharedPtr cmd_msg; - bool received_lateral_command = false; - // Node - std::shared_ptr node = makeLateralNode(); - // Publisher/Subscribers - rclcpp::Publisher::SharedPtr traj_pub = - this->create_publisher("lateral_controller/input/reference_trajectory"); - rclcpp::Publisher::SharedPtr odom_pub = - this->create_publisher("lateral_controller/input/current_odometry"); - rclcpp::Publisher::SharedPtr steer_pub = - this->create_publisher("lateral_controller/input/current_steering"); - rclcpp::Subscription::SharedPtr cmd_sub = - this->create_subscription( - "lateral_controller/output/control_cmd", *this->get_fake_node(), - [&cmd_msg, &received_lateral_command](const LateralCommand::SharedPtr msg) { - cmd_msg = msg; - received_lateral_command = true; - }); - std::shared_ptr br = - std::make_shared(this->get_fake_node()); - - // Dummy transform: ego is at (0.0, 0.0) in map frame - geometry_msgs::msg::TransformStamped transform = test_utils::getDummyTransform(); - transform.header.stamp = node->now(); - br->sendTransform(transform); - - // Spin for transform to be published - test_utils::spinWhile(node); - - // Straight trajectory: expect no steering - received_lateral_command = false; - Trajectory traj_msg; - traj_msg.header.stamp = node->now(); - traj_msg.header.frame_id = "map"; - VehicleOdometry odom_msg; - SteeringReport steer_msg; - TrajectoryPoint p; - traj_msg.header.stamp = node->now(); - p.pose.position.x = -1.0; - p.pose.position.y = 0.0; - p.longitudinal_velocity_mps = 1.0f; - traj_msg.points.push_back(p); - p.pose.position.x = 0.0; - p.pose.position.y = 0.0; - p.longitudinal_velocity_mps = 1.0f; - traj_msg.points.push_back(p); - p.pose.position.x = 1.0; - p.pose.position.y = 0.0; - p.longitudinal_velocity_mps = 1.0f; - traj_msg.points.push_back(p); - p.pose.position.x = 2.0; - p.pose.position.y = 0.0; - p.longitudinal_velocity_mps = 1.0f; - traj_msg.points.push_back(p); - traj_pub->publish(traj_msg); - odom_msg.header.stamp = node->now(); - odom_msg.twist.twist.linear.x = 1.0; - steer_msg.stamp = node->now(); - steer_msg.steering_tire_angle = 0.0; - odom_pub->publish(odom_msg); - steer_pub->publish(steer_msg); - - test_utils::waitForMessage(node, this, received_lateral_command); - ASSERT_TRUE(received_lateral_command); - EXPECT_EQ(cmd_msg->steering_tire_angle, 0.0f); - EXPECT_EQ(cmd_msg->steering_tire_rotation_rate, 0.0f); - EXPECT_GT(rclcpp::Time(cmd_msg->stamp), rclcpp::Time(traj_msg.header.stamp)); -} - -TEST_F(FakeNodeFixture, right_turn) -{ - // Data to test - LateralCommand::SharedPtr cmd_msg; - bool received_lateral_command = false; - // Node - std::shared_ptr node = makeLateralNode(); - // Publisher/Subscribers - rclcpp::Publisher::SharedPtr traj_pub = - this->create_publisher("lateral_controller/input/reference_trajectory"); - rclcpp::Publisher::SharedPtr odom_pub = - this->create_publisher("lateral_controller/input/current_odometry"); - rclcpp::Publisher::SharedPtr steer_pub = - this->create_publisher("lateral_controller/input/current_steering"); - rclcpp::Subscription::SharedPtr cmd_sub = - this->create_subscription( - "lateral_controller/output/control_cmd", *this->get_fake_node(), - [&cmd_msg, &received_lateral_command](const LateralCommand::SharedPtr msg) { - cmd_msg = msg; - received_lateral_command = true; - }); - std::shared_ptr br = - std::make_shared(this->get_fake_node()); - - // Dummy transform: ego is at (0.0, 0.0) in map frame - geometry_msgs::msg::TransformStamped transform = test_utils::getDummyTransform(); - transform.header.stamp = node->now(); - br->sendTransform(transform); - - // Spin for transform to be published - test_utils::spinWhile(node); - - // Right turning trajectory: expect right steering - received_lateral_command = false; - Trajectory traj_msg; - traj_msg.header.stamp = node->now(); - traj_msg.header.frame_id = "map"; - VehicleOdometry odom_msg; - SteeringReport steer_msg; - TrajectoryPoint p; - traj_msg.points.clear(); - p.pose.position.x = -1.0; - p.pose.position.y = -1.0; - p.longitudinal_velocity_mps = 1.0f; - traj_msg.points.push_back(p); - p.pose.position.x = 0.0; - p.pose.position.y = 0.0; - p.longitudinal_velocity_mps = 1.0f; - traj_msg.points.push_back(p); - p.pose.position.x = 1.0; - p.pose.position.y = -1.0; - p.longitudinal_velocity_mps = 1.0f; - traj_msg.points.push_back(p); - p.pose.position.x = 2.0; - p.pose.position.y = -2.0; - p.longitudinal_velocity_mps = 1.0f; - traj_msg.points.push_back(p); - traj_pub->publish(traj_msg); - odom_msg.header.stamp = node->now(); - odom_msg.twist.twist.linear.x = 1.0; - steer_msg.stamp = node->now(); - steer_msg.steering_tire_angle = 0.0; - odom_pub->publish(odom_msg); - steer_pub->publish(steer_msg); - - test_utils::waitForMessage(node, this, received_lateral_command); - ASSERT_TRUE(received_lateral_command); - EXPECT_LT(cmd_msg->steering_tire_angle, 0.0f); - EXPECT_LT(cmd_msg->steering_tire_rotation_rate, 0.0f); - EXPECT_GT(rclcpp::Time(cmd_msg->stamp), rclcpp::Time(traj_msg.header.stamp)); -} - -TEST_F(FakeNodeFixture, left_turn) -{ - // Data to test - LateralCommand::SharedPtr cmd_msg; - bool received_lateral_command = false; - // Node - std::shared_ptr node = makeLateralNode(); - // Publisher/Subscribers - rclcpp::Publisher::SharedPtr traj_pub = - this->create_publisher("lateral_controller/input/reference_trajectory"); - rclcpp::Publisher::SharedPtr odom_pub = - this->create_publisher("lateral_controller/input/current_odometry"); - rclcpp::Publisher::SharedPtr steer_pub = - this->create_publisher("lateral_controller/input/current_steering"); - rclcpp::Subscription::SharedPtr cmd_sub = - this->create_subscription( - "lateral_controller/output/control_cmd", *this->get_fake_node(), - [&cmd_msg, &received_lateral_command](const LateralCommand::SharedPtr msg) { - cmd_msg = msg; - received_lateral_command = true; - }); - std::shared_ptr br = - std::make_shared(this->get_fake_node()); - - // Dummy transform: ego is at (0.0, 0.0) in map frame - geometry_msgs::msg::TransformStamped transform = test_utils::getDummyTransform(); - transform.header.stamp = node->now(); - br->sendTransform(transform); - - // Spin for transform to be published - test_utils::spinWhile(node); - - // Left turning trajectory: expect left steering - received_lateral_command = false; - Trajectory traj_msg; - traj_msg.header.stamp = node->now(); - traj_msg.header.frame_id = "map"; - VehicleOdometry odom_msg; - SteeringReport steer_msg; - TrajectoryPoint p; - traj_msg.points.clear(); - p.pose.position.x = -1.0; - p.pose.position.y = 1.0; - p.longitudinal_velocity_mps = 1.0f; - traj_msg.points.push_back(p); - p.pose.position.x = 0.0; - p.pose.position.y = 0.0; - p.longitudinal_velocity_mps = 1.0f; - traj_msg.points.push_back(p); - p.pose.position.x = 1.0; - p.pose.position.y = 1.0; - p.longitudinal_velocity_mps = 1.0f; - traj_msg.points.push_back(p); - p.pose.position.x = 2.0; - p.pose.position.y = 2.0; - p.longitudinal_velocity_mps = 1.0f; - traj_msg.points.push_back(p); - traj_pub->publish(traj_msg); - odom_msg.header.stamp = node->now(); - odom_msg.twist.twist.linear.x = 1.0; - steer_msg.stamp = node->now(); - steer_msg.steering_tire_angle = 0.0; - odom_pub->publish(odom_msg); - steer_pub->publish(steer_msg); - - test_utils::waitForMessage(node, this, received_lateral_command); - ASSERT_TRUE(received_lateral_command); - EXPECT_GT(cmd_msg->steering_tire_angle, 0.0f); - EXPECT_GT(cmd_msg->steering_tire_rotation_rate, 0.0f); - EXPECT_GT(rclcpp::Time(cmd_msg->stamp), rclcpp::Time(traj_msg.header.stamp)); -} - -TEST_F(FakeNodeFixture, stopped) -{ - // Data to test - LateralCommand::SharedPtr cmd_msg; - bool received_lateral_command = false; - // Node - std::shared_ptr node = makeLateralNode(); - // Publisher/Subscribers - rclcpp::Publisher::SharedPtr traj_pub = - this->create_publisher("lateral_controller/input/reference_trajectory"); - rclcpp::Publisher::SharedPtr odom_pub = - this->create_publisher("lateral_controller/input/current_odometry"); - rclcpp::Publisher::SharedPtr steer_pub = - this->create_publisher("lateral_controller/input/current_steering"); - rclcpp::Subscription::SharedPtr cmd_sub = - this->create_subscription( - "lateral_controller/output/control_cmd", *this->get_fake_node(), - [&cmd_msg, &received_lateral_command](const LateralCommand::SharedPtr msg) { - cmd_msg = msg; - received_lateral_command = true; - }); - std::shared_ptr br = - std::make_shared(this->get_fake_node()); - - // Dummy transform: ego is at (0.0, 0.0) in map frame - geometry_msgs::msg::TransformStamped transform = test_utils::getDummyTransform(); - transform.header.stamp = node->now(); - br->sendTransform(transform); - - // Spin for transform to be published - test_utils::spinWhile(node); - - // Straight trajectory: expect no steering - received_lateral_command = false; - Trajectory traj_msg; - traj_msg.header.stamp = node->now(); - traj_msg.header.frame_id = "map"; - VehicleOdometry odom_msg; - SteeringReport steer_msg; - TrajectoryPoint p; - traj_msg.header.stamp = node->now(); - p.pose.position.x = -1.0; - p.pose.position.y = 0.0; - // Set a 0 current velocity and 0 target velocity -> stopped state - p.longitudinal_velocity_mps = 0.0f; - traj_msg.points.push_back(p); - p.pose.position.x = 0.0; - p.pose.position.y = 0.0; - p.longitudinal_velocity_mps = 0.0f; - traj_msg.points.push_back(p); - p.pose.position.x = 1.0; - p.pose.position.y = 0.0; - p.longitudinal_velocity_mps = 0.0f; - traj_msg.points.push_back(p); - p.pose.position.x = 2.0; - p.pose.position.y = 0.0; - p.longitudinal_velocity_mps = 0.0f; - traj_msg.points.push_back(p); - traj_pub->publish(traj_msg); - odom_msg.header.stamp = node->now(); - odom_msg.twist.twist.linear.x = 0.0; - steer_msg.stamp = node->now(); - steer_msg.steering_tire_angle = -0.5; - odom_pub->publish(odom_msg); - steer_pub->publish(steer_msg); - - test_utils::waitForMessage(node, this, received_lateral_command); - ASSERT_TRUE(received_lateral_command); - EXPECT_EQ(cmd_msg->steering_tire_angle, steer_msg.steering_tire_angle); - EXPECT_EQ(cmd_msg->steering_tire_rotation_rate, 0.0f); - EXPECT_GT(rclcpp::Time(cmd_msg->stamp), rclcpp::Time(traj_msg.header.stamp)); -} diff --git a/control/trajectory_follower_node/test/test_longitudinal_controller_node.cpp b/control/trajectory_follower_node/test/test_longitudinal_controller_node.cpp deleted file mode 100644 index badafd5f2fd15..0000000000000 --- a/control/trajectory_follower_node/test/test_longitudinal_controller_node.cpp +++ /dev/null @@ -1,472 +0,0 @@ -// Copyright 2021 Tier IV, Inc. -// -// 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 "ament_index_cpp/get_package_share_directory.hpp" -#include "fake_test_node/fake_test_node.hpp" -#include "gtest/gtest.h" -#include "rclcpp/rclcpp.hpp" -#include "rclcpp/time.hpp" -#include "trajectory_follower_test_utils.hpp" - -#include - -#include "autoware_auto_control_msgs/msg/longitudinal_command.hpp" -#include "autoware_auto_planning_msgs/msg/trajectory.hpp" -#include "autoware_auto_vehicle_msgs/msg/vehicle_odometry.hpp" -#include "geometry_msgs/msg/pose.hpp" -#include "geometry_msgs/msg/pose_stamped.hpp" -#include "geometry_msgs/msg/transform_stamped.hpp" -#include "nav_msgs/msg/odometry.hpp" - -#include - -using LongitudinalController = - autoware::motion::control::trajectory_follower_node::LongitudinalController; -using LongitudinalCommand = autoware_auto_control_msgs::msg::LongitudinalCommand; -using Trajectory = autoware_auto_planning_msgs::msg::Trajectory; -using TrajectoryPoint = autoware_auto_planning_msgs::msg::TrajectoryPoint; -using VehicleOdometry = nav_msgs::msg::Odometry; - -using FakeNodeFixture = autoware::tools::testing::FakeTestNode; - -std::shared_ptr makeLongitudinalNode() -{ - // Pass default parameter file to the node - const auto share_dir = ament_index_cpp::get_package_share_directory("trajectory_follower_node"); - rclcpp::NodeOptions node_options; - node_options.arguments( - {"--ros-args", "--params-file", - share_dir + "/param/longitudinal_controller_defaults.param.yaml", "--params-file", - share_dir + "/param/test_vehicle_info.param.yaml", "--params-file", - share_dir + "/param/test_nearest_search.param.yaml"}); - std::shared_ptr node = - std::make_shared(node_options); - - // Enable all logging in the node - auto ret = - rcutils_logging_set_logger_level(node->get_logger().get_name(), RCUTILS_LOG_SEVERITY_DEBUG); - if (ret != RCUTILS_RET_OK) { - std::cout << "Failed to set logging severity to DEBUG\n"; - } - return node; -} - -TEST_F(FakeNodeFixture, longitudinal_keep_velocity) -{ - // Data to test - LongitudinalCommand::SharedPtr cmd_msg; - bool received_longitudinal_command = false; - - // Node - std::shared_ptr node = makeLongitudinalNode(); - - // Publisher/Subscribers - rclcpp::Publisher::SharedPtr odom_pub = - this->create_publisher("longitudinal_controller/input/current_odometry"); - rclcpp::Publisher::SharedPtr traj_pub = - this->create_publisher("longitudinal_controller/input/current_trajectory"); - rclcpp::Subscription::SharedPtr cmd_sub = - this->create_subscription( - "longitudinal_controller/output/control_cmd", *this->get_fake_node(), - [&cmd_msg, &received_longitudinal_command](const LongitudinalCommand::SharedPtr msg) { - cmd_msg = msg; - received_longitudinal_command = true; - }); - std::shared_ptr br = - std::make_shared(this->get_fake_node()); - - // Dummy transform: ego is at (0.0, 0.0) in map frame - geometry_msgs::msg::TransformStamped transform = test_utils::getDummyTransform(); - transform.header.stamp = node->now(); - br->sendTransform(transform); - /// Already running at target vel + Non stopping trajectory -> no change in velocity - // Publish velocity - VehicleOdometry odom_msg; - odom_msg.header.stamp = node->now(); - odom_msg.twist.twist.linear.x = 1.0; - odom_pub->publish(odom_msg); - // the node needs to receive two velocity msg - rclcpp::spin_some(node); - rclcpp::spin_some(this->get_fake_node()); - odom_msg.header.stamp = node->now(); - odom_pub->publish(odom_msg); - // Publish non stopping trajectory - Trajectory traj; - traj.header.stamp = node->now(); - traj.header.frame_id = "map"; - TrajectoryPoint point; - point.pose.position.x = 0.0; - point.pose.position.y = 0.0; - point.longitudinal_velocity_mps = 1.0; - traj.points.push_back(point); - point.pose.position.x = 50.0; - point.pose.position.y = 0.0; - point.longitudinal_velocity_mps = 1.0; - traj.points.push_back(point); - point.pose.position.x = 100.0; - point.pose.position.y = 0.0; - point.longitudinal_velocity_mps = 1.0; - traj.points.push_back(point); - traj_pub->publish(traj); - test_utils::waitForMessage(node, this, received_longitudinal_command); - - ASSERT_TRUE(received_longitudinal_command); - EXPECT_DOUBLE_EQ(cmd_msg->speed, 1.0); - EXPECT_DOUBLE_EQ(cmd_msg->acceleration, 0.0); - - // Generate another control message - received_longitudinal_command = false; - traj_pub->publish(traj); - test_utils::waitForMessage(node, this, received_longitudinal_command); - ASSERT_TRUE(received_longitudinal_command); - EXPECT_DOUBLE_EQ(cmd_msg->speed, 1.0); - EXPECT_DOUBLE_EQ(cmd_msg->acceleration, 0.0); -} - -TEST_F(FakeNodeFixture, longitudinal_slow_down) -{ - // Data to test - LongitudinalCommand::SharedPtr cmd_msg; - bool received_longitudinal_command = false; - - // Node - std::shared_ptr node = makeLongitudinalNode(); - - // Publisher/Subscribers - rclcpp::Publisher::SharedPtr odom_pub = - this->create_publisher("longitudinal_controller/input/current_odometry"); - rclcpp::Publisher::SharedPtr traj_pub = - this->create_publisher("longitudinal_controller/input/current_trajectory"); - rclcpp::Subscription::SharedPtr cmd_sub = - this->create_subscription( - "longitudinal_controller/output/control_cmd", *this->get_fake_node(), - [&cmd_msg, &received_longitudinal_command](const LongitudinalCommand::SharedPtr msg) { - cmd_msg = msg; - received_longitudinal_command = true; - }); - std::shared_ptr br = - std::make_shared(this->get_fake_node()); - - // Dummy transform: ego is at (0.0, 0.0) in map frame - geometry_msgs::msg::TransformStamped transform = test_utils::getDummyTransform(); - transform.header.stamp = node->now(); - br->sendTransform(transform); - /// Already running at target vel + Non stopping trajectory -> no change in velocity - // Publish velocity - VehicleOdometry odom_msg; - odom_msg.header.stamp = node->now(); - odom_msg.twist.twist.linear.x = 1.0; - odom_pub->publish(odom_msg); - // the node needs to receive two velocity msg - rclcpp::spin_some(node); - rclcpp::spin_some(this->get_fake_node()); - odom_msg.header.stamp = node->now(); - odom_pub->publish(odom_msg); - // Publish non stopping trajectory - Trajectory traj; - traj.header.stamp = node->now(); - traj.header.frame_id = "map"; - TrajectoryPoint point; - point.pose.position.x = 0.0; - point.pose.position.y = 0.0; - point.longitudinal_velocity_mps = 0.5; - traj.points.push_back(point); - point.pose.position.x = 50.0; - point.pose.position.y = 0.0; - point.longitudinal_velocity_mps = 0.5; - traj.points.push_back(point); - point.pose.position.x = 100.0; - point.pose.position.y = 0.0; - point.longitudinal_velocity_mps = 0.5; - traj.points.push_back(point); - traj_pub->publish(traj); - test_utils::waitForMessage(node, this, received_longitudinal_command); - - ASSERT_TRUE(received_longitudinal_command); - EXPECT_LT(cmd_msg->speed, static_cast(odom_msg.twist.twist.linear.x)); - EXPECT_LT(cmd_msg->acceleration, 0.0f); - - // Generate another control message - received_longitudinal_command = false; - traj_pub->publish(traj); - test_utils::waitForMessage(node, this, received_longitudinal_command); - ASSERT_TRUE(received_longitudinal_command); - EXPECT_LT(cmd_msg->speed, static_cast(odom_msg.twist.twist.linear.x)); - EXPECT_LT(cmd_msg->acceleration, 0.0f); -} - -TEST_F(FakeNodeFixture, longitudinal_accelerate) -{ - // Data to test - LongitudinalCommand::SharedPtr cmd_msg; - bool received_longitudinal_command = false; - - // Node - std::shared_ptr node = makeLongitudinalNode(); - - // Publisher/Subscribers - rclcpp::Publisher::SharedPtr odom_pub = - this->create_publisher("longitudinal_controller/input/current_odometry"); - rclcpp::Publisher::SharedPtr traj_pub = - this->create_publisher("longitudinal_controller/input/current_trajectory"); - rclcpp::Subscription::SharedPtr cmd_sub = - this->create_subscription( - "longitudinal_controller/output/control_cmd", *this->get_fake_node(), - [&cmd_msg, &received_longitudinal_command](const LongitudinalCommand::SharedPtr msg) { - cmd_msg = msg; - received_longitudinal_command = true; - }); - std::shared_ptr br = - std::make_shared(this->get_fake_node()); - - // Dummy transform: ego is at (0.0, 0.0) in map frame - geometry_msgs::msg::TransformStamped transform = test_utils::getDummyTransform(); - transform.header.stamp = node->now(); - br->sendTransform(transform); - /// Below target vel + Non stopping trajectory -> accelerate - // Publish velocity - VehicleOdometry odom_msg; - odom_msg.header.stamp = node->now(); - odom_msg.twist.twist.linear.x = 0.5; - odom_pub->publish(odom_msg); - // the node needs to receive two velocity msg - rclcpp::spin_some(node); - rclcpp::spin_some(this->get_fake_node()); - odom_msg.header.stamp = node->now(); - odom_pub->publish(odom_msg); - // Publish non stopping trajectory - Trajectory traj; - traj.header.stamp = node->now(); - traj.header.frame_id = "map"; - TrajectoryPoint point; - point.pose.position.x = 0.0; - point.pose.position.y = 0.0; - point.longitudinal_velocity_mps = 1.0; - traj.points.push_back(point); - point.pose.position.x = 50.0; - point.pose.position.y = 0.0; - point.longitudinal_velocity_mps = 1.0; - traj.points.push_back(point); - point.pose.position.x = 100.0; - point.pose.position.y = 0.0; - point.longitudinal_velocity_mps = 1.0; - traj.points.push_back(point); - traj_pub->publish(traj); - test_utils::waitForMessage(node, this, received_longitudinal_command); - - ASSERT_TRUE(received_longitudinal_command); - EXPECT_GT(cmd_msg->speed, static_cast(odom_msg.twist.twist.linear.x)); - EXPECT_GT(cmd_msg->acceleration, 0.0f); - - // Generate another control message - received_longitudinal_command = false; - traj_pub->publish(traj); - test_utils::waitForMessage(node, this, received_longitudinal_command); - ASSERT_TRUE(received_longitudinal_command); - EXPECT_GT(cmd_msg->speed, static_cast(odom_msg.twist.twist.linear.x)); - EXPECT_GT(cmd_msg->acceleration, 0.0f); -} - -TEST_F(FakeNodeFixture, longitudinal_stopped) -{ - // Data to test - LongitudinalCommand::SharedPtr cmd_msg; - bool received_longitudinal_command = false; - - // Node - std::shared_ptr node = makeLongitudinalNode(); - - // Publisher/Subscribers - rclcpp::Publisher::SharedPtr odom_pub = - this->create_publisher("longitudinal_controller/input/current_odometry"); - rclcpp::Publisher::SharedPtr traj_pub = - this->create_publisher("longitudinal_controller/input/current_trajectory"); - rclcpp::Subscription::SharedPtr cmd_sub = - this->create_subscription( - "longitudinal_controller/output/control_cmd", *this->get_fake_node(), - [&cmd_msg, &received_longitudinal_command](const LongitudinalCommand::SharedPtr msg) { - cmd_msg = msg; - received_longitudinal_command = true; - }); - std::shared_ptr br = - std::make_shared(this->get_fake_node()); - - // Dummy transform: ego is at (0.0, 0.0) in map frame - geometry_msgs::msg::TransformStamped transform = test_utils::getDummyTransform(); - transform.header.stamp = node->now(); - br->sendTransform(transform); - /// Below target vel + Non stopping trajectory -> accelerate - // Publish velocity - VehicleOdometry odom_msg; - odom_msg.header.stamp = node->now(); - odom_msg.twist.twist.linear.x = 0.0; - odom_pub->publish(odom_msg); - // the node needs to receive two velocity msg - rclcpp::spin_some(node); - rclcpp::spin_some(this->get_fake_node()); - odom_msg.header.stamp = node->now(); - odom_pub->publish(odom_msg); - // Publish stopping trajectory - Trajectory traj; - traj.header.stamp = node->now(); - traj.header.frame_id = "map"; - 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); - point.pose.position.x = 50.0; - point.pose.position.y = 0.0; - point.longitudinal_velocity_mps = 0.0; - traj.points.push_back(point); - point.pose.position.x = 100.0; - point.pose.position.y = 0.0; - point.longitudinal_velocity_mps = 0.0; - traj.points.push_back(point); - traj_pub->publish(traj); - test_utils::waitForMessage(node, this, received_longitudinal_command); - - ASSERT_TRUE(received_longitudinal_command); - EXPECT_DOUBLE_EQ(cmd_msg->speed, 0.0f); - EXPECT_LT(cmd_msg->acceleration, 0.0f); // when stopped negative acceleration to brake -} - -TEST_F(FakeNodeFixture, longitudinal_reverse) -{ - // Data to test - LongitudinalCommand::SharedPtr cmd_msg; - bool received_longitudinal_command = false; - - // Node - std::shared_ptr node = makeLongitudinalNode(); - - // Publisher/Subscribers - rclcpp::Publisher::SharedPtr odom_pub = - this->create_publisher("longitudinal_controller/input/current_odometry"); - rclcpp::Publisher::SharedPtr traj_pub = - this->create_publisher("longitudinal_controller/input/current_trajectory"); - rclcpp::Subscription::SharedPtr cmd_sub = - this->create_subscription( - "longitudinal_controller/output/control_cmd", *this->get_fake_node(), - [&cmd_msg, &received_longitudinal_command](const LongitudinalCommand::SharedPtr msg) { - cmd_msg = msg; - received_longitudinal_command = true; - }); - std::shared_ptr br = - std::make_shared(this->get_fake_node()); - - // Dummy transform: ego is at (0.0, 0.0) in map frame - geometry_msgs::msg::TransformStamped transform = test_utils::getDummyTransform(); - transform.header.stamp = node->now(); - br->sendTransform(transform); - /// Below target vel + Non stopping trajectory -> accelerate - // Publish velocity - VehicleOdometry odom_msg; - odom_msg.header.stamp = node->now(); - odom_msg.twist.twist.linear.x = 0.0; - odom_pub->publish(odom_msg); - // the node needs to receive two velocity msg - rclcpp::spin_some(node); - rclcpp::spin_some(this->get_fake_node()); - odom_msg.header.stamp = node->now(); - odom_pub->publish(odom_msg); - // Publish reverse - Trajectory traj; - traj.header.stamp = node->now(); - traj.header.frame_id = "map"; - TrajectoryPoint point; - point.pose.position.x = 0.0; - point.pose.position.y = 0.0; - point.longitudinal_velocity_mps = -1.0; - traj.points.push_back(point); - point.pose.position.x = 50.0; - point.pose.position.y = 0.0; - point.longitudinal_velocity_mps = -1.0; - traj.points.push_back(point); - point.pose.position.x = 100.0; - point.pose.position.y = 0.0; - point.longitudinal_velocity_mps = -1.0; - traj.points.push_back(point); - traj_pub->publish(traj); - test_utils::waitForMessage(node, this, received_longitudinal_command); - - ASSERT_TRUE(received_longitudinal_command); - EXPECT_LT(cmd_msg->speed, 0.0f); - EXPECT_GT(cmd_msg->acceleration, 0.0f); -} - -TEST_F(FakeNodeFixture, longitudinal_emergency) -{ - // Data to test - LongitudinalCommand::SharedPtr cmd_msg; - bool received_longitudinal_command = false; - - // Node - std::shared_ptr node = makeLongitudinalNode(); - - // Publisher/Subscribers - rclcpp::Publisher::SharedPtr odom_pub = - this->create_publisher("longitudinal_controller/input/current_odometry"); - rclcpp::Publisher::SharedPtr traj_pub = - this->create_publisher("longitudinal_controller/input/current_trajectory"); - rclcpp::Subscription::SharedPtr cmd_sub = - this->create_subscription( - "longitudinal_controller/output/control_cmd", *this->get_fake_node(), - [&cmd_msg, &received_longitudinal_command](const LongitudinalCommand::SharedPtr msg) { - cmd_msg = msg; - received_longitudinal_command = true; - }); - std::shared_ptr br = - std::make_shared(this->get_fake_node()); - - // Dummy transform: ego is at (0.0, 0.0) in map frame - geometry_msgs::msg::TransformStamped transform = test_utils::getDummyTransform(); - transform.header.stamp = node->now(); - br->sendTransform(transform); - /// Below target vel + Non stopping trajectory -> accelerate - // Publish velocity - VehicleOdometry odom_msg; - odom_msg.header.stamp = node->now(); - odom_msg.twist.twist.linear.x = 0.0; - odom_pub->publish(odom_msg); - // the node needs to receive two velocity msg - rclcpp::spin_some(node); - rclcpp::spin_some(this->get_fake_node()); - odom_msg.header.stamp = node->now(); - odom_pub->publish(odom_msg); - // Publish trajectory starting away from the current ego pose - Trajectory traj; - traj.header.stamp = node->now(); - traj.header.frame_id = "map"; - TrajectoryPoint point; - point.pose.position.x = 10.0; - point.pose.position.y = 0.0; - point.longitudinal_velocity_mps = 1.0; - traj.points.push_back(point); - point.pose.position.x = 50.0; - point.pose.position.y = 0.0; - point.longitudinal_velocity_mps = 1.0; - traj.points.push_back(point); - point.pose.position.x = 100.0; - point.pose.position.y = 0.0; - point.longitudinal_velocity_mps = 1.0; - traj.points.push_back(point); - traj_pub->publish(traj); - test_utils::waitForMessage(node, this, received_longitudinal_command); - - ASSERT_TRUE(received_longitudinal_command); - // Emergencies (e.g., far from trajectory) produces braking command (0 vel, negative accel) - EXPECT_DOUBLE_EQ(cmd_msg->speed, 0.0f); - EXPECT_LT(cmd_msg->acceleration, 0.0f); -}