From 6b279c35bbc6176964d596c1ba963dacd6543c81 Mon Sep 17 00:00:00 2001 From: JaeyoungLim Date: Mon, 27 Nov 2023 22:32:47 +0100 Subject: [PATCH] Move testers to terrain-planner-benchmarks F F F --- .../terrain_navigation_ros/terrain_planner.h | 2 +- .../terrain_navigation_ros/visualization.h | 208 ++++++++++++++++- terrain_planner/CMakeLists.txt | 21 -- .../include/terrain_planner/visualization.h | 215 ------------------ terrain_planner_benchmark/CMakeLists.txt | 18 ++ .../terrain_planner_benchmark.h | 2 +- .../src/surface_visualization.cpp | 2 +- .../src/terrain_planner_benchmark_node.cpp | 2 +- terrain_planner_benchmark/src/test_ics.cpp | 2 +- .../src/test_ompl_dubins.cpp | 2 +- .../src/test_ompl_dubins_to_circle.cpp | 2 +- .../src/test_rrt_circle_goal.cpp | 2 +- .../src/test_rrt_node.cpp | 2 +- .../src/test_rrt_replanning_node.cpp | 2 +- 14 files changed, 228 insertions(+), 254 deletions(-) delete mode 100644 terrain_planner/include/terrain_planner/visualization.h rename {terrain_planner => terrain_planner_benchmark}/src/test_ompl_dubins.cpp (99%) rename {terrain_planner => terrain_planner_benchmark}/src/test_ompl_dubins_to_circle.cpp (99%) rename {terrain_planner => terrain_planner_benchmark}/src/test_rrt_node.cpp (99%) diff --git a/terrain_navigation_ros/include/terrain_navigation_ros/terrain_planner.h b/terrain_navigation_ros/include/terrain_navigation_ros/terrain_planner.h index 171346f3..aef83cc4 100644 --- a/terrain_navigation_ros/include/terrain_navigation_ros/terrain_planner.h +++ b/terrain_navigation_ros/include/terrain_navigation_ros/terrain_planner.h @@ -61,9 +61,9 @@ #include #include +#include "terrain_navigation_ros/visualization.h" #include "terrain_planner/common.h" #include "terrain_planner/terrain_ompl_rrt.h" -#include "terrain_planner/visualization.h" #include #include diff --git a/terrain_navigation_ros/include/terrain_navigation_ros/visualization.h b/terrain_navigation_ros/include/terrain_navigation_ros/visualization.h index 73cc22ed..919884fe 100644 --- a/terrain_navigation_ros/include/terrain_navigation_ros/visualization.h +++ b/terrain_navigation_ros/include/terrain_navigation_ros/visualization.h @@ -41,7 +41,7 @@ #include #include -inline geometry_msgs::Point toPoint(const Eigen::Vector3d &p) { +inline geometry_msgs::Point toPoint(const Eigen::Vector3d& p) { geometry_msgs::Point position; position.x = p(0); position.y = p(1); @@ -49,8 +49,8 @@ inline geometry_msgs::Point toPoint(const Eigen::Vector3d &p) { return position; } -inline void publishVehiclePose(const ros::Publisher pub, const Eigen::Vector3d &position, - const Eigen::Vector4d &attitude, std::string mesh_resource_path) { +inline void publishVehiclePose(const ros::Publisher pub, const Eigen::Vector3d& position, + const Eigen::Vector4d& attitude, std::string mesh_resource_path) { Eigen::Vector4d mesh_attitude = quatMultiplication(attitude, Eigen::Vector4d(std::cos(M_PI / 2), 0.0, 0.0, std::sin(M_PI / 2))); geometry_msgs::Pose vehicle_pose = vector3d2PoseMsg(position, mesh_attitude); @@ -71,7 +71,7 @@ inline void publishVehiclePose(const ros::Publisher pub, const Eigen::Vector3d & pub.publish(marker); } -inline visualization_msgs::Marker Viewpoint2MarkerMsg(int id, ViewPoint &viewpoint, +inline visualization_msgs::Marker Viewpoint2MarkerMsg(int id, ViewPoint& viewpoint, Eigen::Vector3d color = Eigen::Vector3d(0.0, 0.0, 1.0)) { double scale{15}; // Size of the viewpoint markers visualization_msgs::Marker marker; @@ -85,7 +85,7 @@ inline visualization_msgs::Marker Viewpoint2MarkerMsg(int id, ViewPoint &viewpoi std::vector points; std::vector corner_ray_vectors = viewpoint.getCornerRayVectors(); std::vector vertex; - for (auto &corner_ray : corner_ray_vectors) { + for (auto& corner_ray : corner_ray_vectors) { vertex.push_back(position + scale * corner_ray); } @@ -111,15 +111,15 @@ inline visualization_msgs::Marker Viewpoint2MarkerMsg(int id, ViewPoint &viewpoi return marker; } -inline void publishCameraView(const ros::Publisher pub, const Eigen::Vector3d &position, - const Eigen::Vector4d &attitude) { +inline void publishCameraView(const ros::Publisher pub, const Eigen::Vector3d& position, + const Eigen::Vector4d& attitude) { visualization_msgs::Marker marker; ViewPoint viewpoint(-1, position, attitude); marker = Viewpoint2MarkerMsg(viewpoint.getIndex(), viewpoint); pub.publish(marker); } -inline void publishViewpoints(const ros::Publisher pub, std::vector &viewpoint_vector, +inline void publishViewpoints(const ros::Publisher pub, std::vector& viewpoint_vector, Eigen::Vector3d color = Eigen::Vector3d(0.0, 0.0, 1.0)) { visualization_msgs::MarkerArray msg; @@ -140,4 +140,196 @@ inline void publishViewpoints(const ros::Publisher pub, std::vector & pub.publish(msg); } +void publishCandidateManeuvers(const ros::Publisher& pub, const std::vector& candidate_maneuvers, + bool visualize_invalid_trajectories = false) { + visualization_msgs::MarkerArray msg; + + std::vector marker; + visualization_msgs::Marker mark; + mark.action = visualization_msgs::Marker::DELETEALL; + marker.push_back(mark); + msg.markers = marker; + pub.publish(msg); + + std::vector maneuver_library_vector; + int i = 0; + for (auto maneuver : candidate_maneuvers) { + if (maneuver.valid() || visualize_invalid_trajectories) { + maneuver_library_vector.insert(maneuver_library_vector.begin(), trajectory2MarkerMsg(maneuver, i)); + } + i++; + } + msg.markers = maneuver_library_vector; + pub.publish(msg); +} + +void publishPositionSetpoints(const ros::Publisher& pub, const Eigen::Vector3d& position, + const Eigen::Vector3d& velocity, + const Eigen::Vector3d scale = Eigen::Vector3d(10.0, 2.0, 2.0)) { + visualization_msgs::Marker marker; + marker.header.stamp = ros::Time::now(); + marker.type = visualization_msgs::Marker::ARROW; + marker.header.frame_id = "map"; + marker.id = 0; + marker.action = visualization_msgs::Marker::DELETEALL; + pub.publish(marker); + + marker.header.stamp = ros::Time::now(); + marker.action = visualization_msgs::Marker::ADD; + marker.scale.x = scale(0); + marker.scale.y = scale(1); + marker.scale.z = scale(2); + marker.color.a = 0.5; // Don't forget to set the alpha! + marker.color.r = 0.0; + marker.color.g = 0.0; + marker.color.b = 1.0; + marker.pose.position = tf2::toMsg(position); + tf2::Quaternion q; + q.setRPY(0, 0, std::atan2(velocity.y(), velocity.x())); + marker.pose.orientation = tf2::toMsg(q); + + pub.publish(marker); +} + +void publishPath(const ros::Publisher& pub, std::vector path, Eigen::Vector3d color) { + visualization_msgs::Marker marker; + marker.header.stamp = ros::Time::now(); + marker.type = visualization_msgs::Marker::LINE_STRIP; + marker.header.frame_id = "map"; + marker.id = 0; + marker.action = visualization_msgs::Marker::ADD; + std::vector points; + for (auto& position : path) { + geometry_msgs::Point point; + point.x = position(0); + point.y = position(1); + point.z = position(2); + points.push_back(point); + } + std::cout << "Points: " << points.size() << std::endl; + marker.points = points; + marker.pose.orientation.x = 0.0; + marker.pose.orientation.y = 0.0; + marker.pose.orientation.z = 0.0; + marker.pose.orientation.w = 1.0; + marker.scale.x = 10.0; + marker.scale.y = 10.0; + marker.scale.z = 10.0; + marker.color.a = 0.8; + marker.color.r = color.x(); + marker.color.g = color.y(); + marker.color.b = color.z(); + pub.publish(marker); +} + +void publishTrajectory(ros::Publisher& pub, std::vector trajectory) { + nav_msgs::Path msg; + std::vector posestampedhistory_vector; + Eigen::Vector4d orientation(1.0, 0.0, 0.0, 0.0); + for (auto pos : trajectory) { + posestampedhistory_vector.insert(posestampedhistory_vector.begin(), vector3d2PoseStampedMsg(pos, orientation)); + } + msg.header.stamp = ros::Time::now(); + msg.header.frame_id = "map"; + msg.poses = posestampedhistory_vector; + pub.publish(msg); +} + +void publishTree(const ros::Publisher& pub, std::shared_ptr planner_data, + std::shared_ptr problem_setup) { + visualization_msgs::MarkerArray marker_array; + std::vector marker; + + planner_data->decoupleFromPlanner(); + + // Create states, a marker and a list to store edges + ompl::base::ScopedState vertex(problem_setup->getSpaceInformation()); + ompl::base::ScopedState neighbor_vertex( + problem_setup->getSpaceInformation()); + size_t marker_idx{0}; + auto dubins_ss = std::make_shared(); + for (size_t i = 0; i < planner_data->numVertices(); i++) { + visualization_msgs::Marker marker; + marker.header.stamp = ros::Time().now(); + marker.header.frame_id = "map"; + vertex = planner_data->getVertex(i).getState(); + marker.ns = "vertex"; + marker.id = marker_idx++; + marker.pose.position.x = vertex[0]; + marker.pose.position.y = vertex[1]; + marker.pose.position.z = vertex[2]; + marker.pose.orientation.w = std::cos(0.5 * vertex[3]); + marker.pose.orientation.x = 0.0; + marker.pose.orientation.y = 0.0; + marker.pose.orientation.z = std::sin(0.5 * vertex[3]); + marker.scale.x = 10.0; + marker.scale.y = 2.0; + marker.scale.z = 2.0; + marker.type = visualization_msgs::Marker::ARROW; + marker.action = visualization_msgs::Marker::ADD; + marker.color.a = 0.5; // Don't forget to set the alpha! + marker.color.r = 1.0; + marker.color.g = 1.0; + marker.color.b = 0.0; + marker_array.markers.push_back(marker); + + // allocate variables + std::vector edge_list; + int num_edges = planner_data->getEdges(i, edge_list); + if (num_edges > 0) { + for (unsigned int edge : edge_list) { + visualization_msgs::Marker edge_marker; + edge_marker.header.stamp = ros::Time().now(); + edge_marker.header.frame_id = "map"; + edge_marker.id = marker_idx++; + edge_marker.type = visualization_msgs::Marker::LINE_STRIP; + edge_marker.ns = "edge"; + neighbor_vertex = planner_data->getVertex(edge).getState(); + // points.push_back(toMsg(Eigen::Vector3d(vertex[0], vertex[1], vertex[2]))); + // points.push_back(toMsg(Eigen::Vector3d(neighbor_vertex[0], neighbor_vertex[1], neighbor_vertex[2]))); + ompl::base::State* state = dubins_ss->allocState(); + ompl::base::State* from = dubins_ss->allocState(); + from->as()->setX(vertex[0]); + from->as()->setY(vertex[1]); + from->as()->setZ(vertex[2]); + from->as()->setYaw(vertex[3]); + + ompl::base::State* to = dubins_ss->allocState(); + to->as()->setX(neighbor_vertex[0]); + to->as()->setY(neighbor_vertex[1]); + to->as()->setZ(neighbor_vertex[2]); + to->as()->setYaw(neighbor_vertex[3]); + if (dubins_ss->equalStates(from, to)) { + continue; + } + std::vector points; + for (double t = 0.0; t < 1.0; t += 0.02) { + dubins_ss->interpolate(from, to, t, state); + auto interpolated_state = + Eigen::Vector3d(state->as()->getX(), + state->as()->getY(), + state->as()->getZ()); + points.push_back(tf2::toMsg(interpolated_state)); + } + points.push_back(tf2::toMsg(Eigen::Vector3d(neighbor_vertex[0], neighbor_vertex[1], neighbor_vertex[2]))); + edge_marker.points = points; + edge_marker.action = visualization_msgs::Marker::ADD; + edge_marker.pose.orientation.w = 1.0; + edge_marker.pose.orientation.x = 0.0; + edge_marker.pose.orientation.y = 0.0; + edge_marker.pose.orientation.z = 0.0; + edge_marker.scale.x = 1.0; + edge_marker.scale.y = 1.0; + edge_marker.scale.z = 1.0; + edge_marker.color.a = 0.5; // Don't forget to set the alpha! + edge_marker.color.r = 1.0; + edge_marker.color.g = 1.0; + edge_marker.color.b = 0.0; + marker_array.markers.push_back(edge_marker); + } + } + } + pub.publish(marker_array); +} + #endif diff --git a/terrain_planner/CMakeLists.txt b/terrain_planner/CMakeLists.txt index 44d36a46..ee56c076 100644 --- a/terrain_planner/CMakeLists.txt +++ b/terrain_planner/CMakeLists.txt @@ -46,27 +46,6 @@ add_library(${PROJECT_NAME} add_dependencies(${PROJECT_NAME} ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS} ${Boost_LIBRARIES}) target_link_libraries(${PROJECT_NAME} ${catkin_LIBRARIES} ${OpenCV_LIBRARIES} ${OMPL_LIBRARIES} ${Boost_LIBRARIES}) -############ -# BINARIES # -############ -add_executable(test_rrt_node - src/test_rrt_node.cpp -) -add_dependencies(test_rrt_node ${PROJECT_NAME} ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS}) -target_link_libraries(test_rrt_node ${PROJECT_NAME} ${catkin_LIBRARIES} ${OpenCV_LIBRARIES} ${OMPL_LIBRARIES}) - -add_executable(test_ompl_dubins - src/test_ompl_dubins.cpp -) -add_dependencies(test_ompl_dubins ${PROJECT_NAME} ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS}) -target_link_libraries(test_ompl_dubins ${PROJECT_NAME} ${catkin_LIBRARIES} ${OpenCV_LIBRARIES} ${OMPL_LIBRARIES}) - -add_executable(test_ompl_dubins_to_circle - src/test_ompl_dubins_to_circle.cpp -) -add_dependencies(test_ompl_dubins_to_circle ${PROJECT_NAME} ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS}) -target_link_libraries(test_ompl_dubins_to_circle ${PROJECT_NAME} ${catkin_LIBRARIES} ${OpenCV_LIBRARIES} ${OMPL_LIBRARIES}) - ########## # EXPORT # ########## diff --git a/terrain_planner/include/terrain_planner/visualization.h b/terrain_planner/include/terrain_planner/visualization.h deleted file mode 100644 index f49d2181..00000000 --- a/terrain_planner/include/terrain_planner/visualization.h +++ /dev/null @@ -1,215 +0,0 @@ -#ifndef TERRAIN_PLANNER_VISUALIZATION_H -#define TERRAIN_PLANNER_VISUALIZATION_H - -#include "terrain_navigation/path.h" -#include "terrain_planner/common.h" -#include "terrain_planner/ompl_setup.h" - -#include -#include - -#include -#include -#include -#include - -#include -#include -#include -#include -#include -#include - -void publishCandidateManeuvers(const ros::Publisher& pub, const std::vector& candidate_maneuvers, - bool visualize_invalid_trajectories = false) { - visualization_msgs::MarkerArray msg; - - std::vector marker; - visualization_msgs::Marker mark; - mark.action = visualization_msgs::Marker::DELETEALL; - marker.push_back(mark); - msg.markers = marker; - pub.publish(msg); - - std::vector maneuver_library_vector; - int i = 0; - for (auto maneuver : candidate_maneuvers) { - if (maneuver.valid() || visualize_invalid_trajectories) { - maneuver_library_vector.insert(maneuver_library_vector.begin(), trajectory2MarkerMsg(maneuver, i)); - } - i++; - } - msg.markers = maneuver_library_vector; - pub.publish(msg); -} - -void publishPositionSetpoints(const ros::Publisher& pub, const Eigen::Vector3d& position, - const Eigen::Vector3d& velocity, - const Eigen::Vector3d scale = Eigen::Vector3d(10.0, 2.0, 2.0)) { - visualization_msgs::Marker marker; - marker.header.stamp = ros::Time::now(); - marker.type = visualization_msgs::Marker::ARROW; - marker.header.frame_id = "map"; - marker.id = 0; - marker.action = visualization_msgs::Marker::DELETEALL; - pub.publish(marker); - - marker.header.stamp = ros::Time::now(); - marker.action = visualization_msgs::Marker::ADD; - marker.scale.x = scale(0); - marker.scale.y = scale(1); - marker.scale.z = scale(2); - marker.color.a = 0.5; // Don't forget to set the alpha! - marker.color.r = 0.0; - marker.color.g = 0.0; - marker.color.b = 1.0; - marker.pose.position = tf2::toMsg(position); - tf2::Quaternion q; - q.setRPY(0, 0, std::atan2(velocity.y(), velocity.x())); - marker.pose.orientation = tf2::toMsg(q); - - pub.publish(marker); -} - -void publishPath(const ros::Publisher& pub, std::vector path, Eigen::Vector3d color) { - visualization_msgs::Marker marker; - marker.header.stamp = ros::Time::now(); - marker.type = visualization_msgs::Marker::LINE_STRIP; - marker.header.frame_id = "map"; - marker.id = 0; - marker.action = visualization_msgs::Marker::ADD; - std::vector points; - for (auto& position : path) { - geometry_msgs::Point point; - point.x = position(0); - point.y = position(1); - point.z = position(2); - points.push_back(point); - } - std::cout << "Points: " << points.size() << std::endl; - marker.points = points; - marker.pose.orientation.x = 0.0; - marker.pose.orientation.y = 0.0; - marker.pose.orientation.z = 0.0; - marker.pose.orientation.w = 1.0; - marker.scale.x = 10.0; - marker.scale.y = 10.0; - marker.scale.z = 10.0; - marker.color.a = 0.8; - marker.color.r = color.x(); - marker.color.g = color.y(); - marker.color.b = color.z(); - pub.publish(marker); -} - -void publishTrajectory(ros::Publisher& pub, std::vector trajectory) { - nav_msgs::Path msg; - std::vector posestampedhistory_vector; - Eigen::Vector4d orientation(1.0, 0.0, 0.0, 0.0); - for (auto pos : trajectory) { - posestampedhistory_vector.insert(posestampedhistory_vector.begin(), vector3d2PoseStampedMsg(pos, orientation)); - } - msg.header.stamp = ros::Time::now(); - msg.header.frame_id = "map"; - msg.poses = posestampedhistory_vector; - pub.publish(msg); -} - -void publishTree(const ros::Publisher& pub, std::shared_ptr planner_data, - std::shared_ptr problem_setup) { - visualization_msgs::MarkerArray marker_array; - std::vector marker; - - planner_data->decoupleFromPlanner(); - - // Create states, a marker and a list to store edges - ompl::base::ScopedState vertex(problem_setup->getSpaceInformation()); - ompl::base::ScopedState neighbor_vertex( - problem_setup->getSpaceInformation()); - size_t marker_idx{0}; - auto dubins_ss = std::make_shared(); - for (size_t i = 0; i < planner_data->numVertices(); i++) { - visualization_msgs::Marker marker; - marker.header.stamp = ros::Time().now(); - marker.header.frame_id = "map"; - vertex = planner_data->getVertex(i).getState(); - marker.ns = "vertex"; - marker.id = marker_idx++; - marker.pose.position.x = vertex[0]; - marker.pose.position.y = vertex[1]; - marker.pose.position.z = vertex[2]; - marker.pose.orientation.w = std::cos(0.5 * vertex[3]); - marker.pose.orientation.x = 0.0; - marker.pose.orientation.y = 0.0; - marker.pose.orientation.z = std::sin(0.5 * vertex[3]); - marker.scale.x = 10.0; - marker.scale.y = 2.0; - marker.scale.z = 2.0; - marker.type = visualization_msgs::Marker::ARROW; - marker.action = visualization_msgs::Marker::ADD; - marker.color.a = 0.5; // Don't forget to set the alpha! - marker.color.r = 1.0; - marker.color.g = 1.0; - marker.color.b = 0.0; - marker_array.markers.push_back(marker); - - // allocate variables - std::vector edge_list; - int num_edges = planner_data->getEdges(i, edge_list); - if (num_edges > 0) { - for (unsigned int edge : edge_list) { - visualization_msgs::Marker edge_marker; - edge_marker.header.stamp = ros::Time().now(); - edge_marker.header.frame_id = "map"; - edge_marker.id = marker_idx++; - edge_marker.type = visualization_msgs::Marker::LINE_STRIP; - edge_marker.ns = "edge"; - neighbor_vertex = planner_data->getVertex(edge).getState(); - // points.push_back(toMsg(Eigen::Vector3d(vertex[0], vertex[1], vertex[2]))); - // points.push_back(toMsg(Eigen::Vector3d(neighbor_vertex[0], neighbor_vertex[1], neighbor_vertex[2]))); - ompl::base::State* state = dubins_ss->allocState(); - ompl::base::State* from = dubins_ss->allocState(); - from->as()->setX(vertex[0]); - from->as()->setY(vertex[1]); - from->as()->setZ(vertex[2]); - from->as()->setYaw(vertex[3]); - - ompl::base::State* to = dubins_ss->allocState(); - to->as()->setX(neighbor_vertex[0]); - to->as()->setY(neighbor_vertex[1]); - to->as()->setZ(neighbor_vertex[2]); - to->as()->setYaw(neighbor_vertex[3]); - if (dubins_ss->equalStates(from, to)) { - continue; - } - std::vector points; - for (double t = 0.0; t < 1.0; t += 0.02) { - dubins_ss->interpolate(from, to, t, state); - auto interpolated_state = - Eigen::Vector3d(state->as()->getX(), - state->as()->getY(), - state->as()->getZ()); - points.push_back(tf2::toMsg(interpolated_state)); - } - points.push_back(tf2::toMsg(Eigen::Vector3d(neighbor_vertex[0], neighbor_vertex[1], neighbor_vertex[2]))); - edge_marker.points = points; - edge_marker.action = visualization_msgs::Marker::ADD; - edge_marker.pose.orientation.w = 1.0; - edge_marker.pose.orientation.x = 0.0; - edge_marker.pose.orientation.y = 0.0; - edge_marker.pose.orientation.z = 0.0; - edge_marker.scale.x = 1.0; - edge_marker.scale.y = 1.0; - edge_marker.scale.z = 1.0; - edge_marker.color.a = 0.5; // Don't forget to set the alpha! - edge_marker.color.r = 1.0; - edge_marker.color.g = 1.0; - edge_marker.color.b = 0.0; - marker_array.markers.push_back(edge_marker); - } - } - } - pub.publish(marker_array); -} - -#endif diff --git a/terrain_planner_benchmark/CMakeLists.txt b/terrain_planner_benchmark/CMakeLists.txt index 7be76adc..4600bfc9 100644 --- a/terrain_planner_benchmark/CMakeLists.txt +++ b/terrain_planner_benchmark/CMakeLists.txt @@ -86,3 +86,21 @@ add_executable(test_rrt_circle_goal ) add_dependencies(test_rrt_circle_goal terrain_planner ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS} ${GDAL_LIBRARY}) target_link_libraries(test_rrt_circle_goal ${PROJECT_NAME} ${catkin_LIBRARIES} ${GDAL_LIBRARY} ${OpenCV_LIBRARIES} ${OMPL_LIBRARIES}) + +add_executable(test_rrt_node + src/test_rrt_node.cpp +) +add_dependencies(test_rrt_node ${PROJECT_NAME} ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS}) +target_link_libraries(test_rrt_node ${PROJECT_NAME} ${catkin_LIBRARIES} ${OpenCV_LIBRARIES} ${OMPL_LIBRARIES}) + +add_executable(test_ompl_dubins + src/test_ompl_dubins.cpp +) +add_dependencies(test_ompl_dubins ${PROJECT_NAME} ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS}) +target_link_libraries(test_ompl_dubins ${PROJECT_NAME} ${catkin_LIBRARIES} ${OpenCV_LIBRARIES} ${OMPL_LIBRARIES}) + +add_executable(test_ompl_dubins_to_circle + src/test_ompl_dubins_to_circle.cpp +) +add_dependencies(test_ompl_dubins_to_circle ${PROJECT_NAME} ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS}) +target_link_libraries(test_ompl_dubins_to_circle ${PROJECT_NAME} ${catkin_LIBRARIES} ${OpenCV_LIBRARIES} ${OMPL_LIBRARIES}) diff --git a/terrain_planner_benchmark/include/terrain_planner_benchmark/terrain_planner_benchmark.h b/terrain_planner_benchmark/include/terrain_planner_benchmark/terrain_planner_benchmark.h index 852cd0b8..4f9398b2 100644 --- a/terrain_planner_benchmark/include/terrain_planner_benchmark/terrain_planner_benchmark.h +++ b/terrain_planner_benchmark/include/terrain_planner_benchmark/terrain_planner_benchmark.h @@ -4,9 +4,9 @@ #include #include "terrain_navigation/data_logger.h" +#include "terrain_navigation_ros/visualization.h" #include "terrain_planner/common.h" #include "terrain_planner/terrain_ompl_rrt.h" -#include "terrain_planner/visualization.h" struct BenchmarkResult { int id; diff --git a/terrain_planner_benchmark/src/surface_visualization.cpp b/terrain_planner_benchmark/src/surface_visualization.cpp index 3a079a07..7a6c8bfd 100644 --- a/terrain_planner_benchmark/src/surface_visualization.cpp +++ b/terrain_planner_benchmark/src/surface_visualization.cpp @@ -38,7 +38,7 @@ * @author Jaeyoung Lim */ -#include "terrain_planner/visualization.h" +#include "terrain_navigation_ros/visualization.h" #include #include "terrain_navigation/data_logger.h" diff --git a/terrain_planner_benchmark/src/terrain_planner_benchmark_node.cpp b/terrain_planner_benchmark/src/terrain_planner_benchmark_node.cpp index 0381cf87..e2652de3 100644 --- a/terrain_planner_benchmark/src/terrain_planner_benchmark_node.cpp +++ b/terrain_planner_benchmark/src/terrain_planner_benchmark_node.cpp @@ -49,9 +49,9 @@ #include #include +#include "terrain_navigation_ros/visualization.h" #include "terrain_planner/common.h" #include "terrain_planner/terrain_ompl_rrt.h" -#include "terrain_planner/visualization.h" int main(int argc, char** argv) { ros::init(argc, argv, "ompl_rrt_planner"); diff --git a/terrain_planner_benchmark/src/test_ics.cpp b/terrain_planner_benchmark/src/test_ics.cpp index 131847b9..b031f710 100644 --- a/terrain_planner_benchmark/src/test_ics.cpp +++ b/terrain_planner_benchmark/src/test_ics.cpp @@ -38,7 +38,7 @@ * @author Jaeyoung Lim */ -#include "terrain_planner/visualization.h" +#include "terrain_navigation_ros/visualization.h" #include #include "terrain_navigation/data_logger.h" diff --git a/terrain_planner/src/test_ompl_dubins.cpp b/terrain_planner_benchmark/src/test_ompl_dubins.cpp similarity index 99% rename from terrain_planner/src/test_ompl_dubins.cpp rename to terrain_planner_benchmark/src/test_ompl_dubins.cpp index 8bc39e31..5426b8ef 100644 --- a/terrain_planner/src/test_ompl_dubins.cpp +++ b/terrain_planner_benchmark/src/test_ompl_dubins.cpp @@ -47,9 +47,9 @@ #include #include +#include "terrain_navigation_ros/visualization.h" #include "terrain_planner/common.h" #include "terrain_planner/terrain_ompl_rrt.h" -#include "terrain_planner/visualization.h" void getDubinsShortestPath(const Eigen::Vector3d start_pos, const double start_yaw, const Eigen::Vector3d goal_pos, const double goal_yaw, std::vector& path) { diff --git a/terrain_planner/src/test_ompl_dubins_to_circle.cpp b/terrain_planner_benchmark/src/test_ompl_dubins_to_circle.cpp similarity index 99% rename from terrain_planner/src/test_ompl_dubins_to_circle.cpp rename to terrain_planner_benchmark/src/test_ompl_dubins_to_circle.cpp index ad1cd2d2..1b6ec686 100644 --- a/terrain_planner/src/test_ompl_dubins_to_circle.cpp +++ b/terrain_planner_benchmark/src/test_ompl_dubins_to_circle.cpp @@ -46,9 +46,9 @@ #include #include +#include "terrain_navigation_ros/visualization.h" #include "terrain_planner/common.h" #include "terrain_planner/terrain_ompl_rrt.h" -#include "terrain_planner/visualization.h" double mod2pi(double x) { return x - 2 * M_PI * floor(x * (0.5 / M_PI)); } diff --git a/terrain_planner_benchmark/src/test_rrt_circle_goal.cpp b/terrain_planner_benchmark/src/test_rrt_circle_goal.cpp index 8c44691f..e84256b0 100644 --- a/terrain_planner_benchmark/src/test_rrt_circle_goal.cpp +++ b/terrain_planner_benchmark/src/test_rrt_circle_goal.cpp @@ -51,9 +51,9 @@ #include #include "terrain_navigation/data_logger.h" +#include "terrain_navigation_ros/visualization.h" #include "terrain_planner/common.h" #include "terrain_planner/terrain_ompl_rrt.h" -#include "terrain_planner/visualization.h" void publishCircleSetpoints(const ros::Publisher& pub, const Eigen::Vector3d& position, const double radius) { visualization_msgs::Marker marker; diff --git a/terrain_planner/src/test_rrt_node.cpp b/terrain_planner_benchmark/src/test_rrt_node.cpp similarity index 99% rename from terrain_planner/src/test_rrt_node.cpp rename to terrain_planner_benchmark/src/test_rrt_node.cpp index 511d4adc..8f8290c7 100644 --- a/terrain_planner/src/test_rrt_node.cpp +++ b/terrain_planner_benchmark/src/test_rrt_node.cpp @@ -48,9 +48,9 @@ #include #include +#include "terrain_navigation_ros/visualization.h" #include "terrain_planner/common.h" #include "terrain_planner/terrain_ompl_rrt.h" -#include "terrain_planner/visualization.h" void publishPathSegments(ros::Publisher& pub, Path& trajectory) { visualization_msgs::MarkerArray msg; diff --git a/terrain_planner_benchmark/src/test_rrt_replanning_node.cpp b/terrain_planner_benchmark/src/test_rrt_replanning_node.cpp index e7c98e34..aa246920 100644 --- a/terrain_planner_benchmark/src/test_rrt_replanning_node.cpp +++ b/terrain_planner_benchmark/src/test_rrt_replanning_node.cpp @@ -48,9 +48,9 @@ #include #include "terrain_navigation/data_logger.h" +#include "terrain_navigation_ros/visualization.h" #include "terrain_planner/common.h" #include "terrain_planner/terrain_ompl_rrt.h" -#include "terrain_planner/visualization.h" #include #include