C++ (Cpp) Trajectory::getPointsSize Examples

Example #1

File: prefer_straight_cost_function.cpp Project: sammarden/ros-hydro

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;

  }

}

Example #2

File: direction_trajectory_scorer.cpp Project: RIVeR-Lab/fall_risk_assessment

 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;
 }

Example #3

File: heading_cost_function.cpp Project: efernandez/iri_navigation

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;
}