21 #include "odometry_thread.h" 22 #include <interfaces/MotorInterface.h> 23 #include <nav_msgs/Odometry.h> 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", 10);
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();
virtual void finalize()
Finalize the thread.
float odometry_position_x() const
Get odometry_position_x value.
Fawkes library namespace.
Thread class encapsulation of pthreads.
virtual void loop()
Code to execute in the thread.
float vx() const
Get vx value.
float odometry_orientation() const
Get odometry_orientation value.
Thread aspect to use blocked timing.
float odometry_position_y() const
Get odometry_position_y value.
float vy() const
Get vy value.
void read()
Read from BlackBoard into local copy.
ROSOdometryThread()
Constructor.
float omega() const
Get omega value.
virtual Interface * open_for_reading(const char *interface_type, const char *identifier, const char *owner=NULL)=0
Open interface for reading.
LockPtr< ros::NodeHandle > rosnode
Central ROS node handle.
MotorInterface Fawkes BlackBoard Interface.
Configuration * config
This is the Configuration member used to access the configuration.
virtual void init()
Initialize the thread.
virtual std::string get_string(const char *path)=0
Get value from configuration which is of type string.
BlackBoard * blackboard
This is the BlackBoard instance you can use to interact with the BlackBoard.
virtual void close(Interface *interface)=0
Close interface.