C++ Trajectory::getPointsSize方法代码示例

  /**
   * return a score for trajectory traj
   */
  virtual double scoreTrajectory(Trajectory &traj)
  { 
      if(traj.getPointsSize()==0)
        return 0.0;

      double px, py, pth;
      traj.getPoint(0, px, py, pth);
      
      double dist_to_goal = hypot(px-goal_x_, py-goal_y_);
      if(dist_to_goal < .2){
        return 0.0;
      }

      
      double start_diff;
      if(!getAngleDiff(px,py,pth,&start_diff))
        return -4.0;

      if(fabs(start_diff) < max_trans_angle_){
	return 0.0;
      }

      if(fabs(traj.xv_) > 0.0 || fabs(traj.yv_) > 0.0)
        return -3.0;
      else if( sign(start_diff) != sign(traj.thetav_) )
        return -2.0;
        
      traj.getPoint(traj.getPointsSize()-1, px, py, pth);
      double end_diff;
      if(!getAngleDiff(px,py,pth,&end_diff))
        return -1.0;
        
      return fabs(end_diff);
  }

void CalibrateAction::visualize_trajectory(Trajectory &traj)
{
    // publishing to rviz
    geometry_msgs::PoseArray poses;
    for(unsigned int i=0; i<traj.getPointsSize();i++)
    {
        geometry_msgs::Pose temp_pose;
        double x_, y_,th_;
        traj.getPoint(i, x_, y_, th_);
        tf::quaternionTFToMsg(tf::createQuaternionFromYaw(th_),temp_pose.orientation);
        temp_pose.position.x = x_;
        temp_pose.position.y = y_;
        poses.poses.push_back(temp_pose);
        //ROS_INFO("Pose: x: %f y: %f th: %f", x_, y_, th_);
    }
    geometry_msgs::Pose temp_pose, temp_pose2;
    double x_, y_,th_;
    traj.getPoint(0, x_, y_, th_);
    tf::quaternionTFToMsg(tf::createQuaternionFromYaw(th_),temp_pose.orientation);
    temp_pose.position.x = x_;
    temp_pose.position.y = y_;
    traj.getEndpoint(x_, y_, th_);
    tf::quaternionTFToMsg(tf::createQuaternionFromYaw(th_),temp_pose2.orientation);
    temp_pose2.position.x = x_;
    temp_pose2.position.y = y_;
    //ROS_INFO("Visualize trajectory with %i points. First Point (%2.1f,%2.1f), last Point (%2.1f,%2.1f)", traj.getPointsSize(),temp_pose.position.x,temp_pose.position.y,temp_pose2.position.x,temp_pose2.position.y);
    poses.header.frame_id = cost_map->getGlobalFrameID();;
    estTraj_pub.publish(poses);
}

double ObstacleCostFunction::scoreTrajectory(Trajectory &traj) {
  double cost = 0;
  double scale = getScalingFactor(traj, scaling_speed_, max_trans_vel_, max_scaling_factor_);
  double px, py, pth;
  if (footprint_spec_.size() == 0) {
    // Bug, should never happen
    ROS_ERROR("Footprint spec is empty, maybe missing call to setFootprint?");
    return -9;
  }

  for (unsigned int i = 0; i < traj.getPointsSize(); ++i) {
    traj.getPoint(i, px, py, pth);
    double f_cost = footprintCost(px, py, pth,
        scale, footprint_spec_,
        costmap_, world_model_);

    if(f_cost < 0){
        return f_cost;
    }

    if(sum_scores_)
        cost +=  f_cost;
    else
        cost = f_cost;
  }
  return cost;
}

double PathOrientationCostFunction::scoreTrajectory(Trajectory &traj) {
  if(traj.getPointsSize()==0)
    return 0.0;

  double px, py, pth;
  traj.getPoint(traj.getPointsSize()-1, px, py, pth);

  unsigned int cell_x, cell_y;
  //we won't allow trajectories that go off the map... shouldn't happen that often anyways
  if ( ! costmap_->worldToMap(px, py, cell_x, cell_y)) {
      //we're off the map
      ROS_WARN("Off Map %f, %f", px, py);
      return -4.0;
  }

  unsigned int path_index = map_(cell_x, cell_y).index;
  if(path_index>=yaws_.size())
   return 0.0;
  double diff = fabs(angles::shortest_angular_distance(pth, yaws_[path_index]));
  if(diff > max_trans_angle_ && (traj.xv_ > 0.0 || traj.yv_ > 0.0))
    return -1.0;
  else
    return diff;
}

bool CalibrateAction::checkTrajectory(Trajectory& traj)
{
    unsigned int size = traj.getPointsSize();
    float smallest_dist = INFINITY;
    bool isValid;
    for(unsigned int i = 0; i< size;i++)
    {
        geometry_msgs::Pose tempPose;
        tf::Pose tfPose;
        traj.getPoint(i,tempPose.position.x, tempPose.position.y, tempPose.orientation.z);
        tf::poseMsgToTF(tempPose, tfPose);
        float dist = getDistanceAtPose(tfPose, &isValid);
        // to be able to inform about closest distance
        if (dist < smallest_dist)
        {
            smallest_dist = dist;
        }
        if ((dist < traj_dist_threshold) && (dist != -INFINITY) && (dist != INFINITY)) // prevent negative infinity value from crashing us
        {
            ROS_INFO("Critical distance was %f at point %i, point valid: %i", dist, i, isValid);
            return false;
        }
    }
    //ROS_INFO("Trajectory check successful, smallest distance was: %f", smallest_dist);
    return true;

    /*
    unsigned int size = traj.getPointsSize();
   float smallest_dist = INFINITY;
    for(unsigned int i = 0; i< size;i++)
    {
        geometry_msgs::Pose testPose;
        traj.getPoint(i,testPose.position.x, testPose.position.y, testPose.orientation.z);
        float dist = voronoi_.getDistance(testPose.position.x, testPose.position.y); // get closest distance from trajectory point to an obstacle
        dist *= costmap_.getResolution(); // distance now in meters
        if (dist < smallest_dist)
        {
            smallest_dist = dist;
        }
        if ((dist < traj_dist_threshold) && (dist != -INFINITY) && (dist != INFINITY)) // prevent negative infinity value from crashing us
        {
            ROS_INFO("Critical distance was %f at point %i", dist, i);
            return false;
        }
    }
    */
}

double MapGridCostFunction::scoreTrajectory(Trajectory &traj) {
  double cost = 0.0;
  if (aggregationType_ == Product) {
    cost = 1.0;
  }
  double px, py, pth;
  double grid_dist;

  for (unsigned int i = 0; i < traj.getPointsSize(); ++i) {
    traj.getPoint(i, px, py, pth);
    grid_dist = scoreCell(px, py, pth);
    if(stop_on_failure_){
      if (grid_dist == map_.obstacleCosts()) {
        return -3.0;
      } else if (grid_dist == map_.unreachableCellCosts()) {
        return -2.0;
      }
    }

    switch( aggregationType_ ) {
    case Last:
      cost = grid_dist;
      break;
    case Sum:
      cost += grid_dist;
      break;
    case Product:
      if (cost > 0) {
        cost *= grid_dist;
      }
      break;
    }
  }

  double factor = costmap_->getResolution() * 0.5;
  return cost * factor;
}