diff --git a/CMakeLists.txt b/CMakeLists.txt
index 5a1daf46de89fc5941dbd0bc230c0bee7b6f7425..9869c49d77241583bbd65b7039bf339b769255dd 100644
--- a/CMakeLists.txt
+++ b/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(
diff --git a/include/iri_opendrive_global_planner/opendrive_planner.h b/include/iri_opendrive_global_planner/opendrive_planner.h
index 32ec424973036b99ed67a90d7788dbf922fac7ba..3387145745d9f221fa6807eff812e3d7f599a748 100644
--- a/include/iri_opendrive_global_planner/opendrive_planner.h
+++ b/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;
diff --git a/package.xml b/package.xml
index e763248fd305451cd295c2854fc08116e5ed7c4d..455dd4f5be534f476a7bf08a259ce3cd9ba340c6 100644
--- a/package.xml
+++ b/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" />
diff --git a/src/opendrive_planner.cpp b/src/opendrive_planner.cpp
index 612d1de271e654c756e700bec72630e99fc12d05..5f8df05c759b8684d768f34f1972529243b67bf4 100644
--- a/src/opendrive_planner.cpp
+++ b/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());
}