Autoware学习笔记waypoint_follower之wf_simulator
什么是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 topic
if (!initial_set_)
{
loop_rate.sleep();
continue;
}
updateVelocity();
publishOdometry();
loop_rate.sleep();
}
return 0;
}
函数中创建两个句柄，然后声明参数并对参数值进行设置，参数值如launch文件中所示。
建立发布者分别用来发布如下类型的消息:
并返回对应的发布对象。
建立接收者来接收如下类型消息:"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 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;
}
主要内容如函数中介绍计算里程计的发布位置,发送一些transform,最终发布出位姿和速度及汽车状态的内容.
总的看来wf_simulator节点是用来计算模拟速度和位置的,但是我在看代码时遇到一些问题,如current_velocity_的获取及"vehicle_cmd"的回调函数中的公式原理
由于水平有限以上内容仅是自己学习理解过程，各位读者朋友发现问题之处还请帮忙提出，不胜感激。