22 #include "odometry_thread.h"
24 #include <interfaces/MotorInterface.h>
25 #include <nav_msgs/Odometry.h>
37 :
Thread(
"OdometryThread",
Thread::OPMODE_WAITFORWAKEUP),
45 std::string motor_if_id =
config->
get_string(
"/ros/odometry/motor_interface_id");
49 odom_covariance_.assign(0.);
50 std::vector<float> cfg_odom_covariance =
config->
get_floats(
"/ros/odometry/odom/covariance");
51 for (uint i = 0; i < cfg_odom_covariance.size() && i < odom_covariance_.size(); i++) {
52 odom_covariance_[i] = cfg_odom_covariance.size();
55 odom_covariance_ = {1e-3, 0., 0., 0., 0., 0., 0., 1e-3, 0., 0., 0., 0.,
56 0., 0., 1e-3, 0., 0., 0., 0., 0., 0., 1e-3, 0., 0.,
57 0., 0., 0., 0., 1e-3, 0., 0., 0., 0., 0., 0., 1e-3};
60 pub_ =
rosnode->advertise<nav_msgs::Odometry>(
"odom", 10);
71 ROSOdometryThread::publish_odom()
73 nav_msgs::Odometry odom;
75 odom.header.stamp = ros::Time::now();
76 odom.header.frame_id = cfg_odom_frame_id_;
80 odom.pose.pose.position.z = 0.0;
81 odom.pose.covariance = odom_covariance_;
83 geometry_msgs::Quaternion odom_quat;
88 odom.pose.pose.orientation = odom_quat;
90 odom.child_frame_id = cfg_base_frame_id_;
91 odom.twist.twist.linear.x = (double)motor_if_->
vx();
92 odom.twist.twist.linear.y = (double)motor_if_->
vy();
93 odom.twist.twist.angular.z = (double)motor_if_->
omega();
94 odom.twist.covariance = odom_covariance_;