Commit 9aca1907 authored by Sergi Hernandez's avatar Sergi Hernandez
Browse files

Added a TF listener to transform the input and output data to and from the opendrive frame.

parent 0dc94e75
...@@ -11,13 +11,14 @@ gen.const("time", int_t, 1, "Use distance and speed to compute costs"), ...@@ -11,13 +11,14 @@ gen.const("time", int_t, 1, "Use distance and speed to compute costs"),
], "Possible costs types.") ], "Possible costs types.")
gen.add("opendrive_file", str_t, 0, "Opendrive map file", "") gen.add("opendrive_file", str_t, 0, "Opendrive map file", "")
gen.add("angle_tol", double_t, 0, "Angle tolerance to find start and end positions on the road map", 0.5, 0.1, 1.5707) gen.add("opendrive_frame", str_t, 0, "Opendrive frame ID", "")
gen.add("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("angle_tol", double_t, 0, "Angle tolerance to find start and end positions on the road map", 0.5, 0.1, 1.5707)
gen.add("multi_hyp", bool_t, 0, "Use multi hypothesis path search", False) gen.add("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("resolution", double_t, 0, "Resolution of the generated path", 0.1, 0.01, 1.0) gen.add("multi_hyp", bool_t, 0, "Use multi hypothesis path search", False)
gen.add("scale_factor", double_t, 0, "Scale factor of the input road description", 1.0, 0.01, 10.0) 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("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("cost_type", int_t, 0, "Cost type", 0, 0, 1, edit_method=enum_cost_type)
exit(gen.generate(PACKAGE, "iri_opendrive_global_planner", "OpendriveGlobalPlanner")) exit(gen.generate(PACKAGE, "iri_opendrive_global_planner", "OpendriveGlobalPlanner"))
...@@ -45,6 +45,8 @@ ...@@ -45,6 +45,8 @@
#include <nav_msgs/GetPlan.h> #include <nav_msgs/GetPlan.h>
#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 <tf2_geometry_msgs/tf2_geometry_msgs.h>
#include <tf2_ros/transform_listener.h>
#include "opendrive_road_map.h" #include "opendrive_road_map.h"
...@@ -114,7 +116,7 @@ class OpendriveGlobalPlanner : public nav_core::BaseGlobalPlanner ...@@ -114,7 +116,7 @@ class OpendriveGlobalPlanner : public nav_core::BaseGlobalPlanner
* @brief Store a copy of the current costmap in \a costmap. Called by makePlan. * @brief Store a copy of the current costmap in \a costmap. Called by makePlan.
*/ */
costmap_2d::Costmap2D* costmap_; costmap_2d::Costmap2D* costmap_;
std::string frame_id_; std::string frame_id_,opendrive_frame_id_;
ros::Publisher plan_pub_; ros::Publisher plan_pub_;
bool initialized_; bool initialized_;
...@@ -135,6 +137,8 @@ class OpendriveGlobalPlanner : public nav_core::BaseGlobalPlanner ...@@ -135,6 +137,8 @@ class OpendriveGlobalPlanner : public nav_core::BaseGlobalPlanner
void reconfigureCB(iri_opendrive_global_planner::OpendriveGlobalPlannerConfig &new_config, uint32_t level); void reconfigureCB(iri_opendrive_global_planner::OpendriveGlobalPlannerConfig &new_config, uint32_t level);
iri_opendrive_global_planner::OpendriveGlobalPlannerConfig config; iri_opendrive_global_planner::OpendriveGlobalPlannerConfig config;
tf2_ros::Buffer tf2_buffer;
tf2_ros::TransformListener tf2_listener;
}; };
} //end namespace iri_opendrive_global_planner } //end namespace iri_opendrive_global_planner
......
...@@ -49,7 +49,7 @@ PLUGINLIB_EXPORT_CLASS(iri_opendrive_global_planner::OpendriveGlobalPlanner, nav ...@@ -49,7 +49,7 @@ PLUGINLIB_EXPORT_CLASS(iri_opendrive_global_planner::OpendriveGlobalPlanner, nav
namespace iri_opendrive_global_planner { namespace iri_opendrive_global_planner {
OpendriveGlobalPlanner::OpendriveGlobalPlanner() : OpendriveGlobalPlanner::OpendriveGlobalPlanner() :
costmap_(NULL), initialized_(false) { costmap_(NULL), initialized_(false), tf2_listener(tf2_buffer) {
this->angle_tol=DEFAULT_ANGLE_THRESHOLD; this->angle_tol=DEFAULT_ANGLE_THRESHOLD;
this->dist_tol=DEFAULT_DIST_THRESHOLD; this->dist_tol=DEFAULT_DIST_THRESHOLD;
this->multi_hyp=false; this->multi_hyp=false;
...@@ -75,8 +75,6 @@ void OpendriveGlobalPlanner::initialize(std::string name, costmap_2d::Costmap2DR ...@@ -75,8 +75,6 @@ void OpendriveGlobalPlanner::initialize(std::string name, costmap_2d::Costmap2DR
void OpendriveGlobalPlanner::initialize(std::string name, costmap_2d::Costmap2D* costmap, std::string frame_id) void OpendriveGlobalPlanner::initialize(std::string name, costmap_2d::Costmap2D* costmap, std::string frame_id)
{ {
double value;
if (!initialized_) if (!initialized_)
{ {
ros::NodeHandle private_nh("~/" + name); ros::NodeHandle private_nh("~/" + name);
...@@ -85,6 +83,9 @@ void OpendriveGlobalPlanner::initialize(std::string name, costmap_2d::Costmap2D* ...@@ -85,6 +83,9 @@ void OpendriveGlobalPlanner::initialize(std::string name, costmap_2d::Costmap2D*
try{ try{
std::string opendrive_file; std::string opendrive_file;
double value;
int cost_type;
private_nh.param("resolution", value,0.0); private_nh.param("resolution", value,0.0);
this->roadmap.set_resolution(value); this->roadmap.set_resolution(value);
private_nh.param("scale_factor", value,0.0); private_nh.param("scale_factor", value,0.0);
...@@ -94,11 +95,10 @@ void OpendriveGlobalPlanner::initialize(std::string name, costmap_2d::Costmap2D* ...@@ -94,11 +95,10 @@ void OpendriveGlobalPlanner::initialize(std::string name, costmap_2d::Costmap2D*
private_nh.param("opendrive_file", opendrive_file,std::string("")); private_nh.param("opendrive_file", opendrive_file,std::string(""));
if(opendrive_file!="") if(opendrive_file!="")
this->roadmap.load(opendrive_file); this->roadmap.load(opendrive_file);
private_nh.param("opendrive_frame", this->opendrive_frame_id_,std::string(""));
private_nh.param("angle_tol", this->angle_tol,DEFAULT_ANGLE_THRESHOLD); private_nh.param("angle_tol", this->angle_tol,DEFAULT_ANGLE_THRESHOLD);
private_nh.param("dist_tol", this->dist_tol,DEFAULT_DIST_THRESHOLD); private_nh.param("dist_tol", this->dist_tol,DEFAULT_DIST_THRESHOLD);
private_nh.param("multi_hyp", this->multi_hyp,false); private_nh.param("multi_hyp", this->multi_hyp,false);
std::cout << "multi hypothessis: " << this->multi_hyp << std::endl;
int cost_type;
private_nh.param("cost_type",cost_type,(int)COST_DIST); private_nh.param("cost_type",cost_type,(int)COST_DIST);
this->roadmap.set_cost_type((cost_t)cost_type); this->roadmap.set_cost_type((cost_t)cost_type);
}catch(CException &e){ }catch(CException &e){
...@@ -125,6 +125,7 @@ void OpendriveGlobalPlanner::reconfigureCB(iri_opendrive_global_planner::Opendri ...@@ -125,6 +125,7 @@ void OpendriveGlobalPlanner::reconfigureCB(iri_opendrive_global_planner::Opendri
this->roadmap.set_min_road_length(new_config.min_road_length); this->roadmap.set_min_road_length(new_config.min_road_length);
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 || new_config.scale_factor!=this->config.scale_factor || new_config.min_road_length!=this->config.min_road_length)
this->roadmap.load(new_config.opendrive_file); this->roadmap.load(new_config.opendrive_file);
this->opendrive_frame_id_=new_config.opendrive_frame;
this->angle_tol=new_config.angle_tol; this->angle_tol=new_config.angle_tol;
this->dist_tol=new_config.dist_tol; this->dist_tol=new_config.dist_tol;
this->multi_hyp=new_config.multi_hyp; this->multi_hyp=new_config.multi_hyp;
...@@ -194,6 +195,12 @@ bool OpendriveGlobalPlanner::makePlan(const geometry_msgs::PoseStamped& start, c ...@@ -194,6 +195,12 @@ bool OpendriveGlobalPlanner::makePlan(const geometry_msgs::PoseStamped& start, c
std::vector< std::vector<unsigned int> > paths; std::vector< std::vector<unsigned int> > paths;
std::vector<double> costs; std::vector<double> costs;
std::vector<TMapPose> start_candidates,end_candidates; std::vector<TMapPose> start_candidates,end_candidates;
geometry_msgs::PoseStamped start_out,goal_out,point;
std::string target_frame;
std::string source_frame;
ros::Time time;
ros::Duration timeout;
geometry_msgs::TransformStamped transform;
boost::mutex::scoped_lock lock(mutex_); boost::mutex::scoped_lock lock(mutex_);
if (!initialized_) { if (!initialized_) {
...@@ -220,45 +227,49 @@ bool OpendriveGlobalPlanner::makePlan(const geometry_msgs::PoseStamped& start, c ...@@ -220,45 +227,49 @@ bool OpendriveGlobalPlanner::makePlan(const geometry_msgs::PoseStamped& start, c
return false; return false;
} }
/* /* transform start and end positions to map opendrive */
double wx = start.pose.position.x; try{
double wy = start.pose.position.y; target_frame = this->opendrive_frame_id_;
source_frame = start.header.frame_id;
unsigned int start_x_i, start_y_i, goal_x_i, goal_y_i; time = ros::Time(0);
double start_x, start_y, goal_x, goal_y; timeout = ros::Duration(0.1);
if(this->tf2_buffer.canTransform(target_frame, source_frame, time, timeout))
if (!costmap_->worldToMap(wx, wy, start_x_i, start_y_i)) {
{ transform = this->tf2_buffer.lookupTransform(target_frame, source_frame, time);
ROS_WARN("The robot's start position is off the global costmap. Planning will always fail, are you sure the robot has been properly localized?"); tf2::doTransform(start,start_out, transform);
return false; }else{
ROS_WARN("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))
{
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());
}
}catch (tf2::TransformException &ex){
ROS_ERROR("TF2 Exception: %s",ex.what());
} }
worldToMap(wx, wy, start_x, start_y);
//clear the starting cell within the costmap because we know it can't be an obstacle
clearRobotCell(start, start_x_i, start_y_i);
int nx = costmap_->getSizeInCellsX(), ny = costmap_->getSizeInCellsY();
*/
try{ try{
ROS_WARN("Make Plan"); ROS_WARN("Make Plan");
yaw=tf2::getYaw(start.pose.orientation); yaw=tf2::getYaw(start_out.pose.orientation);
this->roadmap.set_start_pose(start.pose.position.x,start.pose.position.y,yaw,this->dist_tol,this->angle_tol); 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); this->roadmap.get_start_pose_candidates(start_candidates);
std::cout << "Start pose candidates: " << std::endl; std::cout << "Start pose candidates: " << std::endl;
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 << " 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_out.pose.orientation);
this->roadmap.set_end_pose(goal.pose.position.x,goal.pose.position.y,yaw,this->dist_tol,this->angle_tol); 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); this->roadmap.get_end_pose_candidates(end_candidates);
std::cout << "End pose candidates: " << std::endl; std::cout << "End pose candidates: " << std::endl;
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 << " 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);
...@@ -279,18 +290,35 @@ bool OpendriveGlobalPlanner::makePlan(const geometry_msgs::PoseStamped& start, c ...@@ -279,18 +290,35 @@ bool OpendriveGlobalPlanner::makePlan(const geometry_msgs::PoseStamped& start, c
std::cout << std::endl; std::cout << std::endl;
this->roadmap.get_trajectory(best_path_index,x,y,heading); this->roadmap.get_trajectory(best_path_index,x,y,heading);
plan.resize(x.size()); plan.resize(x.size());
ros::Time plan_time=ros::Time::now(); try{
for(unsigned int i=0;i<x.size();i++) target_frame = this->frame_id_;
{ source_frame = this->opendrive_frame_id_;
plan[i].header.frame_id=frame_id_; time = ros::Time(0);
plan[i].header.stamp=plan_time; timeout = ros::Duration(0.1);
plan[i].pose.position.x=x[i]; if(this->tf2_buffer.canTransform(target_frame, source_frame, time, timeout))
plan[i].pose.position.y=y[i]; {
plan[i].pose.position.y=y[i]; transform = this->tf2_buffer.lookupTransform(target_frame, source_frame, time);
tf2::Quaternion orientation; point.header.frame_id=this->opendrive_frame_id_;
orientation.setRPY(0.0,0.0,heading[i]); point.header.stamp=time;
plan[i].pose.orientation=tf2::toMsg(orientation); for(unsigned int i=0;i<x.size();i++)
{
point.pose.position.x=x[i];
point.pose.position.y=y[i];
point.pose.position.z=0.0;
tf2::Quaternion orientation;
orientation.setRPY(0.0,0.0,heading[i]);
point.pose.orientation=tf2::toMsg(orientation);
tf2::doTransform(point,plan[i],transform);
plan[i].header.frame_id=this->opendrive_frame_id_;
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());
}
}catch (tf2::TransformException &ex){
ROS_ERROR("TF2 Exception: %s",ex.what());
} }
}catch(CException &e){ }catch(CException &e){
ROS_ERROR_STREAM(e.what()); ROS_ERROR_STREAM(e.what());
return false; return false;
......
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