Added a topic (RVIZ) and a service to get the full opendrive map as an occupancy grid.

CMakeLists.txt

@@ -14,6 +14,7 @@ find_package(catkin REQUIRED
     roscpp
     tf2_geometry_msgs
     tf2_ros
+    iri_adc_msgs
 )
 
 find_package(iriutils REQUIRED)
@@ -36,6 +37,7 @@ catkin_package(
     pluginlib
     roscpp
     tf2_ros
+    iri_adc_msgs
 )
 
 include_directories(

include/iri_opendrive_global_planner/opendrive_planner.h

@@ -47,6 +47,8 @@
 #include <iri_opendrive_global_planner/OpendriveGlobalPlannerConfig.h>
 #include <tf2_geometry_msgs/tf2_geometry_msgs.h>
 #include <tf2_ros/transform_listener.h>
+#include <ros/single_subscriber_publisher.h>
+#include <iri_adc_msgs/get_opendrive_map.h>
 
 #include "opendrive_road_map.h"
 
@@ -118,6 +120,13 @@ class OpendriveGlobalPlanner : public nav_core::BaseGlobalPlanner
     costmap_2d::Costmap2D* costmap_;
     std::string frame_id_,opendrive_frame_id_;
     ros::Publisher plan_pub_;
+    ros::Publisher opendrive_map_pub_;
+    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);
+    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;        

package.xml

@@ -24,6 +24,7 @@
   <depend>pluginlib</depend>
   <depend>roscpp</depend>
   <depend>tf2_ros</depend>
+  <depend>iri_adc_msgs</depend>
 
   <export>
       <nav_core plugin="${prefix}/opendrive_gp_plugin.xml" />

src/opendrive_planner.cpp

@@ -40,6 +40,7 @@
 #include <costmap_2d/cost_values.h>
 #include <costmap_2d/costmap_2d.h>
 #include <tf2/utils.h>
+#include <limits>
 
 #include "exceptions.h"
 
@@ -83,18 +84,21 @@ void OpendriveGlobalPlanner::initialize(std::string name, costmap_2d::Costmap2D*
 
     try{
       std::string opendrive_file;
-      double value;
+      double resolution,scale_factor,min_road_length;
       int cost_type;
 
-      private_nh.param("resolution", value,0.0);
-      this->roadmap.set_resolution(value);
-      private_nh.param("scale_factor", value,0.0);
-      this->roadmap.set_scale_factor(value);
-      private_nh.param("min_road_length", value,0.0);
-      this->roadmap.set_min_road_length(value);
+      private_nh.param("resolution", resolution,0.0);
+      this->roadmap.set_resolution(resolution);
+      private_nh.param("scale_factor", scale_factor,0.0);
+      this->roadmap.set_scale_factor(scale_factor);
+      private_nh.param("min_road_length", min_road_length,0.0);
+      this->roadmap.set_min_road_length(min_road_length);
       private_nh.param("opendrive_file", opendrive_file,std::string(""));
       if(opendrive_file!="")
+      {
         this->roadmap.load(opendrive_file);
+        this->create_opendrive_map(opendrive_file,resolution,scale_factor,min_road_length);
+      }
       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);
@@ -108,6 +112,10 @@ void OpendriveGlobalPlanner::initialize(std::string name, costmap_2d::Costmap2D*
     plan_pub_ = private_nh.advertise<nav_msgs::Path>("plan", 1);
     make_plan_srv_ = private_nh.advertiseService("make_plan", &OpendriveGlobalPlanner::makePlanService, this);
 
+    this->opendrive_map_pub_ = private_nh.advertise<nav_msgs::OccupancyGrid>("opendrive_map", 1,boost::bind(&OpendriveGlobalPlanner::map_connect_callback, this, _1));
+
+    this->opendrive_map_service=private_nh.advertiseService("get_opendrive_map", &OpendriveGlobalPlanner::get_opendrive_map,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);
@@ -117,20 +125,85 @@ void OpendriveGlobalPlanner::initialize(std::string name, costmap_2d::Costmap2D*
     ROS_WARN("This planner has already been initialized, you can't call it twice, doing nothing");
 }
 
+void OpendriveGlobalPlanner::map_connect_callback(const ros::SingleSubscriberPublisher &subs)
+{
+  subs.publish(this->full_path_);
+}
+
+void OpendriveGlobalPlanner::create_opendrive_map(std::string &filename,double resolution,double scale_factor,double min_road_length)
+{
+  std::vector<double> x,partial_x,y,partial_y;
+  double max_x=std::numeric_limits<double>::min(),min_x=std::numeric_limits<double>::max(),max_y=std::numeric_limits<double>::min(),min_y=std::numeric_limits<double>::max();
+  COpendriveRoad road;
+
+  road.load(filename);
+  road.set_resolution(resolution);
+  road.set_scale_factor(scale_factor);
+  road.set_min_road_length(min_road_length);
+  this->full_path_.header.frame_id = this->opendrive_frame_id_;
+  this->full_path_.header.stamp = ros::Time::now();
+  this->full_path_.info.map_load_time = ros::Time::now();
+  this->full_path_.info.resolution = resolution;
+  for(unsigned int i=0;i<road.get_num_nodes();i++)
+  {
+    const COpendriveRoadNode &node=road.get_node(i);
+    for(unsigned int j=0;j<node.get_num_links();j++)
+    {
+      const COpendriveLink &link=node.get_link(j);
+      link.get_trajectory(partial_x,partial_y);
+      x.insert(x.end(),partial_x.begin(),partial_x.end());
+      y.insert(y.end(),partial_y.begin(),partial_y.end());
+      for(unsigned int k=0;k<partial_x.size();k++)
+      {
+        if(partial_x[k]>max_x)
+          max_x=partial_x[k];
+        else if(partial_x[k]<min_x)
+          min_x=partial_x[k];
+        if(partial_y[k]>max_y)
+          max_y=partial_y[k];
+        else if(partial_y[k]<min_y)
+          min_y=partial_y[k];
+      }
+    }
+  }
+  this->full_path_.info.width=(max_x-min_x)/resolution;
+  this->full_path_.info.height=(max_y-min_y)/resolution;
+  this->full_path_.info.origin.position.x=min_x;
+  this->full_path_.info.origin.position.y=min_y;
+  this->full_path_.info.origin.position.z=0.0;
+  this->full_path_.info.origin.orientation.x=0.0;
+  this->full_path_.info.origin.orientation.y=0.0;
+  this->full_path_.info.origin.orientation.z=0.0;
+  this->full_path_.info.origin.orientation.w=1.0;
+  this->full_path_.data.resize(this->full_path_.info.width*this->full_path_.info.height,255);
+  for(unsigned int i=0;i<x.size();i++)
+    this->full_path_.data[this->full_path_.info.width*((unsigned int)((y[i]-min_y)/resolution))+((unsigned int)((x[i]-min_x)/resolution))]=0;
+}
+
+bool OpendriveGlobalPlanner::get_opendrive_map(iri_adc_msgs::get_opendrive_map::Request  &req,iri_adc_msgs::get_opendrive_map::Response &res)
+{
+  res.opendrive_map=this->full_path_;
+
+  return true;
+}
+
 void OpendriveGlobalPlanner::reconfigureCB(iri_opendrive_global_planner::OpendriveGlobalPlannerConfig& new_config, uint32_t level) 
 {
   try{
     this->roadmap.set_resolution(new_config.resolution);
     this->roadmap.set_scale_factor(new_config.scale_factor);
     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;
-    this->config=new_config;
+    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->create_opendrive_map(new_config.opendrive_file,new_config.resolution,new_config.scale_factor,new_config.min_road_length);
+    } 
     this->roadmap.set_cost_type((cost_t)new_config.cost_type);
+    this->config=new_config;
   }catch(CException &e){
     ROS_ERROR_STREAM(e.what());
   }