Autoware-Pure_pursuit代码解析

// calculation relative coordinate of point from current_pose frame

计算全局坐标系相对于机器人坐标系中的位置,即point相对于current_pose坐标系的位置

公式参考，R=L^2/(2*y),论文--一种纯追踪模型改进算法

geometry_msgs::Point calcRelativeCoordinate(geometry_msgs::Point point_msg, geometry_msgs::Pose current_pose)
{
  tf::Transform inverse;
  tf::poseMsgToTF(current_pose, inverse);
  tf::Transform transform = inverse.inverse();

  tf::Point p;
  pointMsgToTF(point_msg, p);
  tf::Point tf_p = transform * p;
  geometry_msgs::Point tf_point_msg;
  pointTFToMsg(tf_p, tf_point_msg);

  return tf_point_msg;

}

例如：

    geometry_msgs::Point point_msg;

    point_msg.x=1;
    point_msg.y=2;
    point_msg.z=0;

    geometry_msgs::Pose current_pose;
    current_pose.position.x=4;
    current_pose.position.y=7;
    current_pose.position.z=0;
    current_pose.orientation.x=0;
    current_pose.orientation.y=0;
    current_pose.orientation.z=0;
    current_pose.orientation.w=1;

计算结果为

x: -3
y: -5i
z: 0

一、设置机器人的预瞄距离
 pp_.setLookaheadDistance(computeLookaheadDistance());

 

pp_为purepursuit类的实例化对象，setLookaheadDistance为设置lookahead_distance_ 

  函数computeLookaheadDistance()为PurePursuitNode类的函数，内容如下：

double PurePursuitNode::computeLookaheadDistance() const

{
  if (param_flag_ == enumToInteger(Mode::dialog))//从runtimemanage对话框获取数据
    return const_lookahead_distance_;

  double maximum_lookahead_distance = current_linear_velocity_ * 10;//最大预瞄距离为当前速度的10倍
  double ld = current_linear_velocity_ * lookahead_distance_ratio_;//通过lookahead_distance_ratio_调成预瞄距离的值

  return ld < minimum_lookahead_distance_ ? minimum_lookahead_distance_//最小预瞄距离在PurePursuitNode中被设置如下minimum_lookahead_distance_(6.0)
        : ld > maximum_lookahead_distance ? maximum_lookahead_distance
        : ld;
}

二、计算机器人的曲率

bool can_get_curvature = pp_.canGetCurvature(&kappa);//计算曲率

canGetCurvature函数计算机器人的曲率

bool PurePursuit::canGetCurvature(double *output_kappa)
{
  // search next waypoint
  getNextWaypoint();//要比lookahead_distance_大才有效
  if (next_waypoint_number_ == -1)
  {
    ROS_INFO("lost next waypoint");
    return false;
  }

  // if is_linear_interpolation_ is false or next waypoint is first or last
  if (!is_linear_interpolation_ || next_waypoint_number_ == 0 ||
      next_waypoint_number_ == (static_cast(current_waypoints_.size() - 1)))
  {
    next_target_position_ = current_waypoints_.at(next_waypoint_number_).pose.pose.position;//获取下一路径点的position
    *output_kappa = calcCurvature(next_target_position_);//计算下一个路径点和机器人当前位置规划圆弧的曲率，参考公式R=L^2/(2*x)
    return true;
  }

  // linear interpolation and calculate angular velocity,计算路径点之间的插值
  bool interpolation = interpolateNextTarget(next_waypoint_number_, &next_target_position_);

  if (!interpolation)
  {
    ROS_INFO_STREAM("lost target! ");
    return false;
  }

  // ROS_INFO("next_target : ( %lf , %lf , %lf)", next_target.x, next_target.y,next_target.z);

  *output_kappa = calcCurvature(next_target_position_);
  return true;
}

canGetCurvature函数中getNextWaypoint为获取下一个路径点，其中核心代码如下：

 // if there exists an effective waypoint 不考虑最开始和最后一个点，只有当路径中的点与机器人当前位置点大于预瞄距离时才会有效
    if (getPlaneDistance(current_waypoints_.at(i).pose.pose.position, current_pose_.position) > lookahead_distance_)
    {
      next_waypoint_number_ = i;
      return;
    }

三、publishTwistStamped(can_get_curvature, kappa);//根据用户的发布的路径点，更新current waypoint ，读取用户设定的速度，发布机器人的速度函数（线速度和角度）

函数如下：

void PurePursuitNode::publishTwistStamped(const bool &can_get_curvature, const double &kappa) const
{
  geometry_msgs::TwistStamped ts;
  ts.header.stamp = ros::Time::now();
  ts.twist.linear.x = can_get_curvature ? computeCommandVelocity() : 0;//给定线速度
  ts.twist.angular.z = can_get_curvature ? kappa * ts.twist.linear.x : 0;//给定角速度
  pub1_.publish(ts);//发布话题nh_.advertise("twist_raw", 10);
}

其中computeCommandVelocity（）函数如下

double PurePursuitNode::computeCommandVelocity() const
{
  if (param_flag_ == enumToInteger(Mode::dialog))
    return kmph2mps(const_velocity_);

  return command_linear_velocity_;
}

command_linear_velocity_通sub1_ = nh_.subscribe("final_waypoints", 10, &PurePursuitNode::callbackFromWayPoints, this);通过final_waypoints话题订阅获得，该话题为用户发布

void PurePursuitNode::callbackFromWayPoints(const waypoint_follower::laneConstPtr &msg)
{
  if (!msg->waypoints.empty())
    command_linear_velocity_ = msg->waypoints.at(0).twist.twist.linear.x;//获取用户设置路径点中的线速度
  else
    command_linear_velocity_ = 0;

  pp_.setCurrentWaypoints(msg->waypoints);//对pure_suit的成员变量std::vector current_waypoints_赋值值
  is_waypoint_set_ = true;//表明接收到了用户的路径设置点
}

四、publishControlCommandStamped(can_get_curvature, kappa);//在话题ctrl_cmd发布机器人的速度和转向角度的消息

函数如下
void PurePursuitNode::publishControlCommandStamped(const bool &can_get_curvature, const double &kappa) const
{
  if (!publishes_for_steering_robot_)
    return;

  waypoint_follower::ControlCommandStamped ccs;
  ccs.header.stamp = ros::Time::now();
  ccs.cmd.linear_velocity = can_get_curvature ? computeCommandVelocity() : 0;//计算机器人的线速度
  ccs.cmd.steering_angle = can_get_curvature ? convertCurvatureToSteeringAngle(wheel_base_, kappa) : 0;//计算机器人的转角，wheel_base为车的轴距

  pub2_.publish(ccs);
}

其中函数convertCurvatureToSteeringAngle为

return atan(wheel_base * kappa);//参考公式theta=arctan(H/R)