23 #include "act_thread.h"
24 #include "sensor_thread.h"
26 #include <interfaces/MotorInterface.h>
27 #include <utils/math/angle.h>
29 #include <rec/robotino/com/Com.h>
30 #include <rec/robotino/com/OmniDrive.h>
31 #include <rec/iocontrol/remotestate/SensorState.h>
34 using namespace fawkes;
47 :
Thread(
"RobotinoActThread",
Thread::OPMODE_WAITFORWAKEUP),
53 sensor_thread_ = sensor_thread;
60 com_ = sensor_thread_->com_;
61 omni_drive_ =
new rec::robotino::com::OmniDrive();
66 rec::iocontrol::remotestate::SetState set_state;
67 set_state.setOdometry =
true;
68 set_state.odometryX = set_state.odometryY = set_state.odometryPhi = 0;
69 com_->setSetState(set_state);
79 if (com_->isConnected()) {
80 rec::iocontrol::remotestate::SetState set_state;
81 set_state.speedSetPoint[0] = 0.;
82 set_state.speedSetPoint[1] = 0.;
83 set_state.speedSetPoint[2] = 0.;
85 com_->setSetState( set_state );
94 if (com_->isConnected()) {
95 rec::iocontrol::remotestate::SetState set_state;
96 bool send_set_state =
false;
103 omni_drive_->project(&m1, &m2, &m3,
104 msg->vx() * 1000., msg->vy() * 1000.,
107 set_state.speedSetPoint[0] = m1;
108 set_state.speedSetPoint[1] = m2;
109 set_state.speedSetPoint[2] = m3;
110 send_set_state =
true;
114 set_state.setOdometry =
true;
115 set_state.odometryX = set_state.odometryY = set_state.odometryPhi = 0;
116 send_set_state =
true;
122 if (send_set_state) com_->setSetState(set_state);
124 rec::iocontrol::remotestate::SensorState sensor_state = com_->sensorState();
125 if (sensor_state.sequenceNumber != last_seqnum_) {
127 omni_drive_->unproject(&vx, &vy, &omega,
128 sensor_state.actualVelocity[0],
129 sensor_state.actualVelocity[1],
130 sensor_state.actualVelocity[2]);
133 motor_if_->
set_vx(vx / 1000.);
134 motor_if_->
set_vy(vy / 1000.);
145 tf::Transform t(tf::Quaternion(tf::Vector3(0,0,1),
146 deg2rad(sensor_state.odometryPhi)),
147 tf::Vector3(sensor_state.odometryX / 1000.f,
148 sensor_state.odometryY / 1000.f,
151 tf_publisher->send_transform(t, now,
"/robotino_odometry",
"/base_link");
154 last_seqnum_ = sensor_state.sequenceNumber;