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"),
gen.add("opendrive_file", str_t, 0, "Opendrive map file", "")
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("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)
......
......@@ -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