double PreferStraightCostFunction::scoreTrajectory(base_local_planner::Trajectory & traj)
{
double score = 0.0;
double prev_th;
for (int i = 0; i < traj.getPointsSize(); ++i)
{
double x, y, th;
traj.getPoint(i, x, y, th);
if (i == 0)
{
prev_th = th;
continue;
}
score += fabs(th - prev_th);
prev_th = th;
}
}
double DirectionTrajectoryScorer::scoreTrajectory(base_local_planner::Trajectory& traj) {
obstacle_scorer_->prepare();
//ROS_WARN("Scorer After Prepare ROS Inscribed radius=%.4f", costmap_ros_->getInscribedRadius()); //sd1074
double obstacle_cost = obstacle_scorer_->scoreTrajectory(traj);
ROS_WARN("Obstacle cost: %f", obstacle_cost);
if (obstacle_cost < 0)
return obstacle_cost;
double px, py, pth;
traj.getPoint(traj.getPointsSize()-1, px, py, pth);
//ROS_WARN("tx=%f, dx=%f", px, desired_[0]);
double d2 = (px-desired_[0])*(px-desired_[0]) + (py-desired_[1])*(py-desired_[1]);
return obstacle_scale_*obstacle_cost + d2;
}
double HeadingCostFunction::scoreTrajectory(base_local_planner::Trajectory &traj)
{
double dist,x,y,theta,near_dist=DBL_MAX,heading_diff=0.0,diff=0.0;
int interval=traj.getPointsSize()/this->num_points;
unsigned int near_index=0,i,j;
for(j=0;j<this->num_points;j++)
{
traj.getPoint((j+1)*interval-1,x,y,theta);
// find the nearrest point on the path
for(i=1;i<this->global_plan.size();i++)
{
dist=sqrt((this->global_plan[i].pose.position.x-x)*(this->global_plan[i].pose.position.x-x)+
(this->global_plan[i].pose.position.y-y)*(this->global_plan[i].pose.position.y-y));
if(dist<near_dist)
{
near_dist=dist;
near_index=i;
}
}
double v1_x,v1_y;
v1_x = this->global_plan[near_index].pose.position.x - this->global_plan[near_index-1].pose.position.x;
v1_y = this->global_plan[near_index].pose.position.y - this->global_plan[near_index-1].pose.position.y;
double v2_x = cos(theta);
double v2_y = sin(theta);
double perp_dot = v1_x * v2_y - v1_y * v2_x;
double dot = v1_x * v2_x + v1_y * v2_y;
//get the signed angle
diff = fabs(atan2(perp_dot, dot));
if(diff>(3.14159/2.0))
diff=fabs(diff-3.14159);
heading_diff+=diff;
}
return heading_diff;
}