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();
SensorData data_
Data struct that must be updated whenever new data is available.
void wait()
Wait until minimum loop time has been reached.
bool new_data_
Flag to indicate new data, set to true if data_ is modified.
virtual void log_info(const char *component, const char *format,...)=0
Log informational message.
virtual void once()
Execute an action exactly once.
Fawkes library namespace.
virtual bool get_bool(const char *path)=0
Get value from configuration which is of type bool.
virtual void init()
Initialize the thread.
A class for handling time.
virtual void set_gripper(bool opened)
Open or close gripper.
OpenRobotinoComThread()
Constructor.
Logger * logger
This is the Logger member used to access the logger.
fawkes::Mutex * data_mutex_
Mutex to protect data_.
Clock * clock
By means of this member access to the clock is given.
virtual void get_act_velocity(float &a1, float &a2, float &a3, unsigned int &seq, fawkes::Time &t)
Get actual velocity.
virtual void set_motor_accel_limits(float min_accel, float max_accel)
Set acceleration limits of motors.
virtual void set_bumper_estop_enabled(bool enabled)
Enable or disable emergency stop on bumper contact.
Base class for exceptions in Fawkes.
virtual void reset_odometry()
Reset odometry to zero.
virtual bool is_connected()
Check if we are connected to OpenRobotino.
Virtual base class for thread that communicates with a Robotino.
const char * name() const
Get name of thread.
virtual void log_warn(const char *component, const char *format,...)=0
Log warning message.
virtual void log_error(const char *component, const char *format,...)=0
Log error message.
void mark_start()
Mark start of loop.
virtual void set_speed_points(float s1, float s2, float s3)
Set speed points for wheels.
virtual void finalize()
Finalize the thread.
Time & stamp()
Set this time to the current time.
virtual void set_digital_output(unsigned int digital_out, bool enable)
Set digital output state.
virtual unsigned int get_uint(const char *path)=0
Get value from configuration which is of type unsigned int.
virtual bool is_gripper_open()
Check if gripper is open.
Mutex mutual exclusion lock.
float deg2rad(float deg)
Convert an angle given in degrees to radians.
virtual void loop()
Code to execute in the thread.
virtual ~OpenRobotinoComThread()
Destructor.
Configuration * config
This is the Configuration member used to access the configuration.
virtual std::string get_string(const char *path)=0
Get value from configuration which is of type string.
virtual void get_odometry(double &x, double &y, double &phi)
Get current odometry.