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); }