Autoware学习笔记waypoint_follower之wf_simulator_way point_follower

什么是wf_simuator

• 使用接收到的车辆控制信号（v，ω）模拟理想的自身位置和速度

wf_simulator.launch文件

<!-- -->
<launch>
  <arg name="initialize_source" default="Rviz"/>  
  <arg name="accel_rate" default="1.0"/>  
  <arg name="angle_error" default="0.0"/>  
  <arg name="position_error" default="0.0"/>  
  <arg name="lidar_height" default="1.0"/>  
  <arg name="use_ctrl_cmd" default="false" />  
  <node pkg="waypoint_follower" type="wf_simulator" name="wf_simulator" output="screen">    
  <param name="initialize_source" value="$(arg initialize_source)"/>    
  <param name="accel_rate" value="$(arg accel_rate)"/>    
  <param name="angle_error" value="$(arg angle_error)"/>    
  <param name="position_error" value="$(arg position_error)"/>    
  <param name="lidar_height" value="$(arg lidar_height)"/>  
  <param name="use_ctrl_cmd" value="$(arg use_ctrl_cmd)"/>  
  </node>
</launch>
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17

launch文件只有一个waypoint_follower功能包下的wf_simulator节点，设置了一些参数并对其进行赋值。

wf_simluator节点

节点的主函数如下所示:

int main(int argc, char** argv)
{  
  ros::init(argc, argv, "wf_simulator");
  ros::NodeHandle nh;  
  ros::NodeHandle private_nh("~");
  std::string initialize_source;
  private_nh.getParam("initialize_source", initialize_source);
  ROS_INFO_STREAM("initialize_source : " << initialize_source);
  double accel_rate;  
  private_nh.param("accel_rate", accel_rate, double(1.0));  
  ROS_INFO_STREAM("accel_rate : " << accel_rate);
  private_nh.param("position_error", position_error_, double(0.0));  
  private_nh.param("angle_error", angle_error_, double(0.0));  
  private_nh.param("lidar_height", lidar_height_, double(1.0));
  private_nh.param("use_ctrl_cmd", use_ctrl_cmd, false);
  nh.param("vehicle_info/wheel_base", wheel_base_, double(2.7));
  // publish topic  
  odometry_publisher_ = nh.advertise<geometry_msgs::PoseStamped>("sim_pose", 10);  
  velocity_publisher_ = nh.advertise<geometry_msgs::TwistStamped>("sim_velocity", 10);  
  vehicle_status_publisher_ = nh.advertise<autoware_msgs::VehicleStatus>("/vehicle_status", 10);
  
  // subscribe topic  
  ros::Subscriber cmd_subscriber =      nh.subscribe<autoware_msgs::VehicleCmd>("vehicle_cmd", 10, boost::bind(CmdCallBack, _1, accel_rate));
  ros::Subscriber waypoint_subcscriber = nh.subscribe("base_waypoints", 10, waypointCallback);
  ros::Subscriber closest_sub = nh.subscribe("closest_waypoint", 10, callbackFromClosestWaypoint); 
  ros::Subscriber initialpose_subscriber;
  if (initialize_source == "Rviz")  
  {    
    initialpose_subscriber = nh.subscribe("initialpose", 10, initialposeCallback);  
  }  
  else if (initialize_source == "lidar_localizer")
  {   
    initialpose_subscriber = nh.subscribe("ndt_pose", 10, callbackFromPoseStamped);  
  }  
  else if (initialize_source == "GNSS") 
  {    
    initialpose_subscriber = nh.subscribe("gnss_pose", 10, callbackFromPoseStamped); 
  }  
  else  
  {    
    ROS_INFO("Set pose initializer!!");
  }
  ros::Rate loop_rate(LOOP_RATE);  while (ros::ok()) 
  {    
    ros::spinOnce();  
   // check subscribe topic
    if (!initial_set_)   
    {      
      loop_rate.sleep(); 
      continue;    
    }
    updateVelocity();  
    publishOdometry();
    loop_rate.sleep(); 
  }
  return 0;
}
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57

函数中创建两个句柄，然后声明参数并对参数值进行设置，参数值如launch文件中所示。
建立发布者分别用来发布如下类型的消息:
<geometry_msgs::PoseStamped>"sim_pose"
<geometry_msgs::TwistStamped>"sim_velocity"
<autoware_msgs::VehicleStatus>"/vehicle_status"
并返回对应的发布对象。
建立接收者来接收如下类型消息:<autoware_msgs::VehicleCmd>"vehicle_cmd"、"base_waypoints"、"closest_waypoint"
并返回对应的接收对象。
还单声明了一个initialpose_subscriber接收对象，通过判断位置初始化方式initialize_source的值来确定其接收内容。
如代码中有如下三种种位姿初始化的方式:"Rviz"、"lidar_localizer"、"GNSS"

第一种初始化方式：RVIZ

当初始位姿方式是"lRVIZ"时(可视化界面中人为估计位置通过2D Pose Estimate工具),其接收的是"initialpose"话题消息回调函数如下:

void initialposeCallback(const geometry_msgs::PoseWithCovarianceStampedConstPtr& input)
{
  tf::StampedTransform transform;
  getTransformFromTF(MAP_FRAME, input->header.frame_id, transform);

  initial_pose_.position.x = input->pose.pose.position.x + transform.getOrigin().x();
  initial_pose_.position.y = input->pose.pose.position.y + transform.getOrigin().y();
  initial_pose_.position.z = input->pose.pose.position.z + transform.getOrigin().z();
  initial_pose_.orientation = input->pose.pose.orientation;

  initial_set_ = true;
  pose_set_ = false;
}
1
2
3
4
5
6
7
8
9
10
11
12
13

新建了一个transform对象。然后调用 getTransformFromTF()函数，函数内容如下:

void getTransformFromTF(const std::string parent_frame, const std::string child_frame, tf::StampedTransform& transform)
{
  static tf::TransformListener listener;

  while (1)
  {
    try
    {
      listener.lookupTransform(parent_frame, child_frame, ros::Time(0), transform);
      break;
    }
    catch (tf::TransformException ex)
    {
      ROS_ERROR("%s", ex.what());
      ros::Duration(1.0).sleep();
    }
  }
}
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18

函数中新建一个TransformListener对象listener，一旦创建了侦听器，它将开始通过网络接收tf转换接收对象，并通过while循环，使用try()和catch()用来捕捉可能的异常，lookupTransform()函数访问tf树中最新可用的从RVIZ输入的坐标到MAP_FRAME的转换。
再回到initialposeCallback回调函数看，接着计算初始化位置在整体坐标系中的位置和放向。
对initial_set_ 参数设置为 true、 pose_set_设为 false;

#第二种位置初始化方式：lidar_localizer

当初始位姿方式是"lidar_localizer"时(激光雷达定位方式),其接收的是"ndt_pose"话题消息回调函数如下:

void callbackFromPoseStamped(const geometry_msgs::PoseStampedConstPtr& msg)
{
  initial_pose_ = msg->pose;
  initial_set_ = true;
}
1
2
3
4
5

设定initial_pose为接收的消息值,并设定initial_set_参数为true .

第三种位置初始化方式：GNSS

当初始位姿方式是"GNSS"时(一般是GPS定位),其接收的是"gnss_pose"话题消息回调函数如下:

void callbackFromPoseStamped(const geometry_msgs::PoseStampedConstPtr& msg)
{
  initial_pose_ = msg->pose;
  initial_set_ = true;
}
1
2
3
4
5

同lidar_localizer的定位方式是相同的回调处理函数设定initial_set_ = true。
初始位置的获取到此便完成了。

处理消息的回调函数

回到主函数下while（ros::ok）接着处理订阅的消息有如下回调函数：
订阅"vehicle_cmd"的回调函数

void CmdCallBack(const autoware_msgs::VehicleCmdConstPtr& msg, double accel_rate)
{  
  if (use_ctrl_cmd == true)  
  {    
    linear_acceleration_ = msg->ctrl_cmd.linear_acceleration;    
    steering_angle_ = msg->ctrl_cmd.steering_angle;  
  }  
  else  
  {    
    static double previous_linear_velocity = 0;
    if (current_velocity_.linear.x < msg->twist_cmd.twist.linear.x)    
    {      
      current_velocity_.linear.x = previous_linear_velocity + accel_rate / (double)LOOP_RATE;
      if (current_velocity_.linear.x > msg->twist_cmd.twist.linear.x)      
      {        
        current_velocity_.linear.x = msg->twist_cmd.twist.linear.x;
      } 
    }
    else    
    {      
      current_velocity_.linear.x = previous_linear_velocity - accel_rate / (double)LOOP_RATE;
      if (current_velocity_.linear.x < msg->twist_cmd.twist.linear.x)      
      {        
        current_velocity_.linear.x = msg->twist_cmd.twist.linear.x;
      }   
    }
      previous_linear_velocity = current_velocity_.linear.x;
      current_velocity_.angular.z = msg->twist_cmd.twist.angular.z;
    //current_velocity_ = msg->twist;  
  }
}
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31

接收话题"vehicle_cmd"的autoware_msgs::VehicleCmd消息。如果use_ctrl_cmd的值为true（使用控制命令），则直接把接受的控制命令的相关之赋给 linear_acceleration_（加速度）和steering_angle_ （转角），但是此节点中不使用控制命令，接着判断当前速度与指令速度之间的关系,按条件赋值/(有点不明白的地方请大佬指教,current_velocity_.linear.x当前速度哪里来的)

订阅"base_waypoints"的回调函数：

void waypointCallback(const autoware_msgs::LaneConstPtr& msg)
{  
  current_waypoints_.setPath(*msg);  
  waypoint_set_ = true；
}
  void setPath(const autoware_msgs::Lane &waypoints)  
{    
  current_waypoints_ = waypoints;
}
1
2
3
4
5
6
7
8
9

把接收到的路径信息直接用做当前lu’jing’d路径点，并设置参数waypoint_set_为true。

订阅closest_waypoint最近点的回调函数

void callbackFromClosestWaypoint(const std_msgs::Int32ConstPtr& msg)
{  
  closest_waypoint_ = msg->data;  
  is_closest_waypoint_subscribed_ = true;
}
1
2
3
4
5

同样直接用了接收的信息，并把is_closest_waypoint_subscribed_参数设置为true

更新速度

void updateVelocity()
{
  if (use_ctrl_cmd == false)
    return;

  current_velocity_.linear.x += linear_acceleration_ / (double)LOOP_RATE;
  current_velocity_.angular.z = current_velocity_.linear.x * std::sin(steering_angle_) / wheel_base_;
}
1
2
3
4
5
6
7
8

上面是实现速度的更新,和角速度更新.

下面是发布odometry的函数:

void publishOdometry()
{
  static ros::Time current_time = ros::Time::now();
  static ros::Time last_time = ros::Time::now();
  static geometry_msgs::Pose pose;
  static double th = 0;
  static tf::TransformBroadcaster tf_broadcaster;
  static double steering_angle = 0.0;

  if (!pose_set_)
  {
    pose.position = initial_pose_.position;
    pose.orientation = initial_pose_.orientation;
    th = tf::getYaw(pose.orientation);
    ROS_INFO_STREAM("pose set : (" << pose.position.x << " " << pose.position.y << " " << pose.position.z << " " << th
                                   << ")");
    pose_set_ = true;
  }

  if (waypoint_set_ && is_closest_waypoint_subscribed_)
    pose.position.z = current_waypoints_.getWaypointPosition(closest_waypoint_).z;
  double vx = current_velocity_.linear.x;
  double vth = current_velocity_.angular.z;
  current_time = ros::Time::now();

  // compute odometry in a typical way given the velocities of the robot
  std::random_device rnd;
  std::mt19937 mt(rnd());
  std::uniform_real_distribution<double> rnd_dist(0.0, 2.0);
  double rnd_value_x = rnd_dist(mt) - 1.0;
  double rnd_value_y = rnd_dist(mt) - 1.0;
  double rnd_value_th = rnd_dist(mt) - 1.0;

  double dt = (current_time - last_time).toSec();
  double delta_x = (vx * cos(th)) * dt + rnd_value_x * position_error_;
  double delta_y = (vx * sin(th)) * dt + rnd_value_y * position_error_;
  double delta_th = vth * dt + rnd_value_th * angle_error_ * M_PI / 180;

  pose.position.x += delta_x;
  pose.position.y += delta_y;
  th += delta_th;
  pose.orientation = tf::createQuaternionMsgFromYaw(th);

  // first, we'll publish the transform over tf
  geometry_msgs::TransformStamped odom_trans;
  odom_trans.header.stamp = current_time;
  odom_trans.header.frame_id = MAP_FRAME;
  odom_trans.child_frame_id = SIMULATION_FRAME;

  odom_trans.transform.translation.x = pose.position.x;
  odom_trans.transform.translation.y = pose.position.y;
  odom_trans.transform.translation.z = pose.position.z;
  odom_trans.transform.rotation = pose.orientation;

  // send odom transform
  tf_broadcaster.sendTransform(odom_trans);

  geometry_msgs::TransformStamped lidar_trans;
  lidar_trans.header.stamp = odom_trans.header.stamp;
  lidar_trans.header.frame_id = SIMULATION_FRAME;
  lidar_trans.child_frame_id = LIDAR_FRAME;
  lidar_trans.transform.translation.z += lidar_height_;
  lidar_trans.transform.rotation.w = 1;

  // send lidar transform
  tf_broadcaster.sendTransform(lidar_trans);

  // next, we'll publish the odometry message over ROS
  std_msgs::Header h;
  h.stamp = current_time;
  h.frame_id = MAP_FRAME;

  geometry_msgs::PoseStamped ps;
  ps.header = h;
  ps.pose = pose;

  geometry_msgs::TwistStamped ts;
  ts.header = h;
  ts.twist.linear.x = vx;
  ts.twist.angular.z = vth;

  autoware_msgs::VehicleStatus vs;
  vs.header = h;
  vs.header.frame_id = "/can";
  vs.speed = vx * 3.6; // [m/s] to [km/h]
  if (std::fabs(vx) > 1.0E-2)
  {
    steering_angle = std::atan(vth * wheel_base_ / vx); // [rad]
  }
  vs.angle = steering_angle;

  // publish the message
  odometry_publisher_.publish(ps);
  velocity_publisher_.publish(ts);
  vehicle_status_publisher_.publish(vs);

  last_time = current_time;
}
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88
89
90
91
92
93
94
95
96
97
98

主要内容如函数中介绍计算里程计的发布位置,发送一些transform,最终发布出位姿和速度及汽车状态的内容.

总的看来wf_simulator节点是用来计算模拟速度和位置的,但是我在看代码时遇到一些问题,如current_velocity_的获取及"vehicle_cmd"的回调函数中的公式原理

由于水平有限以上内容仅是自己学习理解过程，各位读者朋友发现问题之处还请帮忙提出，不胜感激。

有问题还请指正，不胜感激。

共同交流，共同进步！