Commit 7ca6636c authored by Sergi Hernandez's avatar Sergi Hernandez
Browse files

The returned path now includes the heading for each point.

parent b03fa0ad
...@@ -39,7 +39,7 @@ ...@@ -39,7 +39,7 @@
#include <pluginlib/class_list_macros.h> #include <pluginlib/class_list_macros.h>
#include <costmap_2d/cost_values.h> #include <costmap_2d/cost_values.h>
#include <costmap_2d/costmap_2d.h> #include <costmap_2d/costmap_2d.h>
#include <tf/transform_datatypes.h> #include <tf2/utils.h>
#include "exceptions.h" #include "exceptions.h"
...@@ -162,9 +162,10 @@ bool OpendriveGlobalPlanner::makePlan(const geometry_msgs::PoseStamped& start, c ...@@ -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) 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<unsigned int> path;
std::vector<double> x,y; std::vector<double> x,y,heading;
TPoint real_goal;
boost::mutex::scoped_lock lock(mutex_); boost::mutex::scoped_lock lock(mutex_);
if (!initialized_) { if (!initialized_) {
...@@ -212,22 +213,16 @@ bool OpendriveGlobalPlanner::makePlan(const geometry_msgs::PoseStamped& start, c ...@@ -212,22 +213,16 @@ bool OpendriveGlobalPlanner::makePlan(const geometry_msgs::PoseStamped& start, c
*/ */
try{ try{
ROS_WARN("Make Plan"); ROS_WARN("Make Plan");
tf::Quaternion q_start(start.pose.orientation.x,start.pose.orientation.y,start.pose.orientation.z,start.pose.orientation.w); yaw=tf2::getYaw(start.pose.orientation);
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);
this->roadmap.set_start_point(start.pose.position.x,start.pose.position.y,yaw); 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); yaw=tf2::getYaw(goal.pose.orientation);
tf::Matrix3x3 m_goal(q_goal); real_goal=this->roadmap.set_target_point(goal.pose.position.x,goal.pose.position.y,yaw);
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);
this->roadmap.find_shortest_path(); this->roadmap.find_shortest_path();
this->roadmap.get_path(path); this->roadmap.get_path(path);
for(unsigned int i=0;i<path.size();i++) // for(unsigned int i=0;i<path.size();i++)
std::cout << path[i] << ","; // std::cout << path[i] << ",";
std::cout << std::endl; // std::cout << std::endl;
this->roadmap.get_trajectory(x,y); this->roadmap.get_trajectory(x,y,heading);
plan.resize(x.size()); plan.resize(x.size());
ros::Time plan_time=ros::Time::now(); ros::Time plan_time=ros::Time::now();
for(unsigned int i=0;i<x.size();i++) for(unsigned int i=0;i<x.size();i++)
...@@ -237,10 +232,9 @@ bool OpendriveGlobalPlanner::makePlan(const geometry_msgs::PoseStamped& start, c ...@@ -237,10 +232,9 @@ bool OpendriveGlobalPlanner::makePlan(const geometry_msgs::PoseStamped& start, c
plan[i].pose.position.x=x[i]; plan[i].pose.position.x=x[i];
plan[i].pose.position.y=y[i]; plan[i].pose.position.y=y[i];
plan[i].pose.position.y=y[i]; plan[i].pose.position.y=y[i];
plan[i].pose.orientation.x=0.0; tf2::Quaternion orientation;
plan[i].pose.orientation.y=0.0; orientation.setRPY(0.0,0.0,heading[i]);
plan[i].pose.orientation.z=0.0; plan[i].pose.orientation=tf2::toMsg(orientation);
plan[i].pose.orientation.w=1.0;
} }
}catch(CException &e){ }catch(CException &e){
ROS_ERROR_STREAM(e.what()); ROS_ERROR_STREAM(e.what());
......
Supports Markdown
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