Commit 27236f60 authored by Sergi Hernandez's avatar Sergi Hernandez
Browse files

Merge branch 'master' into 'development'

Master

See merge request !1
parents 192b9ec5 faf5eb47
...@@ -13,7 +13,7 @@ gen.const("time", int_t, 1, "Use distance and speed to compute costs"), ...@@ -13,7 +13,7 @@ gen.const("time", int_t, 1, "Use distance and speed to compute costs"),
gen.add("opendrive_file", str_t, 0, "Opendrive map file", "") gen.add("opendrive_file", str_t, 0, "Opendrive map file", "")
gen.add("opendrive_frame", str_t, 0, "Opendrive frame ID", "") gen.add("opendrive_frame", str_t, 0, "Opendrive frame ID", "")
gen.add("angle_tol", double_t, 0, "Angle tolerance to find start and end positions on the road map", 0.5, 0.1, 1.5707) gen.add("angle_tol", double_t, 0, "Angle tolerance to find start and end positions on the road map", 0.5, 0.1, 10.0)
gen.add("dist_tol", double_t, 0, "Distance tolerance to find start and end positions on the road map", 3.0, 0.5, 10.0) gen.add("dist_tol", double_t, 0, "Distance tolerance to find start and end positions on the road map", 3.0, 0.5, 10.0)
gen.add("multi_hyp", bool_t, 0, "Use multi hypothesis path search", False) gen.add("multi_hyp", bool_t, 0, "Use multi hypothesis path search", False)
gen.add("resolution", double_t, 0, "Resolution of the generated path", 0.1, 0.01, 1.0) gen.add("resolution", double_t, 0, "Resolution of the generated path", 0.1, 0.01, 1.0)
......
...@@ -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{
......
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