Added a TF listener to transform the input and output data to and from the opendrive frame.

cfg/OpendriveGlobalPlanner.cfg

@@ -11,13 +11,14 @@ gen.const("time",  int_t, 1, "Use distance and speed to compute costs"),
 ], "Possible costs types.")
 
 
-gen.add("opendrive_file", str_t, 0, "Opendrive map file", "")
-gen.add("angle_tol", double_t, 0, "Angle tolerance to find start and end positions on the road map", 0.5, 0.1, 1.5707)
-gen.add("dist_tol", double_t, 0, "Distance tolerance to find start and end positions on the road map", 3.0, 0.5, 10.0)
-gen.add("multi_hyp", bool_t, 0, "Use multi hypothesis path search", False)
-gen.add("resolution", double_t, 0, "Resolution of the generated path", 0.1, 0.01, 1.0)
-gen.add("scale_factor", double_t, 0, "Scale factor of the input road description", 1.0, 0.01, 10.0)
+gen.add("opendrive_file",  str_t,    0, "Opendrive map file", "")
+gen.add("opendrive_frame", str_t,    0, "Opendrive frame ID", "")
+gen.add("angle_tol",       double_t, 0, "Angle tolerance to find start and end positions on the road map", 0.5, 0.1, 1.5707)
+gen.add("dist_tol",        double_t, 0, "Distance tolerance to find start and end positions on the road map", 3.0, 0.5, 10.0)
+gen.add("multi_hyp",       bool_t,   0, "Use multi hypothesis path search", False)
+gen.add("resolution",      double_t, 0, "Resolution of the generated path", 0.1, 0.01, 1.0)
+gen.add("scale_factor",    double_t, 0, "Scale factor of the input road description", 1.0, 0.01, 10.0)
 gen.add("min_road_length", double_t, 0, "Minimum road length to take it into account", 0.1, 0.01, 1.0)
-gen.add("cost_type", int_t, 0, "Cost type", 0, 0, 1, edit_method=enum_cost_type)
+gen.add("cost_type",       int_t,    0, "Cost type", 0, 0, 1, edit_method=enum_cost_type)
 
 exit(gen.generate(PACKAGE, "iri_opendrive_global_planner", "OpendriveGlobalPlanner"))

include/iri_opendrive_global_planner/opendrive_planner.h

@@ -45,6 +45,8 @@
 #include <nav_msgs/GetPlan.h>
 #include <dynamic_reconfigure/server.h>
 #include <iri_opendrive_global_planner/OpendriveGlobalPlannerConfig.h>
+#include <tf2_geometry_msgs/tf2_geometry_msgs.h>
+#include <tf2_ros/transform_listener.h>
 
 #include "opendrive_road_map.h"
 
@@ -114,7 +116,7 @@ class OpendriveGlobalPlanner : public nav_core::BaseGlobalPlanner
      * @brief Store a copy of the current costmap in \a costmap.  Called by makePlan.
      */
     costmap_2d::Costmap2D* costmap_;
-    std::string frame_id_;
+    std::string frame_id_,opendrive_frame_id_;
     ros::Publisher plan_pub_;
     bool initialized_;
 
@@ -135,6 +137,8 @@ class OpendriveGlobalPlanner : public nav_core::BaseGlobalPlanner
     void reconfigureCB(iri_opendrive_global_planner::OpendriveGlobalPlannerConfig &new_config, uint32_t level);
     iri_opendrive_global_planner::OpendriveGlobalPlannerConfig config;
 
+    tf2_ros::Buffer tf2_buffer;
+    tf2_ros::TransformListener tf2_listener;
 };
 
 } //end namespace iri_opendrive_global_planner

src/opendrive_planner.cpp

@@ -49,7 +49,7 @@ PLUGINLIB_EXPORT_CLASS(iri_opendrive_global_planner::OpendriveGlobalPlanner, nav
 namespace iri_opendrive_global_planner {
 
 OpendriveGlobalPlanner::OpendriveGlobalPlanner() :
-  costmap_(NULL), initialized_(false) {
+  costmap_(NULL), initialized_(false), tf2_listener(tf2_buffer) {
   this->angle_tol=DEFAULT_ANGLE_THRESHOLD;
   this->dist_tol=DEFAULT_DIST_THRESHOLD;
   this->multi_hyp=false;
@@ -75,8 +75,6 @@ void OpendriveGlobalPlanner::initialize(std::string name, costmap_2d::Costmap2DR
 
 void OpendriveGlobalPlanner::initialize(std::string name, costmap_2d::Costmap2D* costmap, std::string frame_id) 
 {
-  double value;
-
   if (!initialized_) 
   {
     ros::NodeHandle private_nh("~/" + name);
@@ -85,6 +83,9 @@ void OpendriveGlobalPlanner::initialize(std::string name, costmap_2d::Costmap2D*
 
     try{
       std::string opendrive_file;
+      double value;
+      int cost_type;
+
       private_nh.param("resolution", value,0.0);
       this->roadmap.set_resolution(value);
       private_nh.param("scale_factor", value,0.0);
@@ -94,11 +95,10 @@ void OpendriveGlobalPlanner::initialize(std::string name, costmap_2d::Costmap2D*
       private_nh.param("opendrive_file", opendrive_file,std::string(""));
       if(opendrive_file!="")
         this->roadmap.load(opendrive_file);
+      private_nh.param("opendrive_frame", this->opendrive_frame_id_,std::string(""));
       private_nh.param("angle_tol", this->angle_tol,DEFAULT_ANGLE_THRESHOLD);
       private_nh.param("dist_tol", this->dist_tol,DEFAULT_DIST_THRESHOLD);
       private_nh.param("multi_hyp", this->multi_hyp,false);
-      std::cout << "multi hypothessis: " << this->multi_hyp << std::endl;
-      int cost_type;
       private_nh.param("cost_type",cost_type,(int)COST_DIST);
       this->roadmap.set_cost_type((cost_t)cost_type);
     }catch(CException &e){
@@ -125,6 +125,7 @@ void OpendriveGlobalPlanner::reconfigureCB(iri_opendrive_global_planner::Opendri
     this->roadmap.set_min_road_length(new_config.min_road_length);
     if(new_config.opendrive_file!=this->config.opendrive_file || new_config.scale_factor!=this->config.scale_factor || new_config.min_road_length!=this->config.min_road_length)
       this->roadmap.load(new_config.opendrive_file);
+    this->opendrive_frame_id_=new_config.opendrive_frame;
     this->angle_tol=new_config.angle_tol;
     this->dist_tol=new_config.dist_tol;
     this->multi_hyp=new_config.multi_hyp;
@@ -194,6 +195,12 @@ bool OpendriveGlobalPlanner::makePlan(const geometry_msgs::PoseStamped& start, c
   std::vector< std::vector<unsigned int> > paths;
   std::vector<double> costs;
   std::vector<TMapPose> start_candidates,end_candidates;
+  geometry_msgs::PoseStamped start_out,goal_out,point;
+  std::string target_frame;
+  std::string source_frame;
+  ros::Time time;
+  ros::Duration timeout;
+  geometry_msgs::TransformStamped transform;
 
   boost::mutex::scoped_lock lock(mutex_);
   if (!initialized_) {
@@ -220,45 +227,49 @@ bool OpendriveGlobalPlanner::makePlan(const geometry_msgs::PoseStamped& start, c
     return false;
   }
 
-/*
-  double wx = start.pose.position.x;
-  double wy = start.pose.position.y;
-
-  unsigned int start_x_i, start_y_i, goal_x_i, goal_y_i;
-  double start_x, start_y, goal_x, goal_y;
-
-  if (!costmap_->worldToMap(wx, wy, start_x_i, start_y_i)) 
-  {
-    ROS_WARN("The robot's start position is off the global costmap. Planning will always fail, are you sure the robot has been properly localized?");
-    return false;
+  /* transform start and end positions to map opendrive */
+  try{
+    target_frame = this->opendrive_frame_id_;
+    source_frame = start.header.frame_id;
+    time = ros::Time(0);
+    timeout = ros::Duration(0.1);
+    if(this->tf2_buffer.canTransform(target_frame, source_frame, time, timeout))
+    {
+      transform = this->tf2_buffer.lookupTransform(target_frame, source_frame, time);
+      tf2::doTransform(start,start_out, transform);
+    }else{
+      ROS_WARN("No transform found for start point from '%s' to '%s'", source_frame.c_str(), target_frame.c_str());
+    }
+    source_frame = goal.header.frame_id;
+    if(this->tf2_buffer.canTransform(target_frame, source_frame, time, timeout))
+    {
+      transform = this->tf2_buffer.lookupTransform(target_frame, source_frame, time);
+      tf2::doTransform(goal,goal_out, transform);
+    }else{
+      ROS_WARN("No transform found for end point from '%s' to '%s'", source_frame.c_str(), target_frame.c_str());
+    }
+  }catch (tf2::TransformException &ex){
+    ROS_ERROR("TF2 Exception: %s",ex.what()); 
   }
-  worldToMap(wx, wy, start_x, start_y);
 
-  //clear the starting cell within the costmap because we know it can't be an obstacle
-  clearRobotCell(start, start_x_i, start_y_i);
-
-  int nx = costmap_->getSizeInCellsX(), ny = costmap_->getSizeInCellsY();
-*/
   try{
     ROS_WARN("Make Plan");
-    yaw=tf2::getYaw(start.pose.orientation);
-    this->roadmap.set_start_pose(start.pose.position.x,start.pose.position.y,yaw,this->dist_tol,this->angle_tol);
+    yaw=tf2::getYaw(start_out.pose.orientation);
+    this->roadmap.set_start_pose(start_out.pose.position.x,start_out.pose.position.y,yaw,this->dist_tol,this->angle_tol);
     this->roadmap.get_start_pose_candidates(start_candidates);
     std::cout << "Start pose candidates: " << std::endl;
     for(unsigned int i=0;i<start_candidates.size();i++)
     {
       std::cout << i << ". x: " << start_candidates[i].pose.x << ", y:" << start_candidates[i].pose.y << ", heading: " << start_candidates[i].pose.heading << std::endl;
-      //std::cout << "   road map node: " << start_candidates[i].node_index << " child: " << start_candidates[i].child_index << std::endl;
       std::cout << "   distance from desired pose: " << start_candidates[i].dist << std::endl;
     }
-    yaw=tf2::getYaw(goal.pose.orientation);
-    this->roadmap.set_end_pose(goal.pose.position.x,goal.pose.position.y,yaw,this->dist_tol,this->angle_tol);
+    yaw=tf2::getYaw(goal_out.pose.orientation);
+    this->roadmap.set_end_pose(goal_out.pose.position.x,goal_out.pose.position.y,yaw,this->dist_tol,this->angle_tol);
     this->roadmap.get_end_pose_candidates(end_candidates);
     std::cout << "End pose candidates: " << std::endl;
     for(unsigned int i=0;i<end_candidates.size();i++)
     {
       std::cout << i << ". x: " << end_candidates[i].pose.x << ", y:" << end_candidates[i].pose.y << ", heading: " << end_candidates[i].pose.heading << std::endl;
-      //std::cout << "   road map node: " << end_candidates[i].node_index << " child: " << end_candidates[i].child_index << std::endl;
       std::cout << "   distance from desired pose: " << end_candidates[i].dist << std::endl;
     }
     this->roadmap.find_shortest_path(this->multi_hyp);
@@ -279,18 +290,35 @@ bool OpendriveGlobalPlanner::makePlan(const geometry_msgs::PoseStamped& start, c
     std::cout << std::endl;
     this->roadmap.get_trajectory(best_path_index,x,y,heading);
     plan.resize(x.size());
-    ros::Time plan_time=ros::Time::now();
-    for(unsigned int i=0;i<x.size();i++)
-    {
-      plan[i].header.frame_id=frame_id_;
-      plan[i].header.stamp=plan_time;
-      plan[i].pose.position.x=x[i];
-      plan[i].pose.position.y=y[i];
-      plan[i].pose.position.y=y[i];
-      tf2::Quaternion orientation;
-      orientation.setRPY(0.0,0.0,heading[i]);
-      plan[i].pose.orientation=tf2::toMsg(orientation);
+    try{
+      target_frame = this->frame_id_;
+      source_frame = this->opendrive_frame_id_;
+      time = ros::Time(0);
+      timeout = ros::Duration(0.1);
+      if(this->tf2_buffer.canTransform(target_frame, source_frame, time, timeout))
+      {
+        transform = this->tf2_buffer.lookupTransform(target_frame, source_frame, time);
+        point.header.frame_id=this->opendrive_frame_id_;
+        point.header.stamp=time;
+        for(unsigned int i=0;i<x.size();i++)
+        {
+          point.pose.position.x=x[i];
+          point.pose.position.y=y[i];
+          point.pose.position.z=0.0;
+          tf2::Quaternion orientation;
+          orientation.setRPY(0.0,0.0,heading[i]);
+          point.pose.orientation=tf2::toMsg(orientation);
+          tf2::doTransform(point,plan[i],transform);
+          plan[i].header.frame_id=this->opendrive_frame_id_;
+          plan[i].header.stamp=time;
+        }
+      }else{
+        ROS_WARN("No transform found for path points from '%s' to '%s'", source_frame.c_str(), target_frame.c_str());
+      }
+    }catch (tf2::TransformException &ex){
+      ROS_ERROR("TF2 Exception: %s",ex.what()); 
     }
+
   }catch(CException &e){
     ROS_ERROR_STREAM(e.what());
     return false;