├── momobot_ws ├── src │ ├── semantic_pose │ │ ├── .gitignore │ │ ├── src │ │ │ ├── launch │ │ │ │ └── semantic_pose.launch │ │ │ └── semantic_pose_node.cpp │ │ ├── include │ │ │ └── places.h │ │ ├── config │ │ │ └── config.yaml │ │ ├── CMakeLists.txt │ │ └── package.xml │ ├── momobot │ │ ├── teensy │ │ │ └── firmware │ │ │ │ ├── .gitignore │ │ │ │ ├── platformio.ini │ │ │ │ └── lib │ │ │ │ ├── encoder │ │ │ │ ├── keywords.txt │ │ │ │ ├── Encoder.cpp │ │ │ │ ├── examples │ │ │ │ │ ├── Basic │ │ │ │ │ │ └── Basic.pde │ │ │ │ │ ├── TwoKnobs │ │ │ │ │ │ └── TwoKnobs.pde │ │ │ │ │ └── NoInterrupts │ │ │ │ │ │ └── NoInterrupts.pde │ │ │ │ └── utility │ │ │ │ │ └── direct_pin_read.h │ │ │ │ ├── motor │ │ │ │ ├── Motor.h │ │ │ │ └── test.ino │ │ │ │ ├── pid │ │ │ │ ├── PID.h │ │ │ │ ├── PID.cpp │ │ │ │ └── test.ino │ │ │ │ ├── imu │ │ │ │ ├── fake_mag.h │ │ │ │ └── Imu.h │ │ │ │ └── ros_lib │ │ │ │ ├── std_msgs │ │ │ │ ├── Empty.h │ │ │ │ ├── Char.h │ │ │ │ ├── UInt8.h │ │ │ │ ├── UInt16.h │ │ │ │ ├── Bool.h │ │ │ │ ├── Byte.h │ │ │ │ ├── Int8.h │ │ │ │ ├── String.h │ │ │ │ ├── UInt32.h │ │ │ │ ├── Int16.h │ │ │ │ ├── Int32.h │ │ │ │ ├── Float32.h │ │ │ │ ├── UInt8MultiArray.h │ │ │ │ ├── UInt64.h │ │ │ │ └── UInt16MultiArray.h │ │ │ │ ├── geometry_msgs │ │ │ │ ├── Wrench.h │ │ │ │ ├── Accel.h │ │ │ │ ├── Twist.h │ │ │ │ ├── PoseStamped.h │ │ │ │ ├── AccelStamped.h │ │ │ │ ├── PointStamped.h │ │ │ │ ├── TwistStamped.h │ │ │ │ ├── Pose.h │ │ │ │ ├── WrenchStamped.h │ │ │ │ ├── Vector3Stamped.h │ │ │ │ ├── InertiaStamped.h │ │ │ │ ├── PolygonStamped.h │ │ │ │ ├── Transform.h │ │ │ │ ├── QuaternionStamped.h │ │ │ │ ├── PoseWithCovarianceStamped.h │ │ │ │ ├── AccelWithCovarianceStamped.h │ │ │ │ ├── TwistWithCovarianceStamped.h │ │ │ │ ├── Polygon.h │ │ │ │ ├── PoseArray.h │ │ │ │ └── TransformStamped.h │ │ │ │ ├── lino_msgs │ │ │ │ └── Imu.h │ │ │ │ ├── rosserial_msgs │ │ │ │ └── Log.h │ │ │ │ └── sensor_msgs │ │ │ │ ├── JoyFeedbackArray.h │ │ │ │ └── NavSatStatus.h │ │ ├── teensy_bakbak │ │ │ └── firmware │ │ │ │ ├── .gitignore │ │ │ │ ├── platformio.ini │ │ │ │ └── lib │ │ │ │ ├── encoder │ │ │ │ ├── keywords.txt │ │ │ │ ├── Encoder.cpp │ │ │ │ ├── examples │ │ │ │ │ ├── Basic │ │ │ │ │ │ └── Basic.pde │ │ │ │ │ ├── TwoKnobs │ │ │ │ │ │ └── TwoKnobs.pde │ │ │ │ │ └── NoInterrupts │ │ │ │ │ │ └── NoInterrupts.pde │ │ │ │ └── utility │ │ │ │ │ └── direct_pin_read.h │ │ │ │ ├── motor │ │ │ │ ├── Motor.h │ │ │ │ └── test.ino │ │ │ │ ├── pid │ │ │ │ ├── PID.h │ │ │ │ ├── PID.cpp │ │ │ │ └── test.ino │ │ │ │ ├── imu │ │ │ │ ├── fake_mag.h │ │ │ │ └── Imu.h │ │ │ │ └── ros_lib │ │ │ │ ├── std_msgs │ │ │ │ ├── Empty.h │ │ │ │ ├── Char.h │ │ │ │ ├── UInt8.h │ │ │ │ ├── UInt16.h │ │ │ │ ├── Bool.h │ │ │ │ ├── Byte.h │ │ │ │ ├── Int8.h │ │ │ │ ├── String.h │ │ │ │ ├── UInt32.h │ │ │ │ ├── Int16.h │ │ │ │ ├── Int32.h │ │ │ │ ├── Float32.h │ │ │ │ ├── UInt8MultiArray.h │ │ │ │ ├── UInt64.h │ │ │ │ └── UInt16MultiArray.h │ │ │ │ ├── geometry_msgs │ │ │ │ ├── Accel.h │ │ │ │ ├── Twist.h │ │ │ │ ├── Wrench.h │ │ │ │ ├── PoseStamped.h │ │ │ │ ├── AccelStamped.h │ │ │ │ ├── PointStamped.h │ │ │ │ ├── Pose.h │ │ │ │ ├── TwistStamped.h │ │ │ │ ├── WrenchStamped.h │ │ │ │ ├── Vector3Stamped.h │ │ │ │ ├── InertiaStamped.h │ │ │ │ ├── PolygonStamped.h │ │ │ │ ├── Transform.h │ │ │ │ ├── QuaternionStamped.h │ │ │ │ ├── PoseWithCovarianceStamped.h │ │ │ │ ├── AccelWithCovarianceStamped.h │ │ │ │ ├── TwistWithCovarianceStamped.h │ │ │ │ ├── Polygon.h │ │ │ │ ├── PoseArray.h │ │ │ │ └── TransformStamped.h │ │ │ │ ├── lino_msgs │ │ │ │ └── Imu.h │ │ │ │ ├── rosserial_msgs │ │ │ │ └── Log.h │ │ │ │ └── sensor_msgs │ │ │ │ ├── JoyFeedbackArray.h │ │ │ │ └── NavSatStatus.h │ │ ├── maps │ │ │ ├── house.pgm │ │ │ ├── admissions.pgm │ │ │ ├── sutd_1_4.pgm │ │ │ ├── SUTD_LEVEL_1.pgm │ │ │ ├── fourth_floor.pgm │ │ │ ├── admissions_open.pgm │ │ │ ├── campus_centre.pgm │ │ │ ├── SUTD_LEVEL_1_LABELLED.pgm │ │ │ ├── SUTD_LEVEL_1_OPEN_HOUSE.pgm │ │ │ ├── SUTD_LEVEL_1_OPEN_HOUSE_CENTER_ONLY.pgm │ │ │ ├── house.yaml │ │ │ ├── SUTD_LEVEL_1.yaml │ │ │ ├── sutd_1_4.yaml │ │ │ ├── admissions.yaml │ │ │ ├── fourth_floor.yaml │ │ │ ├── SUTD_LEVEL_1_LABELLED.yaml │ │ │ ├── campus_centre.yaml │ │ │ ├── SUTD_LEVEL_1_OPEN_HOUSE.yaml │ │ │ └── SUTD_LEVEL_1_OPEN_HOUSE_CENTER_ONLY.yaml │ │ ├── src │ │ │ └── momo_base_node.cpp │ │ ├── param │ │ │ ├── navigation_bak │ │ │ │ ├── global_costmap_params.yaml │ │ │ │ ├── costmap_common_params.yaml │ │ │ │ ├── local_costmap_params.yaml │ │ │ │ ├── move_base_params.yaml │ │ │ │ ├── base_local_planner_default_params.yaml │ │ │ │ ├── base_local_planner_holonomic_params.yaml │ │ │ │ └── base_local_planner_ackermann_params.yaml │ │ │ ├── imu │ │ │ │ └── imu_calib.yaml │ │ │ ├── navigation │ │ │ │ ├── local_costmap_params.yaml │ │ │ │ ├── move_base_params.yaml │ │ │ │ ├── costmap_common_params.yaml │ │ │ │ ├── global_costmap_params.yaml │ │ │ │ └── base_local_planner_default_params.yaml │ │ │ └── ekf │ │ │ │ └── robot_localization.yaml │ │ ├── launch │ │ │ ├── include │ │ │ │ ├── lidar │ │ │ │ │ ├── xv11.launch │ │ │ │ │ ├── sweep.launch │ │ │ │ │ ├── lms111.launch │ │ │ │ │ ├── rplidar.launch │ │ │ │ │ ├── kinect.launch │ │ │ │ │ ├── realsense.launch │ │ │ │ │ ├── hokuyo.launch │ │ │ │ │ └── ydlidar.launch │ │ │ │ ├── laser.launch │ │ │ │ ├── move_base │ │ │ │ │ └── move_base_2wd.launch │ │ │ │ └── imu │ │ │ │ │ └── imu.launch │ │ │ ├── navigate.launch │ │ │ ├── minimal.launch │ │ │ ├── bringup.launch │ │ │ └── slam.launch │ │ ├── filters │ │ │ └── laser_shadow_filter.yaml │ │ ├── include │ │ │ └── momo_base.h │ │ ├── CMakeLists.txt │ │ ├── LICENSE │ │ └── package.xml │ ├── CMakeLists.txt │ ├── momo_emotions │ │ ├── src │ │ │ ├── momo_1.png │ │ │ ├── momo_2.png │ │ │ ├── momo_3.png │ │ │ ├── momo_XD.png │ │ │ ├── momo_neutral.png │ │ │ ├── PIDController.pyc │ │ │ └── PIDController.py │ │ ├── CMakeLists.txt │ │ ├── setup.py │ │ └── package.xml │ └── semantic_pose_sounder │ │ ├── media │ │ ├── lt_1.mp3 │ │ ├── mrt.mp3 │ │ ├── gom_gom.mp3 │ │ ├── library.mp3 │ │ ├── mont_calzone.mp3 │ │ ├── watashimomo.mp3 │ │ ├── campus_centre.mp3 │ │ └── crooked_cooks.mp3 │ │ ├── CMakeLists.txt │ │ ├── setup.py │ │ ├── launch │ │ └── semantic_pose_sounder.launch │ │ ├── package.xml │ │ ├── config │ │ └── config.yaml │ │ └── src │ │ └── semantic_pose_sounder ├── .catkin_workspace └── setup_scripts │ └── Linux │ └── README.md ├── assets ├── cute.png ├── colin-ni-10.jpg ├── MOMO_logo_full.png └── youtube_thumbnail.png ├── docs └── assets │ ├── Capture.PNG │ ├── Capture2.PNG │ ├── 1555308468557.png │ ├── 1555308481700.png │ ├── 1555308490615.png │ ├── 1555308503717.png │ ├── 1555308527000.png │ ├── 1555308539968.png │ ├── 1555308548007.png │ ├── 1555308556859.png │ └── 06_be544658-0682-45ea-8461-9ecee8e482f2_2048x2048.jpg ├── .gitignore ├── .gitmodules └── LICENSE /momobot_ws/src/semantic_pose/.gitignore: -------------------------------------------------------------------------------- 1 | .vscode -------------------------------------------------------------------------------- /momobot_ws/src/momobot/teensy/firmware/.gitignore: -------------------------------------------------------------------------------- 1 | .pioenvs 2 | -------------------------------------------------------------------------------- /momobot_ws/src/momobot/teensy_bakbak/firmware/.gitignore: -------------------------------------------------------------------------------- 1 | .pioenvs 2 | -------------------------------------------------------------------------------- /momobot_ws/src/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | /opt/ros/melodic/share/catkin/cmake/toplevel.cmake -------------------------------------------------------------------------------- /assets/cute.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/OpenSUTD/momobot/master/assets/cute.png -------------------------------------------------------------------------------- /assets/colin-ni-10.jpg: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/OpenSUTD/momobot/master/assets/colin-ni-10.jpg -------------------------------------------------------------------------------- /docs/assets/Capture.PNG: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/OpenSUTD/momobot/master/docs/assets/Capture.PNG -------------------------------------------------------------------------------- /docs/assets/Capture2.PNG: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/OpenSUTD/momobot/master/docs/assets/Capture2.PNG -------------------------------------------------------------------------------- /assets/MOMO_logo_full.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/OpenSUTD/momobot/master/assets/MOMO_logo_full.png -------------------------------------------------------------------------------- /assets/youtube_thumbnail.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/OpenSUTD/momobot/master/assets/youtube_thumbnail.png -------------------------------------------------------------------------------- /docs/assets/1555308468557.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/OpenSUTD/momobot/master/docs/assets/1555308468557.png -------------------------------------------------------------------------------- /docs/assets/1555308481700.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/OpenSUTD/momobot/master/docs/assets/1555308481700.png -------------------------------------------------------------------------------- /docs/assets/1555308490615.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/OpenSUTD/momobot/master/docs/assets/1555308490615.png -------------------------------------------------------------------------------- /docs/assets/1555308503717.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/OpenSUTD/momobot/master/docs/assets/1555308503717.png -------------------------------------------------------------------------------- /docs/assets/1555308527000.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/OpenSUTD/momobot/master/docs/assets/1555308527000.png -------------------------------------------------------------------------------- /docs/assets/1555308539968.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/OpenSUTD/momobot/master/docs/assets/1555308539968.png -------------------------------------------------------------------------------- /docs/assets/1555308548007.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/OpenSUTD/momobot/master/docs/assets/1555308548007.png -------------------------------------------------------------------------------- /docs/assets/1555308556859.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/OpenSUTD/momobot/master/docs/assets/1555308556859.png -------------------------------------------------------------------------------- /momobot_ws/src/momobot/maps/house.pgm: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/OpenSUTD/momobot/master/momobot_ws/src/momobot/maps/house.pgm -------------------------------------------------------------------------------- /momobot_ws/.catkin_workspace: -------------------------------------------------------------------------------- 1 | # This file currently only serves to mark the location of a catkin workspace for tool integration 2 | -------------------------------------------------------------------------------- /momobot_ws/src/momobot/maps/admissions.pgm: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/OpenSUTD/momobot/master/momobot_ws/src/momobot/maps/admissions.pgm -------------------------------------------------------------------------------- /momobot_ws/src/momobot/maps/sutd_1_4.pgm: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/OpenSUTD/momobot/master/momobot_ws/src/momobot/maps/sutd_1_4.pgm -------------------------------------------------------------------------------- /momobot_ws/src/momo_emotions/src/momo_1.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/OpenSUTD/momobot/master/momobot_ws/src/momo_emotions/src/momo_1.png -------------------------------------------------------------------------------- /momobot_ws/src/momo_emotions/src/momo_2.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/OpenSUTD/momobot/master/momobot_ws/src/momo_emotions/src/momo_2.png -------------------------------------------------------------------------------- /momobot_ws/src/momo_emotions/src/momo_3.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/OpenSUTD/momobot/master/momobot_ws/src/momo_emotions/src/momo_3.png -------------------------------------------------------------------------------- /momobot_ws/src/momo_emotions/src/momo_XD.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/OpenSUTD/momobot/master/momobot_ws/src/momo_emotions/src/momo_XD.png -------------------------------------------------------------------------------- /momobot_ws/src/momobot/maps/SUTD_LEVEL_1.pgm: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/OpenSUTD/momobot/master/momobot_ws/src/momobot/maps/SUTD_LEVEL_1.pgm -------------------------------------------------------------------------------- /momobot_ws/src/momobot/maps/fourth_floor.pgm: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/OpenSUTD/momobot/master/momobot_ws/src/momobot/maps/fourth_floor.pgm -------------------------------------------------------------------------------- /momobot_ws/src/momobot/maps/admissions_open.pgm: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/OpenSUTD/momobot/master/momobot_ws/src/momobot/maps/admissions_open.pgm -------------------------------------------------------------------------------- /momobot_ws/src/momobot/maps/campus_centre.pgm: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/OpenSUTD/momobot/master/momobot_ws/src/momobot/maps/campus_centre.pgm -------------------------------------------------------------------------------- /.gitignore: -------------------------------------------------------------------------------- 1 | *.swp 2 | *.swap 3 | *.save 4 | *.bak 5 | *.no_toc 6 | 7 | credentials/ 8 | build/ 9 | devel/ 10 | .catkin_tools/ 11 | logs/ 12 | -------------------------------------------------------------------------------- /momobot_ws/src/momo_emotions/src/momo_neutral.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/OpenSUTD/momobot/master/momobot_ws/src/momo_emotions/src/momo_neutral.png -------------------------------------------------------------------------------- /momobot_ws/src/momo_emotions/src/PIDController.pyc: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/OpenSUTD/momobot/master/momobot_ws/src/momo_emotions/src/PIDController.pyc -------------------------------------------------------------------------------- /momobot_ws/src/semantic_pose_sounder/media/lt_1.mp3: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/OpenSUTD/momobot/master/momobot_ws/src/semantic_pose_sounder/media/lt_1.mp3 -------------------------------------------------------------------------------- /momobot_ws/src/semantic_pose_sounder/media/mrt.mp3: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/OpenSUTD/momobot/master/momobot_ws/src/semantic_pose_sounder/media/mrt.mp3 -------------------------------------------------------------------------------- /momobot_ws/src/momobot/maps/SUTD_LEVEL_1_LABELLED.pgm: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/OpenSUTD/momobot/master/momobot_ws/src/momobot/maps/SUTD_LEVEL_1_LABELLED.pgm -------------------------------------------------------------------------------- /momobot_ws/src/semantic_pose_sounder/media/gom_gom.mp3: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/OpenSUTD/momobot/master/momobot_ws/src/semantic_pose_sounder/media/gom_gom.mp3 -------------------------------------------------------------------------------- /momobot_ws/src/semantic_pose_sounder/media/library.mp3: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/OpenSUTD/momobot/master/momobot_ws/src/semantic_pose_sounder/media/library.mp3 -------------------------------------------------------------------------------- /momobot_ws/src/momobot/maps/SUTD_LEVEL_1_OPEN_HOUSE.pgm: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/OpenSUTD/momobot/master/momobot_ws/src/momobot/maps/SUTD_LEVEL_1_OPEN_HOUSE.pgm -------------------------------------------------------------------------------- /momobot_ws/src/semantic_pose_sounder/media/mont_calzone.mp3: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/OpenSUTD/momobot/master/momobot_ws/src/semantic_pose_sounder/media/mont_calzone.mp3 -------------------------------------------------------------------------------- /momobot_ws/src/semantic_pose_sounder/media/watashimomo.mp3: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/OpenSUTD/momobot/master/momobot_ws/src/semantic_pose_sounder/media/watashimomo.mp3 -------------------------------------------------------------------------------- /momobot_ws/src/momobot/teensy/firmware/platformio.ini: -------------------------------------------------------------------------------- 1 | [env:teensy31] 2 | 3 | platform = teensy 4 | framework = arduino 5 | board = teensy31 6 | upload_port = /dev/momobase 7 | -------------------------------------------------------------------------------- /momobot_ws/src/semantic_pose_sounder/media/campus_centre.mp3: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/OpenSUTD/momobot/master/momobot_ws/src/semantic_pose_sounder/media/campus_centre.mp3 -------------------------------------------------------------------------------- /momobot_ws/src/semantic_pose_sounder/media/crooked_cooks.mp3: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/OpenSUTD/momobot/master/momobot_ws/src/semantic_pose_sounder/media/crooked_cooks.mp3 -------------------------------------------------------------------------------- /momobot_ws/src/momobot/teensy_bakbak/firmware/platformio.ini: -------------------------------------------------------------------------------- 1 | [env:teensy31] 2 | 3 | platform = teensy 4 | framework = arduino 5 | board = teensy31 6 | upload_port = /dev/momobase 7 | -------------------------------------------------------------------------------- /docs/assets/06_be544658-0682-45ea-8461-9ecee8e482f2_2048x2048.jpg: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/OpenSUTD/momobot/master/docs/assets/06_be544658-0682-45ea-8461-9ecee8e482f2_2048x2048.jpg -------------------------------------------------------------------------------- /momobot_ws/src/momobot/maps/SUTD_LEVEL_1_OPEN_HOUSE_CENTER_ONLY.pgm: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/OpenSUTD/momobot/master/momobot_ws/src/momobot/maps/SUTD_LEVEL_1_OPEN_HOUSE_CENTER_ONLY.pgm -------------------------------------------------------------------------------- /momobot_ws/src/momobot/maps/house.yaml: -------------------------------------------------------------------------------- 1 | image: house.pgm 2 | resolution: 0.050000 3 | origin: [-50.000000, -50.000000, 0.000000] 4 | negate: 0 5 | occupied_thresh: 0.65 6 | free_thresh: 0.196 7 | -------------------------------------------------------------------------------- /momobot_ws/src/momobot/maps/SUTD_LEVEL_1.yaml: -------------------------------------------------------------------------------- 1 | image: SUTD_LEVEL_1.pgm 2 | resolution: 0.050000 3 | origin: [-44.06, -97.0, 0.000000] 4 | negate: 0 5 | occupied_thresh: 0.65 6 | free_thresh: 0.196 7 | -------------------------------------------------------------------------------- /momobot_ws/src/momobot/maps/sutd_1_4.yaml: -------------------------------------------------------------------------------- 1 | image: sutd_1_4.pgm 2 | resolution: 0.050000 3 | origin: [-50.000000, -69.200000, 0.000000] 4 | negate: 0 5 | occupied_thresh: 0.65 6 | free_thresh: 0.196 7 | 8 | -------------------------------------------------------------------------------- /momobot_ws/src/momobot/teensy/firmware/lib/encoder/keywords.txt: -------------------------------------------------------------------------------- 1 | ENCODER_USE_INTERRUPTS LITERAL1 2 | ENCODER_OPTIMIZE_INTERRUPTS LITERAL1 3 | ENCODER_DO_NOT_USE_INTERRUPTS LITERAL1 4 | Encoder KEYWORD1 5 | -------------------------------------------------------------------------------- /momobot_ws/src/momobot/maps/admissions.yaml: -------------------------------------------------------------------------------- 1 | image: admissions.pgm 2 | resolution: 0.050000 3 | origin: [-50.000000, -50.000000, 0.000000] 4 | negate: 0 5 | occupied_thresh: 0.65 6 | free_thresh: 0.196 7 | 8 | -------------------------------------------------------------------------------- /momobot_ws/src/momobot/maps/fourth_floor.yaml: -------------------------------------------------------------------------------- 1 | image: fourth_floor.pgm 2 | resolution: 0.050000 3 | origin: [-50.000000, -50.000000, 0.000000] 4 | negate: 0 5 | occupied_thresh: 0.65 6 | free_thresh: 0.196 7 | 8 | -------------------------------------------------------------------------------- /momobot_ws/src/momobot/teensy_bakbak/firmware/lib/encoder/keywords.txt: -------------------------------------------------------------------------------- 1 | ENCODER_USE_INTERRUPTS LITERAL1 2 | ENCODER_OPTIMIZE_INTERRUPTS LITERAL1 3 | ENCODER_DO_NOT_USE_INTERRUPTS LITERAL1 4 | Encoder KEYWORD1 5 | -------------------------------------------------------------------------------- /momobot_ws/src/momobot/maps/SUTD_LEVEL_1_LABELLED.yaml: -------------------------------------------------------------------------------- 1 | image: SUTD_LEVEL_1_LABELLED.pgm 2 | resolution: 0.050000 3 | origin: [-44.06, -97.0, 0.000000] 4 | negate: 0 5 | occupied_thresh: 0.65 6 | free_thresh: 0.196 7 | -------------------------------------------------------------------------------- /momobot_ws/src/momobot/maps/campus_centre.yaml: -------------------------------------------------------------------------------- 1 | image: campus_centre.pgm 2 | resolution: 0.050000 3 | origin: [-50.000000, -50.000000, 0.000000] 4 | negate: 0 5 | occupied_thresh: 0.65 6 | free_thresh: 0.196 7 | 8 | -------------------------------------------------------------------------------- /momobot_ws/src/momobot/maps/SUTD_LEVEL_1_OPEN_HOUSE.yaml: -------------------------------------------------------------------------------- 1 | image: SUTD_LEVEL_1_OPEN_HOUSE.pgm 2 | resolution: 0.050000 3 | origin: [-44.06, -97.0, 0.000000] 4 | negate: 0 5 | occupied_thresh: 0.65 6 | free_thresh: 0.196 7 | -------------------------------------------------------------------------------- /momobot_ws/src/momobot/maps/SUTD_LEVEL_1_OPEN_HOUSE_CENTER_ONLY.yaml: -------------------------------------------------------------------------------- 1 | image: SUTD_LEVEL_1_OPEN_HOUSE_CENTER_ONLY.pgm 2 | resolution: 0.050000 3 | origin: [-44.06, -97.0, 0.000000] 4 | negate: 0 5 | occupied_thresh: 0.65 6 | free_thresh: 0.196 7 | -------------------------------------------------------------------------------- /momobot_ws/src/momobot/src/momo_base_node.cpp: -------------------------------------------------------------------------------- 1 | #include 2 | #include "momo_base.h" 3 | 4 | int main(int argc, char** argv ) 5 | { 6 | ros::init(argc, argv, "momo_base_node"); 7 | MomoBase momo; 8 | ros::spin(); 9 | return 0; 10 | } 11 | -------------------------------------------------------------------------------- /momobot_ws/src/momo_emotions/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 2.8.3) 2 | project(momo_emotions) 3 | 4 | find_package(catkin REQUIRED COMPONENTS 5 | rospy 6 | geometry_msgs 7 | sensor_msgs 8 | ) 9 | 10 | catkin_package() 11 | catkin_python_setup() 12 | -------------------------------------------------------------------------------- /momobot_ws/src/semantic_pose_sounder/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 2.8.3) 2 | project(semantic_pose_sounder) 3 | 4 | find_package(catkin REQUIRED COMPONENTS 5 | rospy 6 | geometry_msgs 7 | sensor_msgs 8 | ) 9 | 10 | catkin_package() 11 | catkin_python_setup() 12 | -------------------------------------------------------------------------------- /momobot_ws/src/momobot/param/navigation_bak/global_costmap_params.yaml: -------------------------------------------------------------------------------- 1 | global_costmap: 2 | global_frame: /map 3 | robot_base_frame: /base_footprint 4 | update_frequency: 1.0 #before: 5.0 5 | publish_frequency: 0.5 #before 0.5 6 | static_map: true 7 | transform_tolerance: 0.5 8 | cost_scaling_factor: 10.0 9 | inflation_radius: 0.55 -------------------------------------------------------------------------------- /momobot_ws/src/momobot/launch/include/lidar/xv11.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | -------------------------------------------------------------------------------- /momobot_ws/src/momobot/teensy/firmware/lib/encoder/Encoder.cpp: -------------------------------------------------------------------------------- 1 | 2 | #include "Encoder.h" 3 | 4 | // Yes, all the code is in the header file, to provide the user 5 | // configure options with #define (before they include it), and 6 | // to facilitate some crafty optimizations! 7 | 8 | Encoder_internal_state_t * Encoder::interruptArgs[]; 9 | 10 | 11 | -------------------------------------------------------------------------------- /momobot_ws/src/momo_emotions/setup.py: -------------------------------------------------------------------------------- 1 | #!/usr/bin/env python 2 | 3 | from distutils.core import setup 4 | from catkin_pkg.python_setup import generate_distutils_setup 5 | 6 | setup_args = generate_distutils_setup( 7 | packages = ['momo_emotions'], 8 | package_dir = {'': 'src'}, 9 | install_requires = [''] 10 | ) 11 | 12 | setup(**setup_args) 13 | -------------------------------------------------------------------------------- /momobot_ws/src/momobot/teensy_bakbak/firmware/lib/encoder/Encoder.cpp: -------------------------------------------------------------------------------- 1 | 2 | #include "Encoder.h" 3 | 4 | // Yes, all the code is in the header file, to provide the user 5 | // configure options with #define (before they include it), and 6 | // to facilitate some crafty optimizations! 7 | 8 | Encoder_internal_state_t * Encoder::interruptArgs[]; 9 | 10 | 11 | -------------------------------------------------------------------------------- /momobot_ws/setup_scripts/Linux/README.md: -------------------------------------------------------------------------------- 1 | # Linux Install Scripts 2 | 3 | Source: 4 | 5 | Once you download these, ensure you enable the executable permissions for them with: 6 | 7 | ```bash 8 | $ chmod +x name_of_file 9 | ``` 10 | 11 | And then run them with 12 | 13 | ```bash 14 | $ ./name_of_file 15 | ``` 16 | 17 | -------------------------------------------------------------------------------- /momobot_ws/src/semantic_pose_sounder/setup.py: -------------------------------------------------------------------------------- 1 | #!/usr/bin/env python 2 | 3 | from distutils.core import setup 4 | from catkin_pkg.python_setup import generate_distutils_setup 5 | 6 | setup_args = generate_distutils_setup( 7 | packages = ['semantic_pose_sounder'], 8 | package_dir = {'': 'src'}, 9 | install_requires = [''] 10 | ) 11 | 12 | setup(**setup_args) 13 | -------------------------------------------------------------------------------- /momobot_ws/src/momobot/param/imu/imu_calib.yaml: -------------------------------------------------------------------------------- 1 | SM: 2 | - 1.011195335294985 3 | - -0.005478610695936285 4 | - -0.005023688364125308 5 | - 0.04996443396208794 6 | - 0.999238462583232 7 | - -0.03743896212053192 8 | - -0.01279156783449262 9 | - 0.08739016446413006 10 | - 1.015073451343559 11 | bias: 12 | - -0.4328585554985973 13 | - -0.3987367212348277 14 | - -0.9287827723881869 -------------------------------------------------------------------------------- /momobot_ws/src/momobot/param/navigation_bak/costmap_common_params.yaml: -------------------------------------------------------------------------------- 1 | obstacle_range: 2.5 2 | raytrace_range: 3.0 3 | footprint: [[-0.24, -0.22], [-0.24, 0.22], [0.24, 0.22], [0.24, -0.22]] 4 | inflation_radius: 0.55 5 | transform_tolerance: 0.5 6 | 7 | observation_sources: scan 8 | scan: 9 | data_type: LaserScan 10 | topic: scan 11 | marking: true 12 | clearing: true 13 | 14 | map_type: costmap 15 | -------------------------------------------------------------------------------- /momobot_ws/src/momobot/launch/include/lidar/sweep.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | -------------------------------------------------------------------------------- /momobot_ws/src/semantic_pose/src/launch/semantic_pose.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | -------------------------------------------------------------------------------- /momobot_ws/src/momobot/param/navigation_bak/local_costmap_params.yaml: -------------------------------------------------------------------------------- 1 | local_costmap: 2 | global_frame: /odom 3 | robot_base_frame: /base_footprint 4 | update_frequency: 1.0 #before 5.0 5 | publish_frequency: 2.0 #before 2.0 6 | static_map: false 7 | rolling_window: true 8 | width: 2.5 9 | height: 2.5 10 | resolution: 0.05 #increase to for higher res 0.025 11 | transform_tolerance: 0.5 12 | cost_scaling_factor: 5 13 | inflation_radius: 0.55 -------------------------------------------------------------------------------- /momobot_ws/src/momobot/launch/include/lidar/lms111.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | -------------------------------------------------------------------------------- /momobot_ws/src/momobot/launch/include/lidar/rplidar.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | -------------------------------------------------------------------------------- /momobot_ws/src/momobot/param/navigation/local_costmap_params.yaml: -------------------------------------------------------------------------------- 1 | local_costmap: 2 | global_frame: /odom 3 | robot_base_frame: /base_link 4 | update_frequency: 5.0 #before 5.0 5 | publish_frequency: 2.0 #before 2.0 6 | static_map: false 7 | rolling_window: true 8 | width: 3.5 9 | height: 3.5 10 | resolution: 0.05 #before 0.05 11 | transform_tolerance: 0.1 12 | 13 | plugins: 14 | - {name: obstacle_layer, type: 'costmap_2d::ObstacleLayer'} 15 | - {name: inflation_layer, type: 'costmap_2d::InflationLayer'} 16 | 17 | -------------------------------------------------------------------------------- /momobot_ws/src/momobot/filters/laser_shadow_filter.yaml: -------------------------------------------------------------------------------- 1 | scan_filter_chain: 2 | - name: shadows 3 | type: ScanShadowsFilter 4 | params: 5 | min_angle: 8 6 | max_angle: 180 7 | neighbors: 5 8 | window: 1 9 | - name: dark_shadows 10 | type: LaserScanIntensityFilter 11 | params: 12 | lower_threshold: 100 13 | upper_threshold: 10000 14 | disp_histogram: 0 15 | - name: laser_cutoff 16 | type: laser_filters/LaserScanAngularBoundsFilter 17 | params: 18 | lower_angle: -2.09 # -1.30 19 | upper_angle: 2.09 # 1.30 20 | 21 | -------------------------------------------------------------------------------- /momobot_ws/src/momobot/teensy/firmware/lib/motor/Motor.h: -------------------------------------------------------------------------------- 1 | #ifndef MOTOR_H 2 | #define MOTOR_H 3 | 4 | #include 5 | #include 6 | 7 | class Controller 8 | { 9 | public: 10 | enum driver {L298, BTS7960, ESC}; 11 | Controller(driver motor_driver, int pwm_pin, int motor_pinA, int motor_pinB); 12 | void spin(int pwm); 13 | 14 | private: 15 | Servo motor_; 16 | driver motor_driver_; 17 | int pwm_pin_; 18 | int motor_pinA_; 19 | int motor_pinB_; 20 | }; 21 | 22 | #endif 23 | -------------------------------------------------------------------------------- /momobot_ws/src/momobot/param/navigation_bak/move_base_params.yaml: -------------------------------------------------------------------------------- 1 | base_global_planner: global_planner/GlobalPlanner 2 | base_local_planner: dwa_local_planner/DWAPlannerROS 3 | 4 | shutdown_costmaps: false 5 | 6 | controller_frequency: 5.0 #before 5.0 7 | controller_patience: 3.0 8 | 9 | planner_frequency: 0.5 10 | planner_patience: 5.0 11 | 12 | oscillation_timeout: 10.0 13 | oscillation_distance: 0.2 14 | 15 | conservative_reset_dist: 0.1 #distance from an obstacle at which it will unstuck itself 16 | 17 | cost_factor: 1.0 18 | neutral_cost: 55 19 | lethal_cost: 253 -------------------------------------------------------------------------------- /momobot_ws/src/momobot/teensy_bakbak/firmware/lib/motor/Motor.h: -------------------------------------------------------------------------------- 1 | #ifndef MOTOR_H 2 | #define MOTOR_H 3 | 4 | #include 5 | #include 6 | 7 | class Controller 8 | { 9 | public: 10 | enum driver {L298, BTS7960, ESC}; 11 | Controller(driver motor_driver, int pwm_pin, int motor_pinA, int motor_pinB); 12 | void spin(int pwm); 13 | 14 | private: 15 | Servo motor_; 16 | driver motor_driver_; 17 | int pwm_pin_; 18 | int motor_pinA_; 19 | int motor_pinB_; 20 | }; 21 | 22 | #endif 23 | -------------------------------------------------------------------------------- /momobot_ws/src/momobot/param/navigation/move_base_params.yaml: -------------------------------------------------------------------------------- 1 | base_global_planner: global_planner/GlobalPlanner 2 | base_local_planner: base_local_planner/TrajectoryPlannerROS 3 | 4 | cost_factor: 1.0 5 | neutral_cost: 55 6 | lethal_cost: 253 7 | 8 | shutdown_costmaps: true 9 | 10 | controller_frequency: 5.0 11 | controller_patience: 3.0 12 | 13 | planner_frequency: 1 14 | planner_patience: 5.0 15 | 16 | oscillation_timeout: 10.0 17 | oscillation_distance: 0.2 18 | 19 | conservative_reset_dist: 0.5 20 | recovery_behavior_enabled: true 21 | clearing_rotation_allowed: false 22 | -------------------------------------------------------------------------------- /momobot_ws/src/momobot/param/navigation/costmap_common_params.yaml: -------------------------------------------------------------------------------- 1 | obstacle_range: 2.5 2 | raytrace_range: 20.0 3 | footprint: [[-0.3, -0.30], [-0.3, 0.30], [0.3, 0.30], [0.3, -0.30]] 4 | 5 | inflation_radius: 2.75 6 | 7 | transform_tolerance: 0.13 8 | 9 | map_type: costmap 10 | 11 | obstacle_layer: 12 | # observation_sources: scan 13 | observation_sources: laser_scan_sensor 14 | # laser_scan_sensor: {data_type: LaserScan, topic: scan_filtered, marking: true, clearing: true} 15 | laser_scan_sensor: {data_type: LaserScan, topic: scan, marking: true, clearing: true} 16 | 17 | -------------------------------------------------------------------------------- /momobot_ws/src/semantic_pose_sounder/launch/semantic_pose_sounder.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | -------------------------------------------------------------------------------- /momobot_ws/src/momobot/launch/include/lidar/kinect.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | -------------------------------------------------------------------------------- /momobot_ws/src/momobot/param/navigation/global_costmap_params.yaml: -------------------------------------------------------------------------------- 1 | global_costmap: 2 | global_frame: /map 3 | robot_base_frame: base_link 4 | update_frequency: 5.0 #before: 5.0 5 | publish_frequency: 2.0 #before 0.5 6 | static_map: true 7 | resolution: 0.05 8 | transform_tolerance: 0.1 9 | footprint_padding: 0.05 10 | cost_scaling_factor: 5.0 11 | inflation_radius: 5.0 12 | 13 | 14 | plugins: 15 | - {name: static_layer, type: 'costmap_2d::StaticLayer'} 16 | - {name: obstacle_layer, type: 'costmap_2d::ObstacleLayer'} 17 | - {name: inflation_layer, type: 'costmap_2d::InflationLayer'} 18 | 19 | -------------------------------------------------------------------------------- /momobot_ws/src/momobot/launch/include/lidar/realsense.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | -------------------------------------------------------------------------------- /momobot_ws/src/momobot/teensy/firmware/lib/pid/PID.h: -------------------------------------------------------------------------------- 1 | #ifndef PID_H 2 | #define PID_H 3 | 4 | #include "Arduino.h" 5 | 6 | class PID 7 | { 8 | public: 9 | PID(float min_val, float max_val, float kp, float ki, float kd); 10 | double compute(float setpoint, float measured_value); 11 | void updateConstants(float kp, float ki, float kd); 12 | 13 | private: 14 | float min_val_; 15 | float max_val_; 16 | float kp_; 17 | float ki_; 18 | float kd_; 19 | double integral_; 20 | double derivative_; 21 | double prev_error_; 22 | }; 23 | 24 | #endif 25 | -------------------------------------------------------------------------------- /momobot_ws/src/momobot/teensy_bakbak/firmware/lib/pid/PID.h: -------------------------------------------------------------------------------- 1 | #ifndef PID_H 2 | #define PID_H 3 | 4 | #include "Arduino.h" 5 | 6 | class PID 7 | { 8 | public: 9 | PID(float min_val, float max_val, float kp, float ki, float kd); 10 | double compute(float setpoint, float measured_value); 11 | void updateConstants(float kp, float ki, float kd); 12 | 13 | private: 14 | float min_val_; 15 | float max_val_; 16 | float kp_; 17 | float ki_; 18 | float kd_; 19 | double integral_; 20 | double derivative_; 21 | double prev_error_; 22 | }; 23 | 24 | #endif 25 | -------------------------------------------------------------------------------- /momobot_ws/src/momobot/param/navigation/base_local_planner_default_params.yaml: -------------------------------------------------------------------------------- 1 | TrajectoryPlannerROS: 2 | max_vel_x: 0.75 3 | min_vel_x: 0.1 4 | max_vel_theta: 0.35 #0.2 5 | min_vel_theta: -0.35 #-0.2 6 | min_in_place_vel_theta: 0.20 #0.4 7 | 8 | # acc theta 1.2 9 | acc_lim_theta: 1.2 10 | acc_lim_x: 1 11 | acc_lim_y: 0.0 12 | 13 | holonomic_robot: false 14 | 15 | xy_goal_tolerance: 0.35 16 | yaw_goal_tolerance: 0.35 17 | 18 | sim_granularity: 0.10 19 | angular_sim_granularity: 0.10 20 | 21 | sim_time: 2.5 #3.5 22 | vx_samples: 20 23 | vtheta_samples: 40 24 | 25 | meter_scoring: true 26 | heading_lookahead: 0.75 #1.0 27 | -------------------------------------------------------------------------------- /momobot_ws/src/semantic_pose/include/places.h: -------------------------------------------------------------------------------- 1 | #ifndef _PLACES_H_ 2 | #define _PLACES_H_ 3 | 4 | #include 5 | #include 6 | #include 7 | 8 | class Places 9 | { 10 | public: 11 | Places(ros::NodeHandle& nh, ros::NodeHandle& nh_priv, const std::string& param_name); 12 | std::vector getPlaces(); 13 | std::vector > getBoundary(); 14 | std::string where_am_i(geometry_msgs::Point pt); 15 | 16 | private: 17 | std::vector places_; 18 | std::vector > boundary_; 19 | }; 20 | 21 | #endif -------------------------------------------------------------------------------- /momobot_ws/src/momobot/launch/navigate.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | -------------------------------------------------------------------------------- /momobot_ws/src/semantic_pose/config/config.yaml: -------------------------------------------------------------------------------- 1 | # config.yaml 2 | 3 | places: 4 | #square boundary 5 | - name: "place1" 6 | boundary: [[-5.01995038986, 0.341648638248], [ -5.02670955658,-2.89252948761], [-6.98518037796, -2.95654678345], [-7.06083536148, 0.271802127361]] 7 | 8 | #irregular shape boundary 9 | - name: "place2" 10 | boundary: [[-7.01539182663, 0.339973062277],[-1.44279897213 ,0.283673077822],[ 4.77711629868,6.89892435074],[4.77711677551 ,9.23537540436],[-5.74889469147 ,9.23537540436],[ -8.45075798035,6.56112480164]] 11 | 12 | #triangle boundary 13 | - name: "place3" 14 | boundary: [[4.73916625977,6.79155063629],[4.66398239136,3.70972466469],[2.89470362663,1.91181647778]] 15 | -------------------------------------------------------------------------------- /momobot_ws/src/momobot/launch/include/laser.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | -------------------------------------------------------------------------------- /momobot_ws/src/semantic_pose/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 2.8.3) 2 | project(semantic_pose) 3 | 4 | find_package(catkin REQUIRED COMPONENTS 5 | roscpp 6 | std_msgs 7 | tf 8 | geometry_msgs 9 | ) 10 | 11 | catkin_package( 12 | INCLUDE_DIRS include 13 | LIBRARIES semantic_pose 14 | CATKIN_DEPENDS roscpp tf std_msgs geometry_msgs 15 | DEPENDS system_lib 16 | ) 17 | 18 | add_compile_options(-std=c++11) 19 | 20 | include_directories( 21 | include 22 | ${catkin_INCLUDE_DIRS} 23 | ) 24 | 25 | add_library(places 26 | src/places.cpp 27 | ) 28 | 29 | add_executable(semantic_pose_node src/semantic_pose_node.cpp) 30 | 31 | target_link_libraries(semantic_pose_node 32 | places 33 | ${catkin_LIBRARIES} 34 | ) 35 | -------------------------------------------------------------------------------- /momobot_ws/src/semantic_pose/package.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | semantic_pose 4 | 0.0.0 5 | The semantic_pose package 6 | 7 | juan 8 | BSD 9 | 10 | catkin 11 | roscpp 12 | std_msgs 13 | tf 14 | geometry_msgs 15 | 16 | roscpp 17 | std_msgs 18 | tf 19 | geometry_msgs 20 | 21 | 22 | 23 | 24 | 25 | -------------------------------------------------------------------------------- /momobot_ws/src/momobot/teensy/firmware/lib/imu/fake_mag.h: -------------------------------------------------------------------------------- 1 | #ifndef _FAKE_MAG_H_ 2 | #define _FAKE_MAG_H_ 3 | 4 | 5 | //This is a pseudo Magnetometer Class as a placeholder for IMUs that do not have a Magnetometer 6 | 7 | class FakeMag 8 | { 9 | public: 10 | FakeMag(); 11 | bool initialize(); 12 | bool testConnection(); 13 | void getHeading(int16_t* mx, int16_t* my, int16_t* mz); 14 | }; 15 | 16 | FakeMag::FakeMag() 17 | { 18 | 19 | } 20 | 21 | bool FakeMag::initialize() 22 | { 23 | return true; 24 | } 25 | 26 | bool FakeMag::testConnection() 27 | { 28 | return true; 29 | } 30 | 31 | void FakeMag::getHeading(int16_t* mx, int16_t* my, int16_t* mz) 32 | { 33 | *mx = 0; 34 | *my = 0; 35 | *mz = 0; 36 | } 37 | 38 | #endif -------------------------------------------------------------------------------- /momobot_ws/src/momobot/teensy_bakbak/firmware/lib/imu/fake_mag.h: -------------------------------------------------------------------------------- 1 | #ifndef _FAKE_MAG_H_ 2 | #define _FAKE_MAG_H_ 3 | 4 | 5 | //This is a pseudo Magnetometer Class as a placeholder for IMUs that do not have a Magnetometer 6 | 7 | class FakeMag 8 | { 9 | public: 10 | FakeMag(); 11 | bool initialize(); 12 | bool testConnection(); 13 | void getHeading(int16_t* mx, int16_t* my, int16_t* mz); 14 | }; 15 | 16 | FakeMag::FakeMag() 17 | { 18 | 19 | } 20 | 21 | bool FakeMag::initialize() 22 | { 23 | return true; 24 | } 25 | 26 | bool FakeMag::testConnection() 27 | { 28 | return true; 29 | } 30 | 31 | void FakeMag::getHeading(int16_t* mx, int16_t* my, int16_t* mz) 32 | { 33 | *mx = 0; 34 | *my = 0; 35 | *mz = 0; 36 | } 37 | 38 | #endif -------------------------------------------------------------------------------- /momobot_ws/src/momobot/include/momo_base.h: -------------------------------------------------------------------------------- 1 | #ifndef MOMO_BASE_H 2 | #define MOMO_BASE_H 3 | 4 | #include 5 | #include 6 | #include 7 | 8 | class MomoBase 9 | { 10 | public: 11 | MomoBase(); 12 | void velCallback(const lino_msgs::Velocities& vel); 13 | 14 | private: 15 | ros::NodeHandle nh_; 16 | ros::Publisher odom_publisher_; 17 | ros::Subscriber velocity_subscriber_; 18 | tf::TransformBroadcaster odom_broadcaster_; 19 | 20 | float steering_angle_; 21 | float linear_velocity_x_; 22 | float linear_velocity_y_; 23 | float angular_velocity_z_; 24 | ros::Time last_vel_time_; 25 | float vel_dt_; 26 | float x_pos_; 27 | float y_pos_; 28 | float heading_; 29 | }; 30 | 31 | #endif 32 | -------------------------------------------------------------------------------- /.gitmodules: -------------------------------------------------------------------------------- 1 | [submodule "src/LMS1xx"] 2 | path = momobot_ws/src/LMS1xx 3 | url = https://github.com/methylDragon/LMS1xx.git 4 | branch = methyl-patch 5 | 6 | [submodule "src/imu_calib"] 7 | path = momobot_ws/src/imu_calib 8 | url = https://github.com/linorobot/imu_calib.git 9 | 10 | [submodule "src/laser_filters"] 11 | path = momobot_ws/src/laser_filters 12 | url = https://github.com/ros-perception/laser_filters.git 13 | 14 | [submodule "src/lino_msgs"] 15 | path = momobot_ws/src/lino_msgs 16 | url = https://github.com/linorobot/lino_msgs.git 17 | 18 | [submodule "src/lino_pid"] 19 | path = momobot_ws/src/lino_pid 20 | url = https://github.com/linorobot/lino_pid.git 21 | 22 | [submodule "src/lino_udev"] 23 | path = momobot_ws/src/lino_udev 24 | url = https://github.com/linorobot/lino_udev.git 25 | -------------------------------------------------------------------------------- /momobot_ws/src/momobot/teensy/firmware/lib/ros_lib/std_msgs/Empty.h: -------------------------------------------------------------------------------- 1 | #ifndef _ROS_std_msgs_Empty_h 2 | #define _ROS_std_msgs_Empty_h 3 | 4 | #include 5 | #include 6 | #include 7 | #include "ros/msg.h" 8 | 9 | namespace std_msgs 10 | { 11 | 12 | class Empty : public ros::Msg 13 | { 14 | public: 15 | 16 | Empty() 17 | { 18 | } 19 | 20 | virtual int serialize(unsigned char *outbuffer) const 21 | { 22 | int offset = 0; 23 | return offset; 24 | } 25 | 26 | virtual int deserialize(unsigned char *inbuffer) 27 | { 28 | int offset = 0; 29 | return offset; 30 | } 31 | 32 | const char * getType(){ return "std_msgs/Empty"; }; 33 | const char * getMD5(){ return "d41d8cd98f00b204e9800998ecf8427e"; }; 34 | 35 | }; 36 | 37 | } 38 | #endif -------------------------------------------------------------------------------- /momobot_ws/src/momobot/teensy_bakbak/firmware/lib/ros_lib/std_msgs/Empty.h: -------------------------------------------------------------------------------- 1 | #ifndef _ROS_std_msgs_Empty_h 2 | #define _ROS_std_msgs_Empty_h 3 | 4 | #include 5 | #include 6 | #include 7 | #include "ros/msg.h" 8 | 9 | namespace std_msgs 10 | { 11 | 12 | class Empty : public ros::Msg 13 | { 14 | public: 15 | 16 | Empty() 17 | { 18 | } 19 | 20 | virtual int serialize(unsigned char *outbuffer) const 21 | { 22 | int offset = 0; 23 | return offset; 24 | } 25 | 26 | virtual int deserialize(unsigned char *inbuffer) 27 | { 28 | int offset = 0; 29 | return offset; 30 | } 31 | 32 | const char * getType(){ return "std_msgs/Empty"; }; 33 | const char * getMD5(){ return "d41d8cd98f00b204e9800998ecf8427e"; }; 34 | 35 | }; 36 | 37 | } 38 | #endif -------------------------------------------------------------------------------- /momobot_ws/src/momobot/launch/include/move_base/move_base_2wd.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | -------------------------------------------------------------------------------- /momobot_ws/src/momobot/teensy/firmware/lib/encoder/examples/Basic/Basic.pde: -------------------------------------------------------------------------------- 1 | /* Encoder Library - Basic Example 2 | * http://www.pjrc.com/teensy/td_libs_Encoder.html 3 | * 4 | * This example code is in the public domain. 5 | */ 6 | 7 | #include 8 | 9 | // Change these two numbers to the pins connected to your encoder. 10 | // Best Performance: both pins have interrupt capability 11 | // Good Performance: only the first pin has interrupt capability 12 | // Low Performance: neither pin has interrupt capability 13 | Encoder myEnc(5, 6); 14 | // avoid using pins with LEDs attached 15 | 16 | void setup() { 17 | Serial.begin(9600); 18 | Serial.println("Basic Encoder Test:"); 19 | } 20 | 21 | long oldPosition = -999; 22 | 23 | void loop() { 24 | long newPosition = myEnc.read(); 25 | if (newPosition != oldPosition) { 26 | oldPosition = newPosition; 27 | Serial.println(newPosition); 28 | } 29 | } 30 | -------------------------------------------------------------------------------- /momobot_ws/src/momobot/teensy_bakbak/firmware/lib/encoder/examples/Basic/Basic.pde: -------------------------------------------------------------------------------- 1 | /* Encoder Library - Basic Example 2 | * http://www.pjrc.com/teensy/td_libs_Encoder.html 3 | * 4 | * This example code is in the public domain. 5 | */ 6 | 7 | #include 8 | 9 | // Change these two numbers to the pins connected to your encoder. 10 | // Best Performance: both pins have interrupt capability 11 | // Good Performance: only the first pin has interrupt capability 12 | // Low Performance: neither pin has interrupt capability 13 | Encoder myEnc(5, 6); 14 | // avoid using pins with LEDs attached 15 | 16 | void setup() { 17 | Serial.begin(9600); 18 | Serial.println("Basic Encoder Test:"); 19 | } 20 | 21 | long oldPosition = -999; 22 | 23 | void loop() { 24 | long newPosition = myEnc.read(); 25 | if (newPosition != oldPosition) { 26 | oldPosition = newPosition; 27 | Serial.println(newPosition); 28 | } 29 | } 30 | -------------------------------------------------------------------------------- /momobot_ws/src/momobot/launch/include/lidar/hokuyo.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | 16 | 17 | 18 | 19 | 20 | 21 | -------------------------------------------------------------------------------- /momobot_ws/src/momo_emotions/package.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | momo_emotions 4 | 0.0.0 5 | MOMObot emotion module 6 | 7 | methylDragon 8 | methylDragon 9 | http://github.com/methylDragon 10 | 11 | MIT 12 | 13 | catkin 14 | 15 | rospy 16 | geometry_msgs 17 | sensor_msgs 18 | 19 | rospy 20 | geometry_msgs 21 | sensor_msgs 22 | 23 | rospy 24 | geometry_msgs 25 | sensor_msgs 26 | 27 | 28 | -------------------------------------------------------------------------------- /momobot_ws/src/semantic_pose_sounder/package.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | semantic_pose_sounder 4 | 0.0.0 5 | Semantic location based sound trigger 6 | 7 | methylDragon 8 | methylDragon 9 | http://github.com/methylDragon 10 | 11 | MIT 12 | 13 | catkin 14 | 15 | rospy 16 | geometry_msgs 17 | sensor_msgs 18 | 19 | rospy 20 | geometry_msgs 21 | sensor_msgs 22 | 23 | rospy 24 | geometry_msgs 25 | sensor_msgs 26 | 27 | 28 | -------------------------------------------------------------------------------- /momobot_ws/src/momobot/teensy/firmware/lib/pid/PID.cpp: -------------------------------------------------------------------------------- 1 | #include "Arduino.h" 2 | #include "PID.h" 3 | 4 | PID::PID(float min_val, float max_val, float kp, float ki, float kd): 5 | min_val_(min_val), 6 | max_val_(max_val), 7 | kp_(kp), 8 | ki_(ki), 9 | kd_(kd) 10 | { 11 | } 12 | 13 | double PID::compute(float setpoint, float measured_value) 14 | { 15 | double error; 16 | double pid; 17 | 18 | //setpoint is constrained between min and max to prevent pid from having too much error 19 | error = setpoint - measured_value; 20 | integral_ += error; 21 | derivative_ = error - prev_error_; 22 | 23 | if(setpoint == 0 && error == 0) 24 | { 25 | integral_ = 0; 26 | } 27 | 28 | pid = (kp_ * error) + (ki_ * integral_) + (kd_ * derivative_); 29 | prev_error_ = error; 30 | 31 | return constrain(pid, min_val_, max_val_); 32 | } 33 | 34 | void PID::updateConstants(float kp, float ki, float kd) 35 | { 36 | kp_ = kp; 37 | ki_ = ki; 38 | kd_ = kd; 39 | } 40 | -------------------------------------------------------------------------------- /momobot_ws/src/momobot/teensy_bakbak/firmware/lib/pid/PID.cpp: -------------------------------------------------------------------------------- 1 | #include "Arduino.h" 2 | #include "PID.h" 3 | 4 | PID::PID(float min_val, float max_val, float kp, float ki, float kd): 5 | min_val_(min_val), 6 | max_val_(max_val), 7 | kp_(kp), 8 | ki_(ki), 9 | kd_(kd) 10 | { 11 | } 12 | 13 | double PID::compute(float setpoint, float measured_value) 14 | { 15 | double error; 16 | double pid; 17 | 18 | //setpoint is constrained between min and max to prevent pid from having too much error 19 | error = setpoint - measured_value; 20 | integral_ += error; 21 | derivative_ = error - prev_error_; 22 | 23 | if(setpoint == 0 && error == 0) 24 | { 25 | integral_ = 0; 26 | } 27 | 28 | pid = (kp_ * error) + (ki_ * integral_) + (kd_ * derivative_); 29 | prev_error_ = error; 30 | 31 | return constrain(pid, min_val_, max_val_); 32 | } 33 | 34 | void PID::updateConstants(float kp, float ki, float kd) 35 | { 36 | kp_ = kp; 37 | ki_ = ki; 38 | kd_ = kd; 39 | } 40 | -------------------------------------------------------------------------------- /momobot_ws/src/momobot/param/navigation_bak/base_local_planner_default_params.yaml: -------------------------------------------------------------------------------- 1 | DWAPlannerROS: 2 | max_trans_vel: 0.50 3 | min_trans_vel: 0.01 4 | max_vel_x: 0.50 5 | min_vel_x: -0.025 6 | max_vel_y: 0.0 7 | min_vel_y: 0.0 8 | max_rot_vel: 0.30 9 | min_rot_vel: -0.30 10 | acc_lim_x: 1.25 11 | acc_lim_y: 0.0 12 | acc_lim_theta: 5 13 | acc_lim_trans: 1.25 14 | 15 | prune_plan: false 16 | 17 | xy_goal_tolerance: 0.25 18 | yaw_goal_tolerance: 0.1 19 | trans_stopped_vel: 0.1 20 | rot_stopped_vel: 0.1 21 | sim_time: 3.0 22 | sim_granularity: 0.1 23 | angular_sim_granularity: 0.1 24 | path_distance_bias: 34.0 25 | goal_distance_bias: 24.0 26 | occdist_scale: 0.05 27 | twirling_scale: 0.0 28 | stop_time_buffer: 0.5 29 | oscillation_reset_dist: 0.05 30 | oscillation_reset_angle: 0.2 31 | forward_point_distance: 0.3 32 | scaling_speed: 0.25 33 | max_scaling_factor: 0.2 34 | vx_samples: 20 35 | vy_samples: 0 36 | vth_samples: 40 37 | 38 | use_dwa: true 39 | restore_defaults: false 40 | -------------------------------------------------------------------------------- /momobot_ws/src/momobot/param/navigation_bak/base_local_planner_holonomic_params.yaml: -------------------------------------------------------------------------------- 1 | DWAPlannerROS: 2 | max_trans_vel: 0.50 3 | min_trans_vel: 0.01 4 | max_vel_x: 0.50 5 | min_vel_x: -0.025 6 | max_vel_y: 0.0 7 | min_vel_y: 0.0 8 | max_rot_vel: 0.30 9 | min_rot_vel: -0.30 10 | acc_lim_x: 1.25 11 | acc_lim_y: 0.0 12 | acc_lim_theta: 5 13 | acc_lim_trans: 1.25 14 | 15 | prune_plan: false 16 | 17 | xy_goal_tolerance: 0.25 18 | yaw_goal_tolerance: 0.1 19 | trans_stopped_vel: 0.1 20 | rot_stopped_vel: 0.1 21 | sim_time: 3.0 22 | sim_granularity: 0.1 23 | angular_sim_granularity: 0.1 24 | path_distance_bias: 34.0 25 | goal_distance_bias: 24.0 26 | occdist_scale: 0.05 27 | twirling_scale: 0.0 28 | stop_time_buffer: 0.5 29 | oscillation_reset_dist: 0.05 30 | oscillation_reset_angle: 0.2 31 | forward_point_distance: 0.3 32 | scaling_speed: 0.25 33 | max_scaling_factor: 0.2 34 | vx_samples: 20 35 | vy_samples: 0 36 | vth_samples: 40 37 | 38 | use_dwa: true 39 | restore_defaults: true 40 | -------------------------------------------------------------------------------- /momobot_ws/src/momobot/teensy/firmware/lib/ros_lib/std_msgs/Char.h: -------------------------------------------------------------------------------- 1 | #ifndef _ROS_std_msgs_Char_h 2 | #define _ROS_std_msgs_Char_h 3 | 4 | #include 5 | #include 6 | #include 7 | #include "ros/msg.h" 8 | 9 | namespace std_msgs 10 | { 11 | 12 | class Char : public ros::Msg 13 | { 14 | public: 15 | uint8_t data; 16 | 17 | Char(): 18 | data(0) 19 | { 20 | } 21 | 22 | virtual int serialize(unsigned char *outbuffer) const 23 | { 24 | int offset = 0; 25 | *(outbuffer + offset + 0) = (this->data >> (8 * 0)) & 0xFF; 26 | offset += sizeof(this->data); 27 | return offset; 28 | } 29 | 30 | virtual int deserialize(unsigned char *inbuffer) 31 | { 32 | int offset = 0; 33 | this->data = ((uint8_t) (*(inbuffer + offset))); 34 | offset += sizeof(this->data); 35 | return offset; 36 | } 37 | 38 | const char * getType(){ return "std_msgs/Char"; }; 39 | const char * getMD5(){ return "1bf77f25acecdedba0e224b162199717"; }; 40 | 41 | }; 42 | 43 | } 44 | #endif -------------------------------------------------------------------------------- /momobot_ws/src/momobot/teensy/firmware/lib/ros_lib/std_msgs/UInt8.h: -------------------------------------------------------------------------------- 1 | #ifndef _ROS_std_msgs_UInt8_h 2 | #define _ROS_std_msgs_UInt8_h 3 | 4 | #include 5 | #include 6 | #include 7 | #include "ros/msg.h" 8 | 9 | namespace std_msgs 10 | { 11 | 12 | class UInt8 : public ros::Msg 13 | { 14 | public: 15 | uint8_t data; 16 | 17 | UInt8(): 18 | data(0) 19 | { 20 | } 21 | 22 | virtual int serialize(unsigned char *outbuffer) const 23 | { 24 | int offset = 0; 25 | *(outbuffer + offset + 0) = (this->data >> (8 * 0)) & 0xFF; 26 | offset += sizeof(this->data); 27 | return offset; 28 | } 29 | 30 | virtual int deserialize(unsigned char *inbuffer) 31 | { 32 | int offset = 0; 33 | this->data = ((uint8_t) (*(inbuffer + offset))); 34 | offset += sizeof(this->data); 35 | return offset; 36 | } 37 | 38 | const char * getType(){ return "std_msgs/UInt8"; }; 39 | const char * getMD5(){ return "7c8164229e7d2c17eb95e9231617fdee"; }; 40 | 41 | }; 42 | 43 | } 44 | #endif -------------------------------------------------------------------------------- /momobot_ws/src/momobot/teensy_bakbak/firmware/lib/ros_lib/std_msgs/Char.h: -------------------------------------------------------------------------------- 1 | #ifndef _ROS_std_msgs_Char_h 2 | #define _ROS_std_msgs_Char_h 3 | 4 | #include 5 | #include 6 | #include 7 | #include "ros/msg.h" 8 | 9 | namespace std_msgs 10 | { 11 | 12 | class Char : public ros::Msg 13 | { 14 | public: 15 | uint8_t data; 16 | 17 | Char(): 18 | data(0) 19 | { 20 | } 21 | 22 | virtual int serialize(unsigned char *outbuffer) const 23 | { 24 | int offset = 0; 25 | *(outbuffer + offset + 0) = (this->data >> (8 * 0)) & 0xFF; 26 | offset += sizeof(this->data); 27 | return offset; 28 | } 29 | 30 | virtual int deserialize(unsigned char *inbuffer) 31 | { 32 | int offset = 0; 33 | this->data = ((uint8_t) (*(inbuffer + offset))); 34 | offset += sizeof(this->data); 35 | return offset; 36 | } 37 | 38 | const char * getType(){ return "std_msgs/Char"; }; 39 | const char * getMD5(){ return "1bf77f25acecdedba0e224b162199717"; }; 40 | 41 | }; 42 | 43 | } 44 | #endif -------------------------------------------------------------------------------- /momobot_ws/src/momobot/param/ekf/robot_localization.yaml: -------------------------------------------------------------------------------- 1 | frequency: 50 2 | 3 | two_d_mode: true 4 | diagnostics_agg: true 5 | 6 | #x , y , z, 7 | #roll , pitch , yaw, 8 | #vx , vy , vz, 9 | #vroll , vpitch, vyaw, 10 | #ax , ay , az 11 | 12 | odom0: /raw_odom 13 | odom0_config: [false, false, false, 14 | false, false, false, 15 | true, true, false, 16 | false, false, true, 17 | false, false, false] 18 | 19 | odom0_differential: true 20 | odom0_relative: false 21 | 22 | imu0: /imu/data 23 | 24 | # NOTE: If you find that your robot has x drift, 25 | # the most likely candidate is the x'' (acceleration) fr$ 26 | # Just set it to false! (It's the first entry on the las$ 27 | imu0_config: [false, false, false, 28 | false, false, true, 29 | false, false, false, 30 | false, false, true, 31 | false, false, false] 32 | 33 | imu0_differential: true 34 | imu0_relative: true 35 | 36 | odom_frame: odom 37 | base_link_frame: base_footprint 38 | world_frame: odom 39 | 40 | -------------------------------------------------------------------------------- /momobot_ws/src/momobot/teensy_bakbak/firmware/lib/ros_lib/std_msgs/UInt8.h: -------------------------------------------------------------------------------- 1 | #ifndef _ROS_std_msgs_UInt8_h 2 | #define _ROS_std_msgs_UInt8_h 3 | 4 | #include 5 | #include 6 | #include 7 | #include "ros/msg.h" 8 | 9 | namespace std_msgs 10 | { 11 | 12 | class UInt8 : public ros::Msg 13 | { 14 | public: 15 | uint8_t data; 16 | 17 | UInt8(): 18 | data(0) 19 | { 20 | } 21 | 22 | virtual int serialize(unsigned char *outbuffer) const 23 | { 24 | int offset = 0; 25 | *(outbuffer + offset + 0) = (this->data >> (8 * 0)) & 0xFF; 26 | offset += sizeof(this->data); 27 | return offset; 28 | } 29 | 30 | virtual int deserialize(unsigned char *inbuffer) 31 | { 32 | int offset = 0; 33 | this->data = ((uint8_t) (*(inbuffer + offset))); 34 | offset += sizeof(this->data); 35 | return offset; 36 | } 37 | 38 | const char * getType(){ return "std_msgs/UInt8"; }; 39 | const char * getMD5(){ return "7c8164229e7d2c17eb95e9231617fdee"; }; 40 | 41 | }; 42 | 43 | } 44 | #endif -------------------------------------------------------------------------------- /momobot_ws/src/momobot/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 2.8.3) 2 | project(momobot) 3 | 4 | ## Find catkin macros and libraries 5 | ## if COMPONENTS list like find_package(catkin REQUIRED COMPONENTS xyz) 6 | ## is used, also find other catkin packages 7 | find_package(catkin REQUIRED COMPONENTS 8 | roscpp 9 | rospy 10 | tf 11 | nav_msgs 12 | lino_msgs 13 | geometry_msgs 14 | sensor_msgs 15 | std_msgs 16 | ) 17 | 18 | catkin_package( 19 | INCLUDE_DIRS include 20 | LIBRARIES momobot 21 | CATKIN_DEPENDS roscpp rospy tf nav_msgs lino_msgs geometry_msgs sensor_msgs std_msgs 22 | DEPENDS system_lib 23 | ) 24 | ########### 25 | ## Build ## 26 | ########### 27 | 28 | include_directories( 29 | include 30 | ${catkin_INCLUDE_DIRS} 31 | ) 32 | 33 | 34 | add_library(momo_base src/momo_base.cpp) 35 | add_executable(momo_base_node src/momo_base_node.cpp) 36 | add_dependencies( 37 | momo_base_node 38 | ${catkin_EXPORTED_TARGETS} 39 | ${momo_base_node_EXPORTED_TARGETS} 40 | ) 41 | 42 | target_link_libraries(momo_base_node momo_base ${catkin_LIBRARIES}) 43 | -------------------------------------------------------------------------------- /momobot_ws/src/momobot/launch/include/lidar/ydlidar.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | 16 | 17 | 18 | -------------------------------------------------------------------------------- /momobot_ws/src/momobot/launch/minimal.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | 16 | 17 | 18 | 19 | 20 | 21 | 22 | 23 | -------------------------------------------------------------------------------- /momobot_ws/src/momobot/launch/include/imu/imu.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | 16 | 17 | 18 | 19 | 20 | 21 | 22 | -------------------------------------------------------------------------------- /momobot_ws/src/momobot/teensy/firmware/lib/encoder/utility/direct_pin_read.h: -------------------------------------------------------------------------------- 1 | #ifndef direct_pin_read_h_ 2 | #define direct_pin_read_h_ 3 | 4 | #if defined(__AVR__) || defined(__MK20DX128__) || defined(__MK20DX256__) || defined(__MK64FX512__) || defined(__MK66FX1M0__) || defined(__MKL26Z64__) 5 | 6 | #define IO_REG_TYPE uint8_t 7 | #define PIN_TO_BASEREG(pin) (portInputRegister(digitalPinToPort(pin))) 8 | #define PIN_TO_BITMASK(pin) (digitalPinToBitMask(pin)) 9 | #define DIRECT_PIN_READ(base, mask) (((*(base)) & (mask)) ? 1 : 0) 10 | 11 | #elif defined(__SAM3X8E__) 12 | 13 | #define IO_REG_TYPE uint32_t 14 | #define PIN_TO_BASEREG(pin) (portInputRegister(digitalPinToPort(pin))) 15 | #define PIN_TO_BITMASK(pin) (digitalPinToBitMask(pin)) 16 | #define DIRECT_PIN_READ(base, mask) (((*(base)) & (mask)) ? 1 : 0) 17 | 18 | #elif defined(__PIC32MX__) 19 | 20 | #define IO_REG_TYPE uint32_t 21 | #define PIN_TO_BASEREG(pin) (portModeRegister(digitalPinToPort(pin))) 22 | #define PIN_TO_BITMASK(pin) (digitalPinToBitMask(pin)) 23 | #define DIRECT_PIN_READ(base, mask) (((*(base+4)) & (mask)) ? 1 : 0) 24 | 25 | #endif 26 | 27 | #endif 28 | -------------------------------------------------------------------------------- /momobot_ws/src/momobot/teensy_bakbak/firmware/lib/encoder/utility/direct_pin_read.h: -------------------------------------------------------------------------------- 1 | #ifndef direct_pin_read_h_ 2 | #define direct_pin_read_h_ 3 | 4 | #if defined(__AVR__) || defined(__MK20DX128__) || defined(__MK20DX256__) || defined(__MK64FX512__) || defined(__MK66FX1M0__) || defined(__MKL26Z64__) 5 | 6 | #define IO_REG_TYPE uint8_t 7 | #define PIN_TO_BASEREG(pin) (portInputRegister(digitalPinToPort(pin))) 8 | #define PIN_TO_BITMASK(pin) (digitalPinToBitMask(pin)) 9 | #define DIRECT_PIN_READ(base, mask) (((*(base)) & (mask)) ? 1 : 0) 10 | 11 | #elif defined(__SAM3X8E__) 12 | 13 | #define IO_REG_TYPE uint32_t 14 | #define PIN_TO_BASEREG(pin) (portInputRegister(digitalPinToPort(pin))) 15 | #define PIN_TO_BITMASK(pin) (digitalPinToBitMask(pin)) 16 | #define DIRECT_PIN_READ(base, mask) (((*(base)) & (mask)) ? 1 : 0) 17 | 18 | #elif defined(__PIC32MX__) 19 | 20 | #define IO_REG_TYPE uint32_t 21 | #define PIN_TO_BASEREG(pin) (portModeRegister(digitalPinToPort(pin))) 22 | #define PIN_TO_BITMASK(pin) (digitalPinToBitMask(pin)) 23 | #define DIRECT_PIN_READ(base, mask) (((*(base+4)) & (mask)) ? 1 : 0) 24 | 25 | #endif 26 | 27 | #endif 28 | -------------------------------------------------------------------------------- /momobot_ws/src/momobot/teensy/firmware/lib/ros_lib/std_msgs/UInt16.h: -------------------------------------------------------------------------------- 1 | #ifndef _ROS_std_msgs_UInt16_h 2 | #define _ROS_std_msgs_UInt16_h 3 | 4 | #include 5 | #include 6 | #include 7 | #include "ros/msg.h" 8 | 9 | namespace std_msgs 10 | { 11 | 12 | class UInt16 : public ros::Msg 13 | { 14 | public: 15 | uint16_t data; 16 | 17 | UInt16(): 18 | data(0) 19 | { 20 | } 21 | 22 | virtual int serialize(unsigned char *outbuffer) const 23 | { 24 | int offset = 0; 25 | *(outbuffer + offset + 0) = (this->data >> (8 * 0)) & 0xFF; 26 | *(outbuffer + offset + 1) = (this->data >> (8 * 1)) & 0xFF; 27 | offset += sizeof(this->data); 28 | return offset; 29 | } 30 | 31 | virtual int deserialize(unsigned char *inbuffer) 32 | { 33 | int offset = 0; 34 | this->data = ((uint16_t) (*(inbuffer + offset))); 35 | this->data |= ((uint16_t) (*(inbuffer + offset + 1))) << (8 * 1); 36 | offset += sizeof(this->data); 37 | return offset; 38 | } 39 | 40 | const char * getType(){ return "std_msgs/UInt16"; }; 41 | const char * getMD5(){ return "1df79edf208b629fe6b81923a544552d"; }; 42 | 43 | }; 44 | 45 | } 46 | #endif -------------------------------------------------------------------------------- /momobot_ws/src/momobot/teensy_bakbak/firmware/lib/ros_lib/std_msgs/UInt16.h: -------------------------------------------------------------------------------- 1 | #ifndef _ROS_std_msgs_UInt16_h 2 | #define _ROS_std_msgs_UInt16_h 3 | 4 | #include 5 | #include 6 | #include 7 | #include "ros/msg.h" 8 | 9 | namespace std_msgs 10 | { 11 | 12 | class UInt16 : public ros::Msg 13 | { 14 | public: 15 | uint16_t data; 16 | 17 | UInt16(): 18 | data(0) 19 | { 20 | } 21 | 22 | virtual int serialize(unsigned char *outbuffer) const 23 | { 24 | int offset = 0; 25 | *(outbuffer + offset + 0) = (this->data >> (8 * 0)) & 0xFF; 26 | *(outbuffer + offset + 1) = (this->data >> (8 * 1)) & 0xFF; 27 | offset += sizeof(this->data); 28 | return offset; 29 | } 30 | 31 | virtual int deserialize(unsigned char *inbuffer) 32 | { 33 | int offset = 0; 34 | this->data = ((uint16_t) (*(inbuffer + offset))); 35 | this->data |= ((uint16_t) (*(inbuffer + offset + 1))) << (8 * 1); 36 | offset += sizeof(this->data); 37 | return offset; 38 | } 39 | 40 | const char * getType(){ return "std_msgs/UInt16"; }; 41 | const char * getMD5(){ return "1df79edf208b629fe6b81923a544552d"; }; 42 | 43 | }; 44 | 45 | } 46 | #endif -------------------------------------------------------------------------------- /momobot_ws/src/momobot/teensy/firmware/lib/ros_lib/geometry_msgs/Wrench.h: -------------------------------------------------------------------------------- 1 | #ifndef _ROS_geometry_msgs_Wrench_h 2 | #define _ROS_geometry_msgs_Wrench_h 3 | 4 | #include 5 | #include 6 | #include 7 | #include "ros/msg.h" 8 | #include "geometry_msgs/Vector3.h" 9 | 10 | namespace geometry_msgs 11 | { 12 | 13 | class Wrench : public ros::Msg 14 | { 15 | public: 16 | geometry_msgs::Vector3 force; 17 | geometry_msgs::Vector3 torque; 18 | 19 | Wrench(): 20 | force(), 21 | torque() 22 | { 23 | } 24 | 25 | virtual int serialize(unsigned char *outbuffer) const 26 | { 27 | int offset = 0; 28 | offset += this->force.serialize(outbuffer + offset); 29 | offset += this->torque.serialize(outbuffer + offset); 30 | return offset; 31 | } 32 | 33 | virtual int deserialize(unsigned char *inbuffer) 34 | { 35 | int offset = 0; 36 | offset += this->force.deserialize(inbuffer + offset); 37 | offset += this->torque.deserialize(inbuffer + offset); 38 | return offset; 39 | } 40 | 41 | const char * getType(){ return "geometry_msgs/Wrench"; }; 42 | const char * getMD5(){ return "4f539cf138b23283b520fd271b567936"; }; 43 | 44 | }; 45 | 46 | } 47 | #endif -------------------------------------------------------------------------------- /momobot_ws/src/momobot/teensy/firmware/lib/ros_lib/geometry_msgs/Accel.h: -------------------------------------------------------------------------------- 1 | #ifndef _ROS_geometry_msgs_Accel_h 2 | #define _ROS_geometry_msgs_Accel_h 3 | 4 | #include 5 | #include 6 | #include 7 | #include "ros/msg.h" 8 | #include "geometry_msgs/Vector3.h" 9 | 10 | namespace geometry_msgs 11 | { 12 | 13 | class Accel : public ros::Msg 14 | { 15 | public: 16 | geometry_msgs::Vector3 linear; 17 | geometry_msgs::Vector3 angular; 18 | 19 | Accel(): 20 | linear(), 21 | angular() 22 | { 23 | } 24 | 25 | virtual int serialize(unsigned char *outbuffer) const 26 | { 27 | int offset = 0; 28 | offset += this->linear.serialize(outbuffer + offset); 29 | offset += this->angular.serialize(outbuffer + offset); 30 | return offset; 31 | } 32 | 33 | virtual int deserialize(unsigned char *inbuffer) 34 | { 35 | int offset = 0; 36 | offset += this->linear.deserialize(inbuffer + offset); 37 | offset += this->angular.deserialize(inbuffer + offset); 38 | return offset; 39 | } 40 | 41 | const char * getType(){ return "geometry_msgs/Accel"; }; 42 | const char * getMD5(){ return "9f195f881246fdfa2798d1d3eebca84a"; }; 43 | 44 | }; 45 | 46 | } 47 | #endif -------------------------------------------------------------------------------- /momobot_ws/src/momobot/teensy/firmware/lib/ros_lib/geometry_msgs/Twist.h: -------------------------------------------------------------------------------- 1 | #ifndef _ROS_geometry_msgs_Twist_h 2 | #define _ROS_geometry_msgs_Twist_h 3 | 4 | #include 5 | #include 6 | #include 7 | #include "ros/msg.h" 8 | #include "geometry_msgs/Vector3.h" 9 | 10 | namespace geometry_msgs 11 | { 12 | 13 | class Twist : public ros::Msg 14 | { 15 | public: 16 | geometry_msgs::Vector3 linear; 17 | geometry_msgs::Vector3 angular; 18 | 19 | Twist(): 20 | linear(), 21 | angular() 22 | { 23 | } 24 | 25 | virtual int serialize(unsigned char *outbuffer) const 26 | { 27 | int offset = 0; 28 | offset += this->linear.serialize(outbuffer + offset); 29 | offset += this->angular.serialize(outbuffer + offset); 30 | return offset; 31 | } 32 | 33 | virtual int deserialize(unsigned char *inbuffer) 34 | { 35 | int offset = 0; 36 | offset += this->linear.deserialize(inbuffer + offset); 37 | offset += this->angular.deserialize(inbuffer + offset); 38 | return offset; 39 | } 40 | 41 | const char * getType(){ return "geometry_msgs/Twist"; }; 42 | const char * getMD5(){ return "9f195f881246fdfa2798d1d3eebca84a"; }; 43 | 44 | }; 45 | 46 | } 47 | #endif -------------------------------------------------------------------------------- /momobot_ws/src/momobot/teensy_bakbak/firmware/lib/ros_lib/geometry_msgs/Accel.h: -------------------------------------------------------------------------------- 1 | #ifndef _ROS_geometry_msgs_Accel_h 2 | #define _ROS_geometry_msgs_Accel_h 3 | 4 | #include 5 | #include 6 | #include 7 | #include "ros/msg.h" 8 | #include "geometry_msgs/Vector3.h" 9 | 10 | namespace geometry_msgs 11 | { 12 | 13 | class Accel : public ros::Msg 14 | { 15 | public: 16 | geometry_msgs::Vector3 linear; 17 | geometry_msgs::Vector3 angular; 18 | 19 | Accel(): 20 | linear(), 21 | angular() 22 | { 23 | } 24 | 25 | virtual int serialize(unsigned char *outbuffer) const 26 | { 27 | int offset = 0; 28 | offset += this->linear.serialize(outbuffer + offset); 29 | offset += this->angular.serialize(outbuffer + offset); 30 | return offset; 31 | } 32 | 33 | virtual int deserialize(unsigned char *inbuffer) 34 | { 35 | int offset = 0; 36 | offset += this->linear.deserialize(inbuffer + offset); 37 | offset += this->angular.deserialize(inbuffer + offset); 38 | return offset; 39 | } 40 | 41 | const char * getType(){ return "geometry_msgs/Accel"; }; 42 | const char * getMD5(){ return "9f195f881246fdfa2798d1d3eebca84a"; }; 43 | 44 | }; 45 | 46 | } 47 | #endif -------------------------------------------------------------------------------- /momobot_ws/src/momobot/teensy_bakbak/firmware/lib/ros_lib/geometry_msgs/Twist.h: -------------------------------------------------------------------------------- 1 | #ifndef _ROS_geometry_msgs_Twist_h 2 | #define _ROS_geometry_msgs_Twist_h 3 | 4 | #include 5 | #include 6 | #include 7 | #include "ros/msg.h" 8 | #include "geometry_msgs/Vector3.h" 9 | 10 | namespace geometry_msgs 11 | { 12 | 13 | class Twist : public ros::Msg 14 | { 15 | public: 16 | geometry_msgs::Vector3 linear; 17 | geometry_msgs::Vector3 angular; 18 | 19 | Twist(): 20 | linear(), 21 | angular() 22 | { 23 | } 24 | 25 | virtual int serialize(unsigned char *outbuffer) const 26 | { 27 | int offset = 0; 28 | offset += this->linear.serialize(outbuffer + offset); 29 | offset += this->angular.serialize(outbuffer + offset); 30 | return offset; 31 | } 32 | 33 | virtual int deserialize(unsigned char *inbuffer) 34 | { 35 | int offset = 0; 36 | offset += this->linear.deserialize(inbuffer + offset); 37 | offset += this->angular.deserialize(inbuffer + offset); 38 | return offset; 39 | } 40 | 41 | const char * getType(){ return "geometry_msgs/Twist"; }; 42 | const char * getMD5(){ return "9f195f881246fdfa2798d1d3eebca84a"; }; 43 | 44 | }; 45 | 46 | } 47 | #endif -------------------------------------------------------------------------------- /momobot_ws/src/momobot/teensy_bakbak/firmware/lib/ros_lib/geometry_msgs/Wrench.h: -------------------------------------------------------------------------------- 1 | #ifndef _ROS_geometry_msgs_Wrench_h 2 | #define _ROS_geometry_msgs_Wrench_h 3 | 4 | #include 5 | #include 6 | #include 7 | #include "ros/msg.h" 8 | #include "geometry_msgs/Vector3.h" 9 | 10 | namespace geometry_msgs 11 | { 12 | 13 | class Wrench : public ros::Msg 14 | { 15 | public: 16 | geometry_msgs::Vector3 force; 17 | geometry_msgs::Vector3 torque; 18 | 19 | Wrench(): 20 | force(), 21 | torque() 22 | { 23 | } 24 | 25 | virtual int serialize(unsigned char *outbuffer) const 26 | { 27 | int offset = 0; 28 | offset += this->force.serialize(outbuffer + offset); 29 | offset += this->torque.serialize(outbuffer + offset); 30 | return offset; 31 | } 32 | 33 | virtual int deserialize(unsigned char *inbuffer) 34 | { 35 | int offset = 0; 36 | offset += this->force.deserialize(inbuffer + offset); 37 | offset += this->torque.deserialize(inbuffer + offset); 38 | return offset; 39 | } 40 | 41 | const char * getType(){ return "geometry_msgs/Wrench"; }; 42 | const char * getMD5(){ return "4f539cf138b23283b520fd271b567936"; }; 43 | 44 | }; 45 | 46 | } 47 | #endif -------------------------------------------------------------------------------- /momobot_ws/src/momobot/launch/bringup.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | 16 | 17 | 18 | 19 | 20 | 21 | 22 | 23 | 24 | 25 | 26 | -------------------------------------------------------------------------------- /momobot_ws/src/momobot/teensy/firmware/lib/ros_lib/geometry_msgs/PoseStamped.h: -------------------------------------------------------------------------------- 1 | #ifndef _ROS_geometry_msgs_PoseStamped_h 2 | #define _ROS_geometry_msgs_PoseStamped_h 3 | 4 | #include 5 | #include 6 | #include 7 | #include "ros/msg.h" 8 | #include "std_msgs/Header.h" 9 | #include "geometry_msgs/Pose.h" 10 | 11 | namespace geometry_msgs 12 | { 13 | 14 | class PoseStamped : public ros::Msg 15 | { 16 | public: 17 | std_msgs::Header header; 18 | geometry_msgs::Pose pose; 19 | 20 | PoseStamped(): 21 | header(), 22 | pose() 23 | { 24 | } 25 | 26 | virtual int serialize(unsigned char *outbuffer) const 27 | { 28 | int offset = 0; 29 | offset += this->header.serialize(outbuffer + offset); 30 | offset += this->pose.serialize(outbuffer + offset); 31 | return offset; 32 | } 33 | 34 | virtual int deserialize(unsigned char *inbuffer) 35 | { 36 | int offset = 0; 37 | offset += this->header.deserialize(inbuffer + offset); 38 | offset += this->pose.deserialize(inbuffer + offset); 39 | return offset; 40 | } 41 | 42 | const char * getType(){ return "geometry_msgs/PoseStamped"; }; 43 | const char * getMD5(){ return "d3812c3cbc69362b77dc0b19b345f8f5"; }; 44 | 45 | }; 46 | 47 | } 48 | #endif -------------------------------------------------------------------------------- /momobot_ws/src/momobot/teensy_bakbak/firmware/lib/ros_lib/geometry_msgs/PoseStamped.h: -------------------------------------------------------------------------------- 1 | #ifndef _ROS_geometry_msgs_PoseStamped_h 2 | #define _ROS_geometry_msgs_PoseStamped_h 3 | 4 | #include 5 | #include 6 | #include 7 | #include "ros/msg.h" 8 | #include "std_msgs/Header.h" 9 | #include "geometry_msgs/Pose.h" 10 | 11 | namespace geometry_msgs 12 | { 13 | 14 | class PoseStamped : public ros::Msg 15 | { 16 | public: 17 | std_msgs::Header header; 18 | geometry_msgs::Pose pose; 19 | 20 | PoseStamped(): 21 | header(), 22 | pose() 23 | { 24 | } 25 | 26 | virtual int serialize(unsigned char *outbuffer) const 27 | { 28 | int offset = 0; 29 | offset += this->header.serialize(outbuffer + offset); 30 | offset += this->pose.serialize(outbuffer + offset); 31 | return offset; 32 | } 33 | 34 | virtual int deserialize(unsigned char *inbuffer) 35 | { 36 | int offset = 0; 37 | offset += this->header.deserialize(inbuffer + offset); 38 | offset += this->pose.deserialize(inbuffer + offset); 39 | return offset; 40 | } 41 | 42 | const char * getType(){ return "geometry_msgs/PoseStamped"; }; 43 | const char * getMD5(){ return "d3812c3cbc69362b77dc0b19b345f8f5"; }; 44 | 45 | }; 46 | 47 | } 48 | #endif -------------------------------------------------------------------------------- /momobot_ws/src/momobot/teensy/firmware/lib/ros_lib/geometry_msgs/AccelStamped.h: -------------------------------------------------------------------------------- 1 | #ifndef _ROS_geometry_msgs_AccelStamped_h 2 | #define _ROS_geometry_msgs_AccelStamped_h 3 | 4 | #include 5 | #include 6 | #include 7 | #include "ros/msg.h" 8 | #include "std_msgs/Header.h" 9 | #include "geometry_msgs/Accel.h" 10 | 11 | namespace geometry_msgs 12 | { 13 | 14 | class AccelStamped : public ros::Msg 15 | { 16 | public: 17 | std_msgs::Header header; 18 | geometry_msgs::Accel accel; 19 | 20 | AccelStamped(): 21 | header(), 22 | accel() 23 | { 24 | } 25 | 26 | virtual int serialize(unsigned char *outbuffer) const 27 | { 28 | int offset = 0; 29 | offset += this->header.serialize(outbuffer + offset); 30 | offset += this->accel.serialize(outbuffer + offset); 31 | return offset; 32 | } 33 | 34 | virtual int deserialize(unsigned char *inbuffer) 35 | { 36 | int offset = 0; 37 | offset += this->header.deserialize(inbuffer + offset); 38 | offset += this->accel.deserialize(inbuffer + offset); 39 | return offset; 40 | } 41 | 42 | const char * getType(){ return "geometry_msgs/AccelStamped"; }; 43 | const char * getMD5(){ return "d8a98a5d81351b6eb0578c78557e7659"; }; 44 | 45 | }; 46 | 47 | } 48 | #endif -------------------------------------------------------------------------------- /momobot_ws/src/momobot/teensy/firmware/lib/ros_lib/geometry_msgs/PointStamped.h: -------------------------------------------------------------------------------- 1 | #ifndef _ROS_geometry_msgs_PointStamped_h 2 | #define _ROS_geometry_msgs_PointStamped_h 3 | 4 | #include 5 | #include 6 | #include 7 | #include "ros/msg.h" 8 | #include "std_msgs/Header.h" 9 | #include "geometry_msgs/Point.h" 10 | 11 | namespace geometry_msgs 12 | { 13 | 14 | class PointStamped : public ros::Msg 15 | { 16 | public: 17 | std_msgs::Header header; 18 | geometry_msgs::Point point; 19 | 20 | PointStamped(): 21 | header(), 22 | point() 23 | { 24 | } 25 | 26 | virtual int serialize(unsigned char *outbuffer) const 27 | { 28 | int offset = 0; 29 | offset += this->header.serialize(outbuffer + offset); 30 | offset += this->point.serialize(outbuffer + offset); 31 | return offset; 32 | } 33 | 34 | virtual int deserialize(unsigned char *inbuffer) 35 | { 36 | int offset = 0; 37 | offset += this->header.deserialize(inbuffer + offset); 38 | offset += this->point.deserialize(inbuffer + offset); 39 | return offset; 40 | } 41 | 42 | const char * getType(){ return "geometry_msgs/PointStamped"; }; 43 | const char * getMD5(){ return "c63aecb41bfdfd6b7e1fac37c7cbe7bf"; }; 44 | 45 | }; 46 | 47 | } 48 | #endif -------------------------------------------------------------------------------- /momobot_ws/src/momobot/teensy/firmware/lib/ros_lib/geometry_msgs/TwistStamped.h: -------------------------------------------------------------------------------- 1 | #ifndef _ROS_geometry_msgs_TwistStamped_h 2 | #define _ROS_geometry_msgs_TwistStamped_h 3 | 4 | #include 5 | #include 6 | #include 7 | #include "ros/msg.h" 8 | #include "std_msgs/Header.h" 9 | #include "geometry_msgs/Twist.h" 10 | 11 | namespace geometry_msgs 12 | { 13 | 14 | class TwistStamped : public ros::Msg 15 | { 16 | public: 17 | std_msgs::Header header; 18 | geometry_msgs::Twist twist; 19 | 20 | TwistStamped(): 21 | header(), 22 | twist() 23 | { 24 | } 25 | 26 | virtual int serialize(unsigned char *outbuffer) const 27 | { 28 | int offset = 0; 29 | offset += this->header.serialize(outbuffer + offset); 30 | offset += this->twist.serialize(outbuffer + offset); 31 | return offset; 32 | } 33 | 34 | virtual int deserialize(unsigned char *inbuffer) 35 | { 36 | int offset = 0; 37 | offset += this->header.deserialize(inbuffer + offset); 38 | offset += this->twist.deserialize(inbuffer + offset); 39 | return offset; 40 | } 41 | 42 | const char * getType(){ return "geometry_msgs/TwistStamped"; }; 43 | const char * getMD5(){ return "98d34b0043a2093cf9d9345ab6eef12e"; }; 44 | 45 | }; 46 | 47 | } 48 | #endif -------------------------------------------------------------------------------- /momobot_ws/src/momobot/teensy/firmware/lib/ros_lib/geometry_msgs/Pose.h: -------------------------------------------------------------------------------- 1 | #ifndef _ROS_geometry_msgs_Pose_h 2 | #define _ROS_geometry_msgs_Pose_h 3 | 4 | #include 5 | #include 6 | #include 7 | #include "ros/msg.h" 8 | #include "geometry_msgs/Point.h" 9 | #include "geometry_msgs/Quaternion.h" 10 | 11 | namespace geometry_msgs 12 | { 13 | 14 | class Pose : public ros::Msg 15 | { 16 | public: 17 | geometry_msgs::Point position; 18 | geometry_msgs::Quaternion orientation; 19 | 20 | Pose(): 21 | position(), 22 | orientation() 23 | { 24 | } 25 | 26 | virtual int serialize(unsigned char *outbuffer) const 27 | { 28 | int offset = 0; 29 | offset += this->position.serialize(outbuffer + offset); 30 | offset += this->orientation.serialize(outbuffer + offset); 31 | return offset; 32 | } 33 | 34 | virtual int deserialize(unsigned char *inbuffer) 35 | { 36 | int offset = 0; 37 | offset += this->position.deserialize(inbuffer + offset); 38 | offset += this->orientation.deserialize(inbuffer + offset); 39 | return offset; 40 | } 41 | 42 | const char * getType(){ return "geometry_msgs/Pose"; }; 43 | const char * getMD5(){ return "e45d45a5a1ce597b249e23fb30fc871f"; }; 44 | 45 | }; 46 | 47 | } 48 | #endif -------------------------------------------------------------------------------- /momobot_ws/src/momobot/teensy_bakbak/firmware/lib/ros_lib/geometry_msgs/AccelStamped.h: -------------------------------------------------------------------------------- 1 | #ifndef _ROS_geometry_msgs_AccelStamped_h 2 | #define _ROS_geometry_msgs_AccelStamped_h 3 | 4 | #include 5 | #include 6 | #include 7 | #include "ros/msg.h" 8 | #include "std_msgs/Header.h" 9 | #include "geometry_msgs/Accel.h" 10 | 11 | namespace geometry_msgs 12 | { 13 | 14 | class AccelStamped : public ros::Msg 15 | { 16 | public: 17 | std_msgs::Header header; 18 | geometry_msgs::Accel accel; 19 | 20 | AccelStamped(): 21 | header(), 22 | accel() 23 | { 24 | } 25 | 26 | virtual int serialize(unsigned char *outbuffer) const 27 | { 28 | int offset = 0; 29 | offset += this->header.serialize(outbuffer + offset); 30 | offset += this->accel.serialize(outbuffer + offset); 31 | return offset; 32 | } 33 | 34 | virtual int deserialize(unsigned char *inbuffer) 35 | { 36 | int offset = 0; 37 | offset += this->header.deserialize(inbuffer + offset); 38 | offset += this->accel.deserialize(inbuffer + offset); 39 | return offset; 40 | } 41 | 42 | const char * getType(){ return "geometry_msgs/AccelStamped"; }; 43 | const char * getMD5(){ return "d8a98a5d81351b6eb0578c78557e7659"; }; 44 | 45 | }; 46 | 47 | } 48 | #endif -------------------------------------------------------------------------------- /momobot_ws/src/momobot/teensy_bakbak/firmware/lib/ros_lib/geometry_msgs/PointStamped.h: -------------------------------------------------------------------------------- 1 | #ifndef _ROS_geometry_msgs_PointStamped_h 2 | #define _ROS_geometry_msgs_PointStamped_h 3 | 4 | #include 5 | #include 6 | #include 7 | #include "ros/msg.h" 8 | #include "std_msgs/Header.h" 9 | #include "geometry_msgs/Point.h" 10 | 11 | namespace geometry_msgs 12 | { 13 | 14 | class PointStamped : public ros::Msg 15 | { 16 | public: 17 | std_msgs::Header header; 18 | geometry_msgs::Point point; 19 | 20 | PointStamped(): 21 | header(), 22 | point() 23 | { 24 | } 25 | 26 | virtual int serialize(unsigned char *outbuffer) const 27 | { 28 | int offset = 0; 29 | offset += this->header.serialize(outbuffer + offset); 30 | offset += this->point.serialize(outbuffer + offset); 31 | return offset; 32 | } 33 | 34 | virtual int deserialize(unsigned char *inbuffer) 35 | { 36 | int offset = 0; 37 | offset += this->header.deserialize(inbuffer + offset); 38 | offset += this->point.deserialize(inbuffer + offset); 39 | return offset; 40 | } 41 | 42 | const char * getType(){ return "geometry_msgs/PointStamped"; }; 43 | const char * getMD5(){ return "c63aecb41bfdfd6b7e1fac37c7cbe7bf"; }; 44 | 45 | }; 46 | 47 | } 48 | #endif -------------------------------------------------------------------------------- /momobot_ws/src/momobot/teensy_bakbak/firmware/lib/ros_lib/geometry_msgs/Pose.h: -------------------------------------------------------------------------------- 1 | #ifndef _ROS_geometry_msgs_Pose_h 2 | #define _ROS_geometry_msgs_Pose_h 3 | 4 | #include 5 | #include 6 | #include 7 | #include "ros/msg.h" 8 | #include "geometry_msgs/Point.h" 9 | #include "geometry_msgs/Quaternion.h" 10 | 11 | namespace geometry_msgs 12 | { 13 | 14 | class Pose : public ros::Msg 15 | { 16 | public: 17 | geometry_msgs::Point position; 18 | geometry_msgs::Quaternion orientation; 19 | 20 | Pose(): 21 | position(), 22 | orientation() 23 | { 24 | } 25 | 26 | virtual int serialize(unsigned char *outbuffer) const 27 | { 28 | int offset = 0; 29 | offset += this->position.serialize(outbuffer + offset); 30 | offset += this->orientation.serialize(outbuffer + offset); 31 | return offset; 32 | } 33 | 34 | virtual int deserialize(unsigned char *inbuffer) 35 | { 36 | int offset = 0; 37 | offset += this->position.deserialize(inbuffer + offset); 38 | offset += this->orientation.deserialize(inbuffer + offset); 39 | return offset; 40 | } 41 | 42 | const char * getType(){ return "geometry_msgs/Pose"; }; 43 | const char * getMD5(){ return "e45d45a5a1ce597b249e23fb30fc871f"; }; 44 | 45 | }; 46 | 47 | } 48 | #endif -------------------------------------------------------------------------------- /momobot_ws/src/momobot/teensy_bakbak/firmware/lib/ros_lib/geometry_msgs/TwistStamped.h: -------------------------------------------------------------------------------- 1 | #ifndef _ROS_geometry_msgs_TwistStamped_h 2 | #define _ROS_geometry_msgs_TwistStamped_h 3 | 4 | #include 5 | #include 6 | #include 7 | #include "ros/msg.h" 8 | #include "std_msgs/Header.h" 9 | #include "geometry_msgs/Twist.h" 10 | 11 | namespace geometry_msgs 12 | { 13 | 14 | class TwistStamped : public ros::Msg 15 | { 16 | public: 17 | std_msgs::Header header; 18 | geometry_msgs::Twist twist; 19 | 20 | TwistStamped(): 21 | header(), 22 | twist() 23 | { 24 | } 25 | 26 | virtual int serialize(unsigned char *outbuffer) const 27 | { 28 | int offset = 0; 29 | offset += this->header.serialize(outbuffer + offset); 30 | offset += this->twist.serialize(outbuffer + offset); 31 | return offset; 32 | } 33 | 34 | virtual int deserialize(unsigned char *inbuffer) 35 | { 36 | int offset = 0; 37 | offset += this->header.deserialize(inbuffer + offset); 38 | offset += this->twist.deserialize(inbuffer + offset); 39 | return offset; 40 | } 41 | 42 | const char * getType(){ return "geometry_msgs/TwistStamped"; }; 43 | const char * getMD5(){ return "98d34b0043a2093cf9d9345ab6eef12e"; }; 44 | 45 | }; 46 | 47 | } 48 | #endif -------------------------------------------------------------------------------- /momobot_ws/src/momobot/teensy/firmware/lib/ros_lib/geometry_msgs/WrenchStamped.h: -------------------------------------------------------------------------------- 1 | #ifndef _ROS_geometry_msgs_WrenchStamped_h 2 | #define _ROS_geometry_msgs_WrenchStamped_h 3 | 4 | #include 5 | #include 6 | #include 7 | #include "ros/msg.h" 8 | #include "std_msgs/Header.h" 9 | #include "geometry_msgs/Wrench.h" 10 | 11 | namespace geometry_msgs 12 | { 13 | 14 | class WrenchStamped : public ros::Msg 15 | { 16 | public: 17 | std_msgs::Header header; 18 | geometry_msgs::Wrench wrench; 19 | 20 | WrenchStamped(): 21 | header(), 22 | wrench() 23 | { 24 | } 25 | 26 | virtual int serialize(unsigned char *outbuffer) const 27 | { 28 | int offset = 0; 29 | offset += this->header.serialize(outbuffer + offset); 30 | offset += this->wrench.serialize(outbuffer + offset); 31 | return offset; 32 | } 33 | 34 | virtual int deserialize(unsigned char *inbuffer) 35 | { 36 | int offset = 0; 37 | offset += this->header.deserialize(inbuffer + offset); 38 | offset += this->wrench.deserialize(inbuffer + offset); 39 | return offset; 40 | } 41 | 42 | const char * getType(){ return "geometry_msgs/WrenchStamped"; }; 43 | const char * getMD5(){ return "d78d3cb249ce23087ade7e7d0c40cfa7"; }; 44 | 45 | }; 46 | 47 | } 48 | #endif -------------------------------------------------------------------------------- /momobot_ws/src/momobot/teensy/firmware/lib/ros_lib/geometry_msgs/Vector3Stamped.h: -------------------------------------------------------------------------------- 1 | #ifndef _ROS_geometry_msgs_Vector3Stamped_h 2 | #define _ROS_geometry_msgs_Vector3Stamped_h 3 | 4 | #include 5 | #include 6 | #include 7 | #include "ros/msg.h" 8 | #include "std_msgs/Header.h" 9 | #include "geometry_msgs/Vector3.h" 10 | 11 | namespace geometry_msgs 12 | { 13 | 14 | class Vector3Stamped : public ros::Msg 15 | { 16 | public: 17 | std_msgs::Header header; 18 | geometry_msgs::Vector3 vector; 19 | 20 | Vector3Stamped(): 21 | header(), 22 | vector() 23 | { 24 | } 25 | 26 | virtual int serialize(unsigned char *outbuffer) const 27 | { 28 | int offset = 0; 29 | offset += this->header.serialize(outbuffer + offset); 30 | offset += this->vector.serialize(outbuffer + offset); 31 | return offset; 32 | } 33 | 34 | virtual int deserialize(unsigned char *inbuffer) 35 | { 36 | int offset = 0; 37 | offset += this->header.deserialize(inbuffer + offset); 38 | offset += this->vector.deserialize(inbuffer + offset); 39 | return offset; 40 | } 41 | 42 | const char * getType(){ return "geometry_msgs/Vector3Stamped"; }; 43 | const char * getMD5(){ return "7b324c7325e683bf02a9b14b01090ec7"; }; 44 | 45 | }; 46 | 47 | } 48 | #endif -------------------------------------------------------------------------------- /momobot_ws/src/momobot/teensy_bakbak/firmware/lib/ros_lib/geometry_msgs/WrenchStamped.h: -------------------------------------------------------------------------------- 1 | #ifndef _ROS_geometry_msgs_WrenchStamped_h 2 | #define _ROS_geometry_msgs_WrenchStamped_h 3 | 4 | #include 5 | #include 6 | #include 7 | #include "ros/msg.h" 8 | #include "std_msgs/Header.h" 9 | #include "geometry_msgs/Wrench.h" 10 | 11 | namespace geometry_msgs 12 | { 13 | 14 | class WrenchStamped : public ros::Msg 15 | { 16 | public: 17 | std_msgs::Header header; 18 | geometry_msgs::Wrench wrench; 19 | 20 | WrenchStamped(): 21 | header(), 22 | wrench() 23 | { 24 | } 25 | 26 | virtual int serialize(unsigned char *outbuffer) const 27 | { 28 | int offset = 0; 29 | offset += this->header.serialize(outbuffer + offset); 30 | offset += this->wrench.serialize(outbuffer + offset); 31 | return offset; 32 | } 33 | 34 | virtual int deserialize(unsigned char *inbuffer) 35 | { 36 | int offset = 0; 37 | offset += this->header.deserialize(inbuffer + offset); 38 | offset += this->wrench.deserialize(inbuffer + offset); 39 | return offset; 40 | } 41 | 42 | const char * getType(){ return "geometry_msgs/WrenchStamped"; }; 43 | const char * getMD5(){ return "d78d3cb249ce23087ade7e7d0c40cfa7"; }; 44 | 45 | }; 46 | 47 | } 48 | #endif -------------------------------------------------------------------------------- /momobot_ws/src/momobot/teensy/firmware/lib/ros_lib/geometry_msgs/InertiaStamped.h: -------------------------------------------------------------------------------- 1 | #ifndef _ROS_geometry_msgs_InertiaStamped_h 2 | #define _ROS_geometry_msgs_InertiaStamped_h 3 | 4 | #include 5 | #include 6 | #include 7 | #include "ros/msg.h" 8 | #include "std_msgs/Header.h" 9 | #include "geometry_msgs/Inertia.h" 10 | 11 | namespace geometry_msgs 12 | { 13 | 14 | class InertiaStamped : public ros::Msg 15 | { 16 | public: 17 | std_msgs::Header header; 18 | geometry_msgs::Inertia inertia; 19 | 20 | InertiaStamped(): 21 | header(), 22 | inertia() 23 | { 24 | } 25 | 26 | virtual int serialize(unsigned char *outbuffer) const 27 | { 28 | int offset = 0; 29 | offset += this->header.serialize(outbuffer + offset); 30 | offset += this->inertia.serialize(outbuffer + offset); 31 | return offset; 32 | } 33 | 34 | virtual int deserialize(unsigned char *inbuffer) 35 | { 36 | int offset = 0; 37 | offset += this->header.deserialize(inbuffer + offset); 38 | offset += this->inertia.deserialize(inbuffer + offset); 39 | return offset; 40 | } 41 | 42 | const char * getType(){ return "geometry_msgs/InertiaStamped"; }; 43 | const char * getMD5(){ return "ddee48caeab5a966c5e8d166654a9ac7"; }; 44 | 45 | }; 46 | 47 | } 48 | #endif -------------------------------------------------------------------------------- /momobot_ws/src/momobot/teensy/firmware/lib/ros_lib/geometry_msgs/PolygonStamped.h: -------------------------------------------------------------------------------- 1 | #ifndef _ROS_geometry_msgs_PolygonStamped_h 2 | #define _ROS_geometry_msgs_PolygonStamped_h 3 | 4 | #include 5 | #include 6 | #include 7 | #include "ros/msg.h" 8 | #include "std_msgs/Header.h" 9 | #include "geometry_msgs/Polygon.h" 10 | 11 | namespace geometry_msgs 12 | { 13 | 14 | class PolygonStamped : public ros::Msg 15 | { 16 | public: 17 | std_msgs::Header header; 18 | geometry_msgs::Polygon polygon; 19 | 20 | PolygonStamped(): 21 | header(), 22 | polygon() 23 | { 24 | } 25 | 26 | virtual int serialize(unsigned char *outbuffer) const 27 | { 28 | int offset = 0; 29 | offset += this->header.serialize(outbuffer + offset); 30 | offset += this->polygon.serialize(outbuffer + offset); 31 | return offset; 32 | } 33 | 34 | virtual int deserialize(unsigned char *inbuffer) 35 | { 36 | int offset = 0; 37 | offset += this->header.deserialize(inbuffer + offset); 38 | offset += this->polygon.deserialize(inbuffer + offset); 39 | return offset; 40 | } 41 | 42 | const char * getType(){ return "geometry_msgs/PolygonStamped"; }; 43 | const char * getMD5(){ return "c6be8f7dc3bee7fe9e8d296070f53340"; }; 44 | 45 | }; 46 | 47 | } 48 | #endif -------------------------------------------------------------------------------- /momobot_ws/src/momobot/teensy_bakbak/firmware/lib/ros_lib/geometry_msgs/Vector3Stamped.h: -------------------------------------------------------------------------------- 1 | #ifndef _ROS_geometry_msgs_Vector3Stamped_h 2 | #define _ROS_geometry_msgs_Vector3Stamped_h 3 | 4 | #include 5 | #include 6 | #include 7 | #include "ros/msg.h" 8 | #include "std_msgs/Header.h" 9 | #include "geometry_msgs/Vector3.h" 10 | 11 | namespace geometry_msgs 12 | { 13 | 14 | class Vector3Stamped : public ros::Msg 15 | { 16 | public: 17 | std_msgs::Header header; 18 | geometry_msgs::Vector3 vector; 19 | 20 | Vector3Stamped(): 21 | header(), 22 | vector() 23 | { 24 | } 25 | 26 | virtual int serialize(unsigned char *outbuffer) const 27 | { 28 | int offset = 0; 29 | offset += this->header.serialize(outbuffer + offset); 30 | offset += this->vector.serialize(outbuffer + offset); 31 | return offset; 32 | } 33 | 34 | virtual int deserialize(unsigned char *inbuffer) 35 | { 36 | int offset = 0; 37 | offset += this->header.deserialize(inbuffer + offset); 38 | offset += this->vector.deserialize(inbuffer + offset); 39 | return offset; 40 | } 41 | 42 | const char * getType(){ return "geometry_msgs/Vector3Stamped"; }; 43 | const char * getMD5(){ return "7b324c7325e683bf02a9b14b01090ec7"; }; 44 | 45 | }; 46 | 47 | } 48 | #endif -------------------------------------------------------------------------------- /momobot_ws/src/momobot/teensy_bakbak/firmware/lib/ros_lib/geometry_msgs/InertiaStamped.h: -------------------------------------------------------------------------------- 1 | #ifndef _ROS_geometry_msgs_InertiaStamped_h 2 | #define _ROS_geometry_msgs_InertiaStamped_h 3 | 4 | #include 5 | #include 6 | #include 7 | #include "ros/msg.h" 8 | #include "std_msgs/Header.h" 9 | #include "geometry_msgs/Inertia.h" 10 | 11 | namespace geometry_msgs 12 | { 13 | 14 | class InertiaStamped : public ros::Msg 15 | { 16 | public: 17 | std_msgs::Header header; 18 | geometry_msgs::Inertia inertia; 19 | 20 | InertiaStamped(): 21 | header(), 22 | inertia() 23 | { 24 | } 25 | 26 | virtual int serialize(unsigned char *outbuffer) const 27 | { 28 | int offset = 0; 29 | offset += this->header.serialize(outbuffer + offset); 30 | offset += this->inertia.serialize(outbuffer + offset); 31 | return offset; 32 | } 33 | 34 | virtual int deserialize(unsigned char *inbuffer) 35 | { 36 | int offset = 0; 37 | offset += this->header.deserialize(inbuffer + offset); 38 | offset += this->inertia.deserialize(inbuffer + offset); 39 | return offset; 40 | } 41 | 42 | const char * getType(){ return "geometry_msgs/InertiaStamped"; }; 43 | const char * getMD5(){ return "ddee48caeab5a966c5e8d166654a9ac7"; }; 44 | 45 | }; 46 | 47 | } 48 | #endif -------------------------------------------------------------------------------- /momobot_ws/src/momobot/teensy_bakbak/firmware/lib/ros_lib/geometry_msgs/PolygonStamped.h: -------------------------------------------------------------------------------- 1 | #ifndef _ROS_geometry_msgs_PolygonStamped_h 2 | #define _ROS_geometry_msgs_PolygonStamped_h 3 | 4 | #include 5 | #include 6 | #include 7 | #include "ros/msg.h" 8 | #include "std_msgs/Header.h" 9 | #include "geometry_msgs/Polygon.h" 10 | 11 | namespace geometry_msgs 12 | { 13 | 14 | class PolygonStamped : public ros::Msg 15 | { 16 | public: 17 | std_msgs::Header header; 18 | geometry_msgs::Polygon polygon; 19 | 20 | PolygonStamped(): 21 | header(), 22 | polygon() 23 | { 24 | } 25 | 26 | virtual int serialize(unsigned char *outbuffer) const 27 | { 28 | int offset = 0; 29 | offset += this->header.serialize(outbuffer + offset); 30 | offset += this->polygon.serialize(outbuffer + offset); 31 | return offset; 32 | } 33 | 34 | virtual int deserialize(unsigned char *inbuffer) 35 | { 36 | int offset = 0; 37 | offset += this->header.deserialize(inbuffer + offset); 38 | offset += this->polygon.deserialize(inbuffer + offset); 39 | return offset; 40 | } 41 | 42 | const char * getType(){ return "geometry_msgs/PolygonStamped"; }; 43 | const char * getMD5(){ return "c6be8f7dc3bee7fe9e8d296070f53340"; }; 44 | 45 | }; 46 | 47 | } 48 | #endif -------------------------------------------------------------------------------- /momobot_ws/src/momobot/teensy/firmware/lib/ros_lib/geometry_msgs/Transform.h: -------------------------------------------------------------------------------- 1 | #ifndef _ROS_geometry_msgs_Transform_h 2 | #define _ROS_geometry_msgs_Transform_h 3 | 4 | #include 5 | #include 6 | #include 7 | #include "ros/msg.h" 8 | #include "geometry_msgs/Vector3.h" 9 | #include "geometry_msgs/Quaternion.h" 10 | 11 | namespace geometry_msgs 12 | { 13 | 14 | class Transform : public ros::Msg 15 | { 16 | public: 17 | geometry_msgs::Vector3 translation; 18 | geometry_msgs::Quaternion rotation; 19 | 20 | Transform(): 21 | translation(), 22 | rotation() 23 | { 24 | } 25 | 26 | virtual int serialize(unsigned char *outbuffer) const 27 | { 28 | int offset = 0; 29 | offset += this->translation.serialize(outbuffer + offset); 30 | offset += this->rotation.serialize(outbuffer + offset); 31 | return offset; 32 | } 33 | 34 | virtual int deserialize(unsigned char *inbuffer) 35 | { 36 | int offset = 0; 37 | offset += this->translation.deserialize(inbuffer + offset); 38 | offset += this->rotation.deserialize(inbuffer + offset); 39 | return offset; 40 | } 41 | 42 | const char * getType(){ return "geometry_msgs/Transform"; }; 43 | const char * getMD5(){ return "ac9eff44abf714214112b05d54a3cf9b"; }; 44 | 45 | }; 46 | 47 | } 48 | #endif -------------------------------------------------------------------------------- /momobot_ws/src/momobot/teensy/firmware/lib/ros_lib/std_msgs/Bool.h: -------------------------------------------------------------------------------- 1 | #ifndef _ROS_std_msgs_Bool_h 2 | #define _ROS_std_msgs_Bool_h 3 | 4 | #include 5 | #include 6 | #include 7 | #include "ros/msg.h" 8 | 9 | namespace std_msgs 10 | { 11 | 12 | class Bool : public ros::Msg 13 | { 14 | public: 15 | bool data; 16 | 17 | Bool(): 18 | data(0) 19 | { 20 | } 21 | 22 | virtual int serialize(unsigned char *outbuffer) const 23 | { 24 | int offset = 0; 25 | union { 26 | bool real; 27 | uint8_t base; 28 | } u_data; 29 | u_data.real = this->data; 30 | *(outbuffer + offset + 0) = (u_data.base >> (8 * 0)) & 0xFF; 31 | offset += sizeof(this->data); 32 | return offset; 33 | } 34 | 35 | virtual int deserialize(unsigned char *inbuffer) 36 | { 37 | int offset = 0; 38 | union { 39 | bool real; 40 | uint8_t base; 41 | } u_data; 42 | u_data.base = 0; 43 | u_data.base |= ((uint8_t) (*(inbuffer + offset + 0))) << (8 * 0); 44 | this->data = u_data.real; 45 | offset += sizeof(this->data); 46 | return offset; 47 | } 48 | 49 | const char * getType(){ return "std_msgs/Bool"; }; 50 | const char * getMD5(){ return "8b94c1b53db61fb6aed406028ad6332a"; }; 51 | 52 | }; 53 | 54 | } 55 | #endif -------------------------------------------------------------------------------- /momobot_ws/src/momobot/teensy/firmware/lib/ros_lib/std_msgs/Byte.h: -------------------------------------------------------------------------------- 1 | #ifndef _ROS_std_msgs_Byte_h 2 | #define _ROS_std_msgs_Byte_h 3 | 4 | #include 5 | #include 6 | #include 7 | #include "ros/msg.h" 8 | 9 | namespace std_msgs 10 | { 11 | 12 | class Byte : public ros::Msg 13 | { 14 | public: 15 | int8_t data; 16 | 17 | Byte(): 18 | data(0) 19 | { 20 | } 21 | 22 | virtual int serialize(unsigned char *outbuffer) const 23 | { 24 | int offset = 0; 25 | union { 26 | int8_t real; 27 | uint8_t base; 28 | } u_data; 29 | u_data.real = this->data; 30 | *(outbuffer + offset + 0) = (u_data.base >> (8 * 0)) & 0xFF; 31 | offset += sizeof(this->data); 32 | return offset; 33 | } 34 | 35 | virtual int deserialize(unsigned char *inbuffer) 36 | { 37 | int offset = 0; 38 | union { 39 | int8_t real; 40 | uint8_t base; 41 | } u_data; 42 | u_data.base = 0; 43 | u_data.base |= ((uint8_t) (*(inbuffer + offset + 0))) << (8 * 0); 44 | this->data = u_data.real; 45 | offset += sizeof(this->data); 46 | return offset; 47 | } 48 | 49 | const char * getType(){ return "std_msgs/Byte"; }; 50 | const char * getMD5(){ return "ad736a2e8818154c487bb80fe42ce43b"; }; 51 | 52 | }; 53 | 54 | } 55 | #endif -------------------------------------------------------------------------------- /momobot_ws/src/momobot/teensy/firmware/lib/ros_lib/std_msgs/Int8.h: -------------------------------------------------------------------------------- 1 | #ifndef _ROS_std_msgs_Int8_h 2 | #define _ROS_std_msgs_Int8_h 3 | 4 | #include 5 | #include 6 | #include 7 | #include "ros/msg.h" 8 | 9 | namespace std_msgs 10 | { 11 | 12 | class Int8 : public ros::Msg 13 | { 14 | public: 15 | int8_t data; 16 | 17 | Int8(): 18 | data(0) 19 | { 20 | } 21 | 22 | virtual int serialize(unsigned char *outbuffer) const 23 | { 24 | int offset = 0; 25 | union { 26 | int8_t real; 27 | uint8_t base; 28 | } u_data; 29 | u_data.real = this->data; 30 | *(outbuffer + offset + 0) = (u_data.base >> (8 * 0)) & 0xFF; 31 | offset += sizeof(this->data); 32 | return offset; 33 | } 34 | 35 | virtual int deserialize(unsigned char *inbuffer) 36 | { 37 | int offset = 0; 38 | union { 39 | int8_t real; 40 | uint8_t base; 41 | } u_data; 42 | u_data.base = 0; 43 | u_data.base |= ((uint8_t) (*(inbuffer + offset + 0))) << (8 * 0); 44 | this->data = u_data.real; 45 | offset += sizeof(this->data); 46 | return offset; 47 | } 48 | 49 | const char * getType(){ return "std_msgs/Int8"; }; 50 | const char * getMD5(){ return "27ffa0c9c4b8fb8492252bcad9e5c57b"; }; 51 | 52 | }; 53 | 54 | } 55 | #endif -------------------------------------------------------------------------------- /momobot_ws/src/momobot/teensy_bakbak/firmware/lib/ros_lib/geometry_msgs/Transform.h: -------------------------------------------------------------------------------- 1 | #ifndef _ROS_geometry_msgs_Transform_h 2 | #define _ROS_geometry_msgs_Transform_h 3 | 4 | #include 5 | #include 6 | #include 7 | #include "ros/msg.h" 8 | #include "geometry_msgs/Vector3.h" 9 | #include "geometry_msgs/Quaternion.h" 10 | 11 | namespace geometry_msgs 12 | { 13 | 14 | class Transform : public ros::Msg 15 | { 16 | public: 17 | geometry_msgs::Vector3 translation; 18 | geometry_msgs::Quaternion rotation; 19 | 20 | Transform(): 21 | translation(), 22 | rotation() 23 | { 24 | } 25 | 26 | virtual int serialize(unsigned char *outbuffer) const 27 | { 28 | int offset = 0; 29 | offset += this->translation.serialize(outbuffer + offset); 30 | offset += this->rotation.serialize(outbuffer + offset); 31 | return offset; 32 | } 33 | 34 | virtual int deserialize(unsigned char *inbuffer) 35 | { 36 | int offset = 0; 37 | offset += this->translation.deserialize(inbuffer + offset); 38 | offset += this->rotation.deserialize(inbuffer + offset); 39 | return offset; 40 | } 41 | 42 | const char * getType(){ return "geometry_msgs/Transform"; }; 43 | const char * getMD5(){ return "ac9eff44abf714214112b05d54a3cf9b"; }; 44 | 45 | }; 46 | 47 | } 48 | #endif -------------------------------------------------------------------------------- /momobot_ws/src/momobot/teensy_bakbak/firmware/lib/ros_lib/std_msgs/Bool.h: -------------------------------------------------------------------------------- 1 | #ifndef _ROS_std_msgs_Bool_h 2 | #define _ROS_std_msgs_Bool_h 3 | 4 | #include 5 | #include 6 | #include 7 | #include "ros/msg.h" 8 | 9 | namespace std_msgs 10 | { 11 | 12 | class Bool : public ros::Msg 13 | { 14 | public: 15 | bool data; 16 | 17 | Bool(): 18 | data(0) 19 | { 20 | } 21 | 22 | virtual int serialize(unsigned char *outbuffer) const 23 | { 24 | int offset = 0; 25 | union { 26 | bool real; 27 | uint8_t base; 28 | } u_data; 29 | u_data.real = this->data; 30 | *(outbuffer + offset + 0) = (u_data.base >> (8 * 0)) & 0xFF; 31 | offset += sizeof(this->data); 32 | return offset; 33 | } 34 | 35 | virtual int deserialize(unsigned char *inbuffer) 36 | { 37 | int offset = 0; 38 | union { 39 | bool real; 40 | uint8_t base; 41 | } u_data; 42 | u_data.base = 0; 43 | u_data.base |= ((uint8_t) (*(inbuffer + offset + 0))) << (8 * 0); 44 | this->data = u_data.real; 45 | offset += sizeof(this->data); 46 | return offset; 47 | } 48 | 49 | const char * getType(){ return "std_msgs/Bool"; }; 50 | const char * getMD5(){ return "8b94c1b53db61fb6aed406028ad6332a"; }; 51 | 52 | }; 53 | 54 | } 55 | #endif -------------------------------------------------------------------------------- /momobot_ws/src/momobot/teensy_bakbak/firmware/lib/ros_lib/std_msgs/Byte.h: -------------------------------------------------------------------------------- 1 | #ifndef _ROS_std_msgs_Byte_h 2 | #define _ROS_std_msgs_Byte_h 3 | 4 | #include 5 | #include 6 | #include 7 | #include "ros/msg.h" 8 | 9 | namespace std_msgs 10 | { 11 | 12 | class Byte : public ros::Msg 13 | { 14 | public: 15 | int8_t data; 16 | 17 | Byte(): 18 | data(0) 19 | { 20 | } 21 | 22 | virtual int serialize(unsigned char *outbuffer) const 23 | { 24 | int offset = 0; 25 | union { 26 | int8_t real; 27 | uint8_t base; 28 | } u_data; 29 | u_data.real = this->data; 30 | *(outbuffer + offset + 0) = (u_data.base >> (8 * 0)) & 0xFF; 31 | offset += sizeof(this->data); 32 | return offset; 33 | } 34 | 35 | virtual int deserialize(unsigned char *inbuffer) 36 | { 37 | int offset = 0; 38 | union { 39 | int8_t real; 40 | uint8_t base; 41 | } u_data; 42 | u_data.base = 0; 43 | u_data.base |= ((uint8_t) (*(inbuffer + offset + 0))) << (8 * 0); 44 | this->data = u_data.real; 45 | offset += sizeof(this->data); 46 | return offset; 47 | } 48 | 49 | const char * getType(){ return "std_msgs/Byte"; }; 50 | const char * getMD5(){ return "ad736a2e8818154c487bb80fe42ce43b"; }; 51 | 52 | }; 53 | 54 | } 55 | #endif -------------------------------------------------------------------------------- /momobot_ws/src/momobot/teensy_bakbak/firmware/lib/ros_lib/std_msgs/Int8.h: -------------------------------------------------------------------------------- 1 | #ifndef _ROS_std_msgs_Int8_h 2 | #define _ROS_std_msgs_Int8_h 3 | 4 | #include 5 | #include 6 | #include 7 | #include "ros/msg.h" 8 | 9 | namespace std_msgs 10 | { 11 | 12 | class Int8 : public ros::Msg 13 | { 14 | public: 15 | int8_t data; 16 | 17 | Int8(): 18 | data(0) 19 | { 20 | } 21 | 22 | virtual int serialize(unsigned char *outbuffer) const 23 | { 24 | int offset = 0; 25 | union { 26 | int8_t real; 27 | uint8_t base; 28 | } u_data; 29 | u_data.real = this->data; 30 | *(outbuffer + offset + 0) = (u_data.base >> (8 * 0)) & 0xFF; 31 | offset += sizeof(this->data); 32 | return offset; 33 | } 34 | 35 | virtual int deserialize(unsigned char *inbuffer) 36 | { 37 | int offset = 0; 38 | union { 39 | int8_t real; 40 | uint8_t base; 41 | } u_data; 42 | u_data.base = 0; 43 | u_data.base |= ((uint8_t) (*(inbuffer + offset + 0))) << (8 * 0); 44 | this->data = u_data.real; 45 | offset += sizeof(this->data); 46 | return offset; 47 | } 48 | 49 | const char * getType(){ return "std_msgs/Int8"; }; 50 | const char * getMD5(){ return "27ffa0c9c4b8fb8492252bcad9e5c57b"; }; 51 | 52 | }; 53 | 54 | } 55 | #endif -------------------------------------------------------------------------------- /momobot_ws/src/momobot/teensy/firmware/lib/ros_lib/geometry_msgs/QuaternionStamped.h: -------------------------------------------------------------------------------- 1 | #ifndef _ROS_geometry_msgs_QuaternionStamped_h 2 | #define _ROS_geometry_msgs_QuaternionStamped_h 3 | 4 | #include 5 | #include 6 | #include 7 | #include "ros/msg.h" 8 | #include "std_msgs/Header.h" 9 | #include "geometry_msgs/Quaternion.h" 10 | 11 | namespace geometry_msgs 12 | { 13 | 14 | class QuaternionStamped : public ros::Msg 15 | { 16 | public: 17 | std_msgs::Header header; 18 | geometry_msgs::Quaternion quaternion; 19 | 20 | QuaternionStamped(): 21 | header(), 22 | quaternion() 23 | { 24 | } 25 | 26 | virtual int serialize(unsigned char *outbuffer) const 27 | { 28 | int offset = 0; 29 | offset += this->header.serialize(outbuffer + offset); 30 | offset += this->quaternion.serialize(outbuffer + offset); 31 | return offset; 32 | } 33 | 34 | virtual int deserialize(unsigned char *inbuffer) 35 | { 36 | int offset = 0; 37 | offset += this->header.deserialize(inbuffer + offset); 38 | offset += this->quaternion.deserialize(inbuffer + offset); 39 | return offset; 40 | } 41 | 42 | const char * getType(){ return "geometry_msgs/QuaternionStamped"; }; 43 | const char * getMD5(){ return "e57f1e547e0e1fd13504588ffc8334e2"; }; 44 | 45 | }; 46 | 47 | } 48 | #endif -------------------------------------------------------------------------------- /momobot_ws/src/momobot/teensy_bakbak/firmware/lib/ros_lib/geometry_msgs/QuaternionStamped.h: -------------------------------------------------------------------------------- 1 | #ifndef _ROS_geometry_msgs_QuaternionStamped_h 2 | #define _ROS_geometry_msgs_QuaternionStamped_h 3 | 4 | #include 5 | #include 6 | #include 7 | #include "ros/msg.h" 8 | #include "std_msgs/Header.h" 9 | #include "geometry_msgs/Quaternion.h" 10 | 11 | namespace geometry_msgs 12 | { 13 | 14 | class QuaternionStamped : public ros::Msg 15 | { 16 | public: 17 | std_msgs::Header header; 18 | geometry_msgs::Quaternion quaternion; 19 | 20 | QuaternionStamped(): 21 | header(), 22 | quaternion() 23 | { 24 | } 25 | 26 | virtual int serialize(unsigned char *outbuffer) const 27 | { 28 | int offset = 0; 29 | offset += this->header.serialize(outbuffer + offset); 30 | offset += this->quaternion.serialize(outbuffer + offset); 31 | return offset; 32 | } 33 | 34 | virtual int deserialize(unsigned char *inbuffer) 35 | { 36 | int offset = 0; 37 | offset += this->header.deserialize(inbuffer + offset); 38 | offset += this->quaternion.deserialize(inbuffer + offset); 39 | return offset; 40 | } 41 | 42 | const char * getType(){ return "geometry_msgs/QuaternionStamped"; }; 43 | const char * getMD5(){ return "e57f1e547e0e1fd13504588ffc8334e2"; }; 44 | 45 | }; 46 | 47 | } 48 | #endif -------------------------------------------------------------------------------- /LICENSE: -------------------------------------------------------------------------------- 1 | BSD 2-Clause License 2 | 3 | Copyright (c) 2018, methylDragon 4 | All rights reserved. 5 | 6 | Redistribution and use in source and binary forms, with or without 7 | modification, are permitted provided that the following conditions are met: 8 | 9 | * Redistributions of source code must retain the above copyright notice, this 10 | list of conditions and the following disclaimer. 11 | 12 | * Redistributions in binary form must reproduce the above copyright notice, 13 | this list of conditions and the following disclaimer in the documentation 14 | and/or other materials provided with the distribution. 15 | 16 | THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" 17 | AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE 18 | IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE 19 | DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE 20 | FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL 21 | DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR 22 | SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER 23 | CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, 24 | OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE 25 | OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. 26 | -------------------------------------------------------------------------------- /momobot_ws/src/momobot/LICENSE: -------------------------------------------------------------------------------- 1 | BSD 2-Clause License 2 | 3 | Copyright (c) 2017, linorobot 4 | All rights reserved. 5 | 6 | Redistribution and use in source and binary forms, with or without 7 | modification, are permitted provided that the following conditions are met: 8 | 9 | * Redistributions of source code must retain the above copyright notice, this 10 | list of conditions and the following disclaimer. 11 | 12 | * Redistributions in binary form must reproduce the above copyright notice, 13 | this list of conditions and the following disclaimer in the documentation 14 | and/or other materials provided with the distribution. 15 | 16 | THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" 17 | AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE 18 | IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE 19 | DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE 20 | FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL 21 | DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR 22 | SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER 23 | CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, 24 | OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE 25 | OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. 26 | -------------------------------------------------------------------------------- /momobot_ws/src/momobot/teensy/firmware/lib/ros_lib/geometry_msgs/PoseWithCovarianceStamped.h: -------------------------------------------------------------------------------- 1 | #ifndef _ROS_geometry_msgs_PoseWithCovarianceStamped_h 2 | #define _ROS_geometry_msgs_PoseWithCovarianceStamped_h 3 | 4 | #include 5 | #include 6 | #include 7 | #include "ros/msg.h" 8 | #include "std_msgs/Header.h" 9 | #include "geometry_msgs/PoseWithCovariance.h" 10 | 11 | namespace geometry_msgs 12 | { 13 | 14 | class PoseWithCovarianceStamped : public ros::Msg 15 | { 16 | public: 17 | std_msgs::Header header; 18 | geometry_msgs::PoseWithCovariance pose; 19 | 20 | PoseWithCovarianceStamped(): 21 | header(), 22 | pose() 23 | { 24 | } 25 | 26 | virtual int serialize(unsigned char *outbuffer) const 27 | { 28 | int offset = 0; 29 | offset += this->header.serialize(outbuffer + offset); 30 | offset += this->pose.serialize(outbuffer + offset); 31 | return offset; 32 | } 33 | 34 | virtual int deserialize(unsigned char *inbuffer) 35 | { 36 | int offset = 0; 37 | offset += this->header.deserialize(inbuffer + offset); 38 | offset += this->pose.deserialize(inbuffer + offset); 39 | return offset; 40 | } 41 | 42 | const char * getType(){ return "geometry_msgs/PoseWithCovarianceStamped"; }; 43 | const char * getMD5(){ return "953b798c0f514ff060a53a3498ce6246"; }; 44 | 45 | }; 46 | 47 | } 48 | #endif -------------------------------------------------------------------------------- /momobot_ws/src/momobot/teensy_bakbak/firmware/lib/ros_lib/geometry_msgs/PoseWithCovarianceStamped.h: -------------------------------------------------------------------------------- 1 | #ifndef _ROS_geometry_msgs_PoseWithCovarianceStamped_h 2 | #define _ROS_geometry_msgs_PoseWithCovarianceStamped_h 3 | 4 | #include 5 | #include 6 | #include 7 | #include "ros/msg.h" 8 | #include "std_msgs/Header.h" 9 | #include "geometry_msgs/PoseWithCovariance.h" 10 | 11 | namespace geometry_msgs 12 | { 13 | 14 | class PoseWithCovarianceStamped : public ros::Msg 15 | { 16 | public: 17 | std_msgs::Header header; 18 | geometry_msgs::PoseWithCovariance pose; 19 | 20 | PoseWithCovarianceStamped(): 21 | header(), 22 | pose() 23 | { 24 | } 25 | 26 | virtual int serialize(unsigned char *outbuffer) const 27 | { 28 | int offset = 0; 29 | offset += this->header.serialize(outbuffer + offset); 30 | offset += this->pose.serialize(outbuffer + offset); 31 | return offset; 32 | } 33 | 34 | virtual int deserialize(unsigned char *inbuffer) 35 | { 36 | int offset = 0; 37 | offset += this->header.deserialize(inbuffer + offset); 38 | offset += this->pose.deserialize(inbuffer + offset); 39 | return offset; 40 | } 41 | 42 | const char * getType(){ return "geometry_msgs/PoseWithCovarianceStamped"; }; 43 | const char * getMD5(){ return "953b798c0f514ff060a53a3498ce6246"; }; 44 | 45 | }; 46 | 47 | } 48 | #endif -------------------------------------------------------------------------------- /momobot_ws/src/momobot/teensy/firmware/lib/ros_lib/geometry_msgs/AccelWithCovarianceStamped.h: -------------------------------------------------------------------------------- 1 | #ifndef _ROS_geometry_msgs_AccelWithCovarianceStamped_h 2 | #define _ROS_geometry_msgs_AccelWithCovarianceStamped_h 3 | 4 | #include 5 | #include 6 | #include 7 | #include "ros/msg.h" 8 | #include "std_msgs/Header.h" 9 | #include "geometry_msgs/AccelWithCovariance.h" 10 | 11 | namespace geometry_msgs 12 | { 13 | 14 | class AccelWithCovarianceStamped : public ros::Msg 15 | { 16 | public: 17 | std_msgs::Header header; 18 | geometry_msgs::AccelWithCovariance accel; 19 | 20 | AccelWithCovarianceStamped(): 21 | header(), 22 | accel() 23 | { 24 | } 25 | 26 | virtual int serialize(unsigned char *outbuffer) const 27 | { 28 | int offset = 0; 29 | offset += this->header.serialize(outbuffer + offset); 30 | offset += this->accel.serialize(outbuffer + offset); 31 | return offset; 32 | } 33 | 34 | virtual int deserialize(unsigned char *inbuffer) 35 | { 36 | int offset = 0; 37 | offset += this->header.deserialize(inbuffer + offset); 38 | offset += this->accel.deserialize(inbuffer + offset); 39 | return offset; 40 | } 41 | 42 | const char * getType(){ return "geometry_msgs/AccelWithCovarianceStamped"; }; 43 | const char * getMD5(){ return "96adb295225031ec8d57fb4251b0a886"; }; 44 | 45 | }; 46 | 47 | } 48 | #endif -------------------------------------------------------------------------------- /momobot_ws/src/momobot/teensy/firmware/lib/ros_lib/geometry_msgs/TwistWithCovarianceStamped.h: -------------------------------------------------------------------------------- 1 | #ifndef _ROS_geometry_msgs_TwistWithCovarianceStamped_h 2 | #define _ROS_geometry_msgs_TwistWithCovarianceStamped_h 3 | 4 | #include 5 | #include 6 | #include 7 | #include "ros/msg.h" 8 | #include "std_msgs/Header.h" 9 | #include "geometry_msgs/TwistWithCovariance.h" 10 | 11 | namespace geometry_msgs 12 | { 13 | 14 | class TwistWithCovarianceStamped : public ros::Msg 15 | { 16 | public: 17 | std_msgs::Header header; 18 | geometry_msgs::TwistWithCovariance twist; 19 | 20 | TwistWithCovarianceStamped(): 21 | header(), 22 | twist() 23 | { 24 | } 25 | 26 | virtual int serialize(unsigned char *outbuffer) const 27 | { 28 | int offset = 0; 29 | offset += this->header.serialize(outbuffer + offset); 30 | offset += this->twist.serialize(outbuffer + offset); 31 | return offset; 32 | } 33 | 34 | virtual int deserialize(unsigned char *inbuffer) 35 | { 36 | int offset = 0; 37 | offset += this->header.deserialize(inbuffer + offset); 38 | offset += this->twist.deserialize(inbuffer + offset); 39 | return offset; 40 | } 41 | 42 | const char * getType(){ return "geometry_msgs/TwistWithCovarianceStamped"; }; 43 | const char * getMD5(){ return "8927a1a12fb2607ceea095b2dc440a96"; }; 44 | 45 | }; 46 | 47 | } 48 | #endif -------------------------------------------------------------------------------- /momobot_ws/src/momobot/teensy_bakbak/firmware/lib/ros_lib/geometry_msgs/AccelWithCovarianceStamped.h: -------------------------------------------------------------------------------- 1 | #ifndef _ROS_geometry_msgs_AccelWithCovarianceStamped_h 2 | #define _ROS_geometry_msgs_AccelWithCovarianceStamped_h 3 | 4 | #include 5 | #include 6 | #include 7 | #include "ros/msg.h" 8 | #include "std_msgs/Header.h" 9 | #include "geometry_msgs/AccelWithCovariance.h" 10 | 11 | namespace geometry_msgs 12 | { 13 | 14 | class AccelWithCovarianceStamped : public ros::Msg 15 | { 16 | public: 17 | std_msgs::Header header; 18 | geometry_msgs::AccelWithCovariance accel; 19 | 20 | AccelWithCovarianceStamped(): 21 | header(), 22 | accel() 23 | { 24 | } 25 | 26 | virtual int serialize(unsigned char *outbuffer) const 27 | { 28 | int offset = 0; 29 | offset += this->header.serialize(outbuffer + offset); 30 | offset += this->accel.serialize(outbuffer + offset); 31 | return offset; 32 | } 33 | 34 | virtual int deserialize(unsigned char *inbuffer) 35 | { 36 | int offset = 0; 37 | offset += this->header.deserialize(inbuffer + offset); 38 | offset += this->accel.deserialize(inbuffer + offset); 39 | return offset; 40 | } 41 | 42 | const char * getType(){ return "geometry_msgs/AccelWithCovarianceStamped"; }; 43 | const char * getMD5(){ return "96adb295225031ec8d57fb4251b0a886"; }; 44 | 45 | }; 46 | 47 | } 48 | #endif -------------------------------------------------------------------------------- /momobot_ws/src/momobot/teensy_bakbak/firmware/lib/ros_lib/geometry_msgs/TwistWithCovarianceStamped.h: -------------------------------------------------------------------------------- 1 | #ifndef _ROS_geometry_msgs_TwistWithCovarianceStamped_h 2 | #define _ROS_geometry_msgs_TwistWithCovarianceStamped_h 3 | 4 | #include 5 | #include 6 | #include 7 | #include "ros/msg.h" 8 | #include "std_msgs/Header.h" 9 | #include "geometry_msgs/TwistWithCovariance.h" 10 | 11 | namespace geometry_msgs 12 | { 13 | 14 | class TwistWithCovarianceStamped : public ros::Msg 15 | { 16 | public: 17 | std_msgs::Header header; 18 | geometry_msgs::TwistWithCovariance twist; 19 | 20 | TwistWithCovarianceStamped(): 21 | header(), 22 | twist() 23 | { 24 | } 25 | 26 | virtual int serialize(unsigned char *outbuffer) const 27 | { 28 | int offset = 0; 29 | offset += this->header.serialize(outbuffer + offset); 30 | offset += this->twist.serialize(outbuffer + offset); 31 | return offset; 32 | } 33 | 34 | virtual int deserialize(unsigned char *inbuffer) 35 | { 36 | int offset = 0; 37 | offset += this->header.deserialize(inbuffer + offset); 38 | offset += this->twist.deserialize(inbuffer + offset); 39 | return offset; 40 | } 41 | 42 | const char * getType(){ return "geometry_msgs/TwistWithCovarianceStamped"; }; 43 | const char * getMD5(){ return "8927a1a12fb2607ceea095b2dc440a96"; }; 44 | 45 | }; 46 | 47 | } 48 | #endif -------------------------------------------------------------------------------- /momobot_ws/src/momobot/teensy/firmware/lib/ros_lib/std_msgs/String.h: -------------------------------------------------------------------------------- 1 | #ifndef _ROS_std_msgs_String_h 2 | #define _ROS_std_msgs_String_h 3 | 4 | #include 5 | #include 6 | #include 7 | #include "ros/msg.h" 8 | 9 | namespace std_msgs 10 | { 11 | 12 | class String : public ros::Msg 13 | { 14 | public: 15 | const char* data; 16 | 17 | String(): 18 | data("") 19 | { 20 | } 21 | 22 | virtual int serialize(unsigned char *outbuffer) const 23 | { 24 | int offset = 0; 25 | uint32_t length_data = strlen(this->data); 26 | memcpy(outbuffer + offset, &length_data, sizeof(uint32_t)); 27 | offset += 4; 28 | memcpy(outbuffer + offset, this->data, length_data); 29 | offset += length_data; 30 | return offset; 31 | } 32 | 33 | virtual int deserialize(unsigned char *inbuffer) 34 | { 35 | int offset = 0; 36 | uint32_t length_data; 37 | memcpy(&length_data, (inbuffer + offset), sizeof(uint32_t)); 38 | offset += 4; 39 | for(unsigned int k= offset; k< offset+length_data; ++k){ 40 | inbuffer[k-1]=inbuffer[k]; 41 | } 42 | inbuffer[offset+length_data-1]=0; 43 | this->data = (char *)(inbuffer + offset-1); 44 | offset += length_data; 45 | return offset; 46 | } 47 | 48 | const char * getType(){ return "std_msgs/String"; }; 49 | const char * getMD5(){ return "992ce8a1687cec8c8bd883ec73ca41d1"; }; 50 | 51 | }; 52 | 53 | } 54 | #endif -------------------------------------------------------------------------------- /momobot_ws/src/momobot/teensy/firmware/lib/encoder/examples/TwoKnobs/TwoKnobs.pde: -------------------------------------------------------------------------------- 1 | /* Encoder Library - TwoKnobs Example 2 | * http://www.pjrc.com/teensy/td_libs_Encoder.html 3 | * 4 | * This example code is in the public domain. 5 | */ 6 | 7 | #include 8 | 9 | // Change these pin numbers to the pins connected to your encoder. 10 | // Best Performance: both pins have interrupt capability 11 | // Good Performance: only the first pin has interrupt capability 12 | // Low Performance: neither pin has interrupt capability 13 | Encoder knobLeft(5, 6); 14 | Encoder knobRight(7, 8); 15 | // avoid using pins with LEDs attached 16 | 17 | void setup() { 18 | Serial.begin(9600); 19 | Serial.println("TwoKnobs Encoder Test:"); 20 | } 21 | 22 | long positionLeft = -999; 23 | long positionRight = -999; 24 | 25 | void loop() { 26 | long newLeft, newRight; 27 | newLeft = knobLeft.read(); 28 | newRight = knobRight.read(); 29 | if (newLeft != positionLeft || newRight != positionRight) { 30 | Serial.print("Left = "); 31 | Serial.print(newLeft); 32 | Serial.print(", Right = "); 33 | Serial.print(newRight); 34 | Serial.println(); 35 | positionLeft = newLeft; 36 | positionRight = newRight; 37 | } 38 | // if a character is sent from the serial monitor, 39 | // reset both back to zero. 40 | if (Serial.available()) { 41 | Serial.read(); 42 | Serial.println("Reset both knobs to zero"); 43 | knobLeft.write(0); 44 | knobRight.write(0); 45 | } 46 | } 47 | -------------------------------------------------------------------------------- /momobot_ws/src/momobot/teensy_bakbak/firmware/lib/ros_lib/std_msgs/String.h: -------------------------------------------------------------------------------- 1 | #ifndef _ROS_std_msgs_String_h 2 | #define _ROS_std_msgs_String_h 3 | 4 | #include 5 | #include 6 | #include 7 | #include "ros/msg.h" 8 | 9 | namespace std_msgs 10 | { 11 | 12 | class String : public ros::Msg 13 | { 14 | public: 15 | const char* data; 16 | 17 | String(): 18 | data("") 19 | { 20 | } 21 | 22 | virtual int serialize(unsigned char *outbuffer) const 23 | { 24 | int offset = 0; 25 | uint32_t length_data = strlen(this->data); 26 | memcpy(outbuffer + offset, &length_data, sizeof(uint32_t)); 27 | offset += 4; 28 | memcpy(outbuffer + offset, this->data, length_data); 29 | offset += length_data; 30 | return offset; 31 | } 32 | 33 | virtual int deserialize(unsigned char *inbuffer) 34 | { 35 | int offset = 0; 36 | uint32_t length_data; 37 | memcpy(&length_data, (inbuffer + offset), sizeof(uint32_t)); 38 | offset += 4; 39 | for(unsigned int k= offset; k< offset+length_data; ++k){ 40 | inbuffer[k-1]=inbuffer[k]; 41 | } 42 | inbuffer[offset+length_data-1]=0; 43 | this->data = (char *)(inbuffer + offset-1); 44 | offset += length_data; 45 | return offset; 46 | } 47 | 48 | const char * getType(){ return "std_msgs/String"; }; 49 | const char * getMD5(){ return "992ce8a1687cec8c8bd883ec73ca41d1"; }; 50 | 51 | }; 52 | 53 | } 54 | #endif -------------------------------------------------------------------------------- /momobot_ws/src/momobot/teensy_bakbak/firmware/lib/encoder/examples/TwoKnobs/TwoKnobs.pde: -------------------------------------------------------------------------------- 1 | /* Encoder Library - TwoKnobs Example 2 | * http://www.pjrc.com/teensy/td_libs_Encoder.html 3 | * 4 | * This example code is in the public domain. 5 | */ 6 | 7 | #include 8 | 9 | // Change these pin numbers to the pins connected to your encoder. 10 | // Best Performance: both pins have interrupt capability 11 | // Good Performance: only the first pin has interrupt capability 12 | // Low Performance: neither pin has interrupt capability 13 | Encoder knobLeft(5, 6); 14 | Encoder knobRight(7, 8); 15 | // avoid using pins with LEDs attached 16 | 17 | void setup() { 18 | Serial.begin(9600); 19 | Serial.println("TwoKnobs Encoder Test:"); 20 | } 21 | 22 | long positionLeft = -999; 23 | long positionRight = -999; 24 | 25 | void loop() { 26 | long newLeft, newRight; 27 | newLeft = knobLeft.read(); 28 | newRight = knobRight.read(); 29 | if (newLeft != positionLeft || newRight != positionRight) { 30 | Serial.print("Left = "); 31 | Serial.print(newLeft); 32 | Serial.print(", Right = "); 33 | Serial.print(newRight); 34 | Serial.println(); 35 | positionLeft = newLeft; 36 | positionRight = newRight; 37 | } 38 | // if a character is sent from the serial monitor, 39 | // reset both back to zero. 40 | if (Serial.available()) { 41 | Serial.read(); 42 | Serial.println("Reset both knobs to zero"); 43 | knobLeft.write(0); 44 | knobRight.write(0); 45 | } 46 | } 47 | -------------------------------------------------------------------------------- /momobot_ws/src/momobot/teensy/firmware/lib/ros_lib/std_msgs/UInt32.h: -------------------------------------------------------------------------------- 1 | #ifndef _ROS_std_msgs_UInt32_h 2 | #define _ROS_std_msgs_UInt32_h 3 | 4 | #include 5 | #include 6 | #include 7 | #include "ros/msg.h" 8 | 9 | namespace std_msgs 10 | { 11 | 12 | class UInt32 : public ros::Msg 13 | { 14 | public: 15 | uint32_t data; 16 | 17 | UInt32(): 18 | data(0) 19 | { 20 | } 21 | 22 | virtual int serialize(unsigned char *outbuffer) const 23 | { 24 | int offset = 0; 25 | *(outbuffer + offset + 0) = (this->data >> (8 * 0)) & 0xFF; 26 | *(outbuffer + offset + 1) = (this->data >> (8 * 1)) & 0xFF; 27 | *(outbuffer + offset + 2) = (this->data >> (8 * 2)) & 0xFF; 28 | *(outbuffer + offset + 3) = (this->data >> (8 * 3)) & 0xFF; 29 | offset += sizeof(this->data); 30 | return offset; 31 | } 32 | 33 | virtual int deserialize(unsigned char *inbuffer) 34 | { 35 | int offset = 0; 36 | this->data = ((uint32_t) (*(inbuffer + offset))); 37 | this->data |= ((uint32_t) (*(inbuffer + offset + 1))) << (8 * 1); 38 | this->data |= ((uint32_t) (*(inbuffer + offset + 2))) << (8 * 2); 39 | this->data |= ((uint32_t) (*(inbuffer + offset + 3))) << (8 * 3); 40 | offset += sizeof(this->data); 41 | return offset; 42 | } 43 | 44 | const char * getType(){ return "std_msgs/UInt32"; }; 45 | const char * getMD5(){ return "304a39449588c7f8ce2df6e8001c5fce"; }; 46 | 47 | }; 48 | 49 | } 50 | #endif -------------------------------------------------------------------------------- /momobot_ws/src/momobot/teensy/firmware/lib/ros_lib/std_msgs/Int16.h: -------------------------------------------------------------------------------- 1 | #ifndef _ROS_std_msgs_Int16_h 2 | #define _ROS_std_msgs_Int16_h 3 | 4 | #include 5 | #include 6 | #include 7 | #include "ros/msg.h" 8 | 9 | namespace std_msgs 10 | { 11 | 12 | class Int16 : public ros::Msg 13 | { 14 | public: 15 | int16_t data; 16 | 17 | Int16(): 18 | data(0) 19 | { 20 | } 21 | 22 | virtual int serialize(unsigned char *outbuffer) const 23 | { 24 | int offset = 0; 25 | union { 26 | int16_t real; 27 | uint16_t base; 28 | } u_data; 29 | u_data.real = this->data; 30 | *(outbuffer + offset + 0) = (u_data.base >> (8 * 0)) & 0xFF; 31 | *(outbuffer + offset + 1) = (u_data.base >> (8 * 1)) & 0xFF; 32 | offset += sizeof(this->data); 33 | return offset; 34 | } 35 | 36 | virtual int deserialize(unsigned char *inbuffer) 37 | { 38 | int offset = 0; 39 | union { 40 | int16_t real; 41 | uint16_t base; 42 | } u_data; 43 | u_data.base = 0; 44 | u_data.base |= ((uint16_t) (*(inbuffer + offset + 0))) << (8 * 0); 45 | u_data.base |= ((uint16_t) (*(inbuffer + offset + 1))) << (8 * 1); 46 | this->data = u_data.real; 47 | offset += sizeof(this->data); 48 | return offset; 49 | } 50 | 51 | const char * getType(){ return "std_msgs/Int16"; }; 52 | const char * getMD5(){ return "8524586e34fbd7cb1c08c5f5f1ca0e57"; }; 53 | 54 | }; 55 | 56 | } 57 | #endif -------------------------------------------------------------------------------- /momobot_ws/src/momobot/teensy_bakbak/firmware/lib/ros_lib/std_msgs/UInt32.h: -------------------------------------------------------------------------------- 1 | #ifndef _ROS_std_msgs_UInt32_h 2 | #define _ROS_std_msgs_UInt32_h 3 | 4 | #include 5 | #include 6 | #include 7 | #include "ros/msg.h" 8 | 9 | namespace std_msgs 10 | { 11 | 12 | class UInt32 : public ros::Msg 13 | { 14 | public: 15 | uint32_t data; 16 | 17 | UInt32(): 18 | data(0) 19 | { 20 | } 21 | 22 | virtual int serialize(unsigned char *outbuffer) const 23 | { 24 | int offset = 0; 25 | *(outbuffer + offset + 0) = (this->data >> (8 * 0)) & 0xFF; 26 | *(outbuffer + offset + 1) = (this->data >> (8 * 1)) & 0xFF; 27 | *(outbuffer + offset + 2) = (this->data >> (8 * 2)) & 0xFF; 28 | *(outbuffer + offset + 3) = (this->data >> (8 * 3)) & 0xFF; 29 | offset += sizeof(this->data); 30 | return offset; 31 | } 32 | 33 | virtual int deserialize(unsigned char *inbuffer) 34 | { 35 | int offset = 0; 36 | this->data = ((uint32_t) (*(inbuffer + offset))); 37 | this->data |= ((uint32_t) (*(inbuffer + offset + 1))) << (8 * 1); 38 | this->data |= ((uint32_t) (*(inbuffer + offset + 2))) << (8 * 2); 39 | this->data |= ((uint32_t) (*(inbuffer + offset + 3))) << (8 * 3); 40 | offset += sizeof(this->data); 41 | return offset; 42 | } 43 | 44 | const char * getType(){ return "std_msgs/UInt32"; }; 45 | const char * getMD5(){ return "304a39449588c7f8ce2df6e8001c5fce"; }; 46 | 47 | }; 48 | 49 | } 50 | #endif -------------------------------------------------------------------------------- /momobot_ws/src/momobot/teensy_bakbak/firmware/lib/ros_lib/std_msgs/Int16.h: -------------------------------------------------------------------------------- 1 | #ifndef _ROS_std_msgs_Int16_h 2 | #define _ROS_std_msgs_Int16_h 3 | 4 | #include 5 | #include 6 | #include 7 | #include "ros/msg.h" 8 | 9 | namespace std_msgs 10 | { 11 | 12 | class Int16 : public ros::Msg 13 | { 14 | public: 15 | int16_t data; 16 | 17 | Int16(): 18 | data(0) 19 | { 20 | } 21 | 22 | virtual int serialize(unsigned char *outbuffer) const 23 | { 24 | int offset = 0; 25 | union { 26 | int16_t real; 27 | uint16_t base; 28 | } u_data; 29 | u_data.real = this->data; 30 | *(outbuffer + offset + 0) = (u_data.base >> (8 * 0)) & 0xFF; 31 | *(outbuffer + offset + 1) = (u_data.base >> (8 * 1)) & 0xFF; 32 | offset += sizeof(this->data); 33 | return offset; 34 | } 35 | 36 | virtual int deserialize(unsigned char *inbuffer) 37 | { 38 | int offset = 0; 39 | union { 40 | int16_t real; 41 | uint16_t base; 42 | } u_data; 43 | u_data.base = 0; 44 | u_data.base |= ((uint16_t) (*(inbuffer + offset + 0))) << (8 * 0); 45 | u_data.base |= ((uint16_t) (*(inbuffer + offset + 1))) << (8 * 1); 46 | this->data = u_data.real; 47 | offset += sizeof(this->data); 48 | return offset; 49 | } 50 | 51 | const char * getType(){ return "std_msgs/Int16"; }; 52 | const char * getMD5(){ return "8524586e34fbd7cb1c08c5f5f1ca0e57"; }; 53 | 54 | }; 55 | 56 | } 57 | #endif -------------------------------------------------------------------------------- /momobot_ws/src/momobot/teensy/firmware/lib/ros_lib/lino_msgs/Imu.h: -------------------------------------------------------------------------------- 1 | #ifndef _ROS_lino_msgs_Imu_h 2 | #define _ROS_lino_msgs_Imu_h 3 | 4 | #include 5 | #include 6 | #include 7 | #include "ros/msg.h" 8 | #include "geometry_msgs/Vector3.h" 9 | 10 | namespace lino_msgs 11 | { 12 | 13 | class Imu : public ros::Msg 14 | { 15 | public: 16 | geometry_msgs::Vector3 linear_acceleration; 17 | geometry_msgs::Vector3 angular_velocity; 18 | geometry_msgs::Vector3 magnetic_field; 19 | 20 | Imu(): 21 | linear_acceleration(), 22 | angular_velocity(), 23 | magnetic_field() 24 | { 25 | } 26 | 27 | virtual int serialize(unsigned char *outbuffer) const 28 | { 29 | int offset = 0; 30 | offset += this->linear_acceleration.serialize(outbuffer + offset); 31 | offset += this->angular_velocity.serialize(outbuffer + offset); 32 | offset += this->magnetic_field.serialize(outbuffer + offset); 33 | return offset; 34 | } 35 | 36 | virtual int deserialize(unsigned char *inbuffer) 37 | { 38 | int offset = 0; 39 | offset += this->linear_acceleration.deserialize(inbuffer + offset); 40 | offset += this->angular_velocity.deserialize(inbuffer + offset); 41 | offset += this->magnetic_field.deserialize(inbuffer + offset); 42 | return offset; 43 | } 44 | 45 | const char * getType(){ return "lino_msgs/Imu"; }; 46 | const char * getMD5(){ return "275110405f08e1b7c0c0f1aba3e19c67"; }; 47 | 48 | }; 49 | 50 | } 51 | #endif -------------------------------------------------------------------------------- /momobot_ws/src/momobot/teensy_bakbak/firmware/lib/ros_lib/lino_msgs/Imu.h: -------------------------------------------------------------------------------- 1 | #ifndef _ROS_lino_msgs_Imu_h 2 | #define _ROS_lino_msgs_Imu_h 3 | 4 | #include 5 | #include 6 | #include 7 | #include "ros/msg.h" 8 | #include "geometry_msgs/Vector3.h" 9 | 10 | namespace lino_msgs 11 | { 12 | 13 | class Imu : public ros::Msg 14 | { 15 | public: 16 | geometry_msgs::Vector3 linear_acceleration; 17 | geometry_msgs::Vector3 angular_velocity; 18 | geometry_msgs::Vector3 magnetic_field; 19 | 20 | Imu(): 21 | linear_acceleration(), 22 | angular_velocity(), 23 | magnetic_field() 24 | { 25 | } 26 | 27 | virtual int serialize(unsigned char *outbuffer) const 28 | { 29 | int offset = 0; 30 | offset += this->linear_acceleration.serialize(outbuffer + offset); 31 | offset += this->angular_velocity.serialize(outbuffer + offset); 32 | offset += this->magnetic_field.serialize(outbuffer + offset); 33 | return offset; 34 | } 35 | 36 | virtual int deserialize(unsigned char *inbuffer) 37 | { 38 | int offset = 0; 39 | offset += this->linear_acceleration.deserialize(inbuffer + offset); 40 | offset += this->angular_velocity.deserialize(inbuffer + offset); 41 | offset += this->magnetic_field.deserialize(inbuffer + offset); 42 | return offset; 43 | } 44 | 45 | const char * getType(){ return "lino_msgs/Imu"; }; 46 | const char * getMD5(){ return "275110405f08e1b7c0c0f1aba3e19c67"; }; 47 | 48 | }; 49 | 50 | } 51 | #endif -------------------------------------------------------------------------------- /momobot_ws/src/momobot/package.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | momobot 4 | 1.3.5 5 | ROS compatible ground robots 6 | t 7 | 8 | BSD 9 | 10 | http://www.linorobot.org 11 | 12 | Juan Miguel Jimeno 13 | 14 | catkin 15 | roscpp 16 | rospy 17 | tf 18 | nav_msgs 19 | lino_msgs 20 | sensor_msgs 21 | geometry_msgs 22 | std_msgs 23 | 24 | roscpp 25 | rospy 26 | rosserial_python 27 | tf 28 | nav_msgs 29 | lino_msgs 30 | sensor_msgs 31 | geometry_msgs 32 | std_msgs 33 | gmapping 34 | map_server 35 | move_base 36 | amcl 37 | xv-11-laser-driver 38 | imu-filter-madgwick 39 | imu_calib 40 | lino_pid 41 | 42 | 43 | 44 | 45 | -------------------------------------------------------------------------------- /momobot_ws/src/momobot/teensy/firmware/lib/ros_lib/std_msgs/Int32.h: -------------------------------------------------------------------------------- 1 | #ifndef _ROS_std_msgs_Int32_h 2 | #define _ROS_std_msgs_Int32_h 3 | 4 | #include 5 | #include 6 | #include 7 | #include "ros/msg.h" 8 | 9 | namespace std_msgs 10 | { 11 | 12 | class Int32 : public ros::Msg 13 | { 14 | public: 15 | int32_t data; 16 | 17 | Int32(): 18 | data(0) 19 | { 20 | } 21 | 22 | virtual int serialize(unsigned char *outbuffer) const 23 | { 24 | int offset = 0; 25 | union { 26 | int32_t real; 27 | uint32_t base; 28 | } u_data; 29 | u_data.real = this->data; 30 | *(outbuffer + offset + 0) = (u_data.base >> (8 * 0)) & 0xFF; 31 | *(outbuffer + offset + 1) = (u_data.base >> (8 * 1)) & 0xFF; 32 | *(outbuffer + offset + 2) = (u_data.base >> (8 * 2)) & 0xFF; 33 | *(outbuffer + offset + 3) = (u_data.base >> (8 * 3)) & 0xFF; 34 | offset += sizeof(this->data); 35 | return offset; 36 | } 37 | 38 | virtual int deserialize(unsigned char *inbuffer) 39 | { 40 | int offset = 0; 41 | union { 42 | int32_t real; 43 | uint32_t base; 44 | } u_data; 45 | u_data.base = 0; 46 | u_data.base |= ((uint32_t) (*(inbuffer + offset + 0))) << (8 * 0); 47 | u_data.base |= ((uint32_t) (*(inbuffer + offset + 1))) << (8 * 1); 48 | u_data.base |= ((uint32_t) (*(inbuffer + offset + 2))) << (8 * 2); 49 | u_data.base |= ((uint32_t) (*(inbuffer + offset + 3))) << (8 * 3); 50 | this->data = u_data.real; 51 | offset += sizeof(this->data); 52 | return offset; 53 | } 54 | 55 | const char * getType(){ return "std_msgs/Int32"; }; 56 | const char * getMD5(){ return "da5909fbe378aeaf85e547e830cc1bb7"; }; 57 | 58 | }; 59 | 60 | } 61 | #endif -------------------------------------------------------------------------------- /momobot_ws/src/momobot/teensy/firmware/lib/ros_lib/std_msgs/Float32.h: -------------------------------------------------------------------------------- 1 | #ifndef _ROS_std_msgs_Float32_h 2 | #define _ROS_std_msgs_Float32_h 3 | 4 | #include 5 | #include 6 | #include 7 | #include "ros/msg.h" 8 | 9 | namespace std_msgs 10 | { 11 | 12 | class Float32 : public ros::Msg 13 | { 14 | public: 15 | float data; 16 | 17 | Float32(): 18 | data(0) 19 | { 20 | } 21 | 22 | virtual int serialize(unsigned char *outbuffer) const 23 | { 24 | int offset = 0; 25 | union { 26 | float real; 27 | uint32_t base; 28 | } u_data; 29 | u_data.real = this->data; 30 | *(outbuffer + offset + 0) = (u_data.base >> (8 * 0)) & 0xFF; 31 | *(outbuffer + offset + 1) = (u_data.base >> (8 * 1)) & 0xFF; 32 | *(outbuffer + offset + 2) = (u_data.base >> (8 * 2)) & 0xFF; 33 | *(outbuffer + offset + 3) = (u_data.base >> (8 * 3)) & 0xFF; 34 | offset += sizeof(this->data); 35 | return offset; 36 | } 37 | 38 | virtual int deserialize(unsigned char *inbuffer) 39 | { 40 | int offset = 0; 41 | union { 42 | float real; 43 | uint32_t base; 44 | } u_data; 45 | u_data.base = 0; 46 | u_data.base |= ((uint32_t) (*(inbuffer + offset + 0))) << (8 * 0); 47 | u_data.base |= ((uint32_t) (*(inbuffer + offset + 1))) << (8 * 1); 48 | u_data.base |= ((uint32_t) (*(inbuffer + offset + 2))) << (8 * 2); 49 | u_data.base |= ((uint32_t) (*(inbuffer + offset + 3))) << (8 * 3); 50 | this->data = u_data.real; 51 | offset += sizeof(this->data); 52 | return offset; 53 | } 54 | 55 | const char * getType(){ return "std_msgs/Float32"; }; 56 | const char * getMD5(){ return "73fcbf46b49191e672908e50842a83d4"; }; 57 | 58 | }; 59 | 60 | } 61 | #endif -------------------------------------------------------------------------------- /momobot_ws/src/semantic_pose_sounder/config/config.yaml: -------------------------------------------------------------------------------- 1 | ## Semantic Pose Setup ## 2 | 3 | base_frame_id: "base_footprint" 4 | map_frame_id: "map" 5 | 6 | publish_rate: 1 7 | 8 | ## Semantic Pose Sounder Setup ## 9 | 10 | playback_delay: 1 # Minimum seconds to wait between playbacks 11 | 12 | ## Location Zone Setup ## (Default topic is /location) 13 | 14 | places: 15 | - name: "mrt" 16 | boundary: [[-42.3269309998, -9.64726257324], [-27.2339553833, -0.137819603086], [-19.3856124878, -18.0246276855]] 17 | 18 | - name: "crooked_cooks" 19 | boundary: [[-25.6212444305, 24.2674217224], [-38.3735313416, 5.47848701477], [-28.5244960785, 1.72261142731], [-16.6392326355, 20.9231834412]] 20 | 21 | - name: "gom_gom" 22 | boundary: [[8.68024921417, -8.78165817261], [9.07768154144, -13.543961525], [23.7762451172, -12.3618183136], [24.1685733795, -9.0656337738]] 23 | 24 | - name: "lt_1" 25 | boundary: [[-6.95016288757, -48.0339584351], [-7.33593225479, -85.0364532471], [12.3969259262, -85.5298843384], [13.1223640442, -47.8884277344]] 26 | 27 | - name: "mont_calzone" 28 | boundary: [[14.925494194, -30.8228912354], [14.6924104691, -40.3478050232], [38.6967544556, -43.9386787415], [39.4649734497, -33.204410553]] 29 | 30 | - name: "library" 31 | boundary: [[55.0568695068, -32.930606842], [56.4948539734, -56.3395729065], [82.8936004639, -47.6475486755]] 32 | 33 | - name: "campus_centre" 34 | boundary: [[50.3807373047, -12.2976636887], [69.9550628662, 21.7182559967], [123.884155273, 1.96545815468], [86.3303527832, -37.6790657043]] 35 | 36 | 37 | ## Link Sounds to Zones ## 38 | 39 | trigger_mappings: {"mrt": "mrt.mp3", "crooked_cooks": "crooked_cooks.mp3", "gom_gom": "gom_gom.mp3", "lt_1": "lt_1.mp3", "mont_calzone": "mont_calzone.mp3", "library": "library.mp3", "campus_centre": "campus_centre.mp3"} 40 | -------------------------------------------------------------------------------- /momobot_ws/src/momobot/teensy_bakbak/firmware/lib/ros_lib/std_msgs/Int32.h: -------------------------------------------------------------------------------- 1 | #ifndef _ROS_std_msgs_Int32_h 2 | #define _ROS_std_msgs_Int32_h 3 | 4 | #include 5 | #include 6 | #include 7 | #include "ros/msg.h" 8 | 9 | namespace std_msgs 10 | { 11 | 12 | class Int32 : public ros::Msg 13 | { 14 | public: 15 | int32_t data; 16 | 17 | Int32(): 18 | data(0) 19 | { 20 | } 21 | 22 | virtual int serialize(unsigned char *outbuffer) const 23 | { 24 | int offset = 0; 25 | union { 26 | int32_t real; 27 | uint32_t base; 28 | } u_data; 29 | u_data.real = this->data; 30 | *(outbuffer + offset + 0) = (u_data.base >> (8 * 0)) & 0xFF; 31 | *(outbuffer + offset + 1) = (u_data.base >> (8 * 1)) & 0xFF; 32 | *(outbuffer + offset + 2) = (u_data.base >> (8 * 2)) & 0xFF; 33 | *(outbuffer + offset + 3) = (u_data.base >> (8 * 3)) & 0xFF; 34 | offset += sizeof(this->data); 35 | return offset; 36 | } 37 | 38 | virtual int deserialize(unsigned char *inbuffer) 39 | { 40 | int offset = 0; 41 | union { 42 | int32_t real; 43 | uint32_t base; 44 | } u_data; 45 | u_data.base = 0; 46 | u_data.base |= ((uint32_t) (*(inbuffer + offset + 0))) << (8 * 0); 47 | u_data.base |= ((uint32_t) (*(inbuffer + offset + 1))) << (8 * 1); 48 | u_data.base |= ((uint32_t) (*(inbuffer + offset + 2))) << (8 * 2); 49 | u_data.base |= ((uint32_t) (*(inbuffer + offset + 3))) << (8 * 3); 50 | this->data = u_data.real; 51 | offset += sizeof(this->data); 52 | return offset; 53 | } 54 | 55 | const char * getType(){ return "std_msgs/Int32"; }; 56 | const char * getMD5(){ return "da5909fbe378aeaf85e547e830cc1bb7"; }; 57 | 58 | }; 59 | 60 | } 61 | #endif -------------------------------------------------------------------------------- /momobot_ws/src/momobot/teensy_bakbak/firmware/lib/ros_lib/std_msgs/Float32.h: -------------------------------------------------------------------------------- 1 | #ifndef _ROS_std_msgs_Float32_h 2 | #define _ROS_std_msgs_Float32_h 3 | 4 | #include 5 | #include 6 | #include 7 | #include "ros/msg.h" 8 | 9 | namespace std_msgs 10 | { 11 | 12 | class Float32 : public ros::Msg 13 | { 14 | public: 15 | float data; 16 | 17 | Float32(): 18 | data(0) 19 | { 20 | } 21 | 22 | virtual int serialize(unsigned char *outbuffer) const 23 | { 24 | int offset = 0; 25 | union { 26 | float real; 27 | uint32_t base; 28 | } u_data; 29 | u_data.real = this->data; 30 | *(outbuffer + offset + 0) = (u_data.base >> (8 * 0)) & 0xFF; 31 | *(outbuffer + offset + 1) = (u_data.base >> (8 * 1)) & 0xFF; 32 | *(outbuffer + offset + 2) = (u_data.base >> (8 * 2)) & 0xFF; 33 | *(outbuffer + offset + 3) = (u_data.base >> (8 * 3)) & 0xFF; 34 | offset += sizeof(this->data); 35 | return offset; 36 | } 37 | 38 | virtual int deserialize(unsigned char *inbuffer) 39 | { 40 | int offset = 0; 41 | union { 42 | float real; 43 | uint32_t base; 44 | } u_data; 45 | u_data.base = 0; 46 | u_data.base |= ((uint32_t) (*(inbuffer + offset + 0))) << (8 * 0); 47 | u_data.base |= ((uint32_t) (*(inbuffer + offset + 1))) << (8 * 1); 48 | u_data.base |= ((uint32_t) (*(inbuffer + offset + 2))) << (8 * 2); 49 | u_data.base |= ((uint32_t) (*(inbuffer + offset + 3))) << (8 * 3); 50 | this->data = u_data.real; 51 | offset += sizeof(this->data); 52 | return offset; 53 | } 54 | 55 | const char * getType(){ return "std_msgs/Float32"; }; 56 | const char * getMD5(){ return "73fcbf46b49191e672908e50842a83d4"; }; 57 | 58 | }; 59 | 60 | } 61 | #endif -------------------------------------------------------------------------------- /momobot_ws/src/momobot/teensy/firmware/lib/encoder/examples/NoInterrupts/NoInterrupts.pde: -------------------------------------------------------------------------------- 1 | /* Encoder Library - NoInterrupts Example 2 | * http://www.pjrc.com/teensy/td_libs_Encoder.html 3 | * 4 | * This example code is in the public domain. 5 | */ 6 | 7 | // If you define ENCODER_DO_NOT_USE_INTERRUPTS *before* including 8 | // Encoder, the library will never use interrupts. This is mainly 9 | // useful to reduce the size of the library when you are using it 10 | // with pins that do not support interrupts. Without interrupts, 11 | // your program must call the read() function rapidly, or risk 12 | // missing changes in position. 13 | #define ENCODER_DO_NOT_USE_INTERRUPTS 14 | #include 15 | 16 | // Beware of Serial.print() speed. Without interrupts, if you 17 | // transmit too much data with Serial.print() it can slow your 18 | // reading from Encoder. Arduino 1.0 has improved transmit code. 19 | // Using the fastest baud rate also helps. Teensy has USB packet 20 | // buffering. But all boards can experience problems if you print 21 | // too much and fill up buffers. 22 | 23 | // Change these two numbers to the pins connected to your encoder. 24 | // With ENCODER_DO_NOT_USE_INTERRUPTS, no interrupts are ever 25 | // used, even if the pin has interrupt capability 26 | Encoder myEnc(5, 6); 27 | // avoid using pins with LEDs attached 28 | 29 | void setup() { 30 | Serial.begin(9600); 31 | Serial.println("Basic NoInterrupts Test:"); 32 | } 33 | 34 | long position = -999; 35 | 36 | void loop() { 37 | long newPos = myEnc.read(); 38 | if (newPos != position) { 39 | position = newPos; 40 | Serial.println(position); 41 | } 42 | // With any substantial delay added, Encoder can only track 43 | // very slow motion. You may uncomment this line to see 44 | // how badly a delay affects your encoder. 45 | //delay(50); 46 | } 47 | -------------------------------------------------------------------------------- /momobot_ws/src/momobot/teensy_bakbak/firmware/lib/encoder/examples/NoInterrupts/NoInterrupts.pde: -------------------------------------------------------------------------------- 1 | /* Encoder Library - NoInterrupts Example 2 | * http://www.pjrc.com/teensy/td_libs_Encoder.html 3 | * 4 | * This example code is in the public domain. 5 | */ 6 | 7 | // If you define ENCODER_DO_NOT_USE_INTERRUPTS *before* including 8 | // Encoder, the library will never use interrupts. This is mainly 9 | // useful to reduce the size of the library when you are using it 10 | // with pins that do not support interrupts. Without interrupts, 11 | // your program must call the read() function rapidly, or risk 12 | // missing changes in position. 13 | #define ENCODER_DO_NOT_USE_INTERRUPTS 14 | #include 15 | 16 | // Beware of Serial.print() speed. Without interrupts, if you 17 | // transmit too much data with Serial.print() it can slow your 18 | // reading from Encoder. Arduino 1.0 has improved transmit code. 19 | // Using the fastest baud rate also helps. Teensy has USB packet 20 | // buffering. But all boards can experience problems if you print 21 | // too much and fill up buffers. 22 | 23 | // Change these two numbers to the pins connected to your encoder. 24 | // With ENCODER_DO_NOT_USE_INTERRUPTS, no interrupts are ever 25 | // used, even if the pin has interrupt capability 26 | Encoder myEnc(5, 6); 27 | // avoid using pins with LEDs attached 28 | 29 | void setup() { 30 | Serial.begin(9600); 31 | Serial.println("Basic NoInterrupts Test:"); 32 | } 33 | 34 | long position = -999; 35 | 36 | void loop() { 37 | long newPos = myEnc.read(); 38 | if (newPos != position) { 39 | position = newPos; 40 | Serial.println(position); 41 | } 42 | // With any substantial delay added, Encoder can only track 43 | // very slow motion. You may uncomment this line to see 44 | // how badly a delay affects your encoder. 45 | //delay(50); 46 | } 47 | -------------------------------------------------------------------------------- /momobot_ws/src/momobot/teensy/firmware/lib/ros_lib/geometry_msgs/Polygon.h: -------------------------------------------------------------------------------- 1 | #ifndef _ROS_geometry_msgs_Polygon_h 2 | #define _ROS_geometry_msgs_Polygon_h 3 | 4 | #include 5 | #include 6 | #include 7 | #include "ros/msg.h" 8 | #include "geometry_msgs/Point32.h" 9 | 10 | namespace geometry_msgs 11 | { 12 | 13 | class Polygon : public ros::Msg 14 | { 15 | public: 16 | uint8_t points_length; 17 | geometry_msgs::Point32 st_points; 18 | geometry_msgs::Point32 * points; 19 | 20 | Polygon(): 21 | points_length(0), points(NULL) 22 | { 23 | } 24 | 25 | virtual int serialize(unsigned char *outbuffer) const 26 | { 27 | int offset = 0; 28 | *(outbuffer + offset++) = points_length; 29 | *(outbuffer + offset++) = 0; 30 | *(outbuffer + offset++) = 0; 31 | *(outbuffer + offset++) = 0; 32 | for( uint8_t i = 0; i < points_length; i++){ 33 | offset += this->points[i].serialize(outbuffer + offset); 34 | } 35 | return offset; 36 | } 37 | 38 | virtual int deserialize(unsigned char *inbuffer) 39 | { 40 | int offset = 0; 41 | uint8_t points_lengthT = *(inbuffer + offset++); 42 | if(points_lengthT > points_length) 43 | this->points = (geometry_msgs::Point32*)realloc(this->points, points_lengthT * sizeof(geometry_msgs::Point32)); 44 | offset += 3; 45 | points_length = points_lengthT; 46 | for( uint8_t i = 0; i < points_length; i++){ 47 | offset += this->st_points.deserialize(inbuffer + offset); 48 | memcpy( &(this->points[i]), &(this->st_points), sizeof(geometry_msgs::Point32)); 49 | } 50 | return offset; 51 | } 52 | 53 | const char * getType(){ return "geometry_msgs/Polygon"; }; 54 | const char * getMD5(){ return "cd60a26494a087f577976f0329fa120e"; }; 55 | 56 | }; 57 | 58 | } 59 | #endif -------------------------------------------------------------------------------- /momobot_ws/src/momobot/teensy_bakbak/firmware/lib/ros_lib/geometry_msgs/Polygon.h: -------------------------------------------------------------------------------- 1 | #ifndef _ROS_geometry_msgs_Polygon_h 2 | #define _ROS_geometry_msgs_Polygon_h 3 | 4 | #include 5 | #include 6 | #include 7 | #include "ros/msg.h" 8 | #include "geometry_msgs/Point32.h" 9 | 10 | namespace geometry_msgs 11 | { 12 | 13 | class Polygon : public ros::Msg 14 | { 15 | public: 16 | uint8_t points_length; 17 | geometry_msgs::Point32 st_points; 18 | geometry_msgs::Point32 * points; 19 | 20 | Polygon(): 21 | points_length(0), points(NULL) 22 | { 23 | } 24 | 25 | virtual int serialize(unsigned char *outbuffer) const 26 | { 27 | int offset = 0; 28 | *(outbuffer + offset++) = points_length; 29 | *(outbuffer + offset++) = 0; 30 | *(outbuffer + offset++) = 0; 31 | *(outbuffer + offset++) = 0; 32 | for( uint8_t i = 0; i < points_length; i++){ 33 | offset += this->points[i].serialize(outbuffer + offset); 34 | } 35 | return offset; 36 | } 37 | 38 | virtual int deserialize(unsigned char *inbuffer) 39 | { 40 | int offset = 0; 41 | uint8_t points_lengthT = *(inbuffer + offset++); 42 | if(points_lengthT > points_length) 43 | this->points = (geometry_msgs::Point32*)realloc(this->points, points_lengthT * sizeof(geometry_msgs::Point32)); 44 | offset += 3; 45 | points_length = points_lengthT; 46 | for( uint8_t i = 0; i < points_length; i++){ 47 | offset += this->st_points.deserialize(inbuffer + offset); 48 | memcpy( &(this->points[i]), &(this->st_points), sizeof(geometry_msgs::Point32)); 49 | } 50 | return offset; 51 | } 52 | 53 | const char * getType(){ return "geometry_msgs/Polygon"; }; 54 | const char * getMD5(){ return "cd60a26494a087f577976f0329fa120e"; }; 55 | 56 | }; 57 | 58 | } 59 | #endif -------------------------------------------------------------------------------- /momobot_ws/src/momobot/teensy/firmware/lib/imu/Imu.h: -------------------------------------------------------------------------------- 1 | #ifndef _IMU2_H_ 2 | #define _IMU2_H_ 3 | 4 | #include "I2Cdev.h" 5 | #include "imu_config.h" 6 | 7 | #include 8 | #include "geometry_msgs/Vector3.h" 9 | 10 | bool initIMU() 11 | { 12 | Wire.begin(); 13 | bool ret; 14 | 15 | accelerometer.initialize(); 16 | ret = accelerometer.testConnection(); 17 | if(!ret) 18 | return false; 19 | 20 | gyroscope.initialize(); 21 | ret = gyroscope.testConnection(); 22 | if(!ret) 23 | return false; 24 | 25 | magnetometer.initialize(); 26 | ret = magnetometer.testConnection(); 27 | if(!ret) 28 | return false; 29 | 30 | return true; 31 | } 32 | 33 | geometry_msgs::Vector3 readAccelerometer() 34 | { 35 | geometry_msgs::Vector3 accel; 36 | int16_t ax, ay, az; 37 | 38 | accelerometer.getAcceleration(&ax, &ay, &az); 39 | 40 | accel.x = ax * (double) ACCEL_SCALE * G_TO_ACCEL; 41 | accel.y = ay * (double) ACCEL_SCALE * G_TO_ACCEL; 42 | accel.z = az * (double) ACCEL_SCALE * G_TO_ACCEL; 43 | 44 | return accel; 45 | } 46 | 47 | geometry_msgs::Vector3 readGyroscope() 48 | { 49 | geometry_msgs::Vector3 gyro; 50 | int16_t gx, gy, gz; 51 | 52 | gyroscope.getRotation(&gx, &gy, &gz); 53 | 54 | gyro.x = gx * (double) GYRO_SCALE * DEG_TO_RAD; 55 | gyro.y = gy * (double) GYRO_SCALE * DEG_TO_RAD; 56 | gyro.z = gz * (double) GYRO_SCALE * DEG_TO_RAD; 57 | 58 | return gyro; 59 | } 60 | 61 | geometry_msgs::Vector3 readMagnetometer() 62 | { 63 | geometry_msgs::Vector3 mag; 64 | int16_t mx, my, mz; 65 | 66 | magnetometer.getHeading(&mx, &my, &mz); 67 | 68 | mag.x = mx * (double) MAG_SCALE * UTESLA_TO_TESLA; 69 | mag.y = my * (double) MAG_SCALE * UTESLA_TO_TESLA; 70 | mag.z = mz * (double) MAG_SCALE * UTESLA_TO_TESLA; 71 | 72 | return mag; 73 | } 74 | 75 | #endif -------------------------------------------------------------------------------- /momobot_ws/src/momobot/teensy_bakbak/firmware/lib/imu/Imu.h: -------------------------------------------------------------------------------- 1 | #ifndef _IMU2_H_ 2 | #define _IMU2_H_ 3 | 4 | #include "I2Cdev.h" 5 | #include "imu_config.h" 6 | 7 | #include 8 | #include "geometry_msgs/Vector3.h" 9 | 10 | bool initIMU() 11 | { 12 | Wire.begin(); 13 | bool ret; 14 | 15 | accelerometer.initialize(); 16 | ret = accelerometer.testConnection(); 17 | if(!ret) 18 | return false; 19 | 20 | gyroscope.initialize(); 21 | ret = gyroscope.testConnection(); 22 | if(!ret) 23 | return false; 24 | 25 | magnetometer.initialize(); 26 | ret = magnetometer.testConnection(); 27 | if(!ret) 28 | return false; 29 | 30 | return true; 31 | } 32 | 33 | geometry_msgs::Vector3 readAccelerometer() 34 | { 35 | geometry_msgs::Vector3 accel; 36 | int16_t ax, ay, az; 37 | 38 | accelerometer.getAcceleration(&ax, &ay, &az); 39 | 40 | accel.x = ax * (double) ACCEL_SCALE * G_TO_ACCEL; 41 | accel.y = ay * (double) ACCEL_SCALE * G_TO_ACCEL; 42 | accel.z = az * (double) ACCEL_SCALE * G_TO_ACCEL; 43 | 44 | return accel; 45 | } 46 | 47 | geometry_msgs::Vector3 readGyroscope() 48 | { 49 | geometry_msgs::Vector3 gyro; 50 | int16_t gx, gy, gz; 51 | 52 | gyroscope.getRotation(&gx, &gy, &gz); 53 | 54 | gyro.x = gx * (double) GYRO_SCALE * DEG_TO_RAD; 55 | gyro.y = gy * (double) GYRO_SCALE * DEG_TO_RAD; 56 | gyro.z = gz * (double) GYRO_SCALE * DEG_TO_RAD; 57 | 58 | return gyro; 59 | } 60 | 61 | geometry_msgs::Vector3 readMagnetometer() 62 | { 63 | geometry_msgs::Vector3 mag; 64 | int16_t mx, my, mz; 65 | 66 | magnetometer.getHeading(&mx, &my, &mz); 67 | 68 | mag.x = mx * (double) MAG_SCALE * UTESLA_TO_TESLA; 69 | mag.y = my * (double) MAG_SCALE * UTESLA_TO_TESLA; 70 | mag.z = mz * (double) MAG_SCALE * UTESLA_TO_TESLA; 71 | 72 | return mag; 73 | } 74 | 75 | #endif -------------------------------------------------------------------------------- /momobot_ws/src/momobot/teensy/firmware/lib/ros_lib/rosserial_msgs/Log.h: -------------------------------------------------------------------------------- 1 | #ifndef _ROS_rosserial_msgs_Log_h 2 | #define _ROS_rosserial_msgs_Log_h 3 | 4 | #include 5 | #include 6 | #include 7 | #include "ros/msg.h" 8 | 9 | namespace rosserial_msgs 10 | { 11 | 12 | class Log : public ros::Msg 13 | { 14 | public: 15 | uint8_t level; 16 | const char* msg; 17 | enum { ROSDEBUG = 0 }; 18 | enum { INFO = 1 }; 19 | enum { WARN = 2 }; 20 | enum { ERROR = 3 }; 21 | enum { FATAL = 4 }; 22 | 23 | Log(): 24 | level(0), 25 | msg("") 26 | { 27 | } 28 | 29 | virtual int serialize(unsigned char *outbuffer) const 30 | { 31 | int offset = 0; 32 | *(outbuffer + offset + 0) = (this->level >> (8 * 0)) & 0xFF; 33 | offset += sizeof(this->level); 34 | uint32_t length_msg = strlen(this->msg); 35 | memcpy(outbuffer + offset, &length_msg, sizeof(uint32_t)); 36 | offset += 4; 37 | memcpy(outbuffer + offset, this->msg, length_msg); 38 | offset += length_msg; 39 | return offset; 40 | } 41 | 42 | virtual int deserialize(unsigned char *inbuffer) 43 | { 44 | int offset = 0; 45 | this->level = ((uint8_t) (*(inbuffer + offset))); 46 | offset += sizeof(this->level); 47 | uint32_t length_msg; 48 | memcpy(&length_msg, (inbuffer + offset), sizeof(uint32_t)); 49 | offset += 4; 50 | for(unsigned int k= offset; k< offset+length_msg; ++k){ 51 | inbuffer[k-1]=inbuffer[k]; 52 | } 53 | inbuffer[offset+length_msg-1]=0; 54 | this->msg = (char *)(inbuffer + offset-1); 55 | offset += length_msg; 56 | return offset; 57 | } 58 | 59 | const char * getType(){ return "rosserial_msgs/Log"; }; 60 | const char * getMD5(){ return "11abd731c25933261cd6183bd12d6295"; }; 61 | 62 | }; 63 | 64 | } 65 | #endif -------------------------------------------------------------------------------- /momobot_ws/src/momobot/teensy_bakbak/firmware/lib/ros_lib/rosserial_msgs/Log.h: -------------------------------------------------------------------------------- 1 | #ifndef _ROS_rosserial_msgs_Log_h 2 | #define _ROS_rosserial_msgs_Log_h 3 | 4 | #include 5 | #include 6 | #include 7 | #include "ros/msg.h" 8 | 9 | namespace rosserial_msgs 10 | { 11 | 12 | class Log : public ros::Msg 13 | { 14 | public: 15 | uint8_t level; 16 | const char* msg; 17 | enum { ROSDEBUG = 0 }; 18 | enum { INFO = 1 }; 19 | enum { WARN = 2 }; 20 | enum { ERROR = 3 }; 21 | enum { FATAL = 4 }; 22 | 23 | Log(): 24 | level(0), 25 | msg("") 26 | { 27 | } 28 | 29 | virtual int serialize(unsigned char *outbuffer) const 30 | { 31 | int offset = 0; 32 | *(outbuffer + offset + 0) = (this->level >> (8 * 0)) & 0xFF; 33 | offset += sizeof(this->level); 34 | uint32_t length_msg = strlen(this->msg); 35 | memcpy(outbuffer + offset, &length_msg, sizeof(uint32_t)); 36 | offset += 4; 37 | memcpy(outbuffer + offset, this->msg, length_msg); 38 | offset += length_msg; 39 | return offset; 40 | } 41 | 42 | virtual int deserialize(unsigned char *inbuffer) 43 | { 44 | int offset = 0; 45 | this->level = ((uint8_t) (*(inbuffer + offset))); 46 | offset += sizeof(this->level); 47 | uint32_t length_msg; 48 | memcpy(&length_msg, (inbuffer + offset), sizeof(uint32_t)); 49 | offset += 4; 50 | for(unsigned int k= offset; k< offset+length_msg; ++k){ 51 | inbuffer[k-1]=inbuffer[k]; 52 | } 53 | inbuffer[offset+length_msg-1]=0; 54 | this->msg = (char *)(inbuffer + offset-1); 55 | offset += length_msg; 56 | return offset; 57 | } 58 | 59 | const char * getType(){ return "rosserial_msgs/Log"; }; 60 | const char * getMD5(){ return "11abd731c25933261cd6183bd12d6295"; }; 61 | 62 | }; 63 | 64 | } 65 | #endif -------------------------------------------------------------------------------- /momobot_ws/src/semantic_pose/src/semantic_pose_node.cpp: -------------------------------------------------------------------------------- 1 | 2 | #include 3 | #include 4 | #include 5 | 6 | #include "places.h" 7 | 8 | int main(int argc, char * * argv) 9 | { 10 | ros::init(argc, argv, "semantic_pose"); 11 | 12 | ros::NodeHandle nh; 13 | ros::NodeHandle nh_priv("~"); 14 | 15 | std::string base_frame_id; 16 | std::string map_frame_id; 17 | float publish_rate; 18 | 19 | nh_priv.param("base_frame_id", base_frame_id, "base_link"); 20 | nh_priv.param("map_frame_id", map_frame_id, "map"); 21 | nh_priv.param("publish_rate", publish_rate, 10.0); 22 | 23 | ROS_INFO("base_frame_id: %s", base_frame_id.c_str()); 24 | ROS_INFO("map_frame_id: %s", map_frame_id.c_str()); 25 | ROS_INFO("publish_rate: %f", publish_rate); 26 | 27 | ros::Publisher location_pub = nh.advertise < std_msgs::String > ("location", 10); 28 | 29 | tf::TransformListener listener; 30 | 31 | Places places(nh, nh, "places"); 32 | 33 | ros::Rate rate(publish_rate); 34 | 35 | while (nh.ok()) 36 | { 37 | tf::StampedTransform transform; 38 | ros::Time t = ros::Time(0); 39 | 40 | try 41 | { 42 | listener.waitForTransform(map_frame_id, base_frame_id, t, ros::Duration(10.0)); 43 | listener.lookupTransform(map_frame_id, base_frame_id, t, transform); 44 | } 45 | catch (tf::TransformException ex) 46 | { 47 | ROS_ERROR("%s", ex.what()); 48 | } 49 | 50 | geometry_msgs::Point pt; 51 | 52 | pt.x = transform.getOrigin().getX(); 53 | pt.y = transform.getOrigin().getY(); 54 | 55 | std_msgs::String loc_msg; 56 | loc_msg.data = places.where_am_i(pt); 57 | location_pub.publish(loc_msg); 58 | 59 | rate.sleep(); 60 | } 61 | return 0; 62 | }; -------------------------------------------------------------------------------- /momobot_ws/src/momobot/launch/slam.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | 16 | 17 | 18 | 19 | 20 | 21 | 22 | 23 | 24 | 25 | 26 | 27 | 28 | 29 | 30 | 31 | 32 | 33 | 34 | 35 | 36 | 37 | 38 | 39 | 40 | -------------------------------------------------------------------------------- /momobot_ws/src/momobot/teensy/firmware/lib/ros_lib/sensor_msgs/JoyFeedbackArray.h: -------------------------------------------------------------------------------- 1 | #ifndef _ROS_sensor_msgs_JoyFeedbackArray_h 2 | #define _ROS_sensor_msgs_JoyFeedbackArray_h 3 | 4 | #include 5 | #include 6 | #include 7 | #include "ros/msg.h" 8 | #include "sensor_msgs/JoyFeedback.h" 9 | 10 | namespace sensor_msgs 11 | { 12 | 13 | class JoyFeedbackArray : public ros::Msg 14 | { 15 | public: 16 | uint8_t array_length; 17 | sensor_msgs::JoyFeedback st_array; 18 | sensor_msgs::JoyFeedback * array; 19 | 20 | JoyFeedbackArray(): 21 | array_length(0), array(NULL) 22 | { 23 | } 24 | 25 | virtual int serialize(unsigned char *outbuffer) const 26 | { 27 | int offset = 0; 28 | *(outbuffer + offset++) = array_length; 29 | *(outbuffer + offset++) = 0; 30 | *(outbuffer + offset++) = 0; 31 | *(outbuffer + offset++) = 0; 32 | for( uint8_t i = 0; i < array_length; i++){ 33 | offset += this->array[i].serialize(outbuffer + offset); 34 | } 35 | return offset; 36 | } 37 | 38 | virtual int deserialize(unsigned char *inbuffer) 39 | { 40 | int offset = 0; 41 | uint8_t array_lengthT = *(inbuffer + offset++); 42 | if(array_lengthT > array_length) 43 | this->array = (sensor_msgs::JoyFeedback*)realloc(this->array, array_lengthT * sizeof(sensor_msgs::JoyFeedback)); 44 | offset += 3; 45 | array_length = array_lengthT; 46 | for( uint8_t i = 0; i < array_length; i++){ 47 | offset += this->st_array.deserialize(inbuffer + offset); 48 | memcpy( &(this->array[i]), &(this->st_array), sizeof(sensor_msgs::JoyFeedback)); 49 | } 50 | return offset; 51 | } 52 | 53 | const char * getType(){ return "sensor_msgs/JoyFeedbackArray"; }; 54 | const char * getMD5(){ return "cde5730a895b1fc4dee6f91b754b213d"; }; 55 | 56 | }; 57 | 58 | } 59 | #endif -------------------------------------------------------------------------------- /momobot_ws/src/momobot/teensy_bakbak/firmware/lib/ros_lib/sensor_msgs/JoyFeedbackArray.h: -------------------------------------------------------------------------------- 1 | #ifndef _ROS_sensor_msgs_JoyFeedbackArray_h 2 | #define _ROS_sensor_msgs_JoyFeedbackArray_h 3 | 4 | #include 5 | #include 6 | #include 7 | #include "ros/msg.h" 8 | #include "sensor_msgs/JoyFeedback.h" 9 | 10 | namespace sensor_msgs 11 | { 12 | 13 | class JoyFeedbackArray : public ros::Msg 14 | { 15 | public: 16 | uint8_t array_length; 17 | sensor_msgs::JoyFeedback st_array; 18 | sensor_msgs::JoyFeedback * array; 19 | 20 | JoyFeedbackArray(): 21 | array_length(0), array(NULL) 22 | { 23 | } 24 | 25 | virtual int serialize(unsigned char *outbuffer) const 26 | { 27 | int offset = 0; 28 | *(outbuffer + offset++) = array_length; 29 | *(outbuffer + offset++) = 0; 30 | *(outbuffer + offset++) = 0; 31 | *(outbuffer + offset++) = 0; 32 | for( uint8_t i = 0; i < array_length; i++){ 33 | offset += this->array[i].serialize(outbuffer + offset); 34 | } 35 | return offset; 36 | } 37 | 38 | virtual int deserialize(unsigned char *inbuffer) 39 | { 40 | int offset = 0; 41 | uint8_t array_lengthT = *(inbuffer + offset++); 42 | if(array_lengthT > array_length) 43 | this->array = (sensor_msgs::JoyFeedback*)realloc(this->array, array_lengthT * sizeof(sensor_msgs::JoyFeedback)); 44 | offset += 3; 45 | array_length = array_lengthT; 46 | for( uint8_t i = 0; i < array_length; i++){ 47 | offset += this->st_array.deserialize(inbuffer + offset); 48 | memcpy( &(this->array[i]), &(this->st_array), sizeof(sensor_msgs::JoyFeedback)); 49 | } 50 | return offset; 51 | } 52 | 53 | const char * getType(){ return "sensor_msgs/JoyFeedbackArray"; }; 54 | const char * getMD5(){ return "cde5730a895b1fc4dee6f91b754b213d"; }; 55 | 56 | }; 57 | 58 | } 59 | #endif -------------------------------------------------------------------------------- /momobot_ws/src/momobot/teensy/firmware/lib/motor/test.ino: -------------------------------------------------------------------------------- 1 | /* 2 | Copyright (c) 2016, Juan Jimeno 3 | All rights reserved. 4 | Redistribution and use in source and binary forms, with or without 5 | modification, are permitted provided that the following conditions are met: 6 | * Redistributions of source code must retain the above copyright notice, 7 | this list of conditions and the following disclaimer. 8 | * Redistributions in binary form must reproduce the above copyright 9 | notice, this list of conditions and the following disclaimer in the 10 | documentation and/or other materials provided with the distribution. 11 | * Neither the name of nor the names of its contributors may be used to 12 | endorse or promote products derived from this software without specific 13 | prior written permission. 14 | THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" 15 | AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE 16 | IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE 17 | ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE 18 | LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR 19 | CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF 20 | SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS 21 | INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN 22 | CONTRACT, STRICT LIABILITY, OR TORTPPIPI (INCLUDING NEGLIGENCE OR OTHERWISE) 23 | ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE 24 | POSSIBILITY OF SUCH DAMAGE. 25 | */ 26 | 27 | #include "Motor.h" 28 | Motor motor1(12,12); 29 | int Motor::counts_per_rev_ = 0; 30 | long ticks = 0; 31 | 32 | void setup() { 33 | Serial.begin(9600); 34 | // Motor::initEncoder(10); 35 | } 36 | 37 | void loop() { 38 | motor1.updateSpeed(ticks); 39 | Serial.println(motor1.current_rpm); 40 | ticks = ticks + 20; 41 | delay(1000); 42 | 43 | } 44 | -------------------------------------------------------------------------------- /momobot_ws/src/momobot/teensy_bakbak/firmware/lib/motor/test.ino: -------------------------------------------------------------------------------- 1 | /* 2 | Copyright (c) 2016, Juan Jimeno 3 | All rights reserved. 4 | Redistribution and use in source and binary forms, with or without 5 | modification, are permitted provided that the following conditions are met: 6 | * Redistributions of source code must retain the above copyright notice, 7 | this list of conditions and the following disclaimer. 8 | * Redistributions in binary form must reproduce the above copyright 9 | notice, this list of conditions and the following disclaimer in the 10 | documentation and/or other materials provided with the distribution. 11 | * Neither the name of nor the names of its contributors may be used to 12 | endorse or promote products derived from this software without specific 13 | prior written permission. 14 | THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" 15 | AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE 16 | IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE 17 | ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE 18 | LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR 19 | CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF 20 | SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS 21 | INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN 22 | CONTRACT, STRICT LIABILITY, OR TORTPPIPI (INCLUDING NEGLIGENCE OR OTHERWISE) 23 | ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE 24 | POSSIBILITY OF SUCH DAMAGE. 25 | */ 26 | 27 | #include "Motor.h" 28 | Motor motor1(12,12); 29 | int Motor::counts_per_rev_ = 0; 30 | long ticks = 0; 31 | 32 | void setup() { 33 | Serial.begin(9600); 34 | // Motor::initEncoder(10); 35 | } 36 | 37 | void loop() { 38 | motor1.updateSpeed(ticks); 39 | Serial.println(motor1.current_rpm); 40 | ticks = ticks + 20; 41 | delay(1000); 42 | 43 | } 44 | -------------------------------------------------------------------------------- /momobot_ws/src/momobot/teensy/firmware/lib/ros_lib/geometry_msgs/PoseArray.h: -------------------------------------------------------------------------------- 1 | #ifndef _ROS_geometry_msgs_PoseArray_h 2 | #define _ROS_geometry_msgs_PoseArray_h 3 | 4 | #include 5 | #include 6 | #include 7 | #include "ros/msg.h" 8 | #include "std_msgs/Header.h" 9 | #include "geometry_msgs/Pose.h" 10 | 11 | namespace geometry_msgs 12 | { 13 | 14 | class PoseArray : public ros::Msg 15 | { 16 | public: 17 | std_msgs::Header header; 18 | uint8_t poses_length; 19 | geometry_msgs::Pose st_poses; 20 | geometry_msgs::Pose * poses; 21 | 22 | PoseArray(): 23 | header(), 24 | poses_length(0), poses(NULL) 25 | { 26 | } 27 | 28 | virtual int serialize(unsigned char *outbuffer) const 29 | { 30 | int offset = 0; 31 | offset += this->header.serialize(outbuffer + offset); 32 | *(outbuffer + offset++) = poses_length; 33 | *(outbuffer + offset++) = 0; 34 | *(outbuffer + offset++) = 0; 35 | *(outbuffer + offset++) = 0; 36 | for( uint8_t i = 0; i < poses_length; i++){ 37 | offset += this->poses[i].serialize(outbuffer + offset); 38 | } 39 | return offset; 40 | } 41 | 42 | virtual int deserialize(unsigned char *inbuffer) 43 | { 44 | int offset = 0; 45 | offset += this->header.deserialize(inbuffer + offset); 46 | uint8_t poses_lengthT = *(inbuffer + offset++); 47 | if(poses_lengthT > poses_length) 48 | this->poses = (geometry_msgs::Pose*)realloc(this->poses, poses_lengthT * sizeof(geometry_msgs::Pose)); 49 | offset += 3; 50 | poses_length = poses_lengthT; 51 | for( uint8_t i = 0; i < poses_length; i++){ 52 | offset += this->st_poses.deserialize(inbuffer + offset); 53 | memcpy( &(this->poses[i]), &(this->st_poses), sizeof(geometry_msgs::Pose)); 54 | } 55 | return offset; 56 | } 57 | 58 | const char * getType(){ return "geometry_msgs/PoseArray"; }; 59 | const char * getMD5(){ return "916c28c5764443f268b296bb671b9d97"; }; 60 | 61 | }; 62 | 63 | } 64 | #endif -------------------------------------------------------------------------------- /momobot_ws/src/momobot/teensy/firmware/lib/ros_lib/std_msgs/UInt8MultiArray.h: -------------------------------------------------------------------------------- 1 | #ifndef _ROS_std_msgs_UInt8MultiArray_h 2 | #define _ROS_std_msgs_UInt8MultiArray_h 3 | 4 | #include 5 | #include 6 | #include 7 | #include "ros/msg.h" 8 | #include "std_msgs/MultiArrayLayout.h" 9 | 10 | namespace std_msgs 11 | { 12 | 13 | class UInt8MultiArray : public ros::Msg 14 | { 15 | public: 16 | std_msgs::MultiArrayLayout layout; 17 | uint8_t data_length; 18 | uint8_t st_data; 19 | uint8_t * data; 20 | 21 | UInt8MultiArray(): 22 | layout(), 23 | data_length(0), data(NULL) 24 | { 25 | } 26 | 27 | virtual int serialize(unsigned char *outbuffer) const 28 | { 29 | int offset = 0; 30 | offset += this->layout.serialize(outbuffer + offset); 31 | *(outbuffer + offset++) = data_length; 32 | *(outbuffer + offset++) = 0; 33 | *(outbuffer + offset++) = 0; 34 | *(outbuffer + offset++) = 0; 35 | for( uint8_t i = 0; i < data_length; i++){ 36 | *(outbuffer + offset + 0) = (this->data[i] >> (8 * 0)) & 0xFF; 37 | offset += sizeof(this->data[i]); 38 | } 39 | return offset; 40 | } 41 | 42 | virtual int deserialize(unsigned char *inbuffer) 43 | { 44 | int offset = 0; 45 | offset += this->layout.deserialize(inbuffer + offset); 46 | uint8_t data_lengthT = *(inbuffer + offset++); 47 | if(data_lengthT > data_length) 48 | this->data = (uint8_t*)realloc(this->data, data_lengthT * sizeof(uint8_t)); 49 | offset += 3; 50 | data_length = data_lengthT; 51 | for( uint8_t i = 0; i < data_length; i++){ 52 | this->st_data = ((uint8_t) (*(inbuffer + offset))); 53 | offset += sizeof(this->st_data); 54 | memcpy( &(this->data[i]), &(this->st_data), sizeof(uint8_t)); 55 | } 56 | return offset; 57 | } 58 | 59 | const char * getType(){ return "std_msgs/UInt8MultiArray"; }; 60 | const char * getMD5(){ return "82373f1612381bb6ee473b5cd6f5d89c"; }; 61 | 62 | }; 63 | 64 | } 65 | #endif -------------------------------------------------------------------------------- /momobot_ws/src/momobot/teensy_bakbak/firmware/lib/ros_lib/geometry_msgs/PoseArray.h: -------------------------------------------------------------------------------- 1 | #ifndef _ROS_geometry_msgs_PoseArray_h 2 | #define _ROS_geometry_msgs_PoseArray_h 3 | 4 | #include 5 | #include 6 | #include 7 | #include "ros/msg.h" 8 | #include "std_msgs/Header.h" 9 | #include "geometry_msgs/Pose.h" 10 | 11 | namespace geometry_msgs 12 | { 13 | 14 | class PoseArray : public ros::Msg 15 | { 16 | public: 17 | std_msgs::Header header; 18 | uint8_t poses_length; 19 | geometry_msgs::Pose st_poses; 20 | geometry_msgs::Pose * poses; 21 | 22 | PoseArray(): 23 | header(), 24 | poses_length(0), poses(NULL) 25 | { 26 | } 27 | 28 | virtual int serialize(unsigned char *outbuffer) const 29 | { 30 | int offset = 0; 31 | offset += this->header.serialize(outbuffer + offset); 32 | *(outbuffer + offset++) = poses_length; 33 | *(outbuffer + offset++) = 0; 34 | *(outbuffer + offset++) = 0; 35 | *(outbuffer + offset++) = 0; 36 | for( uint8_t i = 0; i < poses_length; i++){ 37 | offset += this->poses[i].serialize(outbuffer + offset); 38 | } 39 | return offset; 40 | } 41 | 42 | virtual int deserialize(unsigned char *inbuffer) 43 | { 44 | int offset = 0; 45 | offset += this->header.deserialize(inbuffer + offset); 46 | uint8_t poses_lengthT = *(inbuffer + offset++); 47 | if(poses_lengthT > poses_length) 48 | this->poses = (geometry_msgs::Pose*)realloc(this->poses, poses_lengthT * sizeof(geometry_msgs::Pose)); 49 | offset += 3; 50 | poses_length = poses_lengthT; 51 | for( uint8_t i = 0; i < poses_length; i++){ 52 | offset += this->st_poses.deserialize(inbuffer + offset); 53 | memcpy( &(this->poses[i]), &(this->st_poses), sizeof(geometry_msgs::Pose)); 54 | } 55 | return offset; 56 | } 57 | 58 | const char * getType(){ return "geometry_msgs/PoseArray"; }; 59 | const char * getMD5(){ return "916c28c5764443f268b296bb671b9d97"; }; 60 | 61 | }; 62 | 63 | } 64 | #endif -------------------------------------------------------------------------------- /momobot_ws/src/momobot/teensy_bakbak/firmware/lib/ros_lib/std_msgs/UInt8MultiArray.h: -------------------------------------------------------------------------------- 1 | #ifndef _ROS_std_msgs_UInt8MultiArray_h 2 | #define _ROS_std_msgs_UInt8MultiArray_h 3 | 4 | #include 5 | #include 6 | #include 7 | #include "ros/msg.h" 8 | #include "std_msgs/MultiArrayLayout.h" 9 | 10 | namespace std_msgs 11 | { 12 | 13 | class UInt8MultiArray : public ros::Msg 14 | { 15 | public: 16 | std_msgs::MultiArrayLayout layout; 17 | uint8_t data_length; 18 | uint8_t st_data; 19 | uint8_t * data; 20 | 21 | UInt8MultiArray(): 22 | layout(), 23 | data_length(0), data(NULL) 24 | { 25 | } 26 | 27 | virtual int serialize(unsigned char *outbuffer) const 28 | { 29 | int offset = 0; 30 | offset += this->layout.serialize(outbuffer + offset); 31 | *(outbuffer + offset++) = data_length; 32 | *(outbuffer + offset++) = 0; 33 | *(outbuffer + offset++) = 0; 34 | *(outbuffer + offset++) = 0; 35 | for( uint8_t i = 0; i < data_length; i++){ 36 | *(outbuffer + offset + 0) = (this->data[i] >> (8 * 0)) & 0xFF; 37 | offset += sizeof(this->data[i]); 38 | } 39 | return offset; 40 | } 41 | 42 | virtual int deserialize(unsigned char *inbuffer) 43 | { 44 | int offset = 0; 45 | offset += this->layout.deserialize(inbuffer + offset); 46 | uint8_t data_lengthT = *(inbuffer + offset++); 47 | if(data_lengthT > data_length) 48 | this->data = (uint8_t*)realloc(this->data, data_lengthT * sizeof(uint8_t)); 49 | offset += 3; 50 | data_length = data_lengthT; 51 | for( uint8_t i = 0; i < data_length; i++){ 52 | this->st_data = ((uint8_t) (*(inbuffer + offset))); 53 | offset += sizeof(this->st_data); 54 | memcpy( &(this->data[i]), &(this->st_data), sizeof(uint8_t)); 55 | } 56 | return offset; 57 | } 58 | 59 | const char * getType(){ return "std_msgs/UInt8MultiArray"; }; 60 | const char * getMD5(){ return "82373f1612381bb6ee473b5cd6f5d89c"; }; 61 | 62 | }; 63 | 64 | } 65 | #endif -------------------------------------------------------------------------------- /momobot_ws/src/momobot/teensy/firmware/lib/ros_lib/std_msgs/UInt64.h: -------------------------------------------------------------------------------- 1 | #ifndef _ROS_std_msgs_UInt64_h 2 | #define _ROS_std_msgs_UInt64_h 3 | 4 | #include 5 | #include 6 | #include 7 | #include "ros/msg.h" 8 | 9 | namespace std_msgs 10 | { 11 | 12 | class UInt64 : public ros::Msg 13 | { 14 | public: 15 | uint64_t data; 16 | 17 | UInt64(): 18 | data(0) 19 | { 20 | } 21 | 22 | virtual int serialize(unsigned char *outbuffer) const 23 | { 24 | int offset = 0; 25 | *(outbuffer + offset + 0) = (this->data >> (8 * 0)) & 0xFF; 26 | *(outbuffer + offset + 1) = (this->data >> (8 * 1)) & 0xFF; 27 | *(outbuffer + offset + 2) = (this->data >> (8 * 2)) & 0xFF; 28 | *(outbuffer + offset + 3) = (this->data >> (8 * 3)) & 0xFF; 29 | *(outbuffer + offset + 4) = (this->data >> (8 * 4)) & 0xFF; 30 | *(outbuffer + offset + 5) = (this->data >> (8 * 5)) & 0xFF; 31 | *(outbuffer + offset + 6) = (this->data >> (8 * 6)) & 0xFF; 32 | *(outbuffer + offset + 7) = (this->data >> (8 * 7)) & 0xFF; 33 | offset += sizeof(this->data); 34 | return offset; 35 | } 36 | 37 | virtual int deserialize(unsigned char *inbuffer) 38 | { 39 | int offset = 0; 40 | this->data = ((uint64_t) (*(inbuffer + offset))); 41 | this->data |= ((uint64_t) (*(inbuffer + offset + 1))) << (8 * 1); 42 | this->data |= ((uint64_t) (*(inbuffer + offset + 2))) << (8 * 2); 43 | this->data |= ((uint64_t) (*(inbuffer + offset + 3))) << (8 * 3); 44 | this->data |= ((uint64_t) (*(inbuffer + offset + 4))) << (8 * 4); 45 | this->data |= ((uint64_t) (*(inbuffer + offset + 5))) << (8 * 5); 46 | this->data |= ((uint64_t) (*(inbuffer + offset + 6))) << (8 * 6); 47 | this->data |= ((uint64_t) (*(inbuffer + offset + 7))) << (8 * 7); 48 | offset += sizeof(this->data); 49 | return offset; 50 | } 51 | 52 | const char * getType(){ return "std_msgs/UInt64"; }; 53 | const char * getMD5(){ return "1b2a79973e8bf53d7b53acb71299cb57"; }; 54 | 55 | }; 56 | 57 | } 58 | #endif -------------------------------------------------------------------------------- /momobot_ws/src/momobot/teensy_bakbak/firmware/lib/ros_lib/std_msgs/UInt64.h: -------------------------------------------------------------------------------- 1 | #ifndef _ROS_std_msgs_UInt64_h 2 | #define _ROS_std_msgs_UInt64_h 3 | 4 | #include 5 | #include 6 | #include 7 | #include "ros/msg.h" 8 | 9 | namespace std_msgs 10 | { 11 | 12 | class UInt64 : public ros::Msg 13 | { 14 | public: 15 | uint64_t data; 16 | 17 | UInt64(): 18 | data(0) 19 | { 20 | } 21 | 22 | virtual int serialize(unsigned char *outbuffer) const 23 | { 24 | int offset = 0; 25 | *(outbuffer + offset + 0) = (this->data >> (8 * 0)) & 0xFF; 26 | *(outbuffer + offset + 1) = (this->data >> (8 * 1)) & 0xFF; 27 | *(outbuffer + offset + 2) = (this->data >> (8 * 2)) & 0xFF; 28 | *(outbuffer + offset + 3) = (this->data >> (8 * 3)) & 0xFF; 29 | *(outbuffer + offset + 4) = (this->data >> (8 * 4)) & 0xFF; 30 | *(outbuffer + offset + 5) = (this->data >> (8 * 5)) & 0xFF; 31 | *(outbuffer + offset + 6) = (this->data >> (8 * 6)) & 0xFF; 32 | *(outbuffer + offset + 7) = (this->data >> (8 * 7)) & 0xFF; 33 | offset += sizeof(this->data); 34 | return offset; 35 | } 36 | 37 | virtual int deserialize(unsigned char *inbuffer) 38 | { 39 | int offset = 0; 40 | this->data = ((uint64_t) (*(inbuffer + offset))); 41 | this->data |= ((uint64_t) (*(inbuffer + offset + 1))) << (8 * 1); 42 | this->data |= ((uint64_t) (*(inbuffer + offset + 2))) << (8 * 2); 43 | this->data |= ((uint64_t) (*(inbuffer + offset + 3))) << (8 * 3); 44 | this->data |= ((uint64_t) (*(inbuffer + offset + 4))) << (8 * 4); 45 | this->data |= ((uint64_t) (*(inbuffer + offset + 5))) << (8 * 5); 46 | this->data |= ((uint64_t) (*(inbuffer + offset + 6))) << (8 * 6); 47 | this->data |= ((uint64_t) (*(inbuffer + offset + 7))) << (8 * 7); 48 | offset += sizeof(this->data); 49 | return offset; 50 | } 51 | 52 | const char * getType(){ return "std_msgs/UInt64"; }; 53 | const char * getMD5(){ return "1b2a79973e8bf53d7b53acb71299cb57"; }; 54 | 55 | }; 56 | 57 | } 58 | #endif -------------------------------------------------------------------------------- /momobot_ws/src/momobot/param/navigation_bak/base_local_planner_ackermann_params.yaml: -------------------------------------------------------------------------------- 1 | TebLocalPlannerROS: 2 | 3 | odom_topic: odom 4 | map_frame: /map 5 | 6 | # Trajectory 7 | 8 | teb_autosize: True 9 | dt_ref: 0.3 10 | dt_hysteresis: 0.1 11 | global_plan_overwrite_orientation: True 12 | max_global_plan_lookahead_dist: 3.0 13 | feasibility_check_no_poses: 2 14 | 15 | # Robot 16 | cmd_angle_instead_rotvel: True 17 | max_vel_x: 0.2 18 | max_vel_x_backwards: 0.2 19 | max_vel_theta: 0.3 20 | acc_lim_x: 1.5 21 | acc_lim_theta: 1.1 22 | min_turning_radius: 0.2 23 | wheelbase: 0.27 24 | footprint_model: # types: "point", "circular", "two_circles", "line", "polygon" 25 | type: "line" 26 | line_start: [-0.135, 0.0] # for type "line" 27 | line_end: [0.135, 0.0] # for type "line" 28 | 29 | # GoalTolerance 30 | 31 | xy_goal_tolerance: 0.3 32 | yaw_goal_tolerance: 0.1 33 | free_goal_vel: False 34 | 35 | # Obstacles 36 | 37 | min_obstacle_dist: 0.16 38 | include_costmap_obstacles: True 39 | costmap_obstacles_behind_robot_dist: 0.35 40 | obstacle_poses_affected: 30 41 | costmap_converter_plugin: "" 42 | costmap_converter_spin_thread: True 43 | costmap_converter_rate: 5 44 | 45 | # Optimization 46 | 47 | no_inner_iterations: 5 48 | no_outer_iterations: 4 49 | optimization_activate: True 50 | optimization_verbose: False 51 | penalty_epsilon: 0.1 52 | weight_max_vel_x: 2 53 | weight_max_vel_theta: 1 54 | weight_acc_lim_x: 1 55 | weight_acc_lim_theta: 1 56 | weight_kinematics_nh: 1000 57 | weight_kinematics_forward_drive: 1 58 | weight_kinematics_turning_radius: 1 59 | weight_optimaltime: 1 60 | weight_obstacle: 50 61 | weight_dynamic_obstacle: 10 # not in use yet 62 | alternative_time_cost: False # not in use yet 63 | 64 | # Homotopy Class Planner 65 | 66 | enable_homotopy_class_planning: False 67 | enable_multithreading: False 68 | simple_exploration: False 69 | max_number_classes: 4 70 | roadmap_graph_no_samples: 15 71 | roadmap_graph_area_width: 5 72 | h_signature_prescaler: 0.5 73 | h_signature_threshold: 0.1 74 | obstacle_keypoint_offset: 0.1 75 | obstacle_heading_threshold: 0.45 76 | visualize_hc_graph: False -------------------------------------------------------------------------------- /momobot_ws/src/momo_emotions/src/PIDController.py: -------------------------------------------------------------------------------- 1 | import time 2 | 3 | class PIDController: 4 | def __init__(self, Kp=0.25, Ki=0.0, Kd=0.0, anti_windup=10.0, cmd_freq=0.0): 5 | self.Kp = Kp 6 | self.Ki = Ki 7 | self.Kd = Kd 8 | 9 | # Set max integral correction per timestep 10 | self.anti_windup = anti_windup 11 | 12 | # Set delay between updates (seconds) 13 | self.cmd_freq = cmd_freq 14 | 15 | self.current_time = time.time() 16 | self.prev_time = self.current_time 17 | 18 | self.reset() 19 | 20 | 21 | def reset(self): 22 | self.setpoint = 0.0 23 | 24 | self.p_ = 0.0 25 | self.i_ = 0.0 26 | self.d_ = 0.0 27 | 28 | self.prev_error = 0.0 29 | 30 | def compute(self, setpoint, measured_value): 31 | ''' Compute PID correction wrt. measured_value - setpoint ''' 32 | self.current_time = time.time() 33 | delta_time = self.current_time - self.prev_time 34 | 35 | if delta_time >= self.cmd_freq: 36 | self.setpoint = setpoint 37 | error = self.setpoint - measured_value 38 | 39 | delta_error = error - self.prev_error 40 | self.accumulated_error = error * delta_time 41 | 42 | # Limit the integration to prevent absolutely wrecking yourself 43 | if self.accumulated_error < -self.anti_windup: 44 | self.accumulated_error = -self.anti_windup 45 | if self.accumulated_error > self.anti_windup: 46 | self.accumulated_error = self.anti_windup 47 | 48 | self.i_ = self.i_ + self.accumulated_error 49 | self.d_ = delta_error / delta_time 50 | 51 | self.prev_error = error 52 | self.prev_time = self.current_time 53 | 54 | return self.Kp * error + self.Ki * self.i_ + self.Kd * self.d_ 55 | 56 | def set_kp(self, kp): 57 | self.Kp = kp 58 | 59 | def set_ki(self, ki): 60 | self.Ki = ki 61 | 62 | def set_kd(self, kd): 63 | self.Kd = kd 64 | 65 | def set_anti_windup(self, anti_windup): 66 | self.anti_windup = anti_windup 67 | -------------------------------------------------------------------------------- /momobot_ws/src/momobot/teensy/firmware/lib/ros_lib/geometry_msgs/TransformStamped.h: -------------------------------------------------------------------------------- 1 | #ifndef _ROS_geometry_msgs_TransformStamped_h 2 | #define _ROS_geometry_msgs_TransformStamped_h 3 | 4 | #include 5 | #include 6 | #include 7 | #include "ros/msg.h" 8 | #include "std_msgs/Header.h" 9 | #include "geometry_msgs/Transform.h" 10 | 11 | namespace geometry_msgs 12 | { 13 | 14 | class TransformStamped : public ros::Msg 15 | { 16 | public: 17 | std_msgs::Header header; 18 | const char* child_frame_id; 19 | geometry_msgs::Transform transform; 20 | 21 | TransformStamped(): 22 | header(), 23 | child_frame_id(""), 24 | transform() 25 | { 26 | } 27 | 28 | virtual int serialize(unsigned char *outbuffer) const 29 | { 30 | int offset = 0; 31 | offset += this->header.serialize(outbuffer + offset); 32 | uint32_t length_child_frame_id = strlen(this->child_frame_id); 33 | memcpy(outbuffer + offset, &length_child_frame_id, sizeof(uint32_t)); 34 | offset += 4; 35 | memcpy(outbuffer + offset, this->child_frame_id, length_child_frame_id); 36 | offset += length_child_frame_id; 37 | offset += this->transform.serialize(outbuffer + offset); 38 | return offset; 39 | } 40 | 41 | virtual int deserialize(unsigned char *inbuffer) 42 | { 43 | int offset = 0; 44 | offset += this->header.deserialize(inbuffer + offset); 45 | uint32_t length_child_frame_id; 46 | memcpy(&length_child_frame_id, (inbuffer + offset), sizeof(uint32_t)); 47 | offset += 4; 48 | for(unsigned int k= offset; k< offset+length_child_frame_id; ++k){ 49 | inbuffer[k-1]=inbuffer[k]; 50 | } 51 | inbuffer[offset+length_child_frame_id-1]=0; 52 | this->child_frame_id = (char *)(inbuffer + offset-1); 53 | offset += length_child_frame_id; 54 | offset += this->transform.deserialize(inbuffer + offset); 55 | return offset; 56 | } 57 | 58 | const char * getType(){ return "geometry_msgs/TransformStamped"; }; 59 | const char * getMD5(){ return "b5764a33bfeb3588febc2682852579b0"; }; 60 | 61 | }; 62 | 63 | } 64 | #endif -------------------------------------------------------------------------------- /momobot_ws/src/momobot/teensy_bakbak/firmware/lib/ros_lib/geometry_msgs/TransformStamped.h: -------------------------------------------------------------------------------- 1 | #ifndef _ROS_geometry_msgs_TransformStamped_h 2 | #define _ROS_geometry_msgs_TransformStamped_h 3 | 4 | #include 5 | #include 6 | #include 7 | #include "ros/msg.h" 8 | #include "std_msgs/Header.h" 9 | #include "geometry_msgs/Transform.h" 10 | 11 | namespace geometry_msgs 12 | { 13 | 14 | class TransformStamped : public ros::Msg 15 | { 16 | public: 17 | std_msgs::Header header; 18 | const char* child_frame_id; 19 | geometry_msgs::Transform transform; 20 | 21 | TransformStamped(): 22 | header(), 23 | child_frame_id(""), 24 | transform() 25 | { 26 | } 27 | 28 | virtual int serialize(unsigned char *outbuffer) const 29 | { 30 | int offset = 0; 31 | offset += this->header.serialize(outbuffer + offset); 32 | uint32_t length_child_frame_id = strlen(this->child_frame_id); 33 | memcpy(outbuffer + offset, &length_child_frame_id, sizeof(uint32_t)); 34 | offset += 4; 35 | memcpy(outbuffer + offset, this->child_frame_id, length_child_frame_id); 36 | offset += length_child_frame_id; 37 | offset += this->transform.serialize(outbuffer + offset); 38 | return offset; 39 | } 40 | 41 | virtual int deserialize(unsigned char *inbuffer) 42 | { 43 | int offset = 0; 44 | offset += this->header.deserialize(inbuffer + offset); 45 | uint32_t length_child_frame_id; 46 | memcpy(&length_child_frame_id, (inbuffer + offset), sizeof(uint32_t)); 47 | offset += 4; 48 | for(unsigned int k= offset; k< offset+length_child_frame_id; ++k){ 49 | inbuffer[k-1]=inbuffer[k]; 50 | } 51 | inbuffer[offset+length_child_frame_id-1]=0; 52 | this->child_frame_id = (char *)(inbuffer + offset-1); 53 | offset += length_child_frame_id; 54 | offset += this->transform.deserialize(inbuffer + offset); 55 | return offset; 56 | } 57 | 58 | const char * getType(){ return "geometry_msgs/TransformStamped"; }; 59 | const char * getMD5(){ return "b5764a33bfeb3588febc2682852579b0"; }; 60 | 61 | }; 62 | 63 | } 64 | #endif -------------------------------------------------------------------------------- /momobot_ws/src/semantic_pose_sounder/src/semantic_pose_sounder: -------------------------------------------------------------------------------- 1 | #!/usr/bin/env python 2 | 3 | import rospy 4 | from std_msgs.msg import String 5 | 6 | import os 7 | import time 8 | import pygame 9 | 10 | # Change directory to music file directory 11 | try: 12 | os.chdir(os.path.dirname(__file__)) 13 | os.chdir("../media") 14 | except: 15 | pass 16 | 17 | class SemanticPoseSounder(): 18 | def __init__(self): 19 | # Init Pygame 20 | pygame.init() 21 | pygame.mixer.pre_init(frequency=44100, size=-16, channels=2, buffer=4096) 22 | 23 | # Init ROS 24 | rospy.init_node('semantic_pose_sounder') 25 | 26 | self.subscriber = rospy.Subscriber("location", String, self.location_callback) 27 | 28 | # Init Parameters 29 | self.playback_delay = rospy.get_param('playback_delay') 30 | 31 | # Load trigger-sound mappings 32 | self.trigger_mappings = rospy.get_param('trigger_mappings') 33 | 34 | # Load sounder helper variables 35 | self.last_playback_time = 0 36 | self.last_location = "" 37 | 38 | rospy.spin() 39 | 40 | def location_callback(self, msg): 41 | ''' Trigger sound on entering new location ''' 42 | 43 | if time.time() - self.last_playback_time >= self.playback_delay: 44 | if msg.data in self.trigger_mappings.keys(): 45 | if self.last_location != msg.data: 46 | try: 47 | pygame.mixer.music.load(self.trigger_mappings[msg.data]) 48 | pygame.mixer.music.play() 49 | 50 | self.last_playback_time = time.time() 51 | self.last_location = msg.data 52 | 53 | except: 54 | rospy.loginfo("UNABLE TO PLAY: " + str(self.trigger_mappings[msg.data])) 55 | rospy.loginfo("AT LOCATION: " + str(msg.data)) 56 | 57 | else: 58 | self.last_location = msg.data 59 | 60 | if __name__ == '__main__': 61 | try: 62 | SemanticPoseSounder() 63 | except rospy.ROSInterruptException: 64 | pygame.quit() 65 | sys.exit() 66 | -------------------------------------------------------------------------------- /momobot_ws/src/momobot/teensy/firmware/lib/pid/test.ino: -------------------------------------------------------------------------------- 1 | /* 2 | Copyright (c) 2016, Juan Jimeno 3 | All rights reserved. 4 | Redistribution and use in source and binary forms, with or without 5 | modification, are permitted provided that the following conditions are met: 6 | * Redistributions of source code must retain the above copyright notice, 7 | this list of conditions and the following disclaimer. 8 | * Redistributions in binary form must reproduce the above copyright 9 | notice, this list of conditions and the following disclaimer in the 10 | documentation and/or other materials provided with the distribution. 11 | * Neither the name of nor the names of its contributors may be used to 12 | endorse or promote products derived from this software without specific 13 | prior written permission. 14 | THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" 15 | AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE 16 | IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE 17 | ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE 18 | LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR 19 | CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF 20 | SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS 21 | INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN 22 | CONTRACT, STRICT LIABILITY, OR TORTPPIPI (INCLUDING NEGLIGENCE OR OTHERWISE) 23 | ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE 24 | POSSIBILITY OF SUCH DAMAGE. 25 | */ 26 | 27 | #include "PID.h" 28 | PID pid( -255, 255, 0.05, 0.9, 0.1); 29 | /*PID(float min_val, float max_val, float kp, float ki, float kd) 30 | * min_val = min output PID value 31 | * max_val = max output PID value 32 | * kp = PID - P constant 33 | * ki = PID - I constant 34 | * di = PID - D constant 35 | */ 36 | 37 | float setpoint = 30; 38 | float measured_value = 0; 39 | void setup() 40 | { 41 | Serial.begin(9600); 42 | } 43 | 44 | void loop() 45 | { 46 | Serial.println(pid.compute(setpoint, measured_value)); 47 | delay(1000); 48 | measured_value++; 49 | } 50 | -------------------------------------------------------------------------------- /momobot_ws/src/momobot/teensy_bakbak/firmware/lib/pid/test.ino: -------------------------------------------------------------------------------- 1 | /* 2 | Copyright (c) 2016, Juan Jimeno 3 | All rights reserved. 4 | Redistribution and use in source and binary forms, with or without 5 | modification, are permitted provided that the following conditions are met: 6 | * Redistributions of source code must retain the above copyright notice, 7 | this list of conditions and the following disclaimer. 8 | * Redistributions in binary form must reproduce the above copyright 9 | notice, this list of conditions and the following disclaimer in the 10 | documentation and/or other materials provided with the distribution. 11 | * Neither the name of nor the names of its contributors may be used to 12 | endorse or promote products derived from this software without specific 13 | prior written permission. 14 | THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" 15 | AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE 16 | IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE 17 | ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE 18 | LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR 19 | CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF 20 | SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS 21 | INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN 22 | CONTRACT, STRICT LIABILITY, OR TORTPPIPI (INCLUDING NEGLIGENCE OR OTHERWISE) 23 | ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE 24 | POSSIBILITY OF SUCH DAMAGE. 25 | */ 26 | 27 | #include "PID.h" 28 | PID pid( -255, 255, 0.05, 0.9, 0.1); 29 | /*PID(float min_val, float max_val, float kp, float ki, float kd) 30 | * min_val = min output PID value 31 | * max_val = max output PID value 32 | * kp = PID - P constant 33 | * ki = PID - I constant 34 | * di = PID - D constant 35 | */ 36 | 37 | float setpoint = 30; 38 | float measured_value = 0; 39 | void setup() 40 | { 41 | Serial.begin(9600); 42 | } 43 | 44 | void loop() 45 | { 46 | Serial.println(pid.compute(setpoint, measured_value)); 47 | delay(1000); 48 | measured_value++; 49 | } 50 | -------------------------------------------------------------------------------- /momobot_ws/src/momobot/teensy/firmware/lib/ros_lib/sensor_msgs/NavSatStatus.h: -------------------------------------------------------------------------------- 1 | #ifndef _ROS_sensor_msgs_NavSatStatus_h 2 | #define _ROS_sensor_msgs_NavSatStatus_h 3 | 4 | #include 5 | #include 6 | #include 7 | #include "ros/msg.h" 8 | 9 | namespace sensor_msgs 10 | { 11 | 12 | class NavSatStatus : public ros::Msg 13 | { 14 | public: 15 | int8_t status; 16 | uint16_t service; 17 | enum { STATUS_NO_FIX = -1 }; 18 | enum { STATUS_FIX = 0 }; 19 | enum { STATUS_SBAS_FIX = 1 }; 20 | enum { STATUS_GBAS_FIX = 2 }; 21 | enum { SERVICE_GPS = 1 }; 22 | enum { SERVICE_GLONASS = 2 }; 23 | enum { SERVICE_COMPASS = 4 }; 24 | enum { SERVICE_GALILEO = 8 }; 25 | 26 | NavSatStatus(): 27 | status(0), 28 | service(0) 29 | { 30 | } 31 | 32 | virtual int serialize(unsigned char *outbuffer) const 33 | { 34 | int offset = 0; 35 | union { 36 | int8_t real; 37 | uint8_t base; 38 | } u_status; 39 | u_status.real = this->status; 40 | *(outbuffer + offset + 0) = (u_status.base >> (8 * 0)) & 0xFF; 41 | offset += sizeof(this->status); 42 | *(outbuffer + offset + 0) = (this->service >> (8 * 0)) & 0xFF; 43 | *(outbuffer + offset + 1) = (this->service >> (8 * 1)) & 0xFF; 44 | offset += sizeof(this->service); 45 | return offset; 46 | } 47 | 48 | virtual int deserialize(unsigned char *inbuffer) 49 | { 50 | int offset = 0; 51 | union { 52 | int8_t real; 53 | uint8_t base; 54 | } u_status; 55 | u_status.base = 0; 56 | u_status.base |= ((uint8_t) (*(inbuffer + offset + 0))) << (8 * 0); 57 | this->status = u_status.real; 58 | offset += sizeof(this->status); 59 | this->service = ((uint16_t) (*(inbuffer + offset))); 60 | this->service |= ((uint16_t) (*(inbuffer + offset + 1))) << (8 * 1); 61 | offset += sizeof(this->service); 62 | return offset; 63 | } 64 | 65 | const char * getType(){ return "sensor_msgs/NavSatStatus"; }; 66 | const char * getMD5(){ return "331cdbddfa4bc96ffc3b9ad98900a54c"; }; 67 | 68 | }; 69 | 70 | } 71 | #endif -------------------------------------------------------------------------------- /momobot_ws/src/momobot/teensy/firmware/lib/ros_lib/std_msgs/UInt16MultiArray.h: -------------------------------------------------------------------------------- 1 | #ifndef _ROS_std_msgs_UInt16MultiArray_h 2 | #define _ROS_std_msgs_UInt16MultiArray_h 3 | 4 | #include 5 | #include 6 | #include 7 | #include "ros/msg.h" 8 | #include "std_msgs/MultiArrayLayout.h" 9 | 10 | namespace std_msgs 11 | { 12 | 13 | class UInt16MultiArray : public ros::Msg 14 | { 15 | public: 16 | std_msgs::MultiArrayLayout layout; 17 | uint8_t data_length; 18 | uint16_t st_data; 19 | uint16_t * data; 20 | 21 | UInt16MultiArray(): 22 | layout(), 23 | data_length(0), data(NULL) 24 | { 25 | } 26 | 27 | virtual int serialize(unsigned char *outbuffer) const 28 | { 29 | int offset = 0; 30 | offset += this->layout.serialize(outbuffer + offset); 31 | *(outbuffer + offset++) = data_length; 32 | *(outbuffer + offset++) = 0; 33 | *(outbuffer + offset++) = 0; 34 | *(outbuffer + offset++) = 0; 35 | for( uint8_t i = 0; i < data_length; i++){ 36 | *(outbuffer + offset + 0) = (this->data[i] >> (8 * 0)) & 0xFF; 37 | *(outbuffer + offset + 1) = (this->data[i] >> (8 * 1)) & 0xFF; 38 | offset += sizeof(this->data[i]); 39 | } 40 | return offset; 41 | } 42 | 43 | virtual int deserialize(unsigned char *inbuffer) 44 | { 45 | int offset = 0; 46 | offset += this->layout.deserialize(inbuffer + offset); 47 | uint8_t data_lengthT = *(inbuffer + offset++); 48 | if(data_lengthT > data_length) 49 | this->data = (uint16_t*)realloc(this->data, data_lengthT * sizeof(uint16_t)); 50 | offset += 3; 51 | data_length = data_lengthT; 52 | for( uint8_t i = 0; i < data_length; i++){ 53 | this->st_data = ((uint16_t) (*(inbuffer + offset))); 54 | this->st_data |= ((uint16_t) (*(inbuffer + offset + 1))) << (8 * 1); 55 | offset += sizeof(this->st_data); 56 | memcpy( &(this->data[i]), &(this->st_data), sizeof(uint16_t)); 57 | } 58 | return offset; 59 | } 60 | 61 | const char * getType(){ return "std_msgs/UInt16MultiArray"; }; 62 | const char * getMD5(){ return "52f264f1c973c4b73790d384c6cb4484"; }; 63 | 64 | }; 65 | 66 | } 67 | #endif -------------------------------------------------------------------------------- /momobot_ws/src/momobot/teensy_bakbak/firmware/lib/ros_lib/sensor_msgs/NavSatStatus.h: -------------------------------------------------------------------------------- 1 | #ifndef _ROS_sensor_msgs_NavSatStatus_h 2 | #define _ROS_sensor_msgs_NavSatStatus_h 3 | 4 | #include 5 | #include 6 | #include 7 | #include "ros/msg.h" 8 | 9 | namespace sensor_msgs 10 | { 11 | 12 | class NavSatStatus : public ros::Msg 13 | { 14 | public: 15 | int8_t status; 16 | uint16_t service; 17 | enum { STATUS_NO_FIX = -1 }; 18 | enum { STATUS_FIX = 0 }; 19 | enum { STATUS_SBAS_FIX = 1 }; 20 | enum { STATUS_GBAS_FIX = 2 }; 21 | enum { SERVICE_GPS = 1 }; 22 | enum { SERVICE_GLONASS = 2 }; 23 | enum { SERVICE_COMPASS = 4 }; 24 | enum { SERVICE_GALILEO = 8 }; 25 | 26 | NavSatStatus(): 27 | status(0), 28 | service(0) 29 | { 30 | } 31 | 32 | virtual int serialize(unsigned char *outbuffer) const 33 | { 34 | int offset = 0; 35 | union { 36 | int8_t real; 37 | uint8_t base; 38 | } u_status; 39 | u_status.real = this->status; 40 | *(outbuffer + offset + 0) = (u_status.base >> (8 * 0)) & 0xFF; 41 | offset += sizeof(this->status); 42 | *(outbuffer + offset + 0) = (this->service >> (8 * 0)) & 0xFF; 43 | *(outbuffer + offset + 1) = (this->service >> (8 * 1)) & 0xFF; 44 | offset += sizeof(this->service); 45 | return offset; 46 | } 47 | 48 | virtual int deserialize(unsigned char *inbuffer) 49 | { 50 | int offset = 0; 51 | union { 52 | int8_t real; 53 | uint8_t base; 54 | } u_status; 55 | u_status.base = 0; 56 | u_status.base |= ((uint8_t) (*(inbuffer + offset + 0))) << (8 * 0); 57 | this->status = u_status.real; 58 | offset += sizeof(this->status); 59 | this->service = ((uint16_t) (*(inbuffer + offset))); 60 | this->service |= ((uint16_t) (*(inbuffer + offset + 1))) << (8 * 1); 61 | offset += sizeof(this->service); 62 | return offset; 63 | } 64 | 65 | const char * getType(){ return "sensor_msgs/NavSatStatus"; }; 66 | const char * getMD5(){ return "331cdbddfa4bc96ffc3b9ad98900a54c"; }; 67 | 68 | }; 69 | 70 | } 71 | #endif -------------------------------------------------------------------------------- /momobot_ws/src/momobot/teensy_bakbak/firmware/lib/ros_lib/std_msgs/UInt16MultiArray.h: -------------------------------------------------------------------------------- 1 | #ifndef _ROS_std_msgs_UInt16MultiArray_h 2 | #define _ROS_std_msgs_UInt16MultiArray_h 3 | 4 | #include 5 | #include 6 | #include 7 | #include "ros/msg.h" 8 | #include "std_msgs/MultiArrayLayout.h" 9 | 10 | namespace std_msgs 11 | { 12 | 13 | class UInt16MultiArray : public ros::Msg 14 | { 15 | public: 16 | std_msgs::MultiArrayLayout layout; 17 | uint8_t data_length; 18 | uint16_t st_data; 19 | uint16_t * data; 20 | 21 | UInt16MultiArray(): 22 | layout(), 23 | data_length(0), data(NULL) 24 | { 25 | } 26 | 27 | virtual int serialize(unsigned char *outbuffer) const 28 | { 29 | int offset = 0; 30 | offset += this->layout.serialize(outbuffer + offset); 31 | *(outbuffer + offset++) = data_length; 32 | *(outbuffer + offset++) = 0; 33 | *(outbuffer + offset++) = 0; 34 | *(outbuffer + offset++) = 0; 35 | for( uint8_t i = 0; i < data_length; i++){ 36 | *(outbuffer + offset + 0) = (this->data[i] >> (8 * 0)) & 0xFF; 37 | *(outbuffer + offset + 1) = (this->data[i] >> (8 * 1)) & 0xFF; 38 | offset += sizeof(this->data[i]); 39 | } 40 | return offset; 41 | } 42 | 43 | virtual int deserialize(unsigned char *inbuffer) 44 | { 45 | int offset = 0; 46 | offset += this->layout.deserialize(inbuffer + offset); 47 | uint8_t data_lengthT = *(inbuffer + offset++); 48 | if(data_lengthT > data_length) 49 | this->data = (uint16_t*)realloc(this->data, data_lengthT * sizeof(uint16_t)); 50 | offset += 3; 51 | data_length = data_lengthT; 52 | for( uint8_t i = 0; i < data_length; i++){ 53 | this->st_data = ((uint16_t) (*(inbuffer + offset))); 54 | this->st_data |= ((uint16_t) (*(inbuffer + offset + 1))) << (8 * 1); 55 | offset += sizeof(this->st_data); 56 | memcpy( &(this->data[i]), &(this->st_data), sizeof(uint16_t)); 57 | } 58 | return offset; 59 | } 60 | 61 | const char * getType(){ return "std_msgs/UInt16MultiArray"; }; 62 | const char * getMD5(){ return "52f264f1c973c4b73790d384c6cb4484"; }; 63 | 64 | }; 65 | 66 | } 67 | #endif --------------------------------------------------------------------------------