From: Jiri Vlasak <jiri.vlasak.2@cvut.cz>
Date: Fri, 3 Jun 2022 15:14:52 +0000 (+0200)
Subject: Use tf2 to consider start/goal headings
X-Git-Tag: v0.1.0^0
X-Git-Url: http://rtime.felk.cvut.cz/gitweb/hubacji1/wrrr.git/commitdiff_plain/9df389424092efb7ede2625c759580b2373e714b

Use tf2 to consider start/goal headings
---

diff --git a/CMakeLists.txt b/CMakeLists.txt
index 4765ae5..22cd7fb 100644
--- 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 422da53..1c0c56e 100644
--- 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 ceebe33..58cfd82 100644
--- 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);
 	}
 }