void
RosAriaNode::cmdvel_cb( const geometry_msgs::TwistConstPtr &msg)
{
veltime = ros::Time::now();
ROS_INFO( "new speed: [%0.2f,%0.2f](%0.3f)", msg->linear.x*1e3, msg->angular.z, veltime.toSec() );
robot->lock();
robot->setVel(msg->linear.x*1e3);
if(robot->hasLatVel())
robot->setLatVel(msg->linear.y*1e3);
robot->setRotVel(msg->angular.z*180/M_PI);
robot->unlock();
ROS_DEBUG("RosAria: sent vels to to aria (time %f): x vel %f mm/s, y vel %f mm/s, ang vel %f deg/s", veltime.toSec(),
(double) msg->linear.x * 1e3, (double) msg->linear.y * 1.3, (double) msg->angular.z * 180/M_PI);
}
void RosAriaNode::spin()
{
//ros::spin();
while(ros::ok()){
if(ros::WallTime::now()-last_command_time_ > ros::WallDuration(1)){
robot->lock();
robot->setVel(0.0);
if(robot->hasLatVel())
robot->setLatVel(0.0);
robot->setRotVel(0.0);
robot->unlock();
}
ros::spinOnce();
}
}
