Commit 8e08bd5c authored by Sergi Hernandez's avatar Sergi Hernandez
Browse files

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

parent 9aca1907
......@@ -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(
......
......@@ -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;
......
......@@ -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" />
......
......@@ -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());
}
......
Markdown is supported
0% or .
You are about to add 0 people to the discussion. Proceed with caution.
Finish editing this message first!
Please register or to comment