Commit 6949e939 authored by Sergi Hernandez's avatar Sergi Hernandez
Browse files

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

parent 192b9ec5
...@@ -49,6 +49,7 @@ ...@@ -49,6 +49,7 @@
#include <tf2_ros/transform_listener.h> #include <tf2_ros/transform_listener.h>
#include <ros/single_subscriber_publisher.h> #include <ros/single_subscriber_publisher.h>
#include <iri_adc_msgs/get_opendrive_map.h> #include <iri_adc_msgs/get_opendrive_map.h>
#include <iri_adc_msgs/get_opendrive_nodes.h>
#include "opendrive_road_map.h" #include "opendrive_road_map.h"
...@@ -124,12 +125,16 @@ class OpendriveGlobalPlanner : public nav_core::BaseGlobalPlanner ...@@ -124,12 +125,16 @@ class OpendriveGlobalPlanner : public nav_core::BaseGlobalPlanner
void map_connect_callback(const ros::SingleSubscriberPublisher &subs); void map_connect_callback(const ros::SingleSubscriberPublisher &subs);
ros::ServiceServer opendrive_map_service; 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); 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); void create_opendrive_map(std::string &filename,double resolution,double scale_factor,double min_road_length);
nav_msgs::OccupancyGrid full_path_; nav_msgs::OccupancyGrid full_path_;
bool initialized_; bool initialized_;
COpendriveRoadMap roadmap; COpendriveRoadMap roadmap;
std::vector<unsigned int> best_path;
ros::Time best_path_stamp;
double angle_tol; double angle_tol;
double dist_tol; double dist_tol;
bool multi_hyp; bool multi_hyp;
......
...@@ -116,6 +116,8 @@ void OpendriveGlobalPlanner::initialize(std::string name, costmap_2d::Costmap2D* ...@@ -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_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)); 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); dynamic_reconfigure::Server<iri_opendrive_global_planner::OpendriveGlobalPlannerConfig>::CallbackType cb = boost::bind(&OpendriveGlobalPlanner::reconfigureCB, this, _1, _2);
dsrv_->setCallback(cb); dsrv_->setCallback(cb);
...@@ -187,6 +189,18 @@ bool OpendriveGlobalPlanner::get_opendrive_map(iri_adc_msgs::get_opendrive_map:: ...@@ -187,6 +189,18 @@ bool OpendriveGlobalPlanner::get_opendrive_map(iri_adc_msgs::get_opendrive_map::
return true; 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) void OpendriveGlobalPlanner::reconfigureCB(iri_opendrive_global_planner::OpendriveGlobalPlannerConfig& new_config, uint32_t level)
{ {
try{ try{
...@@ -263,7 +277,6 @@ bool OpendriveGlobalPlanner::makePlan(const geometry_msgs::PoseStamped& start, c ...@@ -263,7 +277,6 @@ bool OpendriveGlobalPlanner::makePlan(const geometry_msgs::PoseStamped& start, c
{ {
double yaw,best_cost; double yaw,best_cost;
unsigned int best_path_index; unsigned int best_path_index;
std::vector<unsigned int> best_path;
std::vector<double> x,y,heading; std::vector<double> x,y,heading;
std::vector< std::vector<unsigned int> > paths; std::vector< std::vector<unsigned int> > paths;
std::vector<double> costs; std::vector<double> costs;
...@@ -354,13 +367,14 @@ bool OpendriveGlobalPlanner::makePlan(const geometry_msgs::PoseStamped& start, c ...@@ -354,13 +367,14 @@ bool OpendriveGlobalPlanner::makePlan(const geometry_msgs::PoseStamped& start, c
std::cout << paths[i][j] << ","; std::cout << paths[i][j] << ",";
std::cout << std::endl; 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) if(best_path_index==-1)
return false; return false;
std::cout << "best path index: " << best_path_index << std::endl; std::cout << "best path index: " << best_path_index << std::endl;
for(unsigned int i=0;i<best_path.size();i++) for(unsigned int i=0;i<this->best_path.size();i++)
std::cout << best_path[i] << ","; std::cout << this->best_path[i] << ",";
std::cout << std::endl; std::cout << std::endl;
this->best_path_stamp=ros::Time::now();
this->roadmap.get_trajectory(best_path_index,x,y,heading); this->roadmap.get_trajectory(best_path_index,x,y,heading);
plan.resize(x.size()); plan.resize(x.size());
try{ try{
......
Supports Markdown
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