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

Library changes updates

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