C++ (Cpp) Odometry::process示例

 void
 odometryupdate(const nubot_common::OdoInfo & _motorinfo_msg)
 {
     static std::vector<double> motor_data(nubot::MOTOR_NUMS_CONST,0);
     static ros::Time time_before = _motorinfo_msg.header.stamp;
     ros::Time time_update = _motorinfo_msg.header.stamp;
     motor_data[0]=(double)_motorinfo_msg.Vx;
     motor_data[1]=(double)_motorinfo_msg.Vy;
     motor_data[2]=(double)_motorinfo_msg.w;
     is_power_off_   = _motorinfo_msg.PowerState;
     is_robot_stuck_ = _motorinfo_msg.RobotStuck;
     ros::Duration duration= time_update-time_before;
     time_before = time_update ;
     double secs=duration.toSec();
     odometry_->process(motor_data,secs);
 }