00001
00009
00010
00011
00012
00013 #include <stdexcept>
00014 #include <ecl/math.hpp>
00015 #include <ecl/geometry/angle.hpp>
00016 #include <ecl/time/sleep.hpp>
00017 #include <ecl/converters.hpp>
00018 #include <ecl/sigslots.hpp>
00019 #include <ecl/geometry/angle.hpp>
00020 #include <ecl/time/timestamp.hpp>
00021 #include "../../include/kobuki_driver/kobuki.hpp"
00022 #include "../../include/kobuki_driver/packet_handler/payload_headers.hpp"
00023
00024
00025
00026
00027
00028 namespace kobuki
00029 {
00030
00031
00032
00033
00034
00035 bool PacketFinder::checkSum()
00036 {
00037 unsigned int packet_size(buffer.size());
00038 unsigned char cs(0);
00039 for (unsigned int i = 2; i < packet_size; i++)
00040 {
00041 cs ^= buffer[i];
00042 }
00043 return cs ? false : true;
00044 }
00045
00046
00047
00048
00049
00050 Kobuki::Kobuki() :
00051 shutdown_requested(false)
00052 , is_enabled(false)
00053 , is_connected(false)
00054 , is_alive(false)
00055 , version_info_reminder(0)
00056 {
00057 }
00058
00062 Kobuki::~Kobuki()
00063 {
00064 disable();
00065 shutdown_requested = true;
00066 thread.join();
00067 sig_debug.emit("Device: kobuki driver terminated.");
00068 }
00069
00070 void Kobuki::init(Parameters ¶meters) throw (ecl::StandardException)
00071 {
00072
00073 if (!parameters.validate())
00074 {
00075 throw ecl::StandardException(LOC, ecl::ConfigurationError, "Kobuki's parameter settings did not validate.");
00076 }
00077 this->parameters = parameters;
00078 std::string sigslots_namespace = parameters.sigslots_namespace;
00079 event_manager.init(sigslots_namespace);
00080
00081
00082 sig_version_info.connect(sigslots_namespace + std::string("/version_info"));
00083 sig_stream_data.connect(sigslots_namespace + std::string("/stream_data"));
00084 sig_raw_data_command.connect(sigslots_namespace + std::string("/raw_data_command"));
00085 sig_raw_data_stream.connect(sigslots_namespace + std::string("/raw_data_stream"));
00086
00087
00088 sig_debug.connect(sigslots_namespace + std::string("/ros_debug"));
00089 sig_info.connect(sigslots_namespace + std::string("/ros_info"));
00090 sig_warn.connect(sigslots_namespace + std::string("/ros_warn"));
00091 sig_error.connect(sigslots_namespace + std::string("/ros_error"));
00092
00093
00094 if (access(parameters.device_port.c_str(), F_OK) == -1)
00095 {
00096 ecl::Sleep waiting(5);
00097 event_manager.update(is_connected, is_alive);
00098 while (access(parameters.device_port.c_str(), F_OK) == -1)
00099 {
00100 sig_info.emit("Device does not exist. Waiting...");
00101 waiting();
00102 }
00103 }
00104
00105 serial.open(parameters.device_port, ecl::BaudRate_115200, ecl::DataBits_8, ecl::StopBits_1, ecl::NoParity);
00106
00107 is_connected = true;
00108 is_alive = true;
00109
00110 serial.block(4000);
00111 serial.clear();
00112 ecl::PushAndPop<unsigned char> stx(2, 0);
00113 ecl::PushAndPop<unsigned char> etx(1);
00114 stx.push_back(0xaa);
00115 stx.push_back(0x55);
00116 packet_finder.configure(sigslots_namespace, stx, etx, 1, 256, 1, true);
00117 acceleration_limiter.init(parameters.enable_acceleration_limiter);
00118
00119
00120 Battery::capacity = parameters.battery_capacity;
00121 Battery::low = parameters.battery_low;
00122 Battery::dangerous = parameters.battery_dangerous;
00123
00124
00125
00126
00127 version_info_reminder = 10;
00128 sendCommand(Command::GetVersionInfo());
00129
00130 thread.start(&Kobuki::spin, *this);
00131 }
00132
00133
00134
00135
00136
00146 void Kobuki::spin()
00147 {
00148 ecl::TimeStamp last_signal_time;
00149 ecl::Duration timeout(0.1);
00150 unsigned char buf[256];
00151
00152
00153
00154
00155
00156 while (!shutdown_requested)
00157 {
00158
00159
00160
00161 if( access( parameters.device_port.c_str(), F_OK ) == -1 ) {
00162 sig_error.emit("Device does not exist.");
00163 is_connected = false;
00164 is_alive = false;
00165 event_manager.update(is_connected, is_alive);
00166
00167 if( serial.open() )
00168 {
00169 sig_info.emit("Device is still open, closing it and will try to open it again.");
00170 serial.close();
00171 }
00172
00173 ecl::Sleep waiting(5);
00174 while( access( parameters.device_port.c_str(), F_OK ) == -1 ) {
00175 sig_info.emit("Device does not exist. Still waiting...");
00176 waiting();
00177 }
00178 serial.open(parameters.device_port, ecl::BaudRate_115200, ecl::DataBits_8, ecl::StopBits_1, ecl::NoParity);
00179 if( serial.open() ) {
00180 sig_info.emit("device is connected.");
00181 is_connected = true;
00182 event_manager.update(is_connected, is_alive);
00183 version_info_reminder = 10;
00184 }
00185 }
00186
00187
00188
00189
00190 int n = serial.read(buf, packet_finder.numberOfDataToRead());
00191 if (n == 0)
00192 {
00193 if (is_alive && ((ecl::TimeStamp() - last_signal_time) > timeout))
00194 {
00195 is_alive = false;
00196 version_info_reminder = 10;
00197 sig_debug.emit("Timed out while waiting for incoming bytes.");
00198 }
00199 event_manager.update(is_connected, is_alive);
00200 continue;
00201 }
00202 else
00203 {
00204 std::ostringstream ostream;
00205 ostream << "kobuki_node : serial_read(" << n << ")"
00206 << ", packet_finder.numberOfDataToRead(" << packet_finder.numberOfDataToRead() << ")";
00207 sig_debug.emit(ostream.str());
00208
00209 }
00210
00211 if (packet_finder.update(buf, n))
00212 {
00213 packet_finder.getBuffer(data_buffer);
00214 PacketFinder::BufferType local_buffer;
00215 local_buffer = data_buffer;
00216 sig_raw_data_stream.emit(local_buffer);
00217
00218
00219 data_buffer.pop_front();
00220 data_buffer.pop_front();
00221 data_buffer.pop_front();
00222
00223 while (data_buffer.size() > 1)
00224 {
00225
00226
00227
00228
00229 switch (data_buffer[0])
00230 {
00231
00232 case Header::CoreSensors:
00233 core_sensors.deserialise(data_buffer);
00234 event_manager.update(core_sensors.data, cliff.data.bottom);
00235 break;
00236 case Header::DockInfraRed:
00237 dock_ir.deserialise(data_buffer);
00238 break;
00239 case Header::Inertia:
00240 inertia.deserialise(data_buffer);
00241 break;
00242 case Header::Cliff:
00243 cliff.deserialise(data_buffer);
00244 break;
00245 case Header::Current:
00246 current.deserialise(data_buffer);
00247 break;
00248 case Header::GpInput:
00249 gp_input.deserialise(data_buffer);
00250 event_manager.update(gp_input.data.digital_input);
00251 break;
00252 case Header::ThreeAxisGyro:
00253 three_axis_gyro.deserialise(data_buffer);
00254 break;
00255
00256 case Header::Hardware:
00257 hardware.deserialise(data_buffer);
00258
00259 break;
00260 case Header::Firmware:
00261 firmware.deserialise(data_buffer);
00262 try
00263 {
00264
00265 int version_match = firmware.check_mayor_version();
00266 if (version_match < 0) {
00267 sig_error.emit("Robot firmware is outdated and needs to be upgraded. Consult how-to on: " \
00268 "http://kobuki.yujinrobot.com/documentation/howtos/upgrading-firmware");
00269 sig_warn.emit("version info - " + VersionInfo::toString(firmware.data.version)
00270 + "; current version is " + firmware.current_version());
00271 shutdown_requested = true;
00272 }
00273 else if (version_match > 0) {
00274 sig_error.emit("Driver version isn't not compatible with robot firmware. Please upgrade driver");
00275 shutdown_requested = true;
00276 }
00277 else
00278 {
00279
00280 version_match = firmware.check_minor_version();
00281 if (version_match < 0) {
00282 sig_warn.emit("Robot firmware is outdated; we suggest you to upgrade it " \
00283 "to benefit from the latest features. Consult how-to on: " \
00284 "http://kobuki.yujinrobot.com/documentation/howtos/upgrading-firmware");
00285 sig_warn.emit("version info - " + VersionInfo::toString(firmware.data.version)
00286 + "; current version is " + firmware.current_version());
00287 }
00288 else if (version_match > 0) {
00289
00290 }
00291 }
00292 }
00293 catch (std::out_of_range& e)
00294 {
00295
00296 sig_error.emit(std::string("Invalid firmware version number: ").append(e.what()));
00297 shutdown_requested = true;
00298 }
00299 break;
00300 case Header::UniqueDeviceID:
00301 unique_device_id.deserialise(data_buffer);
00302 sig_version_info.emit( VersionInfo( firmware.data.version, hardware.data.version
00303 , unique_device_id.data.udid0, unique_device_id.data.udid1, unique_device_id.data.udid2 ));
00304 sig_info.emit("version info - Hardware: " + VersionInfo::toString(hardware.data.version)
00305 + ". Firmware: " + VersionInfo::toString(firmware.data.version));
00306 version_info_reminder = 0;
00307 break;
00308 default:
00309 if (data_buffer.size() < 3 ) {
00310 sig_error.emit("malformed subpayload detected.");
00311 data_buffer.clear();
00312 } else {
00313 std::stringstream ostream;
00314 unsigned int header_id = static_cast<unsigned int>(data_buffer.pop_front());
00315 unsigned int length = static_cast<unsigned int>(data_buffer.pop_front());
00316 unsigned int remains = data_buffer.size();
00317 unsigned int to_pop;
00318
00319 ostream << "[" << header_id << "]";
00320 ostream << "[" << length << "] ";
00321
00322 ostream << "[";
00323 ostream << std::setfill('0') << std::uppercase;
00324 ostream << std::hex << std::setw(2) << header_id << " " << std::dec;
00325 ostream << std::hex << std::setw(2) << length << " " << std::dec;
00326
00327 if (remains < length) to_pop = remains;
00328 else to_pop = length;
00329
00330 for (unsigned int i = 0; i < to_pop; i++ ) {
00331 unsigned int byte = static_cast<unsigned int>(data_buffer.pop_front());
00332 ostream << std::hex << std::setw(2) << byte << " " << std::dec;
00333 }
00334 ostream << "]";
00335
00336 if (remains < length) sig_error.emit("malformed sub-payload detected. " + ostream.str());
00337 else sig_debug.emit("unexpected sub-payload received. " + ostream.str());
00338
00339 }
00340 break;
00341 }
00342 }
00343
00344 is_alive = true;
00345 event_manager.update(is_connected, is_alive);
00346 last_signal_time.stamp();
00347 sig_stream_data.emit();
00348 sendBaseControlCommand();
00349 if( version_info_reminder > 0 ) sendCommand(Command::GetVersionInfo());
00350 }
00351 else
00352 {
00353
00354 if (is_alive && ((ecl::TimeStamp() - last_signal_time) > timeout))
00355 {
00356 is_alive = false;
00357
00358 }
00359 }
00360 }
00361 sig_error.emit("Driver worker thread shutdown!");
00362 }
00363
00364
00365
00366
00367
00368 ecl::Angle<double> Kobuki::getHeading() const
00369 {
00370 ecl::Angle<double> heading;
00371
00372 heading = (static_cast<double>(inertia.data.angle) / 100.0) * ecl::pi / 180.0;
00373 return heading;
00374 }
00375
00376 double Kobuki::getAngularVelocity() const
00377 {
00378
00379 return (static_cast<double>(inertia.data.angle_rate) / 100.0) * ecl::pi / 180.0;
00380 }
00381
00382
00383
00384
00385
00386 void Kobuki::resetOdometry()
00387 {
00388 diff_drive.reset(inertia.data.angle);
00389 }
00390
00391 void Kobuki::getWheelJointStates(double &wheel_left_angle, double &wheel_left_angle_rate, double &wheel_right_angle,
00392 double &wheel_right_angle_rate)
00393 {
00394 diff_drive.getWheelJointStates(wheel_left_angle, wheel_left_angle_rate, wheel_right_angle, wheel_right_angle_rate);
00395 }
00396
00408 void Kobuki::updateOdometry(ecl::Pose2D<double> &pose_update, ecl::linear_algebra::Vector3d &pose_update_rates)
00409 {
00410 diff_drive.update(core_sensors.data.time_stamp, core_sensors.data.left_encoder, core_sensors.data.right_encoder,
00411 pose_update, pose_update_rates);
00412 }
00413
00414
00415
00416
00417
00418 void Kobuki::setLed(const enum LedNumber &number, const enum LedColour &colour)
00419 {
00420 sendCommand(Command::SetLedArray(number, colour, kobuki_command.data));
00421 }
00422
00423 void Kobuki::setDigitalOutput(const DigitalOutput &digital_output) {
00424 sendCommand(Command::SetDigitalOutput(digital_output, kobuki_command.data));
00425 }
00426
00427 void Kobuki::setExternalPower(const DigitalOutput &digital_output) {
00428 sendCommand(Command::SetExternalPower(digital_output, kobuki_command.data));
00429 }
00430
00431
00432
00433
00434
00435
00436 void Kobuki::playSoundSequence(const enum SoundSequences &number)
00437 {
00438 sendCommand(Command::PlaySoundSequence(number, kobuki_command.data));
00439 }
00440
00441 void Kobuki::setBaseControl(const double &linear_velocity, const double &angular_velocity)
00442 {
00443 diff_drive.setVelocityCommands(linear_velocity, angular_velocity);
00444 }
00445
00446 void Kobuki::sendBaseControlCommand()
00447 {
00448 if( acceleration_limiter.isEnabled() ) {
00449 diff_drive.velocityCommands(acceleration_limiter.limit(diff_drive.pointVelocity()));
00450 } else {
00451 diff_drive.velocityCommands(diff_drive.pointVelocity());
00452 }
00453 std::vector<short> velocity_commands = diff_drive.velocityCommands();
00454
00455 sendCommand(Command::SetVelocityControl(velocity_commands[0], velocity_commands[1]));
00456 }
00457
00468 void Kobuki::sendCommand(Command command)
00469 {
00470 if( !is_alive || !is_connected ) {
00471
00472 sig_debug.emit("Device state is not ready yet.");
00473 if( !is_alive ) sig_debug.emit(" - Device is not alive.");
00474 if( !is_connected ) sig_debug.emit(" - Device is not connected.");
00475
00476 return;
00477 }
00478 command_mutex.lock();
00479 kobuki_command.resetBuffer(command_buffer);
00480
00481 if (!command.serialise(command_buffer))
00482 {
00483 sig_error.emit("command serialise failed.");
00484 }
00485 command_buffer[2] = command_buffer.size() - 3;
00486 unsigned char checksum = 0;
00487 for (unsigned int i = 2; i < command_buffer.size(); i++)
00488 checksum ^= (command_buffer[i]);
00489
00490 command_buffer.push_back(checksum);
00491
00492 serial.write(&command_buffer[0], command_buffer.size());
00493
00494 sig_raw_data_command.emit(command_buffer);
00495 command_mutex.unlock();
00496 }
00497
00498 bool Kobuki::enable()
00499 {
00500 is_enabled = true;
00501 return true;
00502 }
00503
00504 bool Kobuki::disable()
00505 {
00506 setBaseControl(0.0f, 0.0f);
00507 sendBaseControlCommand();
00508 is_enabled = false;
00509 return true;
00510 }
00511
00520 void Kobuki::printSigSlotConnections() const {
00521
00522 std::cout << "========== Void ==========" << std::endl;
00523 ecl::SigSlotsManager<>::printStatistics();
00524 std::cout << "========= String =========" << std::endl;
00525 ecl::SigSlotsManager<const std::string&>::printStatistics();
00526 std::cout << "====== Button Event ======" << std::endl;
00527 ecl::SigSlotsManager<const ButtonEvent&>::printStatistics();
00528 std::cout << "====== Bumper Event ======" << std::endl;
00529 ecl::SigSlotsManager<const BumperEvent&>::printStatistics();
00530 std::cout << "====== Cliff Event =======" << std::endl;
00531 ecl::SigSlotsManager<const CliffEvent&>::printStatistics();
00532 std::cout << "====== Wheel Event =======" << std::endl;
00533 ecl::SigSlotsManager<const WheelEvent&>::printStatistics();
00534 std::cout << "====== Power Event =======" << std::endl;
00535 ecl::SigSlotsManager<const PowerEvent&>::printStatistics();
00536 std::cout << "====== Input Event =======" << std::endl;
00537 ecl::SigSlotsManager<const InputEvent&>::printStatistics();
00538 std::cout << "====== Robot Event =======" << std::endl;
00539 ecl::SigSlotsManager<const RobotEvent&>::printStatistics();
00540 std::cout << "====== VersionInfo =======" << std::endl;
00541 ecl::SigSlotsManager<const VersionInfo&>::printStatistics();
00542 std::cout << "===== Command Buffer =====" << std::endl;
00543 ecl::SigSlotsManager<const Command::Buffer&>::printStatistics();
00544 }
00545
00546 }