21 #include "odometry_thread.h"
22 #include <interfaces/MotorInterface.h>
23 #include <nav_msgs/Odometry.h>
26 using namespace fawkes;
35 :
Thread(
"OdometryThread",
Thread::OPMODE_WAITFORWAKEUP),
43 std::string motor_if_id =
config->
get_string(
"/ros/odometry/motor_interface_id");
47 pub_ =
rosnode->advertise<nav_msgs::Odometry>(
"odom", 100,
this);
59 ROSOdometryThread::publish_odom()
61 nav_msgs::Odometry odom;
63 odom.header.stamp = ros::Time::now();
64 odom.header.frame_id = cfg_odom_frame_id_;
68 odom.pose.pose.position.z = 0.0;
70 geometry_msgs::Quaternion odom_quat;
75 odom.pose.pose.orientation = odom_quat;
77 odom.child_frame_id = cfg_base_frame_id_;
78 odom.twist.twist.linear.x = (double)motor_if_->
vx();
79 odom.twist.twist.linear.y = (double)motor_if_->
vy();
80 odom.twist.twist.angular.z = (double)motor_if_->
omega();