The returned path now includes the heading for each point.

src/opendrive_planner.cpp

@@ -39,7 +39,7 @@
 #include <pluginlib/class_list_macros.h>
 #include <costmap_2d/cost_values.h>
 #include <costmap_2d/costmap_2d.h>
-#include <tf/transform_datatypes.h>
+#include <tf2/utils.h>
 
 #include "exceptions.h"
 
@@ -162,9 +162,10 @@ bool OpendriveGlobalPlanner::makePlan(const geometry_msgs::PoseStamped& start, c
 
 bool OpendriveGlobalPlanner::makePlan(const geometry_msgs::PoseStamped& start, const geometry_msgs::PoseStamped& goal,double tolerance, std::vector<geometry_msgs::PoseStamped>& plan) 
 {
-  double roll, pitch, yaw;
+  double yaw;
   std::vector<unsigned int> path;
-  std::vector<double> x,y;
+  std::vector<double> x,y,heading;
+  TPoint real_goal;
 
   boost::mutex::scoped_lock lock(mutex_);
   if (!initialized_) {
@@ -212,22 +213,16 @@ bool OpendriveGlobalPlanner::makePlan(const geometry_msgs::PoseStamped& start, c
 */
   try{
     ROS_WARN("Make Plan");
-    tf::Quaternion q_start(start.pose.orientation.x,start.pose.orientation.y,start.pose.orientation.z,start.pose.orientation.w);
-    tf::Matrix3x3 m_start(q_start);
-    m_start.getRPY(roll, pitch, yaw);
-    ROS_WARN_STREAM("start position: x: " << start.pose.position.x << ", y: " << start.pose.position.y << ", yaw: " << yaw);
+    yaw=tf2::getYaw(start.pose.orientation);
     this->roadmap.set_start_point(start.pose.position.x,start.pose.position.y,yaw);
-    tf::Quaternion q_goal(goal.pose.orientation.x,goal.pose.orientation.y,goal.pose.orientation.z,goal.pose.orientation.w);
-    tf::Matrix3x3 m_goal(q_goal);
-    m_goal.getRPY(roll, pitch, yaw);
-    ROS_WARN_STREAM("start position: x: " << goal.pose.position.x << ", y: " << goal.pose.position.y << ", yaw: " << yaw);
-    this->roadmap.set_target_point(goal.pose.position.x,goal.pose.position.y,yaw);
+    yaw=tf2::getYaw(goal.pose.orientation);
+    real_goal=this->roadmap.set_target_point(goal.pose.position.x,goal.pose.position.y,yaw);
     this->roadmap.find_shortest_path();
     this->roadmap.get_path(path);
-    for(unsigned int i=0;i<path.size();i++)
-      std::cout << path[i] << ",";
-    std::cout << std::endl;
-    this->roadmap.get_trajectory(x,y);
+//    for(unsigned int i=0;i<path.size();i++)
+//      std::cout << path[i] << ",";
+//    std::cout << std::endl;
+    this->roadmap.get_trajectory(x,y,heading);
     plan.resize(x.size());
     ros::Time plan_time=ros::Time::now();
     for(unsigned int i=0;i<x.size();i++)
@@ -237,10 +232,9 @@ bool OpendriveGlobalPlanner::makePlan(const geometry_msgs::PoseStamped& start, c
       plan[i].pose.position.x=x[i];
       plan[i].pose.position.y=y[i];
       plan[i].pose.position.y=y[i];
-      plan[i].pose.orientation.x=0.0;
-      plan[i].pose.orientation.y=0.0;
-      plan[i].pose.orientation.z=0.0;
-      plan[i].pose.orientation.w=1.0;
+      tf2::Quaternion orientation;
+      orientation.setRPY(0.0,0.0,heading[i]);
+      plan[i].pose.orientation=tf2::toMsg(orientation);
     }
   }catch(CException &e){
     ROS_ERROR_STREAM(e.what());