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

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

// ROS includes
#include .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;
  }

};