21 #include "cmdvel_thread.h"
22 #include <interfaces/MotorInterface.h>
23 #include <ros/node_handle.h>
24 #include <geometry_msgs/Twist.h>
27 using namespace fawkes;
36 :
Thread(
"ROSCmdVelThread",
Thread::OPMODE_WAITFORWAKEUP)
43 std::string motor_if_id =
config->
get_string(
"/ros/cmdvel/motor_interface_id");
45 sub_ =
rosnode->subscribe(
"cmd_vel", 10, &ROSCmdVelThread::twist_msg_cb,
this);
49 ROSCmdVelThread::twist_msg_cb(
const geometry_msgs::Twist::ConstPtr &msg)
51 send_transrot(msg->linear.x, msg->linear.y, msg->angular.z);
69 ROSCmdVelThread::send_transrot(
float vx,
float vy,
float omega)
77 ROSCmdVelThread::stop()
79 send_transrot(0., 0., 0.);