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 @@
#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;
......
......@@ -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{
......
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