WIP Port terrain_planner_benchmarks

WIP

Author: Jaeyoung-Lim

terrain_planner_benchmark/CMakeLists.txt

@@ -29,13 +29,19 @@ find_package(grid_map_geo REQUIRED)
 find_package(terrain_navigation REQUIRED)
 find_package(Boost REQUIRED) # Workaround for terrain_planner not exporting this correctly
 find_package(terrain_planner REQUIRED)
+find_package(geometry_msgs REQUIRED)
 
 add_library(${PROJECT_NAME})
 add_library(${PROJECT_NAME}::${PROJECT_NAME} ALIAS ${PROJECT_NAME})
 
 target_include_directories(${PROJECT_NAME}
   PUBLIC 
     "$<BUILD_INTERFACE:${CMAKE_CURRENT_SOURCE_DIR}/include>"
+    "$<INSTALL_INTERFACE:include/${PROJECT_NAME}>"
+)
+
+target_link_libraries(${PROJECT_NAME} PUBLIC
+  ${geometry_msgs_TARGETS}
 )
 
 add_executable(terrain_planner_benchmark_node)
@@ -45,9 +51,13 @@ add_executable(test_rrt_replanning_node EXCLUDE_FROM_ALL)
 add_executable(surface_visualization EXCLUDE_FROM_ALL)
 add_executable(test_dubins_classification EXCLUDE_FROM_ALL)
 add_executable(run_replay EXCLUDE_FROM_ALL)
-add_executable(test_rrt_circle_goal EXCLUDE_FROM_ALL)
+add_executable(test_rrt_circle_goal)
 add_subdirectory(src)
 
+target_link_libraries(test_rrt_circle_goal PUBLIC
+  ${PROJECT_NAME}
+)
+
 include(GNUInstallDirs)
 install(
   TARGETS

terrain_planner_benchmark/launch/test_ompl_rrt_circle.launch

@@ -0,0 +1,22 @@
+<?xml version="1.0" encoding="UTF-8"?>
+<launch>
+  <!-- <node pkg="turtlesim" exec="turtlesim_node" name="sim" namespace="turtlesim1" />
+  <node pkg="turtlesim" exec="turtlesim_node" name="sim" namespace="turtlesim2" />
+  <node pkg="turtlesim" exec="mimic" name="mimic">
+    <remap from="/input/pose" to="/turtlesim1/turtle1/pose" />
+    <remap from="/output/cmd_vel" to="/turtlesim2/turtle1/cmd_vel" />
+  </node> -->
+  <node pkg="terrain_planner_benchmark" exec="test_rrt_circle_goal" name="rrt_planner">
+  </node>
+</launch>
+<!-- <?xml version="1.0" encoding="UTF-8"?>
+<launch>
+    <arg name="visualization" default="true"/>
+    <arg name="location" default="gotthard"/>
+
+    <node pkg="tf" type="static_transform_publisher" name="world_map" args="0 0 0 0 0 0 world map 10"/>
+
+    <group if="$(arg visualization)">
+        <node type="rviz" name="rviz" pkg="rviz" args="-d $(find terrain_planner)/launch/config_ompl.rviz" />
+    </group>
+</launch> -->

terrain_planner_benchmark/launch/test_ompl_rrt_circle_launch.xml

@@ -25,6 +25,7 @@
   <build_depend>terrain_navigation</build_depend>
   <build_depend>ompl</build_depend>
   <build_depend>terrain_planner</build_depend>
+  <build_depend>geometry_msgs</build_depend>
   <exec_depend>python3-gdal</exec_depend>
 
   <export>

terrain_planner_benchmark/package.xml

@@ -38,13 +38,13 @@
  * @author Jaeyoung Lim <[email protected]>
  */
 
-#include <geometry_msgs/Point.h>
-#include <ros/ros.h>
+#include <geometry_msgs/msg/point.hpp>
+#include <rclcpp/rclcpp.hpp>
 #include <terrain_navigation/terrain_map.h>
 #include <tf2/LinearMath/Quaternion.h>
 #include <tf2_eigen/tf2_eigen.h>
 #include <tf2_geometry_msgs/tf2_geometry_msgs.h>
-#include <visualization_msgs/MarkerArray.h>
+#include <visualization_msgs/msg/marker.hpp>
 
 #include <any>
 #include <grid_map_ros/GridMapRosConverter.hpp>
@@ -54,23 +54,24 @@
 #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) {
+void publishCircleSetpoints(rclcpp::Publisher<visualization_msgs::msg::Marker>::SharedPtr pub, 
+  const Eigen::Vector3d& position, const double radius) {
   visualization_msgs::msg::Marker marker;
   marker.header.stamp = rclcpp::Clock().now();
   marker.type = visualization_msgs::msg::Marker::LINE_STRIP;
   marker.action = visualization_msgs::msg::Marker::ADD;
   marker.header.frame_id = "map";
   marker.id = 0;
   marker.header.stamp = rclcpp::Clock().now();
-  std::vector<geometry_msgs::Point> points;
+  std::vector<geometry_msgs::msg::Point> points;
   for (double t = 0.0; t <= 1.0; t += 0.02) {
-    geometry_msgs::Point point;
+    geometry_msgs::msg::Point point;
     point.x = position.x() + radius * std::cos(t * 2 * M_PI);
     point.y = position.y() + radius * std::sin(t * 2 * M_PI);
     point.z = position.z();
     points.push_back(point);
   }
-  geometry_msgs::Point start_point;
+  geometry_msgs::msg::Point start_point;
   start_point.x = position.x() + radius * std::cos(0.0);
   start_point.y = position.y() + radius * std::sin(0.0);
   start_point.z = position.z();
@@ -88,7 +89,7 @@ void publishCircleSetpoints(const ros::Publisher& pub, const Eigen::Vector3d& po
   marker.pose.orientation.x = 0.0;
   marker.pose.orientation.y = 0.0;
   marker.pose.orientation.z = 0.0;
-  pub.publish(marker);
+  pub->publish(marker);
 }
 
 void getDubinsShortestPath(std::shared_ptr<fw_planning::spaces::DubinsAirplaneStateSpace>& dubins_ss,
@@ -143,9 +144,7 @@ PathSegment getLoiterPath(Eigen::Vector3d end_position, Eigen::Vector3d end_velo
 }
 
 int main(int argc, char** argv) {
-  ros::init(argc, argv, "ompl_rrt_planner");
-  ros::NodeHandle nh("");
-  ros::NodeHandle nh_private("~");
+  rclcpp::init(argc, argv);
 
   // Initialize ROS related publishers for visualization
   auto start_pos_pub = nh.advertise<visualization_msgs::msg::Marker>("start_position", 1, true);
@@ -248,7 +247,7 @@ int main(int argc, char** argv) {
   std::string output_file_path = output_directory + "/" + location + "_planned_path.csv";
   data_logger->writeToFile(output_file_path);
 
-  ros::Duration(1.0).sleep();
-  ros::spin();
+  rclcpp::spin(ompl_rrt_planner);
+  rclcpp::shutdown();
   return 0;
 }