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"),
], "Possible costs types.")
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("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("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, 1.5707)
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("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("cost_type", int_t, 0, "Cost type", 0, 0, 1, edit_method=enum_cost_type)
exit(gen.generate(PACKAGE, "iri_opendrive_global_planner", "OpendriveGlobalPlanner"))
......@@ -45,6 +45,8 @@
#include <nav_msgs/GetPlan.h>
#include <dynamic_reconfigure/server.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"
......@@ -114,7 +116,7 @@ class OpendriveGlobalPlanner : public nav_core::BaseGlobalPlanner
* @brief Store a copy of the current costmap in \a costmap. Called by makePlan.
*/
costmap_2d::Costmap2D* costmap_;
std::string frame_id_;
std::string frame_id_,opendrive_frame_id_;
ros::Publisher plan_pub_;
bool initialized_;
......@@ -135,6 +137,8 @@ class OpendriveGlobalPlanner : public nav_core::BaseGlobalPlanner
void reconfigureCB(iri_opendrive_global_planner::OpendriveGlobalPlannerConfig &new_config, uint32_t level);
iri_opendrive_global_planner::OpendriveGlobalPlannerConfig config;
tf2_ros::Buffer tf2_buffer;
tf2_ros::TransformListener tf2_listener;
};
} //end namespace iri_opendrive_global_planner
......
......@@ -49,7 +49,7 @@ PLUGINLIB_EXPORT_CLASS(iri_opendrive_global_planner::OpendriveGlobalPlanner, nav
namespace iri_opendrive_global_planner {
OpendriveGlobalPlanner::OpendriveGlobalPlanner() :
costmap_(NULL), initialized_(false) {
costmap_(NULL), initialized_(false), tf2_listener(tf2_buffer) {
this->angle_tol=DEFAULT_ANGLE_THRESHOLD;
this->dist_tol=DEFAULT_DIST_THRESHOLD;
this->multi_hyp=false;
......@@ -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)
{
double value;
if (!initialized_)
{
ros::NodeHandle private_nh("~/" + name);
......@@ -85,6 +83,9 @@ void OpendriveGlobalPlanner::initialize(std::string name, costmap_2d::Costmap2D*
try{
std::string opendrive_file;
double value;
int cost_type;
private_nh.param("resolution", value,0.0);
this->roadmap.set_resolution(value);
private_nh.param("scale_factor", value,0.0);
......@@ -94,11 +95,10 @@ void OpendriveGlobalPlanner::initialize(std::string name, costmap_2d::Costmap2D*
private_nh.param("opendrive_file", opendrive_file,std::string(""));
if(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("dist_tol", this->dist_tol,DEFAULT_DIST_THRESHOLD);
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);
this->roadmap.set_cost_type((cost_t)cost_type);
}catch(CException &e){
......@@ -125,6 +125,7 @@ void OpendriveGlobalPlanner::reconfigureCB(iri_opendrive_global_planner::Opendri
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)
this->roadmap.load(new_config.opendrive_file);
this->opendrive_frame_id_=new_config.opendrive_frame;
this->angle_tol=new_config.angle_tol;
this->dist_tol=new_config.dist_tol;
this->multi_hyp=new_config.multi_hyp;
......@@ -194,6 +195,12 @@ bool OpendriveGlobalPlanner::makePlan(const geometry_msgs::PoseStamped& start, c
std::vector< std::vector<unsigned int> > paths;
std::vector<double> costs;
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_);
if (!initialized_) {
......@@ -220,45 +227,49 @@ bool OpendriveGlobalPlanner::makePlan(const geometry_msgs::PoseStamped& start, c
return false;
}
/*
double wx = start.pose.position.x;
double wy = start.pose.position.y;
unsigned int start_x_i, start_y_i, goal_x_i, goal_y_i;
double start_x, start_y, goal_x, goal_y;
if (!costmap_->worldToMap(wx, wy, start_x_i, start_y_i))
{
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?");
return false;
/* transform start and end positions to map opendrive */
try{
target_frame = this->opendrive_frame_id_;
source_frame = start.header.frame_id;
time = ros::Time(0);
timeout = ros::Duration(0.1);
if(this->tf2_buffer.canTransform(target_frame, source_frame, time, timeout))
{
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());
}
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{
ROS_WARN("Make Plan");
yaw=tf2::getYaw(start.pose.orientation);
this->roadmap.set_start_pose(start.pose.position.x,start.pose.position.y,yaw,this->dist_tol,this->angle_tol);
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++)
{
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;
}
yaw=tf2::getYaw(goal.pose.orientation);
this->roadmap.set_end_pose(goal.pose.position.x,goal.pose.position.y,yaw,this->dist_tol,this->angle_tol);
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++)
{
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;
}
this->roadmap.find_shortest_path(this->multi_hyp);
......@@ -279,18 +290,35 @@ bool OpendriveGlobalPlanner::makePlan(const geometry_msgs::PoseStamped& start, c
std::cout << std::endl;
this->roadmap.get_trajectory(best_path_index,x,y,heading);
plan.resize(x.size());
ros::Time plan_time=ros::Time::now();
for(unsigned int i=0;i<x.size();i++)
{
plan[i].header.frame_id=frame_id_;
plan[i].header.stamp=plan_time;
plan[i].pose.position.x=x[i];
plan[i].pose.position.y=y[i];
plan[i].pose.position.y=y[i];
tf2::Quaternion orientation;
orientation.setRPY(0.0,0.0,heading[i]);
plan[i].pose.orientation=tf2::toMsg(orientation);
try{
target_frame = this->frame_id_;
source_frame = this->opendrive_frame_id_;
time = ros::Time(0);
timeout = ros::Duration(0.1);
if(this->tf2_buffer.canTransform(target_frame, source_frame, time, timeout))
{
transform = this->tf2_buffer.lookupTransform(target_frame, source_frame, time);
point.header.frame_id=this->opendrive_frame_id_;
point.header.stamp=time;
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){
ROS_ERROR_STREAM(e.what());
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