From 36b4ff5e9b17c4834f3b3f643a593122ef52d66a Mon Sep 17 00:00:00 2001 From: Takamasa Horibe Date: Sat, 17 Sep 2022 07:17:09 +0900 Subject: [PATCH] refactor: replace acc calculation in planning control modules (#1213) * [obstacle_cruise_planner] replace acceleration calculation Signed-off-by: Takamasa Horibe * [obstacle_stop_planner] replace acceleration calculation Signed-off-by: Takamasa Horibe * [trajectory_follower] replace acceleration calculation Signed-off-by: Takamasa Horibe * remap topic name in lanuch Signed-off-by: Takamasa Horibe * fix nullptr check Signed-off-by: Takamasa Horibe * fix controller test Signed-off-by: Takamasa Horibe * fix Signed-off-by: Takamasa Horibe Signed-off-by: Takamasa Horibe --- .../trajectory_follower/input_data.hpp | 2 + .../pid_longitudinal_controller.hpp | 12 +- .../src/pid_longitudinal_controller.cpp | 37 +- .../controller_node.hpp | 4 + .../src/controller_node.cpp | 7 + .../test/test_controller_node.cpp | 1006 +++++------------ .../launch/control.launch.py | 1 + .../obstacle_cruise_planner.param.yaml | 2 - .../motion_planning/motion_planning.launch.py | 2 + .../config/obstacle_cruise_planner.param.yaml | 2 - .../include/obstacle_cruise_planner/node.hpp | 11 +- planning/obstacle_cruise_planner/src/node.cpp | 40 +- planning/obstacle_stop_planner/README.md | 1 - .../config/obstacle_stop_planner.param.yaml | 1 - .../include/obstacle_stop_planner/node.hpp | 16 +- .../obstacle_stop_planner/planner_data.hpp | 3 - planning/obstacle_stop_planner/src/node.cpp | 66 +- 17 files changed, 401 insertions(+), 812 deletions(-) diff --git a/control/trajectory_follower/include/trajectory_follower/input_data.hpp b/control/trajectory_follower/include/trajectory_follower/input_data.hpp index 505e5e0de2545..b356095e2f78e 100644 --- a/control/trajectory_follower/include/trajectory_follower/input_data.hpp +++ b/control/trajectory_follower/include/trajectory_follower/input_data.hpp @@ -17,6 +17,7 @@ #include "autoware_auto_planning_msgs/msg/trajectory.hpp" #include "autoware_auto_vehicle_msgs/msg/steering_report.hpp" +#include "geometry_msgs/msg/accel_with_covariance_stamped.hpp" #include "nav_msgs/msg/odometry.hpp" namespace autoware @@ -32,6 +33,7 @@ struct InputData autoware_auto_planning_msgs::msg::Trajectory::SharedPtr current_trajectory_ptr; nav_msgs::msg::Odometry::SharedPtr current_odometry_ptr; autoware_auto_vehicle_msgs::msg::SteeringReport::SharedPtr current_steering_ptr; + geometry_msgs::msg::AccelWithCovarianceStamped::SharedPtr current_accel_ptr; }; } // namespace trajectory_follower } // namespace control diff --git a/control/trajectory_follower/include/trajectory_follower/pid_longitudinal_controller.hpp b/control/trajectory_follower/include/trajectory_follower/pid_longitudinal_controller.hpp index 60c7cca13ad02..c7b5e73d5982c 100644 --- a/control/trajectory_follower/include/trajectory_follower/pid_longitudinal_controller.hpp +++ b/control/trajectory_follower/include/trajectory_follower/pid_longitudinal_controller.hpp @@ -97,7 +97,7 @@ class TRAJECTORY_FOLLOWER_PUBLIC PidLongitudinalController : public Longitudinal // pointers for ros topic nav_msgs::msg::Odometry::ConstSharedPtr m_current_kinematic_state_ptr{nullptr}; - nav_msgs::msg::Odometry::ConstSharedPtr m_prev_kienmatic_state_ptr{nullptr}; + geometry_msgs::msg::AccelWithCovarianceStamped::ConstSharedPtr m_current_accel_ptr{nullptr}; autoware_auto_planning_msgs::msg::Trajectory::ConstSharedPtr m_trajectory_ptr{nullptr}; // vehicle info @@ -185,9 +185,6 @@ class TRAJECTORY_FOLLOWER_PUBLIC PidLongitudinalController : public Longitudinal double m_ego_nearest_dist_threshold; double m_ego_nearest_yaw_threshold; - // 1st order lowpass filter for acceleration - std::shared_ptr m_lpf_acc{nullptr}; - // buffer of send command std::vector m_ctrl_cmd_vec; @@ -213,6 +210,13 @@ class TRAJECTORY_FOLLOWER_PUBLIC PidLongitudinalController : public Longitudinal */ void setKinematicState(const nav_msgs::msg::Odometry::ConstSharedPtr msg); + /** + * @brief set current acceleration with received message + * @param [in] msg trajectory message + */ + void setCurrentAcceleration( + const geometry_msgs::msg::AccelWithCovarianceStamped::ConstSharedPtr msg); + /** * @brief set reference trajectory with received message * @param [in] msg trajectory message diff --git a/control/trajectory_follower/src/pid_longitudinal_controller.cpp b/control/trajectory_follower/src/pid_longitudinal_controller.cpp index f8919d06638a4..53c3f71fc11bd 100644 --- a/control/trajectory_follower/src/pid_longitudinal_controller.cpp +++ b/control/trajectory_follower/src/pid_longitudinal_controller.cpp @@ -199,26 +199,27 @@ PidLongitudinalController::PidLongitudinalController(rclcpp::Node & node) : node // set parameter callback m_set_param_res = node_->add_on_set_parameters_callback( std::bind(&PidLongitudinalController::paramCallback, this, _1)); - - // set lowpass filter for acc - m_lpf_acc = std::make_shared(0.0, 0.2); } void PidLongitudinalController::setInputData(InputData const & input_data) { setTrajectory(input_data.current_trajectory_ptr); setKinematicState(input_data.current_odometry_ptr); + setCurrentAcceleration(input_data.current_accel_ptr); } void PidLongitudinalController::setKinematicState(const nav_msgs::msg::Odometry::ConstSharedPtr msg) { if (!msg) return; - - if (m_current_kinematic_state_ptr) { - m_prev_kienmatic_state_ptr = m_current_kinematic_state_ptr; - } m_current_kinematic_state_ptr = msg; } +void PidLongitudinalController::setCurrentAcceleration( + const geometry_msgs::msg::AccelWithCovarianceStamped::ConstSharedPtr msg) +{ + if (!msg) return; + m_current_accel_ptr = msg; +} + void PidLongitudinalController::setTrajectory( const autoware_auto_planning_msgs::msg::Trajectory::ConstSharedPtr msg) { @@ -365,7 +366,7 @@ rcl_interfaces::msg::SetParametersResult PidLongitudinalController::paramCallbac boost::optional PidLongitudinalController::run() { // wait for initial pointers - if (!m_current_kinematic_state_ptr || !m_prev_kienmatic_state_ptr || !m_trajectory_ptr) { + if (!m_current_kinematic_state_ptr || !m_trajectory_ptr || !m_current_accel_ptr) { return boost::none; } @@ -415,7 +416,8 @@ PidLongitudinalController::ControlData PidLongitudinalController::getControlData control_data.dt = getDt(); // current velocity and acceleration - control_data.current_motion = getCurrentMotion(); + control_data.current_motion.vel = m_current_kinematic_state_ptr->twist.twist.linear.x; + control_data.current_motion.acc = m_current_accel_ptr->accel.accel.linear.x; // nearest idx const size_t nearest_idx = motion_utils::findFirstNearestIndexWithSoftConstraints( @@ -699,23 +701,6 @@ float64_t PidLongitudinalController::getDt() return std::max(std::min(dt, max_dt), min_dt); } -PidLongitudinalController::Motion PidLongitudinalController::getCurrentMotion() const -{ - const float64_t dv = m_current_kinematic_state_ptr->twist.twist.linear.x - - m_prev_kienmatic_state_ptr->twist.twist.linear.x; - const float64_t dt = std::max( - (rclcpp::Time(m_current_kinematic_state_ptr->header.stamp) - - rclcpp::Time(m_prev_kienmatic_state_ptr->header.stamp)) - .seconds(), - 1e-03); - const float64_t accel = dv / dt; - - const float64_t current_vel = m_current_kinematic_state_ptr->twist.twist.linear.x; - const float64_t current_acc = m_lpf_acc->filter(accel); - - return Motion{current_vel, current_acc}; -} - enum PidLongitudinalController::Shift PidLongitudinalController::getCurrentShift( const size_t nearest_idx) const { diff --git a/control/trajectory_follower_nodes/include/trajectory_follower_nodes/controller_node.hpp b/control/trajectory_follower_nodes/include/trajectory_follower_nodes/controller_node.hpp index 91b99fc7837c9..2c8d51b4b69e2 100644 --- a/control/trajectory_follower_nodes/include/trajectory_follower_nodes/controller_node.hpp +++ b/control/trajectory_follower_nodes/include/trajectory_follower_nodes/controller_node.hpp @@ -37,6 +37,8 @@ #include "autoware_auto_planning_msgs/msg/trajectory.hpp" #include "autoware_auto_system_msgs/msg/float32_multi_array_diagnostic.hpp" #include "autoware_auto_vehicle_msgs/msg/vehicle_odometry.hpp" +#include "geometry_msgs/msg/accel_stamped.hpp" +#include "geometry_msgs/msg/accel_with_covariance_stamped.hpp" #include "geometry_msgs/msg/pose_stamped.hpp" #include "nav_msgs/msg/odometry.hpp" #include "tf2_msgs/msg/tf_message.hpp" @@ -82,6 +84,7 @@ class TRAJECTORY_FOLLOWER_PUBLIC Controller : public rclcpp::Node rclcpp::Subscription::SharedPtr sub_ref_path_; rclcpp::Subscription::SharedPtr sub_odometry_; rclcpp::Subscription::SharedPtr sub_steering_; + rclcpp::Subscription::SharedPtr sub_accel_; rclcpp::Publisher::SharedPtr control_cmd_pub_; @@ -102,6 +105,7 @@ class TRAJECTORY_FOLLOWER_PUBLIC Controller : public rclcpp::Node void onTrajectory(const autoware_auto_planning_msgs::msg::Trajectory::SharedPtr); void onOdometry(const nav_msgs::msg::Odometry::SharedPtr msg); void onSteering(const autoware_auto_vehicle_msgs::msg::SteeringReport::SharedPtr msg); + void onAccel(const geometry_msgs::msg::AccelWithCovarianceStamped::SharedPtr msg); bool isTimeOut(); LateralControllerMode getLateralControllerMode(const std::string & algorithm_name) const; LongitudinalControllerMode getLongitudinalControllerMode( diff --git a/control/trajectory_follower_nodes/src/controller_node.cpp b/control/trajectory_follower_nodes/src/controller_node.cpp index 2eaf51d8a7652..f720dfc9c27fd 100644 --- a/control/trajectory_follower_nodes/src/controller_node.cpp +++ b/control/trajectory_follower_nodes/src/controller_node.cpp @@ -76,6 +76,8 @@ Controller::Controller(const rclcpp::NodeOptions & node_options) : Node("control "~/input/current_steering", rclcpp::QoS{1}, std::bind(&Controller::onSteering, this, _1)); sub_odometry_ = create_subscription( "~/input/current_odometry", rclcpp::QoS{1}, std::bind(&Controller::onOdometry, this, _1)); + sub_accel_ = create_subscription( + "~/input/current_accel", rclcpp::QoS{1}, std::bind(&Controller::onAccel, this, _1)); control_cmd_pub_ = create_publisher( "~/output/control_cmd", rclcpp::QoS{1}.transient_local()); @@ -120,6 +122,11 @@ void Controller::onSteering(const autoware_auto_vehicle_msgs::msg::SteeringRepor input_data_.current_steering_ptr = msg; } +void Controller::onAccel(const geometry_msgs::msg::AccelWithCovarianceStamped::SharedPtr msg) +{ + input_data_.current_accel_ptr = msg; +} + bool Controller::isTimeOut() { const auto now = this->now(); diff --git a/control/trajectory_follower_nodes/test/test_controller_node.cpp b/control/trajectory_follower_nodes/test/test_controller_node.cpp index d0a2b96302124..acb172f66ca28 100644 --- a/control/trajectory_follower_nodes/test/test_controller_node.cpp +++ b/control/trajectory_follower_nodes/test/test_controller_node.cpp @@ -24,6 +24,7 @@ #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/accel_with_covariance_stamped.hpp" #include "geometry_msgs/msg/pose.hpp" #include "geometry_msgs/msg/pose_stamped.hpp" #include "geometry_msgs/msg/transform_stamped.hpp" @@ -38,6 +39,7 @@ 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 geometry_msgs::msg::AccelWithCovarianceStamped; using FakeNodeFixture = autoware::tools::testing::FakeTestNode; @@ -68,815 +70,415 @@ std::shared_ptr makeNode() return node; } -TEST_F(FakeNodeFixture, no_input) +class ControllerTester { - // Data to test +public: + explicit ControllerTester(FakeNodeFixture * _fnf) : fnf(_fnf) {} + + std::shared_ptr node = makeNode(); + + FakeNodeFixture * fnf; AckermannControlCommand::SharedPtr cmd_msg; bool received_control_command = false; - // Node - std::shared_ptr node = makeNode(); - // Publisher/Subscribers + + void publish_default_odom() + { + VehicleOdometry odom_msg; + odom_msg.header.stamp = node->now(); + odom_pub->publish(odom_msg); + }; + + void publish_odom_vx(const double vx) + { + VehicleOdometry odom_msg; + odom_msg.header.stamp = node->now(); + odom_msg.twist.twist.linear.x = vx; + odom_pub->publish(odom_msg); + }; + + void publish_default_steer() + { + SteeringReport steer_msg; + steer_msg.stamp = node->now(); + steer_pub->publish(steer_msg); + }; + + void publish_steer_angle(const double steer) + { + SteeringReport steer_msg; + steer_msg.stamp = node->now(); + steer_msg.steering_tire_angle = steer; + steer_pub->publish(steer_msg); + }; + + void publish_default_acc() + { + AccelWithCovarianceStamped acc_msg; + acc_msg.header.stamp = node->now(); + accel_pub->publish(acc_msg); + }; + + void publish_default_traj() + { + Trajectory traj_msg; + traj_msg.header.stamp = node->now(); + traj_msg.header.frame_id = "map"; + traj_pub->publish(traj_msg); + }; + + void send_default_transform() + { + // 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); + }; + rclcpp::Publisher::SharedPtr traj_pub = - this->create_publisher("controller/input/reference_trajectory"); + fnf->create_publisher("controller/input/reference_trajectory"); + rclcpp::Publisher::SharedPtr odom_pub = - this->create_publisher("controller/input/current_odometry"); + fnf->create_publisher("controller/input/current_odometry"); + rclcpp::Publisher::SharedPtr steer_pub = - this->create_publisher("controller/input/current_steering"); + fnf->create_publisher("controller/input/current_steering"); + + rclcpp::Publisher::SharedPtr accel_pub = + fnf->create_publisher("controller/input/current_accel"); + rclcpp::Subscription::SharedPtr cmd_sub = - this->create_subscription( - "controller/output/control_cmd", *this->get_fake_node(), - [&cmd_msg, &received_control_command](const AckermannControlCommand::SharedPtr msg) { + fnf->create_subscription( + "controller/output/control_cmd", *fnf->get_fake_node(), + [this](const AckermannControlCommand::SharedPtr msg) { cmd_msg = msg; received_control_command = true; }); + std::shared_ptr br = - std::make_shared(this->get_fake_node()); + std::make_shared(fnf->get_fake_node()); +}; + +TrajectoryPoint make_traj_point(const double px, const double py, const float vx) +{ + TrajectoryPoint p; + p.pose.position.x = px; + p.pose.position.y = py; + p.longitudinal_velocity_mps = vx; + return p; +} + +TEST_F(FakeNodeFixture, no_input) +{ + ControllerTester tester(this); // No published data: expect a stopped command test_utils::waitForMessage( - node, this, received_control_command, std::chrono::seconds{1LL}, false); - ASSERT_FALSE(received_control_command); + tester.node, this, tester.received_control_command, std::chrono::seconds{1LL}, false); + ASSERT_FALSE(tester.received_control_command); } TEST_F(FakeNodeFixture, empty_trajectory) { - // Data to test - AckermannControlCommand::SharedPtr cmd_msg; - bool received_control_command = false; - // Node - std::shared_ptr node = makeNode(); - // Publisher/Subscribers - rclcpp::Publisher::SharedPtr traj_pub = - this->create_publisher("controller/input/reference_trajectory"); - rclcpp::Publisher::SharedPtr odom_pub = - this->create_publisher("controller/input/current_odometry"); - rclcpp::Publisher::SharedPtr steer_pub = - this->create_publisher("controller/input/current_steering"); - rclcpp::Subscription::SharedPtr cmd_sub = - this->create_subscription( - "controller/output/control_cmd", *this->get_fake_node(), - [&cmd_msg, &received_control_command](const AckermannControlCommand::SharedPtr msg) { - cmd_msg = msg; - received_control_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); + ControllerTester tester(this); - // Spin for transform to be published - test_utils::spinWhile(node); + tester.send_default_transform(); // 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); + tester.publish_default_traj(); + tester.publish_default_odom(); + tester.publish_default_acc(); + tester.publish_default_steer(); test_utils::waitForMessage( - node, this, received_control_command, std::chrono::seconds{1LL}, false); - ASSERT_FALSE(received_control_command); + tester.node, this, tester.received_control_command, std::chrono::seconds{1LL}, false); + ASSERT_FALSE(tester.received_control_command); } // lateral TEST_F(FakeNodeFixture, straight_trajectory) { - // Data to test - AckermannControlCommand::SharedPtr cmd_msg; - bool received_control_command = false; - // Node - std::shared_ptr node = makeNode(); - // Publisher/Subscribers - rclcpp::Publisher::SharedPtr traj_pub = - this->create_publisher("controller/input/reference_trajectory"); - rclcpp::Publisher::SharedPtr odom_pub = - this->create_publisher("controller/input/current_odometry"); - rclcpp::Publisher::SharedPtr steer_pub = - this->create_publisher("controller/input/current_steering"); - rclcpp::Subscription::SharedPtr cmd_sub = - this->create_subscription( - "controller/output/control_cmd", *this->get_fake_node(), - [&cmd_msg, &received_control_command](const AckermannControlCommand::SharedPtr msg) { - cmd_msg = msg; - received_control_command = true; - }); - std::shared_ptr br = - std::make_shared(this->get_fake_node()); + ControllerTester tester(this); - // 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); + tester.send_default_transform(); + tester.publish_odom_vx(1.0); + tester.publish_default_steer(); + tester.publish_default_acc(); - // Spin for transform to be published - test_utils::spinWhile(node); - - // Straight trajectory: expect no steering - received_control_command = false; Trajectory traj_msg; - traj_msg.header.stamp = node->now(); + traj_msg.header.stamp = tester.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_control_command); - ASSERT_TRUE(received_control_command); - EXPECT_EQ(cmd_msg->lateral.steering_tire_angle, 0.0f); - EXPECT_EQ(cmd_msg->lateral.steering_tire_rotation_rate, 0.0f); - EXPECT_GT(rclcpp::Time(cmd_msg->stamp), rclcpp::Time(traj_msg.header.stamp)); + traj_msg.points.push_back(make_traj_point(-1.0, 0.0, 1.0f)); + traj_msg.points.push_back(make_traj_point(0.0, 0.0, 1.0f)); + traj_msg.points.push_back(make_traj_point(1.0, 0.0, 1.0f)); + traj_msg.points.push_back(make_traj_point(2.0, 0.0, 1.0f)); + tester.traj_pub->publish(traj_msg); + + test_utils::waitForMessage(tester.node, this, tester.received_control_command); + ASSERT_TRUE(tester.received_control_command); + 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(rclcpp::Time(tester.cmd_msg->stamp), rclcpp::Time(traj_msg.header.stamp)); } TEST_F(FakeNodeFixture, right_turn) { - // Data to test - AckermannControlCommand::SharedPtr cmd_msg; - bool received_control_command = false; - // Node - std::shared_ptr node = makeNode(); - // Publisher/Subscribers - rclcpp::Publisher::SharedPtr traj_pub = - this->create_publisher("controller/input/reference_trajectory"); - rclcpp::Publisher::SharedPtr odom_pub = - this->create_publisher("controller/input/current_odometry"); - rclcpp::Publisher::SharedPtr steer_pub = - this->create_publisher("controller/input/current_steering"); - rclcpp::Subscription::SharedPtr cmd_sub = - this->create_subscription( - "controller/output/control_cmd", *this->get_fake_node(), - [&cmd_msg, &received_control_command](const AckermannControlCommand::SharedPtr msg) { - cmd_msg = msg; - received_control_command = true; - }); - std::shared_ptr br = - std::make_shared(this->get_fake_node()); + ControllerTester tester(this); - // 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); + tester.send_default_transform(); + tester.publish_odom_vx(1.0); + tester.publish_default_steer(); + tester.publish_default_acc(); // Right turning trajectory: expect right steering - received_control_command = false; Trajectory traj_msg; - traj_msg.header.stamp = node->now(); + traj_msg.header.stamp = tester.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_control_command); - ASSERT_TRUE(received_control_command); - EXPECT_LT(cmd_msg->lateral.steering_tire_angle, 0.0f); - EXPECT_LT(cmd_msg->lateral.steering_tire_rotation_rate, 0.0f); - EXPECT_GT(rclcpp::Time(cmd_msg->stamp), rclcpp::Time(traj_msg.header.stamp)); + traj_msg.points.push_back(make_traj_point(-1.0, -1.0, 1.0f)); + traj_msg.points.push_back(make_traj_point(0.0, 0.0, 1.0f)); + traj_msg.points.push_back(make_traj_point(1.0, -1.0, 1.0f)); + traj_msg.points.push_back(make_traj_point(2.0, -2.0, 1.0f)); + tester.traj_pub->publish(traj_msg); + + test_utils::waitForMessage(tester.node, this, tester.received_control_command); + ASSERT_TRUE(tester.received_control_command); + EXPECT_LT(tester.cmd_msg->lateral.steering_tire_angle, 0.0f); + EXPECT_LT(tester.cmd_msg->lateral.steering_tire_rotation_rate, 0.0f); + EXPECT_GT(rclcpp::Time(tester.cmd_msg->stamp), rclcpp::Time(traj_msg.header.stamp)); } TEST_F(FakeNodeFixture, left_turn) { - // Data to test - AckermannControlCommand::SharedPtr cmd_msg; - bool received_control_command = false; - // Node - std::shared_ptr node = makeNode(); - // Publisher/Subscribers - rclcpp::Publisher::SharedPtr traj_pub = - this->create_publisher("controller/input/reference_trajectory"); - rclcpp::Publisher::SharedPtr odom_pub = - this->create_publisher("controller/input/current_odometry"); - rclcpp::Publisher::SharedPtr steer_pub = - this->create_publisher("controller/input/current_steering"); - rclcpp::Subscription::SharedPtr cmd_sub = - this->create_subscription( - "controller/output/control_cmd", *this->get_fake_node(), - [&cmd_msg, &received_control_command](const AckermannControlCommand::SharedPtr msg) { - cmd_msg = msg; - received_control_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); + ControllerTester tester(this); - // Spin for transform to be published - test_utils::spinWhile(node); + tester.send_default_transform(); + tester.publish_odom_vx(1.0); + tester.publish_default_steer(); + tester.publish_default_acc(); // Left turning trajectory: expect left steering - received_control_command = false; Trajectory traj_msg; - traj_msg.header.stamp = node->now(); + traj_msg.header.stamp = tester.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_control_command); - ASSERT_TRUE(received_control_command); - EXPECT_GT(cmd_msg->lateral.steering_tire_angle, 0.0f); - EXPECT_GT(cmd_msg->lateral.steering_tire_rotation_rate, 0.0f); - EXPECT_GT(rclcpp::Time(cmd_msg->stamp), rclcpp::Time(traj_msg.header.stamp)); + traj_msg.points.push_back(make_traj_point(-1.0, 1.0, 1.0f)); + traj_msg.points.push_back(make_traj_point(0.0, 0.0, 1.0f)); + traj_msg.points.push_back(make_traj_point(1.0, 1.0, 1.0f)); + traj_msg.points.push_back(make_traj_point(2.0, 2.0, 1.0f)); + tester.traj_pub->publish(traj_msg); + + test_utils::waitForMessage(tester.node, this, tester.received_control_command); + ASSERT_TRUE(tester.received_control_command); + EXPECT_GT(tester.cmd_msg->lateral.steering_tire_angle, 0.0f); + EXPECT_GT(tester.cmd_msg->lateral.steering_tire_rotation_rate, 0.0f); + EXPECT_GT(rclcpp::Time(tester.cmd_msg->stamp), rclcpp::Time(traj_msg.header.stamp)); } TEST_F(FakeNodeFixture, stopped) { - // Data to test - AckermannControlCommand::SharedPtr cmd_msg; - bool received_control_command = false; - // Node - std::shared_ptr node = makeNode(); - // Publisher/Subscribers - rclcpp::Publisher::SharedPtr traj_pub = - this->create_publisher("controller/input/reference_trajectory"); - rclcpp::Publisher::SharedPtr odom_pub = - this->create_publisher("controller/input/current_odometry"); - rclcpp::Publisher::SharedPtr steer_pub = - this->create_publisher("controller/input/current_steering"); - rclcpp::Subscription::SharedPtr cmd_sub = - this->create_subscription( - "controller/output/control_cmd", *this->get_fake_node(), - [&cmd_msg, &received_control_command](const AckermannControlCommand::SharedPtr msg) { - cmd_msg = msg; - received_control_command = true; - }); - std::shared_ptr br = - std::make_shared(this->get_fake_node()); + ControllerTester tester(this); - // 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); + tester.send_default_transform(); + tester.publish_default_odom(); + tester.publish_default_acc(); - // Spin for transform to be published - test_utils::spinWhile(node); + const double steering_tire_angle = -0.5; + tester.publish_steer_angle(steering_tire_angle); // Straight trajectory: expect no steering - received_control_command = false; Trajectory traj_msg; - traj_msg.header.stamp = node->now(); + traj_msg.header.stamp = tester.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_control_command); - ASSERT_TRUE(received_control_command); - EXPECT_EQ(cmd_msg->lateral.steering_tire_angle, steer_msg.steering_tire_angle); - EXPECT_EQ(cmd_msg->lateral.steering_tire_rotation_rate, 0.0f); - EXPECT_GT(rclcpp::Time(cmd_msg->stamp), rclcpp::Time(traj_msg.header.stamp)); + traj_msg.points.push_back(make_traj_point(-1.0, 0.0, 0.0f)); + traj_msg.points.push_back(make_traj_point(0.0, 0.0, 0.0f)); + traj_msg.points.push_back(make_traj_point(1.0, 0.0, 0.0f)); + traj_msg.points.push_back(make_traj_point(2.0, 0.0, 0.0f)); + tester.traj_pub->publish(traj_msg); + + test_utils::waitForMessage(tester.node, this, tester.received_control_command); + ASSERT_TRUE(tester.received_control_command); + EXPECT_EQ(tester.cmd_msg->lateral.steering_tire_angle, steering_tire_angle); + EXPECT_EQ(tester.cmd_msg->lateral.steering_tire_rotation_rate, 0.0f); + EXPECT_GT(rclcpp::Time(tester.cmd_msg->stamp), rclcpp::Time(traj_msg.header.stamp)); } // longitudinal TEST_F(FakeNodeFixture, longitudinal_keep_velocity) { - // Data to test - AckermannControlCommand::SharedPtr cmd_msg; - bool received_control_command = false; - // Node - std::shared_ptr node = makeNode(); - // Publisher/Subscribers - rclcpp::Publisher::SharedPtr traj_pub = - this->create_publisher("controller/input/reference_trajectory"); - rclcpp::Publisher::SharedPtr odom_pub = - this->create_publisher("controller/input/current_odometry"); - rclcpp::Publisher::SharedPtr steer_pub = - this->create_publisher("controller/input/current_steering"); - rclcpp::Subscription::SharedPtr cmd_sub = - this->create_subscription( - "controller/output/control_cmd", *this->get_fake_node(), - [&cmd_msg, &received_control_command](const AckermannControlCommand::SharedPtr msg) { - cmd_msg = msg; - received_control_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 steering - SteeringReport steer_msg; - steer_msg.stamp = node->now(); - steer_msg.steering_tire_angle = 0.0; - steer_pub->publish(steer_msg); + ControllerTester tester(this); + + tester.send_default_transform(); + tester.publish_odom_vx(1.0); + tester.publish_default_steer(); + tester.publish_default_acc(); + // 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_control_command); - - ASSERT_TRUE(received_control_command); - EXPECT_DOUBLE_EQ(cmd_msg->longitudinal.speed, 1.0); - EXPECT_DOUBLE_EQ(cmd_msg->longitudinal.acceleration, 0.0); + Trajectory traj_msg; + traj_msg.header.stamp = tester.node->now(); + traj_msg.header.frame_id = "map"; + traj_msg.points.push_back(make_traj_point(0.0, 0.0, 1.0f)); + traj_msg.points.push_back(make_traj_point(50.0, 0.0, 1.0f)); + traj_msg.points.push_back(make_traj_point(100.0, 0.0, 1.0f)); + tester.traj_pub->publish(traj_msg); + + test_utils::waitForMessage(tester.node, this, tester.received_control_command); + + ASSERT_TRUE(tester.received_control_command); + EXPECT_DOUBLE_EQ(tester.cmd_msg->longitudinal.speed, 1.0); + EXPECT_DOUBLE_EQ(tester.cmd_msg->longitudinal.acceleration, 0.0); // Generate another control message - received_control_command = false; - traj_pub->publish(traj); - test_utils::waitForMessage(node, this, received_control_command); - ASSERT_TRUE(received_control_command); - EXPECT_DOUBLE_EQ(cmd_msg->longitudinal.speed, 1.0); - EXPECT_DOUBLE_EQ(cmd_msg->longitudinal.acceleration, 0.0); + tester.traj_pub->publish(traj_msg); + test_utils::waitForMessage(tester.node, this, tester.received_control_command); + ASSERT_TRUE(tester.received_control_command); + EXPECT_DOUBLE_EQ(tester.cmd_msg->longitudinal.speed, 1.0); + EXPECT_DOUBLE_EQ(tester.cmd_msg->longitudinal.acceleration, 0.0); } TEST_F(FakeNodeFixture, longitudinal_slow_down) { - // Data to test - AckermannControlCommand::SharedPtr cmd_msg; - bool received_control_command = false; - // Node - std::shared_ptr node = makeNode(); - // Publisher/Subscribers - rclcpp::Publisher::SharedPtr traj_pub = - this->create_publisher("controller/input/reference_trajectory"); - rclcpp::Publisher::SharedPtr odom_pub = - this->create_publisher("controller/input/current_odometry"); - rclcpp::Publisher::SharedPtr steer_pub = - this->create_publisher("controller/input/current_steering"); - rclcpp::Subscription::SharedPtr cmd_sub = - this->create_subscription( - "controller/output/control_cmd", *this->get_fake_node(), - [&cmd_msg, &received_control_command](const AckermannControlCommand::SharedPtr msg) { - cmd_msg = msg; - received_control_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 steering - SteeringReport steer_msg; - steer_msg.stamp = node->now(); - steer_msg.steering_tire_angle = 0.0; - steer_pub->publish(steer_msg); + ControllerTester tester(this); + + tester.send_default_transform(); + tester.publish_default_acc(); + tester.publish_default_steer(); + + const double odom_vx = 1.0; + tester.publish_odom_vx(odom_vx); + // Publish non stopping trajectory Trajectory traj; - traj.header.stamp = node->now(); + traj.header.stamp = tester.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_control_command); - - ASSERT_TRUE(received_control_command); - EXPECT_LT(cmd_msg->longitudinal.speed, static_cast(odom_msg.twist.twist.linear.x)); - EXPECT_LT(cmd_msg->longitudinal.acceleration, 0.0f); + traj.points.push_back(make_traj_point(0.0, 0.0, 0.5f)); + traj.points.push_back(make_traj_point(50.0, 0.0, 0.5f)); + traj.points.push_back(make_traj_point(100.0, 0.0, 0.5f)); + tester.traj_pub->publish(traj); + + test_utils::waitForMessage(tester.node, this, tester.received_control_command); + + ASSERT_TRUE(tester.received_control_command); + EXPECT_LT(tester.cmd_msg->longitudinal.speed, static_cast(odom_vx)); + EXPECT_LT(tester.cmd_msg->longitudinal.acceleration, 0.0f); // Generate another control message - received_control_command = false; - traj_pub->publish(traj); - test_utils::waitForMessage(node, this, received_control_command); - ASSERT_TRUE(received_control_command); - EXPECT_LT(cmd_msg->longitudinal.speed, static_cast(odom_msg.twist.twist.linear.x)); - EXPECT_LT(cmd_msg->longitudinal.acceleration, 0.0f); + tester.traj_pub->publish(traj); + test_utils::waitForMessage(tester.node, this, tester.received_control_command); + ASSERT_TRUE(tester.received_control_command); + EXPECT_LT(tester.cmd_msg->longitudinal.speed, static_cast(odom_vx)); + EXPECT_LT(tester.cmd_msg->longitudinal.acceleration, 0.0f); } TEST_F(FakeNodeFixture, longitudinal_accelerate) { - // Data to test - AckermannControlCommand::SharedPtr cmd_msg; - bool received_control_command = false; - // Node - std::shared_ptr node = makeNode(); - // Publisher/Subscribers - rclcpp::Publisher::SharedPtr traj_pub = - this->create_publisher("controller/input/reference_trajectory"); - rclcpp::Publisher::SharedPtr odom_pub = - this->create_publisher("controller/input/current_odometry"); - rclcpp::Publisher::SharedPtr steer_pub = - this->create_publisher("controller/input/current_steering"); - rclcpp::Subscription::SharedPtr cmd_sub = - this->create_subscription( - "controller/output/control_cmd", *this->get_fake_node(), - [&cmd_msg, &received_control_command](const AckermannControlCommand::SharedPtr msg) { - cmd_msg = msg; - received_control_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 steering - SteeringReport steer_msg; - steer_msg.stamp = node->now(); - steer_msg.steering_tire_angle = 0.0; - steer_pub->publish(steer_msg); + ControllerTester tester(this); + + tester.send_default_transform(); + tester.publish_default_steer(); + tester.publish_default_acc(); + + const double odom_vx = 0.5; + tester.publish_odom_vx(odom_vx); + // Publish non stopping trajectory Trajectory traj; - traj.header.stamp = node->now(); + traj.header.stamp = tester.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_control_command); - - ASSERT_TRUE(received_control_command); - EXPECT_GT(cmd_msg->longitudinal.speed, static_cast(odom_msg.twist.twist.linear.x)); - EXPECT_GT(cmd_msg->longitudinal.acceleration, 0.0f); + traj.points.push_back(make_traj_point(0.0, 0.0, 1.0f)); + traj.points.push_back(make_traj_point(50.0, 0.0, 1.0f)); + traj.points.push_back(make_traj_point(100.0, 0.0, 1.0f)); + tester.traj_pub->publish(traj); + + test_utils::waitForMessage(tester.node, this, tester.received_control_command); + + ASSERT_TRUE(tester.received_control_command); + EXPECT_GT(tester.cmd_msg->longitudinal.speed, static_cast(odom_vx)); + EXPECT_GT(tester.cmd_msg->longitudinal.acceleration, 0.0f); // Generate another control message - received_control_command = false; - traj_pub->publish(traj); - test_utils::waitForMessage(node, this, received_control_command); - ASSERT_TRUE(received_control_command); - EXPECT_GT(cmd_msg->longitudinal.speed, static_cast(odom_msg.twist.twist.linear.x)); - EXPECT_GT(cmd_msg->longitudinal.acceleration, 0.0f); + tester.traj_pub->publish(traj); + test_utils::waitForMessage(tester.node, this, tester.received_control_command); + ASSERT_TRUE(tester.received_control_command); + EXPECT_GT(tester.cmd_msg->longitudinal.speed, static_cast(odom_vx)); + EXPECT_GT(tester.cmd_msg->longitudinal.acceleration, 0.0f); } TEST_F(FakeNodeFixture, longitudinal_stopped) { - // Data to test - AckermannControlCommand::SharedPtr cmd_msg; - bool received_control_command = false; - // Node - std::shared_ptr node = makeNode(); - // Publisher/Subscribers - rclcpp::Publisher::SharedPtr traj_pub = - this->create_publisher("controller/input/reference_trajectory"); - rclcpp::Publisher::SharedPtr odom_pub = - this->create_publisher("controller/input/current_odometry"); - rclcpp::Publisher::SharedPtr steer_pub = - this->create_publisher("controller/input/current_steering"); - rclcpp::Subscription::SharedPtr cmd_sub = - this->create_subscription( - "controller/output/control_cmd", *this->get_fake_node(), - [&cmd_msg, &received_control_command](const AckermannControlCommand::SharedPtr msg) { - cmd_msg = msg; - received_control_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 steering - SteeringReport steer_msg; - steer_msg.stamp = node->now(); - steer_msg.steering_tire_angle = 0.0; - steer_pub->publish(steer_msg); + ControllerTester tester(this); + + tester.send_default_transform(); + tester.publish_default_odom(); + tester.publish_default_steer(); + tester.publish_default_acc(); + // Publish stopping trajectory Trajectory traj; - traj.header.stamp = node->now(); + traj.header.stamp = tester.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_control_command); - - ASSERT_TRUE(received_control_command); - EXPECT_DOUBLE_EQ(cmd_msg->longitudinal.speed, 0.0f); + traj.points.push_back(make_traj_point(0.0, 0.0, 0.0f)); + traj.points.push_back(make_traj_point(50.0, 0.0, 0.0f)); + traj.points.push_back(make_traj_point(100.0, 0.0, 0.0f)); + tester.traj_pub->publish(traj); + + test_utils::waitForMessage(tester.node, this, tester.received_control_command); + + ASSERT_TRUE(tester.received_control_command); + EXPECT_DOUBLE_EQ(tester.cmd_msg->longitudinal.speed, 0.0f); EXPECT_LT( - cmd_msg->longitudinal.acceleration, 0.0f); // when stopped negative acceleration to brake + tester.cmd_msg->longitudinal.acceleration, + 0.0f); // when stopped negative acceleration to brake } TEST_F(FakeNodeFixture, longitudinal_reverse) { - // Data to test - AckermannControlCommand::SharedPtr cmd_msg; - bool received_control_command = false; - // Node - std::shared_ptr node = makeNode(); - // Publisher/Subscribers - rclcpp::Publisher::SharedPtr traj_pub = - this->create_publisher("controller/input/reference_trajectory"); - rclcpp::Publisher::SharedPtr odom_pub = - this->create_publisher("controller/input/current_odometry"); - rclcpp::Publisher::SharedPtr steer_pub = - this->create_publisher("controller/input/current_steering"); - rclcpp::Subscription::SharedPtr cmd_sub = - this->create_subscription( - "controller/output/control_cmd", *this->get_fake_node(), - [&cmd_msg, &received_control_command](const AckermannControlCommand::SharedPtr msg) { - cmd_msg = msg; - received_control_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 steering - SteeringReport steer_msg; - steer_msg.stamp = node->now(); - steer_msg.steering_tire_angle = 0.0; - steer_pub->publish(steer_msg); + ControllerTester tester(this); + + tester.send_default_transform(); + + tester.publish_default_odom(); + tester.publish_default_steer(); + tester.publish_default_acc(); + // Publish reverse Trajectory traj; - traj.header.stamp = node->now(); + traj.header.stamp = tester.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_control_command); - - ASSERT_TRUE(received_control_command); - EXPECT_LT(cmd_msg->longitudinal.speed, 0.0f); - EXPECT_GT(cmd_msg->longitudinal.acceleration, 0.0f); + traj.points.push_back(make_traj_point(0.0, 0.0, -1.0f)); + traj.points.push_back(make_traj_point(50.0, 0.0, -1.0f)); + traj.points.push_back(make_traj_point(100.0, 0.0, -1.0f)); + tester.traj_pub->publish(traj); + + test_utils::waitForMessage(tester.node, this, tester.received_control_command); + + ASSERT_TRUE(tester.received_control_command); + EXPECT_LT(tester.cmd_msg->longitudinal.speed, 0.0f); + EXPECT_GT(tester.cmd_msg->longitudinal.acceleration, 0.0f); } TEST_F(FakeNodeFixture, longitudinal_emergency) { - // Data to test - AckermannControlCommand::SharedPtr cmd_msg; - bool received_control_command = false; - // Node - std::shared_ptr node = makeNode(); - // Publisher/Subscribers - rclcpp::Publisher::SharedPtr traj_pub = - this->create_publisher("controller/input/reference_trajectory"); - rclcpp::Publisher::SharedPtr odom_pub = - this->create_publisher("controller/input/current_odometry"); - rclcpp::Publisher::SharedPtr steer_pub = - this->create_publisher("controller/input/current_steering"); - rclcpp::Subscription::SharedPtr cmd_sub = - this->create_subscription( - "controller/output/control_cmd", *this->get_fake_node(), - [&cmd_msg, &received_control_command](const AckermannControlCommand::SharedPtr msg) { - cmd_msg = msg; - received_control_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 steering - SteeringReport steer_msg; - steer_msg.stamp = node->now(); - steer_msg.steering_tire_angle = 0.0; - steer_pub->publish(steer_msg); + ControllerTester tester(this); + + tester.send_default_transform(); + tester.publish_default_odom(); + tester.publish_default_steer(); + tester.publish_default_acc(); + // Publish trajectory starting away from the current ego pose Trajectory traj; - traj.header.stamp = node->now(); + traj.header.stamp = tester.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_control_command); - - ASSERT_TRUE(received_control_command); + traj.points.push_back(make_traj_point(10.0, 0.0, 1.0f)); + traj.points.push_back(make_traj_point(50.0, 0.0, 1.0f)); + traj.points.push_back(make_traj_point(100.0, 0.0, 1.0f)); + tester.traj_pub->publish(traj); + + test_utils::waitForMessage(tester.node, this, tester.received_control_command); + + ASSERT_TRUE(tester.received_control_command); // Emergencies (e.g., far from trajectory) produces braking command (0 vel, negative accel) - EXPECT_DOUBLE_EQ(cmd_msg->longitudinal.speed, 0.0f); - EXPECT_LT(cmd_msg->longitudinal.acceleration, 0.0f); + EXPECT_DOUBLE_EQ(tester.cmd_msg->longitudinal.speed, 0.0f); + EXPECT_LT(tester.cmd_msg->longitudinal.acceleration, 0.0f); } diff --git a/launch/tier4_control_launch/launch/control.launch.py b/launch/tier4_control_launch/launch/control.launch.py index 471c1ad4541c0..044c5e3a8136a 100644 --- a/launch/tier4_control_launch/launch/control.launch.py +++ b/launch/tier4_control_launch/launch/control.launch.py @@ -99,6 +99,7 @@ def launch_setup(context, *args, **kwargs): ("~/input/reference_trajectory", "/planning/scenario_planning/trajectory"), ("~/input/current_odometry", "/localization/kinematic_state"), ("~/input/current_steering", "/vehicle/status/steering_status"), + ("~/input/current_accel", "/localization/acceleration"), ("~/output/predicted_trajectory", "lateral/predicted_trajectory"), ("~/output/lateral_diagnostic", "lateral/diagnostic"), ("~/output/slope_angle", "longitudinal/slope_angle"), diff --git a/launch/tier4_planning_launch/config/scenario_planning/lane_driving/motion_planning/obstacle_cruise_planner/obstacle_cruise_planner.param.yaml b/launch/tier4_planning_launch/config/scenario_planning/lane_driving/motion_planning/obstacle_cruise_planner/obstacle_cruise_planner.param.yaml index 240ea8def4db8..f8f9a85deed5c 100644 --- a/launch/tier4_planning_launch/config/scenario_planning/lane_driving/motion_planning/obstacle_cruise_planner/obstacle_cruise_planner.param.yaml +++ b/launch/tier4_planning_launch/config/scenario_planning/lane_driving/motion_planning/obstacle_cruise_planner/obstacle_cruise_planner.param.yaml @@ -11,8 +11,6 @@ min_object_accel_for_rss: -1.0 # front obstacle's acceleration to calculate RSS distance [m/ss] safe_distance_margin : 6.0 # This is also used as a stop margin [m] - lpf_gain_for_accel: 0.2 # gain of low pass filter for ego acceleration [-] - nearest_dist_deviation_threshold: 3.0 # [m] for finding nearest index nearest_yaw_deviation_threshold: 1.57 # [rad] for finding nearest index min_behavior_stop_margin: 3.0 # [m] diff --git a/launch/tier4_planning_launch/launch/scenario_planning/lane_driving/motion_planning/motion_planning.launch.py b/launch/tier4_planning_launch/launch/scenario_planning/lane_driving/motion_planning/motion_planning.launch.py index f86b9a8858bff..99ab74f0856fe 100644 --- a/launch/tier4_planning_launch/launch/scenario_planning/lane_driving/motion_planning/motion_planning.launch.py +++ b/launch/tier4_planning_launch/launch/scenario_planning/lane_driving/motion_planning/motion_planning.launch.py @@ -160,6 +160,7 @@ def launch_setup(context, *args, **kwargs): "/planning/scenario_planning/clear_velocity_limit", ), ("~/output/trajectory", "/planning/scenario_planning/lane_driving/trajectory"), + ("~/input/acceleration", "/localization/acceleration"), ( "~/input/pointcloud", "/perception/obstacle_segmentation/pointcloud", @@ -198,6 +199,7 @@ def launch_setup(context, *args, **kwargs): remappings=[ ("~/input/trajectory", "obstacle_avoidance_planner/trajectory"), ("~/input/odometry", "/localization/kinematic_state"), + ("~/input/acceleration", "/localization/acceleration"), ("~/input/objects", "/perception/object_recognition/objects"), ("~/output/trajectory", "/planning/scenario_planning/lane_driving/trajectory"), ("~/output/velocity_limit", "/planning/scenario_planning/max_velocity_candidates"), diff --git a/planning/obstacle_cruise_planner/config/obstacle_cruise_planner.param.yaml b/planning/obstacle_cruise_planner/config/obstacle_cruise_planner.param.yaml index 240ea8def4db8..f8f9a85deed5c 100644 --- a/planning/obstacle_cruise_planner/config/obstacle_cruise_planner.param.yaml +++ b/planning/obstacle_cruise_planner/config/obstacle_cruise_planner.param.yaml @@ -11,8 +11,6 @@ min_object_accel_for_rss: -1.0 # front obstacle's acceleration to calculate RSS distance [m/ss] safe_distance_margin : 6.0 # This is also used as a stop margin [m] - lpf_gain_for_accel: 0.2 # gain of low pass filter for ego acceleration [-] - nearest_dist_deviation_threshold: 3.0 # [m] for finding nearest index nearest_yaw_deviation_threshold: 1.57 # [rad] for finding nearest index min_behavior_stop_margin: 3.0 # [m] diff --git a/planning/obstacle_cruise_planner/include/obstacle_cruise_planner/node.hpp b/planning/obstacle_cruise_planner/include/obstacle_cruise_planner/node.hpp index 40426bb2e8c12..7570b1ad12274 100644 --- a/planning/obstacle_cruise_planner/include/obstacle_cruise_planner/node.hpp +++ b/planning/obstacle_cruise_planner/include/obstacle_cruise_planner/node.hpp @@ -26,6 +26,8 @@ #include "autoware_auto_perception_msgs/msg/predicted_object.hpp" #include "autoware_auto_perception_msgs/msg/predicted_objects.hpp" #include "autoware_auto_planning_msgs/msg/trajectory.hpp" +#include "geometry_msgs/msg/accel_stamped.hpp" +#include "geometry_msgs/msg/accel_with_covariance_stamped.hpp" #include "geometry_msgs/msg/point_stamped.hpp" #include "geometry_msgs/msg/transform_stamped.hpp" #include "geometry_msgs/msg/twist_stamped.hpp" @@ -48,6 +50,8 @@ using autoware_auto_perception_msgs::msg::PredictedObjects; using autoware_auto_perception_msgs::msg::PredictedPath; using autoware_auto_planning_msgs::msg::Trajectory; using autoware_auto_planning_msgs::msg::TrajectoryPoint; +using geometry_msgs::msg::AccelStamped; +using geometry_msgs::msg::AccelWithCovarianceStamped; using nav_msgs::msg::Odometry; using tier4_debug_msgs::msg::Float32Stamped; using tier4_planning_msgs::msg::StopReasonArray; @@ -68,6 +72,7 @@ class ObstacleCruisePlannerNode : public rclcpp::Node const std::vector & parameters); void onObjects(const PredictedObjects::ConstSharedPtr msg); void onOdometry(const Odometry::ConstSharedPtr); + void onAccel(const AccelWithCovarianceStamped::ConstSharedPtr); void onTrajectory(const Trajectory::ConstSharedPtr msg); void onSmoothedTrajectory(const Trajectory::ConstSharedPtr msg); @@ -78,7 +83,6 @@ class ObstacleCruisePlannerNode : public rclcpp::Node ObstacleCruisePlannerData createStopData( const Trajectory & trajectory, const geometry_msgs::msg::Pose & current_pose, const std::vector & obstacles, const bool is_driving_forward); - double calcCurrentAccel() const; std::vector getTargetObstacles( const Trajectory & trajectory, const geometry_msgs::msg::Pose & current_pose, const double current_vel, const bool is_driving_forward, DebugData & debug_data); @@ -131,6 +135,7 @@ class ObstacleCruisePlannerNode : public rclcpp::Node rclcpp::Subscription::SharedPtr smoothed_trajectory_sub_; rclcpp::Subscription::SharedPtr objects_sub_; rclcpp::Subscription::SharedPtr odom_sub_; + rclcpp::Subscription::SharedPtr acc_sub_; // self pose listener tier4_autoware_utils::SelfPoseListener self_pose_listener_; @@ -138,10 +143,8 @@ class ObstacleCruisePlannerNode : public rclcpp::Node // data for callback functions PredictedObjects::ConstSharedPtr in_objects_ptr_; geometry_msgs::msg::TwistStamped::SharedPtr current_twist_ptr_; - geometry_msgs::msg::TwistStamped::SharedPtr prev_twist_ptr_; - // low pass filter of acceleration - std::shared_ptr lpf_acc_ptr_; + geometry_msgs::msg::AccelStamped::SharedPtr current_accel_ptr_; // Vehicle Parameters VehicleInfo vehicle_info_; diff --git a/planning/obstacle_cruise_planner/src/node.cpp b/planning/obstacle_cruise_planner/src/node.cpp index 5b3bbc15cfa91..0b970f35d2069 100644 --- a/planning/obstacle_cruise_planner/src/node.cpp +++ b/planning/obstacle_cruise_planner/src/node.cpp @@ -196,7 +196,6 @@ ObstacleCruisePlannerNode::ObstacleCruisePlannerNode(const rclcpp::NodeOptions & : Node("obstacle_cruise_planner", node_options), self_pose_listener_(this), in_objects_ptr_(nullptr), - lpf_acc_ptr_(nullptr), vehicle_info_(vehicle_info_util::VehicleInfoUtil(*this).getVehicleInfo()) { using std::placeholders::_1; @@ -213,6 +212,9 @@ ObstacleCruisePlannerNode::ObstacleCruisePlannerNode(const rclcpp::NodeOptions & odom_sub_ = create_subscription( "~/input/odometry", rclcpp::QoS{1}, std::bind(&ObstacleCruisePlannerNode::onOdometry, this, std::placeholders::_1)); + acc_sub_ = create_subscription( + "~/input/acceleration", rclcpp::QoS{1}, + std::bind(&ObstacleCruisePlannerNode::onAccel, this, std::placeholders::_1)); // publisher trajectory_pub_ = create_publisher("~/output/trajectory", 1); @@ -261,10 +263,6 @@ ObstacleCruisePlannerNode::ObstacleCruisePlannerNode(const rclcpp::NodeOptions & is_showing_debug_info_ = declare_parameter("common.is_showing_debug_info"); - // low pass filter for ego acceleration - const double lpf_gain_for_accel = declare_parameter("common.lpf_gain_for_accel"); - lpf_acc_ptr_ = std::make_shared(lpf_gain_for_accel); - { // Obstacle filtering parameters obstacle_filtering_param_.rough_detection_area_expand_width = declare_parameter("obstacle_filtering.rough_detection_area_expand_width"); @@ -503,15 +501,18 @@ void ObstacleCruisePlannerNode::onObjects(const PredictedObjects::ConstSharedPtr void ObstacleCruisePlannerNode::onOdometry(const Odometry::ConstSharedPtr msg) { - if (current_twist_ptr_) { - prev_twist_ptr_ = current_twist_ptr_; - } - current_twist_ptr_ = std::make_unique(); current_twist_ptr_->header = msg->header; current_twist_ptr_->twist = msg->twist.twist; } +void ObstacleCruisePlannerNode::onAccel(const AccelWithCovarianceStamped::ConstSharedPtr msg) +{ + current_accel_ptr_ = std::make_unique(); + current_accel_ptr_->header = msg->header; + current_accel_ptr_->accel = msg->accel.accel; +} + void ObstacleCruisePlannerNode::onSmoothedTrajectory(const Trajectory::ConstSharedPtr msg) { planner_ptr_->setSmoothedTrajectory(msg); @@ -522,9 +523,7 @@ void ObstacleCruisePlannerNode::onTrajectory(const Trajectory::ConstSharedPtr ms const auto current_pose_ptr = self_pose_listener_.getCurrentPose(); // check if subscribed variables are ready - if ( - msg->points.empty() || !current_twist_ptr_ || !prev_twist_ptr_ || !in_objects_ptr_ || - !current_pose_ptr) { + if (msg->points.empty() || !current_twist_ptr_ || !in_objects_ptr_ || !current_pose_ptr) { return; } @@ -589,7 +588,7 @@ ObstacleCruisePlannerData ObstacleCruisePlannerNode::createStopData( { const auto current_time = now(); const double current_vel = current_twist_ptr_->twist.linear.x; - const double current_accel = calcCurrentAccel(); + const double current_accel = current_accel_ptr_->accel.linear.x; // create planner_stop data ObstacleCruisePlannerData planner_data; @@ -629,7 +628,7 @@ ObstacleCruisePlannerData ObstacleCruisePlannerNode::createCruiseData( { const auto current_time = now(); const double current_vel = current_twist_ptr_->twist.linear.x; - const double current_accel = calcCurrentAccel(); + const double current_accel = current_accel_ptr_->accel.linear.x; // create planner_stop data ObstacleCruisePlannerData planner_data; @@ -648,19 +647,6 @@ ObstacleCruisePlannerData ObstacleCruisePlannerNode::createCruiseData( return planner_data; } -double ObstacleCruisePlannerNode::calcCurrentAccel() const -{ - const double diff_vel = current_twist_ptr_->twist.linear.x - prev_twist_ptr_->twist.linear.x; - const double diff_time = std::max( - (rclcpp::Time(current_twist_ptr_->header.stamp) - rclcpp::Time(prev_twist_ptr_->header.stamp)) - .seconds(), - 1e-03); - - const double accel = diff_vel / diff_time; - - return lpf_acc_ptr_->filter(accel); -} - std::vector ObstacleCruisePlannerNode::getTargetObstacles( const Trajectory & trajectory, const geometry_msgs::msg::Pose & current_pose, const double current_vel, const bool is_driving_forward, DebugData & debug_data) diff --git a/planning/obstacle_stop_planner/README.md b/planning/obstacle_stop_planner/README.md index 3b0a38d194a78..a76b8a9b814f4 100644 --- a/planning/obstacle_stop_planner/README.md +++ b/planning/obstacle_stop_planner/README.md @@ -36,7 +36,6 @@ | `enable_slow_down` | bool | enable slow down planner [-] | | `max_velocity` | double | max velocity [m/s] | | `hunting_threshold` | double | even if the obstacle disappears, the stop judgment continues for hunting_threshold [s] | -| `lowpass_gain` | double | low pass gain for calculating acceleration [-] | ## Obstacle Stop Planner diff --git a/planning/obstacle_stop_planner/config/obstacle_stop_planner.param.yaml b/planning/obstacle_stop_planner/config/obstacle_stop_planner.param.yaml index ed4e632ebd6b3..ea506d349dc34 100644 --- a/planning/obstacle_stop_planner/config/obstacle_stop_planner.param.yaml +++ b/planning/obstacle_stop_planner/config/obstacle_stop_planner.param.yaml @@ -1,7 +1,6 @@ /**: ros__parameters: hunting_threshold: 0.5 # even if the obstacle disappears, the stop judgment continues for hunting_threshold [s] - lowpass_gain: 0.9 # gain parameter for low pass filter [-] max_velocity: 20.0 # max velocity [m/s] enable_slow_down: False # whether to use slow down planner [-] diff --git a/planning/obstacle_stop_planner/include/obstacle_stop_planner/node.hpp b/planning/obstacle_stop_planner/include/obstacle_stop_planner/node.hpp index 1bd3deb70da5a..2312400065eaf 100644 --- a/planning/obstacle_stop_planner/include/obstacle_stop_planner/node.hpp +++ b/planning/obstacle_stop_planner/include/obstacle_stop_planner/node.hpp @@ -26,7 +26,6 @@ #include #include #include -#include #include #include #include @@ -34,6 +33,8 @@ #include #include #include +#include +#include #include #include #include @@ -66,6 +67,7 @@ namespace bg = boost::geometry; using diagnostic_msgs::msg::DiagnosticStatus; using diagnostic_msgs::msg::KeyValue; +using geometry_msgs::msg::AccelWithCovarianceStamped; using geometry_msgs::msg::Point; using geometry_msgs::msg::Pose; using geometry_msgs::msg::TransformStamped; @@ -101,6 +103,8 @@ class ObstacleStopPlannerNode : public rclcpp::Node rclcpp::Subscription::SharedPtr sub_odometry_; + rclcpp::Subscription::SharedPtr sub_acceleration_; + rclcpp::Subscription::SharedPtr sub_dynamic_objects_; rclcpp::Subscription::SharedPtr sub_expand_stop_range_; @@ -115,7 +119,6 @@ class ObstacleStopPlannerNode : public rclcpp::Node std::unique_ptr acc_controller_; std::shared_ptr debug_ptr_; - std::shared_ptr lpf_acc_{nullptr}; boost::optional latest_stop_point_{boost::none}; boost::optional latest_slow_down_section_{boost::none}; tf2_ros::Buffer tf_buffer_{get_clock()}; @@ -126,8 +129,7 @@ class ObstacleStopPlannerNode : public rclcpp::Node rclcpp::Time last_detect_time_slowdown_point_; Odometry::ConstSharedPtr current_velocity_ptr_{nullptr}; - Odometry::ConstSharedPtr prev_velocity_ptr_{nullptr}; - double current_acc_{0.0}; + AccelWithCovarianceStamped::ConstSharedPtr current_acceleration_ptr_{nullptr}; bool is_driving_forward_{true}; bool set_velocity_limit_{false}; @@ -137,9 +139,9 @@ class ObstacleStopPlannerNode : public rclcpp::Node StopParam stop_param_; SlowDownParam slow_down_param_; - // mutex for vehicle_info_, stop_param_, current_acc_, lpf_acc_, obstacle_ros_pointcloud_ptr_ + // mutex for vehicle_info_, stop_param_, current_acc_, obstacle_ros_pointcloud_ptr_ // NOTE: shared_ptr itself is thread safe so we do not have to care if *ptr is not used - // (current_velocity_ptr_, prev_velocity_ptr_) + // (current_velocity_ptr_) std::mutex mutex_; void searchObstacle( @@ -191,6 +193,8 @@ class ObstacleStopPlannerNode : public rclcpp::Node void onOdometry(const Odometry::ConstSharedPtr input_msg); + void onAcceleration(const AccelWithCovarianceStamped::ConstSharedPtr input_msg); + void onPointCloud(const PointCloud2::ConstSharedPtr input_msg); void onDynamicObjects(const PredictedObjects::ConstSharedPtr input_msg); diff --git a/planning/obstacle_stop_planner/include/obstacle_stop_planner/planner_data.hpp b/planning/obstacle_stop_planner/include/obstacle_stop_planner/planner_data.hpp index 6fee4a049a541..9ce3bd6213b88 100644 --- a/planning/obstacle_stop_planner/include/obstacle_stop_planner/planner_data.hpp +++ b/planning/obstacle_stop_planner/include/obstacle_stop_planner/planner_data.hpp @@ -62,9 +62,6 @@ struct NodeParam // max velocity [m/s] double max_velocity; - // smoothing calculated current acceleration [-] - double lowpass_gain; - // keep slow down or stop state if obstacle vanished [s] double hunting_threshold; diff --git a/planning/obstacle_stop_planner/src/node.cpp b/planning/obstacle_stop_planner/src/node.cpp index 833ece2d16527..0166a56d3c322 100644 --- a/planning/obstacle_stop_planner/src/node.cpp +++ b/planning/obstacle_stop_planner/src/node.cpp @@ -66,8 +66,6 @@ ObstacleStopPlannerNode::ObstacleStopPlannerNode(const rclcpp::NodeOptions & nod p.enable_slow_down = declare_parameter("enable_slow_down"); p.max_velocity = declare_parameter("max_velocity"); p.hunting_threshold = declare_parameter("hunting_threshold"); - p.lowpass_gain = declare_parameter("lowpass_gain"); - lpf_acc_ = std::make_shared(p.lowpass_gain); p.ego_nearest_dist_threshold = declare_parameter("ego_nearest_dist_threshold"); p.ego_nearest_yaw_threshold = declare_parameter("ego_nearest_yaw_threshold"); } @@ -181,6 +179,11 @@ ObstacleStopPlannerNode::ObstacleStopPlannerNode(const rclcpp::NodeOptions & nod std::bind(&ObstacleStopPlannerNode::onOdometry, this, std::placeholders::_1), createSubscriptionOptions(this)); + sub_acceleration_ = this->create_subscription( + "~/input/acceleration", 1, + std::bind(&ObstacleStopPlannerNode::onAcceleration, this, std::placeholders::_1), + createSubscriptionOptions(this)); + sub_dynamic_objects_ = this->create_subscription( "~/input/objects", 1, std::bind(&ObstacleStopPlannerNode::onDynamicObjects, this, std::placeholders::_1), @@ -223,31 +226,35 @@ void ObstacleStopPlannerNode::onTrigger(const Trajectory::ConstSharedPtr input_m const auto vehicle_info = vehicle_info_; const auto stop_param = stop_param_; const auto obstacle_ros_pointcloud_ptr = obstacle_ros_pointcloud_ptr_; - const auto current_vel = current_velocity_ptr_->twist.twist.linear.x; - const auto current_acc = current_acc_; + const auto object_ptr = object_ptr_; + const auto current_velocity_ptr = current_velocity_ptr_; + const auto current_acceleration_ptr = current_acceleration_ptr_; mutex_.unlock(); { - std::lock_guard lock(mutex_); - - if (!object_ptr_) { + const auto waiting = [this](const auto & str) { RCLCPP_WARN_THROTTLE( - get_logger(), *get_clock(), std::chrono::milliseconds(1000).count(), - "waiting for dynamic objects..."); + get_logger(), *get_clock(), std::chrono::milliseconds(1000).count(), "waiting for %s ...", + str); + }; + + if (!object_ptr) { + waiting("perception object"); return; } if (!obstacle_ros_pointcloud_ptr) { - RCLCPP_WARN_THROTTLE( - get_logger(), *get_clock(), std::chrono::milliseconds(1000).count(), - "waiting for obstacle pointcloud..."); + waiting("obstacle pointcloud"); return; } - if (!current_velocity_ptr_ && node_param_.enable_slow_down) { - RCLCPP_WARN_THROTTLE( - get_logger(), *get_clock(), std::chrono::milliseconds(1000).count(), - "waiting for current velocity..."); + if (!current_velocity_ptr) { + waiting("current velocity"); + return; + } + + if (!current_acceleration_ptr) { + waiting("current acceleration"); return; } @@ -256,6 +263,9 @@ void ObstacleStopPlannerNode::onTrigger(const Trajectory::ConstSharedPtr input_m } } + const auto current_vel = current_velocity_ptr->twist.twist.linear.x; + const auto current_acc = current_acceleration_ptr->accel.accel.linear.x; + // TODO(someone): support backward path const auto is_driving_forward = motion_utils::isDrivingForwardWithTwist(input_msg->points); is_driving_forward_ = is_driving_forward ? is_driving_forward.get() : is_driving_forward_; @@ -816,26 +826,14 @@ void ObstacleStopPlannerNode::onOdometry(const Odometry::ConstSharedPtr input_ms { // mutex for current_acc_, lpf_acc_ std::lock_guard lock(mutex_); - current_velocity_ptr_ = input_msg; +} - if (!prev_velocity_ptr_) { - prev_velocity_ptr_ = current_velocity_ptr_; - return; - } - - const double dv = - current_velocity_ptr_->twist.twist.linear.x - prev_velocity_ptr_->twist.twist.linear.x; - const double dt = std::max( - (rclcpp::Time(current_velocity_ptr_->header.stamp) - - rclcpp::Time(prev_velocity_ptr_->header.stamp)) - .seconds(), - 1e-03); - - const double accel = dv / dt; - - current_acc_ = lpf_acc_->filter(accel); - prev_velocity_ptr_ = current_velocity_ptr_; +void ObstacleStopPlannerNode::onAcceleration( + const AccelWithCovarianceStamped::ConstSharedPtr input_msg) +{ + std::lock_guard lock(mutex_); + current_acceleration_ptr_ = input_msg; } TrajectoryPoints ObstacleStopPlannerNode::trimTrajectoryWithIndexFromSelfPose(