00001
00002
00003
00004
00005
00006
00007
00008
00009
00010
00011
00012
00013
00014
00015
00016
00017
00018
00019
00020
00021
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
00034
00035
00036
00037
00038
00039
00040
00041
00042
00043
00044
00045
00046
00047
00048
00049
00050
00051
00052
00053 const uint32_t NavigatorInterface::ERROR_NONE = 0u;
00054
00055 const uint32_t NavigatorInterface::ERROR_MOTOR = 1u;
00056
00057 const uint32_t NavigatorInterface::ERROR_OBSTRUCTION = 2u;
00058
00059 const uint32_t NavigatorInterface::ERROR_UNKNOWN_PLACE = 4u;
00060
00061 const uint32_t NavigatorInterface::FLAG_NONE = 0u;
00062
00063 const uint32_t NavigatorInterface::FLAG_CART_GOTO = 1u;
00064
00065 const uint32_t NavigatorInterface::FLAG_POLAR_GOTO = 2u;
00066
00067 const uint32_t NavigatorInterface::FLAG_PLACE_GOTO = 4u;
00068
00069 const uint32_t NavigatorInterface::FLAG_UPDATES_DEST_DIST = 8u;
00070
00071 const uint32_t NavigatorInterface::FLAG_SECURITY_DISTANCE = 16u;
00072
00073 const uint32_t NavigatorInterface::FLAG_ESCAPING = 32u;
00074
00075
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
00111 NavigatorInterface::~NavigatorInterface()
00112 {
00113 free(data_ptr);
00114 }
00115
00116
00117
00118
00119
00120
00121 uint32_t
00122 NavigatorInterface::flags() const
00123 {
00124 return data->flags;
00125 }
00126
00127
00128
00129
00130
00131 size_t
00132 NavigatorInterface::maxlenof_flags() const
00133 {
00134 return 1;
00135 }
00136
00137
00138
00139
00140
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
00150
00151
00152
00153 float
00154 NavigatorInterface::x() const
00155 {
00156 return data->x;
00157 }
00158
00159
00160
00161
00162
00163 size_t
00164 NavigatorInterface::maxlenof_x() const
00165 {
00166 return 1;
00167 }
00168
00169
00170
00171
00172
00173 void
00174 NavigatorInterface::set_x(const float new_x)
00175 {
00176 data->x = new_x;
00177 data_changed = true;
00178 }
00179
00180
00181
00182
00183
00184 float
00185 NavigatorInterface::y() const
00186 {
00187 return data->y;
00188 }
00189
00190
00191
00192
00193
00194 size_t
00195 NavigatorInterface::maxlenof_y() const
00196 {
00197 return 1;
00198 }
00199
00200
00201
00202
00203
00204 void
00205 NavigatorInterface::set_y(const float new_y)
00206 {
00207 data->y = new_y;
00208 data_changed = true;
00209 }
00210
00211
00212
00213
00214
00215 float
00216 NavigatorInterface::dest_x() const
00217 {
00218 return data->dest_x;
00219 }
00220
00221
00222
00223
00224
00225 size_t
00226 NavigatorInterface::maxlenof_dest_x() const
00227 {
00228 return 1;
00229 }
00230
00231
00232
00233
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
00243
00244
00245
00246 float
00247 NavigatorInterface::dest_y() const
00248 {
00249 return data->dest_y;
00250 }
00251
00252
00253
00254
00255
00256 size_t
00257 NavigatorInterface::maxlenof_dest_y() const
00258 {
00259 return 1;
00260 }
00261
00262
00263
00264
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
00274
00275
00276
00277 float
00278 NavigatorInterface::dest_ori() const
00279 {
00280 return data->dest_ori;
00281 }
00282
00283
00284
00285
00286
00287 size_t
00288 NavigatorInterface::maxlenof_dest_ori() const
00289 {
00290 return 1;
00291 }
00292
00293
00294
00295
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
00305
00306
00307
00308 float
00309 NavigatorInterface::dest_dist() const
00310 {
00311 return data->dest_dist;
00312 }
00313
00314
00315
00316
00317
00318 size_t
00319 NavigatorInterface::maxlenof_dest_dist() const
00320 {
00321 return 1;
00322 }
00323
00324
00325
00326
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
00336
00337
00338
00339
00340 uint32_t
00341 NavigatorInterface::msgid() const
00342 {
00343 return data->msgid;
00344 }
00345
00346
00347
00348
00349
00350 size_t
00351 NavigatorInterface::maxlenof_msgid() const
00352 {
00353 return 1;
00354 }
00355
00356
00357
00358
00359
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
00369
00370
00371
00372
00373 bool
00374 NavigatorInterface::is_final() const
00375 {
00376 return data->final;
00377 }
00378
00379
00380
00381
00382
00383 size_t
00384 NavigatorInterface::maxlenof_final() const
00385 {
00386 return 1;
00387 }
00388
00389
00390
00391
00392
00393
00394 void
00395 NavigatorInterface::set_final(const bool new_final)
00396 {
00397 data->final = new_final;
00398 data_changed = true;
00399 }
00400
00401
00402
00403
00404
00405
00406
00407 uint32_t
00408 NavigatorInterface::error_code() const
00409 {
00410 return data->error_code;
00411 }
00412
00413
00414
00415
00416
00417 size_t
00418 NavigatorInterface::maxlenof_error_code() const
00419 {
00420 return 1;
00421 }
00422
00423
00424
00425
00426
00427
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
00437
00438
00439
00440 float
00441 NavigatorInterface::max_velocity() const
00442 {
00443 return data->max_velocity;
00444 }
00445
00446
00447
00448
00449
00450 size_t
00451 NavigatorInterface::maxlenof_max_velocity() const
00452 {
00453 return 1;
00454 }
00455
00456
00457
00458
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
00468
00469
00470
00471
00472 float
00473 NavigatorInterface::security_distance() const
00474 {
00475 return data->security_distance;
00476 }
00477
00478
00479
00480
00481
00482 size_t
00483 NavigatorInterface::maxlenof_security_distance() const
00484 {
00485 return 1;
00486 }
00487
00488
00489
00490
00491
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
00501
00502
00503
00504
00505
00506
00507 bool
00508 NavigatorInterface::is_escaping_enabled() const
00509 {
00510 return data->escaping_enabled;
00511 }
00512
00513
00514
00515
00516
00517 size_t
00518 NavigatorInterface::maxlenof_escaping_enabled() const
00519 {
00520 return 1;
00521 }
00522
00523
00524
00525
00526
00527
00528
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
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
00569
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
00589
00590
00591
00592
00593
00594
00595
00596
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
00607 NavigatorInterface::StopMessage::~StopMessage()
00608 {
00609 free(data_ptr);
00610 }
00611
00612
00613
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
00625
00626
00627
00628
00629
00630 Message *
00631 NavigatorInterface::StopMessage::clone() const
00632 {
00633 return new NavigatorInterface::StopMessage(this);
00634 }
00635
00636
00637
00638
00639
00640
00641
00642
00643
00644
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
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
00671 NavigatorInterface::TurnMessage::~TurnMessage()
00672 {
00673 free(data_ptr);
00674 }
00675
00676
00677
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
00689
00690
00691
00692
00693 float
00694 NavigatorInterface::TurnMessage::angle() const
00695 {
00696 return data->angle;
00697 }
00698
00699
00700
00701
00702
00703 size_t
00704 NavigatorInterface::TurnMessage::maxlenof_angle() const
00705 {
00706 return 1;
00707 }
00708
00709
00710
00711
00712
00713 void
00714 NavigatorInterface::TurnMessage::set_angle(const float new_angle)
00715 {
00716 data->angle = new_angle;
00717 }
00718
00719
00720
00721
00722
00723
00724 float
00725 NavigatorInterface::TurnMessage::velocity() const
00726 {
00727 return data->velocity;
00728 }
00729
00730
00731
00732
00733
00734 size_t
00735 NavigatorInterface::TurnMessage::maxlenof_velocity() const
00736 {
00737 return 1;
00738 }
00739
00740
00741
00742
00743
00744
00745 void
00746 NavigatorInterface::TurnMessage::set_velocity(const float new_velocity)
00747 {
00748 data->velocity = new_velocity;
00749 }
00750
00751
00752
00753
00754
00755
00756 Message *
00757 NavigatorInterface::TurnMessage::clone() const
00758 {
00759 return new NavigatorInterface::TurnMessage(this);
00760 }
00761
00762
00763
00764
00765
00766
00767
00768
00769
00770
00771
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
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
00801 NavigatorInterface::CartesianGotoMessage::~CartesianGotoMessage()
00802 {
00803 free(data_ptr);
00804 }
00805
00806
00807
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
00819
00820
00821
00822
00823 float
00824 NavigatorInterface::CartesianGotoMessage::x() const
00825 {
00826 return data->x;
00827 }
00828
00829
00830
00831
00832
00833 size_t
00834 NavigatorInterface::CartesianGotoMessage::maxlenof_x() const
00835 {
00836 return 1;
00837 }
00838
00839
00840
00841
00842
00843 void
00844 NavigatorInterface::CartesianGotoMessage::set_x(const float new_x)
00845 {
00846 data->x = new_x;
00847 }
00848
00849
00850
00851
00852
00853 float
00854 NavigatorInterface::CartesianGotoMessage::y() const
00855 {
00856 return data->y;
00857 }
00858
00859
00860
00861
00862
00863 size_t
00864 NavigatorInterface::CartesianGotoMessage::maxlenof_y() const
00865 {
00866 return 1;
00867 }
00868
00869
00870
00871
00872
00873 void
00874 NavigatorInterface::CartesianGotoMessage::set_y(const float new_y)
00875 {
00876 data->y = new_y;
00877 }
00878
00879
00880
00881
00882
00883 float
00884 NavigatorInterface::CartesianGotoMessage::orientation() const
00885 {
00886 return data->orientation;
00887 }
00888
00889
00890
00891
00892
00893 size_t
00894 NavigatorInterface::CartesianGotoMessage::maxlenof_orientation() const
00895 {
00896 return 1;
00897 }
00898
00899
00900
00901
00902
00903 void
00904 NavigatorInterface::CartesianGotoMessage::set_orientation(const float new_orientation)
00905 {
00906 data->orientation = new_orientation;
00907 }
00908
00909
00910
00911
00912
00913
00914 Message *
00915 NavigatorInterface::CartesianGotoMessage::clone() const
00916 {
00917 return new NavigatorInterface::CartesianGotoMessage(this);
00918 }
00919
00920
00921
00922
00923
00924
00925
00926
00927
00928
00929
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
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
00959 NavigatorInterface::PolarGotoMessage::~PolarGotoMessage()
00960 {
00961 free(data_ptr);
00962 }
00963
00964
00965
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
00977
00978
00979
00980
00981 float
00982 NavigatorInterface::PolarGotoMessage::phi() const
00983 {
00984 return data->phi;
00985 }
00986
00987
00988
00989
00990
00991 size_t
00992 NavigatorInterface::PolarGotoMessage::maxlenof_phi() const
00993 {
00994 return 1;
00995 }
00996
00997
00998
00999
01000
01001 void
01002 NavigatorInterface::PolarGotoMessage::set_phi(const float new_phi)
01003 {
01004 data->phi = new_phi;
01005 }
01006
01007
01008
01009
01010
01011 float
01012 NavigatorInterface::PolarGotoMessage::dist() const
01013 {
01014 return data->dist;
01015 }
01016
01017
01018
01019
01020
01021 size_t
01022 NavigatorInterface::PolarGotoMessage::maxlenof_dist() const
01023 {
01024 return 1;
01025 }
01026
01027
01028
01029
01030
01031 void
01032 NavigatorInterface::PolarGotoMessage::set_dist(const float new_dist)
01033 {
01034 data->dist = new_dist;
01035 }
01036
01037
01038
01039
01040
01041 float
01042 NavigatorInterface::PolarGotoMessage::orientation() const
01043 {
01044 return data->orientation;
01045 }
01046
01047
01048
01049
01050
01051 size_t
01052 NavigatorInterface::PolarGotoMessage::maxlenof_orientation() const
01053 {
01054 return 1;
01055 }
01056
01057
01058
01059
01060
01061 void
01062 NavigatorInterface::PolarGotoMessage::set_orientation(const float new_orientation)
01063 {
01064 data->orientation = new_orientation;
01065 }
01066
01067
01068
01069
01070
01071
01072 Message *
01073 NavigatorInterface::PolarGotoMessage::clone() const
01074 {
01075 return new NavigatorInterface::PolarGotoMessage(this);
01076 }
01077
01078
01079
01080
01081
01082
01083
01084
01085
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
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
01109 NavigatorInterface::PlaceGotoMessage::~PlaceGotoMessage()
01110 {
01111 free(data_ptr);
01112 }
01113
01114
01115
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
01127
01128
01129
01130
01131 char *
01132 NavigatorInterface::PlaceGotoMessage::place() const
01133 {
01134 return data->place;
01135 }
01136
01137
01138
01139
01140
01141 size_t
01142 NavigatorInterface::PlaceGotoMessage::maxlenof_place() const
01143 {
01144 return 64;
01145 }
01146
01147
01148
01149
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
01158
01159
01160
01161
01162 Message *
01163 NavigatorInterface::PlaceGotoMessage::clone() const
01164 {
01165 return new NavigatorInterface::PlaceGotoMessage(this);
01166 }
01167
01168
01169
01170
01171
01172
01173
01174
01175
01176
01177
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
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
01207 NavigatorInterface::ObstacleMessage::~ObstacleMessage()
01208 {
01209 free(data_ptr);
01210 }
01211
01212
01213
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
01225
01226
01227
01228
01229 float
01230 NavigatorInterface::ObstacleMessage::x() const
01231 {
01232 return data->x;
01233 }
01234
01235
01236
01237
01238
01239 size_t
01240 NavigatorInterface::ObstacleMessage::maxlenof_x() const
01241 {
01242 return 1;
01243 }
01244
01245
01246
01247
01248
01249 void
01250 NavigatorInterface::ObstacleMessage::set_x(const float new_x)
01251 {
01252 data->x = new_x;
01253 }
01254
01255
01256
01257
01258
01259 float
01260 NavigatorInterface::ObstacleMessage::y() const
01261 {
01262 return data->y;
01263 }
01264
01265
01266
01267
01268
01269 size_t
01270 NavigatorInterface::ObstacleMessage::maxlenof_y() const
01271 {
01272 return 1;
01273 }
01274
01275
01276
01277
01278
01279 void
01280 NavigatorInterface::ObstacleMessage::set_y(const float new_y)
01281 {
01282 data->y = new_y;
01283 }
01284
01285
01286
01287
01288
01289 float
01290 NavigatorInterface::ObstacleMessage::width() const
01291 {
01292 return data->width;
01293 }
01294
01295
01296
01297
01298
01299 size_t
01300 NavigatorInterface::ObstacleMessage::maxlenof_width() const
01301 {
01302 return 1;
01303 }
01304
01305
01306
01307
01308
01309 void
01310 NavigatorInterface::ObstacleMessage::set_width(const float new_width)
01311 {
01312 data->width = new_width;
01313 }
01314
01315
01316
01317
01318
01319
01320 Message *
01321 NavigatorInterface::ObstacleMessage::clone() const
01322 {
01323 return new NavigatorInterface::ObstacleMessage(this);
01324 }
01325
01326
01327
01328
01329
01330
01331
01332
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
01343 NavigatorInterface::ResetOdometryMessage::~ResetOdometryMessage()
01344 {
01345 free(data_ptr);
01346 }
01347
01348
01349
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
01361
01362
01363
01364
01365
01366 Message *
01367 NavigatorInterface::ResetOdometryMessage::clone() const
01368 {
01369 return new NavigatorInterface::ResetOdometryMessage(this);
01370 }
01371
01372
01373
01374
01375
01376
01377
01378
01379
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
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
01403 NavigatorInterface::SetMaxVelocityMessage::~SetMaxVelocityMessage()
01404 {
01405 free(data_ptr);
01406 }
01407
01408
01409
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
01421
01422
01423
01424
01425 float
01426 NavigatorInterface::SetMaxVelocityMessage::max_velocity() const
01427 {
01428 return data->max_velocity;
01429 }
01430
01431
01432
01433
01434
01435 size_t
01436 NavigatorInterface::SetMaxVelocityMessage::maxlenof_max_velocity() const
01437 {
01438 return 1;
01439 }
01440
01441
01442
01443
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
01452
01453
01454
01455
01456 Message *
01457 NavigatorInterface::SetMaxVelocityMessage::clone() const
01458 {
01459 return new NavigatorInterface::SetMaxVelocityMessage(this);
01460 }
01461
01462
01463
01464
01465
01466
01467
01468
01469
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
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
01493 NavigatorInterface::SetEscapingMessage::~SetEscapingMessage()
01494 {
01495 free(data_ptr);
01496 }
01497
01498
01499
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
01511
01512
01513
01514
01515
01516
01517
01518 bool
01519 NavigatorInterface::SetEscapingMessage::is_escaping_enabled() const
01520 {
01521 return data->escaping_enabled;
01522 }
01523
01524
01525
01526
01527
01528 size_t
01529 NavigatorInterface::SetEscapingMessage::maxlenof_escaping_enabled() const
01530 {
01531 return 1;
01532 }
01533
01534
01535
01536
01537
01538
01539
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
01548
01549
01550
01551
01552 Message *
01553 NavigatorInterface::SetEscapingMessage::clone() const
01554 {
01555 return new NavigatorInterface::SetEscapingMessage(this);
01556 }
01557
01558
01559
01560
01561
01562
01563
01564
01565
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
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
01589 NavigatorInterface::SetSecurityDistanceMessage::~SetSecurityDistanceMessage()
01590 {
01591 free(data_ptr);
01592 }
01593
01594
01595
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
01607
01608
01609
01610
01611
01612 float
01613 NavigatorInterface::SetSecurityDistanceMessage::security_distance() const
01614 {
01615 return data->security_distance;
01616 }
01617
01618
01619
01620
01621
01622 size_t
01623 NavigatorInterface::SetSecurityDistanceMessage::maxlenof_security_distance() const
01624 {
01625 return 1;
01626 }
01627
01628
01629
01630
01631
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
01640
01641
01642
01643
01644 Message *
01645 NavigatorInterface::SetSecurityDistanceMessage::clone() const
01646 {
01647 return new NavigatorInterface::SetSecurityDistanceMessage(this);
01648 }
01649
01650
01651
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
01700 EXPORT_INTERFACE(NavigatorInterface)
01701
01702
01703
01704 }