Commit 95cfe570 authored by Fernando Herrero's avatar Fernando Herrero
Browse files

Library changes updates

parent 4ab2a40b
......@@ -46,7 +46,7 @@
#include <dynamic_reconfigure/server.h>
#include <iri_opendrive_global_planner/OpendriveGlobalPlannerConfig.h>
#include "road_map.h"
#include "opendrive_road_map.h"
namespace iri_opendrive_global_planner {
......@@ -118,7 +118,7 @@ class OpendriveGlobalPlanner : public nav_core::BaseGlobalPlanner
ros::Publisher plan_pub_;
bool initialized_;
CRoadMap roadmap;
COpendriveRoadMap roadmap;
double angle_tol;
double dist_tol;
bool multi_hyp;
......
......@@ -248,7 +248,7 @@ bool OpendriveGlobalPlanner::makePlan(const geometry_msgs::PoseStamped& start, c
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 << " road map node: " << start_candidates[i].node_index << " child: " << start_candidates[i].child_index << std::endl;
//std::cout << " road map node: " << start_candidates[i].node_index << " child: " << start_candidates[i].child_index << std::endl;
std::cout << " distance from desired pose: " << start_candidates[i].dist << std::endl;
}
yaw=tf2::getYaw(goal.pose.orientation);
......@@ -258,7 +258,7 @@ bool OpendriveGlobalPlanner::makePlan(const geometry_msgs::PoseStamped& start, c
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 << " road map node: " << end_candidates[i].node_index << " child: " << end_candidates[i].child_index << std::endl;
//std::cout << " road map node: " << end_candidates[i].node_index << " child: " << end_candidates[i].child_index << std::endl;
std::cout << " distance from desired pose: " << end_candidates[i].dist << std::endl;
}
this->roadmap.find_shortest_path(this->multi_hyp);
......
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