Skip to content

Commit

Permalink
test(freespace_planning_algorithms): done't dump rosbag by default (a…
Browse files Browse the repository at this point in the history 
…utowarefoundation#2504)

Signed-off-by: Hirokazu Ishida <[email protected]>

Signed-off-by: Hirokazu Ishida <[email protected]>
Signed-off-by: Kotaro Yoshimoto <[email protected]>
  • Loading branch information
@HiroIshida @HansRobo
HiroIshida authored and HansRobo committed Dec 16, 2022
1 parent 43a853a commit 281e163
Showing 1 changed file with 43 additions and 44 deletions.
87 changes: 43 additions & 44 deletions planning/freespace_planning_algorithms/test/src/test_freespace_planning_algorithms.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -40,16 +40,12 @@ 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<double, 3> start_pose{5.5, 4., pi * 0.5};
const std::array<double, 3> goal_pose1{8.0, 26.3, pi * 1.5}; // easiest
const std::array<std::array<double, 3>, 1> goal_poses{goal_pose1};

// the tests for following goals randomly fail. needs to be fixed.
// https://github.com/autowarefoundation/autoware.universe/issues/2439
// const std::array<double, 3> goal_pose2{15.0, 11.6, pi * 0.5}; // second easiest
// const std::array<double, 3> goal_pose3{18.4, 26.3, pi * 1.5}; // third easiest
// const std::array<double, 3> goal_pose4{25.0, 26.3, pi * 1.5}; // most difficult
// const std::array<std::array<double, 3>, 4> goal_poses{
// goal_pose1, goal_pose2, goal_pose3, goal_pose4};
const std::array<double, 3> goal_pose1{8.0, 26.3, pi * 1.5}; // easiest
const std::array<double, 3> goal_pose2{15.0, 11.6, pi * 0.5}; // second easiest
const std::array<double, 3> goal_pose3{18.4, 26.3, pi * 1.5}; // third easiest
const std::array<double, 3> goal_pose4{25.0, 26.3, pi * 1.5}; // most difficult
const std::array<std::array<double, 3>, 4> goal_poses{
goal_pose1, goal_pose2, goal_pose3, goal_pose4};

geometry_msgs::msg::Pose create_pose_msg(std::array<double, 3> pose3d)
{
Expand Down Expand Up @@ -250,7 +246,7 @@ std::unordered_map<AlgorithmType, std::string> rosbag_dir_prefix_table(
{RRTSTAR_UPDATE, "fpalgos-rrtstar_update"},
{RRTSTAR_INFORMED_UPDATE, "fpalgos-rrtstar_informed_update"}});

bool test_algorithm(enum AlgorithmType algo_type)
bool test_algorithm(enum AlgorithmType algo_type, bool dump_rosbag = false)
{
std::unique_ptr<fpa::AbstractPlanningAlgorithm> algo;
if (algo_type == AlgorithmType::ASTAR_SINGLE) {
Expand All @@ -273,8 +269,6 @@ bool test_algorithm(enum AlgorithmType algo_type)

rclcpp::Clock clock{RCL_SYSTEM_TIME};
for (size_t i = 0; i < goal_poses.size(); ++i) {
const std::string dir_name =
"/tmp/" + rosbag_dir_prefix_table[algo_type] + "-case" + std::to_string(i);
const auto goal_pose = goal_poses.at(i);

bool success_local = true;
Expand Down Expand Up @@ -325,41 +319,46 @@ bool test_algorithm(enum AlgorithmType algo_type)
success_all = false;
}

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<rosbag2_cpp::writers::SequentialWriter>());
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");
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<rosbag2_cpp::writers::SequentialWriter>());
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;
}

// the following test fails https://github.com/autowarefoundation/autoware.universe/issues/2439
// TEST(AstarSearchTestSuite, SingleCurvature)
// {
// EXPECT_TRUE(test_algorithm(AlgorithmType::ASTAR_SINGLE));
// }
TEST(AstarSearchTestSuite, SingleCurvature)
{
EXPECT_TRUE(test_algorithm(AlgorithmType::ASTAR_SINGLE));
}

TEST(AstarSearchTestSuite, MultiCurvature)
{
Expand Down

0 comments on commit 281e163

Please sign in to comment.