Added a service to get the opendrive nodes of the current path.

include/iri_opendrive_global_planner/opendrive_planner.h

@@ -49,6 +49,7 @@
 #include <tf2_ros/transform_listener.h>
 #include <ros/single_subscriber_publisher.h>
 #include <iri_adc_msgs/get_opendrive_map.h>
+#include <iri_adc_msgs/get_opendrive_nodes.h>
 
 #include "opendrive_road_map.h"
 
@@ -124,12 +125,16 @@ class OpendriveGlobalPlanner : public nav_core::BaseGlobalPlanner
     void map_connect_callback(const ros::SingleSubscriberPublisher &subs);
     ros::ServiceServer opendrive_map_service;
     bool get_opendrive_map(iri_adc_msgs::get_opendrive_map::Request  &req,iri_adc_msgs::get_opendrive_map::Response &res);
+    ros::ServiceServer opendrive_nodes_service;
+    bool get_opendrive_nodes(iri_adc_msgs::get_opendrive_nodes::Request  &req,iri_adc_msgs::get_opendrive_nodes::Response &res);
     void create_opendrive_map(std::string &filename,double resolution,double scale_factor,double min_road_length);
     nav_msgs::OccupancyGrid full_path_;
 
     bool initialized_;
 
     COpendriveRoadMap roadmap;        
+    std::vector<unsigned int> best_path;
+    ros::Time best_path_stamp;
     double angle_tol;
     double dist_tol;
     bool multi_hyp;

src/opendrive_planner.cpp

@@ -116,6 +116,8 @@ void OpendriveGlobalPlanner::initialize(std::string name, costmap_2d::Costmap2D*
 
     this->opendrive_map_service=private_nh.advertiseService("get_opendrive_map", &OpendriveGlobalPlanner::get_opendrive_map,this);
 
+    this->opendrive_nodes_service=private_nh.advertiseService("get_opendrive_nodes", &OpendriveGlobalPlanner::get_opendrive_nodes,this);
+
     dsrv_ = new dynamic_reconfigure::Server<iri_opendrive_global_planner::OpendriveGlobalPlannerConfig>(ros::NodeHandle("~/" + name));
     dynamic_reconfigure::Server<iri_opendrive_global_planner::OpendriveGlobalPlannerConfig>::CallbackType cb = boost::bind(&OpendriveGlobalPlanner::reconfigureCB, this, _1, _2);
     dsrv_->setCallback(cb);
@@ -187,6 +189,18 @@ bool OpendriveGlobalPlanner::get_opendrive_map(iri_adc_msgs::get_opendrive_map::
   return true;
 }
 
+bool OpendriveGlobalPlanner::get_opendrive_nodes(iri_adc_msgs::get_opendrive_nodes::Request  &req,iri_adc_msgs::get_opendrive_nodes::Response &res)
+{
+  res.opendrive_nodes.header.frame_id=this->opendrive_frame_id_;
+  res.opendrive_nodes.header.stamp=this->best_path_stamp;
+  res.opendrive_nodes.nodes.resize(this->best_path.size());
+  for(unsigned int i=0;i<this->best_path.size();i++)
+    res.opendrive_nodes.nodes[i]=this->best_path[i];
+  res.opendrive_nodes.nodes=this->best_path;
+
+  return true;
+}
+
 void OpendriveGlobalPlanner::reconfigureCB(iri_opendrive_global_planner::OpendriveGlobalPlannerConfig& new_config, uint32_t level) 
 {
   try{
@@ -263,7 +277,6 @@ bool OpendriveGlobalPlanner::makePlan(const geometry_msgs::PoseStamped& start, c
 {
   double yaw,best_cost;
   unsigned int best_path_index;
-  std::vector<unsigned int> best_path;
   std::vector<double> x,y,heading;
   std::vector< std::vector<unsigned int> > paths;
   std::vector<double> costs;
@@ -354,13 +367,14 @@ bool OpendriveGlobalPlanner::makePlan(const geometry_msgs::PoseStamped& start, c
         std::cout << paths[i][j] << ",";
       std::cout << std::endl;
     }
-    best_path_index=this->roadmap.get_best_path(best_cost,best_path);
+    best_path_index=this->roadmap.get_best_path(best_cost,this->best_path);
     if(best_path_index==-1)
       return false;
     std::cout << "best path index: " << best_path_index << std::endl;
-    for(unsigned int i=0;i<best_path.size();i++)
-      std::cout << best_path[i] << ",";
+    for(unsigned int i=0;i<this->best_path.size();i++)
+      std::cout << this->best_path[i] << ",";
     std::cout << std::endl;
+    this->best_path_stamp=ros::Time::now();
     this->roadmap.get_trajectory(best_path_index,x,y,heading);
     plan.resize(x.size());
     try{