Example #1
0
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);
}
Example #2
0
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();
           }
}