22 #include "openrobotino_com_thread.h"
23 #ifdef HAVE_OPENROBOTINO_API_1
24 # include <rec/iocontrol/remotestate/SensorState.h>
25 # include <rec/iocontrol/robotstate/State.h>
26 # include <rec/robotino/com/Com.h>
27 # include <rec/sharedmemory/sharedmemory.h>
29 # include <rec/robotino/api2/AnalogInputArray.h>
30 # include <rec/robotino/api2/Bumper.h>
31 # include <rec/robotino/api2/Com.h>
32 # include <rec/robotino/api2/DigitalInputArray.h>
33 # include <rec/robotino/api2/DistanceSensorArray.h>
34 # include <rec/robotino/api2/ElectricalGripper.h>
35 # include <rec/robotino/api2/Gyroscope.h>
36 # include <rec/robotino/api2/MotorArray.h>
37 # include <rec/robotino/api2/Odometry.h>
38 # include <rec/robotino/api2/PowerManagement.h>
40 #include <baseapp/run.h>
41 #include <core/threading/mutex.h>
42 #include <core/threading/mutex_locker.h>
44 #include <utils/math/angle.h>
45 #include <utils/time/wait.h>
59 #ifdef HAVE_OPENROBOTINO_API_1
72 cfg_hostname_ =
config->
get_string(
"/hardware/robotino/openrobotino/hostname");
73 cfg_quit_on_disconnect_ =
config->
get_bool(
"/hardware/robotino/openrobotino/quit_on_disconnect");
74 cfg_sensor_update_cycle_time_ =
config->
get_uint(
"/hardware/robotino/cycle-time");
75 cfg_gripper_enabled_ =
config->
get_bool(
"/hardware/robotino/gripper/enable_gripper");
76 cfg_enable_gyro_ =
config->
get_bool(
"/hardware/robotino/gyro/enable");
78 #ifdef HAVE_OPENROBOTINO_API_1
79 statemem_ =
new rec::sharedmemory::SharedMemory<rec::iocontrol::robotstate::State>(
80 rec::iocontrol::robotstate::State::sharedMemoryKey);
81 state_ = statemem_->getData();
82 state_mutex_ =
new Mutex();
83 set_state_ =
new rec::iocontrol::remotestate::SetState();
84 set_state_->gripper_isEnabled = cfg_gripper_enabled_;
89 time_wait_ =
new TimeWait(
clock, cfg_sensor_update_cycle_time_ * 1000);
91 if (cfg_enable_gyro_) {
92 data_.imu_enabled =
true;
94 for (
int i = 0; i < 3; ++i)
95 data_.imu_angular_velocity[i] = 0.;
96 for (
int i = 0; i < 4; ++i)
97 data_.imu_orientation[i] = 0.;
98 for (
int i = 0; i < 9; ++i)
99 data_.imu_angular_velocity_covariance[i] = 0.;
102 data_.imu_angular_velocity_covariance[8] =
deg2rad(0.1);
105 #ifdef HAVE_OPENROBOTINO_API_1
106 com_->setAddress(cfg_hostname_.c_str());
107 com_->setMinimumUpdateCycleTime(cfg_sensor_update_cycle_time_);
108 com_->connect(
false);
110 com_ =
new rec::robotino::api2::Com(
"Fawkes");
111 com_->setAddress(cfg_hostname_.c_str());
112 com_->setAutoReconnectEnabled(
false);
113 com_->connectToServer(
true);
115 analog_inputs_com_ =
new rec::robotino::api2::AnalogInputArray();
116 bumper_com_ =
new rec::robotino::api2::Bumper();
117 digital_inputs_com_ =
new rec::robotino::api2::DigitalInputArray();
118 distances_com_ =
new rec::robotino::api2::DistanceSensorArray();
119 gripper_com_ =
new rec::robotino::api2::ElectricalGripper();
120 gyroscope_com_ =
new rec::robotino::api2::Gyroscope();
121 motors_com_ =
new rec::robotino::api2::MotorArray();
122 odom_com_ =
new rec::robotino::api2::Odometry();
123 power_com_ =
new rec::robotino::api2::PowerManagement();
125 analog_inputs_com_->setComId(com_->id());
126 bumper_com_->setComId(com_->id());
127 digital_inputs_com_->setComId(com_->id());
128 distances_com_->setComId(com_->id());
129 gripper_com_->setComId(com_->id());
130 gyroscope_com_->setComId(com_->id());
131 motors_com_->setComId(com_->id());
132 odom_com_->setComId(com_->id());
133 power_com_->setComId(com_->id());
141 #ifdef HAVE_OPENROBOTINO_API_1
142 set_state_->speedSetPoint[0] = 0.;
143 set_state_->speedSetPoint[1] = 0.;
144 set_state_->speedSetPoint[2] = 0.;
145 set_state_->gripper_isEnabled =
false;
146 com_->setSetState(*set_state_);
152 float speeds[3] = {0, 0, 0};
153 motors_com_->setSpeedSetPoints(speeds, 3);
155 delete analog_inputs_com_;
157 delete digital_inputs_com_;
158 delete distances_com_;
160 delete gyroscope_com_;
179 if (com_->isConnected()) {
181 #ifdef HAVE_OPENROBOTINO_API_1
187 #ifdef HAVE_OPENROBOTINO_API_1
188 }
else if (com_->connectionState() == rec::robotino::com::Com::NotConnected) {
192 if (cfg_quit_on_disconnect_) {
194 fawkes::runtime::quit();
197 #ifdef HAVE_OPENROBOTINO_API_1
198 com_->connect(
false);
200 com_->connectToServer(
true);
209 OpenRobotinoComThread::process_api_v1()
211 #ifdef HAVE_OPENROBOTINO_API_1
212 state_mutex_->lock();
214 rec::iocontrol::remotestate::SensorState sensor_state = sensor_states_[active_state_];
215 state_mutex_->unlock();
217 if (sensor_state.sequenceNumber != last_seqnum_) {
219 last_seqnum_ = sensor_state.sequenceNumber;
222 for (
int i = 0; i < 3; ++i) {
223 data_.mot_velocity[i] = sensor_state.actualVelocity[i];
224 data_.mot_position[i] = sensor_state.actualPosition[i];
225 data_.mot_current[i] = sensor_state.motorCurrent[i];
227 data_.bumper = sensor_state.bumper;
228 data_.bumper_estop_enabled = state_->emergencyStop.isEnabled;
229 for (
int i = 0; i < 8; ++i) {
230 data_.digital_in[i] = sensor_state.dIn[i];
231 data_.analog_in[i] = sensor_state.aIn[i];
233 if (cfg_enable_gyro_) {
234 if (state_->gyro.port == rec::serialport::UNDEFINED) {
235 if (fabs(
data_.imu_angular_velocity[0] + 1.) > 0.00001) {
236 data_.imu_angular_velocity[0] = -1.;
237 data_.imu_angular_velocity[2] = 0.;
238 data_.imu_orientation[0] = -1.;
241 data_.imu_angular_velocity[0] = 0.;
242 data_.imu_angular_velocity[2] = state_->gyro.rate;
244 tf::Quaternion q = tf::create_quaternion_from_yaw(state_->gyro.angle);
245 data_.imu_orientation[0] = q.x();
246 data_.imu_orientation[1] = q.y();
247 data_.imu_orientation[2] = q.z();
248 data_.imu_orientation[3] = q.w();
252 for (
int i = 0; i < NUM_IR_SENSORS; ++i) {
253 data_.ir_voltages[i] = sensor_state.distanceSensor[i];
256 data_.bat_voltage = roundf(sensor_state.voltage * 1000.);
257 data_.bat_current = roundf(sensor_state.current);
260 float soc = (sensor_state.voltage - 21.0f) / 5.f;
261 soc = std::min(1.f, std::max(0.f, soc));
262 data_.bat_absolute_soc = soc;
268 OpenRobotinoComThread::process_api_v2()
270 #ifdef HAVE_OPENROBOTINO_API_2
271 com_->processComEvents();
273 double odo_x = 0, odo_y = 0, odo_phi = 0;
274 unsigned int odo_seq = 0;
276 odom_com_->readings(&odo_x, &odo_y, &odo_phi, &odo_seq);
278 if (odo_seq != last_seqnum_) {
280 last_seqnum_ = odo_seq;
282 if (motors_com_->numMotors() != 3) {
284 "Invalid number of motors, got %u, expected 3",
285 motors_com_->numMotors());
288 motors_com_->actualVelocities(
data_.mot_velocity);
289 motors_com_->actualPositions(
data_.mot_position);
290 motors_com_->motorCurrents(
data_.mot_current);
292 data_.bumper = bumper_com_->value();
294 if (digital_inputs_com_->numDigitalInputs() != 8) {
296 "Invalid number of digital inputs, got %u, expected 8",
297 digital_inputs_com_->numDigitalInputs());
300 int digin_readings[8];
301 digital_inputs_com_->values(digin_readings);
302 for (
unsigned int i = 0; i < 8; ++i)
303 data_.digital_in[i] = (digin_readings[i] != 0);
305 if (analog_inputs_com_->numAnalogInputs() != 8) {
307 "Invalid number of analog inputs, got %u, expected 8",
308 analog_inputs_com_->numAnalogInputs());
311 analog_inputs_com_->values(
data_.analog_in);
313 if (distances_com_->numDistanceSensors() != NUM_IR_SENSORS) {
315 "Invalid number of distance sensors, got %u, expected 9",
316 distances_com_->numDistanceSensors());
322 distances_com_->voltages(
data_.ir_voltages);
324 float pow_current = power_com_->current() * 1000.;
325 float pow_voltage = power_com_->voltage() * 1000.;
327 float gyro_angle = gyroscope_com_->angle();
328 float gyro_rate = gyroscope_com_->rate();
330 data_.bumper_estop_enabled =
false;
331 data_.imu_angular_velocity[0] = 0.;
332 data_.imu_angular_velocity[2] = gyro_rate;
334 tf::Quaternion q = tf::create_quaternion_from_yaw(gyro_angle);
335 data_.imu_orientation[0] = q.x();
336 data_.imu_orientation[1] = q.y();
337 data_.imu_orientation[2] = q.z();
338 data_.imu_orientation[3] = q.w();
340 data_.bat_voltage = roundf(pow_voltage);
342 data_.bat_current = roundf(pow_current);
345 float soc = (power_com_->voltage() - 22.0f) / 2.5f;
346 soc = std::min(1.f, std::max(0.f, soc));
347 data_.bat_absolute_soc = soc;
352 #ifdef HAVE_OPENROBOTINO_API_1
355 OpenRobotinoComThread::updateEvent()
357 unsigned int next_state = 1 - active_state_;
358 sensor_states_[next_state] = sensorState();
359 times_[next_state].stamp();
362 active_state_ = next_state;
370 if (com_->isConnected()) {
371 #ifdef HAVE_OPENROBOTINO_API_1
372 set_state_->setOdometry =
true;
373 set_state_->odometryX = set_state_->odometryY = set_state_->odometryPhi = 0.;
374 com_->setSetState(*set_state_);
375 set_state_->setOdometry =
false;
377 odom_com_->set(0., 0., 0.,
true);
386 "Setting motor accel limits for OpenRobotino driver not supported, configure controld3");
392 logger->
log_error(
name(),
"Setting digital outputs not supported with openrobotino driver");
401 return com_->isConnected();
420 #ifdef HAVE_OPENROBOTINO_API_1
421 state_mutex_->lock();
422 t = times_[active_state_];
423 rec::iocontrol::remotestate::SensorState sensor_state = sensor_states_[active_state_];
424 state_mutex_->unlock();
427 a1 = sensor_state.actualVelocity[0] / 1000.f;
428 a2 = sensor_state.actualVelocity[1] / 1000.f;
429 a3 = sensor_state.actualVelocity[2] / 1000.f;
430 seq = sensor_state.sequenceNumber;
434 float mot_act_vel[motors_com_->numMotors()];
435 motors_com_->actualVelocities(mot_act_vel);
437 double odo_x = 0, odo_y = 0, odo_phi = 0;
438 odom_com_->readings(&odo_x, &odo_y, &odo_phi, &seq);
456 #ifdef HAVE_OPENROBOTINO_API_1
457 state_mutex_->lock();
458 rec::iocontrol::remotestate::SensorState sensor_state = sensor_states_[active_state_];
459 state_mutex_->unlock();
461 x = sensor_state.odometryX / 1000.f;
462 y = sensor_state.odometryY / 1000.f;
463 phi =
deg2rad(sensor_state.odometryPhi);
467 odom_com_->readings(&x, &y, &phi, &seq);
479 #ifdef HAVE_OPENROBOTINO_API_1
480 state_mutex_->lock();
481 rec::iocontrol::remotestate::SensorState sensor_state = sensor_states_[active_state_];
482 state_mutex_->unlock();
484 return sensor_state.isGripperOpened;
486 return gripper_com_->isOpened();
498 #ifdef HAVE_OPENROBOTINO_API_1
499 set_state_->speedSetPoint[0] = s1;
500 set_state_->speedSetPoint[1] = s2;
501 set_state_->speedSetPoint[2] = s3;
503 com_->setSetState(*set_state_);
505 float speeds[3] = {s1, s2, s3};
506 motors_com_->setSpeedSetPoints(speeds, 3);
516 #ifdef HAVE_OPENROBOTINO_API_1
517 set_state_->gripper_close = !opened;
518 com_->setSetState(*set_state_);
521 gripper_com_->open();
523 gripper_com_->close();
531 #ifdef HAVE_OPENROBOTINO_API_1
532 logger->
log_info(
name(),
"%sabling bumper estop on request", msg->is_enabled() ?
"En" :
"Dis");
533 state_->emergencyStop.isEnabled = msg->is_enabled();