From c3ae35db04a0fef8aac31a67b2886954256eaeaa Mon Sep 17 00:00:00 2001 From: Aswin Vijay Date: Fri, 15 Mar 2024 15:22:24 +0900 Subject: [PATCH 1/5] feat(freespace_planning_algorithms): add tests for updated astar algorithm --- .../CMakeLists.txt | 8 +- ...st_freespace_planning_algorithms_astar.cpp | 359 ++++++++++++++++++ 2 files changed, 366 insertions(+), 1 deletion(-) create mode 100644 planning/freespace_planning_algorithms/test/src/test_freespace_planning_algorithms_astar.cpp diff --git a/planning/freespace_planning_algorithms/CMakeLists.txt b/planning/freespace_planning_algorithms/CMakeLists.txt index d5a9d32db1817..29d5cee425642 100644 --- a/planning/freespace_planning_algorithms/CMakeLists.txt +++ b/planning/freespace_planning_algorithms/CMakeLists.txt @@ -34,7 +34,13 @@ if(BUILD_TESTING) target_link_libraries(freespace_planning_algorithms-test freespace_planning_algorithms ) - + find_package(ament_cmake_ros REQUIRED) + ament_add_ros_isolated_gtest(freespace_planning_algorithms_astar-test + test/src/test_freespace_planning_algorithms_astar.cpp + ) + target_link_libraries(freespace_planning_algorithms_astar-test + freespace_planning_algorithms + ) find_package(ament_cmake_ros REQUIRED) ament_add_gtest(rrtstar_core_informed-test test/src/test_rrtstar_core_informed.cpp diff --git a/planning/freespace_planning_algorithms/test/src/test_freespace_planning_algorithms_astar.cpp b/planning/freespace_planning_algorithms/test/src/test_freespace_planning_algorithms_astar.cpp new file mode 100644 index 0000000000000..ce518235ee119 --- /dev/null +++ b/planning/freespace_planning_algorithms/test/src/test_freespace_planning_algorithms_astar.cpp @@ -0,0 +1,359 @@ +// Copyright 2021 Tier IV, Inc. All rights reserved. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#include "freespace_planning_algorithms/abstract_algorithm.hpp" +#include "freespace_planning_algorithms/astar_search.hpp" +#include "freespace_planning_algorithms/rrtstar.hpp" + +#include +#include +#include + +#include + +#include +#include +#include + +#include +#include +#include +#include +#include +#include + +namespace fpa = freespace_planning_algorithms; + +const double length_lexus = 5.5; +const double width_lexus = 2.75; +const fpa::VehicleShape vehicle_shape = fpa::VehicleShape(length_lexus, width_lexus, 1.5); +const double pi = 3.1415926; +const std::array start_pose1{8., 7., pi * 0.5}; +const std::array start_pose2{9., 12., pi * 0.25}; +const std::array start_pose3{13.3, 9., pi * 0.5}; +const std::array start_pose4{25.0, 16., - pi * 0.25}; +const std::array start_pose5{20.5, 8.5, 0.0}; +const std::array goal_pose{18.0, 10.5, pi * 0.5}; +const std::array, 5> start_poses{ + start_pose1, start_pose2, start_pose3, start_pose4, start_pose5}; + +geometry_msgs::msg::Pose create_pose_msg(std::array pose3d) +{ + geometry_msgs::msg::Pose pose{}; + tf2::Quaternion quat{}; + quat.setRPY(0, 0, pose3d[2]); + tf2::convert(quat, pose.orientation); + pose.position.x = pose3d[0]; + pose.position.y = pose3d[1]; + pose.position.z = 0.0; + return pose; +} + +std_msgs::msg::Float64 create_float_msg(double val) +{ + std_msgs::msg::Float64 msg; + msg.data = val; + return msg; +} + +nav_msgs::msg::OccupancyGrid construct_cost_map( + size_t width, size_t height, double resolution, size_t n_padding) +{ + nav_msgs::msg::OccupancyGrid costmap_msg{}; + + // create info + costmap_msg.info.width = width; + costmap_msg.info.height = height; + costmap_msg.info.resolution = resolution; + + // create data + const size_t n_elem = width * height; + for (size_t i = 0; i < n_elem; ++i) { + costmap_msg.data.push_back(0.0); + } + + for (size_t i = 0; i < height; ++i) { + for (size_t j = 0; j < width; ++j) { + const double x = j * resolution; + const double y = i * resolution; + + //road + if (5.4 < x && x < 5.4 + 4.5 && 0.0 < y && y < 0.0 + 19.4) { + costmap_msg.data[i * width + j] = 40.0; + } + + if (9.0 < x && x < 40.0 && 15.4 < y && y < 15.4 + 4.0) { + costmap_msg.data[i * width + j] = 40.0; + } + + double width_p = 3.0; + double height_p = 6.0; + + //parking 1 (16,9) + if (16.0 < x && x < 16.0 + width_p && 9.0 < y && y < 9.0 + height_p) { + costmap_msg.data[i * width + j] = 50.0; + } + + //parking 2 + if (19.3 < x && x < 19.3 + width_p && 9.0 < y && y < 9.0 + height_p) { + costmap_msg.data[i * width + j] = 50.0; + } + + //parking 3 + if (22.6 < x && x < 22.6 + width_p && 9.0 < y && y < 9.0 + height_p) { + costmap_msg.data[i * width + j] = 50.0; + } + + //parking 4 + if (25.9 < x && x < 25.9 + width_p && 9.0 < y && y < 9.0 + height_p) { + costmap_msg.data[i * width + j] = 50.0; + } + + //parking 5 + if (29.2 < x && x < 29.2 + width_p && 9.0 < y && y < 9.0 + height_p) { + costmap_msg.data[i * width + j] = 50.0; + } + } + } + + for (size_t i = 0; i < n_padding; ++i) { + // fill left + for (size_t j = width * i; j <= width * (i + 1); ++j) { + costmap_msg.data[j] = 100.0; + } + // fill right + for (size_t j = width * (height - n_padding + i); j <= width * (height - n_padding + i + 1); + ++j) { + costmap_msg.data[j] = 100.0; + } + } + + for (size_t i = 0; i < height; ++i) { + // fill bottom + for (size_t j = i * width; j <= i * width + n_padding; ++j) { + costmap_msg.data[j] = 100.0; + } + for (size_t j = (i + 1) * width - n_padding; j <= (i + 1) * width; ++j) { + costmap_msg.data[j] = 100.0; + } + } + + return costmap_msg; +} + +template +void add_message_to_rosbag( + rosbag2_cpp::Writer & writer, const MessageT & message, const std::string & name, + const std::string & type) +{ + rclcpp::SerializedMessage serialized_msg; + rclcpp::Serialization serialization; + serialization.serialize_message(&message, &serialized_msg); + + rosbag2_storage::TopicMetadata tm; + tm.name = name; + tm.type = type; + tm.serialization_format = "cdr"; + writer.create_topic(tm); + + auto bag_message = std::make_shared(); + auto ret = rcutils_system_time_now(&bag_message->time_stamp); + if (ret != RCL_RET_OK) { + RCLCPP_ERROR(rclcpp::get_logger("saveToBag"), "couldn't assign time rosbag message"); + } + + bag_message->topic_name = tm.name; + bag_message->serialized_data = std::shared_ptr( + &serialized_msg.get_rcl_serialized_message(), [](rcutils_uint8_array_t * /* data */) {}); + writer.write(bag_message); +} + +fpa::PlannerCommonParam get_default_planner_params() +{ + // set problem configuration + const double time_limit = 10 * 1000.0; + const double minimum_turning_radius = 9.0; + const double maximum_turning_radius = 9.0; + const int turning_radius_size = 1; + + // Transitions every 2.5 deg + const int theta_size = 144; + + const double curve_weight = 1.5; + const double reverse_weight = 2.0; + + const double lateral_goal_range = 0.5; + const double longitudinal_goal_range = 2.0; + const double angle_goal_range = 6.0; + const int obstacle_threshold = 100; + + return fpa::PlannerCommonParam{ + time_limit, + minimum_turning_radius, + maximum_turning_radius, + turning_radius_size, + theta_size, + curve_weight, + reverse_weight, + lateral_goal_range, + longitudinal_goal_range, + angle_goal_range, + obstacle_threshold}; +} + +std::unique_ptr configure_astar(bool use_multi, bool cw) +{ + auto planner_common_param = get_default_planner_params(); + + // configure astar param + const bool only_behind_solutions = false; + const bool use_back = true; + const bool use_curve_weight = false; + const bool use_complete_astar = false; + const double distance_heuristic_weight = 1.0; + auto astar_param = + fpa::AstarParam{only_behind_solutions, use_back, use_curve_weight, use_complete_astar, distance_heuristic_weight}; + + if (use_multi) { + planner_common_param.minimum_turning_radius = 5.0; + planner_common_param.maximum_turning_radius = 9.0; + planner_common_param.turning_radius_size = 3; + // if multi-curvature and using curve weight + if (cw){ + astar_param.use_curve_weight = true; + } + astar_param.use_complete_astar = true; + } + + auto algo = std::make_unique(planner_common_param, vehicle_shape, astar_param); + return algo; +} + +enum AlgorithmType { + ASTAR_SINGLE, + ASTAR_MULTI, + ASTAR_MULTI_NOCW, +}; +// cspell: ignore fpalgos +std::unordered_map rosbag_dir_prefix_table( + {{ASTAR_SINGLE, "fpalgos-astar_single"}, + {ASTAR_MULTI, "fpalgos-astar_multi"}, + {ASTAR_MULTI_NOCW, "fpalgos-astar_multi_nocw"}, + }); + +bool test_algorithm(enum AlgorithmType algo_type, bool dump_rosbag = false) +{ + std::unique_ptr algo; + if (algo_type == AlgorithmType::ASTAR_SINGLE) { + algo = configure_astar(false, false); + } else if (algo_type == AlgorithmType::ASTAR_MULTI) { + algo = configure_astar(true, true); + } else if (algo_type == AlgorithmType::ASTAR_MULTI_NOCW) { + algo = configure_astar(true, false); + } else { + throw std::runtime_error("invalid algorithm time"); + } + + // All algorithms have the same interface. + const auto costmap_msg = construct_cost_map(200, 120, 0.2, 10); + bool success_all = true; // if any local test below fails, overwrite this function + + rclcpp::Clock clock{RCL_SYSTEM_TIME}; + for (size_t i = 0; i < start_poses.size(); ++i) { + const auto start_pose = start_poses.at(i); + + algo->setMap(costmap_msg); + double msec; + double cost; + + { + const rclcpp::Time begin = clock.now(); + if (!algo->makePlan(create_pose_msg(start_pose), create_pose_msg(goal_pose))) { + success_all = false; + std::cout << "plan fail" << std::endl; + continue; + } + const rclcpp::Time now = clock.now(); + msec = (now - begin).seconds() * 1000.0; + cost = algo->getWaypoints().compute_length(); + } + + std::cout << "plan success : " << msec << "[msec]" + << ", solution cost : " << cost << std::endl; + const auto result = algo->getWaypoints(); + geometry_msgs::msg::PoseArray trajectory; + for (const auto & pose : result.waypoints) { + trajectory.poses.push_back(pose.pose.pose); + } + if (algo->hasObstacleOnTrajectory(trajectory)) { + std::cout << "not feasible trajectory" << std::endl; + success_all = false; + } + + if (dump_rosbag) { + // dump rosbag for visualization using python script + const std::string dir_name = + "/tmp/" + rosbag_dir_prefix_table[algo_type] + "-case" + std::to_string(i); + + rcpputils::fs::remove_all(dir_name); + + rosbag2_storage::StorageOptions storage_options; + storage_options.uri = dir_name; + storage_options.storage_id = "sqlite3"; + + rosbag2_cpp::ConverterOptions converter_options; + converter_options.input_serialization_format = "cdr"; + converter_options.output_serialization_format = "cdr"; + + rosbag2_cpp::Writer writer(std::make_unique()); + writer.open(storage_options, converter_options); + + add_message_to_rosbag( + writer, create_float_msg(vehicle_shape.length), "vehicle_length", "std_msgs/msg/Float64"); + add_message_to_rosbag( + writer, create_float_msg(vehicle_shape.width), "vehicle_width", "std_msgs/msg/Float64"); + add_message_to_rosbag( + writer, create_float_msg(vehicle_shape.base2back), "vehicle_base2back", + "std_msgs/msg/Float64"); + + add_message_to_rosbag(writer, costmap_msg, "costmap", "nav_msgs/msg/OccupancyGrid"); + add_message_to_rosbag(writer, create_pose_msg(start_pose), "start", "geometry_msgs/msg/Pose"); + add_message_to_rosbag(writer, create_pose_msg(goal_pose), "goal", "geometry_msgs/msg/Pose"); + add_message_to_rosbag(writer, trajectory, "trajectory", "geometry_msgs/msg/PoseArray"); + add_message_to_rosbag(writer, create_float_msg(msec), "elapsed_time", "std_msgs/msg/Float64"); + } + } + return success_all; +} + +TEST(AstarSearchTestSuite, SingleCurvature) +{ + EXPECT_TRUE(test_algorithm(AlgorithmType::ASTAR_SINGLE)); +} + +TEST(AstarSearchTestSuite, MultiCurvature) +{ + EXPECT_TRUE(test_algorithm(AlgorithmType::ASTAR_MULTI)); +} + +TEST(AstarSearchTestSuite, MultiCurvaturenocw) +{ + EXPECT_TRUE(test_algorithm(AlgorithmType::ASTAR_MULTI_NOCW)); +} + +int main(int argc, char ** argv) +{ + testing::InitGoogleTest(&argc, argv); + return RUN_ALL_TESTS(); +} From 0ef18f779b29e163c983a608de0ed2577a4d1fcd Mon Sep 17 00:00:00 2001 From: Aswin Vijay Date: Fri, 15 Mar 2024 18:14:17 +0900 Subject: [PATCH 2/5] add rrtstar algorithm --- .../CMakeLists.txt | 6 +- ...pace_planning_algorithms_no_obstacles.cpp} | 63 ++++++++++++++++++- 2 files changed, 64 insertions(+), 5 deletions(-) rename planning/freespace_planning_algorithms/test/src/{test_freespace_planning_algorithms_astar.cpp => test_freespace_planning_algorithms_no_obstacles.cpp} (83%) diff --git a/planning/freespace_planning_algorithms/CMakeLists.txt b/planning/freespace_planning_algorithms/CMakeLists.txt index 29d5cee425642..5061a0453b94d 100644 --- a/planning/freespace_planning_algorithms/CMakeLists.txt +++ b/planning/freespace_planning_algorithms/CMakeLists.txt @@ -35,10 +35,10 @@ if(BUILD_TESTING) freespace_planning_algorithms ) find_package(ament_cmake_ros REQUIRED) - ament_add_ros_isolated_gtest(freespace_planning_algorithms_astar-test - test/src/test_freespace_planning_algorithms_astar.cpp + ament_add_ros_isolated_gtest(freespace_planning_algorithms_no_obstacles-test + test/src/test_freespace_planning_algorithms_no_obstacles.cpp ) - target_link_libraries(freespace_planning_algorithms_astar-test + target_link_libraries(freespace_planning_algorithms_no_obstacles-test freespace_planning_algorithms ) find_package(ament_cmake_ros REQUIRED) diff --git a/planning/freespace_planning_algorithms/test/src/test_freespace_planning_algorithms_astar.cpp b/planning/freespace_planning_algorithms/test/src/test_freespace_planning_algorithms_no_obstacles.cpp similarity index 83% rename from planning/freespace_planning_algorithms/test/src/test_freespace_planning_algorithms_astar.cpp rename to planning/freespace_planning_algorithms/test/src/test_freespace_planning_algorithms_no_obstacles.cpp index ce518235ee119..dafce645a7f5e 100644 --- a/planning/freespace_planning_algorithms/test/src/test_freespace_planning_algorithms_astar.cpp +++ b/planning/freespace_planning_algorithms/test/src/test_freespace_planning_algorithms_no_obstacles.cpp @@ -1,4 +1,4 @@ -// Copyright 2021 Tier IV, Inc. All rights reserved. +// Copyright 2024 Tier IV, Inc. All rights reserved. // // Licensed under the Apache License, Version 2.0 (the "License"); // you may not use this file except in compliance with the License. @@ -240,16 +240,35 @@ std::unique_ptr configure_astar(bool use_multi, return algo; } +std::unique_ptr configure_rrtstar(bool informed, bool update) +{ + auto planner_common_param = get_default_planner_params(); + + // configure rrtstar param + const double mu = 12.0; + const double margin = 0.2; + const double max_planning_time = 200; + const auto rrtstar_param = fpa::RRTStarParam{update, informed, max_planning_time, mu, margin}; + auto algo = std::make_unique(planner_common_param, vehicle_shape, rrtstar_param); + return algo; +} + enum AlgorithmType { ASTAR_SINGLE, ASTAR_MULTI, ASTAR_MULTI_NOCW, + RRTSTAR_FASTEST, + RRTSTAR_UPDATE, + RRTSTAR_INFORMED_UPDATE, }; // cspell: ignore fpalgos std::unordered_map rosbag_dir_prefix_table( {{ASTAR_SINGLE, "fpalgos-astar_single"}, {ASTAR_MULTI, "fpalgos-astar_multi"}, {ASTAR_MULTI_NOCW, "fpalgos-astar_multi_nocw"}, + {RRTSTAR_FASTEST, "fpalgos-rrtstar_fastest"}, + {RRTSTAR_UPDATE, "fpalgos-rrtstar_update"}, + {RRTSTAR_INFORMED_UPDATE, "fpalgos-rrtstar_informed_update"} }); bool test_algorithm(enum AlgorithmType algo_type, bool dump_rosbag = false) @@ -261,6 +280,12 @@ bool test_algorithm(enum AlgorithmType algo_type, bool dump_rosbag = false) algo = configure_astar(true, true); } else if (algo_type == AlgorithmType::ASTAR_MULTI_NOCW) { algo = configure_astar(true, false); + } else if (algo_type == AlgorithmType::RRTSTAR_FASTEST) { + algo = configure_rrtstar(false, false); + } else if (algo_type == AlgorithmType::RRTSTAR_UPDATE) { + algo = configure_rrtstar(false, true); + } else if (algo_type == AlgorithmType::RRTSTAR_INFORMED_UPDATE) { + algo = configure_rrtstar(true, true); } else { throw std::runtime_error("invalid algorithm time"); } @@ -277,7 +302,26 @@ bool test_algorithm(enum AlgorithmType algo_type, bool dump_rosbag = false) double msec; double cost; - { + if (algo_type == RRTSTAR_FASTEST || algo_type == RRTSTAR_UPDATE) { + std::cout << "measuring average performance ..." << std::endl; + const size_t N_mc = (algo_type == RRTSTAR_UPDATE ? 5 : 100); + double time_sum = 0.0; + double cost_sum = 0.0; + for (size_t j = 0; j < N_mc; j++) { + const rclcpp::Time begin = clock.now(); + if (!algo->makePlan(create_pose_msg(start_pose), create_pose_msg(goal_pose))) { + success_all = false; + std::cout << "plan fail" << std::endl; + continue; + } + const rclcpp::Time now = clock.now(); + time_sum += (now - begin).seconds() * 1000.0; + cost_sum += algo->getWaypoints().compute_length(); + } + msec = time_sum / N_mc; // average performance + cost = cost_sum / N_mc; + + } else { const rclcpp::Time begin = clock.now(); if (!algo->makePlan(create_pose_msg(start_pose), create_pose_msg(goal_pose))) { success_all = false; @@ -352,6 +396,21 @@ TEST(AstarSearchTestSuite, MultiCurvaturenocw) EXPECT_TRUE(test_algorithm(AlgorithmType::ASTAR_MULTI_NOCW)); } +TEST(RRTStarTestSuite, Fastest) +{ + EXPECT_TRUE(test_algorithm(AlgorithmType::RRTSTAR_FASTEST)); +} + +TEST(RRTStarTestSuite, Update) +{ + EXPECT_TRUE(test_algorithm(AlgorithmType::RRTSTAR_UPDATE)); +} + +TEST(RRTStarTestSuite, InformedUpdate) +{ + EXPECT_TRUE(test_algorithm(AlgorithmType::RRTSTAR_INFORMED_UPDATE)); +} + int main(int argc, char ** argv) { testing::InitGoogleTest(&argc, argv); From 628ae0e711c318f12b07a3ed422e7f0bef4cd8a7 Mon Sep 17 00:00:00 2001 From: Aswin Vijay Date: Wed, 10 Apr 2024 10:52:17 +0900 Subject: [PATCH 3/5] update test files --- planning/freespace_planning_algorithms/CMakeLists.txt | 6 ++---- .../test/src/test_freespace_planning_algorithms.cpp | 7 ++++--- 2 files changed, 6 insertions(+), 7 deletions(-) diff --git a/planning/freespace_planning_algorithms/CMakeLists.txt b/planning/freespace_planning_algorithms/CMakeLists.txt index 5061a0453b94d..41144dd7648df 100644 --- a/planning/freespace_planning_algorithms/CMakeLists.txt +++ b/planning/freespace_planning_algorithms/CMakeLists.txt @@ -34,14 +34,12 @@ if(BUILD_TESTING) target_link_libraries(freespace_planning_algorithms-test freespace_planning_algorithms ) - find_package(ament_cmake_ros REQUIRED) - ament_add_ros_isolated_gtest(freespace_planning_algorithms_no_obstacles-test + ament_add_ros_isolated_gtest(freespace_planning_algorithms_no_obs-test test/src/test_freespace_planning_algorithms_no_obstacles.cpp ) - target_link_libraries(freespace_planning_algorithms_no_obstacles-test + target_link_libraries(freespace_planning_algorithms_no_obs-test freespace_planning_algorithms ) - find_package(ament_cmake_ros REQUIRED) ament_add_gtest(rrtstar_core_informed-test test/src/test_rrtstar_core_informed.cpp ) diff --git a/planning/freespace_planning_algorithms/test/src/test_freespace_planning_algorithms.cpp b/planning/freespace_planning_algorithms/test/src/test_freespace_planning_algorithms.cpp index c11c3df73d467..d5d894db557cf 100644 --- a/planning/freespace_planning_algorithms/test/src/test_freespace_planning_algorithms.cpp +++ b/planning/freespace_planning_algorithms/test/src/test_freespace_planning_algorithms.cpp @@ -134,8 +134,7 @@ nav_msgs::msg::OccupancyGrid construct_cost_map( } } } - return costmap_msg; - + return costmap_msg; } @@ -210,9 +209,11 @@ std::unique_ptr configure_astar(bool use_multi) // configure astar param const bool only_behind_solutions = false; const bool use_back = true; + const bool use_curve_weight = true; + const bool use_complete_astar = true; const double distance_heuristic_weight = 1.0; const auto astar_param = - fpa::AstarParam{only_behind_solutions, use_back, distance_heuristic_weight}; + fpa::AstarParam{only_behind_solutions, use_back, use_curve_weight, use_complete_astar, distance_heuristic_weight}; auto algo = std::make_unique(planner_common_param, vehicle_shape, astar_param); return algo; From a68c6e50ad874090264881a825279b11400a964f Mon Sep 17 00:00:00 2001 From: Aswin Vijay Date: Wed, 10 Apr 2024 16:43:22 +0900 Subject: [PATCH 4/5] fix copyright name --- .../test/src/test_freespace_planning_algorithms.cpp | 2 +- .../src/test_freespace_planning_algorithms_no_obstacles.cpp | 2 +- 2 files changed, 2 insertions(+), 2 deletions(-) diff --git a/planning/freespace_planning_algorithms/test/src/test_freespace_planning_algorithms.cpp b/planning/freespace_planning_algorithms/test/src/test_freespace_planning_algorithms.cpp index d5d894db557cf..613e038c7c885 100644 --- a/planning/freespace_planning_algorithms/test/src/test_freespace_planning_algorithms.cpp +++ b/planning/freespace_planning_algorithms/test/src/test_freespace_planning_algorithms.cpp @@ -1,4 +1,4 @@ -// Copyright 2021 Tier IV, Inc. All rights reserved. +// Copyright 2021 TIER IV, Inc. All rights reserved. // // Licensed under the Apache License, Version 2.0 (the "License"); // you may not use this file except in compliance with the License. diff --git a/planning/freespace_planning_algorithms/test/src/test_freespace_planning_algorithms_no_obstacles.cpp b/planning/freespace_planning_algorithms/test/src/test_freespace_planning_algorithms_no_obstacles.cpp index dafce645a7f5e..ad7bd8977f47d 100644 --- a/planning/freespace_planning_algorithms/test/src/test_freespace_planning_algorithms_no_obstacles.cpp +++ b/planning/freespace_planning_algorithms/test/src/test_freespace_planning_algorithms_no_obstacles.cpp @@ -1,4 +1,4 @@ -// Copyright 2024 Tier IV, Inc. All rights reserved. +// Copyright 2024 TIER IV, Inc. All rights reserved. // // Licensed under the Apache License, Version 2.0 (the "License"); // you may not use this file except in compliance with the License. From 9ec58dca94d0c85c709ae197f6bea7f839dd3bac Mon Sep 17 00:00:00 2001 From: "pre-commit-ci[bot]" <66853113+pre-commit-ci[bot]@users.noreply.github.com> Date: Wed, 10 Apr 2024 07:45:43 +0000 Subject: [PATCH 5/5] style(pre-commit): autofix --- .../test_freespace_planning_algorithms.cpp | 7 ++-- ...space_planning_algorithms_no_obstacles.cpp | 34 +++++++++---------- 2 files changed, 21 insertions(+), 20 deletions(-) diff --git a/planning/freespace_planning_algorithms/test/src/test_freespace_planning_algorithms.cpp b/planning/freespace_planning_algorithms/test/src/test_freespace_planning_algorithms.cpp index 613e038c7c885..89519a0fb8a4e 100644 --- a/planning/freespace_planning_algorithms/test/src/test_freespace_planning_algorithms.cpp +++ b/planning/freespace_planning_algorithms/test/src/test_freespace_planning_algorithms.cpp @@ -134,7 +134,7 @@ nav_msgs::msg::OccupancyGrid construct_cost_map( } } } - + return costmap_msg; } @@ -212,8 +212,9 @@ std::unique_ptr configure_astar(bool use_multi) const bool use_curve_weight = true; const bool use_complete_astar = true; const double distance_heuristic_weight = 1.0; - const auto astar_param = - fpa::AstarParam{only_behind_solutions, use_back, use_curve_weight, use_complete_astar, distance_heuristic_weight}; + const auto astar_param = fpa::AstarParam{ + only_behind_solutions, use_back, use_curve_weight, use_complete_astar, + distance_heuristic_weight}; auto algo = std::make_unique(planner_common_param, vehicle_shape, astar_param); return algo; diff --git a/planning/freespace_planning_algorithms/test/src/test_freespace_planning_algorithms_no_obstacles.cpp b/planning/freespace_planning_algorithms/test/src/test_freespace_planning_algorithms_no_obstacles.cpp index ad7bd8977f47d..388e9116ae948 100644 --- a/planning/freespace_planning_algorithms/test/src/test_freespace_planning_algorithms_no_obstacles.cpp +++ b/planning/freespace_planning_algorithms/test/src/test_freespace_planning_algorithms_no_obstacles.cpp @@ -42,9 +42,9 @@ const double pi = 3.1415926; const std::array start_pose1{8., 7., pi * 0.5}; const std::array start_pose2{9., 12., pi * 0.25}; const std::array start_pose3{13.3, 9., pi * 0.5}; -const std::array start_pose4{25.0, 16., - pi * 0.25}; +const std::array start_pose4{25.0, 16., -pi * 0.25}; const std::array start_pose5{20.5, 8.5, 0.0}; -const std::array goal_pose{18.0, 10.5, pi * 0.5}; +const std::array goal_pose{18.0, 10.5, pi * 0.5}; const std::array, 5> start_poses{ start_pose1, start_pose2, start_pose3, start_pose4, start_pose5}; @@ -88,7 +88,7 @@ nav_msgs::msg::OccupancyGrid construct_cost_map( const double x = j * resolution; const double y = i * resolution; - //road + // road if (5.4 < x && x < 5.4 + 4.5 && 0.0 < y && y < 0.0 + 19.4) { costmap_msg.data[i * width + j] = 40.0; } @@ -100,27 +100,27 @@ nav_msgs::msg::OccupancyGrid construct_cost_map( double width_p = 3.0; double height_p = 6.0; - //parking 1 (16,9) + // parking 1 (16,9) if (16.0 < x && x < 16.0 + width_p && 9.0 < y && y < 9.0 + height_p) { costmap_msg.data[i * width + j] = 50.0; } - //parking 2 + // parking 2 if (19.3 < x && x < 19.3 + width_p && 9.0 < y && y < 9.0 + height_p) { costmap_msg.data[i * width + j] = 50.0; } - //parking 3 + // parking 3 if (22.6 < x && x < 22.6 + width_p && 9.0 < y && y < 9.0 + height_p) { costmap_msg.data[i * width + j] = 50.0; } - //parking 4 + // parking 4 if (25.9 < x && x < 25.9 + width_p && 9.0 < y && y < 9.0 + height_p) { costmap_msg.data[i * width + j] = 50.0; } - //parking 5 + // parking 5 if (29.2 < x && x < 29.2 + width_p && 9.0 < y && y < 9.0 + height_p) { costmap_msg.data[i * width + j] = 50.0; } @@ -148,7 +148,7 @@ nav_msgs::msg::OccupancyGrid construct_cost_map( costmap_msg.data[j] = 100.0; } } - + return costmap_msg; } @@ -187,7 +187,7 @@ fpa::PlannerCommonParam get_default_planner_params() const double maximum_turning_radius = 9.0; const int turning_radius_size = 1; - // Transitions every 2.5 deg + // Transitions every 2.5 deg const int theta_size = 144; const double curve_weight = 1.5; @@ -222,16 +222,17 @@ std::unique_ptr configure_astar(bool use_multi, const bool use_curve_weight = false; const bool use_complete_astar = false; const double distance_heuristic_weight = 1.0; - auto astar_param = - fpa::AstarParam{only_behind_solutions, use_back, use_curve_weight, use_complete_astar, distance_heuristic_weight}; - + auto astar_param = fpa::AstarParam{ + only_behind_solutions, use_back, use_curve_weight, use_complete_astar, + distance_heuristic_weight}; + if (use_multi) { planner_common_param.minimum_turning_radius = 5.0; planner_common_param.maximum_turning_radius = 9.0; planner_common_param.turning_radius_size = 3; // if multi-curvature and using curve weight - if (cw){ - astar_param.use_curve_weight = true; + if (cw) { + astar_param.use_curve_weight = true; } astar_param.use_complete_astar = true; } @@ -268,8 +269,7 @@ std::unordered_map rosbag_dir_prefix_table( {ASTAR_MULTI_NOCW, "fpalgos-astar_multi_nocw"}, {RRTSTAR_FASTEST, "fpalgos-rrtstar_fastest"}, {RRTSTAR_UPDATE, "fpalgos-rrtstar_update"}, - {RRTSTAR_INFORMED_UPDATE, "fpalgos-rrtstar_informed_update"} - }); + {RRTSTAR_INFORMED_UPDATE, "fpalgos-rrtstar_informed_update"}}); bool test_algorithm(enum AlgorithmType algo_type, bool dump_rosbag = false) {