Autoware学习笔记waypoint_follower之wf

什么是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>

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 topicif (!initial_set_)   {      loop_rate.sleep(); continue;    }updateVelocity();  publishOdometry();loop_rate.sleep(); }return 0;
}

函数中创建两个句柄，然后声明参数并对参数值进行设置，参数值如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;
}

新建了一个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();}}
}

函数中新建一个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;
}

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

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

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

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

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

接收话题"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;
}

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

订阅closest_waypoint最近点的回调函数

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

同样直接用了接收的信息，并把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_;
}

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

下面是发布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 robotstd::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 tfgeometry_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 transformtf_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 transformtf_broadcaster.sendTransform(lidar_trans);// next, we'll publish the odometry message over ROSstd_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 messageodometry_publisher_.publish(ps);velocity_publisher_.publish(ts);vehicle_status_publisher_.publish(vs);last_time = current_time;
}

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

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

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

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

共同交流，共同进步！