Skip to content
Snippets Groups Projects
Commit 1ba69cde authored by Fernando Herrero's avatar Fernando Herrero
Browse files

Merge branch 'refactor' into 'master'

Refactor

See merge request !3
parents 2fff4788 62444a74
No related branches found
No related tags found
1 merge request!3Refactor
......@@ -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}
......
......@@ -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)
......
......@@ -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;
......
......@@ -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);
......
0% Loading or .
You are about to add 0 people to the discussion. Proceed with caution.
Finish editing this message first!
Please register or to comment