From 7ca6636cbf898d6d1c17d3f7c66f1307ea211368 Mon Sep 17 00:00:00 2001 From: Sergi Hernandez Juan <shernand@iri.upc.edu> Date: Mon, 28 Sep 2020 15:57:07 +0200 Subject: [PATCH] The returned path now includes the heading for each point. --- src/opendrive_planner.cpp | 34 ++++++++++++++-------------------- 1 file changed, 14 insertions(+), 20 deletions(-) diff --git a/src/opendrive_planner.cpp b/src/opendrive_planner.cpp index 1bfc2bb..f0d4696 100644 --- a/src/opendrive_planner.cpp +++ b/src/opendrive_planner.cpp @@ -39,7 +39,7 @@ #include <pluginlib/class_list_macros.h> #include <costmap_2d/cost_values.h> #include <costmap_2d/costmap_2d.h> -#include <tf/transform_datatypes.h> +#include <tf2/utils.h> #include "exceptions.h" @@ -162,9 +162,10 @@ bool OpendriveGlobalPlanner::makePlan(const geometry_msgs::PoseStamped& start, c bool OpendriveGlobalPlanner::makePlan(const geometry_msgs::PoseStamped& start, const geometry_msgs::PoseStamped& goal,double tolerance, std::vector<geometry_msgs::PoseStamped>& plan) { - double roll, pitch, yaw; + double yaw; std::vector<unsigned int> path; - std::vector<double> x,y; + std::vector<double> x,y,heading; + TPoint real_goal; boost::mutex::scoped_lock lock(mutex_); if (!initialized_) { @@ -212,22 +213,16 @@ bool OpendriveGlobalPlanner::makePlan(const geometry_msgs::PoseStamped& start, c */ try{ ROS_WARN("Make Plan"); - tf::Quaternion q_start(start.pose.orientation.x,start.pose.orientation.y,start.pose.orientation.z,start.pose.orientation.w); - tf::Matrix3x3 m_start(q_start); - m_start.getRPY(roll, pitch, yaw); - ROS_WARN_STREAM("start position: x: " << start.pose.position.x << ", y: " << start.pose.position.y << ", yaw: " << yaw); + yaw=tf2::getYaw(start.pose.orientation); this->roadmap.set_start_point(start.pose.position.x,start.pose.position.y,yaw); - tf::Quaternion q_goal(goal.pose.orientation.x,goal.pose.orientation.y,goal.pose.orientation.z,goal.pose.orientation.w); - tf::Matrix3x3 m_goal(q_goal); - m_goal.getRPY(roll, pitch, yaw); - ROS_WARN_STREAM("start position: x: " << goal.pose.position.x << ", y: " << goal.pose.position.y << ", yaw: " << yaw); - this->roadmap.set_target_point(goal.pose.position.x,goal.pose.position.y,yaw); + yaw=tf2::getYaw(goal.pose.orientation); + real_goal=this->roadmap.set_target_point(goal.pose.position.x,goal.pose.position.y,yaw); this->roadmap.find_shortest_path(); this->roadmap.get_path(path); - for(unsigned int i=0;i<path.size();i++) - std::cout << path[i] << ","; - std::cout << std::endl; - this->roadmap.get_trajectory(x,y); +// for(unsigned int i=0;i<path.size();i++) +// std::cout << path[i] << ","; +// std::cout << std::endl; + this->roadmap.get_trajectory(x,y,heading); plan.resize(x.size()); ros::Time plan_time=ros::Time::now(); for(unsigned int i=0;i<x.size();i++) @@ -237,10 +232,9 @@ bool OpendriveGlobalPlanner::makePlan(const geometry_msgs::PoseStamped& start, c plan[i].pose.position.x=x[i]; plan[i].pose.position.y=y[i]; plan[i].pose.position.y=y[i]; - plan[i].pose.orientation.x=0.0; - plan[i].pose.orientation.y=0.0; - plan[i].pose.orientation.z=0.0; - plan[i].pose.orientation.w=1.0; + tf2::Quaternion orientation; + orientation.setRPY(0.0,0.0,heading[i]); + plan[i].pose.orientation=tf2::toMsg(orientation); } }catch(CException &e){ ROS_ERROR_STREAM(e.what()); -- GitLab