autoware之轮式里程计计算

这部分代码主要是接收了底盘的can消息 然后计算一个轮速里程计。具体的：

1.can_status_translator节点

  // setup subscriber
  sub1_ = nh_.subscribe("can_info", 100, &CanStatusTranslatorNode::callbackFromCANInfo, this);
  sub2_ = nh_.subscribe("vehicle_status", 10, &CanStatusTranslatorNode::callbackFromVehicleStatus, this);

  // setup publisher
  pub1_ = nh_.advertise<geometry_msgs::TwistStamped>("can_velocity", 10);
  pub2_ = nh_.advertise<std_msgs::Float32>("linear_velocity_viz", 10);
  pub3_ = nh_.advertise<autoware_msgs::VehicleStatus>("vehicle_status", 10);

一方面，接收底盘消息can_info 进入回调函数callbackFromCANInfo，主要是发布了
vehicle_status消息内容

autoware_msgs::VehicleStatus vs;
  vs.header = msg->header;
  vs.tm = msg->tm;
  vs.drivemode = msg->devmode;  // I think devmode is typo in CANInfo...
  vs.steeringmode = msg->strmode;
  vs.gearshift = msg->driveshift;
  vs.speed = msg->speed;
  vs.drivepedal = msg->drivepedal;
  vs.brakepedal = msg->brakepedal;
  vs.angle = msg->angle;
  vs.lamp = 0;
  vs.light = msg->light;

  pub3_.publish(vs);

另一方面，订阅vehicle_status消息，进入到callbackFromVehicleStatus 进而通过计算发布了can_velocity和linear_velocity_viz两个消息
can_velocity消息

void CanStatusTranslatorNode::publishVelocity(const autoware_msgs::VehicleStatusConstPtr& msg)
{
  geometry_msgs::TwistStamped tw;
  tw.header = msg->header;

  // linear velocity
  tw.twist.linear.x = kmph2mps(msg->speed);  // km/h -> m/s

  // angular velocity
  tw.twist.angular.z = v_info_.convertSteeringAngleToAngularVelocity(kmph2mps(msg->speed), msg->angle);

  pub1_.publish(tw);
}

linear_velocity_viz消息

void CanStatusTranslatorNode::publishVelocityViz(const autoware_msgs::VehicleStatusConstPtr& msg)
{
  std_msgs::Float32 fl;
  fl.data = msg->speed;
  pub2_.publish(fl);
}

2.can_odometry节点

  // setup subscriber
  sub1_ = nh_.subscribe("vehicle_status", 10, &CanOdometryNode::callbackFromVehicleStatus, this);

  // setup publisher
  pub1_ = nh_.advertise<nav_msgs::Odometry>("/vehicle/odom", 10);

订阅了vehicle_status消息，进入到callbackFromVehicleStatus回调中，计算轮式里程计
里程计的计算用到了阿克曼转向模型：
（相应的理论可查阅资料）

  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
  pub1_.publish(odom);

这里需要注意的是车体坐标系下的角速度计算，输入是车体速度和车轮转向角。

  double vth = v_info_.convertSteeringAngleToAngularVelocity(kmph2mps(msg->speed), msg->angle);

具体的

  VehicleInfo()
  {
    is_stored = false;
    wheel_base = 0.0;//前后轮距 L
    minimum_turning_radius = 0.0;//最小转弯半径  R
    maximum_steering_angle = 0.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;
  }

里程计计算

  void updateOdometry(const double vx, const double vth, const ros::Time &cur_time)
  {
    if (stamp.sec == 0 && stamp.nsec == 0)
    {
      stamp = 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;

    ROS_INFO("dt : %f delta (x y th) : (%f %f %f %f)", dt, delta_x, delta_y, delta_th);

    x += delta_x;
    y += delta_y;
    th += delta_th;
    stamp = cur_time;
  }

也可以通过阿克曼转向模型理论将其里程计运动模型写成如下形式

double dt=msg->Header.stamp.toSec()-last_time;
last_time=msg->Header.stamp.toSec();
if(abs(veh_info.steering_angle)>=0.01)
  beta=atan((dist_backwheel/(dist_backwheel+dist_frontwheel))*tan(veh_info.steering_angle*(M_PI/180)));//msg.steering_angle 弧度or 角度  这里需要的弧度
else
  beta=0.0;
if(veh_info.vehicle_speed >=1e-5 &&veh_info.gear_position==03)
{
  float vehicle_speed=veh_info.vehicle_speed*velocity_scale_;
  odom_out.v_x=vehicle_speed*cos(odom_out.theta+beta);
  odom_out.v_y=vehicle_speed*sin(odom_out.theta+beta);
  odom_out.v_theta=(vehicle_speed/dist_frontwheel)*sin(beta)*angle_scale_;

  odom_out.x+=odom_out.v_x*dt;
  odom_out.y+=odom_out.v_y*dt;
  odom_out.theta+=odom_out.v_theta*dt;
}
if(veh_info.vehicle_speed >1e-5&& veh_info.gear_position==04)
{
  float vehicle_speed=veh_info.vehicle_speed*velocity_scale_;
  odom_out.v_x=vehicle_speed*cos(odom_out.theta+beta);
  odom_out.v_y=vehicle_speed*sin(odom_out.theta+beta);
  odom_out.v_theta=(vehicle_speed/dist_frontwheel)*sin(beta)*angle_scale_;
  odom_out.x+=odom_out.v_x*dt;
  odom_out.y+=odom_out.v_y*dt;
  odom_out.theta+=odom_out.v_theta*dt;
}
geometry_msgs::Quaternion odom_quat = tf::createQuaternionMsgFromYaw(odom_out.theta);
geometry_msgs::TransformStamped odom_trans;
odom_trans.header.stamp =current_time;
odom_trans.header.frame_id = "map";
odom_trans.child_frame_id = "odom";

odom_trans.transform.translation.x = odom_out.x;
odom_trans.transform.translation.y = odom_out.y;
odom_trans.transform.translation.z = 0.0;
odom_trans.transform.rotation = odom_quat;
odom_broadcaster.sendTransform(odom_trans);

nav_msgs::Odometry odom;
odom.header.stamp = current_time;
odom.header.frame_id = "map";
odom.child_frame_id = "odom";
//set the position
odom.pose.pose.position.x = odom_out.x;
odom.pose.pose.position.y = odom_out.y;
odom.pose.pose.position.z = 0.0;
odom.pose.pose.orientation = odom_quat;

odom.twist.twist.linear.x =   odom_out.v_x;
odom.twist.twist.linear.y =   odom_out.v_y;
odom.twist.twist.angular.z =  odom_out.v_theta;
odom_pub_.publish(odom);