diff --git a/CMakeLists.txt b/CMakeLists.txt
index 9bd2b9b5c6c07182ddfda0681dbac0fc2b11351f..f6c5f66fd5e5f1636eae8df94d3b12bbc37f60b5 100644
--- a/CMakeLists.txt
+++ b/CMakeLists.txt
@@ -19,7 +19,7 @@ find_package(catkin REQUIRED
find_package(iriutils REQUIRED)
find_package(autonomous_driving_tools REQUIRED)
-find_package(opendrive_road_map REQUIRED)
+find_package(road_map_planner REQUIRED)
generate_dynamic_reconfigure_options(
cfg/OpendriveGlobalPlanner.cfg
@@ -45,7 +45,7 @@ include_directories(
${catkin_INCLUDE_DIRS}
${iriutils_INCLUDE_DIRS}
${autonomous_driving_tools_INCLUDE_DIR}
- ${opendrive_road_map_INCLUDE_DIRS}
+ ${road_map_planner_INCLUDE_DIRS}
)
add_library(${PROJECT_NAME}
@@ -55,7 +55,7 @@ add_dependencies(${PROJECT_NAME} ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EX
target_link_libraries(${PROJECT_NAME} ${catkin_LIBRARIES})
target_link_libraries(${PROJECT_NAME} ${iriutils_LIBRARIES})
target_link_libraries(${PROJECT_NAME} ${autonomous_driving_tools_LIBRARIES})
-target_link_libraries(${PROJECT_NAME} ${opendrive_road_map_LIBRARIES})
+target_link_libraries(${PROJECT_NAME} ${road_map_planner_LIBRARIES})
install(TARGETS ${PROJECT_NAME}
ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION}
diff --git a/cfg/OpendriveGlobalPlanner.cfg b/cfg/OpendriveGlobalPlanner.cfg
index c12e6bf230de9cbb86d6150e34cdfb58278106e4..b04891a08fa5ae6c37bda2c746fdd5b51a814e48 100755
--- a/cfg/OpendriveGlobalPlanner.cfg
+++ b/cfg/OpendriveGlobalPlanner.cfg
@@ -18,8 +18,6 @@ gen.add("end_angle_tol", double_t, 0, "Angle tolerance to find start and end p
gen.add("end_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)
-gen.add("scale_factor", double_t, 0, "Scale factor of the input road description", 1.0, 0.01, 10.0)
-gen.add("min_road_length", double_t, 0, "Minimum road length to take it into account", 0.1, 0.01, 1.0)
gen.add("cost_type", int_t, 0, "Cost type", 0, 0, 1, edit_method=enum_cost_type)
gen.add("debug", bool_t, 0, "Show debug messages", False)
diff --git a/include/iri_opendrive_global_planner/opendrive_planner.h b/include/iri_opendrive_global_planner/opendrive_planner.h
index 786f38bf2d8859d5ceffd94c6cc441ae7639917f..6779c90ee40178e4720f4abdc3057316b8e6d1c4 100644
--- a/include/iri_opendrive_global_planner/opendrive_planner.h
+++ b/include/iri_opendrive_global_planner/opendrive_planner.h
@@ -51,7 +51,7 @@
#include <iri_nav_module/get_opendrive_map.h>
#include <iri_nav_module/get_opendrive_nodes.h>
-#include "opendrive_road_map.h"
+#include "road_map_planner.h"
#include "mutex.h"
namespace iri_opendrive_global_planner {
@@ -128,13 +128,13 @@ class OpendriveGlobalPlanner : public nav_core::BaseGlobalPlanner
bool get_opendrive_map(iri_nav_module::get_opendrive_map::Request &req,iri_nav_module::get_opendrive_map::Response &res);
ros::ServiceServer opendrive_nodes_service;
bool get_opendrive_nodes(iri_nav_module::get_opendrive_nodes::Request &req,iri_nav_module::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);
nav_msgs::OccupancyGrid full_path_;
CMutex path_access;
bool initialized_;
- COpendriveRoadMap roadmap;
+ CRoadMapPlanner roadmap;
std::vector<unsigned int> best_path;
ros::Time best_path_stamp;
double start_angle_tol;
diff --git a/src/opendrive_planner.cpp b/src/opendrive_planner.cpp
index 0ac42a4cf5bbacc18798c2d3a1cee19954507a21..5c16cdeabc402f70b9718d0742125c0d4f3d6609 100644
--- a/src/opendrive_planner.cpp
+++ b/src/opendrive_planner.cpp
@@ -51,10 +51,10 @@ namespace iri_opendrive_global_planner {
OpendriveGlobalPlanner::OpendriveGlobalPlanner() :
costmap_(NULL), initialized_(false), tf2_listener(tf2_buffer) {
- this->start_angle_tol=DEFAULT_ANGLE_THRESHOLD;
- this->start_dist_tol=DEFAULT_DIST_THRESHOLD;
- this->end_angle_tol=DEFAULT_ANGLE_THRESHOLD;
- this->end_dist_tol=DEFAULT_DIST_THRESHOLD;
+ this->start_angle_tol=3.14159;
+ this->start_dist_tol=4.0;
+ this->end_angle_tol=3.14159;
+ this->end_dist_tol=4.0;
this->multi_hyp=false;
}
@@ -86,28 +86,25 @@ void OpendriveGlobalPlanner::initialize(std::string name, costmap_2d::Costmap2D*
try{
std::string opendrive_file;
- double resolution,scale_factor,min_road_length;
+ double resolution;
int cost_type;
if(private_nh.getParam("resolution", resolution))
this->roadmap.set_resolution(resolution);
else
ROS_ERROR("OpendriveGlobalPlanner: Map resolution not defined");
- if(private_nh.getParam("scale_factor", scale_factor))
- this->roadmap.set_scale_factor(scale_factor);
- else
- ROS_ERROR("OpendriveGlobalPlanner: Map scale factor not defined");
- if(private_nh.getParam("min_road_length", min_road_length))
- this->roadmap.set_min_road_length(min_road_length);
- else
- ROS_ERROR("OpendriveGlobalPlanner: Map minimum road length not defined");
if(private_nh.getParam("opendrive_file", opendrive_file))
{
if(opendrive_file!="")
{
try{
- this->roadmap.load(opendrive_file);
- this->create_opendrive_map(opendrive_file,resolution,scale_factor,min_road_length);
+ if(opendrive_file.find("xodr")!=std::string::npos)
+ roadmap.load_opendrive(opendrive_file);
+ else if(opendrive_file.find("osm")!=std::string::npos)
+ roadmap.load_osm(opendrive_file);
+ else
+ ROS_ERROR("Unknown file type");
+ this->create_opendrive_map(opendrive_file,resolution);
}catch(CException &e){
ROS_ERROR_STREAM(e.what());
}
@@ -160,42 +157,35 @@ void OpendriveGlobalPlanner::map_connect_callback(const ros::SingleSubscriberPub
this->path_access.exit();
}
-void OpendriveGlobalPlanner::create_opendrive_map(std::string &filename,double resolution,double scale_factor,double min_road_length)
+void OpendriveGlobalPlanner::create_opendrive_map(std::string &filename,double resolution)
{
- std::vector<double> x,partial_x,y,partial_y;
+ std::vector<double> x,partial_x,y,partial_y,heading;
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;
+ CRoadMap road;
this->path_access.enter();
- road.load(filename);
+ if(filename.find("xodr")!=std::string::npos)
+ road.load_opendrive(filename);
+ else if(filename.find("osm")!=std::string::npos)
+ road.load_osm(filename);
+ else
+ return;
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++)
+ road.get_lane_geometry(x,y,heading);
+ for(unsigned int i=0;i<x.size();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];
- }
- }
+ if(x[i]>max_x)
+ max_x=x[i];
+ else if(x[i]<min_x)
+ min_x=x[i];
+ if(y[i]>max_y)
+ max_y=y[i];
+ else if(y[i]<min_y)
+ min_y=y[i];
}
this->full_path_.info.width=((max_x-min_x)/resolution)+1;
this->full_path_.info.height=((max_y-min_y)/resolution)+1;
@@ -238,18 +228,21 @@ void OpendriveGlobalPlanner::reconfigureCB(iri_opendrive_global_planner::Opendri
try{
boost::mutex::scoped_lock lock(mutex_);
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);
this->opendrive_frame_id_=new_config.opendrive_frame;
this->start_angle_tol=new_config.start_angle_tol;
this->start_dist_tol=new_config.start_dist_tol;
this->end_angle_tol=new_config.end_angle_tol;
this->end_dist_tol=new_config.end_dist_tol;
this->multi_hyp=new_config.multi_hyp;
- 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)
+ if(new_config.opendrive_file!=this->config.opendrive_file)
{
- 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);
+ if(new_config.opendrive_file.find("xodr")!=std::string::npos)
+ roadmap.load_opendrive(new_config.opendrive_file);
+ else if(new_config.opendrive_file.find("osm")!=std::string::npos)
+ roadmap.load_osm(new_config.opendrive_file);
+ else
+ ROS_ERROR("Unknown file type");
+ this->create_opendrive_map(new_config.opendrive_file,new_config.resolution);
}
this->roadmap.set_cost_type((cost_t)new_config.cost_type);
this->config=new_config;
@@ -377,27 +370,29 @@ bool OpendriveGlobalPlanner::makePlan(const geometry_msgs::PoseStamped& start, c
{
//ROS_WARN("Make Plan");
yaw=tf2::getYaw(start_out.pose.orientation);
- this->roadmap.set_start_pose(start_out.pose.position.x,start_out.pose.position.y,yaw,this->start_dist_tol,this->start_angle_tol);
- this->roadmap.get_start_pose_candidates(start_candidates);
+ this->roadmap.set_start_point(start_out.pose.position.x,start_out.pose.position.y,yaw,this->start_dist_tol,this->start_angle_tol);
+ this->roadmap.get_start_point_candidates(start_candidates);
if(this->config.debug)
{
std::cout << "--- Start pose candidates: " << std::endl;
for(unsigned int i=0;i<start_candidates.size();i++)
{
- std::cout <<"--- " << i << ". x: " << start_candidates[i].pose.x << ", y:" << start_candidates[i].pose.y << ", heading: " << start_candidates[i].pose.heading << std::endl;
- std::cout << "--- distance from desired pose: " << start_candidates[i].dist << std::endl;
+ std::cout <<"--- " << i << ". x: " << start_candidates[i].point.x << ", y:" << start_candidates[i].point.y << ", heading: " << start_candidates[i].point.heading << std::endl;
+ std::cout << "--- distance from desired pose: " << start_candidates[i].distance << std::endl;
+ std::cout << "--- lateral offset from desired pose: " << start_candidates[i].lateral_offset << std::endl;
}
}
yaw=tf2::getYaw(goal_out.pose.orientation);
- this->roadmap.set_end_pose(goal_out.pose.position.x,goal_out.pose.position.y,yaw,this->end_dist_tol,this->end_angle_tol);
- this->roadmap.get_end_pose_candidates(end_candidates);
+ this->roadmap.set_end_point(goal_out.pose.position.x,goal_out.pose.position.y,yaw,this->end_dist_tol,this->end_angle_tol);
+ this->roadmap.get_end_point_candidates(end_candidates);
if(this->config.debug)
{
std::cout << "--- End pose candidates: " << std::endl;
for(unsigned int i=0;i<end_candidates.size();i++)
{
- std::cout <<"--- "<< i << ". x: " << end_candidates[i].pose.x << ", y:" << end_candidates[i].pose.y << ", heading: " << end_candidates[i].pose.heading << std::endl;
- std::cout << "--- distance from desired pose: " << end_candidates[i].dist << std::endl;
+ std::cout <<"--- "<< i << ". x: " << end_candidates[i].point.x << ", y:" << end_candidates[i].point.y << ", heading: " << end_candidates[i].point.heading << std::endl;
+ std::cout << "--- distance from desired pose: " << end_candidates[i].distance << std::endl;
+ std::cout << "--- lateral offset from desired pose: " << end_candidates[i].lateral_offset << std::endl;
}
}
this->roadmap.find_shortest_path(this->multi_hyp);