Fawkes API Fawkes Development Version
|
00001 00002 /*************************************************************************** 00003 * NavigatorInterface.cpp - Fawkes BlackBoard Interface - NavigatorInterface 00004 * 00005 * Templated created: Thu Oct 12 10:49:19 2006 00006 * Copyright 2007-2009 Martin Liebenberg, Daniel Beck, Tim Niemueller 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. A runtime exception applies to 00014 * this software (see LICENSE.GPL_WRE file mentioned below for details). 00015 * 00016 * This program is distributed in the hope that it will be useful, 00017 * but WITHOUT ANY WARRANTY; without even the implied warranty of 00018 * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the 00019 * GNU Library General Public License for more details. 00020 * 00021 * Read the full text in the LICENSE.GPL_WRE file in the doc directory. 00022 */ 00023 00024 #include <interfaces/NavigatorInterface.h> 00025 00026 #include <core/exceptions/software.h> 00027 00028 #include <cstring> 00029 #include <cstdlib> 00030 00031 namespace fawkes { 00032 00033 /** @class NavigatorInterface <interfaces/NavigatorInterface.h> 00034 * NavigatorInterface Fawkes BlackBoard Interface. 00035 * 00036 The navigator interface is used by the navigator to export information about 00037 the current status of the navigator and to define all messages by which the navigator 00038 can be instructed. 00039 00040 There are three coordinate systems, the robot system which is a right-handed cartesian 00041 coordinate system with the robot in its origin, X axis pointing forward, Y pointing to 00042 the left and Z pointing upwards. The second coordinate system is the so-called 00043 navigator system. It is a coordinate system similar to the robot system, but the 00044 origin is defined on the initialization of the navigator. The last system is the 00045 odometry system. It is again a similar system, but the origin is reset from time 00046 to time and the robot's position in this system gives the odometry deltas. 00047 00048 * @ingroup FawkesInterfaces 00049 */ 00050 00051 00052 /** ERROR_NONE constant */ 00053 const uint32_t NavigatorInterface::ERROR_NONE = 0u; 00054 /** ERROR_MOTOR constant */ 00055 const uint32_t NavigatorInterface::ERROR_MOTOR = 1u; 00056 /** ERROR_OBSTRUCTION constant */ 00057 const uint32_t NavigatorInterface::ERROR_OBSTRUCTION = 2u; 00058 /** ERROR_UNKNOWN_PLACE constant */ 00059 const uint32_t NavigatorInterface::ERROR_UNKNOWN_PLACE = 4u; 00060 /** FLAG_NONE constant */ 00061 const uint32_t NavigatorInterface::FLAG_NONE = 0u; 00062 /** FLAG_CART_GOTO constant */ 00063 const uint32_t NavigatorInterface::FLAG_CART_GOTO = 1u; 00064 /** FLAG_POLAR_GOTO constant */ 00065 const uint32_t NavigatorInterface::FLAG_POLAR_GOTO = 2u; 00066 /** FLAG_PLACE_GOTO constant */ 00067 const uint32_t NavigatorInterface::FLAG_PLACE_GOTO = 4u; 00068 /** FLAG_UPDATES_DEST_DIST constant */ 00069 const uint32_t NavigatorInterface::FLAG_UPDATES_DEST_DIST = 8u; 00070 /** FLAG_SECURITY_DISTANCE constant */ 00071 const uint32_t NavigatorInterface::FLAG_SECURITY_DISTANCE = 16u; 00072 /** FLAG_ESCAPING constant */ 00073 const uint32_t NavigatorInterface::FLAG_ESCAPING = 32u; 00074 00075 /** Constructor */ 00076 NavigatorInterface::NavigatorInterface() : Interface() 00077 { 00078 data_size = sizeof(NavigatorInterface_data_t); 00079 data_ptr = malloc(data_size); 00080 data = (NavigatorInterface_data_t *)data_ptr; 00081 data_ts = (interface_data_ts_t *)data_ptr; 00082 memset(data_ptr, 0, data_size); 00083 add_fieldinfo(IFT_UINT32, "flags", 1, &data->flags); 00084 add_fieldinfo(IFT_FLOAT, "x", 1, &data->x); 00085 add_fieldinfo(IFT_FLOAT, "y", 1, &data->y); 00086 add_fieldinfo(IFT_FLOAT, "dest_x", 1, &data->dest_x); 00087 add_fieldinfo(IFT_FLOAT, "dest_y", 1, &data->dest_y); 00088 add_fieldinfo(IFT_FLOAT, "dest_ori", 1, &data->dest_ori); 00089 add_fieldinfo(IFT_FLOAT, "dest_dist", 1, &data->dest_dist); 00090 add_fieldinfo(IFT_UINT32, "msgid", 1, &data->msgid); 00091 add_fieldinfo(IFT_BOOL, "final", 1, &data->final); 00092 add_fieldinfo(IFT_UINT32, "error_code", 1, &data->error_code); 00093 add_fieldinfo(IFT_FLOAT, "max_velocity", 1, &data->max_velocity); 00094 add_fieldinfo(IFT_FLOAT, "security_distance", 1, &data->security_distance); 00095 add_fieldinfo(IFT_BOOL, "escaping_enabled", 1, &data->escaping_enabled); 00096 add_messageinfo("StopMessage"); 00097 add_messageinfo("TurnMessage"); 00098 add_messageinfo("CartesianGotoMessage"); 00099 add_messageinfo("PolarGotoMessage"); 00100 add_messageinfo("PlaceGotoMessage"); 00101 add_messageinfo("ObstacleMessage"); 00102 add_messageinfo("ResetOdometryMessage"); 00103 add_messageinfo("SetMaxVelocityMessage"); 00104 add_messageinfo("SetEscapingMessage"); 00105 add_messageinfo("SetSecurityDistanceMessage"); 00106 unsigned char tmp_hash[] = {0x90, 0x6b, 0x4d, 0xeb, 0x52, 0x4d, 0x53, 0x73, 0x4c, 0xbc, 0x82, 0x5, 0x80, 0x81, 0xf1, 0x39}; 00107 set_hash(tmp_hash); 00108 } 00109 00110 /** Destructor */ 00111 NavigatorInterface::~NavigatorInterface() 00112 { 00113 free(data_ptr); 00114 } 00115 /* Methods */ 00116 /** Get flags value. 00117 * Bit-wise combination of 00118 FLAG_* constants denoting navigator component features. 00119 * @return flags value 00120 */ 00121 uint32_t 00122 NavigatorInterface::flags() const 00123 { 00124 return data->flags; 00125 } 00126 00127 /** Get maximum length of flags value. 00128 * @return length of flags value, can be length of the array or number of 00129 * maximum number of characters for a string 00130 */ 00131 size_t 00132 NavigatorInterface::maxlenof_flags() const 00133 { 00134 return 1; 00135 } 00136 00137 /** Set flags value. 00138 * Bit-wise combination of 00139 FLAG_* constants denoting navigator component features. 00140 * @param new_flags new flags value 00141 */ 00142 void 00143 NavigatorInterface::set_flags(const uint32_t new_flags) 00144 { 00145 data->flags = new_flags; 00146 data_changed = true; 00147 } 00148 00149 /** Get x value. 00150 * Current X-coordinate in the navigator coordinate system. 00151 * @return x value 00152 */ 00153 float 00154 NavigatorInterface::x() const 00155 { 00156 return data->x; 00157 } 00158 00159 /** Get maximum length of x value. 00160 * @return length of x value, can be length of the array or number of 00161 * maximum number of characters for a string 00162 */ 00163 size_t 00164 NavigatorInterface::maxlenof_x() const 00165 { 00166 return 1; 00167 } 00168 00169 /** Set x value. 00170 * Current X-coordinate in the navigator coordinate system. 00171 * @param new_x new x value 00172 */ 00173 void 00174 NavigatorInterface::set_x(const float new_x) 00175 { 00176 data->x = new_x; 00177 data_changed = true; 00178 } 00179 00180 /** Get y value. 00181 * Current Y-coordinate in the navigator coordinate system. 00182 * @return y value 00183 */ 00184 float 00185 NavigatorInterface::y() const 00186 { 00187 return data->y; 00188 } 00189 00190 /** Get maximum length of y value. 00191 * @return length of y value, can be length of the array or number of 00192 * maximum number of characters for a string 00193 */ 00194 size_t 00195 NavigatorInterface::maxlenof_y() const 00196 { 00197 return 1; 00198 } 00199 00200 /** Set y value. 00201 * Current Y-coordinate in the navigator coordinate system. 00202 * @param new_y new y value 00203 */ 00204 void 00205 NavigatorInterface::set_y(const float new_y) 00206 { 00207 data->y = new_y; 00208 data_changed = true; 00209 } 00210 00211 /** Get dest_x value. 00212 * X-coordinate of the current destination, or 0.0 if no target has been set. 00213 * @return dest_x value 00214 */ 00215 float 00216 NavigatorInterface::dest_x() const 00217 { 00218 return data->dest_x; 00219 } 00220 00221 /** Get maximum length of dest_x value. 00222 * @return length of dest_x value, can be length of the array or number of 00223 * maximum number of characters for a string 00224 */ 00225 size_t 00226 NavigatorInterface::maxlenof_dest_x() const 00227 { 00228 return 1; 00229 } 00230 00231 /** Set dest_x value. 00232 * X-coordinate of the current destination, or 0.0 if no target has been set. 00233 * @param new_dest_x new dest_x value 00234 */ 00235 void 00236 NavigatorInterface::set_dest_x(const float new_dest_x) 00237 { 00238 data->dest_x = new_dest_x; 00239 data_changed = true; 00240 } 00241 00242 /** Get dest_y value. 00243 * Y-coordinate of the current destination, or 0.0 if no target has been set. 00244 * @return dest_y value 00245 */ 00246 float 00247 NavigatorInterface::dest_y() const 00248 { 00249 return data->dest_y; 00250 } 00251 00252 /** Get maximum length of dest_y value. 00253 * @return length of dest_y value, can be length of the array or number of 00254 * maximum number of characters for a string 00255 */ 00256 size_t 00257 NavigatorInterface::maxlenof_dest_y() const 00258 { 00259 return 1; 00260 } 00261 00262 /** Set dest_y value. 00263 * Y-coordinate of the current destination, or 0.0 if no target has been set. 00264 * @param new_dest_y new dest_y value 00265 */ 00266 void 00267 NavigatorInterface::set_dest_y(const float new_dest_y) 00268 { 00269 data->dest_y = new_dest_y; 00270 data_changed = true; 00271 } 00272 00273 /** Get dest_ori value. 00274 * Orientation of the current destination, or 0.0 if no target has been set. 00275 * @return dest_ori value 00276 */ 00277 float 00278 NavigatorInterface::dest_ori() const 00279 { 00280 return data->dest_ori; 00281 } 00282 00283 /** Get maximum length of dest_ori value. 00284 * @return length of dest_ori value, can be length of the array or number of 00285 * maximum number of characters for a string 00286 */ 00287 size_t 00288 NavigatorInterface::maxlenof_dest_ori() const 00289 { 00290 return 1; 00291 } 00292 00293 /** Set dest_ori value. 00294 * Orientation of the current destination, or 0.0 if no target has been set. 00295 * @param new_dest_ori new dest_ori value 00296 */ 00297 void 00298 NavigatorInterface::set_dest_ori(const float new_dest_ori) 00299 { 00300 data->dest_ori = new_dest_ori; 00301 data_changed = true; 00302 } 00303 00304 /** Get dest_dist value. 00305 * Distance to destination in m. 00306 * @return dest_dist value 00307 */ 00308 float 00309 NavigatorInterface::dest_dist() const 00310 { 00311 return data->dest_dist; 00312 } 00313 00314 /** Get maximum length of dest_dist value. 00315 * @return length of dest_dist value, can be length of the array or number of 00316 * maximum number of characters for a string 00317 */ 00318 size_t 00319 NavigatorInterface::maxlenof_dest_dist() const 00320 { 00321 return 1; 00322 } 00323 00324 /** Set dest_dist value. 00325 * Distance to destination in m. 00326 * @param new_dest_dist new dest_dist value 00327 */ 00328 void 00329 NavigatorInterface::set_dest_dist(const float new_dest_dist) 00330 { 00331 data->dest_dist = new_dest_dist; 00332 data_changed = true; 00333 } 00334 00335 /** Get msgid value. 00336 * The ID of the message that is currently being 00337 processed, or 0 if no message is being processed. 00338 * @return msgid value 00339 */ 00340 uint32_t 00341 NavigatorInterface::msgid() const 00342 { 00343 return data->msgid; 00344 } 00345 00346 /** Get maximum length of msgid value. 00347 * @return length of msgid value, can be length of the array or number of 00348 * maximum number of characters for a string 00349 */ 00350 size_t 00351 NavigatorInterface::maxlenof_msgid() const 00352 { 00353 return 1; 00354 } 00355 00356 /** Set msgid value. 00357 * The ID of the message that is currently being 00358 processed, or 0 if no message is being processed. 00359 * @param new_msgid new msgid value 00360 */ 00361 void 00362 NavigatorInterface::set_msgid(const uint32_t new_msgid) 00363 { 00364 data->msgid = new_msgid; 00365 data_changed = true; 00366 } 00367 00368 /** Get final value. 00369 * True, if the last goto command has been finished, 00370 false if it is still running 00371 * @return final value 00372 */ 00373 bool 00374 NavigatorInterface::is_final() const 00375 { 00376 return data->final; 00377 } 00378 00379 /** Get maximum length of final value. 00380 * @return length of final value, can be length of the array or number of 00381 * maximum number of characters for a string 00382 */ 00383 size_t 00384 NavigatorInterface::maxlenof_final() const 00385 { 00386 return 1; 00387 } 00388 00389 /** Set final value. 00390 * True, if the last goto command has been finished, 00391 false if it is still running 00392 * @param new_final new final value 00393 */ 00394 void 00395 NavigatorInterface::set_final(const bool new_final) 00396 { 00397 data->final = new_final; 00398 data_changed = true; 00399 } 00400 00401 /** Get error_code value. 00402 * Failure code set if 00403 final is true. 0 if no error occured, an error code from ERROR_* 00404 constants otherwise (or a bit-wise combination). 00405 * @return error_code value 00406 */ 00407 uint32_t 00408 NavigatorInterface::error_code() const 00409 { 00410 return data->error_code; 00411 } 00412 00413 /** Get maximum length of error_code value. 00414 * @return length of error_code value, can be length of the array or number of 00415 * maximum number of characters for a string 00416 */ 00417 size_t 00418 NavigatorInterface::maxlenof_error_code() const 00419 { 00420 return 1; 00421 } 00422 00423 /** Set error_code value. 00424 * Failure code set if 00425 final is true. 0 if no error occured, an error code from ERROR_* 00426 constants otherwise (or a bit-wise combination). 00427 * @param new_error_code new error_code value 00428 */ 00429 void 00430 NavigatorInterface::set_error_code(const uint32_t new_error_code) 00431 { 00432 data->error_code = new_error_code; 00433 data_changed = true; 00434 } 00435 00436 /** Get max_velocity value. 00437 * Maximum velocity 00438 * @return max_velocity value 00439 */ 00440 float 00441 NavigatorInterface::max_velocity() const 00442 { 00443 return data->max_velocity; 00444 } 00445 00446 /** Get maximum length of max_velocity value. 00447 * @return length of max_velocity value, can be length of the array or number of 00448 * maximum number of characters for a string 00449 */ 00450 size_t 00451 NavigatorInterface::maxlenof_max_velocity() const 00452 { 00453 return 1; 00454 } 00455 00456 /** Set max_velocity value. 00457 * Maximum velocity 00458 * @param new_max_velocity new max_velocity value 00459 */ 00460 void 00461 NavigatorInterface::set_max_velocity(const float new_max_velocity) 00462 { 00463 data->max_velocity = new_max_velocity; 00464 data_changed = true; 00465 } 00466 00467 /** Get security_distance value. 00468 * Security distance to 00469 keep to obstacles 00470 * @return security_distance value 00471 */ 00472 float 00473 NavigatorInterface::security_distance() const 00474 { 00475 return data->security_distance; 00476 } 00477 00478 /** Get maximum length of security_distance value. 00479 * @return length of security_distance value, can be length of the array or number of 00480 * maximum number of characters for a string 00481 */ 00482 size_t 00483 NavigatorInterface::maxlenof_security_distance() const 00484 { 00485 return 1; 00486 } 00487 00488 /** Set security_distance value. 00489 * Security distance to 00490 keep to obstacles 00491 * @param new_security_distance new security_distance value 00492 */ 00493 void 00494 NavigatorInterface::set_security_distance(const float new_security_distance) 00495 { 00496 data->security_distance = new_security_distance; 00497 data_changed = true; 00498 } 00499 00500 /** Get escaping_enabled value. 00501 * This is used for 00502 navigation components with integrated collision avoidance, to 00503 check whether the navigator should stop when an obstacle 00504 obstructs the path, or if it should escape. 00505 * @return escaping_enabled value 00506 */ 00507 bool 00508 NavigatorInterface::is_escaping_enabled() const 00509 { 00510 return data->escaping_enabled; 00511 } 00512 00513 /** Get maximum length of escaping_enabled value. 00514 * @return length of escaping_enabled value, can be length of the array or number of 00515 * maximum number of characters for a string 00516 */ 00517 size_t 00518 NavigatorInterface::maxlenof_escaping_enabled() const 00519 { 00520 return 1; 00521 } 00522 00523 /** Set escaping_enabled value. 00524 * This is used for 00525 navigation components with integrated collision avoidance, to 00526 check whether the navigator should stop when an obstacle 00527 obstructs the path, or if it should escape. 00528 * @param new_escaping_enabled new escaping_enabled value 00529 */ 00530 void 00531 NavigatorInterface::set_escaping_enabled(const bool new_escaping_enabled) 00532 { 00533 data->escaping_enabled = new_escaping_enabled; 00534 data_changed = true; 00535 } 00536 00537 /* =========== message create =========== */ 00538 Message * 00539 NavigatorInterface::create_message(const char *type) const 00540 { 00541 if ( strncmp("StopMessage", type, __INTERFACE_MESSAGE_TYPE_SIZE) == 0 ) { 00542 return new StopMessage(); 00543 } else if ( strncmp("TurnMessage", type, __INTERFACE_MESSAGE_TYPE_SIZE) == 0 ) { 00544 return new TurnMessage(); 00545 } else if ( strncmp("CartesianGotoMessage", type, __INTERFACE_MESSAGE_TYPE_SIZE) == 0 ) { 00546 return new CartesianGotoMessage(); 00547 } else if ( strncmp("PolarGotoMessage", type, __INTERFACE_MESSAGE_TYPE_SIZE) == 0 ) { 00548 return new PolarGotoMessage(); 00549 } else if ( strncmp("PlaceGotoMessage", type, __INTERFACE_MESSAGE_TYPE_SIZE) == 0 ) { 00550 return new PlaceGotoMessage(); 00551 } else if ( strncmp("ObstacleMessage", type, __INTERFACE_MESSAGE_TYPE_SIZE) == 0 ) { 00552 return new ObstacleMessage(); 00553 } else if ( strncmp("ResetOdometryMessage", type, __INTERFACE_MESSAGE_TYPE_SIZE) == 0 ) { 00554 return new ResetOdometryMessage(); 00555 } else if ( strncmp("SetMaxVelocityMessage", type, __INTERFACE_MESSAGE_TYPE_SIZE) == 0 ) { 00556 return new SetMaxVelocityMessage(); 00557 } else if ( strncmp("SetEscapingMessage", type, __INTERFACE_MESSAGE_TYPE_SIZE) == 0 ) { 00558 return new SetEscapingMessage(); 00559 } else if ( strncmp("SetSecurityDistanceMessage", type, __INTERFACE_MESSAGE_TYPE_SIZE) == 0 ) { 00560 return new SetSecurityDistanceMessage(); 00561 } else { 00562 throw UnknownTypeException("The given type '%s' does not match any known " 00563 "message type for this interface type.", type); 00564 } 00565 } 00566 00567 00568 /** Copy values from other interface. 00569 * @param other other interface to copy values from 00570 */ 00571 void 00572 NavigatorInterface::copy_values(const Interface *other) 00573 { 00574 const NavigatorInterface *oi = dynamic_cast<const NavigatorInterface *>(other); 00575 if (oi == NULL) { 00576 throw TypeMismatchException("Can only copy values from interface of same type (%s vs. %s)", 00577 type(), other->type()); 00578 } 00579 memcpy(data, oi->data, sizeof(NavigatorInterface_data_t)); 00580 } 00581 00582 const char * 00583 NavigatorInterface::enum_tostring(const char *enumtype, int val) const 00584 { 00585 throw UnknownTypeException("Unknown enum type %s", enumtype); 00586 } 00587 00588 /* =========== messages =========== */ 00589 /** @class NavigatorInterface::StopMessage <interfaces/NavigatorInterface.h> 00590 * StopMessage Fawkes BlackBoard Interface Message. 00591 * 00592 00593 */ 00594 00595 00596 /** Constructor */ 00597 NavigatorInterface::StopMessage::StopMessage() : Message("StopMessage") 00598 { 00599 data_size = sizeof(StopMessage_data_t); 00600 data_ptr = malloc(data_size); 00601 memset(data_ptr, 0, data_size); 00602 data = (StopMessage_data_t *)data_ptr; 00603 data_ts = (message_data_ts_t *)data_ptr; 00604 } 00605 00606 /** Destructor */ 00607 NavigatorInterface::StopMessage::~StopMessage() 00608 { 00609 free(data_ptr); 00610 } 00611 00612 /** Copy constructor. 00613 * @param m message to copy from 00614 */ 00615 NavigatorInterface::StopMessage::StopMessage(const StopMessage *m) : Message("StopMessage") 00616 { 00617 data_size = m->data_size; 00618 data_ptr = malloc(data_size); 00619 memcpy(data_ptr, m->data_ptr, data_size); 00620 data = (StopMessage_data_t *)data_ptr; 00621 data_ts = (message_data_ts_t *)data_ptr; 00622 } 00623 00624 /* Methods */ 00625 /** Clone this message. 00626 * Produces a message of the same type as this message and copies the 00627 * data to the new message. 00628 * @return clone of this message 00629 */ 00630 Message * 00631 NavigatorInterface::StopMessage::clone() const 00632 { 00633 return new NavigatorInterface::StopMessage(this); 00634 } 00635 /** @class NavigatorInterface::TurnMessage <interfaces/NavigatorInterface.h> 00636 * TurnMessage Fawkes BlackBoard Interface Message. 00637 * 00638 00639 */ 00640 00641 00642 /** Constructor with initial values. 00643 * @param ini_angle initial value for angle 00644 * @param ini_velocity initial value for velocity 00645 */ 00646 NavigatorInterface::TurnMessage::TurnMessage(const float ini_angle, const float ini_velocity) : Message("TurnMessage") 00647 { 00648 data_size = sizeof(TurnMessage_data_t); 00649 data_ptr = malloc(data_size); 00650 memset(data_ptr, 0, data_size); 00651 data = (TurnMessage_data_t *)data_ptr; 00652 data_ts = (message_data_ts_t *)data_ptr; 00653 data->angle = ini_angle; 00654 data->velocity = ini_velocity; 00655 add_fieldinfo(IFT_FLOAT, "angle", 1, &data->angle); 00656 add_fieldinfo(IFT_FLOAT, "velocity", 1, &data->velocity); 00657 } 00658 /** Constructor */ 00659 NavigatorInterface::TurnMessage::TurnMessage() : Message("TurnMessage") 00660 { 00661 data_size = sizeof(TurnMessage_data_t); 00662 data_ptr = malloc(data_size); 00663 memset(data_ptr, 0, data_size); 00664 data = (TurnMessage_data_t *)data_ptr; 00665 data_ts = (message_data_ts_t *)data_ptr; 00666 add_fieldinfo(IFT_FLOAT, "angle", 1, &data->angle); 00667 add_fieldinfo(IFT_FLOAT, "velocity", 1, &data->velocity); 00668 } 00669 00670 /** Destructor */ 00671 NavigatorInterface::TurnMessage::~TurnMessage() 00672 { 00673 free(data_ptr); 00674 } 00675 00676 /** Copy constructor. 00677 * @param m message to copy from 00678 */ 00679 NavigatorInterface::TurnMessage::TurnMessage(const TurnMessage *m) : Message("TurnMessage") 00680 { 00681 data_size = m->data_size; 00682 data_ptr = malloc(data_size); 00683 memcpy(data_ptr, m->data_ptr, data_size); 00684 data = (TurnMessage_data_t *)data_ptr; 00685 data_ts = (message_data_ts_t *)data_ptr; 00686 } 00687 00688 /* Methods */ 00689 /** Get angle value. 00690 * Angle of the turn. 00691 * @return angle value 00692 */ 00693 float 00694 NavigatorInterface::TurnMessage::angle() const 00695 { 00696 return data->angle; 00697 } 00698 00699 /** Get maximum length of angle value. 00700 * @return length of angle value, can be length of the array or number of 00701 * maximum number of characters for a string 00702 */ 00703 size_t 00704 NavigatorInterface::TurnMessage::maxlenof_angle() const 00705 { 00706 return 1; 00707 } 00708 00709 /** Set angle value. 00710 * Angle of the turn. 00711 * @param new_angle new angle value 00712 */ 00713 void 00714 NavigatorInterface::TurnMessage::set_angle(const float new_angle) 00715 { 00716 data->angle = new_angle; 00717 } 00718 00719 /** Get velocity value. 00720 * The desired turning velocity in rad/s, 00721 set to zero to use default value. 00722 * @return velocity value 00723 */ 00724 float 00725 NavigatorInterface::TurnMessage::velocity() const 00726 { 00727 return data->velocity; 00728 } 00729 00730 /** Get maximum length of velocity value. 00731 * @return length of velocity value, can be length of the array or number of 00732 * maximum number of characters for a string 00733 */ 00734 size_t 00735 NavigatorInterface::TurnMessage::maxlenof_velocity() const 00736 { 00737 return 1; 00738 } 00739 00740 /** Set velocity value. 00741 * The desired turning velocity in rad/s, 00742 set to zero to use default value. 00743 * @param new_velocity new velocity value 00744 */ 00745 void 00746 NavigatorInterface::TurnMessage::set_velocity(const float new_velocity) 00747 { 00748 data->velocity = new_velocity; 00749 } 00750 00751 /** Clone this message. 00752 * Produces a message of the same type as this message and copies the 00753 * data to the new message. 00754 * @return clone of this message 00755 */ 00756 Message * 00757 NavigatorInterface::TurnMessage::clone() const 00758 { 00759 return new NavigatorInterface::TurnMessage(this); 00760 } 00761 /** @class NavigatorInterface::CartesianGotoMessage <interfaces/NavigatorInterface.h> 00762 * CartesianGotoMessage Fawkes BlackBoard Interface Message. 00763 * 00764 00765 */ 00766 00767 00768 /** Constructor with initial values. 00769 * @param ini_x initial value for x 00770 * @param ini_y initial value for y 00771 * @param ini_orientation initial value for orientation 00772 */ 00773 NavigatorInterface::CartesianGotoMessage::CartesianGotoMessage(const float ini_x, const float ini_y, const float ini_orientation) : Message("CartesianGotoMessage") 00774 { 00775 data_size = sizeof(CartesianGotoMessage_data_t); 00776 data_ptr = malloc(data_size); 00777 memset(data_ptr, 0, data_size); 00778 data = (CartesianGotoMessage_data_t *)data_ptr; 00779 data_ts = (message_data_ts_t *)data_ptr; 00780 data->x = ini_x; 00781 data->y = ini_y; 00782 data->orientation = ini_orientation; 00783 add_fieldinfo(IFT_FLOAT, "x", 1, &data->x); 00784 add_fieldinfo(IFT_FLOAT, "y", 1, &data->y); 00785 add_fieldinfo(IFT_FLOAT, "orientation", 1, &data->orientation); 00786 } 00787 /** Constructor */ 00788 NavigatorInterface::CartesianGotoMessage::CartesianGotoMessage() : Message("CartesianGotoMessage") 00789 { 00790 data_size = sizeof(CartesianGotoMessage_data_t); 00791 data_ptr = malloc(data_size); 00792 memset(data_ptr, 0, data_size); 00793 data = (CartesianGotoMessage_data_t *)data_ptr; 00794 data_ts = (message_data_ts_t *)data_ptr; 00795 add_fieldinfo(IFT_FLOAT, "x", 1, &data->x); 00796 add_fieldinfo(IFT_FLOAT, "y", 1, &data->y); 00797 add_fieldinfo(IFT_FLOAT, "orientation", 1, &data->orientation); 00798 } 00799 00800 /** Destructor */ 00801 NavigatorInterface::CartesianGotoMessage::~CartesianGotoMessage() 00802 { 00803 free(data_ptr); 00804 } 00805 00806 /** Copy constructor. 00807 * @param m message to copy from 00808 */ 00809 NavigatorInterface::CartesianGotoMessage::CartesianGotoMessage(const CartesianGotoMessage *m) : Message("CartesianGotoMessage") 00810 { 00811 data_size = m->data_size; 00812 data_ptr = malloc(data_size); 00813 memcpy(data_ptr, m->data_ptr, data_size); 00814 data = (CartesianGotoMessage_data_t *)data_ptr; 00815 data_ts = (message_data_ts_t *)data_ptr; 00816 } 00817 00818 /* Methods */ 00819 /** Get x value. 00820 * X-coordinate of the target, in the robot's coordinate system. 00821 * @return x value 00822 */ 00823 float 00824 NavigatorInterface::CartesianGotoMessage::x() const 00825 { 00826 return data->x; 00827 } 00828 00829 /** Get maximum length of x value. 00830 * @return length of x value, can be length of the array or number of 00831 * maximum number of characters for a string 00832 */ 00833 size_t 00834 NavigatorInterface::CartesianGotoMessage::maxlenof_x() const 00835 { 00836 return 1; 00837 } 00838 00839 /** Set x value. 00840 * X-coordinate of the target, in the robot's coordinate system. 00841 * @param new_x new x value 00842 */ 00843 void 00844 NavigatorInterface::CartesianGotoMessage::set_x(const float new_x) 00845 { 00846 data->x = new_x; 00847 } 00848 00849 /** Get y value. 00850 * Y-coordinate of the target, in the robot's coordinate system. 00851 * @return y value 00852 */ 00853 float 00854 NavigatorInterface::CartesianGotoMessage::y() const 00855 { 00856 return data->y; 00857 } 00858 00859 /** Get maximum length of y value. 00860 * @return length of y value, can be length of the array or number of 00861 * maximum number of characters for a string 00862 */ 00863 size_t 00864 NavigatorInterface::CartesianGotoMessage::maxlenof_y() const 00865 { 00866 return 1; 00867 } 00868 00869 /** Set y value. 00870 * Y-coordinate of the target, in the robot's coordinate system. 00871 * @param new_y new y value 00872 */ 00873 void 00874 NavigatorInterface::CartesianGotoMessage::set_y(const float new_y) 00875 { 00876 data->y = new_y; 00877 } 00878 00879 /** Get orientation value. 00880 * The orientation of the robot at the target. 00881 * @return orientation value 00882 */ 00883 float 00884 NavigatorInterface::CartesianGotoMessage::orientation() const 00885 { 00886 return data->orientation; 00887 } 00888 00889 /** Get maximum length of orientation value. 00890 * @return length of orientation value, can be length of the array or number of 00891 * maximum number of characters for a string 00892 */ 00893 size_t 00894 NavigatorInterface::CartesianGotoMessage::maxlenof_orientation() const 00895 { 00896 return 1; 00897 } 00898 00899 /** Set orientation value. 00900 * The orientation of the robot at the target. 00901 * @param new_orientation new orientation value 00902 */ 00903 void 00904 NavigatorInterface::CartesianGotoMessage::set_orientation(const float new_orientation) 00905 { 00906 data->orientation = new_orientation; 00907 } 00908 00909 /** Clone this message. 00910 * Produces a message of the same type as this message and copies the 00911 * data to the new message. 00912 * @return clone of this message 00913 */ 00914 Message * 00915 NavigatorInterface::CartesianGotoMessage::clone() const 00916 { 00917 return new NavigatorInterface::CartesianGotoMessage(this); 00918 } 00919 /** @class NavigatorInterface::PolarGotoMessage <interfaces/NavigatorInterface.h> 00920 * PolarGotoMessage Fawkes BlackBoard Interface Message. 00921 * 00922 00923 */ 00924 00925 00926 /** Constructor with initial values. 00927 * @param ini_phi initial value for phi 00928 * @param ini_dist initial value for dist 00929 * @param ini_orientation initial value for orientation 00930 */ 00931 NavigatorInterface::PolarGotoMessage::PolarGotoMessage(const float ini_phi, const float ini_dist, const float ini_orientation) : Message("PolarGotoMessage") 00932 { 00933 data_size = sizeof(PolarGotoMessage_data_t); 00934 data_ptr = malloc(data_size); 00935 memset(data_ptr, 0, data_size); 00936 data = (PolarGotoMessage_data_t *)data_ptr; 00937 data_ts = (message_data_ts_t *)data_ptr; 00938 data->phi = ini_phi; 00939 data->dist = ini_dist; 00940 data->orientation = ini_orientation; 00941 add_fieldinfo(IFT_FLOAT, "phi", 1, &data->phi); 00942 add_fieldinfo(IFT_FLOAT, "dist", 1, &data->dist); 00943 add_fieldinfo(IFT_FLOAT, "orientation", 1, &data->orientation); 00944 } 00945 /** Constructor */ 00946 NavigatorInterface::PolarGotoMessage::PolarGotoMessage() : Message("PolarGotoMessage") 00947 { 00948 data_size = sizeof(PolarGotoMessage_data_t); 00949 data_ptr = malloc(data_size); 00950 memset(data_ptr, 0, data_size); 00951 data = (PolarGotoMessage_data_t *)data_ptr; 00952 data_ts = (message_data_ts_t *)data_ptr; 00953 add_fieldinfo(IFT_FLOAT, "phi", 1, &data->phi); 00954 add_fieldinfo(IFT_FLOAT, "dist", 1, &data->dist); 00955 add_fieldinfo(IFT_FLOAT, "orientation", 1, &data->orientation); 00956 } 00957 00958 /** Destructor */ 00959 NavigatorInterface::PolarGotoMessage::~PolarGotoMessage() 00960 { 00961 free(data_ptr); 00962 } 00963 00964 /** Copy constructor. 00965 * @param m message to copy from 00966 */ 00967 NavigatorInterface::PolarGotoMessage::PolarGotoMessage(const PolarGotoMessage *m) : Message("PolarGotoMessage") 00968 { 00969 data_size = m->data_size; 00970 data_ptr = malloc(data_size); 00971 memcpy(data_ptr, m->data_ptr, data_size); 00972 data = (PolarGotoMessage_data_t *)data_ptr; 00973 data_ts = (message_data_ts_t *)data_ptr; 00974 } 00975 00976 /* Methods */ 00977 /** Get phi value. 00978 * Angle between the robot's front and the target. 00979 * @return phi value 00980 */ 00981 float 00982 NavigatorInterface::PolarGotoMessage::phi() const 00983 { 00984 return data->phi; 00985 } 00986 00987 /** Get maximum length of phi value. 00988 * @return length of phi value, can be length of the array or number of 00989 * maximum number of characters for a string 00990 */ 00991 size_t 00992 NavigatorInterface::PolarGotoMessage::maxlenof_phi() const 00993 { 00994 return 1; 00995 } 00996 00997 /** Set phi value. 00998 * Angle between the robot's front and the target. 00999 * @param new_phi new phi value 01000 */ 01001 void 01002 NavigatorInterface::PolarGotoMessage::set_phi(const float new_phi) 01003 { 01004 data->phi = new_phi; 01005 } 01006 01007 /** Get dist value. 01008 * Distance to the target. 01009 * @return dist value 01010 */ 01011 float 01012 NavigatorInterface::PolarGotoMessage::dist() const 01013 { 01014 return data->dist; 01015 } 01016 01017 /** Get maximum length of dist value. 01018 * @return length of dist value, can be length of the array or number of 01019 * maximum number of characters for a string 01020 */ 01021 size_t 01022 NavigatorInterface::PolarGotoMessage::maxlenof_dist() const 01023 { 01024 return 1; 01025 } 01026 01027 /** Set dist value. 01028 * Distance to the target. 01029 * @param new_dist new dist value 01030 */ 01031 void 01032 NavigatorInterface::PolarGotoMessage::set_dist(const float new_dist) 01033 { 01034 data->dist = new_dist; 01035 } 01036 01037 /** Get orientation value. 01038 * The orientation of the robot at the target. 01039 * @return orientation value 01040 */ 01041 float 01042 NavigatorInterface::PolarGotoMessage::orientation() const 01043 { 01044 return data->orientation; 01045 } 01046 01047 /** Get maximum length of orientation value. 01048 * @return length of orientation value, can be length of the array or number of 01049 * maximum number of characters for a string 01050 */ 01051 size_t 01052 NavigatorInterface::PolarGotoMessage::maxlenof_orientation() const 01053 { 01054 return 1; 01055 } 01056 01057 /** Set orientation value. 01058 * The orientation of the robot at the target. 01059 * @param new_orientation new orientation value 01060 */ 01061 void 01062 NavigatorInterface::PolarGotoMessage::set_orientation(const float new_orientation) 01063 { 01064 data->orientation = new_orientation; 01065 } 01066 01067 /** Clone this message. 01068 * Produces a message of the same type as this message and copies the 01069 * data to the new message. 01070 * @return clone of this message 01071 */ 01072 Message * 01073 NavigatorInterface::PolarGotoMessage::clone() const 01074 { 01075 return new NavigatorInterface::PolarGotoMessage(this); 01076 } 01077 /** @class NavigatorInterface::PlaceGotoMessage <interfaces/NavigatorInterface.h> 01078 * PlaceGotoMessage Fawkes BlackBoard Interface Message. 01079 * 01080 01081 */ 01082 01083 01084 /** Constructor with initial values. 01085 * @param ini_place initial value for place 01086 */ 01087 NavigatorInterface::PlaceGotoMessage::PlaceGotoMessage(const char * ini_place) : Message("PlaceGotoMessage") 01088 { 01089 data_size = sizeof(PlaceGotoMessage_data_t); 01090 data_ptr = malloc(data_size); 01091 memset(data_ptr, 0, data_size); 01092 data = (PlaceGotoMessage_data_t *)data_ptr; 01093 data_ts = (message_data_ts_t *)data_ptr; 01094 strncpy(data->place, ini_place, 64); 01095 add_fieldinfo(IFT_STRING, "place", 64, data->place); 01096 } 01097 /** Constructor */ 01098 NavigatorInterface::PlaceGotoMessage::PlaceGotoMessage() : Message("PlaceGotoMessage") 01099 { 01100 data_size = sizeof(PlaceGotoMessage_data_t); 01101 data_ptr = malloc(data_size); 01102 memset(data_ptr, 0, data_size); 01103 data = (PlaceGotoMessage_data_t *)data_ptr; 01104 data_ts = (message_data_ts_t *)data_ptr; 01105 add_fieldinfo(IFT_STRING, "place", 64, data->place); 01106 } 01107 01108 /** Destructor */ 01109 NavigatorInterface::PlaceGotoMessage::~PlaceGotoMessage() 01110 { 01111 free(data_ptr); 01112 } 01113 01114 /** Copy constructor. 01115 * @param m message to copy from 01116 */ 01117 NavigatorInterface::PlaceGotoMessage::PlaceGotoMessage(const PlaceGotoMessage *m) : Message("PlaceGotoMessage") 01118 { 01119 data_size = m->data_size; 01120 data_ptr = malloc(data_size); 01121 memcpy(data_ptr, m->data_ptr, data_size); 01122 data = (PlaceGotoMessage_data_t *)data_ptr; 01123 data_ts = (message_data_ts_t *)data_ptr; 01124 } 01125 01126 /* Methods */ 01127 /** Get place value. 01128 * Place to go to. 01129 * @return place value 01130 */ 01131 char * 01132 NavigatorInterface::PlaceGotoMessage::place() const 01133 { 01134 return data->place; 01135 } 01136 01137 /** Get maximum length of place value. 01138 * @return length of place value, can be length of the array or number of 01139 * maximum number of characters for a string 01140 */ 01141 size_t 01142 NavigatorInterface::PlaceGotoMessage::maxlenof_place() const 01143 { 01144 return 64; 01145 } 01146 01147 /** Set place value. 01148 * Place to go to. 01149 * @param new_place new place value 01150 */ 01151 void 01152 NavigatorInterface::PlaceGotoMessage::set_place(const char * new_place) 01153 { 01154 strncpy(data->place, new_place, sizeof(data->place)); 01155 } 01156 01157 /** Clone this message. 01158 * Produces a message of the same type as this message and copies the 01159 * data to the new message. 01160 * @return clone of this message 01161 */ 01162 Message * 01163 NavigatorInterface::PlaceGotoMessage::clone() const 01164 { 01165 return new NavigatorInterface::PlaceGotoMessage(this); 01166 } 01167 /** @class NavigatorInterface::ObstacleMessage <interfaces/NavigatorInterface.h> 01168 * ObstacleMessage Fawkes BlackBoard Interface Message. 01169 * 01170 01171 */ 01172 01173 01174 /** Constructor with initial values. 01175 * @param ini_x initial value for x 01176 * @param ini_y initial value for y 01177 * @param ini_width initial value for width 01178 */ 01179 NavigatorInterface::ObstacleMessage::ObstacleMessage(const float ini_x, const float ini_y, const float ini_width) : Message("ObstacleMessage") 01180 { 01181 data_size = sizeof(ObstacleMessage_data_t); 01182 data_ptr = malloc(data_size); 01183 memset(data_ptr, 0, data_size); 01184 data = (ObstacleMessage_data_t *)data_ptr; 01185 data_ts = (message_data_ts_t *)data_ptr; 01186 data->x = ini_x; 01187 data->y = ini_y; 01188 data->width = ini_width; 01189 add_fieldinfo(IFT_FLOAT, "x", 1, &data->x); 01190 add_fieldinfo(IFT_FLOAT, "y", 1, &data->y); 01191 add_fieldinfo(IFT_FLOAT, "width", 1, &data->width); 01192 } 01193 /** Constructor */ 01194 NavigatorInterface::ObstacleMessage::ObstacleMessage() : Message("ObstacleMessage") 01195 { 01196 data_size = sizeof(ObstacleMessage_data_t); 01197 data_ptr = malloc(data_size); 01198 memset(data_ptr, 0, data_size); 01199 data = (ObstacleMessage_data_t *)data_ptr; 01200 data_ts = (message_data_ts_t *)data_ptr; 01201 add_fieldinfo(IFT_FLOAT, "x", 1, &data->x); 01202 add_fieldinfo(IFT_FLOAT, "y", 1, &data->y); 01203 add_fieldinfo(IFT_FLOAT, "width", 1, &data->width); 01204 } 01205 01206 /** Destructor */ 01207 NavigatorInterface::ObstacleMessage::~ObstacleMessage() 01208 { 01209 free(data_ptr); 01210 } 01211 01212 /** Copy constructor. 01213 * @param m message to copy from 01214 */ 01215 NavigatorInterface::ObstacleMessage::ObstacleMessage(const ObstacleMessage *m) : Message("ObstacleMessage") 01216 { 01217 data_size = m->data_size; 01218 data_ptr = malloc(data_size); 01219 memcpy(data_ptr, m->data_ptr, data_size); 01220 data = (ObstacleMessage_data_t *)data_ptr; 01221 data_ts = (message_data_ts_t *)data_ptr; 01222 } 01223 01224 /* Methods */ 01225 /** Get x value. 01226 * X-coordinate of the obstacle. 01227 * @return x value 01228 */ 01229 float 01230 NavigatorInterface::ObstacleMessage::x() const 01231 { 01232 return data->x; 01233 } 01234 01235 /** Get maximum length of x value. 01236 * @return length of x value, can be length of the array or number of 01237 * maximum number of characters for a string 01238 */ 01239 size_t 01240 NavigatorInterface::ObstacleMessage::maxlenof_x() const 01241 { 01242 return 1; 01243 } 01244 01245 /** Set x value. 01246 * X-coordinate of the obstacle. 01247 * @param new_x new x value 01248 */ 01249 void 01250 NavigatorInterface::ObstacleMessage::set_x(const float new_x) 01251 { 01252 data->x = new_x; 01253 } 01254 01255 /** Get y value. 01256 * Y-coordinate of the obstacle. 01257 * @return y value 01258 */ 01259 float 01260 NavigatorInterface::ObstacleMessage::y() const 01261 { 01262 return data->y; 01263 } 01264 01265 /** Get maximum length of y value. 01266 * @return length of y value, can be length of the array or number of 01267 * maximum number of characters for a string 01268 */ 01269 size_t 01270 NavigatorInterface::ObstacleMessage::maxlenof_y() const 01271 { 01272 return 1; 01273 } 01274 01275 /** Set y value. 01276 * Y-coordinate of the obstacle. 01277 * @param new_y new y value 01278 */ 01279 void 01280 NavigatorInterface::ObstacleMessage::set_y(const float new_y) 01281 { 01282 data->y = new_y; 01283 } 01284 01285 /** Get width value. 01286 * Width of the obstacle. 01287 * @return width value 01288 */ 01289 float 01290 NavigatorInterface::ObstacleMessage::width() const 01291 { 01292 return data->width; 01293 } 01294 01295 /** Get maximum length of width value. 01296 * @return length of width value, can be length of the array or number of 01297 * maximum number of characters for a string 01298 */ 01299 size_t 01300 NavigatorInterface::ObstacleMessage::maxlenof_width() const 01301 { 01302 return 1; 01303 } 01304 01305 /** Set width value. 01306 * Width of the obstacle. 01307 * @param new_width new width value 01308 */ 01309 void 01310 NavigatorInterface::ObstacleMessage::set_width(const float new_width) 01311 { 01312 data->width = new_width; 01313 } 01314 01315 /** Clone this message. 01316 * Produces a message of the same type as this message and copies the 01317 * data to the new message. 01318 * @return clone of this message 01319 */ 01320 Message * 01321 NavigatorInterface::ObstacleMessage::clone() const 01322 { 01323 return new NavigatorInterface::ObstacleMessage(this); 01324 } 01325 /** @class NavigatorInterface::ResetOdometryMessage <interfaces/NavigatorInterface.h> 01326 * ResetOdometryMessage Fawkes BlackBoard Interface Message. 01327 * 01328 01329 */ 01330 01331 01332 /** Constructor */ 01333 NavigatorInterface::ResetOdometryMessage::ResetOdometryMessage() : Message("ResetOdometryMessage") 01334 { 01335 data_size = sizeof(ResetOdometryMessage_data_t); 01336 data_ptr = malloc(data_size); 01337 memset(data_ptr, 0, data_size); 01338 data = (ResetOdometryMessage_data_t *)data_ptr; 01339 data_ts = (message_data_ts_t *)data_ptr; 01340 } 01341 01342 /** Destructor */ 01343 NavigatorInterface::ResetOdometryMessage::~ResetOdometryMessage() 01344 { 01345 free(data_ptr); 01346 } 01347 01348 /** Copy constructor. 01349 * @param m message to copy from 01350 */ 01351 NavigatorInterface::ResetOdometryMessage::ResetOdometryMessage(const ResetOdometryMessage *m) : Message("ResetOdometryMessage") 01352 { 01353 data_size = m->data_size; 01354 data_ptr = malloc(data_size); 01355 memcpy(data_ptr, m->data_ptr, data_size); 01356 data = (ResetOdometryMessage_data_t *)data_ptr; 01357 data_ts = (message_data_ts_t *)data_ptr; 01358 } 01359 01360 /* Methods */ 01361 /** Clone this message. 01362 * Produces a message of the same type as this message and copies the 01363 * data to the new message. 01364 * @return clone of this message 01365 */ 01366 Message * 01367 NavigatorInterface::ResetOdometryMessage::clone() const 01368 { 01369 return new NavigatorInterface::ResetOdometryMessage(this); 01370 } 01371 /** @class NavigatorInterface::SetMaxVelocityMessage <interfaces/NavigatorInterface.h> 01372 * SetMaxVelocityMessage Fawkes BlackBoard Interface Message. 01373 * 01374 01375 */ 01376 01377 01378 /** Constructor with initial values. 01379 * @param ini_max_velocity initial value for max_velocity 01380 */ 01381 NavigatorInterface::SetMaxVelocityMessage::SetMaxVelocityMessage(const float ini_max_velocity) : Message("SetMaxVelocityMessage") 01382 { 01383 data_size = sizeof(SetMaxVelocityMessage_data_t); 01384 data_ptr = malloc(data_size); 01385 memset(data_ptr, 0, data_size); 01386 data = (SetMaxVelocityMessage_data_t *)data_ptr; 01387 data_ts = (message_data_ts_t *)data_ptr; 01388 data->max_velocity = ini_max_velocity; 01389 add_fieldinfo(IFT_FLOAT, "max_velocity", 1, &data->max_velocity); 01390 } 01391 /** Constructor */ 01392 NavigatorInterface::SetMaxVelocityMessage::SetMaxVelocityMessage() : Message("SetMaxVelocityMessage") 01393 { 01394 data_size = sizeof(SetMaxVelocityMessage_data_t); 01395 data_ptr = malloc(data_size); 01396 memset(data_ptr, 0, data_size); 01397 data = (SetMaxVelocityMessage_data_t *)data_ptr; 01398 data_ts = (message_data_ts_t *)data_ptr; 01399 add_fieldinfo(IFT_FLOAT, "max_velocity", 1, &data->max_velocity); 01400 } 01401 01402 /** Destructor */ 01403 NavigatorInterface::SetMaxVelocityMessage::~SetMaxVelocityMessage() 01404 { 01405 free(data_ptr); 01406 } 01407 01408 /** Copy constructor. 01409 * @param m message to copy from 01410 */ 01411 NavigatorInterface::SetMaxVelocityMessage::SetMaxVelocityMessage(const SetMaxVelocityMessage *m) : Message("SetMaxVelocityMessage") 01412 { 01413 data_size = m->data_size; 01414 data_ptr = malloc(data_size); 01415 memcpy(data_ptr, m->data_ptr, data_size); 01416 data = (SetMaxVelocityMessage_data_t *)data_ptr; 01417 data_ts = (message_data_ts_t *)data_ptr; 01418 } 01419 01420 /* Methods */ 01421 /** Get max_velocity value. 01422 * Maximum velocity 01423 * @return max_velocity value 01424 */ 01425 float 01426 NavigatorInterface::SetMaxVelocityMessage::max_velocity() const 01427 { 01428 return data->max_velocity; 01429 } 01430 01431 /** Get maximum length of max_velocity value. 01432 * @return length of max_velocity value, can be length of the array or number of 01433 * maximum number of characters for a string 01434 */ 01435 size_t 01436 NavigatorInterface::SetMaxVelocityMessage::maxlenof_max_velocity() const 01437 { 01438 return 1; 01439 } 01440 01441 /** Set max_velocity value. 01442 * Maximum velocity 01443 * @param new_max_velocity new max_velocity value 01444 */ 01445 void 01446 NavigatorInterface::SetMaxVelocityMessage::set_max_velocity(const float new_max_velocity) 01447 { 01448 data->max_velocity = new_max_velocity; 01449 } 01450 01451 /** Clone this message. 01452 * Produces a message of the same type as this message and copies the 01453 * data to the new message. 01454 * @return clone of this message 01455 */ 01456 Message * 01457 NavigatorInterface::SetMaxVelocityMessage::clone() const 01458 { 01459 return new NavigatorInterface::SetMaxVelocityMessage(this); 01460 } 01461 /** @class NavigatorInterface::SetEscapingMessage <interfaces/NavigatorInterface.h> 01462 * SetEscapingMessage Fawkes BlackBoard Interface Message. 01463 * 01464 01465 */ 01466 01467 01468 /** Constructor with initial values. 01469 * @param ini_escaping_enabled initial value for escaping_enabled 01470 */ 01471 NavigatorInterface::SetEscapingMessage::SetEscapingMessage(const bool ini_escaping_enabled) : Message("SetEscapingMessage") 01472 { 01473 data_size = sizeof(SetEscapingMessage_data_t); 01474 data_ptr = malloc(data_size); 01475 memset(data_ptr, 0, data_size); 01476 data = (SetEscapingMessage_data_t *)data_ptr; 01477 data_ts = (message_data_ts_t *)data_ptr; 01478 data->escaping_enabled = ini_escaping_enabled; 01479 add_fieldinfo(IFT_BOOL, "escaping_enabled", 1, &data->escaping_enabled); 01480 } 01481 /** Constructor */ 01482 NavigatorInterface::SetEscapingMessage::SetEscapingMessage() : Message("SetEscapingMessage") 01483 { 01484 data_size = sizeof(SetEscapingMessage_data_t); 01485 data_ptr = malloc(data_size); 01486 memset(data_ptr, 0, data_size); 01487 data = (SetEscapingMessage_data_t *)data_ptr; 01488 data_ts = (message_data_ts_t *)data_ptr; 01489 add_fieldinfo(IFT_BOOL, "escaping_enabled", 1, &data->escaping_enabled); 01490 } 01491 01492 /** Destructor */ 01493 NavigatorInterface::SetEscapingMessage::~SetEscapingMessage() 01494 { 01495 free(data_ptr); 01496 } 01497 01498 /** Copy constructor. 01499 * @param m message to copy from 01500 */ 01501 NavigatorInterface::SetEscapingMessage::SetEscapingMessage(const SetEscapingMessage *m) : Message("SetEscapingMessage") 01502 { 01503 data_size = m->data_size; 01504 data_ptr = malloc(data_size); 01505 memcpy(data_ptr, m->data_ptr, data_size); 01506 data = (SetEscapingMessage_data_t *)data_ptr; 01507 data_ts = (message_data_ts_t *)data_ptr; 01508 } 01509 01510 /* Methods */ 01511 /** Get escaping_enabled value. 01512 * This is used for 01513 navigation components with integrated collision avoidance, to 01514 check whether the navigator should stop when an obstacle 01515 obstructs the path, or if it should escape. 01516 * @return escaping_enabled value 01517 */ 01518 bool 01519 NavigatorInterface::SetEscapingMessage::is_escaping_enabled() const 01520 { 01521 return data->escaping_enabled; 01522 } 01523 01524 /** Get maximum length of escaping_enabled value. 01525 * @return length of escaping_enabled value, can be length of the array or number of 01526 * maximum number of characters for a string 01527 */ 01528 size_t 01529 NavigatorInterface::SetEscapingMessage::maxlenof_escaping_enabled() const 01530 { 01531 return 1; 01532 } 01533 01534 /** Set escaping_enabled value. 01535 * This is used for 01536 navigation components with integrated collision avoidance, to 01537 check whether the navigator should stop when an obstacle 01538 obstructs the path, or if it should escape. 01539 * @param new_escaping_enabled new escaping_enabled value 01540 */ 01541 void 01542 NavigatorInterface::SetEscapingMessage::set_escaping_enabled(const bool new_escaping_enabled) 01543 { 01544 data->escaping_enabled = new_escaping_enabled; 01545 } 01546 01547 /** Clone this message. 01548 * Produces a message of the same type as this message and copies the 01549 * data to the new message. 01550 * @return clone of this message 01551 */ 01552 Message * 01553 NavigatorInterface::SetEscapingMessage::clone() const 01554 { 01555 return new NavigatorInterface::SetEscapingMessage(this); 01556 } 01557 /** @class NavigatorInterface::SetSecurityDistanceMessage <interfaces/NavigatorInterface.h> 01558 * SetSecurityDistanceMessage Fawkes BlackBoard Interface Message. 01559 * 01560 01561 */ 01562 01563 01564 /** Constructor with initial values. 01565 * @param ini_security_distance initial value for security_distance 01566 */ 01567 NavigatorInterface::SetSecurityDistanceMessage::SetSecurityDistanceMessage(const float ini_security_distance) : Message("SetSecurityDistanceMessage") 01568 { 01569 data_size = sizeof(SetSecurityDistanceMessage_data_t); 01570 data_ptr = malloc(data_size); 01571 memset(data_ptr, 0, data_size); 01572 data = (SetSecurityDistanceMessage_data_t *)data_ptr; 01573 data_ts = (message_data_ts_t *)data_ptr; 01574 data->security_distance = ini_security_distance; 01575 add_fieldinfo(IFT_FLOAT, "security_distance", 1, &data->security_distance); 01576 } 01577 /** Constructor */ 01578 NavigatorInterface::SetSecurityDistanceMessage::SetSecurityDistanceMessage() : Message("SetSecurityDistanceMessage") 01579 { 01580 data_size = sizeof(SetSecurityDistanceMessage_data_t); 01581 data_ptr = malloc(data_size); 01582 memset(data_ptr, 0, data_size); 01583 data = (SetSecurityDistanceMessage_data_t *)data_ptr; 01584 data_ts = (message_data_ts_t *)data_ptr; 01585 add_fieldinfo(IFT_FLOAT, "security_distance", 1, &data->security_distance); 01586 } 01587 01588 /** Destructor */ 01589 NavigatorInterface::SetSecurityDistanceMessage::~SetSecurityDistanceMessage() 01590 { 01591 free(data_ptr); 01592 } 01593 01594 /** Copy constructor. 01595 * @param m message to copy from 01596 */ 01597 NavigatorInterface::SetSecurityDistanceMessage::SetSecurityDistanceMessage(const SetSecurityDistanceMessage *m) : Message("SetSecurityDistanceMessage") 01598 { 01599 data_size = m->data_size; 01600 data_ptr = malloc(data_size); 01601 memcpy(data_ptr, m->data_ptr, data_size); 01602 data = (SetSecurityDistanceMessage_data_t *)data_ptr; 01603 data_ts = (message_data_ts_t *)data_ptr; 01604 } 01605 01606 /* Methods */ 01607 /** Get security_distance value. 01608 * Security distance to 01609 keep to obstacles 01610 * @return security_distance value 01611 */ 01612 float 01613 NavigatorInterface::SetSecurityDistanceMessage::security_distance() const 01614 { 01615 return data->security_distance; 01616 } 01617 01618 /** Get maximum length of security_distance value. 01619 * @return length of security_distance value, can be length of the array or number of 01620 * maximum number of characters for a string 01621 */ 01622 size_t 01623 NavigatorInterface::SetSecurityDistanceMessage::maxlenof_security_distance() const 01624 { 01625 return 1; 01626 } 01627 01628 /** Set security_distance value. 01629 * Security distance to 01630 keep to obstacles 01631 * @param new_security_distance new security_distance value 01632 */ 01633 void 01634 NavigatorInterface::SetSecurityDistanceMessage::set_security_distance(const float new_security_distance) 01635 { 01636 data->security_distance = new_security_distance; 01637 } 01638 01639 /** Clone this message. 01640 * Produces a message of the same type as this message and copies the 01641 * data to the new message. 01642 * @return clone of this message 01643 */ 01644 Message * 01645 NavigatorInterface::SetSecurityDistanceMessage::clone() const 01646 { 01647 return new NavigatorInterface::SetSecurityDistanceMessage(this); 01648 } 01649 /** Check if message is valid and can be enqueued. 01650 * @param message Message to check 01651 * @return true if the message is valid, false otherwise. 01652 */ 01653 bool 01654 NavigatorInterface::message_valid(const Message *message) const 01655 { 01656 const StopMessage *m0 = dynamic_cast<const StopMessage *>(message); 01657 if ( m0 != NULL ) { 01658 return true; 01659 } 01660 const TurnMessage *m1 = dynamic_cast<const TurnMessage *>(message); 01661 if ( m1 != NULL ) { 01662 return true; 01663 } 01664 const CartesianGotoMessage *m2 = dynamic_cast<const CartesianGotoMessage *>(message); 01665 if ( m2 != NULL ) { 01666 return true; 01667 } 01668 const PolarGotoMessage *m3 = dynamic_cast<const PolarGotoMessage *>(message); 01669 if ( m3 != NULL ) { 01670 return true; 01671 } 01672 const PlaceGotoMessage *m4 = dynamic_cast<const PlaceGotoMessage *>(message); 01673 if ( m4 != NULL ) { 01674 return true; 01675 } 01676 const ObstacleMessage *m5 = dynamic_cast<const ObstacleMessage *>(message); 01677 if ( m5 != NULL ) { 01678 return true; 01679 } 01680 const ResetOdometryMessage *m6 = dynamic_cast<const ResetOdometryMessage *>(message); 01681 if ( m6 != NULL ) { 01682 return true; 01683 } 01684 const SetMaxVelocityMessage *m7 = dynamic_cast<const SetMaxVelocityMessage *>(message); 01685 if ( m7 != NULL ) { 01686 return true; 01687 } 01688 const SetEscapingMessage *m8 = dynamic_cast<const SetEscapingMessage *>(message); 01689 if ( m8 != NULL ) { 01690 return true; 01691 } 01692 const SetSecurityDistanceMessage *m9 = dynamic_cast<const SetSecurityDistanceMessage *>(message); 01693 if ( m9 != NULL ) { 01694 return true; 01695 } 01696 return false; 01697 } 01698 01699 /// @cond INTERNALS 01700 EXPORT_INTERFACE(NavigatorInterface) 01701 /// @endcond 01702 01703 01704 } // end namespace fawkes