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/MotorInterface.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 const uint32_t MotorInterface::MOTOR_ENABLED = 0u;
00044
00045 const uint32_t MotorInterface::MOTOR_DISABLED = 1u;
00046
00047 const uint32_t MotorInterface::DRIVE_MODE_RPM = 1u;
00048
00049 const uint32_t MotorInterface::DRIVE_MODE_TRANS = 2u;
00050
00051 const uint32_t MotorInterface::DRIVE_MODE_ROT = 3u;
00052
00053 const uint32_t MotorInterface::DRIVE_MODE_TRANS_ROT = 4u;
00054
00055 const uint32_t MotorInterface::DRIVE_MODE_ORBIT = 5u;
00056
00057 const uint32_t MotorInterface::DRIVE_MODE_LINE_TRANS_ROT = 6u;
00058
00059
00060 MotorInterface::MotorInterface() : Interface()
00061 {
00062 data_size = sizeof(MotorInterface_data_t);
00063 data_ptr = malloc(data_size);
00064 data = (MotorInterface_data_t *)data_ptr;
00065 data_ts = (interface_data_ts_t *)data_ptr;
00066 memset(data_ptr, 0, data_size);
00067 add_fieldinfo(IFT_UINT32, "motor_state", 1, &data->motor_state);
00068 add_fieldinfo(IFT_UINT32, "drive_mode", 1, &data->drive_mode);
00069 add_fieldinfo(IFT_INT32, "right_rpm", 1, &data->right_rpm);
00070 add_fieldinfo(IFT_INT32, "rear_rpm", 1, &data->rear_rpm);
00071 add_fieldinfo(IFT_INT32, "left_rpm", 1, &data->left_rpm);
00072 add_fieldinfo(IFT_FLOAT, "odometry_path_length", 1, &data->odometry_path_length);
00073 add_fieldinfo(IFT_FLOAT, "odometry_position_x", 1, &data->odometry_position_x);
00074 add_fieldinfo(IFT_FLOAT, "odometry_position_y", 1, &data->odometry_position_y);
00075 add_fieldinfo(IFT_FLOAT, "odometry_orientation", 1, &data->odometry_orientation);
00076 add_fieldinfo(IFT_FLOAT, "vx", 1, &data->vx);
00077 add_fieldinfo(IFT_FLOAT, "vy", 1, &data->vy);
00078 add_fieldinfo(IFT_FLOAT, "omega", 1, &data->omega);
00079 add_fieldinfo(IFT_UINT32, "controller", 1, &data->controller);
00080 add_fieldinfo(IFT_STRING, "controller_thread_name", 64, data->controller_thread_name);
00081 add_messageinfo("SetMotorStateMessage");
00082 add_messageinfo("AcquireControlMessage");
00083 add_messageinfo("ResetOdometryMessage");
00084 add_messageinfo("DriveRPMMessage");
00085 add_messageinfo("GotoMessage");
00086 add_messageinfo("TransMessage");
00087 add_messageinfo("RotMessage");
00088 add_messageinfo("TransRotMessage");
00089 add_messageinfo("OrbitMessage");
00090 add_messageinfo("LinTransRotMessage");
00091 unsigned char tmp_hash[] = {0x4d, 0x9b, 0xb8, 0x1e, 0xb7, 0x9e, 0xc1, 0xea, 0xc6, 0xad, 0xf9, 0x6, 0xd7, 0x6d, 0xa4, 0xa5};
00092 set_hash(tmp_hash);
00093 }
00094
00095
00096 MotorInterface::~MotorInterface()
00097 {
00098 free(data_ptr);
00099 }
00100
00101
00102
00103
00104
00105
00106
00107 uint32_t
00108 MotorInterface::motor_state() const
00109 {
00110 return data->motor_state;
00111 }
00112
00113
00114
00115
00116
00117 size_t
00118 MotorInterface::maxlenof_motor_state() const
00119 {
00120 return 1;
00121 }
00122
00123
00124
00125
00126
00127
00128
00129 void
00130 MotorInterface::set_motor_state(const uint32_t new_motor_state)
00131 {
00132 data->motor_state = new_motor_state;
00133 data_changed = true;
00134 }
00135
00136
00137
00138
00139
00140
00141
00142 uint32_t
00143 MotorInterface::drive_mode() const
00144 {
00145 return data->drive_mode;
00146 }
00147
00148
00149
00150
00151
00152 size_t
00153 MotorInterface::maxlenof_drive_mode() const
00154 {
00155 return 1;
00156 }
00157
00158
00159
00160
00161
00162
00163
00164 void
00165 MotorInterface::set_drive_mode(const uint32_t new_drive_mode)
00166 {
00167 data->drive_mode = new_drive_mode;
00168 data_changed = true;
00169 }
00170
00171
00172
00173
00174
00175
00176
00177 int32_t
00178 MotorInterface::right_rpm() const
00179 {
00180 return data->right_rpm;
00181 }
00182
00183
00184
00185
00186
00187 size_t
00188 MotorInterface::maxlenof_right_rpm() const
00189 {
00190 return 1;
00191 }
00192
00193
00194
00195
00196
00197
00198
00199 void
00200 MotorInterface::set_right_rpm(const int32_t new_right_rpm)
00201 {
00202 data->right_rpm = new_right_rpm;
00203 data_changed = true;
00204 }
00205
00206
00207
00208
00209
00210
00211
00212 int32_t
00213 MotorInterface::rear_rpm() const
00214 {
00215 return data->rear_rpm;
00216 }
00217
00218
00219
00220
00221
00222 size_t
00223 MotorInterface::maxlenof_rear_rpm() const
00224 {
00225 return 1;
00226 }
00227
00228
00229
00230
00231
00232
00233
00234 void
00235 MotorInterface::set_rear_rpm(const int32_t new_rear_rpm)
00236 {
00237 data->rear_rpm = new_rear_rpm;
00238 data_changed = true;
00239 }
00240
00241
00242
00243
00244
00245
00246
00247 int32_t
00248 MotorInterface::left_rpm() const
00249 {
00250 return data->left_rpm;
00251 }
00252
00253
00254
00255
00256
00257 size_t
00258 MotorInterface::maxlenof_left_rpm() const
00259 {
00260 return 1;
00261 }
00262
00263
00264
00265
00266
00267
00268
00269 void
00270 MotorInterface::set_left_rpm(const int32_t new_left_rpm)
00271 {
00272 data->left_rpm = new_left_rpm;
00273 data_changed = true;
00274 }
00275
00276
00277
00278
00279
00280
00281
00282 float
00283 MotorInterface::odometry_path_length() const
00284 {
00285 return data->odometry_path_length;
00286 }
00287
00288
00289
00290
00291
00292 size_t
00293 MotorInterface::maxlenof_odometry_path_length() const
00294 {
00295 return 1;
00296 }
00297
00298
00299
00300
00301
00302
00303
00304 void
00305 MotorInterface::set_odometry_path_length(const float new_odometry_path_length)
00306 {
00307 data->odometry_path_length = new_odometry_path_length;
00308 data_changed = true;
00309 }
00310
00311
00312
00313
00314
00315
00316
00317 float
00318 MotorInterface::odometry_position_x() const
00319 {
00320 return data->odometry_position_x;
00321 }
00322
00323
00324
00325
00326
00327 size_t
00328 MotorInterface::maxlenof_odometry_position_x() const
00329 {
00330 return 1;
00331 }
00332
00333
00334
00335
00336
00337
00338
00339 void
00340 MotorInterface::set_odometry_position_x(const float new_odometry_position_x)
00341 {
00342 data->odometry_position_x = new_odometry_position_x;
00343 data_changed = true;
00344 }
00345
00346
00347
00348
00349
00350
00351
00352 float
00353 MotorInterface::odometry_position_y() const
00354 {
00355 return data->odometry_position_y;
00356 }
00357
00358
00359
00360
00361
00362 size_t
00363 MotorInterface::maxlenof_odometry_position_y() const
00364 {
00365 return 1;
00366 }
00367
00368
00369
00370
00371
00372
00373
00374 void
00375 MotorInterface::set_odometry_position_y(const float new_odometry_position_y)
00376 {
00377 data->odometry_position_y = new_odometry_position_y;
00378 data_changed = true;
00379 }
00380
00381
00382
00383
00384
00385
00386
00387 float
00388 MotorInterface::odometry_orientation() const
00389 {
00390 return data->odometry_orientation;
00391 }
00392
00393
00394
00395
00396
00397 size_t
00398 MotorInterface::maxlenof_odometry_orientation() const
00399 {
00400 return 1;
00401 }
00402
00403
00404
00405
00406
00407
00408
00409 void
00410 MotorInterface::set_odometry_orientation(const float new_odometry_orientation)
00411 {
00412 data->odometry_orientation = new_odometry_orientation;
00413 data_changed = true;
00414 }
00415
00416
00417
00418
00419
00420
00421
00422 float
00423 MotorInterface::vx() const
00424 {
00425 return data->vx;
00426 }
00427
00428
00429
00430
00431
00432 size_t
00433 MotorInterface::maxlenof_vx() const
00434 {
00435 return 1;
00436 }
00437
00438
00439
00440
00441
00442
00443
00444 void
00445 MotorInterface::set_vx(const float new_vx)
00446 {
00447 data->vx = new_vx;
00448 data_changed = true;
00449 }
00450
00451
00452
00453
00454
00455
00456
00457 float
00458 MotorInterface::vy() const
00459 {
00460 return data->vy;
00461 }
00462
00463
00464
00465
00466
00467 size_t
00468 MotorInterface::maxlenof_vy() const
00469 {
00470 return 1;
00471 }
00472
00473
00474
00475
00476
00477
00478
00479 void
00480 MotorInterface::set_vy(const float new_vy)
00481 {
00482 data->vy = new_vy;
00483 data_changed = true;
00484 }
00485
00486
00487
00488
00489
00490
00491
00492 float
00493 MotorInterface::omega() const
00494 {
00495 return data->omega;
00496 }
00497
00498
00499
00500
00501
00502 size_t
00503 MotorInterface::maxlenof_omega() const
00504 {
00505 return 1;
00506 }
00507
00508
00509
00510
00511
00512
00513
00514 void
00515 MotorInterface::set_omega(const float new_omega)
00516 {
00517 data->omega = new_omega;
00518 data_changed = true;
00519 }
00520
00521
00522
00523
00524
00525
00526
00527
00528 uint32_t
00529 MotorInterface::controller() const
00530 {
00531 return data->controller;
00532 }
00533
00534
00535
00536
00537
00538 size_t
00539 MotorInterface::maxlenof_controller() const
00540 {
00541 return 1;
00542 }
00543
00544
00545
00546
00547
00548
00549
00550
00551 void
00552 MotorInterface::set_controller(const uint32_t new_controller)
00553 {
00554 data->controller = new_controller;
00555 data_changed = true;
00556 }
00557
00558
00559
00560
00561
00562
00563
00564
00565
00566 char *
00567 MotorInterface::controller_thread_name() const
00568 {
00569 return data->controller_thread_name;
00570 }
00571
00572
00573
00574
00575
00576 size_t
00577 MotorInterface::maxlenof_controller_thread_name() const
00578 {
00579 return 64;
00580 }
00581
00582
00583
00584
00585
00586
00587
00588
00589
00590 void
00591 MotorInterface::set_controller_thread_name(const char * new_controller_thread_name)
00592 {
00593 strncpy(data->controller_thread_name, new_controller_thread_name, sizeof(data->controller_thread_name));
00594 data_changed = true;
00595 }
00596
00597
00598 Message *
00599 MotorInterface::create_message(const char *type) const
00600 {
00601 if ( strncmp("SetMotorStateMessage", type, __INTERFACE_MESSAGE_TYPE_SIZE) == 0 ) {
00602 return new SetMotorStateMessage();
00603 } else if ( strncmp("AcquireControlMessage", type, __INTERFACE_MESSAGE_TYPE_SIZE) == 0 ) {
00604 return new AcquireControlMessage();
00605 } else if ( strncmp("ResetOdometryMessage", type, __INTERFACE_MESSAGE_TYPE_SIZE) == 0 ) {
00606 return new ResetOdometryMessage();
00607 } else if ( strncmp("DriveRPMMessage", type, __INTERFACE_MESSAGE_TYPE_SIZE) == 0 ) {
00608 return new DriveRPMMessage();
00609 } else if ( strncmp("GotoMessage", type, __INTERFACE_MESSAGE_TYPE_SIZE) == 0 ) {
00610 return new GotoMessage();
00611 } else if ( strncmp("TransMessage", type, __INTERFACE_MESSAGE_TYPE_SIZE) == 0 ) {
00612 return new TransMessage();
00613 } else if ( strncmp("RotMessage", type, __INTERFACE_MESSAGE_TYPE_SIZE) == 0 ) {
00614 return new RotMessage();
00615 } else if ( strncmp("TransRotMessage", type, __INTERFACE_MESSAGE_TYPE_SIZE) == 0 ) {
00616 return new TransRotMessage();
00617 } else if ( strncmp("OrbitMessage", type, __INTERFACE_MESSAGE_TYPE_SIZE) == 0 ) {
00618 return new OrbitMessage();
00619 } else if ( strncmp("LinTransRotMessage", type, __INTERFACE_MESSAGE_TYPE_SIZE) == 0 ) {
00620 return new LinTransRotMessage();
00621 } else {
00622 throw UnknownTypeException("The given type '%s' does not match any known "
00623 "message type for this interface type.", type);
00624 }
00625 }
00626
00627
00628
00629
00630
00631 void
00632 MotorInterface::copy_values(const Interface *other)
00633 {
00634 const MotorInterface *oi = dynamic_cast<const MotorInterface *>(other);
00635 if (oi == NULL) {
00636 throw TypeMismatchException("Can only copy values from interface of same type (%s vs. %s)",
00637 type(), other->type());
00638 }
00639 memcpy(data, oi->data, sizeof(MotorInterface_data_t));
00640 }
00641
00642 const char *
00643 MotorInterface::enum_tostring(const char *enumtype, int val) const
00644 {
00645 throw UnknownTypeException("Unknown enum type %s", enumtype);
00646 }
00647
00648
00649
00650
00651
00652
00653
00654
00655
00656
00657
00658
00659 MotorInterface::SetMotorStateMessage::SetMotorStateMessage(const uint32_t ini_motor_state) : Message("SetMotorStateMessage")
00660 {
00661 data_size = sizeof(SetMotorStateMessage_data_t);
00662 data_ptr = malloc(data_size);
00663 memset(data_ptr, 0, data_size);
00664 data = (SetMotorStateMessage_data_t *)data_ptr;
00665 data_ts = (message_data_ts_t *)data_ptr;
00666 data->motor_state = ini_motor_state;
00667 add_fieldinfo(IFT_UINT32, "motor_state", 1, &data->motor_state);
00668 }
00669
00670 MotorInterface::SetMotorStateMessage::SetMotorStateMessage() : Message("SetMotorStateMessage")
00671 {
00672 data_size = sizeof(SetMotorStateMessage_data_t);
00673 data_ptr = malloc(data_size);
00674 memset(data_ptr, 0, data_size);
00675 data = (SetMotorStateMessage_data_t *)data_ptr;
00676 data_ts = (message_data_ts_t *)data_ptr;
00677 add_fieldinfo(IFT_UINT32, "motor_state", 1, &data->motor_state);
00678 }
00679
00680
00681 MotorInterface::SetMotorStateMessage::~SetMotorStateMessage()
00682 {
00683 free(data_ptr);
00684 }
00685
00686
00687
00688
00689 MotorInterface::SetMotorStateMessage::SetMotorStateMessage(const SetMotorStateMessage *m) : Message("SetMotorStateMessage")
00690 {
00691 data_size = m->data_size;
00692 data_ptr = malloc(data_size);
00693 memcpy(data_ptr, m->data_ptr, data_size);
00694 data = (SetMotorStateMessage_data_t *)data_ptr;
00695 data_ts = (message_data_ts_t *)data_ptr;
00696 }
00697
00698
00699
00700
00701
00702
00703
00704
00705 uint32_t
00706 MotorInterface::SetMotorStateMessage::motor_state() const
00707 {
00708 return data->motor_state;
00709 }
00710
00711
00712
00713
00714
00715 size_t
00716 MotorInterface::SetMotorStateMessage::maxlenof_motor_state() const
00717 {
00718 return 1;
00719 }
00720
00721
00722
00723
00724
00725
00726
00727 void
00728 MotorInterface::SetMotorStateMessage::set_motor_state(const uint32_t new_motor_state)
00729 {
00730 data->motor_state = new_motor_state;
00731 }
00732
00733
00734
00735
00736
00737
00738 Message *
00739 MotorInterface::SetMotorStateMessage::clone() const
00740 {
00741 return new MotorInterface::SetMotorStateMessage(this);
00742 }
00743
00744
00745
00746
00747
00748
00749
00750
00751
00752
00753
00754 MotorInterface::AcquireControlMessage::AcquireControlMessage(const uint32_t ini_controller, const char * ini_controller_thread_name) : Message("AcquireControlMessage")
00755 {
00756 data_size = sizeof(AcquireControlMessage_data_t);
00757 data_ptr = malloc(data_size);
00758 memset(data_ptr, 0, data_size);
00759 data = (AcquireControlMessage_data_t *)data_ptr;
00760 data_ts = (message_data_ts_t *)data_ptr;
00761 data->controller = ini_controller;
00762 strncpy(data->controller_thread_name, ini_controller_thread_name, 64);
00763 add_fieldinfo(IFT_UINT32, "controller", 1, &data->controller);
00764 add_fieldinfo(IFT_STRING, "controller_thread_name", 64, data->controller_thread_name);
00765 }
00766
00767 MotorInterface::AcquireControlMessage::AcquireControlMessage() : Message("AcquireControlMessage")
00768 {
00769 data_size = sizeof(AcquireControlMessage_data_t);
00770 data_ptr = malloc(data_size);
00771 memset(data_ptr, 0, data_size);
00772 data = (AcquireControlMessage_data_t *)data_ptr;
00773 data_ts = (message_data_ts_t *)data_ptr;
00774 add_fieldinfo(IFT_UINT32, "controller", 1, &data->controller);
00775 add_fieldinfo(IFT_STRING, "controller_thread_name", 64, data->controller_thread_name);
00776 }
00777
00778
00779 MotorInterface::AcquireControlMessage::~AcquireControlMessage()
00780 {
00781 free(data_ptr);
00782 }
00783
00784
00785
00786
00787 MotorInterface::AcquireControlMessage::AcquireControlMessage(const AcquireControlMessage *m) : Message("AcquireControlMessage")
00788 {
00789 data_size = m->data_size;
00790 data_ptr = malloc(data_size);
00791 memcpy(data_ptr, m->data_ptr, data_size);
00792 data = (AcquireControlMessage_data_t *)data_ptr;
00793 data_ts = (message_data_ts_t *)data_ptr;
00794 }
00795
00796
00797
00798
00799
00800
00801
00802
00803
00804 uint32_t
00805 MotorInterface::AcquireControlMessage::controller() const
00806 {
00807 return data->controller;
00808 }
00809
00810
00811
00812
00813
00814 size_t
00815 MotorInterface::AcquireControlMessage::maxlenof_controller() const
00816 {
00817 return 1;
00818 }
00819
00820
00821
00822
00823
00824
00825
00826
00827 void
00828 MotorInterface::AcquireControlMessage::set_controller(const uint32_t new_controller)
00829 {
00830 data->controller = new_controller;
00831 }
00832
00833
00834
00835
00836
00837
00838
00839
00840
00841 char *
00842 MotorInterface::AcquireControlMessage::controller_thread_name() const
00843 {
00844 return data->controller_thread_name;
00845 }
00846
00847
00848
00849
00850
00851 size_t
00852 MotorInterface::AcquireControlMessage::maxlenof_controller_thread_name() const
00853 {
00854 return 64;
00855 }
00856
00857
00858
00859
00860
00861
00862
00863
00864
00865 void
00866 MotorInterface::AcquireControlMessage::set_controller_thread_name(const char * new_controller_thread_name)
00867 {
00868 strncpy(data->controller_thread_name, new_controller_thread_name, sizeof(data->controller_thread_name));
00869 }
00870
00871
00872
00873
00874
00875
00876 Message *
00877 MotorInterface::AcquireControlMessage::clone() const
00878 {
00879 return new MotorInterface::AcquireControlMessage(this);
00880 }
00881
00882
00883
00884
00885
00886
00887
00888
00889 MotorInterface::ResetOdometryMessage::ResetOdometryMessage() : Message("ResetOdometryMessage")
00890 {
00891 data_size = sizeof(ResetOdometryMessage_data_t);
00892 data_ptr = malloc(data_size);
00893 memset(data_ptr, 0, data_size);
00894 data = (ResetOdometryMessage_data_t *)data_ptr;
00895 data_ts = (message_data_ts_t *)data_ptr;
00896 }
00897
00898
00899 MotorInterface::ResetOdometryMessage::~ResetOdometryMessage()
00900 {
00901 free(data_ptr);
00902 }
00903
00904
00905
00906
00907 MotorInterface::ResetOdometryMessage::ResetOdometryMessage(const ResetOdometryMessage *m) : Message("ResetOdometryMessage")
00908 {
00909 data_size = m->data_size;
00910 data_ptr = malloc(data_size);
00911 memcpy(data_ptr, m->data_ptr, data_size);
00912 data = (ResetOdometryMessage_data_t *)data_ptr;
00913 data_ts = (message_data_ts_t *)data_ptr;
00914 }
00915
00916
00917
00918
00919
00920
00921
00922 Message *
00923 MotorInterface::ResetOdometryMessage::clone() const
00924 {
00925 return new MotorInterface::ResetOdometryMessage(this);
00926 }
00927
00928
00929
00930
00931
00932
00933
00934
00935
00936
00937
00938
00939 MotorInterface::DriveRPMMessage::DriveRPMMessage(const float ini_front_right, const float ini_front_left, const float ini_rear) : Message("DriveRPMMessage")
00940 {
00941 data_size = sizeof(DriveRPMMessage_data_t);
00942 data_ptr = malloc(data_size);
00943 memset(data_ptr, 0, data_size);
00944 data = (DriveRPMMessage_data_t *)data_ptr;
00945 data_ts = (message_data_ts_t *)data_ptr;
00946 data->front_right = ini_front_right;
00947 data->front_left = ini_front_left;
00948 data->rear = ini_rear;
00949 add_fieldinfo(IFT_FLOAT, "front_right", 1, &data->front_right);
00950 add_fieldinfo(IFT_FLOAT, "front_left", 1, &data->front_left);
00951 add_fieldinfo(IFT_FLOAT, "rear", 1, &data->rear);
00952 }
00953
00954 MotorInterface::DriveRPMMessage::DriveRPMMessage() : Message("DriveRPMMessage")
00955 {
00956 data_size = sizeof(DriveRPMMessage_data_t);
00957 data_ptr = malloc(data_size);
00958 memset(data_ptr, 0, data_size);
00959 data = (DriveRPMMessage_data_t *)data_ptr;
00960 data_ts = (message_data_ts_t *)data_ptr;
00961 add_fieldinfo(IFT_FLOAT, "front_right", 1, &data->front_right);
00962 add_fieldinfo(IFT_FLOAT, "front_left", 1, &data->front_left);
00963 add_fieldinfo(IFT_FLOAT, "rear", 1, &data->rear);
00964 }
00965
00966
00967 MotorInterface::DriveRPMMessage::~DriveRPMMessage()
00968 {
00969 free(data_ptr);
00970 }
00971
00972
00973
00974
00975 MotorInterface::DriveRPMMessage::DriveRPMMessage(const DriveRPMMessage *m) : Message("DriveRPMMessage")
00976 {
00977 data_size = m->data_size;
00978 data_ptr = malloc(data_size);
00979 memcpy(data_ptr, m->data_ptr, data_size);
00980 data = (DriveRPMMessage_data_t *)data_ptr;
00981 data_ts = (message_data_ts_t *)data_ptr;
00982 }
00983
00984
00985
00986
00987
00988
00989 float
00990 MotorInterface::DriveRPMMessage::front_right() const
00991 {
00992 return data->front_right;
00993 }
00994
00995
00996
00997
00998
00999 size_t
01000 MotorInterface::DriveRPMMessage::maxlenof_front_right() const
01001 {
01002 return 1;
01003 }
01004
01005
01006
01007
01008
01009 void
01010 MotorInterface::DriveRPMMessage::set_front_right(const float new_front_right)
01011 {
01012 data->front_right = new_front_right;
01013 }
01014
01015
01016
01017
01018
01019 float
01020 MotorInterface::DriveRPMMessage::front_left() const
01021 {
01022 return data->front_left;
01023 }
01024
01025
01026
01027
01028
01029 size_t
01030 MotorInterface::DriveRPMMessage::maxlenof_front_left() const
01031 {
01032 return 1;
01033 }
01034
01035
01036
01037
01038
01039 void
01040 MotorInterface::DriveRPMMessage::set_front_left(const float new_front_left)
01041 {
01042 data->front_left = new_front_left;
01043 }
01044
01045
01046
01047
01048
01049 float
01050 MotorInterface::DriveRPMMessage::rear() const
01051 {
01052 return data->rear;
01053 }
01054
01055
01056
01057
01058
01059 size_t
01060 MotorInterface::DriveRPMMessage::maxlenof_rear() const
01061 {
01062 return 1;
01063 }
01064
01065
01066
01067
01068
01069 void
01070 MotorInterface::DriveRPMMessage::set_rear(const float new_rear)
01071 {
01072 data->rear = new_rear;
01073 }
01074
01075
01076
01077
01078
01079
01080 Message *
01081 MotorInterface::DriveRPMMessage::clone() const
01082 {
01083 return new MotorInterface::DriveRPMMessage(this);
01084 }
01085
01086
01087
01088
01089
01090
01091
01092
01093
01094
01095
01096
01097
01098 MotorInterface::GotoMessage::GotoMessage(const float ini_x, const float ini_y, const float ini_phi, const float ini_time_sec) : Message("GotoMessage")
01099 {
01100 data_size = sizeof(GotoMessage_data_t);
01101 data_ptr = malloc(data_size);
01102 memset(data_ptr, 0, data_size);
01103 data = (GotoMessage_data_t *)data_ptr;
01104 data_ts = (message_data_ts_t *)data_ptr;
01105 data->x = ini_x;
01106 data->y = ini_y;
01107 data->phi = ini_phi;
01108 data->time_sec = ini_time_sec;
01109 add_fieldinfo(IFT_FLOAT, "x", 1, &data->x);
01110 add_fieldinfo(IFT_FLOAT, "y", 1, &data->y);
01111 add_fieldinfo(IFT_FLOAT, "phi", 1, &data->phi);
01112 add_fieldinfo(IFT_FLOAT, "time_sec", 1, &data->time_sec);
01113 }
01114
01115 MotorInterface::GotoMessage::GotoMessage() : Message("GotoMessage")
01116 {
01117 data_size = sizeof(GotoMessage_data_t);
01118 data_ptr = malloc(data_size);
01119 memset(data_ptr, 0, data_size);
01120 data = (GotoMessage_data_t *)data_ptr;
01121 data_ts = (message_data_ts_t *)data_ptr;
01122 add_fieldinfo(IFT_FLOAT, "x", 1, &data->x);
01123 add_fieldinfo(IFT_FLOAT, "y", 1, &data->y);
01124 add_fieldinfo(IFT_FLOAT, "phi", 1, &data->phi);
01125 add_fieldinfo(IFT_FLOAT, "time_sec", 1, &data->time_sec);
01126 }
01127
01128
01129 MotorInterface::GotoMessage::~GotoMessage()
01130 {
01131 free(data_ptr);
01132 }
01133
01134
01135
01136
01137 MotorInterface::GotoMessage::GotoMessage(const GotoMessage *m) : Message("GotoMessage")
01138 {
01139 data_size = m->data_size;
01140 data_ptr = malloc(data_size);
01141 memcpy(data_ptr, m->data_ptr, data_size);
01142 data = (GotoMessage_data_t *)data_ptr;
01143 data_ts = (message_data_ts_t *)data_ptr;
01144 }
01145
01146
01147
01148
01149
01150
01151 float
01152 MotorInterface::GotoMessage::x() const
01153 {
01154 return data->x;
01155 }
01156
01157
01158
01159
01160
01161 size_t
01162 MotorInterface::GotoMessage::maxlenof_x() const
01163 {
01164 return 1;
01165 }
01166
01167
01168
01169
01170
01171 void
01172 MotorInterface::GotoMessage::set_x(const float new_x)
01173 {
01174 data->x = new_x;
01175 }
01176
01177
01178
01179
01180
01181 float
01182 MotorInterface::GotoMessage::y() const
01183 {
01184 return data->y;
01185 }
01186
01187
01188
01189
01190
01191 size_t
01192 MotorInterface::GotoMessage::maxlenof_y() const
01193 {
01194 return 1;
01195 }
01196
01197
01198
01199
01200
01201 void
01202 MotorInterface::GotoMessage::set_y(const float new_y)
01203 {
01204 data->y = new_y;
01205 }
01206
01207
01208
01209
01210
01211 float
01212 MotorInterface::GotoMessage::phi() const
01213 {
01214 return data->phi;
01215 }
01216
01217
01218
01219
01220
01221 size_t
01222 MotorInterface::GotoMessage::maxlenof_phi() const
01223 {
01224 return 1;
01225 }
01226
01227
01228
01229
01230
01231 void
01232 MotorInterface::GotoMessage::set_phi(const float new_phi)
01233 {
01234 data->phi = new_phi;
01235 }
01236
01237
01238
01239
01240
01241 float
01242 MotorInterface::GotoMessage::time_sec() const
01243 {
01244 return data->time_sec;
01245 }
01246
01247
01248
01249
01250
01251 size_t
01252 MotorInterface::GotoMessage::maxlenof_time_sec() const
01253 {
01254 return 1;
01255 }
01256
01257
01258
01259
01260
01261 void
01262 MotorInterface::GotoMessage::set_time_sec(const float new_time_sec)
01263 {
01264 data->time_sec = new_time_sec;
01265 }
01266
01267
01268
01269
01270
01271
01272 Message *
01273 MotorInterface::GotoMessage::clone() const
01274 {
01275 return new MotorInterface::GotoMessage(this);
01276 }
01277
01278
01279
01280
01281
01282
01283
01284
01285
01286
01287
01288 MotorInterface::TransMessage::TransMessage(const float ini_vx, const float ini_vy) : Message("TransMessage")
01289 {
01290 data_size = sizeof(TransMessage_data_t);
01291 data_ptr = malloc(data_size);
01292 memset(data_ptr, 0, data_size);
01293 data = (TransMessage_data_t *)data_ptr;
01294 data_ts = (message_data_ts_t *)data_ptr;
01295 data->vx = ini_vx;
01296 data->vy = ini_vy;
01297 add_fieldinfo(IFT_FLOAT, "vx", 1, &data->vx);
01298 add_fieldinfo(IFT_FLOAT, "vy", 1, &data->vy);
01299 }
01300
01301 MotorInterface::TransMessage::TransMessage() : Message("TransMessage")
01302 {
01303 data_size = sizeof(TransMessage_data_t);
01304 data_ptr = malloc(data_size);
01305 memset(data_ptr, 0, data_size);
01306 data = (TransMessage_data_t *)data_ptr;
01307 data_ts = (message_data_ts_t *)data_ptr;
01308 add_fieldinfo(IFT_FLOAT, "vx", 1, &data->vx);
01309 add_fieldinfo(IFT_FLOAT, "vy", 1, &data->vy);
01310 }
01311
01312
01313 MotorInterface::TransMessage::~TransMessage()
01314 {
01315 free(data_ptr);
01316 }
01317
01318
01319
01320
01321 MotorInterface::TransMessage::TransMessage(const TransMessage *m) : Message("TransMessage")
01322 {
01323 data_size = m->data_size;
01324 data_ptr = malloc(data_size);
01325 memcpy(data_ptr, m->data_ptr, data_size);
01326 data = (TransMessage_data_t *)data_ptr;
01327 data_ts = (message_data_ts_t *)data_ptr;
01328 }
01329
01330
01331
01332
01333
01334
01335 float
01336 MotorInterface::TransMessage::vx() const
01337 {
01338 return data->vx;
01339 }
01340
01341
01342
01343
01344
01345 size_t
01346 MotorInterface::TransMessage::maxlenof_vx() const
01347 {
01348 return 1;
01349 }
01350
01351
01352
01353
01354
01355 void
01356 MotorInterface::TransMessage::set_vx(const float new_vx)
01357 {
01358 data->vx = new_vx;
01359 }
01360
01361
01362
01363
01364
01365 float
01366 MotorInterface::TransMessage::vy() const
01367 {
01368 return data->vy;
01369 }
01370
01371
01372
01373
01374
01375 size_t
01376 MotorInterface::TransMessage::maxlenof_vy() const
01377 {
01378 return 1;
01379 }
01380
01381
01382
01383
01384
01385 void
01386 MotorInterface::TransMessage::set_vy(const float new_vy)
01387 {
01388 data->vy = new_vy;
01389 }
01390
01391
01392
01393
01394
01395
01396 Message *
01397 MotorInterface::TransMessage::clone() const
01398 {
01399 return new MotorInterface::TransMessage(this);
01400 }
01401
01402
01403
01404
01405
01406
01407
01408
01409
01410
01411 MotorInterface::RotMessage::RotMessage(const float ini_omega) : Message("RotMessage")
01412 {
01413 data_size = sizeof(RotMessage_data_t);
01414 data_ptr = malloc(data_size);
01415 memset(data_ptr, 0, data_size);
01416 data = (RotMessage_data_t *)data_ptr;
01417 data_ts = (message_data_ts_t *)data_ptr;
01418 data->omega = ini_omega;
01419 add_fieldinfo(IFT_FLOAT, "omega", 1, &data->omega);
01420 }
01421
01422 MotorInterface::RotMessage::RotMessage() : Message("RotMessage")
01423 {
01424 data_size = sizeof(RotMessage_data_t);
01425 data_ptr = malloc(data_size);
01426 memset(data_ptr, 0, data_size);
01427 data = (RotMessage_data_t *)data_ptr;
01428 data_ts = (message_data_ts_t *)data_ptr;
01429 add_fieldinfo(IFT_FLOAT, "omega", 1, &data->omega);
01430 }
01431
01432
01433 MotorInterface::RotMessage::~RotMessage()
01434 {
01435 free(data_ptr);
01436 }
01437
01438
01439
01440
01441 MotorInterface::RotMessage::RotMessage(const RotMessage *m) : Message("RotMessage")
01442 {
01443 data_size = m->data_size;
01444 data_ptr = malloc(data_size);
01445 memcpy(data_ptr, m->data_ptr, data_size);
01446 data = (RotMessage_data_t *)data_ptr;
01447 data_ts = (message_data_ts_t *)data_ptr;
01448 }
01449
01450
01451
01452
01453
01454
01455 float
01456 MotorInterface::RotMessage::omega() const
01457 {
01458 return data->omega;
01459 }
01460
01461
01462
01463
01464
01465 size_t
01466 MotorInterface::RotMessage::maxlenof_omega() const
01467 {
01468 return 1;
01469 }
01470
01471
01472
01473
01474
01475 void
01476 MotorInterface::RotMessage::set_omega(const float new_omega)
01477 {
01478 data->omega = new_omega;
01479 }
01480
01481
01482
01483
01484
01485
01486 Message *
01487 MotorInterface::RotMessage::clone() const
01488 {
01489 return new MotorInterface::RotMessage(this);
01490 }
01491
01492
01493
01494
01495
01496
01497
01498
01499
01500
01501
01502
01503 MotorInterface::TransRotMessage::TransRotMessage(const float ini_vx, const float ini_vy, const float ini_omega) : Message("TransRotMessage")
01504 {
01505 data_size = sizeof(TransRotMessage_data_t);
01506 data_ptr = malloc(data_size);
01507 memset(data_ptr, 0, data_size);
01508 data = (TransRotMessage_data_t *)data_ptr;
01509 data_ts = (message_data_ts_t *)data_ptr;
01510 data->vx = ini_vx;
01511 data->vy = ini_vy;
01512 data->omega = ini_omega;
01513 add_fieldinfo(IFT_FLOAT, "vx", 1, &data->vx);
01514 add_fieldinfo(IFT_FLOAT, "vy", 1, &data->vy);
01515 add_fieldinfo(IFT_FLOAT, "omega", 1, &data->omega);
01516 }
01517
01518 MotorInterface::TransRotMessage::TransRotMessage() : Message("TransRotMessage")
01519 {
01520 data_size = sizeof(TransRotMessage_data_t);
01521 data_ptr = malloc(data_size);
01522 memset(data_ptr, 0, data_size);
01523 data = (TransRotMessage_data_t *)data_ptr;
01524 data_ts = (message_data_ts_t *)data_ptr;
01525 add_fieldinfo(IFT_FLOAT, "vx", 1, &data->vx);
01526 add_fieldinfo(IFT_FLOAT, "vy", 1, &data->vy);
01527 add_fieldinfo(IFT_FLOAT, "omega", 1, &data->omega);
01528 }
01529
01530
01531 MotorInterface::TransRotMessage::~TransRotMessage()
01532 {
01533 free(data_ptr);
01534 }
01535
01536
01537
01538
01539 MotorInterface::TransRotMessage::TransRotMessage(const TransRotMessage *m) : Message("TransRotMessage")
01540 {
01541 data_size = m->data_size;
01542 data_ptr = malloc(data_size);
01543 memcpy(data_ptr, m->data_ptr, data_size);
01544 data = (TransRotMessage_data_t *)data_ptr;
01545 data_ts = (message_data_ts_t *)data_ptr;
01546 }
01547
01548
01549
01550
01551
01552
01553 float
01554 MotorInterface::TransRotMessage::vx() const
01555 {
01556 return data->vx;
01557 }
01558
01559
01560
01561
01562
01563 size_t
01564 MotorInterface::TransRotMessage::maxlenof_vx() const
01565 {
01566 return 1;
01567 }
01568
01569
01570
01571
01572
01573 void
01574 MotorInterface::TransRotMessage::set_vx(const float new_vx)
01575 {
01576 data->vx = new_vx;
01577 }
01578
01579
01580
01581
01582
01583 float
01584 MotorInterface::TransRotMessage::vy() const
01585 {
01586 return data->vy;
01587 }
01588
01589
01590
01591
01592
01593 size_t
01594 MotorInterface::TransRotMessage::maxlenof_vy() const
01595 {
01596 return 1;
01597 }
01598
01599
01600
01601
01602
01603 void
01604 MotorInterface::TransRotMessage::set_vy(const float new_vy)
01605 {
01606 data->vy = new_vy;
01607 }
01608
01609
01610
01611
01612
01613 float
01614 MotorInterface::TransRotMessage::omega() const
01615 {
01616 return data->omega;
01617 }
01618
01619
01620
01621
01622
01623 size_t
01624 MotorInterface::TransRotMessage::maxlenof_omega() const
01625 {
01626 return 1;
01627 }
01628
01629
01630
01631
01632
01633 void
01634 MotorInterface::TransRotMessage::set_omega(const float new_omega)
01635 {
01636 data->omega = new_omega;
01637 }
01638
01639
01640
01641
01642
01643
01644 Message *
01645 MotorInterface::TransRotMessage::clone() const
01646 {
01647 return new MotorInterface::TransRotMessage(this);
01648 }
01649
01650
01651
01652
01653
01654
01655
01656
01657
01658
01659
01660
01661 MotorInterface::OrbitMessage::OrbitMessage(const float ini_px, const float ini_py, const float ini_omega) : Message("OrbitMessage")
01662 {
01663 data_size = sizeof(OrbitMessage_data_t);
01664 data_ptr = malloc(data_size);
01665 memset(data_ptr, 0, data_size);
01666 data = (OrbitMessage_data_t *)data_ptr;
01667 data_ts = (message_data_ts_t *)data_ptr;
01668 data->px = ini_px;
01669 data->py = ini_py;
01670 data->omega = ini_omega;
01671 add_fieldinfo(IFT_FLOAT, "px", 1, &data->px);
01672 add_fieldinfo(IFT_FLOAT, "py", 1, &data->py);
01673 add_fieldinfo(IFT_FLOAT, "omega", 1, &data->omega);
01674 }
01675
01676 MotorInterface::OrbitMessage::OrbitMessage() : Message("OrbitMessage")
01677 {
01678 data_size = sizeof(OrbitMessage_data_t);
01679 data_ptr = malloc(data_size);
01680 memset(data_ptr, 0, data_size);
01681 data = (OrbitMessage_data_t *)data_ptr;
01682 data_ts = (message_data_ts_t *)data_ptr;
01683 add_fieldinfo(IFT_FLOAT, "px", 1, &data->px);
01684 add_fieldinfo(IFT_FLOAT, "py", 1, &data->py);
01685 add_fieldinfo(IFT_FLOAT, "omega", 1, &data->omega);
01686 }
01687
01688
01689 MotorInterface::OrbitMessage::~OrbitMessage()
01690 {
01691 free(data_ptr);
01692 }
01693
01694
01695
01696
01697 MotorInterface::OrbitMessage::OrbitMessage(const OrbitMessage *m) : Message("OrbitMessage")
01698 {
01699 data_size = m->data_size;
01700 data_ptr = malloc(data_size);
01701 memcpy(data_ptr, m->data_ptr, data_size);
01702 data = (OrbitMessage_data_t *)data_ptr;
01703 data_ts = (message_data_ts_t *)data_ptr;
01704 }
01705
01706
01707
01708
01709
01710
01711 float
01712 MotorInterface::OrbitMessage::px() const
01713 {
01714 return data->px;
01715 }
01716
01717
01718
01719
01720
01721 size_t
01722 MotorInterface::OrbitMessage::maxlenof_px() const
01723 {
01724 return 1;
01725 }
01726
01727
01728
01729
01730
01731 void
01732 MotorInterface::OrbitMessage::set_px(const float new_px)
01733 {
01734 data->px = new_px;
01735 }
01736
01737
01738
01739
01740
01741 float
01742 MotorInterface::OrbitMessage::py() const
01743 {
01744 return data->py;
01745 }
01746
01747
01748
01749
01750
01751 size_t
01752 MotorInterface::OrbitMessage::maxlenof_py() const
01753 {
01754 return 1;
01755 }
01756
01757
01758
01759
01760
01761 void
01762 MotorInterface::OrbitMessage::set_py(const float new_py)
01763 {
01764 data->py = new_py;
01765 }
01766
01767
01768
01769
01770
01771 float
01772 MotorInterface::OrbitMessage::omega() const
01773 {
01774 return data->omega;
01775 }
01776
01777
01778
01779
01780
01781 size_t
01782 MotorInterface::OrbitMessage::maxlenof_omega() const
01783 {
01784 return 1;
01785 }
01786
01787
01788
01789
01790
01791 void
01792 MotorInterface::OrbitMessage::set_omega(const float new_omega)
01793 {
01794 data->omega = new_omega;
01795 }
01796
01797
01798
01799
01800
01801
01802 Message *
01803 MotorInterface::OrbitMessage::clone() const
01804 {
01805 return new MotorInterface::OrbitMessage(this);
01806 }
01807
01808
01809
01810
01811
01812
01813
01814
01815
01816
01817
01818
01819 MotorInterface::LinTransRotMessage::LinTransRotMessage(const float ini_vx, const float ini_vy, const float ini_omega) : Message("LinTransRotMessage")
01820 {
01821 data_size = sizeof(LinTransRotMessage_data_t);
01822 data_ptr = malloc(data_size);
01823 memset(data_ptr, 0, data_size);
01824 data = (LinTransRotMessage_data_t *)data_ptr;
01825 data_ts = (message_data_ts_t *)data_ptr;
01826 data->vx = ini_vx;
01827 data->vy = ini_vy;
01828 data->omega = ini_omega;
01829 add_fieldinfo(IFT_FLOAT, "vx", 1, &data->vx);
01830 add_fieldinfo(IFT_FLOAT, "vy", 1, &data->vy);
01831 add_fieldinfo(IFT_FLOAT, "omega", 1, &data->omega);
01832 }
01833
01834 MotorInterface::LinTransRotMessage::LinTransRotMessage() : Message("LinTransRotMessage")
01835 {
01836 data_size = sizeof(LinTransRotMessage_data_t);
01837 data_ptr = malloc(data_size);
01838 memset(data_ptr, 0, data_size);
01839 data = (LinTransRotMessage_data_t *)data_ptr;
01840 data_ts = (message_data_ts_t *)data_ptr;
01841 add_fieldinfo(IFT_FLOAT, "vx", 1, &data->vx);
01842 add_fieldinfo(IFT_FLOAT, "vy", 1, &data->vy);
01843 add_fieldinfo(IFT_FLOAT, "omega", 1, &data->omega);
01844 }
01845
01846
01847 MotorInterface::LinTransRotMessage::~LinTransRotMessage()
01848 {
01849 free(data_ptr);
01850 }
01851
01852
01853
01854
01855 MotorInterface::LinTransRotMessage::LinTransRotMessage(const LinTransRotMessage *m) : Message("LinTransRotMessage")
01856 {
01857 data_size = m->data_size;
01858 data_ptr = malloc(data_size);
01859 memcpy(data_ptr, m->data_ptr, data_size);
01860 data = (LinTransRotMessage_data_t *)data_ptr;
01861 data_ts = (message_data_ts_t *)data_ptr;
01862 }
01863
01864
01865
01866
01867
01868
01869 float
01870 MotorInterface::LinTransRotMessage::vx() const
01871 {
01872 return data->vx;
01873 }
01874
01875
01876
01877
01878
01879 size_t
01880 MotorInterface::LinTransRotMessage::maxlenof_vx() const
01881 {
01882 return 1;
01883 }
01884
01885
01886
01887
01888
01889 void
01890 MotorInterface::LinTransRotMessage::set_vx(const float new_vx)
01891 {
01892 data->vx = new_vx;
01893 }
01894
01895
01896
01897
01898
01899 float
01900 MotorInterface::LinTransRotMessage::vy() const
01901 {
01902 return data->vy;
01903 }
01904
01905
01906
01907
01908
01909 size_t
01910 MotorInterface::LinTransRotMessage::maxlenof_vy() const
01911 {
01912 return 1;
01913 }
01914
01915
01916
01917
01918
01919 void
01920 MotorInterface::LinTransRotMessage::set_vy(const float new_vy)
01921 {
01922 data->vy = new_vy;
01923 }
01924
01925
01926
01927
01928
01929 float
01930 MotorInterface::LinTransRotMessage::omega() const
01931 {
01932 return data->omega;
01933 }
01934
01935
01936
01937
01938
01939 size_t
01940 MotorInterface::LinTransRotMessage::maxlenof_omega() const
01941 {
01942 return 1;
01943 }
01944
01945
01946
01947
01948
01949 void
01950 MotorInterface::LinTransRotMessage::set_omega(const float new_omega)
01951 {
01952 data->omega = new_omega;
01953 }
01954
01955
01956
01957
01958
01959
01960 Message *
01961 MotorInterface::LinTransRotMessage::clone() const
01962 {
01963 return new MotorInterface::LinTransRotMessage(this);
01964 }
01965
01966
01967
01968
01969 bool
01970 MotorInterface::message_valid(const Message *message) const
01971 {
01972 const SetMotorStateMessage *m0 = dynamic_cast<const SetMotorStateMessage *>(message);
01973 if ( m0 != NULL ) {
01974 return true;
01975 }
01976 const AcquireControlMessage *m1 = dynamic_cast<const AcquireControlMessage *>(message);
01977 if ( m1 != NULL ) {
01978 return true;
01979 }
01980 const ResetOdometryMessage *m2 = dynamic_cast<const ResetOdometryMessage *>(message);
01981 if ( m2 != NULL ) {
01982 return true;
01983 }
01984 const DriveRPMMessage *m3 = dynamic_cast<const DriveRPMMessage *>(message);
01985 if ( m3 != NULL ) {
01986 return true;
01987 }
01988 const GotoMessage *m4 = dynamic_cast<const GotoMessage *>(message);
01989 if ( m4 != NULL ) {
01990 return true;
01991 }
01992 const TransMessage *m5 = dynamic_cast<const TransMessage *>(message);
01993 if ( m5 != NULL ) {
01994 return true;
01995 }
01996 const RotMessage *m6 = dynamic_cast<const RotMessage *>(message);
01997 if ( m6 != NULL ) {
01998 return true;
01999 }
02000 const TransRotMessage *m7 = dynamic_cast<const TransRotMessage *>(message);
02001 if ( m7 != NULL ) {
02002 return true;
02003 }
02004 const OrbitMessage *m8 = dynamic_cast<const OrbitMessage *>(message);
02005 if ( m8 != NULL ) {
02006 return true;
02007 }
02008 const LinTransRotMessage *m9 = dynamic_cast<const LinTransRotMessage *>(message);
02009 if ( m9 != NULL ) {
02010 return true;
02011 }
02012 return false;
02013 }
02014
02015
02016 EXPORT_INTERFACE(MotorInterface)
02017
02018
02019
02020 }