diff --git a/RosAria.cpp b/RosAria.cpp index ab5fb9ee838b634af8bc079d513f7a08129333dd..52a5ce67a22f6018319aa37d854e7f53ff4dc351 100644 --- a/RosAria.cpp +++ b/RosAria.cpp @@ -530,7 +530,7 @@ void RosAriaNode::publish() pos = robot->getPose(); tf::poseTFToMsg(tf::Transform(tf::createQuaternionFromYaw(pos.getTh()*M_PI/180), tf::Vector3(pos.getX()/1000, pos.getY()/1000, 0)), position.pose.pose); //Aria returns pose in mm. - position.twist.twist.linear.x = robot->getVel()/1000; //Aria returns velocity in mm/s. + position.twist.twist.linear.x = robot->getVel()/1000.0; //Aria returns velocity in mm/s. position.twist.twist.linear.y = robot->getLatVel()/1000.0; position.twist.twist.angular.z = robot->getRotVel()*M_PI/180; @@ -539,17 +539,16 @@ void RosAriaNode::publish() position.header.stamp = ros::Time::now(); pose_pub.publish(position); - ROS_DEBUG("RosAria: publish: (time %f) pose x: %f, y: %f, angle: %f; linear vel x: %f, y: %f; angular vel z: %f", + ROS_DEBUG("RosAria: publish: (time %f) pose x: %f, pose y: %f, pose angle: %f; linear vel x: %f, vel y: %f; angular vel z: %f", position.header.stamp.toSec(), (double)position.pose.pose.position.x, (double)position.pose.pose.position.y, (double)position.pose.pose.orientation.w, - (double) position.twist.twist.linear.x, - (double) position.twist.twist.linear.y, - (double) position.twist.twist.angular.z + (double)position.twist.twist.linear.x, + (double)position.twist.twist.linear.y, + (double)position.twist.twist.angular.z ); - // publishing transform odom->base_link odom_trans.header.stamp = ros::Time::now(); odom_trans.header.frame_id = frame_id_odom;