From d4f665803bca52bd844d37a47d6895872bea8ac0 Mon Sep 17 00:00:00 2001
From: fherrero <fherrero@iri.upc.edu>
Date: Tue, 4 Oct 2022 16:45:16 +0200
Subject: [PATCH] Add debug param to show/hide cout messages
---
cfg/OpendriveGlobalPlanner.cfg | 2 +-
src/opendrive_planner.cpp | 89 +++++++++++++++++++---------------
2 files changed, 52 insertions(+), 39 deletions(-)
diff --git a/cfg/OpendriveGlobalPlanner.cfg b/cfg/OpendriveGlobalPlanner.cfg
index 1483478..0453c04 100755
--- a/cfg/OpendriveGlobalPlanner.cfg
+++ b/cfg/OpendriveGlobalPlanner.cfg
@@ -10,7 +10,6 @@ gen.const("dist", int_t, 0, "Use only distance to compute costs"),
gen.const("time", int_t, 1, "Use distance and speed to compute costs"),
], "Possible costs types.")
-
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, 10.0)
@@ -20,5 +19,6 @@ gen.add("resolution", double_t, 0, "Resolution of the generated path", 0.1,
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)
exit(gen.generate(PACKAGE, "iri_opendrive_global_planner", "OpendriveGlobalPlanner"))
diff --git a/src/opendrive_planner.cpp b/src/opendrive_planner.cpp
index 2c51eb0..6851b97 100644
--- a/src/opendrive_planner.cpp
+++ b/src/opendrive_planner.cpp
@@ -90,15 +90,15 @@ void OpendriveGlobalPlanner::initialize(std::string name, costmap_2d::Costmap2D*
if(private_nh.getParam("resolution", resolution))
this->roadmap.set_resolution(resolution);
else
- ROS_ERROR("Map resolution not defined");
+ 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("Map scale factor not defined");
+ 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("Map minimum road length not defined");
+ ROS_ERROR("OpendriveGlobalPlanner: Map minimum road length not defined");
if(private_nh.getParam("opendrive_file", opendrive_file))
{
if(opendrive_file!="")
@@ -112,17 +112,17 @@ void OpendriveGlobalPlanner::initialize(std::string name, costmap_2d::Costmap2D*
}
}
else
- ROS_ERROR("Opendrive map file not defined");
+ ROS_ERROR("OpendriveGlobalPlanner: Opendrive map file not defined");
if(!private_nh.getParam("opendrive_frame", this->opendrive_frame_id_))
- ROS_ERROR("Opendrive frame ID not defined");
+ ROS_ERROR("OpendriveGlobalPlanner: Opendrive frame ID not defined");
if(!private_nh.getParam("angle_tol", this->angle_tol))
- ROS_ERROR("Angle tolerance for the goals not defined");
+ ROS_ERROR("OpendriveGlobalPlanner: Angle tolerance for the goals not defined");
if(!private_nh.getParam("dist_tol", this->dist_tol))
- ROS_ERROR("Distance tolerance for the goals not defined");
+ ROS_ERROR("OpendriveGlobalPlanner: Distance tolerance for the goals not defined");
if(!private_nh.getParam("multi_hyp", this->multi_hyp))
- ROS_ERROR("Multihypotheis feature not defined");
+ ROS_ERROR("OpendriveGlobalPlanner: Multihypotheis feature not defined");
if(!private_nh.getParam("cost_type",cost_type))
- ROS_ERROR("Desired cost type not defined");
+ ROS_ERROR("OpendriveGlobalPlanner: Desired cost type not defined");
this->roadmap.set_cost_type((cost_t)cost_type);
}catch(CException &e){
ROS_ERROR_STREAM(e.what());
@@ -143,7 +143,7 @@ void OpendriveGlobalPlanner::initialize(std::string name, costmap_2d::Costmap2D*
initialized_ = true;
} else
- ROS_WARN("This planner has already been initialized, you can't call it twice, doing nothing");
+ ROS_WARN("OpendriveGlobalPlanner: this planner has already been initialized, you can't call it twice, doing nothing");
}
void OpendriveGlobalPlanner::map_connect_callback(const ros::SingleSubscriberPublisher &subs)
@@ -246,7 +246,7 @@ void OpendriveGlobalPlanner::clearRobotCell(const geometry_msgs::PoseStamped& gl
{
if (!initialized_)
{
- ROS_ERROR("This planner has not been initialized yet, but it is being used, please call initialize() before use");
+ ROS_ERROR("OpendriveGlobalPlanner: this planner has not been initialized yet, but it is being used, please call initialize() before use");
return;
}
@@ -309,7 +309,7 @@ bool OpendriveGlobalPlanner::makePlan(const geometry_msgs::PoseStamped& start, c
boost::mutex::scoped_lock lock(mutex_);
if (!initialized_) {
- ROS_ERROR("This planner has not been initialized yet, but it is being used, please call initialize() before use");
+ ROS_ERROR("OpendriveGlobalPlanner: this planner has not been initialized yet, but it is being used, please call initialize() before use");
return false;
}
@@ -322,13 +322,13 @@ bool OpendriveGlobalPlanner::makePlan(const geometry_msgs::PoseStamped& start, c
//until tf can handle transforming things that are way in the past... we'll require the goal to be in our global frame
if (goal.header.frame_id != global_frame)
{
- ROS_ERROR("The goal pose passed to this planner must be in the %s frame. It is instead in the %s frame.", global_frame.c_str(), goal.header.frame_id.c_str());
+ ROS_ERROR("OpendriveGlobalPlanner: the goal pose passed to this planner must be in the %s frame. It is instead in the %s frame.", global_frame.c_str(), goal.header.frame_id.c_str());
return false;
}
if (start.header.frame_id != global_frame)
{
- ROS_ERROR("The start pose passed to this planner must be in the %s frame. It is instead in the %s frame.", global_frame.c_str(), start.header.frame_id.c_str());
+ ROS_ERROR("OpendriveGlobalPlanner: the start pose passed to this planner must be in the %s frame. It is instead in the %s frame.", global_frame.c_str(), start.header.frame_id.c_str());
return false;
}
@@ -343,7 +343,7 @@ bool OpendriveGlobalPlanner::makePlan(const geometry_msgs::PoseStamped& start, c
transform = this->tf2_buffer.lookupTransform(target_frame, source_frame, time);
tf2::doTransform(start,start_out, transform);
}else{
- ROS_WARN("No transform found for start point from '%s' to '%s'", source_frame.c_str(), target_frame.c_str());
+ ROS_WARN("OpendriveGlobalPlanner: no transform found for start point from '%s' to '%s'", source_frame.c_str(), target_frame.c_str());
}
source_frame = goal.header.frame_id;
if(this->tf2_buffer.canTransform(target_frame, source_frame, time, timeout))
@@ -351,48 +351,61 @@ bool OpendriveGlobalPlanner::makePlan(const geometry_msgs::PoseStamped& start, c
transform = this->tf2_buffer.lookupTransform(target_frame, source_frame, time);
tf2::doTransform(goal,goal_out, transform);
}else{
- ROS_WARN("No transform found for end point from '%s' to '%s'", source_frame.c_str(), target_frame.c_str());
+ ROS_WARN("OpendriveGlobalPlanner: no transform found for end point from '%s' to '%s'", source_frame.c_str(), target_frame.c_str());
}
}catch (tf2::TransformException &ex){
ROS_ERROR("TF2 Exception: %s",ex.what());
}
- try{
- ROS_WARN("Make Plan");
+ try
+ {
+ //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->dist_tol,this->angle_tol);
this->roadmap.get_start_pose_candidates(start_candidates);
- std::cout << "Start pose candidates: " << std::endl;
- for(unsigned int i=0;i<start_candidates.size();i++)
+ if(this->config.debug)
{
- 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 << "--- 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;
+ }
}
yaw=tf2::getYaw(goal_out.pose.orientation);
this->roadmap.set_end_pose(goal_out.pose.position.x,goal_out.pose.position.y,yaw,this->dist_tol,this->angle_tol);
this->roadmap.get_end_pose_candidates(end_candidates);
- std::cout << "End pose candidates: " << std::endl;
- for(unsigned int i=0;i<end_candidates.size();i++)
+ if(this->config.debug)
{
- 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 << "--- 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;
+ }
}
this->roadmap.find_shortest_path(this->multi_hyp);
this->roadmap.get_paths(costs,paths);
- for(unsigned int i=0;i<paths.size();i++)
+ if(this->config.debug)
{
- std::cout << "path " << i << ":" << std::endl;
- for(unsigned int j=0;j<paths[i].size();j++)
- std::cout << paths[i][j] << ",";
- std::cout << std::endl;
+ for(unsigned int i=0;i<paths.size();i++)
+ {
+ std::cout << "--- path " << i << ":" << std::endl;
+ for(unsigned int j=0;j<paths[i].size();j++)
+ std::cout <<"--- " <<paths[i][j] << ",";
+ std::cout << std::endl;
+ }
}
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<this->best_path.size();i++)
- std::cout << this->best_path[i] << ",";
- std::cout << std::endl;
+ if(this->config.debug)
+ {
+ std::cout << "--- best path index: " << best_path_index << std::endl;
+ 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());
@@ -419,10 +432,10 @@ bool OpendriveGlobalPlanner::makePlan(const geometry_msgs::PoseStamped& start, c
plan[i].header.stamp=time;
}
}else{
- ROS_WARN("No transform found for path points from '%s' to '%s'", source_frame.c_str(), target_frame.c_str());
+ ROS_WARN("OpendriveGlobalPlanner: no transform found for path points from '%s' to '%s'", source_frame.c_str(), target_frame.c_str());
}
}catch (tf2::TransformException &ex){
- ROS_ERROR("TF2 Exception: %s",ex.what());
+ ROS_ERROR("OpendriveGlobalPlanner: TF2 Exception: %s",ex.what());
}
}catch(CException &e){
@@ -440,7 +453,7 @@ void OpendriveGlobalPlanner::publishPlan(const std::vector<geometry_msgs::PoseSt
{
if (!initialized_)
{
- ROS_ERROR("This planner has not been initialized yet, but it is being used, please call initialize() before use");
+ ROS_ERROR("OpendriveGlobalPlanner: this planner has not been initialized yet, but it is being used, please call initialize() before use");
return;
}
--
GitLab