Use tf2 to consider start/goal headings

diff --git a/CMakeLists.txt b/CMakeLists.txt
index 4765ae56397631b010b3f11669af951b86d79dca..22cd7fbe794c4801fcdf2c0ab1d6d23e35dba429 100644 (file)
--- a/CMakeLists.txt
+++ b/CMakeLists.txt
@@ -9,12 +9,18 @@ endif()
 find_package(ament_cmake REQUIRED)
 find_package(rclcpp REQUIRED)
 find_package(nav_msgs REQUIRED)
+find_package(tf2_geometry_msgs REQUIRED)
 
 if (NOT TARGET rrts)
   add_subdirectory(rrts)
 endif()
 
 include_directories(include)
+# FIXME the following is the workaround, see
+# https://answers.ros.org/question/344496/cannot-include-tf2_geometry_msgsh/
+include_directories(/opt/ros/rolling/include/tf2_geometry_msgs)
+include_directories(/opt/ros/rolling/include/tf2)
+include_directories(/opt/ros/rolling/include/tf2_ros)
 
 add_executable(rrts_planner src/rrts_wr.cc)
 target_link_libraries(rrts_planner rrts)

diff --git a/package.xml b/package.xml
index 422da531247c8de949abf8f4b71e439fce190c1e..1c0c56ee9d5ccfdd1a3da640ce5fa0f33b8e58a7 100644 (file)
--- a/package.xml
+++ b/package.xml
@@ -12,6 +12,7 @@
   <buildtool_depend>ament_cmake</buildtool_depend>
   <buildtool_depend>rclcpp</buildtool_depend>
   <buildtool_depend>nav_msgs</buildtool_depend>
+  <buildtool_depend>tf2_geometry_msgs</buildtool_depend>
 
   <export>
     <build_type>ament_cmake</build_type>

diff --git a/src/rrts_wr.cc b/src/rrts_wr.cc
index ceebe337ee4c10502cc40e9497685c0287fe3c04..58cfd823fd236978643a31c288a3cf45354c0266 100644 (file)
--- a/src/rrts_wr.cc
+++ b/src/rrts_wr.cc
@@ -1,5 +1,6 @@
-#include "rclcpp/rclcpp.hpp"
 #include "nav_msgs/srv/get_plan.hpp"
+#include "rclcpp/rclcpp.hpp"
+#include "tf2_geometry_msgs/tf2_geometry_msgs.hpp"
 #include "rrtsp.hh"
 
 void
@@ -9,12 +10,17 @@ plan_path(const std::shared_ptr<nav_msgs::srv::GetPlan::Request> req,
        rrts::P39 p;
        double sx = req->start.pose.position.x;
        double sy = req->start.pose.position.y;
-       double sh = 0.0;  // FIXME
+       tf2::Quaternion qh;
+       tf2::fromMsg(req->start.pose.orientation, qh);
+       qh.normalize();
+       double sh = qh.getAngle();
        p.set_start(sx, sy, sh);
        double gx = req->goal.pose.position.x;
        double gy = req->goal.pose.position.y;
-       double gb = 0.0;  // FIXME
-       double ge = 0.0;  // FIXME
+       tf2::fromMsg(req->goal.pose.orientation, qh);
+       qh.normalize();
+       double gb = qh.getAngle();
+       double ge = gb + std::abs(req->tolerance);
        p.set_goal(gx, gy, gb, ge);
        // TODO make 1000 settable over ROS (max iters before reset)
        p.set_imax_reset(1000);
@@ -53,11 +59,9 @@ plan_path(const std::shared_ptr<nav_msgs::srv::GetPlan::Request> req,
                p.pose.position.x = pp.x();
                p.pose.position.y = pp.y();
                p.pose.position.z = 0.0;
-               // TODO use pp.h() to compute orientation
-               p.pose.orientation.x = 0.0;
-               p.pose.orientation.y = 0.0;
-               p.pose.orientation.z = 0.0;
-               p.pose.orientation.w = 0.0;
+               tf2::Quaternion qh;
+               qh.setRPY(0, 0, pp.h());
+               p.pose.orientation = tf2::toMsg(qh);
                res->plan.poses.push_back(p);
        }
 }