From 398dc28705530e224331bd500f8ea9c0566adacb Mon Sep 17 00:00:00 2001 From: "pre-commit-ci[bot]" <66853113+pre-commit-ci[bot]@users.noreply.github.com> Date: Thu, 23 Jun 2022 00:28:16 +0000 Subject: [PATCH] ci(pre-commit): autofix --- .../design/recordreplay_planner-design.md | 26 ++-- .../recordreplay_planner.hpp | 32 ++--- .../visibility_control.hpp | 24 ++-- planning/recordreplay_planner/package.xml | 8 +- .../recordreplay_planner.cpp | 115 ++++++--------- .../test/sanity_checks.cpp | 135 ++++++++---------- .../recordreplay_planner_nodes-design.md | 31 ++-- .../recordreplay_planner_node.hpp | 36 +++-- .../visibility_control.hpp | 25 ++-- .../recordreplay_planner_node.launch.py | 26 ++-- .../recordreplay_planner_nodes/package.xml | 14 +- .../param/test.param.yaml | 2 +- .../recordreplay_planner_node.cpp | 88 +++++------- .../test/recordreplay_planner_node.test.py | 22 ++- .../test/sanity_checks.cpp | 31 ++-- 15 files changed, 260 insertions(+), 355 deletions(-) diff --git a/planning/recordreplay_planner/design/recordreplay_planner-design.md b/planning/recordreplay_planner/design/recordreplay_planner-design.md index cf515d22841ca..00d25d6ddde74 100644 --- a/planning/recordreplay_planner/design/recordreplay_planner-design.md +++ b/planning/recordreplay_planner/design/recordreplay_planner-design.md @@ -1,5 +1,4 @@ -RecordReplay planner {#recordreplay-planner} -==================== +# RecordReplay planner {#recordreplay-planner} # Purpose / Use cases @@ -10,7 +9,6 @@ driving a trajectory, then reset vehicle position, then start replay and see wha The trajectories can be a loop , which can be determined naively by checking the distance between the first and last point of the trajectory, but the user has the ultimate control over toggling this behavior. - # Design Recording will add states at the end of an internal list of states. @@ -27,10 +25,10 @@ There is no interpolation between points along the trajectory, and localization The list of recorded states is simply iterated over. The stopping concept only works if one can assume that the downstream controller and the vehicle are able -to track the desired velocity going to zero in a single trajectory step. +to track the desired velocity going to zero in a single trajectory step. Making sure this assumption is satisfied by construction would involve creating a dynamically feasible -velocity profile for stopping - this has not been done yet. +velocity profile for stopping - this has not been done yet. It is the user's responsibility to check if the trajectory is suitable for looping, and call `set_loop` accordingly to toggle the looping behavior with regard to the current trajectory that the controller is replaying. `is_loop`, @@ -47,30 +45,28 @@ the path file that the user obtained while recording. Inputs: -* `VehicleKinematicState.msg` is the state that gets recorded -* `BoundingBoxArray.msg` is a list of bounding boxes of obstacles +- `VehicleKinematicState.msg` is the state that gets recorded +- `BoundingBoxArray.msg` is a list of bounding boxes of obstacles Outputs: -* `Trajectory.msg` is the trajectory that gets published - +- `Trajectory.msg` is the trajectory that gets published ## Complexity Recording is `O(1)` in time and `O(n)` in space, replay is `O(n)` in both time and space, where `n` is the number of recorded states. Collision checking currently happens on every replay even if obstacles do not -change, and has a complexity that is linear in the number of obstacles but proportional to the product of +change, and has a complexity that is linear in the number of obstacles but proportional to the product of the number of halfplanes in the ego vehicle and a single obstacle. -# Security considerations +# Security considerations TBD by a security specialist. # Future extensions / Unimplemented parts -* Trajectory buffer clearing -* Proper `tf` support -* rosbag2 support for recording and later replaying +- Trajectory buffer clearing +- Proper `tf` support +- rosbag2 support for recording and later replaying # Related issues - diff --git a/planning/recordreplay_planner/include/recordreplay_planner/recordreplay_planner.hpp b/planning/recordreplay_planner/include/recordreplay_planner/recordreplay_planner.hpp index bea4e1a5b2cf2..121f1e32311a5 100644 --- a/planning/recordreplay_planner/include/recordreplay_planner/recordreplay_planner.hpp +++ b/planning/recordreplay_planner/include/recordreplay_planner/recordreplay_planner.hpp @@ -15,14 +15,15 @@ #ifndef RECORDREPLAY_PLANNER__RECORDREPLAY_PLANNER_HPP_ #define RECORDREPLAY_PLANNER__RECORDREPLAY_PLANNER_HPP_ +#include #include -#include + #include -#include +#include #include -#include #include +#include using autoware::common::types::bool8_t; using autoware::common::types::float64_t; @@ -36,12 +37,7 @@ namespace recordreplay_planner using State = autoware_auto_vehicle_msgs::msg::VehicleKinematicState; using autoware_auto_planning_msgs::msg::Trajectory; -enum class RecordReplayState -{ - IDLE, - RECORDING, - REPLAYING -}; // enum class RecordReplayState +enum class RecordReplayState { IDLE, RECORDING, REPLAYING }; // enum class RecordReplayState /// \brief A class for recording trajectories and replaying them as plans class RECORDREPLAY_PLANNER_PUBLIC RecordReplayPlanner @@ -95,9 +91,10 @@ class RECORDREPLAY_PLANNER_PUBLIC RecordReplayPlanner /** * \brief Judges whether current_state has reached the last point in record buffer * \param current_state current state of the vehicle - * \param distance_thresh threshold of euclidean distance between the current state the and the last point in meters - * \param angle_thresh threshold of difference in the headings of the current_state and the last point in radians - * \return true if both distance and angle conditions are satisfied + * \param distance_thresh threshold of euclidean distance between the current state the and the + * last point in meters \param angle_thresh threshold of difference in the headings of the + * current_state and the last point in radians \return true if both distance and angle conditions + * are satisfied */ bool8_t reached_goal( const State & current_state, const float64_t & distance_thresh, @@ -108,14 +105,9 @@ class RECORDREPLAY_PLANNER_PUBLIC RecordReplayPlanner * \param distance_thresh threshold of euclidean state between the first and last points in meters * \return true if the distance threshold is met */ - bool8_t is_loop( - const float64_t & distance_thresh - ) const; - - auto get_record_buffer() - { - return m_record_buffer; - } + bool8_t is_loop(const float64_t & distance_thresh) const; + + auto get_record_buffer() { return m_record_buffer; } private: // Obtain a trajectory from the internally-stored recording buffer diff --git a/planning/recordreplay_planner/include/recordreplay_planner/visibility_control.hpp b/planning/recordreplay_planner/include/recordreplay_planner/visibility_control.hpp index d051982efb3ca..a54cac5a4efa1 100644 --- a/planning/recordreplay_planner/include/recordreplay_planner/visibility_control.hpp +++ b/planning/recordreplay_planner/include/recordreplay_planner/visibility_control.hpp @@ -16,21 +16,21 @@ #define RECORDREPLAY_PLANNER__VISIBILITY_CONTROL_HPP_ #if defined(__WIN32) - #if defined(RECORDREPLAY_PLANNER_BUILDING_DLL) || defined(RECORDREPLAY_PLANNER_EXPORTS) - #define RECORDREPLAY_PLANNER_PUBLIC __declspec(dllexport) - #define RECORDREPLAY_PLANNER_LOCAL - #else // defined(RECORDREPLAY_PLANNER_BUILDING_DLL) || defined(RECORDREPLAY_PLANNER_EXPORTS) - #define RECORDREPLAY_PLANNER_PUBLIC __declspec(dllimport) - #define RECORDREPLAY_PLANNER_LOCAL - #endif // defined(RECORDREPLAY_PLANNER_BUILDING_DLL) || defined(RECORDREPLAY_PLANNER_EXPORTS) +#if defined(RECORDREPLAY_PLANNER_BUILDING_DLL) || defined(RECORDREPLAY_PLANNER_EXPORTS) +#define RECORDREPLAY_PLANNER_PUBLIC __declspec(dllexport) +#define RECORDREPLAY_PLANNER_LOCAL +#else // defined(RECORDREPLAY_PLANNER_BUILDING_DLL) || defined(RECORDREPLAY_PLANNER_EXPORTS) +#define RECORDREPLAY_PLANNER_PUBLIC __declspec(dllimport) +#define RECORDREPLAY_PLANNER_LOCAL +#endif // defined(RECORDREPLAY_PLANNER_BUILDING_DLL) || defined(RECORDREPLAY_PLANNER_EXPORTS) #elif defined(__linux__) - #define RECORDREPLAY_PLANNER_PUBLIC __attribute__((visibility("default"))) - #define RECORDREPLAY_PLANNER_LOCAL __attribute__((visibility("hidden"))) +#define RECORDREPLAY_PLANNER_PUBLIC __attribute__((visibility("default"))) +#define RECORDREPLAY_PLANNER_LOCAL __attribute__((visibility("hidden"))) #elif defined(__APPLE__) - #define RECORDREPLAY_PLANNER_PUBLIC __attribute__((visibility("default"))) - #define RECORDREPLAY_PLANNER_LOCAL __attribute__((visibility("hidden"))) +#define RECORDREPLAY_PLANNER_PUBLIC __attribute__((visibility("default"))) +#define RECORDREPLAY_PLANNER_LOCAL __attribute__((visibility("hidden"))) #else // defined(_LINUX) - #error "Unsupported Build Configuration" +#error "Unsupported Build Configuration" #endif // defined(_WINDOWS) #endif // RECORDREPLAY_PLANNER__VISIBILITY_CONTROL_HPP_ diff --git a/planning/recordreplay_planner/package.xml b/planning/recordreplay_planner/package.xml index af17da6a705b5..aa3c26e0856ef 100644 --- a/planning/recordreplay_planner/package.xml +++ b/planning/recordreplay_planner/package.xml @@ -3,9 +3,7 @@ recordreplay_planner 1.0.0 - - RecordReplay trajectory planner - + RecordReplay trajectory planner Embotech AG Apache License 2.0 @@ -23,5 +21,7 @@ ament_lint_auto ament_lint_common - ament_cmake + + ament_cmake + diff --git a/planning/recordreplay_planner/src/recordreplay_planner/recordreplay_planner.cpp b/planning/recordreplay_planner/src/recordreplay_planner/recordreplay_planner.cpp index 11ef96c7d9f9b..cfc0bbe6e47be 100644 --- a/planning/recordreplay_planner/src/recordreplay_planner/recordreplay_planner.cpp +++ b/planning/recordreplay_planner/src/recordreplay_planner/recordreplay_planner.cpp @@ -14,25 +14,26 @@ #include "recordreplay_planner/recordreplay_planner.hpp" -#include -#include #include +#include + +#include #include #include #include #include +#include #include #include #include #include -#include using autoware::common::types::bool8_t; using autoware::common::types::char8_t; -using autoware::common::types::uchar8_t; using autoware::common::types::float32_t; using autoware::common::types::float64_t; +using autoware::common::types::uchar8_t; using Csv = std::vector>; using Association = std::map; namespace @@ -69,8 +70,7 @@ void deleteUnit(std::string & string) } bool loadData( - const std::string & file_name, Association & label_row_association_map, - Csv & file_data) + const std::string & file_name, Association & label_row_association_map, Csv & file_data) { file_data.clear(); label_row_association_map.clear(); @@ -146,17 +146,13 @@ void RecordReplayPlanner::stop_replaying() noexcept m_recordreplaystate = RecordReplayState::IDLE; } -void RecordReplayPlanner::clear_record() noexcept -{ - m_record_buffer.clear(); -} +void RecordReplayPlanner::clear_record() noexcept { m_record_buffer.clear(); } std::size_t RecordReplayPlanner::get_record_length() const noexcept { return m_record_buffer.size(); } - void RecordReplayPlanner::set_heading_weight(float64_t heading_weight) { if (heading_weight < 0.0) { @@ -165,10 +161,7 @@ void RecordReplayPlanner::set_heading_weight(float64_t heading_weight) m_heading_weight = heading_weight; } -float64_t RecordReplayPlanner::get_heading_weight() -{ - return m_heading_weight; -} +float64_t RecordReplayPlanner::get_heading_weight() { return m_heading_weight; } void RecordReplayPlanner::set_min_record_distance(float64_t min_record_distance) { @@ -178,10 +171,7 @@ void RecordReplayPlanner::set_min_record_distance(float64_t min_record_distance) m_min_record_distance = min_record_distance; } -float64_t RecordReplayPlanner::get_min_record_distance() const -{ - return m_min_record_distance; -} +float64_t RecordReplayPlanner::get_min_record_distance() const { return m_min_record_distance; } void RecordReplayPlanner::set_skip_first_velocity(const bool8_t skip_first_velocity) { @@ -198,11 +188,11 @@ bool RecordReplayPlanner::record_state(const State & state_to_record) auto previous_state = m_record_buffer.back(); auto distance_sq = (state_to_record.state.pose.position.x - previous_state.state.pose.position.x) * - (state_to_record.state.pose.position.x - previous_state.state.pose.position.x) + + (state_to_record.state.pose.position.x - previous_state.state.pose.position.x) + (state_to_record.state.pose.position.y - previous_state.state.pose.position.y) * - (state_to_record.state.pose.position.y - previous_state.state.pose.position.y); + (state_to_record.state.pose.position.y - previous_state.state.pose.position.y); - if (distance_sq >= (m_min_record_distance * m_min_record_distance) ) { + if (distance_sq >= (m_min_record_distance * m_min_record_distance)) { m_record_buffer.push_back(state_to_record); return true; } else { @@ -215,34 +205,27 @@ const Trajectory & RecordReplayPlanner::plan(const State & current_state) return from_record(current_state); } - std::size_t RecordReplayPlanner::get_closest_state(const State & current_state) { // Find the closest point to the current state in the stored states buffer - const auto distance_from_current_state = - [this, ¤t_state](State & other_state) { - const auto s1 = current_state.state, s2 = other_state.state; - return (s1.pose.position.x - s2.pose.position.x) * (s1.pose.position.x - s2.pose.position.x) + - (s1.pose.position.y - s2.pose.position.y) * (s1.pose.position.y - s2.pose.position.y) + - m_heading_weight * std::abs( - tier4_autoware_utils::getRPY( - s1.pose.orientation - s2.pose.orientation).z); - }; - const auto comparison_function = - [&distance_from_current_state](State & one, State & two) - {return distance_from_current_state(one) < distance_from_current_state(two);}; - + const auto distance_from_current_state = [this, ¤t_state](State & other_state) { + const auto s1 = current_state.state, s2 = other_state.state; + return (s1.pose.position.x - s2.pose.position.x) * (s1.pose.position.x - s2.pose.position.x) + + (s1.pose.position.y - s2.pose.position.y) * (s1.pose.position.y - s2.pose.position.y) + + m_heading_weight * + std::abs(tier4_autoware_utils::getRPY(s1.pose.orientation - s2.pose.orientation).z); + }; + const auto comparison_function = [&distance_from_current_state](State & one, State & two) { + return distance_from_current_state(one) < distance_from_current_state(two); + }; const auto minimum_index_iterator = - std::min_element( - std::begin(m_record_buffer), std::end(m_record_buffer), - comparison_function); + std::min_element(std::begin(m_record_buffer), std::end(m_record_buffer), comparison_function); auto minimum_idx = std::distance(std::begin(m_record_buffer), minimum_index_iterator); return static_cast(minimum_idx); } - const Trajectory & RecordReplayPlanner::from_record(const State & current_state) { // Find out where on the recorded buffer we should start replaying @@ -259,12 +242,9 @@ const Trajectory & RecordReplayPlanner::from_record(const State & current_state) // can record - this will either be the distance from // the start index to the end of the recording, or // the maximum length of recording, whichever is shorter. - publication_len = - std::min( - record_length - m_traj_start_idx, trajectory.points.max_size()); + publication_len = std::min(record_length - m_traj_start_idx, trajectory.points.max_size()); } else { - publication_len = - std::min(record_length, trajectory.points.max_size()); + publication_len = std::min(record_length, trajectory.points.max_size()); } m_traj_end_idx = m_traj_start_idx + publication_len; @@ -288,8 +268,8 @@ const Trajectory & RecordReplayPlanner::from_record(const State & current_state) for (std::size_t i = 0; i < publication_len; ++i) { // Make the time spacing of the points match the recorded timing trajectory.points[i] = m_record_buffer[m_traj_start_idx + i].state; - trajectory.points[i].time_from_start = rclcpp::Time( - m_record_buffer[m_traj_start_idx + i].header.stamp) - t0; + trajectory.points[i].time_from_start = + rclcpp::Time(m_record_buffer[m_traj_start_idx + i].header.stamp) - t0; } } else { // We need two loops here - first fills from current position to end of original traj, @@ -359,19 +339,19 @@ void RecordReplayPlanner::writeTrajectoryBufferToFile(const std::string & record if (!ofs.is_open()) { throw std::runtime_error("Could not open file."); } - ofs << "t_sec, t_nanosec, x, y, orientation_x, orientation_y, orientation_z, orientation_w, " << - "longitudinal_velocity_mps, lateral_velocity_mps, acceleration_mps2, heading_rate_rps, " << - "front_wheel_angle_rad, rear_wheel_angle_rad" << std::endl; + ofs << "t_sec, t_nanosec, x, y, orientation_x, orientation_y, orientation_z, orientation_w, " + << "longitudinal_velocity_mps, lateral_velocity_mps, acceleration_mps2, heading_rate_rps, " + << "front_wheel_angle_rad, rear_wheel_angle_rad" << std::endl; for (const auto & trajectory_point : m_record_buffer) { const auto & s = trajectory_point.state; const auto & t = s.time_from_start; - ofs << t.sec << ", " << t.nanosec << ", " << s.pose.position.x << ", " << s.pose.position.y << - ", " << s.pose.orientation.x << ", " << s.pose.orientation.y << ", " << - s.pose.orientation.z << ", " << s.pose.orientation.w << ", " << - s.longitudinal_velocity_mps << ", " << s.lateral_velocity_mps << ", " << - s.acceleration_mps2 << ", " << s.heading_rate_rps << ", " << s.front_wheel_angle_rad << - ", " << s.rear_wheel_angle_rad << std::endl; + ofs << t.sec << ", " << t.nanosec << ", " << s.pose.position.x << ", " << s.pose.position.y + << ", " << s.pose.orientation.x << ", " << s.pose.orientation.y << ", " + << s.pose.orientation.z << ", " << s.pose.orientation.w << ", " + << s.longitudinal_velocity_mps << ", " << s.lateral_velocity_mps << ", " + << s.acceleration_mps2 << ", " << s.heading_rate_rps << ", " << s.front_wheel_angle_rad + << ", " << s.rear_wheel_angle_rad << std::endl; } ofs.close(); } @@ -430,19 +410,12 @@ void RecordReplayPlanner::readTrajectoryBufferFromFile(const std::string & repla } } -void RecordReplayPlanner::set_loop(const bool8_t loop) -{ - m_enable_loop = loop; -} +void RecordReplayPlanner::set_loop(const bool8_t loop) { m_enable_loop = loop; } -bool8_t RecordReplayPlanner::get_loop() const -{ - return m_enable_loop; -} +bool8_t RecordReplayPlanner::get_loop() const { return m_enable_loop; } bool8_t RecordReplayPlanner::reached_goal( - const State & current_state, - const float64_t & distance_thresh, + const State & current_state, const float64_t & distance_thresh, const float64_t & angle_thresh) const { if (m_trajectory.points.empty()) { @@ -456,9 +429,7 @@ bool8_t RecordReplayPlanner::reached_goal( ego_state.pose.position.y - goal_state.pose.position.y); const auto angle_diff_rad = std::abs( tier4_autoware_utils::getRPY(ego_state.pose.orientation - goal_state.pose.orientation).z); - if (distance2d < distance_thresh && - angle_diff_rad < angle_thresh) - { + if (distance2d < distance_thresh && angle_diff_rad < angle_thresh) { return true; } return false; @@ -466,8 +437,7 @@ bool8_t RecordReplayPlanner::reached_goal( // Uses code similar to RecordReplayPlanner::reached_goal(), // but does not check the heading alignment -bool8_t RecordReplayPlanner::is_loop( - const float64_t & distance_thresh) const +bool8_t RecordReplayPlanner::is_loop(const float64_t & distance_thresh) const { bool retval = false; @@ -475,8 +445,7 @@ bool8_t RecordReplayPlanner::is_loop( if (get_record_length() > 1) { const auto & goal_state = m_record_buffer.front().state; const auto & start_state = m_record_buffer.back().state; - const auto distance2d = - std::hypot( + const auto distance2d = std::hypot( start_state.pose.position.x - goal_state.pose.position.x, start_state.pose.position.y - goal_state.pose.position.y); diff --git a/planning/recordreplay_planner/test/sanity_checks.cpp b/planning/recordreplay_planner/test/sanity_checks.cpp index eb34aa2029aa4..c254340a01166 100644 --- a/planning/recordreplay_planner/test/sanity_checks.cpp +++ b/planning/recordreplay_planner/test/sanity_checks.cpp @@ -12,30 +12,31 @@ // See the License for the specific language governing permissions and // limitations under the License. -#include -#include +#include #include +#include + #include #include #include -#include +#include +#include #include +#include #include -#include #include -#include -using motion::planning::recordreplay_planner::RecordReplayPlanner; -using std::chrono::system_clock; -using motion::motion_testing::make_state; -using autoware_auto_planning_msgs::msg::Trajectory; -using autoware_auto_planning_msgs::msg::TrajectoryPoint; -using geometry_msgs::msg::Point32; using autoware::common::types::bool8_t; using autoware::common::types::float32_t; using autoware::common::types::float64_t; +using autoware_auto_planning_msgs::msg::Trajectory; +using autoware_auto_planning_msgs::msg::TrajectoryPoint; +using geometry_msgs::msg::Point32; +using motion::motion_testing::make_state; +using motion::planning::recordreplay_planner::RecordReplayPlanner; +using std::chrono::system_clock; class sanity_checks_base : public ::testing::Test { @@ -43,7 +44,6 @@ class sanity_checks_base : public ::testing::Test RecordReplayPlanner planner_{}; }; - //------------------ Test basic properties of a recorded, then replayed trajectory struct PropertyTestParameters { @@ -51,9 +51,10 @@ struct PropertyTestParameters system_clock::time_point starting_time; }; -class SanityChecksTrajectoryProperties - : public sanity_checks_base, public testing::WithParamInterface -{}; +class SanityChecksTrajectoryProperties : public sanity_checks_base, + public testing::WithParamInterface +{ +}; TEST_P(SanityChecksTrajectoryProperties, Basicproperties) { @@ -66,9 +67,7 @@ TEST_P(SanityChecksTrajectoryProperties, Basicproperties) const auto time_increment = p.time_spacing_ms; const auto v = dx / (1.0e-3F * p.time_spacing_ms.count()); for (uint32_t k = {}; k < N; ++k) { - const auto next_state = make_state( - dx * k, 0.0F, 0.0F, v, 0.0F, 0.0F, - t0 + k * time_increment); + const auto next_state = make_state(dx * k, 0.0F, 0.0F, v, 0.0F, 0.0F, t0 + k * time_increment); planner_.record_state(next_state); } @@ -77,24 +76,22 @@ TEST_P(SanityChecksTrajectoryProperties, Basicproperties) // Test: Check that the plan returned has the expected time length auto trajectory = planner_.plan(make_state(0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, t0)); - float64_t trajectory_time_length = trajectory.points[N - 1].time_from_start.sec + 1e-9F * - trajectory.points[N - 1].time_from_start.nanosec; + float64_t trajectory_time_length = trajectory.points[N - 1].time_from_start.sec + + 1e-9F * trajectory.points[N - 1].time_from_start.nanosec; float64_t endpoint_sec = (1.0F * (N - 1) * time_increment).count() * 1.0e-3; float64_t ep = 1.0e-5; EXPECT_NEAR(trajectory_time_length, endpoint_sec, ep); } INSTANTIATE_TEST_CASE_P( - TrajectoryProperties, - SanityChecksTrajectoryProperties, + TrajectoryProperties, SanityChecksTrajectoryProperties, testing::Values( PropertyTestParameters{std::chrono::milliseconds(100), system_clock::from_time_t({})}, PropertyTestParameters{std::chrono::milliseconds(200), system_clock::from_time_t({})}, PropertyTestParameters{std::chrono::milliseconds(100), system_clock::from_time_t(10)}, PropertyTestParameters{std::chrono::milliseconds(200), system_clock::from_time_t(10)} // cppcheck-suppress syntaxError - ), ); - + ), ); //------------------ Test that length cropping properly works struct LengthTestParameters @@ -103,18 +100,17 @@ struct LengthTestParameters uint32_t number_of_points; }; - -class SanityChecksTrajectoryLength - : public sanity_checks_base, public testing::WithParamInterface -{}; +class SanityChecksTrajectoryLength : public sanity_checks_base, + public testing::WithParamInterface +{ +}; TEST_P(SanityChecksTrajectoryLength, Length) { const auto p = GetParam(); const auto N = p.number_of_points; - const auto dummy_state = make_state( - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - system_clock::from_time_t({})); + const auto dummy_state = + make_state(0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, system_clock::from_time_t({})); for (uint32_t k = {}; k < N; ++k) { planner_.record_state(dummy_state); @@ -125,18 +121,12 @@ TEST_P(SanityChecksTrajectoryLength, Length) auto trajectory = planner_.plan(dummy_state); EXPECT_EQ( - trajectory.points.size(), - std::min(N, static_cast(trajectory.points.max_size()))); + trajectory.points.size(), std::min(N, static_cast(trajectory.points.max_size()))); } INSTANTIATE_TEST_CASE_P( - TrajectoryLength, - SanityChecksTrajectoryLength, - testing::Values( - LengthTestParameters{80}, - LengthTestParameters{200} - ), ); - + TrajectoryLength, SanityChecksTrajectoryLength, + testing::Values(LengthTestParameters{80}, LengthTestParameters{200}), ); // Test setup helper function. This creates a planner and records a trajectory // that goes along the points (0,0), (1,0), .... (N-1,0) with the heading set to @@ -148,16 +138,13 @@ RecordReplayPlanner helper_create_and_record_example(uint32_t N) // Record some states going from for (uint32_t k = {}; k < N; ++k) { - planner.record_state( - make_state( - 1.0F * k, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - t0 + k * std::chrono::milliseconds{100LL})); + planner.record_state(make_state( + 1.0F * k, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, t0 + k * std::chrono::milliseconds{100LL})); } return planner; } - //------------------ Test that "receding horizon" planning properly works: happy case TEST(RecordreplaySanityChecks, RecedingHorizonHappycase) { @@ -209,32 +196,32 @@ TEST(RecordreplaySanityChecks, StateSettingMechanism) auto planner = RecordReplayPlanner{}; // Make sure setting and reading the recording state works - EXPECT_FALSE(planner.is_recording() ); - EXPECT_FALSE(planner.is_replaying() ); + EXPECT_FALSE(planner.is_recording()); + EXPECT_FALSE(planner.is_replaying()); planner.start_recording(); - EXPECT_TRUE(planner.is_recording() ); - EXPECT_FALSE(planner.is_replaying() ); + EXPECT_TRUE(planner.is_recording()); + EXPECT_FALSE(planner.is_replaying()); planner.stop_recording(); - EXPECT_FALSE(planner.is_recording() ); - EXPECT_FALSE(planner.is_replaying() ); + EXPECT_FALSE(planner.is_recording()); + EXPECT_FALSE(planner.is_replaying()); // Make sure setting and reading the replaying state works - EXPECT_FALSE(planner.is_recording() ); - EXPECT_FALSE(planner.is_replaying() ); + EXPECT_FALSE(planner.is_recording()); + EXPECT_FALSE(planner.is_replaying()); planner.start_replaying(); - EXPECT_FALSE(planner.is_recording() ); - EXPECT_TRUE(planner.is_replaying() ); + EXPECT_FALSE(planner.is_recording()); + EXPECT_TRUE(planner.is_replaying()); planner.stop_replaying(); - EXPECT_FALSE(planner.is_recording() ); - EXPECT_FALSE(planner.is_replaying() ); + EXPECT_FALSE(planner.is_recording()); + EXPECT_FALSE(planner.is_replaying()); } TEST(RecordreplaySanityChecks, HeadingWeightSetting) @@ -301,9 +288,8 @@ TEST(RecordreplayReachGoal, checkReachGoalCondition) { const float32_t x = 3.5F; const float32_t heading = 0.0F; - const auto vehicle_state = make_state( - x, 0.0F, heading, 0.0F, 0.0F, 0.0F, - system_clock::from_time_t({})); + const auto vehicle_state = + make_state(x, 0.0F, heading, 0.0F, 0.0F, 0.0F, system_clock::from_time_t({})); planner.plan(vehicle_state); EXPECT_FALSE(planner.reached_goal(vehicle_state, distance_thresh, angle_thresh)); } @@ -312,9 +298,8 @@ TEST(RecordreplayReachGoal, checkReachGoalCondition) { const float32_t x = 5.0F; const float32_t heading = -autoware::common::types::PI; - const auto vehicle_state = make_state( - x, 0.0F, heading, 0.0F, 0.0F, 0.0F, - system_clock::from_time_t({})); + const auto vehicle_state = + make_state(x, 0.0F, heading, 0.0F, 0.0F, 0.0F, system_clock::from_time_t({})); planner.plan(vehicle_state); EXPECT_FALSE(planner.reached_goal(vehicle_state, distance_thresh, angle_thresh)); } @@ -323,15 +308,15 @@ TEST(RecordreplayReachGoal, checkReachGoalCondition) { const float32_t x = 5.0F; const float32_t heading = 0.0F; - const auto vehicle_state = make_state( - x, 0.0F, heading, 0.0F, 0.0F, 0.0F, - system_clock::from_time_t({})); + const auto vehicle_state = + make_state(x, 0.0F, heading, 0.0F, 0.0F, 0.0F, system_clock::from_time_t({})); planner.plan(vehicle_state); EXPECT_TRUE(planner.reached_goal(vehicle_state, distance_thresh, angle_thresh)); } } -TEST(RecordreplayLoopingTrajectories, maintainTrajectoryLength) { +TEST(RecordreplayLoopingTrajectories, maintainTrajectoryLength) +{ // This test assumes that `trajectory.points.max_size() > 5 // As of 2021-08-12 it is 100 uint32_t N = 5; @@ -342,9 +327,8 @@ TEST(RecordreplayLoopingTrajectories, maintainTrajectoryLength) { // We will start in the middle of this, and we expect // that the return trajectory is of the full length - auto vehicle_state = make_state( - N / 2, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, system_clock::from_time_t({})); + auto vehicle_state = + make_state(N / 2, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, system_clock::from_time_t({})); auto traj = planner.plan(vehicle_state); EXPECT_EQ(traj.points.size(), static_cast(N)); @@ -363,15 +347,14 @@ RecordReplayPlanner helper_create_and_record_pseudo_loop(uint32_t N) // time associated with the last point in the trajectory // but this isn't that important to testing loop functionality planner.record_state( - make_state( - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - system_clock::from_time_t({}))); + make_state(0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, system_clock::from_time_t({}))); return planner; } } -TEST(RecordreplayLoopingTrajectories, correctLoopHandling) { +TEST(RecordreplayLoopingTrajectories, correctLoopHandling) +{ auto planner = helper_create_and_record_pseudo_loop(500); planner.set_loop(planner.is_loop(5)); @@ -381,8 +364,8 @@ TEST(RecordreplayLoopingTrajectories, correctLoopHandling) { auto vehicle_trajectory_point = record_buf[record_buf.size() - 10].state; - autoware_auto_vehicle_msgs::msg::VehicleKinematicState vehicle_state - {rosidl_runtime_cpp::MessageInitialization::ALL}; + autoware_auto_vehicle_msgs::msg::VehicleKinematicState vehicle_state{ + rosidl_runtime_cpp::MessageInitialization::ALL}; vehicle_state.state.pose.position.x = vehicle_trajectory_point.pose.position.x; vehicle_state.state.pose.position.y = vehicle_trajectory_point.pose.position.y; diff --git a/planning/recordreplay_planner_nodes/design/recordreplay_planner_nodes-design.md b/planning/recordreplay_planner_nodes/design/recordreplay_planner_nodes-design.md index 6cb58f458e8ef..70de28c76da22 100644 --- a/planning/recordreplay_planner_nodes/design/recordreplay_planner_nodes-design.md +++ b/planning/recordreplay_planner_nodes/design/recordreplay_planner_nodes-design.md @@ -1,5 +1,4 @@ -RecordReplay planner nodes {#recordreplay-planner-nodes} -========================== +# RecordReplay planner nodes {#recordreplay-planner-nodes} # Purpose / Use cases @@ -14,60 +13,58 @@ point of the trajectory are too far away. # Design This is a wrapper around `recordreplay_planner`. Its behavior can be controlled via actions. It can record -the ego state of the vehicle and play back a set of recorded states at a later time. It does stop for obstacles, +the ego state of the vehicle and play back a set of recorded states at a later time. It does stop for obstacles, from `BoundingBoxArray`, publsihed in any `tf` frame that can transform to `VehicleKinematicState`'s frame. - ## Assumptions / Known limits This node assumes that the recorded states can be played back without any `tf` changes. There is an open issue -in https://gitlab.com/autowarefoundation/autoware.auto/AutowareAuto/issues/265 to address this limitation. +in to address this limitation. See the `recordreplay_planner` design documentation for more details. - ## Inputs / Outputs / API Actions: This node uses two actions to control its behavior: -* `RecordTrajectory.action` is used to record a trajectory. It runs until canceled. While the action is +- `RecordTrajectory.action` is used to record a trajectory. It runs until canceled. While the action is running, the node subscribes to a `VehicleKinematicState.msg` topic by a provided name and records all - states that are published on that topic. -* `ReplayTrajectory.action` is used to replay a trajectory. It runs until canceled. While the action is + states that are published on that topic. +- `ReplayTrajectory.action` is used to replay a trajectory. It runs until canceled. While the action is running, the node subscribes to the same `VehicleKinematicState.msg` topic as when recording. When messages are published on that topic, the node publishes a trajectory starting approximately at that point (see the - `recordreplay_planner` design documentation on how that point is determined). + `recordreplay_planner` design documentation on how that point is determined). The actions are defined in `autoware_auto_vehicle_msgs`, `autoware_auto_planning_msgs` and `autoware_auto_perception_msgs`. Clients: -* `autoware_auto_planning_msgs/srv/ModifyTrajectory` is the client for `obstacle_collision_estimator` which will check for obstacle collision on the trajectory internally generated by this node, and hand back a modifed one to avoid obstacles. This behavior can be toggled via `enable_object_collision_estimator` parameter. +- `autoware_auto_planning_msgs/srv/ModifyTrajectory` is the client for `obstacle_collision_estimator` which will check for obstacle collision on the trajectory internally generated by this node, and hand back a modifed one to avoid obstacles. This behavior can be toggled via `enable_object_collision_estimator` parameter. Inputs: -* `autoware_auto_vehicle_msgs/msg/VehicleKinematicState` is the state used as recorded points for replay, and also to prune starting point of replay trajectory +- `autoware_auto_vehicle_msgs/msg/VehicleKinematicState` is the state used as recorded points for replay, and also to prune starting point of replay trajectory Outputs: -* `autoware_auto_planning_msgs/msg/Trajectory` is the trajectory that gets published. If `enable_object_collision_estimator`, then it is the trajectory after modification. +- `autoware_auto_planning_msgs/msg/Trajectory` is the trajectory that gets published. If `enable_object_collision_estimator`, then it is the trajectory after modification. ## Complexity See `recordreplay_planner`. -# Security considerations +# Security considerations TBD by a security specialist. # Future extensions / Unimplemented parts -* Proper `tf` support +- Proper `tf` support # Related issues -* #265 Extension to support `tf` properly -* #273 Fix and extend integration tests +- #265 Extension to support `tf` properly +- #273 Fix and extend integration tests diff --git a/planning/recordreplay_planner_nodes/include/recordreplay_planner_nodes/recordreplay_planner_node.hpp b/planning/recordreplay_planner_nodes/include/recordreplay_planner_nodes/recordreplay_planner_node.hpp index c88ce7e0b7f81..1c11423269563 100644 --- a/planning/recordreplay_planner_nodes/include/recordreplay_planner_nodes/recordreplay_planner_node.hpp +++ b/planning/recordreplay_planner_nodes/include/recordreplay_planner_nodes/recordreplay_planner_node.hpp @@ -14,27 +14,27 @@ #ifndef RECORDREPLAY_PLANNER_NODES__RECORDREPLAY_PLANNER_NODE_HPP_ #define RECORDREPLAY_PLANNER_NODES__RECORDREPLAY_PLANNER_NODE_HPP_ -#include -#include -#include +#include +#include +#include #include +#include + #include #include - #include #include -#include #include +#include #include #include #include -#include -#include -#include +#include +#include -#include #include +#include using autoware::common::types::float64_t; @@ -45,11 +45,11 @@ namespace planning namespace recordreplay_planner_nodes { using PlannerPtr = std::unique_ptr; +using autoware_auto_planning_msgs::action::RecordTrajectory; +using autoware_auto_planning_msgs::action::ReplayTrajectory; using autoware_auto_planning_msgs::msg::Trajectory; using autoware_auto_planning_msgs::msg::TrajectoryPoint; using autoware_auto_planning_msgs::srv::ModifyTrajectory; -using autoware_auto_planning_msgs::action::RecordTrajectory; -using autoware_auto_planning_msgs::action::ReplayTrajectory; using State = autoware_auto_vehicle_msgs::msg::VehicleKinematicState; using Transform = geometry_msgs::msg::TransformStamped; using visualization_msgs::msg::Marker; @@ -86,17 +86,15 @@ class RECORDREPLAY_PLANNER_NODES_PUBLIC RecordReplayPlannerNode : public rclcpp: /// \param[in] ns The namespace for the marker /// \returns A visualization_msgs::msg::Marker RECORDREPLAY_PLANNER_NODES_LOCAL Marker to_marker( - const TrajectoryPoint & traj_point, - const std::string & frame_id, - int32_t index, + const TrajectoryPoint & traj_point, const std::string & frame_id, int32_t index, const std::string & ns); /// \brief Converts a Trajectory to a MarkerArray for visualization /// \param[in] traj The Trajectory /// \param[in] ns The namespace for the markers /// \returns A visaulization_msgs::msg::MarkerArray - RECORDREPLAY_PLANNER_NODES_LOCAL MarkerArray to_markers( - const Trajectory & traj, const std::string & ns); + RECORDREPLAY_PLANNER_NODES_LOCAL MarkerArray + to_markers(const Trajectory & traj, const std::string & ns); /// \brief Clears the list of recorded markers RECORDREPLAY_PLANNER_NODES_LOCAL void clear_recorded_markers(); @@ -106,16 +104,14 @@ class RECORDREPLAY_PLANNER_NODES_PUBLIC RecordReplayPlannerNode : public rclcpp: rclcpp::Client::SharedFuture future); RECORDREPLAY_PLANNER_NODES_LOCAL rclcpp_action::GoalResponse record_handle_goal( - const rclcpp_action::GoalUUID & uuid, - const std::shared_ptr goal); + const rclcpp_action::GoalUUID & uuid, const std::shared_ptr goal); RECORDREPLAY_PLANNER_NODES_LOCAL rclcpp_action::CancelResponse record_handle_cancel( const std::shared_ptr goal_handle); RECORDREPLAY_PLANNER_NODES_LOCAL void record_handle_accepted( const std::shared_ptr goal_handle); RECORDREPLAY_PLANNER_NODES_LOCAL rclcpp_action::GoalResponse replay_handle_goal( - const rclcpp_action::GoalUUID & uuid, - const std::shared_ptr goal); + const rclcpp_action::GoalUUID & uuid, const std::shared_ptr goal); RECORDREPLAY_PLANNER_NODES_LOCAL rclcpp_action::CancelResponse replay_handle_cancel( const std::shared_ptr goal_handle); RECORDREPLAY_PLANNER_NODES_LOCAL void replay_handle_accepted( diff --git a/planning/recordreplay_planner_nodes/include/recordreplay_planner_nodes/visibility_control.hpp b/planning/recordreplay_planner_nodes/include/recordreplay_planner_nodes/visibility_control.hpp index deba3b062ecba..18e475c4e9523 100644 --- a/planning/recordreplay_planner_nodes/include/recordreplay_planner_nodes/visibility_control.hpp +++ b/planning/recordreplay_planner_nodes/include/recordreplay_planner_nodes/visibility_control.hpp @@ -16,24 +16,23 @@ #define RECORDREPLAY_PLANNER_NODES__VISIBILITY_CONTROL_HPP_ #if defined(__WIN32) - #if defined(RECORDREPLAY_PLANNER_NODES_BUILDING_DLL) || \ - defined(RECORDREPLAY_PLANNER_NODES_EXPORTS) - #define RECORDREPLAY_PLANNER_NODES_PUBLIC __declspec(dllexport) - #define RECORDREPLAY_PLANNER_NODES_LOCAL - #else +#if defined(RECORDREPLAY_PLANNER_NODES_BUILDING_DLL) || defined(RECORDREPLAY_PLANNER_NODES_EXPORTS) +#define RECORDREPLAY_PLANNER_NODES_PUBLIC __declspec(dllexport) +#define RECORDREPLAY_PLANNER_NODES_LOCAL +#else // defined(RECORDREPLAY_PLANNER_NODES_BUILDING_DLL) || defined(RECORDREPLAY_PLANNER_NODES_EXPORTS) - #define RECORDREPLAY_PLANNER_NODES_PUBLIC __declspec(dllimport) - #define RECORDREPLAY_PLANNER_NODES_LOCAL - #endif +#define RECORDREPLAY_PLANNER_NODES_PUBLIC __declspec(dllimport) +#define RECORDREPLAY_PLANNER_NODES_LOCAL +#endif // defined(RECORDREPLAY_PLANNER_NODES_BUILDING_DLL) || defined(RECORDREPLAY_PLANNER_NODES_EXPORTS) #elif defined(__linux__) - #define RECORDREPLAY_PLANNER_NODES_PUBLIC __attribute__((visibility("default"))) - #define RECORDREPLAY_PLANNER_NODES_LOCAL __attribute__((visibility("hidden"))) +#define RECORDREPLAY_PLANNER_NODES_PUBLIC __attribute__((visibility("default"))) +#define RECORDREPLAY_PLANNER_NODES_LOCAL __attribute__((visibility("hidden"))) #elif defined(__APPLE__) - #define RECORDREPLAY_PLANNER_NODES_PUBLIC __attribute__((visibility("default"))) - #define RECORDREPLAY_PLANNER_NODES_LOCAL __attribute__((visibility("hidden"))) +#define RECORDREPLAY_PLANNER_NODES_PUBLIC __attribute__((visibility("default"))) +#define RECORDREPLAY_PLANNER_NODES_LOCAL __attribute__((visibility("hidden"))) #else // defined(_LINUX) - #error "Unsupported Build Configuration" +#error "Unsupported Build Configuration" #endif // defined(_WINDOWS) #endif // RECORDREPLAY_PLANNER_NODES__VISIBILITY_CONTROL_HPP_ diff --git a/planning/recordreplay_planner_nodes/launch/recordreplay_planner_node.launch.py b/planning/recordreplay_planner_nodes/launch/recordreplay_planner_node.launch.py index e558f8ca12986..fdbac511462d5 100644 --- a/planning/recordreplay_planner_nodes/launch/recordreplay_planner_node.launch.py +++ b/planning/recordreplay_planner_nodes/launch/recordreplay_planner_node.launch.py @@ -22,26 +22,22 @@ def generate_launch_description(): """Launch recordreplay_planner_node with default configuration.""" # -------------------------------- Nodes----------------------------------- recordreplay_planner_node = launch_ros.actions.Node( - package='recordreplay_planner_nodes', - executable='recordreplay_planner_node_exe', - name='recordreplay_planner', - namespace='planning', - output='screen', + package="recordreplay_planner_nodes", + executable="recordreplay_planner_node_exe", + name="recordreplay_planner", + namespace="planning", + output="screen", parameters=[ "{}/param/defaults.param.yaml".format( - ament_index_python.get_package_share_directory( - "recordreplay_planner_nodes" - ) + ament_index_python.get_package_share_directory("recordreplay_planner_nodes") ), ], remappings=[ - ('vehicle_state', '/vehicle/vehicle_kinematic_state'), - ('planned_trajectory', '/planning/trajectory'), - ('obstacle_bounding_boxes', '/perception/lidar_bounding_boxes'), - ] + ("vehicle_state", "/vehicle/vehicle_kinematic_state"), + ("planned_trajectory", "/planning/trajectory"), + ("obstacle_bounding_boxes", "/perception/lidar_bounding_boxes"), + ], ) - ld = launch.LaunchDescription([ - recordreplay_planner_node] - ) + ld = launch.LaunchDescription([recordreplay_planner_node]) return ld diff --git a/planning/recordreplay_planner_nodes/package.xml b/planning/recordreplay_planner_nodes/package.xml index 14c659c0c5718..68c82336fb636 100644 --- a/planning/recordreplay_planner_nodes/package.xml +++ b/planning/recordreplay_planner_nodes/package.xml @@ -11,19 +11,19 @@ autoware_auto_cmake autoware_auto_planning_msgs + autoware_auto_tf2 autoware_auto_vehicle_msgs - autoware_auto_tf2 - recordreplay_planner - rclcpp rclcpp_action rclcpp_components + recordreplay_planner tf2 - tf2_ros tf2_geometry_msgs + tf2_ros + - motion_testing autoware_auto_common + motion_testing launch_ros rosidl_default_runtime @@ -34,5 +34,7 @@ ament_lint_common autoware_testing - ament_cmake + + ament_cmake + diff --git a/planning/recordreplay_planner_nodes/param/test.param.yaml b/planning/recordreplay_planner_nodes/param/test.param.yaml index 80f6db0cb0123..4eea818be76cc 100644 --- a/planning/recordreplay_planner_nodes/param/test.param.yaml +++ b/planning/recordreplay_planner_nodes/param/test.param.yaml @@ -8,4 +8,4 @@ loop_trajectory: False loop_max_gap_m: 5.0 skip_first_velocity: False - recording_frame: "map" \ No newline at end of file + recording_frame: "map" diff --git a/planning/recordreplay_planner_nodes/src/recordreplay_planner_nodes/recordreplay_planner_node.cpp b/planning/recordreplay_planner_nodes/src/recordreplay_planner_nodes/recordreplay_planner_node.cpp index f93dddca31329..f54380f57527d 100644 --- a/planning/recordreplay_planner_nodes/src/recordreplay_planner_nodes/recordreplay_planner_node.cpp +++ b/planning/recordreplay_planner_nodes/src/recordreplay_planner_nodes/recordreplay_planner_node.cpp @@ -12,18 +12,19 @@ // See the License for the specific language governing permissions and // limitations under the License. - +#include #include #include -#include + #include + #include #include #include +using autoware::common::types::bool8_t; using autoware::common::types::float32_t; using autoware::common::types::float64_t; -using autoware::common::types::bool8_t; namespace motion { @@ -53,42 +54,32 @@ RecordReplayPlannerNode::RecordReplayPlannerNode(const rclcpp::NodeOptions & nod rclcpp::Clock::SharedPtr clock = std::make_shared(RCL_ROS_TIME); tf_buffer_ = std::make_shared(clock); tf_listener_ = std::make_shared( - *tf_buffer_, - std::shared_ptr(this, [](auto) {}), false); + *tf_buffer_, std::shared_ptr(this, [](auto) {}), false); // Set up action for control of recording and replaying m_recordserver = rclcpp_action::create_server( - this->get_node_base_interface(), - this->get_node_clock_interface(), - this->get_node_logging_interface(), - this->get_node_waitables_interface(), - "recordtrajectory", - [this](auto uuid, auto goal) {return this->record_handle_goal(uuid, goal);}, - [this](auto goal_handle) {return this->record_handle_cancel(goal_handle);}, - [this](auto goal_handle) {return this->record_handle_accepted(goal_handle);}); + this->get_node_base_interface(), this->get_node_clock_interface(), + this->get_node_logging_interface(), this->get_node_waitables_interface(), "recordtrajectory", + [this](auto uuid, auto goal) { return this->record_handle_goal(uuid, goal); }, + [this](auto goal_handle) { return this->record_handle_cancel(goal_handle); }, + [this](auto goal_handle) { return this->record_handle_accepted(goal_handle); }); m_replayserver = rclcpp_action::create_server( - this->get_node_base_interface(), - this->get_node_clock_interface(), - this->get_node_logging_interface(), - this->get_node_waitables_interface(), - "replaytrajectory", - [this](auto uuid, auto goal) {return this->replay_handle_goal(uuid, goal);}, - [this](auto goal_handle) {return this->replay_handle_cancel(goal_handle);}, - [this](auto goal_handle) {return this->replay_handle_accepted(goal_handle);}); + this->get_node_base_interface(), this->get_node_clock_interface(), + this->get_node_logging_interface(), this->get_node_waitables_interface(), "replaytrajectory", + [this](auto uuid, auto goal) { return this->replay_handle_goal(uuid, goal); }, + [this](auto goal_handle) { return this->replay_handle_cancel(goal_handle); }, + [this](auto goal_handle) { return this->replay_handle_accepted(goal_handle); }); // Set up subscribers for the actual recording using SubAllocT = rclcpp::SubscriptionOptionsWithAllocator>; m_ego_sub = create_subscription( - ego_topic, QoS{10}, - [this](const State::SharedPtr msg) {on_ego(msg);}, SubAllocT{}); + ego_topic, QoS{10}, [this](const State::SharedPtr msg) { on_ego(msg); }, SubAllocT{}); // Set up publishers using PubAllocT = rclcpp::PublisherOptionsWithAllocator>; - m_trajectory_pub = - create_publisher(trajectory_topic, QoS{10}, PubAllocT{}); - m_trajectory_viz_pub = - create_publisher(trajectory_viz_topic, QoS{10}); + m_trajectory_pub = create_publisher(trajectory_topic, QoS{10}, PubAllocT{}); + m_trajectory_viz_pub = create_publisher(trajectory_viz_topic, QoS{10}); // Set up services if (declare_parameter("enable_object_collision_estimator").get()) { @@ -111,9 +102,7 @@ RecordReplayPlannerNode::RecordReplayPlannerNode(const rclcpp::NodeOptions & nod } Marker RecordReplayPlannerNode::to_marker( - const TrajectoryPoint & traj_point, - const std::string & frame_id, - int32_t index, + const TrajectoryPoint & traj_point, const std::string & frame_id, int32_t index, const std::string & ns) { Marker marker; @@ -180,9 +169,8 @@ void RecordReplayPlannerNode::on_ego(const State::SharedPtr & msg) Transform tf_map2odom; State msg_world = *msg; try { - tf_map2odom = tf_buffer_->lookupTransform( - m_recording_frame, msg->header.frame_id, - tf2::TimePointZero); + tf_map2odom = + tf_buffer_->lookupTransform(m_recording_frame, msg->header.frame_id, tf2::TimePointZero); } catch (...) { // skip recording/replaying if tf failed to get a transform to world frame. RCLCPP_ERROR_THROTTLE( @@ -190,8 +178,8 @@ void RecordReplayPlannerNode::on_ego(const State::SharedPtr & msg) "Failed to transform ego pose to world frame."); return; } - tf2::doTransform(*msg.state.pose, msg_world.state.pose, tf_map2odom) - msg_world.header.frame_id = m_recording_frame; + tf2::doTransform(*msg.state.pose, msg_world.state.pose, tf_map2odom) msg_world.header.frame_id = + m_recording_frame; if (m_planner->is_recording()) { RCLCPP_INFO_ONCE(this->get_logger(), "Recording ego position"); @@ -200,12 +188,9 @@ void RecordReplayPlannerNode::on_ego(const State::SharedPtr & msg) if (added) { // Publish visualization markers - m_recorded_markers.markers.push_back( - to_marker( - msg_world.state, - msg_world.header.frame_id, - static_cast(m_planner->get_record_length()), - "record")); + m_recorded_markers.markers.push_back(to_marker( + msg_world.state, msg_world.header.frame_id, + static_cast(m_planner->get_record_length()), "record")); m_trajectory_viz_pub->publish(m_recorded_markers); } @@ -224,9 +209,9 @@ void RecordReplayPlannerNode::on_ego(const State::SharedPtr & msg) auto request = std::make_shared(); request->original_trajectory = traj_raw; auto result = m_modify_trajectory_client->async_send_request( - request, std::bind( - &RecordReplayPlannerNode::modify_trajectory_response, - this, std::placeholders::_1)); + request, + std::bind( + &RecordReplayPlannerNode::modify_trajectory_response, this, std::placeholders::_1)); } else { m_trajectory_pub->publish(traj_raw); @@ -242,8 +227,7 @@ void RecordReplayPlannerNode::on_ego(const State::SharedPtr & msg) // If we reach the destination and are not in a loop, terminate replay if (m_planner->reached_goal( - msg_world, m_goal_distance_threshold_m, m_goal_angle_threshold_rad)) - { + msg_world, m_goal_distance_threshold_m, m_goal_angle_threshold_rad)) { if (!m_planner->get_loop()) { m_replaygoalhandle->succeed(std::make_shared()); m_planner->stop_replaying(); @@ -271,8 +255,7 @@ void RecordReplayPlannerNode::modify_trajectory_response( } rclcpp_action::GoalResponse RecordReplayPlannerNode::record_handle_goal( - const rclcpp_action::GoalUUID & uuid, - const std::shared_ptr goal) + const rclcpp_action::GoalUUID & uuid, const std::shared_ptr goal) { (void)goal; (void)uuid; @@ -296,8 +279,7 @@ rclcpp_action::CancelResponse RecordReplayPlannerNode::record_handle_cancel( // If a path is specified if (record_path.length() > 0) { // Write trajectory to file - m_planner->writeTrajectoryBufferToFile( - goal_handle->get_goal()->record_path); + m_planner->writeTrajectoryBufferToFile(goal_handle->get_goal()->record_path); } } @@ -317,8 +299,7 @@ void RecordReplayPlannerNode::record_handle_accepted( } rclcpp_action::GoalResponse RecordReplayPlannerNode::replay_handle_goal( - const rclcpp_action::GoalUUID & uuid, - const std::shared_ptr goal) + const rclcpp_action::GoalUUID & uuid, const std::shared_ptr goal) { (void)goal; (void)uuid; @@ -353,8 +334,7 @@ void RecordReplayPlannerNode::replay_handle_accepted( // If a path is specified if (replay_path.length() > 0) { // Read trajectory from file - m_planner->readTrajectoryBufferFromFile( - goal_handle->get_goal()->replay_path); + m_planner->readTrajectoryBufferFromFile(goal_handle->get_goal()->replay_path); // Set looping behavior const auto loop_good = m_planner->is_loop(m_max_loop_gap_m); m_planner->set_loop(m_enable_loop && loop_good); diff --git a/planning/recordreplay_planner_nodes/test/recordreplay_planner_node.test.py b/planning/recordreplay_planner_nodes/test/recordreplay_planner_node.test.py index f128480730d06..7fe7be1d62269 100644 --- a/planning/recordreplay_planner_nodes/test/recordreplay_planner_node.test.py +++ b/planning/recordreplay_planner_nodes/test/recordreplay_planner_node.test.py @@ -15,21 +15,21 @@ # This file contains tests for the record and replay behavior of the containing node. # I'll start by conceptually documenting the tests I want to add, then implement them. +import subprocess +import time import unittest import ament_index_python +from autoware_auto_planning_msgs.action import RecordTrajectory +from autoware_auto_planning_msgs.action import ReplayTrajectory +from autoware_auto_planning_msgs.msg import Trajectory +from autoware_auto_vehicle_msgs.msg import VehicleKinematicState import launch import launch_ros.actions import launch_testing import rclpy -from rclpy.node import Node from rclpy.action import ActionClient -from autoware_auto_planning_msgs.action import RecordTrajectory, ReplayTrajectory -from autoware_auto_planning_msgs.msg import Trajectory -from autoware_auto_vehicle_msgs.msg import VehicleKinematicState - -import subprocess -import time +from rclpy.node import Node # Class to publish some dummy states @@ -83,9 +83,7 @@ def goal_response_callback(self, future): self._goal_handle = goal_handle def feedback_callback(self, feedback): - self.get_logger().info( - "Received feedback: {0}".format(feedback.feedback.sequence) - ) + self.get_logger().info("Received feedback: {0}".format(feedback.feedback.sequence)) def manual_cancel(self): self.get_logger().info("Canceling goal") @@ -114,9 +112,7 @@ def generate_test_description(): name="recordreplay_planner", parameters=[ "{}/defaults.param.yaml".format( - ament_index_python.get_package_share_directory( - "recordreplay_planner_nodes" - ) + ament_index_python.get_package_share_directory("recordreplay_planner_nodes") ) ], ) diff --git a/planning/recordreplay_planner_nodes/test/sanity_checks.cpp b/planning/recordreplay_planner_nodes/test/sanity_checks.cpp index 30ba5c6712363..135399d7a3cc0 100644 --- a/planning/recordreplay_planner_nodes/test/sanity_checks.cpp +++ b/planning/recordreplay_planner_nodes/test/sanity_checks.cpp @@ -12,24 +12,25 @@ // See the License for the specific language governing permissions and // limitations under the License. -#include -#include -#include +#include "object_collision_estimator_nodes/object_collision_estimator_node.hpp" + #include #include +#include -#include -#include -#include +#include -#include "object_collision_estimator_nodes/object_collision_estimator_node.hpp" +#include +#include +#include +#include -using motion::planning::recordreplay_planner_nodes::RecordReplayPlannerNode; -using motion::planning::object_collision_estimator_nodes::ObjectCollisionEstimatorNode; +using autoware_auto_planning_msgs::msg::Trajectory; using motion::motion_testing::make_state; +using motion::planning::object_collision_estimator_nodes::ObjectCollisionEstimatorNode; +using motion::planning::recordreplay_planner_nodes::RecordReplayPlannerNode; using std::chrono::system_clock; -using autoware_auto_planning_msgs::msg::Trajectory; using State = autoware_auto_vehicle_msgs::msg::VehicleKinematicState; TEST(MytestBase, Basic) @@ -55,8 +56,8 @@ TEST(MytestBase, Basic) node_options_ob.append_parameter_override("trajectory_smoother.kernel_size", 25); node_options_ob.append_parameter_override("staleness_threshold_ms", 500); node_options_ob.append_parameter_override("target_frame_id", "map"); - auto object_collision_estimator_node = std::make_shared( - node_options_ob); + auto object_collision_estimator_node = + std::make_shared(node_options_ob); node_options_rr.append_parameter_override("heading_weight", heading_weight); node_options_rr.append_parameter_override("min_record_distance", min_record_distance); @@ -66,15 +67,13 @@ TEST(MytestBase, Basic) node_options_rr.append_parameter_override("loop_trajectory", false); node_options_rr.append_parameter_override("loop_max_gap_m", 0.0); node_options_rr.append_parameter_override( - "goal_angle_threshold_rad", - autoware::common::types::PI_2); + "goal_angle_threshold_rad", autoware::common::types::PI_2); node_options_rr.append_parameter_override("skip_first_velocity", false); node_options_rr.append_parameter_override("recording_frame", "odom"); auto plannernode = std::make_shared(node_options_rr); using PubAllocT = rclcpp::PublisherOptionsWithAllocator>; - const auto publisher = std::make_shared( - "recordreplay_node_testpublisher"); + const auto publisher = std::make_shared("recordreplay_node_testpublisher"); const auto pub = publisher->create_publisher( "vehicle_state", rclcpp::QoS{10}.transient_local(), PubAllocT{});