Skip to content
Snippets Groups Projects
Commit 128010b2 authored by Sergi Hernandez's avatar Sergi Hernandez
Browse files

Merge branch 'devel' into 'master'

To compute if a segment is backward or forward, only use the part of the...

See merge request !8
parents de209482 65a741b5
No related branches found
No related tags found
1 merge request!8To compute if a segment is backward or forward, only use the part of the...
......@@ -97,7 +97,11 @@ bool AckermannPlannerUtil::get_goal(geometry_msgs::PoseStamped& goal_pose)
return base_local_planner::getGoalPose(*tf_,global_plan_,global_frame_,goal_pose);
}
bool AckermannPlannerUtil::set_plan(const std::vector<geometry_msgs::PoseStamped>& orig_global_plan) {
bool AckermannPlannerUtil::set_plan(const std::vector<geometry_msgs::PoseStamped>& orig_global_plan)
{
unsigned int last_index;
double start_x,start_y,distance,costmap_size;
if(!initialized_)
{
ROS_ERROR("AckermannPlannerUtil: planner utils have not been initialized, please call initialize() first");
......@@ -108,8 +112,7 @@ bool AckermannPlannerUtil::set_plan(const std::vector<geometry_msgs::PoseStamped
forward_segment.clear();
subPathIndex = 0;
double pathLength = 0, yaw;
for(unsigned int i = 0; i < orig_global_plan.size(); ++i)
std::cout << orig_global_plan[i].pose.position.x << "," << orig_global_plan[i].pose.position.y << "," << tf::getYaw(orig_global_plan[i].pose.orientation) << std::endl;
if(orig_global_plan.size()<5)
{
for(unsigned int i = 0; i < orig_global_plan.size(); ++i)
......@@ -117,6 +120,10 @@ bool AckermannPlannerUtil::set_plan(const std::vector<geometry_msgs::PoseStamped
}
else
{
start_x=orig_global_plan[0].pose.position.x;
start_y=orig_global_plan[0].pose.position.y;
last_index=0;
costmap_size=(this->costmap_->getSizeInMetersX()+this->costmap_->getSizeInMetersY())/2.0;
////divide plan by manuveurs
for(unsigned int i = 0; i < orig_global_plan.size(); ++i)
{
......@@ -158,7 +165,7 @@ bool AckermannPlannerUtil::set_plan(const std::vector<geometry_msgs::PoseStamped
yaw=tf::getYaw(subPath[0].pose.orientation);
if(yaw<0.0)
yaw+=3.14159*2.0;
if(((subPath[subPath.size()-1].pose.position.x-subPath[0].pose.position.x)*cos(yaw)+(subPath[subPath.size()-1].pose.position.y-subPath[0].pose.position.y)*sin(yaw))<0.0)
if(((subPath[last_index].pose.position.x-subPath[0].pose.position.x)*cos(yaw)+(subPath[last_index].pose.position.y-subPath[0].pose.position.y)*sin(yaw))<0.0)
{
std::cout << "backward segment" << std::endl;
this->forward_segment.push_back(false);
......@@ -168,6 +175,9 @@ bool AckermannPlannerUtil::set_plan(const std::vector<geometry_msgs::PoseStamped
std::cout << "forward segment" << std::endl;
this->forward_segment.push_back(true);
}
start_x=subPath[subPath.size()-1].pose.position.x;
start_y=subPath[subPath.size()-1].pose.position.y;
last_index=0;
subPathList.push_back(subPath);
}
else //ignored subpath, too short
......@@ -178,6 +188,12 @@ bool AckermannPlannerUtil::set_plan(const std::vector<geometry_msgs::PoseStamped
pathLength=0.0;
}
}
else
{
distance=sqrt(pow(start_x-orig_global_plan[i].pose.position.x,2.0)+pow(start_y-orig_global_plan[i].pose.position.y,2.0));
if(distance<costmap_size)
last_index++;
}
}
subPath.push_back(orig_global_plan[i]);
}
......@@ -185,7 +201,7 @@ bool AckermannPlannerUtil::set_plan(const std::vector<geometry_msgs::PoseStamped
yaw=tf::getYaw(subPath[0].pose.orientation);
if(yaw<0.0)
yaw+=3.14159*2.0;
if(((subPath[subPath.size()-1].pose.position.x-subPath[0].pose.position.x)*cos(yaw)+(subPath[subPath.size()-1].pose.position.y-subPath[0].pose.position.y)*sin(yaw))<0.0)
if(((subPath[last_index].pose.position.x-subPath[0].pose.position.x)*cos(yaw)+(subPath[last_index].pose.position.y-subPath[0].pose.position.y)*sin(yaw))<0.0)
{
std::cout << "backward segment" << std::endl;
this->forward_segment.push_back(false);
......
0% Loading or .
You are about to add 0 people to the discussion. Proceed with caution.
Finish editing this message first!
Please register or to comment