Auto Ware 代码解析系列-can_info_translator节点

节点订阅vehicle_receiver发布过来的/can_info话题，主节点如下：

// ROS includes#include <ros/ros.h>#include "can_info_translator_core.h"int main(int argc, char **argv){  ros::init(argc, argv, "can_info_translator");  autoware_connector::VelPoseConnectNode n;  n.run();  return 0;}

代码中autoware_connector::VelPoseConnectNode n生命了autoware_connector::VelPoseConnectNode对象，并调用了类的成员函数 n.run()，下面对autoware_connector::VelPoseConnectNode类进行解析：
在can_info_translator_core.h中声明如下：

class VelPoseConnectNode{public:  VelPoseConnectNode();  ~VelPoseConnectNode();  void run();private://///声明handle  ros::NodeHandle nh_;  ros::NodeHandle private_nh_;  // publisher  ros::Publisher pub1_, pub2_, pub3_;  // subscriber  ros::Subscriber sub1_,sub2_;  // variables  VehicleInfo v_info_;  Odometry odom_;  // tf::TransformBroadcaster odom_broadcaster_;  // callbacks  void callbackFromCanInfo(const vehicle_socket::CanInfoConstPtr &msg);  // initializer  void initForROS();  // functions  void publishVelocity(const vehicle_socket::CanInfoConstPtr &msg);//发布速度  void publishVelocityViz(const vehicle_socket::CanInfoConstPtr &msg);//发布用于rviz显示的速度  void publishOdometry(const vehicle_socket::CanInfoConstPtr &msg);//发布odom信息};

代码initForROS()在类的构造函数中实现了调用：

VelPoseConnectNode::VelPoseConnectNode() : private_nh_("~"), v_info_(), odom_(ros::Time::now()){  initForROS();}

在回调函数中发布上述三个发布消息

void VelPoseConnectNode::callbackFromCanInfo(const vehicle_socket::CanInfoConstPtr &msg){  publishVelocity(msg);  publishVelocityViz(msg);  publishOdometry(msg);}

下面重点对第三个发布函数进行说明，代码如下

void VelPoseConnectNode::publishOdometry(const vehicle_socket::CanInfoConstPtr &msg){  double vx = kmph2mps(msg->speed);  double vth = v_info_.convertSteeringAngleToAngularVelocity(kmph2mps(msg->speed), msg->angle);  odom_.updateOdometry(vx, vth, msg->header.stamp);  geometry_msgs::Quaternion odom_quat = tf::createQuaternionMsgFromYaw(odom_.th);  // first, we'll publish the transform over tf  /*geometry_msgs::TransformStamped odom_trans;  odom_trans.header.stamp = msg->header.stamp;  odom_trans.header.frame_id = "odom";  odom_trans.child_frame_id = "base_link";  odom_trans.transform.translation.x = odom_.x;  odom_trans.transform.translation.y = odom_.y;  odom_trans.transform.translation.z = 0.0;  odom_trans.transform.rotation = odom_quat;  // send the transform  odom_broadcaster_.sendTransform(odom_trans);  */  // next, we'll publish the odometry message over ROS  nav_msgs::Odometry odom;  odom.header.stamp = msg->header.stamp;  odom.header.frame_id = "odom";  // set the position  odom.pose.pose.position.x = odom_.x;  odom.pose.pose.position.y = odom_.y;  odom.pose.pose.position.z = 0.0;  odom.pose.pose.orientation = odom_quat;  // set the velocity  odom.child_frame_id = "base_link";  odom.twist.twist.linear.x = vx;  odom.twist.twist.angular.z = vth;  // publish the message  pub3_.publish(odom);}

代码定义处有

double vth = v_info_.convertSteeringAngleToAngularVelocity(kmph2mps(msg->speed), msg->angle);  odom_.updateOdometry(vx, vth, msg->header.stamp);

其中convertSteeringAngleToAngularVelocity主要是将控制器输出的转向角度转化成车的角速度，该部分的函数定义在VehicleInfo结构体中

namespace autoware_connector{struct VehicleInfo {  bool is_stored;  double wheel_base;  double minimum_turning_radius;  double maximum_steering_angle;  VehicleInfo()  {    is_stored = false;    wheel_base = 0;    minimum_turning_radius = 0;    maximum_steering_angle = 0;  }  double convertSteeringAngleToAngularVelocity(const double cur_vel_mps, const double cur_angle_deg) // rad/s  {    return is_stored ? tan(deg2rad(getCurrentTireAngle(cur_angle_deg))) * cur_vel_mps / wheel_base : 0;  }  double getCurrentTireAngle(const double angle_deg) // steering [degree] -> tire [degree]  {    return is_stored ? angle_deg * getMaximumTireAngle() / maximum_steering_angle: 0;  }  double getMaximumTireAngle() // degree  {    return is_stored ? rad2deg(asin(wheel_base / minimum_turning_radius)) : 0;  }  double rad2deg(double rad)  {    return rad * 180 / M_PI;  }  // convert degree to radian  inline double deg2rad(double deg)  {    return deg * M_PI / 180;  }};

上述函数中TireAngle和SteeringAngle分别为车轮的转向角和方向盘的转向角度，因为两者之间存在减速比，故定义上述函数，rad2deg(asin(wheel_base / minimum_turning_radius))中可能和有些资料中定义不太一样，该处主要取决与用户如何定义：

函数中tan(deg2rad(getCurrentTireAngle(cur_angle_deg))) * cur_vel_mps / wheel_base 为汽车的基本运动方程，参考如下：

整车的里程计定义结构体如下，为基本的运动方程，不错详细阐述：

    struct Odometry {  double x;  double y;  double th;  ros::Time stamp;  Odometry(const ros::Time &time)  {    x = 0;    y = 0;    th = 0;    stamp = time;  }  void updateOdometry(const double vx, const double vth, const ros::Time &cur_time)  {    double dt = (cur_time - stamp).toSec();    double delta_x = (vx * cos(th)) * dt;    double delta_y = (vx * sin(th)) * dt;    double delta_th = vth * dt;    std::cout << "dt : " << dt << "delta (x y th) : (" << delta_x << " " << delta_y << " " << delta_th << ")" << std::endl;    x += delta_x;    y += delta_y;    th += delta_th;    stamp = cur_time;  }};