Fawkes API
Fawkes Development Version
|
00001 00002 /*************************************************************************** 00003 * act_thread.cpp - Robotino act thread 00004 * 00005 * Created: Sun Nov 13 16:07:40 2011 00006 * Copyright 2011 Tim Niemueller [www.niemueller.de] 00007 * 00008 ****************************************************************************/ 00009 00010 /* This program is free software; you can redistribute it and/or modify 00011 * it under the terms of the GNU General Public License as published by 00012 * the Free Software Foundation; either version 2 of the License, or 00013 * (at your option) any later version. 00014 * 00015 * This program is distributed in the hope that it will be useful, 00016 * but WITHOUT ANY WARRANTY; without even the implied warranty of 00017 * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the 00018 * GNU Library General Public License for more details. 00019 * 00020 * Read the full text in the LICENSE.GPL file in the doc directory. 00021 */ 00022 00023 #include "act_thread.h" 00024 #include "sensor_thread.h" 00025 00026 #include <interfaces/MotorInterface.h> 00027 #include <utils/math/angle.h> 00028 00029 #include <rec/robotino/com/Com.h> 00030 #include <rec/robotino/com/OmniDrive.h> 00031 #include <rec/iocontrol/remotestate/SensorState.h> 00032 00033 00034 using namespace fawkes; 00035 00036 /** @class RobotinoActThread "act_thread.h" 00037 * Robotino act hook integration thread. 00038 * This thread integrates into the Fawkes main loop at the ACT hook and 00039 * executes motion commands. 00040 * @author Tim Niemueller 00041 */ 00042 00043 /** Constructor. 00044 * @param sensor_thread sensor thread from which to get the Com instance 00045 */ 00046 RobotinoActThread::RobotinoActThread(RobotinoSensorThread *sensor_thread) 00047 : Thread("RobotinoActThread", Thread::OPMODE_WAITFORWAKEUP), 00048 #ifdef HAVE_TF 00049 TransformAspect(TransformAspect::ONLY_PUBLISHER, "Robotino Odometry"), 00050 #endif 00051 BlockedTimingAspect(BlockedTimingAspect::WAKEUP_HOOK_ACT) 00052 { 00053 sensor_thread_ = sensor_thread; 00054 } 00055 00056 00057 void 00058 RobotinoActThread::init() 00059 { 00060 com_ = sensor_thread_->com_; 00061 omni_drive_ = new rec::robotino::com::OmniDrive(); 00062 00063 last_seqnum_ = 0; 00064 00065 // reset odometry once on startup 00066 rec::iocontrol::remotestate::SetState set_state; 00067 set_state.setOdometry = true; 00068 set_state.odometryX = set_state.odometryY = set_state.odometryPhi = 0; 00069 com_->setSetState(set_state); 00070 00071 motor_if_ = blackboard->open_for_writing<MotorInterface>("Robotino"); 00072 } 00073 00074 00075 void 00076 RobotinoActThread::finalize() 00077 { 00078 blackboard->close(motor_if_); 00079 if (com_->isConnected()) { 00080 rec::iocontrol::remotestate::SetState set_state; 00081 set_state.speedSetPoint[0] = 0.; 00082 set_state.speedSetPoint[1] = 0.; 00083 set_state.speedSetPoint[2] = 0.; 00084 00085 com_->setSetState( set_state ); 00086 } 00087 com_ = NULL; 00088 } 00089 00090 00091 void 00092 RobotinoActThread::loop() 00093 { 00094 if (com_->isConnected()) { 00095 rec::iocontrol::remotestate::SetState set_state; 00096 bool send_set_state = false; 00097 while (! motor_if_->msgq_empty()) { 00098 00099 if (MotorInterface::TransRotMessage *msg = 00100 motor_if_->msgq_first_safe(msg)) 00101 { 00102 float m1, m2, m3; 00103 omni_drive_->project(&m1, &m2, &m3, 00104 msg->vx() * 1000., msg->vy() * 1000., 00105 rad2deg(msg->omega())); 00106 00107 set_state.speedSetPoint[0] = m1; 00108 set_state.speedSetPoint[1] = m2; 00109 set_state.speedSetPoint[2] = m3; 00110 send_set_state = true; 00111 } 00112 else if (motor_if_->msgq_first_is<MotorInterface::ResetOdometryMessage>()) 00113 { 00114 set_state.setOdometry = true; 00115 set_state.odometryX = set_state.odometryY = set_state.odometryPhi = 0; 00116 send_set_state = true; 00117 } 00118 00119 motor_if_->msgq_pop(); 00120 } 00121 00122 if (send_set_state) com_->setSetState(set_state); 00123 00124 rec::iocontrol::remotestate::SensorState sensor_state = com_->sensorState(); 00125 if (sensor_state.sequenceNumber != last_seqnum_) { 00126 float vx, vy, omega; 00127 omni_drive_->unproject(&vx, &vy, &omega, 00128 sensor_state.actualVelocity[0], 00129 sensor_state.actualVelocity[1], 00130 sensor_state.actualVelocity[2]); 00131 00132 // div by 1000 to convert from mm to m 00133 motor_if_->set_vx(vx / 1000.); 00134 motor_if_->set_vy(vy / 1000.); 00135 motor_if_->set_omega(deg2rad(omega)); 00136 00137 motor_if_->set_odometry_position_x(sensor_state.odometryX / 1000.f); 00138 motor_if_->set_odometry_position_y(sensor_state.odometryY / 1000.f); 00139 motor_if_->set_odometry_orientation(deg2rad(sensor_state.odometryPhi)); 00140 motor_if_->write(); 00141 00142 #ifdef HAVE_TF 00143 fawkes::Time now(clock); 00144 00145 tf::Transform t(tf::Quaternion(tf::Vector3(0,0,1), 00146 deg2rad(sensor_state.odometryPhi)), 00147 tf::Vector3(sensor_state.odometryX / 1000.f, 00148 sensor_state.odometryY / 1000.f, 00149 0)); 00150 00151 tf_publisher->send_transform(t, now, "/robotino_odometry", "/base_link"); 00152 #endif 00153 00154 last_seqnum_ = sensor_state.sequenceNumber; 00155 } 00156 00157 } else { 00158 if (! motor_if_->msgq_empty()) { 00159 logger->log_warn(name(), "Motor commands received while not connected"); 00160 motor_if_->msgq_flush(); 00161 } 00162 } 00163 }