Beispiel #1
0
void Trajectory2d::shortcut()
{
  if (m_trajectory.empty())
  {
    return;
  }

  /* Transform each 'following' pose into the cs
   * of the 'current' pose. A violation to the desired
   * driving direction is found if
   *    x-value <= 0 and driving forward
   *    x-value >= 0 and driving backward
   */
  Pose2d temp;
  for (ContainerType::iterator current=m_trajectory.begin(),
       next=current+1;
       (current!=m_trajectory.end() && next!=m_trajectory.end());
       ++current, ++next)
  {
    temp = current->pose().inverse() * next->pose();
    if (((m_is_forward_trajectory && (temp.translation().x() <=0.)))
        || ((!m_is_forward_trajectory) && (temp.translation().x() >=0.)))
    {
      erase(next);
      --current; // step back to check the same place again
      next = current+1;
    }
  }

  // recalculate curvature.
  if (curvatureAvailable())
  {
    calculateCurvature();
  }
}
Position2d PositionController::getStopLineReachedPose(const Pose2d& junction_start_pose) const
{
  Pose2d diff;
  PoseTraits<Pose2d>::fromPositionAndOrientationRPY(diff, -0.6, -0.23, 0.0);

  const Pose2d stop_pose = junction_start_pose * diff;

  ExtendedPose2d projection;
  double distance;
  std::size_t index;

  m_lateral_controller.calculateProjection(m_lateral_controller.getTrajectory(), stop_pose.translation(), projection, distance, index);

  return projection.getPosition();
}