├── .gitignore ├── lib ├── ros_lib │ ├── examples │ │ ├── ServiceClient │ │ │ ├── client.py │ │ │ └── ServiceClient.pde │ │ ├── HelloWorld │ │ │ └── HelloWorld.pde │ │ ├── Blink │ │ │ └── Blink.pde │ │ ├── pubsub │ │ │ └── pubsub.pde │ │ ├── TimeTF │ │ │ └── TimeTF.pde │ │ ├── ServiceServer │ │ │ └── ServiceServer.pde │ │ ├── Logging │ │ │ └── Logging.pde │ │ ├── Odom │ │ │ └── Odom.pde │ │ ├── ServoControl │ │ │ └── ServoControl.pde │ │ ├── ADC │ │ │ └── ADC.pde │ │ ├── button_example │ │ │ └── button_example.pde │ │ ├── Ultrasound │ │ │ └── Ultrasound.pde │ │ └── IrRanger │ │ │ └── IrRanger.pde │ ├── tests │ │ ├── time_test │ │ │ └── time_test.pde │ │ ├── float64_test │ │ │ └── float64_test.pde │ │ └── array_test │ │ │ └── array_test.pde │ ├── 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 │ │ ├── UInt64.h │ │ ├── UInt8MultiArray.h │ │ └── UInt16MultiArray.h │ ├── nav_msgs │ │ ├── GetMapGoal.h │ │ ├── GetMapFeedback.h │ │ ├── GetMapResult.h │ │ ├── GetMapActionGoal.h │ │ ├── GetMapActionResult.h │ │ ├── GetMapActionFeedback.h │ │ ├── GetMapAction.h │ │ ├── GetMap.h │ │ └── Path.h │ ├── actionlib │ │ ├── TwoIntsFeedback.h │ │ ├── TestRequestFeedback.h │ │ ├── TestActionGoal.h │ │ ├── TwoIntsActionGoal.h │ │ ├── TestActionResult.h │ │ ├── TestActionFeedback.h │ │ ├── TwoIntsActionResult.h │ │ ├── TestRequestActionGoal.h │ │ ├── TwoIntsActionFeedback.h │ │ ├── TestRequestActionResult.h │ │ ├── TestAction.h │ │ ├── TestRequestActionFeedback.h │ │ ├── TwoIntsAction.h │ │ ├── TestRequestAction.h │ │ ├── TestGoal.h │ │ ├── TestResult.h │ │ └── TestFeedback.h │ ├── control_msgs │ │ ├── PointHeadResult.h │ │ ├── JointTrajectoryResult.h │ │ ├── JointTrajectoryFeedback.h │ │ ├── SingleJointPositionResult.h │ │ ├── GripperCommandGoal.h │ │ ├── JointTrajectoryGoal.h │ │ ├── PointHeadActionGoal.h │ │ ├── PointHeadActionResult.h │ │ ├── GripperCommandActionGoal.h │ │ ├── JointTrajectoryActionGoal.h │ │ ├── PointHeadActionFeedback.h │ │ ├── GripperCommandActionResult.h │ │ ├── JointTrajectoryActionResult.h │ │ ├── SingleJointPositionActionGoal.h │ │ ├── GripperCommandActionFeedback.h │ │ ├── FollowJointTrajectoryActionGoal.h │ │ ├── JointTrajectoryActionFeedback.h │ │ ├── SingleJointPositionActionResult.h │ │ ├── FollowJointTrajectoryActionResult.h │ │ ├── SingleJointPositionActionFeedback.h │ │ ├── FollowJointTrajectoryActionFeedback.h │ │ ├── PointHeadAction.h │ │ ├── GripperCommandAction.h │ │ ├── JointTrajectoryAction.h │ │ ├── SingleJointPositionAction.h │ │ └── FollowJointTrajectoryAction.h │ ├── turtle_actionlib │ │ ├── ShapeFeedback.h │ │ ├── ShapeActionGoal.h │ │ ├── ShapeActionResult.h │ │ ├── ShapeActionFeedback.h │ │ └── ShapeAction.h │ ├── tf2_msgs │ │ ├── LookupTransformFeedback.h │ │ ├── LookupTransformResult.h │ │ ├── LookupTransformActionGoal.h │ │ ├── LookupTransformActionResult.h │ │ ├── LookupTransformActionFeedback.h │ │ ├── LookupTransformAction.h │ │ ├── TFMessage.h │ │ └── TF2Error.h │ ├── driver_base │ │ └── SensorLevels.h │ ├── dynamic_reconfigure │ │ ├── SensorLevels.h │ │ ├── Reconfigure.h │ │ └── BoolParameter.h │ ├── bond │ │ └── Constants.h │ ├── geometry_msgs │ │ ├── Twist.h │ │ ├── Wrench.h │ │ ├── PoseStamped.h │ │ ├── PointStamped.h │ │ ├── Pose.h │ │ ├── TwistStamped.h │ │ ├── WrenchStamped.h │ │ ├── Vector3Stamped.h │ │ ├── PolygonStamped.h │ │ ├── Transform.h │ │ ├── QuaternionStamped.h │ │ ├── PoseWithCovarianceStamped.h │ │ ├── TwistWithCovarianceStamped.h │ │ ├── Polygon.h │ │ ├── PoseArray.h │ │ └── TransformStamped.h │ ├── STM32Hardware.h │ ├── turtlesim │ │ ├── Color.h │ │ └── Kill.h │ ├── roscpp │ │ ├── Empty.h │ │ └── Logger.h │ ├── std_srvs │ │ └── Empty.h │ ├── actionlib_tutorials │ │ ├── AveragingActionGoal.h │ │ ├── FibonacciActionGoal.h │ │ ├── AveragingActionResult.h │ │ ├── FibonacciActionResult.h │ │ ├── AveragingActionFeedback.h │ │ ├── FibonacciActionFeedback.h │ │ ├── AveragingAction.h │ │ ├── FibonacciAction.h │ │ ├── FibonacciGoal.h │ │ └── AveragingGoal.h │ ├── rospy_tutorials │ │ └── HeaderString.h │ ├── shape_msgs │ │ └── MeshTriangle.h │ ├── rosserial_msgs │ │ └── Log.h │ ├── sensor_msgs │ │ └── JoyFeedbackArray.h │ ├── hector_nav_msgs │ │ └── GetRobotTrajectory.h │ ├── tf │ │ └── tfMessage.h │ ├── visualization_msgs │ │ ├── MarkerArray.h │ │ └── InteractiveMarkerPose.h │ ├── hector_mapping │ │ └── HectorDebugInfo.h │ ├── gazebo_msgs │ │ └── ContactsState.h │ ├── ros.h │ └── diagnostic_msgs │ │ ├── DiagnosticArray.h │ │ └── KeyValue.h └── linker │ ├── STM32F103CBT6.ld │ └── STM32F103VET6.ld ├── README.md ├── LICENSE └── src └── main.cpp /.gitignore: -------------------------------------------------------------------------------- 1 | # Compiled Object files 2 | *.slo 3 | *.lo 4 | *.o 5 | *.obj 6 | 7 | # Precompiled Headers 8 | *.gch 9 | *.pch 10 | 11 | # Compiled Dynamic libraries 12 | *.so 13 | *.dylib 14 | *.dll 15 | 16 | # Fortran module files 17 | *.mod 18 | 19 | # Compiled Static libraries 20 | *.lai 21 | *.la 22 | *.a 23 | *.lib 24 | 25 | # Executables 26 | *.exe 27 | *.out 28 | *.app 29 | -------------------------------------------------------------------------------- /lib/ros_lib/examples/ServiceClient/client.py: -------------------------------------------------------------------------------- 1 | #!/usr/bin/env python 2 | 3 | """ 4 | Sample code to use with ServiceClient.pde 5 | """ 6 | 7 | import roslib; roslib.load_manifest("rosserial_arduino") 8 | import rospy 9 | 10 | from rosserial_arduino.srv import * 11 | 12 | def callback(req): 13 | print "The arduino is calling! Please send it a message:" 14 | t = TestResponse() 15 | t.output = raw_input() 16 | return t 17 | 18 | rospy.init_node("service_client_test") 19 | rospy.Service("test_srv", Test, callback) 20 | rospy.spin() 21 | -------------------------------------------------------------------------------- /lib/ros_lib/tests/time_test/time_test.pde: -------------------------------------------------------------------------------- 1 | /* 2 | * rosserial::std_msgs::Time Test 3 | * Publishes current time 4 | */ 5 | 6 | #include 7 | #include 8 | #include 9 | 10 | 11 | ros::NodeHandle nh; 12 | 13 | std_msgs::Time test; 14 | ros::Publisher p("my_topic", &test); 15 | 16 | void setup() 17 | { 18 | pinMode(13, OUTPUT); 19 | nh.initNode(); 20 | nh.advertise(p); 21 | } 22 | 23 | void loop() 24 | { 25 | test.data = nh.now(); 26 | p.publish( &test ); 27 | nh.spinOnce(); 28 | delay(10); 29 | } 30 | 31 | -------------------------------------------------------------------------------- /lib/ros_lib/examples/HelloWorld/HelloWorld.pde: -------------------------------------------------------------------------------- 1 | /* 2 | * rosserial Publisher Example 3 | * Prints "hello world!" 4 | */ 5 | 6 | #include 7 | #include 8 | 9 | ros::NodeHandle nh; 10 | 11 | std_msgs::String str_msg; 12 | ros::Publisher chatter("chatter", &str_msg); 13 | 14 | char hello[13] = "hello world!"; 15 | 16 | void setup() 17 | { 18 | nh.initNode(); 19 | nh.advertise(chatter); 20 | } 21 | 22 | void loop() 23 | { 24 | str_msg.data = hello; 25 | chatter.publish( &str_msg ); 26 | nh.spinOnce(); 27 | delay(1000); 28 | } 29 | -------------------------------------------------------------------------------- /lib/ros_lib/examples/Blink/Blink.pde: -------------------------------------------------------------------------------- 1 | /* 2 | * rosserial Subscriber Example 3 | * Blinks an LED on callback 4 | */ 5 | 6 | #include 7 | #include 8 | 9 | ros::NodeHandle nh; 10 | 11 | void messageCb( const std_msgs::Empty& toggle_msg){ 12 | digitalWrite(13, HIGH-digitalRead(13)); // blink the led 13 | } 14 | 15 | ros::Subscriber sub("toggle_led", &messageCb ); 16 | 17 | void setup() 18 | { 19 | pinMode(13, OUTPUT); 20 | nh.initNode(); 21 | nh.subscribe(sub); 22 | } 23 | 24 | void loop() 25 | { 26 | nh.spinOnce(); 27 | delay(1); 28 | } 29 | 30 | -------------------------------------------------------------------------------- /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 | virtual int serialize(unsigned char *outbuffer) const 17 | { 18 | int offset = 0; 19 | return offset; 20 | } 21 | 22 | virtual int deserialize(unsigned char *inbuffer) 23 | { 24 | int offset = 0; 25 | return offset; 26 | } 27 | 28 | const char * getType(){ return "std_msgs/Empty"; }; 29 | const char * getMD5(){ return "d41d8cd98f00b204e9800998ecf8427e"; }; 30 | 31 | }; 32 | 33 | } 34 | #endif -------------------------------------------------------------------------------- /README.md: -------------------------------------------------------------------------------- 1 | stm32f1_rosserial 2 | ================= 3 | 4 | STM32 rosserial sample 5 | 6 | DEFAULT : USART1 / baud 57600bps / send buffer 1024byte / recieve buffer 1024byte 7 | If you want to change another USART, edit "./lib/ros_lib/STM32Hardware.h" 8 | 9 | ===!NOTICE!=== 10 | SpiralRay uses following INTELLECTUAL PROPERTIES under each licence. 11 | Those sourcecode are included in this project. 12 | 13 | --- LIBRARIES --- 14 | * Nemui Trinomius's TFT/OLED Control Sample with ChaN's FatFs. 15 | http://nemuisan.blog.bai.ne.jp/?eid=192848 16 | 17 | * Andrew Brown's stm32plus 18 | https://github.com/andysworkshop/stm32plus 19 | 20 | Using this fork to support STM32F103CBT6 21 | https://github.com/spiralray/stm32plus 22 | 23 | * rosserial 24 | http://wiki.ros.org/rosserial -------------------------------------------------------------------------------- /lib/ros_lib/nav_msgs/GetMapGoal.h: -------------------------------------------------------------------------------- 1 | #ifndef _ROS_nav_msgs_GetMapGoal_h 2 | #define _ROS_nav_msgs_GetMapGoal_h 3 | 4 | #include 5 | #include 6 | #include 7 | #include "ros/msg.h" 8 | 9 | namespace nav_msgs 10 | { 11 | 12 | class GetMapGoal : public ros::Msg 13 | { 14 | public: 15 | 16 | virtual int serialize(unsigned char *outbuffer) const 17 | { 18 | int offset = 0; 19 | return offset; 20 | } 21 | 22 | virtual int deserialize(unsigned char *inbuffer) 23 | { 24 | int offset = 0; 25 | return offset; 26 | } 27 | 28 | const char * getType(){ return "nav_msgs/GetMapGoal"; }; 29 | const char * getMD5(){ return "d41d8cd98f00b204e9800998ecf8427e"; }; 30 | 31 | }; 32 | 33 | } 34 | #endif -------------------------------------------------------------------------------- /lib/linker/STM32F103CBT6.ld: -------------------------------------------------------------------------------- 1 | /* Linker script UpperSide for STM32F103VET6! */ 2 | /* Based on STMicro's RIDE Project Sample thanks! */ 3 | 4 | OUTPUT_FORMAT ("elf32-littlearm") 5 | 6 | /* Memory Spaces Definitions */ 7 | MEMORY 8 | { 9 | RAM1 (xrw) : ORIGIN = 0x20000000, LENGTH = 20K /* Main Embedded SRAM */ 10 | RAM5 (xrw) : ORIGIN = 0x68000000, LENGTH = 0K /* External SRAM (FSMC) */ 11 | ROM (rx) : ORIGIN = 0x08000000, LENGTH = 128K /* Main Embedded FlashROM */ 12 | } 13 | 14 | /* higher address of the stack bottom */ 15 | _estack = ORIGIN(RAM1)+LENGTH(RAM1); 16 | 17 | /* higher address of the heap end */ 18 | /*_heap_end = ORIGIN(RAM1)+LENGTH(RAM1)-4;*/ /* due to 4byte alignments */ 19 | 20 | 21 | /* include the section management sub-script */ 22 | INCLUDE "STM32F1x_FLASH.ld" 23 | -------------------------------------------------------------------------------- /lib/ros_lib/nav_msgs/GetMapFeedback.h: -------------------------------------------------------------------------------- 1 | #ifndef _ROS_nav_msgs_GetMapFeedback_h 2 | #define _ROS_nav_msgs_GetMapFeedback_h 3 | 4 | #include 5 | #include 6 | #include 7 | #include "ros/msg.h" 8 | 9 | namespace nav_msgs 10 | { 11 | 12 | class GetMapFeedback : public ros::Msg 13 | { 14 | public: 15 | 16 | virtual int serialize(unsigned char *outbuffer) const 17 | { 18 | int offset = 0; 19 | return offset; 20 | } 21 | 22 | virtual int deserialize(unsigned char *inbuffer) 23 | { 24 | int offset = 0; 25 | return offset; 26 | } 27 | 28 | const char * getType(){ return "nav_msgs/GetMapFeedback"; }; 29 | const char * getMD5(){ return "d41d8cd98f00b204e9800998ecf8427e"; }; 30 | 31 | }; 32 | 33 | } 34 | #endif -------------------------------------------------------------------------------- /lib/ros_lib/actionlib/TwoIntsFeedback.h: -------------------------------------------------------------------------------- 1 | #ifndef _ROS_actionlib_TwoIntsFeedback_h 2 | #define _ROS_actionlib_TwoIntsFeedback_h 3 | 4 | #include 5 | #include 6 | #include 7 | #include "ros/msg.h" 8 | 9 | namespace actionlib 10 | { 11 | 12 | class TwoIntsFeedback : public ros::Msg 13 | { 14 | public: 15 | 16 | virtual int serialize(unsigned char *outbuffer) const 17 | { 18 | int offset = 0; 19 | return offset; 20 | } 21 | 22 | virtual int deserialize(unsigned char *inbuffer) 23 | { 24 | int offset = 0; 25 | return offset; 26 | } 27 | 28 | const char * getType(){ return "actionlib/TwoIntsFeedback"; }; 29 | const char * getMD5(){ return "d41d8cd98f00b204e9800998ecf8427e"; }; 30 | 31 | }; 32 | 33 | } 34 | #endif -------------------------------------------------------------------------------- /lib/ros_lib/control_msgs/PointHeadResult.h: -------------------------------------------------------------------------------- 1 | #ifndef _ROS_control_msgs_PointHeadResult_h 2 | #define _ROS_control_msgs_PointHeadResult_h 3 | 4 | #include 5 | #include 6 | #include 7 | #include "ros/msg.h" 8 | 9 | namespace control_msgs 10 | { 11 | 12 | class PointHeadResult : public ros::Msg 13 | { 14 | public: 15 | 16 | virtual int serialize(unsigned char *outbuffer) const 17 | { 18 | int offset = 0; 19 | return offset; 20 | } 21 | 22 | virtual int deserialize(unsigned char *inbuffer) 23 | { 24 | int offset = 0; 25 | return offset; 26 | } 27 | 28 | const char * getType(){ return "control_msgs/PointHeadResult"; }; 29 | const char * getMD5(){ return "d41d8cd98f00b204e9800998ecf8427e"; }; 30 | 31 | }; 32 | 33 | } 34 | #endif -------------------------------------------------------------------------------- /lib/ros_lib/actionlib/TestRequestFeedback.h: -------------------------------------------------------------------------------- 1 | #ifndef _ROS_actionlib_TestRequestFeedback_h 2 | #define _ROS_actionlib_TestRequestFeedback_h 3 | 4 | #include 5 | #include 6 | #include 7 | #include "ros/msg.h" 8 | 9 | namespace actionlib 10 | { 11 | 12 | class TestRequestFeedback : public ros::Msg 13 | { 14 | public: 15 | 16 | virtual int serialize(unsigned char *outbuffer) const 17 | { 18 | int offset = 0; 19 | return offset; 20 | } 21 | 22 | virtual int deserialize(unsigned char *inbuffer) 23 | { 24 | int offset = 0; 25 | return offset; 26 | } 27 | 28 | const char * getType(){ return "actionlib/TestRequestFeedback"; }; 29 | const char * getMD5(){ return "d41d8cd98f00b204e9800998ecf8427e"; }; 30 | 31 | }; 32 | 33 | } 34 | #endif -------------------------------------------------------------------------------- /lib/ros_lib/turtle_actionlib/ShapeFeedback.h: -------------------------------------------------------------------------------- 1 | #ifndef _ROS_turtle_actionlib_ShapeFeedback_h 2 | #define _ROS_turtle_actionlib_ShapeFeedback_h 3 | 4 | #include 5 | #include 6 | #include 7 | #include "ros/msg.h" 8 | 9 | namespace turtle_actionlib 10 | { 11 | 12 | class ShapeFeedback : public ros::Msg 13 | { 14 | public: 15 | 16 | virtual int serialize(unsigned char *outbuffer) const 17 | { 18 | int offset = 0; 19 | return offset; 20 | } 21 | 22 | virtual int deserialize(unsigned char *inbuffer) 23 | { 24 | int offset = 0; 25 | return offset; 26 | } 27 | 28 | const char * getType(){ return "turtle_actionlib/ShapeFeedback"; }; 29 | const char * getMD5(){ return "d41d8cd98f00b204e9800998ecf8427e"; }; 30 | 31 | }; 32 | 33 | } 34 | #endif -------------------------------------------------------------------------------- /lib/ros_lib/tests/float64_test/float64_test.pde: -------------------------------------------------------------------------------- 1 | /* 2 | * rosserial::std_msgs::Float64 Test 3 | * Receives a Float64 input, subtracts 1.0, and publishes it 4 | */ 5 | 6 | #include 7 | #include 8 | 9 | 10 | ros::NodeHandle nh; 11 | 12 | float x; 13 | 14 | void messageCb( const std_msgs::Float64& msg){ 15 | x = msg.data - 1.0; 16 | digitalWrite(13, HIGH-digitalRead(13)); // blink the led 17 | } 18 | 19 | std_msgs::Float64 test; 20 | ros::Subscriber s("your_topic", &messageCb); 21 | ros::Publisher p("my_topic", &test); 22 | 23 | void setup() 24 | { 25 | pinMode(13, OUTPUT); 26 | nh.initNode(); 27 | nh.advertise(p); 28 | nh.subscribe(s); 29 | } 30 | 31 | void loop() 32 | { 33 | test.data = x; 34 | p.publish( &test ); 35 | nh.spinOnce(); 36 | delay(10); 37 | } 38 | 39 | -------------------------------------------------------------------------------- /lib/ros_lib/tf2_msgs/LookupTransformFeedback.h: -------------------------------------------------------------------------------- 1 | #ifndef _ROS_tf2_msgs_LookupTransformFeedback_h 2 | #define _ROS_tf2_msgs_LookupTransformFeedback_h 3 | 4 | #include 5 | #include 6 | #include 7 | #include "ros/msg.h" 8 | 9 | namespace tf2_msgs 10 | { 11 | 12 | class LookupTransformFeedback : public ros::Msg 13 | { 14 | public: 15 | 16 | virtual int serialize(unsigned char *outbuffer) const 17 | { 18 | int offset = 0; 19 | return offset; 20 | } 21 | 22 | virtual int deserialize(unsigned char *inbuffer) 23 | { 24 | int offset = 0; 25 | return offset; 26 | } 27 | 28 | const char * getType(){ return "tf2_msgs/LookupTransformFeedback"; }; 29 | const char * getMD5(){ return "d41d8cd98f00b204e9800998ecf8427e"; }; 30 | 31 | }; 32 | 33 | } 34 | #endif -------------------------------------------------------------------------------- /lib/ros_lib/control_msgs/JointTrajectoryResult.h: -------------------------------------------------------------------------------- 1 | #ifndef _ROS_control_msgs_JointTrajectoryResult_h 2 | #define _ROS_control_msgs_JointTrajectoryResult_h 3 | 4 | #include 5 | #include 6 | #include 7 | #include "ros/msg.h" 8 | 9 | namespace control_msgs 10 | { 11 | 12 | class JointTrajectoryResult : public ros::Msg 13 | { 14 | public: 15 | 16 | virtual int serialize(unsigned char *outbuffer) const 17 | { 18 | int offset = 0; 19 | return offset; 20 | } 21 | 22 | virtual int deserialize(unsigned char *inbuffer) 23 | { 24 | int offset = 0; 25 | return offset; 26 | } 27 | 28 | const char * getType(){ return "control_msgs/JointTrajectoryResult"; }; 29 | const char * getMD5(){ return "d41d8cd98f00b204e9800998ecf8427e"; }; 30 | 31 | }; 32 | 33 | } 34 | #endif -------------------------------------------------------------------------------- /lib/linker/STM32F103VET6.ld: -------------------------------------------------------------------------------- 1 | /* Linker script UpperSide for STM32F103VET6! */ 2 | /* (c) Nemui Trinomius (http://nemuisan.blog.bai.ne.jp) */ 3 | /* Based on STMicro's RIDE Project Sample thanks! */ 4 | 5 | OUTPUT_FORMAT ("elf32-littlearm") 6 | 7 | /* Memory Spaces Definitions */ 8 | MEMORY 9 | { 10 | RAM1 (xrw) : ORIGIN = 0x20000000, LENGTH = 64k /* Main Embedded SRAM */ 11 | RAM5 (xrw) : ORIGIN = 0x68000000, LENGTH = 0K /* External SRAM (FSMC) */ 12 | ROM (rx) : ORIGIN = 0x08000000, LENGTH = 1024K /* Main Embedded FlashROM */ 13 | } 14 | 15 | /* higher address of the stack bottom */ 16 | _estack = ORIGIN(RAM1)+LENGTH(RAM1); 17 | 18 | /* higher address of the heap end */ 19 | /*_heap_end = ORIGIN(RAM1)+LENGTH(RAM1)-4;*/ /* due to 4byte alignments */ 20 | 21 | 22 | /* include the section management sub-script */ 23 | INCLUDE "STM32F1x_FLASH.ld" 24 | -------------------------------------------------------------------------------- /lib/ros_lib/control_msgs/JointTrajectoryFeedback.h: -------------------------------------------------------------------------------- 1 | #ifndef _ROS_control_msgs_JointTrajectoryFeedback_h 2 | #define _ROS_control_msgs_JointTrajectoryFeedback_h 3 | 4 | #include 5 | #include 6 | #include 7 | #include "ros/msg.h" 8 | 9 | namespace control_msgs 10 | { 11 | 12 | class JointTrajectoryFeedback : public ros::Msg 13 | { 14 | public: 15 | 16 | virtual int serialize(unsigned char *outbuffer) const 17 | { 18 | int offset = 0; 19 | return offset; 20 | } 21 | 22 | virtual int deserialize(unsigned char *inbuffer) 23 | { 24 | int offset = 0; 25 | return offset; 26 | } 27 | 28 | const char * getType(){ return "control_msgs/JointTrajectoryFeedback"; }; 29 | const char * getMD5(){ return "d41d8cd98f00b204e9800998ecf8427e"; }; 30 | 31 | }; 32 | 33 | } 34 | #endif -------------------------------------------------------------------------------- /lib/ros_lib/control_msgs/SingleJointPositionResult.h: -------------------------------------------------------------------------------- 1 | #ifndef _ROS_control_msgs_SingleJointPositionResult_h 2 | #define _ROS_control_msgs_SingleJointPositionResult_h 3 | 4 | #include 5 | #include 6 | #include 7 | #include "ros/msg.h" 8 | 9 | namespace control_msgs 10 | { 11 | 12 | class SingleJointPositionResult : public ros::Msg 13 | { 14 | public: 15 | 16 | virtual int serialize(unsigned char *outbuffer) const 17 | { 18 | int offset = 0; 19 | return offset; 20 | } 21 | 22 | virtual int deserialize(unsigned char *inbuffer) 23 | { 24 | int offset = 0; 25 | return offset; 26 | } 27 | 28 | const char * getType(){ return "control_msgs/SingleJointPositionResult"; }; 29 | const char * getMD5(){ return "d41d8cd98f00b204e9800998ecf8427e"; }; 30 | 31 | }; 32 | 33 | } 34 | #endif -------------------------------------------------------------------------------- /lib/ros_lib/examples/pubsub/pubsub.pde: -------------------------------------------------------------------------------- 1 | /* 2 | * rosserial PubSub Example 3 | * Prints "hello world!" and toggles led 4 | */ 5 | 6 | #include 7 | #include 8 | #include 9 | 10 | ros::NodeHandle nh; 11 | 12 | 13 | void messageCb( const std_msgs::Empty& toggle_msg){ 14 | digitalWrite(13, HIGH-digitalRead(13)); // blink the led 15 | } 16 | 17 | ros::Subscriber sub("toggle_led", messageCb ); 18 | 19 | 20 | 21 | std_msgs::String str_msg; 22 | ros::Publisher chatter("chatter", &str_msg); 23 | 24 | char hello[13] = "hello world!"; 25 | 26 | void setup() 27 | { 28 | pinMode(13, OUTPUT); 29 | nh.initNode(); 30 | nh.advertise(chatter); 31 | nh.subscribe(sub); 32 | } 33 | 34 | void loop() 35 | { 36 | str_msg.data = hello; 37 | chatter.publish( &str_msg ); 38 | nh.spinOnce(); 39 | delay(500); 40 | } 41 | -------------------------------------------------------------------------------- /lib/ros_lib/examples/TimeTF/TimeTF.pde: -------------------------------------------------------------------------------- 1 | /* 2 | * rosserial Time and TF Example 3 | * Publishes a transform at current time 4 | */ 5 | 6 | #include 7 | #include 8 | #include 9 | 10 | ros::NodeHandle nh; 11 | 12 | geometry_msgs::TransformStamped t; 13 | tf::TransformBroadcaster broadcaster; 14 | 15 | char base_link[] = "/base_link"; 16 | char odom[] = "/odom"; 17 | 18 | void setup() 19 | { 20 | nh.initNode(); 21 | broadcaster.init(nh); 22 | } 23 | 24 | void loop() 25 | { 26 | t.header.frame_id = odom; 27 | t.child_frame_id = base_link; 28 | t.transform.translation.x = 1.0; 29 | t.transform.rotation.x = 0.0; 30 | t.transform.rotation.y = 0.0; 31 | t.transform.rotation.z = 0.0; 32 | t.transform.rotation.w = 1.0; 33 | t.header.stamp = nh.now(); 34 | broadcaster.sendTransform(t); 35 | nh.spinOnce(); 36 | delay(10); 37 | } 38 | -------------------------------------------------------------------------------- /lib/ros_lib/examples/ServiceClient/ServiceClient.pde: -------------------------------------------------------------------------------- 1 | /* 2 | * rosserial Service Client 3 | */ 4 | 5 | #include 6 | #include 7 | #include 8 | 9 | ros::NodeHandle nh; 10 | using rosserial_arduino::Test; 11 | 12 | ros::ServiceClient client("test_srv"); 13 | 14 | std_msgs::String str_msg; 15 | ros::Publisher chatter("chatter", &str_msg); 16 | 17 | char hello[13] = "hello world!"; 18 | 19 | void setup() 20 | { 21 | nh.initNode(); 22 | nh.serviceClient(client); 23 | nh.advertise(chatter); 24 | while(!nh.connected()) nh.spinOnce(); 25 | nh.loginfo("Startup complete"); 26 | } 27 | 28 | void loop() 29 | { 30 | Test::Request req; 31 | Test::Response res; 32 | req.input = hello; 33 | client.call(req, res); 34 | str_msg.data = res.output; 35 | chatter.publish( &str_msg ); 36 | nh.spinOnce(); 37 | delay(100); 38 | } 39 | -------------------------------------------------------------------------------- /lib/ros_lib/examples/ServiceServer/ServiceServer.pde: -------------------------------------------------------------------------------- 1 | /* 2 | * rosserial Service Server 3 | */ 4 | 5 | #include 6 | #include 7 | #include 8 | 9 | ros::NodeHandle nh; 10 | using rosserial_arduino::Test; 11 | 12 | int i; 13 | void callback(const Test::Request & req, Test::Response & res){ 14 | if((i++)%2) 15 | res.output = "hello"; 16 | else 17 | res.output = "world"; 18 | } 19 | 20 | ros::ServiceServer server("test_srv",&callback); 21 | 22 | std_msgs::String str_msg; 23 | ros::Publisher chatter("chatter", &str_msg); 24 | 25 | char hello[13] = "hello world!"; 26 | 27 | void setup() 28 | { 29 | nh.initNode(); 30 | nh.advertiseService(server); 31 | nh.advertise(chatter); 32 | } 33 | 34 | void loop() 35 | { 36 | str_msg.data = hello; 37 | chatter.publish( &str_msg ); 38 | nh.spinOnce(); 39 | delay(10); 40 | } 41 | -------------------------------------------------------------------------------- /lib/ros_lib/driver_base/SensorLevels.h: -------------------------------------------------------------------------------- 1 | #ifndef _ROS_driver_base_SensorLevels_h 2 | #define _ROS_driver_base_SensorLevels_h 3 | 4 | #include 5 | #include 6 | #include 7 | #include "ros/msg.h" 8 | 9 | namespace driver_base 10 | { 11 | 12 | class SensorLevels : public ros::Msg 13 | { 14 | public: 15 | enum { RECONFIGURE_CLOSE = 3 }; 16 | enum { RECONFIGURE_STOP = 1 }; 17 | enum { RECONFIGURE_RUNNING = 0 }; 18 | 19 | virtual int serialize(unsigned char *outbuffer) const 20 | { 21 | int offset = 0; 22 | return offset; 23 | } 24 | 25 | virtual int deserialize(unsigned char *inbuffer) 26 | { 27 | int offset = 0; 28 | return offset; 29 | } 30 | 31 | const char * getType(){ return "driver_base/SensorLevels"; }; 32 | const char * getMD5(){ return "6322637bee96d5489db6e2127c47602c"; }; 33 | 34 | }; 35 | 36 | } 37 | #endif -------------------------------------------------------------------------------- /lib/ros_lib/examples/Logging/Logging.pde: -------------------------------------------------------------------------------- 1 | /* 2 | * rosserial PubSub Example 3 | * Prints "hello world!" and toggles led 4 | */ 5 | 6 | #include 7 | #include 8 | #include 9 | 10 | ros::NodeHandle nh; 11 | 12 | 13 | std_msgs::String str_msg; 14 | ros::Publisher chatter("chatter", &str_msg); 15 | 16 | char hello[13] = "hello world!"; 17 | 18 | 19 | char debug[]= "debug statements"; 20 | char info[] = "infos"; 21 | char warn[] = "warnings"; 22 | char error[] = "errors"; 23 | char fatal[] = "fatalities"; 24 | 25 | void setup() 26 | { 27 | pinMode(13, OUTPUT); 28 | nh.initNode(); 29 | nh.advertise(chatter); 30 | } 31 | 32 | void loop() 33 | { 34 | str_msg.data = hello; 35 | chatter.publish( &str_msg ); 36 | 37 | nh.logdebug(debug); 38 | nh.loginfo(info); 39 | nh.logwarn(warn); 40 | nh.logerror(error); 41 | nh.logfatal(fatal); 42 | 43 | nh.spinOnce(); 44 | delay(500); 45 | } 46 | -------------------------------------------------------------------------------- /lib/ros_lib/dynamic_reconfigure/SensorLevels.h: -------------------------------------------------------------------------------- 1 | #ifndef _ROS_dynamic_reconfigure_SensorLevels_h 2 | #define _ROS_dynamic_reconfigure_SensorLevels_h 3 | 4 | #include 5 | #include 6 | #include 7 | #include "ros/msg.h" 8 | 9 | namespace dynamic_reconfigure 10 | { 11 | 12 | class SensorLevels : public ros::Msg 13 | { 14 | public: 15 | enum { RECONFIGURE_CLOSE = 3 }; 16 | enum { RECONFIGURE_STOP = 1 }; 17 | enum { RECONFIGURE_RUNNING = 0 }; 18 | 19 | virtual int serialize(unsigned char *outbuffer) const 20 | { 21 | int offset = 0; 22 | return offset; 23 | } 24 | 25 | virtual int deserialize(unsigned char *inbuffer) 26 | { 27 | int offset = 0; 28 | return offset; 29 | } 30 | 31 | const char * getType(){ return "dynamic_reconfigure/SensorLevels"; }; 32 | const char * getMD5(){ return "6322637bee96d5489db6e2127c47602c"; }; 33 | 34 | }; 35 | 36 | } 37 | #endif -------------------------------------------------------------------------------- /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 | virtual int serialize(unsigned char *outbuffer) const 18 | { 19 | int offset = 0; 20 | *(outbuffer + offset + 0) = (this->data >> (8 * 0)) & 0xFF; 21 | offset += sizeof(this->data); 22 | return offset; 23 | } 24 | 25 | virtual int deserialize(unsigned char *inbuffer) 26 | { 27 | int offset = 0; 28 | this->data = ((uint8_t) (*(inbuffer + offset))); 29 | offset += sizeof(this->data); 30 | return offset; 31 | } 32 | 33 | const char * getType(){ return "std_msgs/Char"; }; 34 | const char * getMD5(){ return "1bf77f25acecdedba0e224b162199717"; }; 35 | 36 | }; 37 | 38 | } 39 | #endif -------------------------------------------------------------------------------- /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 | virtual int serialize(unsigned char *outbuffer) const 18 | { 19 | int offset = 0; 20 | *(outbuffer + offset + 0) = (this->data >> (8 * 0)) & 0xFF; 21 | offset += sizeof(this->data); 22 | return offset; 23 | } 24 | 25 | virtual int deserialize(unsigned char *inbuffer) 26 | { 27 | int offset = 0; 28 | this->data = ((uint8_t) (*(inbuffer + offset))); 29 | offset += sizeof(this->data); 30 | return offset; 31 | } 32 | 33 | const char * getType(){ return "std_msgs/UInt8"; }; 34 | const char * getMD5(){ return "7c8164229e7d2c17eb95e9231617fdee"; }; 35 | 36 | }; 37 | 38 | } 39 | #endif -------------------------------------------------------------------------------- /lib/ros_lib/nav_msgs/GetMapResult.h: -------------------------------------------------------------------------------- 1 | #ifndef _ROS_nav_msgs_GetMapResult_h 2 | #define _ROS_nav_msgs_GetMapResult_h 3 | 4 | #include 5 | #include 6 | #include 7 | #include "ros/msg.h" 8 | #include "nav_msgs/OccupancyGrid.h" 9 | 10 | namespace nav_msgs 11 | { 12 | 13 | class GetMapResult : public ros::Msg 14 | { 15 | public: 16 | nav_msgs::OccupancyGrid map; 17 | 18 | virtual int serialize(unsigned char *outbuffer) const 19 | { 20 | int offset = 0; 21 | offset += this->map.serialize(outbuffer + offset); 22 | return offset; 23 | } 24 | 25 | virtual int deserialize(unsigned char *inbuffer) 26 | { 27 | int offset = 0; 28 | offset += this->map.deserialize(inbuffer + offset); 29 | return offset; 30 | } 31 | 32 | const char * getType(){ return "nav_msgs/GetMapResult"; }; 33 | const char * getMD5(){ return "6cdd0a18e0aff5b0a3ca2326a89b54ff"; }; 34 | 35 | }; 36 | 37 | } 38 | #endif -------------------------------------------------------------------------------- /lib/ros_lib/control_msgs/GripperCommandGoal.h: -------------------------------------------------------------------------------- 1 | #ifndef _ROS_control_msgs_GripperCommandGoal_h 2 | #define _ROS_control_msgs_GripperCommandGoal_h 3 | 4 | #include 5 | #include 6 | #include 7 | #include "ros/msg.h" 8 | #include "control_msgs/GripperCommand.h" 9 | 10 | namespace control_msgs 11 | { 12 | 13 | class GripperCommandGoal : public ros::Msg 14 | { 15 | public: 16 | control_msgs::GripperCommand command; 17 | 18 | virtual int serialize(unsigned char *outbuffer) const 19 | { 20 | int offset = 0; 21 | offset += this->command.serialize(outbuffer + offset); 22 | return offset; 23 | } 24 | 25 | virtual int deserialize(unsigned char *inbuffer) 26 | { 27 | int offset = 0; 28 | offset += this->command.deserialize(inbuffer + offset); 29 | return offset; 30 | } 31 | 32 | const char * getType(){ return "control_msgs/GripperCommandGoal"; }; 33 | const char * getMD5(){ return "86fd82f4ddc48a4cb6856cfa69217e43"; }; 34 | 35 | }; 36 | 37 | } 38 | #endif -------------------------------------------------------------------------------- /lib/ros_lib/control_msgs/JointTrajectoryGoal.h: -------------------------------------------------------------------------------- 1 | #ifndef _ROS_control_msgs_JointTrajectoryGoal_h 2 | #define _ROS_control_msgs_JointTrajectoryGoal_h 3 | 4 | #include 5 | #include 6 | #include 7 | #include "ros/msg.h" 8 | #include "trajectory_msgs/JointTrajectory.h" 9 | 10 | namespace control_msgs 11 | { 12 | 13 | class JointTrajectoryGoal : public ros::Msg 14 | { 15 | public: 16 | trajectory_msgs::JointTrajectory trajectory; 17 | 18 | virtual int serialize(unsigned char *outbuffer) const 19 | { 20 | int offset = 0; 21 | offset += this->trajectory.serialize(outbuffer + offset); 22 | return offset; 23 | } 24 | 25 | virtual int deserialize(unsigned char *inbuffer) 26 | { 27 | int offset = 0; 28 | offset += this->trajectory.deserialize(inbuffer + offset); 29 | return offset; 30 | } 31 | 32 | const char * getType(){ return "control_msgs/JointTrajectoryGoal"; }; 33 | const char * getMD5(){ return "2a0eff76c870e8595636c2a562ca298e"; }; 34 | 35 | }; 36 | 37 | } 38 | #endif -------------------------------------------------------------------------------- /lib/ros_lib/bond/Constants.h: -------------------------------------------------------------------------------- 1 | #ifndef _ROS_bond_Constants_h 2 | #define _ROS_bond_Constants_h 3 | 4 | #include 5 | #include 6 | #include 7 | #include "ros/msg.h" 8 | 9 | namespace bond 10 | { 11 | 12 | class Constants : public ros::Msg 13 | { 14 | public: 15 | enum { DEAD_PUBLISH_PERIOD = 0.05 }; 16 | enum { DEFAULT_CONNECT_TIMEOUT = 10.0 }; 17 | enum { DEFAULT_HEARTBEAT_TIMEOUT = 4.0 }; 18 | enum { DEFAULT_DISCONNECT_TIMEOUT = 2.0 }; 19 | enum { DEFAULT_HEARTBEAT_PERIOD = 1.0 }; 20 | enum { DISABLE_HEARTBEAT_TIMEOUT_PARAM = /bond_disable_heartbeat_timeout }; 21 | 22 | virtual int serialize(unsigned char *outbuffer) const 23 | { 24 | int offset = 0; 25 | return offset; 26 | } 27 | 28 | virtual int deserialize(unsigned char *inbuffer) 29 | { 30 | int offset = 0; 31 | return offset; 32 | } 33 | 34 | const char * getType(){ return "bond/Constants"; }; 35 | const char * getMD5(){ return "6fc594dc1d7bd7919077042712f8c8b0"; }; 36 | 37 | }; 38 | 39 | } 40 | #endif -------------------------------------------------------------------------------- /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 | virtual int serialize(unsigned char *outbuffer) const 18 | { 19 | int offset = 0; 20 | *(outbuffer + offset + 0) = (this->data >> (8 * 0)) & 0xFF; 21 | *(outbuffer + offset + 1) = (this->data >> (8 * 1)) & 0xFF; 22 | offset += sizeof(this->data); 23 | return offset; 24 | } 25 | 26 | virtual int deserialize(unsigned char *inbuffer) 27 | { 28 | int offset = 0; 29 | this->data = ((uint16_t) (*(inbuffer + offset))); 30 | this->data |= ((uint16_t) (*(inbuffer + offset + 1))) << (8 * 1); 31 | offset += sizeof(this->data); 32 | return offset; 33 | } 34 | 35 | const char * getType(){ return "std_msgs/UInt16"; }; 36 | const char * getMD5(){ return "1df79edf208b629fe6b81923a544552d"; }; 37 | 38 | }; 39 | 40 | } 41 | #endif -------------------------------------------------------------------------------- /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 | virtual int serialize(unsigned char *outbuffer) const 20 | { 21 | int offset = 0; 22 | offset += this->linear.serialize(outbuffer + offset); 23 | offset += this->angular.serialize(outbuffer + offset); 24 | return offset; 25 | } 26 | 27 | virtual int deserialize(unsigned char *inbuffer) 28 | { 29 | int offset = 0; 30 | offset += this->linear.deserialize(inbuffer + offset); 31 | offset += this->angular.deserialize(inbuffer + offset); 32 | return offset; 33 | } 34 | 35 | const char * getType(){ return "geometry_msgs/Twist"; }; 36 | const char * getMD5(){ return "9f195f881246fdfa2798d1d3eebca84a"; }; 37 | 38 | }; 39 | 40 | } 41 | #endif -------------------------------------------------------------------------------- /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 | virtual int serialize(unsigned char *outbuffer) const 20 | { 21 | int offset = 0; 22 | offset += this->force.serialize(outbuffer + offset); 23 | offset += this->torque.serialize(outbuffer + offset); 24 | return offset; 25 | } 26 | 27 | virtual int deserialize(unsigned char *inbuffer) 28 | { 29 | int offset = 0; 30 | offset += this->force.deserialize(inbuffer + offset); 31 | offset += this->torque.deserialize(inbuffer + offset); 32 | return offset; 33 | } 34 | 35 | const char * getType(){ return "geometry_msgs/Wrench"; }; 36 | const char * getMD5(){ return "4f539cf138b23283b520fd271b567936"; }; 37 | 38 | }; 39 | 40 | } 41 | #endif -------------------------------------------------------------------------------- /lib/ros_lib/tests/array_test/array_test.pde: -------------------------------------------------------------------------------- 1 | /* 2 | * rosserial::geometry_msgs::PoseArray Test 3 | * Sums an array, publishes sum 4 | */ 5 | 6 | #include 7 | #include 8 | #include 9 | 10 | 11 | ros::NodeHandle nh; 12 | 13 | 14 | bool set_; 15 | 16 | 17 | geometry_msgs::Pose sum_msg; 18 | ros::Publisher p("sum", &sum_msg); 19 | 20 | void messageCb(const geometry_msgs::PoseArray& msg){ 21 | sum_msg.position.x = 0; 22 | sum_msg.position.y = 0; 23 | sum_msg.position.z = 0; 24 | for(int i = 0; i < msg.poses_length; i++) 25 | { 26 | sum_msg.position.x += msg.poses[i].position.x; 27 | sum_msg.position.y += msg.poses[i].position.y; 28 | sum_msg.position.z += msg.poses[i].position.z; 29 | } 30 | digitalWrite(13, HIGH-digitalRead(13)); // blink the led 31 | } 32 | 33 | ros::Subscriber s("poses",messageCb); 34 | 35 | void setup() 36 | { 37 | pinMode(13, OUTPUT); 38 | nh.initNode(); 39 | nh.subscribe(s); 40 | nh.advertise(p); 41 | } 42 | 43 | void loop() 44 | { 45 | p.publish(&sum_msg); 46 | nh.spinOnce(); 47 | delay(10); 48 | } 49 | 50 | -------------------------------------------------------------------------------- /lib/ros_lib/examples/Odom/Odom.pde: -------------------------------------------------------------------------------- 1 | /* 2 | * rosserial Planar Odometry Example 3 | */ 4 | 5 | #include 6 | #include 7 | #include 8 | #include 9 | 10 | ros::NodeHandle nh; 11 | 12 | geometry_msgs::TransformStamped t; 13 | tf::TransformBroadcaster broadcaster; 14 | 15 | double x = 1.0; 16 | double y = 0.0; 17 | double theta = 1.57; 18 | 19 | char base_link[] = "/base_link"; 20 | char odom[] = "/odom"; 21 | 22 | void setup() 23 | { 24 | nh.initNode(); 25 | broadcaster.init(nh); 26 | } 27 | 28 | void loop() 29 | { 30 | // drive in a circle 31 | double dx = 0.2; 32 | double dtheta = 0.18; 33 | x += cos(theta)*dx*0.1; 34 | y += sin(theta)*dx*0.1; 35 | theta += dtheta*0.1; 36 | if(theta > 3.14) 37 | theta=-3.14; 38 | 39 | // tf odom->base_link 40 | t.header.frame_id = odom; 41 | t.child_frame_id = base_link; 42 | 43 | t.transform.translation.x = x; 44 | t.transform.translation.y = y; 45 | 46 | t.transform.rotation = tf::createQuaternionFromYaw(theta); 47 | t.header.stamp = nh.now(); 48 | 49 | broadcaster.sendTransform(t); 50 | nh.spinOnce(); 51 | 52 | delay(10); 53 | } 54 | -------------------------------------------------------------------------------- /lib/ros_lib/examples/ServoControl/ServoControl.pde: -------------------------------------------------------------------------------- 1 | /* 2 | * rosserial Servo Control Example 3 | * 4 | * This sketch demonstrates the control of hobby R/C servos 5 | * using ROS and the arduiono 6 | * 7 | * For the full tutorial write up, visit 8 | * www.ros.org/wiki/rosserial_arduino_demos 9 | * 10 | * For more information on the Arduino Servo Library 11 | * Checkout : 12 | * http://www.arduino.cc/en/Reference/Servo 13 | */ 14 | 15 | #if (ARDUINO >= 100) 16 | #include 17 | #else 18 | #include 19 | #endif 20 | 21 | #include 22 | #include 23 | #include 24 | 25 | ros::NodeHandle nh; 26 | 27 | Servo servo; 28 | 29 | void servo_cb( const std_msgs::UInt16& cmd_msg){ 30 | servo.write(cmd_msg.data); //set servo angle, should be from 0-180 31 | digitalWrite(13, HIGH-digitalRead(13)); //toggle led 32 | } 33 | 34 | 35 | ros::Subscriber sub("servo", servo_cb); 36 | 37 | void setup(){ 38 | pinMode(13, OUTPUT); 39 | 40 | nh.initNode(); 41 | nh.subscribe(sub); 42 | 43 | servo.attach(9); //attach it to pin 9 44 | } 45 | 46 | void loop(){ 47 | nh.spinOnce(); 48 | delay(1); 49 | } 50 | -------------------------------------------------------------------------------- /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 | virtual int serialize(unsigned char *outbuffer) const 21 | { 22 | int offset = 0; 23 | offset += this->header.serialize(outbuffer + offset); 24 | offset += this->pose.serialize(outbuffer + offset); 25 | return offset; 26 | } 27 | 28 | virtual int deserialize(unsigned char *inbuffer) 29 | { 30 | int offset = 0; 31 | offset += this->header.deserialize(inbuffer + offset); 32 | offset += this->pose.deserialize(inbuffer + offset); 33 | return offset; 34 | } 35 | 36 | const char * getType(){ return "geometry_msgs/PoseStamped"; }; 37 | const char * getMD5(){ return "d3812c3cbc69362b77dc0b19b345f8f5"; }; 38 | 39 | }; 40 | 41 | } 42 | #endif -------------------------------------------------------------------------------- /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 | virtual int serialize(unsigned char *outbuffer) const 21 | { 22 | int offset = 0; 23 | offset += this->header.serialize(outbuffer + offset); 24 | offset += this->point.serialize(outbuffer + offset); 25 | return offset; 26 | } 27 | 28 | virtual int deserialize(unsigned char *inbuffer) 29 | { 30 | int offset = 0; 31 | offset += this->header.deserialize(inbuffer + offset); 32 | offset += this->point.deserialize(inbuffer + offset); 33 | return offset; 34 | } 35 | 36 | const char * getType(){ return "geometry_msgs/PointStamped"; }; 37 | const char * getMD5(){ return "c63aecb41bfdfd6b7e1fac37c7cbe7bf"; }; 38 | 39 | }; 40 | 41 | } 42 | #endif -------------------------------------------------------------------------------- /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 | virtual int serialize(unsigned char *outbuffer) const 21 | { 22 | int offset = 0; 23 | offset += this->position.serialize(outbuffer + offset); 24 | offset += this->orientation.serialize(outbuffer + offset); 25 | return offset; 26 | } 27 | 28 | virtual int deserialize(unsigned char *inbuffer) 29 | { 30 | int offset = 0; 31 | offset += this->position.deserialize(inbuffer + offset); 32 | offset += this->orientation.deserialize(inbuffer + offset); 33 | return offset; 34 | } 35 | 36 | const char * getType(){ return "geometry_msgs/Pose"; }; 37 | const char * getMD5(){ return "e45d45a5a1ce597b249e23fb30fc871f"; }; 38 | 39 | }; 40 | 41 | } 42 | #endif -------------------------------------------------------------------------------- /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 | virtual int serialize(unsigned char *outbuffer) const 21 | { 22 | int offset = 0; 23 | offset += this->header.serialize(outbuffer + offset); 24 | offset += this->twist.serialize(outbuffer + offset); 25 | return offset; 26 | } 27 | 28 | virtual int deserialize(unsigned char *inbuffer) 29 | { 30 | int offset = 0; 31 | offset += this->header.deserialize(inbuffer + offset); 32 | offset += this->twist.deserialize(inbuffer + offset); 33 | return offset; 34 | } 35 | 36 | const char * getType(){ return "geometry_msgs/TwistStamped"; }; 37 | const char * getMD5(){ return "98d34b0043a2093cf9d9345ab6eef12e"; }; 38 | 39 | }; 40 | 41 | } 42 | #endif -------------------------------------------------------------------------------- /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 | virtual int serialize(unsigned char *outbuffer) const 21 | { 22 | int offset = 0; 23 | offset += this->header.serialize(outbuffer + offset); 24 | offset += this->wrench.serialize(outbuffer + offset); 25 | return offset; 26 | } 27 | 28 | virtual int deserialize(unsigned char *inbuffer) 29 | { 30 | int offset = 0; 31 | offset += this->header.deserialize(inbuffer + offset); 32 | offset += this->wrench.deserialize(inbuffer + offset); 33 | return offset; 34 | } 35 | 36 | const char * getType(){ return "geometry_msgs/WrenchStamped"; }; 37 | const char * getMD5(){ return "d78d3cb249ce23087ade7e7d0c40cfa7"; }; 38 | 39 | }; 40 | 41 | } 42 | #endif -------------------------------------------------------------------------------- /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 | virtual int serialize(unsigned char *outbuffer) const 21 | { 22 | int offset = 0; 23 | offset += this->header.serialize(outbuffer + offset); 24 | offset += this->vector.serialize(outbuffer + offset); 25 | return offset; 26 | } 27 | 28 | virtual int deserialize(unsigned char *inbuffer) 29 | { 30 | int offset = 0; 31 | offset += this->header.deserialize(inbuffer + offset); 32 | offset += this->vector.deserialize(inbuffer + offset); 33 | return offset; 34 | } 35 | 36 | const char * getType(){ return "geometry_msgs/Vector3Stamped"; }; 37 | const char * getMD5(){ return "7b324c7325e683bf02a9b14b01090ec7"; }; 38 | 39 | }; 40 | 41 | } 42 | #endif -------------------------------------------------------------------------------- /lib/ros_lib/examples/ADC/ADC.pde: -------------------------------------------------------------------------------- 1 | /* 2 | * rosserial ADC Example 3 | * 4 | * This is a poor man's Oscilloscope. It does not have the sampling 5 | * rate or accuracy of a commerical scope, but it is great to get 6 | * an analog value into ROS in a pinch. 7 | */ 8 | 9 | #if (ARDUINO >= 100) 10 | #include 11 | #else 12 | #include 13 | #endif 14 | #include 15 | #include 16 | 17 | ros::NodeHandle nh; 18 | 19 | rosserial_arduino::Adc adc_msg; 20 | ros::Publisher p("adc", &adc_msg); 21 | 22 | void setup() 23 | { 24 | pinMode(13, OUTPUT); 25 | nh.initNode(); 26 | 27 | nh.advertise(p); 28 | } 29 | 30 | //We average the analog reading to elminate some of the noise 31 | int averageAnalog(int pin){ 32 | int v=0; 33 | for(int i=0; i<4; i++) v+= analogRead(pin); 34 | return v/4; 35 | } 36 | 37 | long adc_timer; 38 | 39 | void loop() 40 | { 41 | adc_msg.adc0 = averageAnalog(0); 42 | adc_msg.adc1 = averageAnalog(1); 43 | adc_msg.adc2 = averageAnalog(2); 44 | adc_msg.adc3 = averageAnalog(3); 45 | adc_msg.adc4 = averageAnalog(4); 46 | adc_msg.adc5 = averageAnalog(5); 47 | 48 | p.publish(&adc_msg); 49 | 50 | nh.spinOnce(); 51 | } 52 | 53 | -------------------------------------------------------------------------------- /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 | virtual int serialize(unsigned char *outbuffer) const 21 | { 22 | int offset = 0; 23 | offset += this->header.serialize(outbuffer + offset); 24 | offset += this->polygon.serialize(outbuffer + offset); 25 | return offset; 26 | } 27 | 28 | virtual int deserialize(unsigned char *inbuffer) 29 | { 30 | int offset = 0; 31 | offset += this->header.deserialize(inbuffer + offset); 32 | offset += this->polygon.deserialize(inbuffer + offset); 33 | return offset; 34 | } 35 | 36 | const char * getType(){ return "geometry_msgs/PolygonStamped"; }; 37 | const char * getMD5(){ return "c6be8f7dc3bee7fe9e8d296070f53340"; }; 38 | 39 | }; 40 | 41 | } 42 | #endif -------------------------------------------------------------------------------- /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 | virtual int serialize(unsigned char *outbuffer) const 21 | { 22 | int offset = 0; 23 | offset += this->translation.serialize(outbuffer + offset); 24 | offset += this->rotation.serialize(outbuffer + offset); 25 | return offset; 26 | } 27 | 28 | virtual int deserialize(unsigned char *inbuffer) 29 | { 30 | int offset = 0; 31 | offset += this->translation.deserialize(inbuffer + offset); 32 | offset += this->rotation.deserialize(inbuffer + offset); 33 | return offset; 34 | } 35 | 36 | const char * getType(){ return "geometry_msgs/Transform"; }; 37 | const char * getMD5(){ return "ac9eff44abf714214112b05d54a3cf9b"; }; 38 | 39 | }; 40 | 41 | } 42 | #endif -------------------------------------------------------------------------------- /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 | virtual int serialize(unsigned char *outbuffer) const 21 | { 22 | int offset = 0; 23 | offset += this->header.serialize(outbuffer + offset); 24 | offset += this->quaternion.serialize(outbuffer + offset); 25 | return offset; 26 | } 27 | 28 | virtual int deserialize(unsigned char *inbuffer) 29 | { 30 | int offset = 0; 31 | offset += this->header.deserialize(inbuffer + offset); 32 | offset += this->quaternion.deserialize(inbuffer + offset); 33 | return offset; 34 | } 35 | 36 | const char * getType(){ return "geometry_msgs/QuaternionStamped"; }; 37 | const char * getMD5(){ return "e57f1e547e0e1fd13504588ffc8334e2"; }; 38 | 39 | }; 40 | 41 | } 42 | #endif -------------------------------------------------------------------------------- /lib/ros_lib/tf2_msgs/LookupTransformResult.h: -------------------------------------------------------------------------------- 1 | #ifndef _ROS_tf2_msgs_LookupTransformResult_h 2 | #define _ROS_tf2_msgs_LookupTransformResult_h 3 | 4 | #include 5 | #include 6 | #include 7 | #include "ros/msg.h" 8 | #include "geometry_msgs/TransformStamped.h" 9 | #include "tf2_msgs/TF2Error.h" 10 | 11 | namespace tf2_msgs 12 | { 13 | 14 | class LookupTransformResult : public ros::Msg 15 | { 16 | public: 17 | geometry_msgs::TransformStamped transform; 18 | tf2_msgs::TF2Error error; 19 | 20 | virtual int serialize(unsigned char *outbuffer) const 21 | { 22 | int offset = 0; 23 | offset += this->transform.serialize(outbuffer + offset); 24 | offset += this->error.serialize(outbuffer + offset); 25 | return offset; 26 | } 27 | 28 | virtual int deserialize(unsigned char *inbuffer) 29 | { 30 | int offset = 0; 31 | offset += this->transform.deserialize(inbuffer + offset); 32 | offset += this->error.deserialize(inbuffer + offset); 33 | return offset; 34 | } 35 | 36 | const char * getType(){ return "tf2_msgs/LookupTransformResult"; }; 37 | const char * getMD5(){ return "3fe5db6a19ca9cfb675418c5ad875c36"; }; 38 | 39 | }; 40 | 41 | } 42 | #endif -------------------------------------------------------------------------------- /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 | virtual int serialize(unsigned char *outbuffer) const 18 | { 19 | int offset = 0; 20 | union { 21 | bool real; 22 | uint8_t base; 23 | } u_data; 24 | u_data.real = this->data; 25 | *(outbuffer + offset + 0) = (u_data.base >> (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 | union { 34 | bool real; 35 | uint8_t base; 36 | } u_data; 37 | u_data.base = 0; 38 | u_data.base |= ((uint8_t) (*(inbuffer + offset + 0))) << (8 * 0); 39 | this->data = u_data.real; 40 | offset += sizeof(this->data); 41 | return offset; 42 | } 43 | 44 | const char * getType(){ return "std_msgs/Bool"; }; 45 | const char * getMD5(){ return "8b94c1b53db61fb6aed406028ad6332a"; }; 46 | 47 | }; 48 | 49 | } 50 | #endif -------------------------------------------------------------------------------- /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 | virtual int serialize(unsigned char *outbuffer) const 18 | { 19 | int offset = 0; 20 | union { 21 | int8_t real; 22 | uint8_t base; 23 | } u_data; 24 | u_data.real = this->data; 25 | *(outbuffer + offset + 0) = (u_data.base >> (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 | union { 34 | int8_t real; 35 | uint8_t base; 36 | } u_data; 37 | u_data.base = 0; 38 | u_data.base |= ((uint8_t) (*(inbuffer + offset + 0))) << (8 * 0); 39 | this->data = u_data.real; 40 | offset += sizeof(this->data); 41 | return offset; 42 | } 43 | 44 | const char * getType(){ return "std_msgs/Byte"; }; 45 | const char * getMD5(){ return "ad736a2e8818154c487bb80fe42ce43b"; }; 46 | 47 | }; 48 | 49 | } 50 | #endif -------------------------------------------------------------------------------- /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 | virtual int serialize(unsigned char *outbuffer) const 18 | { 19 | int offset = 0; 20 | union { 21 | int8_t real; 22 | uint8_t base; 23 | } u_data; 24 | u_data.real = this->data; 25 | *(outbuffer + offset + 0) = (u_data.base >> (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 | union { 34 | int8_t real; 35 | uint8_t base; 36 | } u_data; 37 | u_data.base = 0; 38 | u_data.base |= ((uint8_t) (*(inbuffer + offset + 0))) << (8 * 0); 39 | this->data = u_data.real; 40 | offset += sizeof(this->data); 41 | return offset; 42 | } 43 | 44 | const char * getType(){ return "std_msgs/Int8"; }; 45 | const char * getMD5(){ return "27ffa0c9c4b8fb8492252bcad9e5c57b"; }; 46 | 47 | }; 48 | 49 | } 50 | #endif -------------------------------------------------------------------------------- /lib/ros_lib/STM32Hardware.h: -------------------------------------------------------------------------------- 1 | #pragma once 2 | 3 | #include "config/stm32plus.h" 4 | #include "utils/UsartWithBuffer.h" 5 | 6 | class STM32Hardware { 7 | 8 | using SERIAL_CLASS = UsartWithBuffer, Usart1InterruptFeature>; 9 | 10 | public: 11 | STM32Hardware(SERIAL_CLASS* _com , long baud= 57600){ 12 | com = _com; 13 | baud_ = baud; 14 | } 15 | STM32Hardware() 16 | { 17 | com = new SERIAL_CLASS(57600); 18 | baud_ = 57600; 19 | } 20 | STM32Hardware(STM32Hardware& h){ 21 | this->baud_ = h.baud_; 22 | } 23 | 24 | void setBaud(long baud){ 25 | this->baud_= baud; 26 | } 27 | 28 | int getBaud(){return baud_;} 29 | 30 | void init(){ 31 | //iostream->begin(baud_); 32 | } 33 | 34 | int read(){ 35 | if(com->dataAvailable()){ 36 | return com->getch(); 37 | } 38 | else{ 39 | return -1; 40 | } 41 | }; 42 | 43 | void write(uint8_t* data, int length){ 44 | for(int i=0; iTXBuffer_FreeSpace() ); 46 | com->putch(data[i]); 47 | } 48 | 49 | } 50 | 51 | unsigned long time(){return MillisecondTimer::millis();} 52 | 53 | protected: 54 | SERIAL_CLASS* com; 55 | long baud_; 56 | }; 57 | -------------------------------------------------------------------------------- /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 | virtual int serialize(unsigned char *outbuffer) const 21 | { 22 | int offset = 0; 23 | offset += this->header.serialize(outbuffer + offset); 24 | offset += this->pose.serialize(outbuffer + offset); 25 | return offset; 26 | } 27 | 28 | virtual int deserialize(unsigned char *inbuffer) 29 | { 30 | int offset = 0; 31 | offset += this->header.deserialize(inbuffer + offset); 32 | offset += this->pose.deserialize(inbuffer + offset); 33 | return offset; 34 | } 35 | 36 | const char * getType(){ return "geometry_msgs/PoseWithCovarianceStamped"; }; 37 | const char * getMD5(){ return "953b798c0f514ff060a53a3498ce6246"; }; 38 | 39 | }; 40 | 41 | } 42 | #endif -------------------------------------------------------------------------------- /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 | virtual int serialize(unsigned char *outbuffer) const 21 | { 22 | int offset = 0; 23 | offset += this->header.serialize(outbuffer + offset); 24 | offset += this->twist.serialize(outbuffer + offset); 25 | return offset; 26 | } 27 | 28 | virtual int deserialize(unsigned char *inbuffer) 29 | { 30 | int offset = 0; 31 | offset += this->header.deserialize(inbuffer + offset); 32 | offset += this->twist.deserialize(inbuffer + offset); 33 | return offset; 34 | } 35 | 36 | const char * getType(){ return "geometry_msgs/TwistWithCovarianceStamped"; }; 37 | const char * getMD5(){ return "8927a1a12fb2607ceea095b2dc440a96"; }; 38 | 39 | }; 40 | 41 | } 42 | #endif -------------------------------------------------------------------------------- /lib/ros_lib/actionlib/TestActionGoal.h: -------------------------------------------------------------------------------- 1 | #ifndef _ROS_actionlib_TestActionGoal_h 2 | #define _ROS_actionlib_TestActionGoal_h 3 | 4 | #include 5 | #include 6 | #include 7 | #include "ros/msg.h" 8 | #include "std_msgs/Header.h" 9 | #include "actionlib_msgs/GoalID.h" 10 | #include "actionlib/TestGoal.h" 11 | 12 | namespace actionlib 13 | { 14 | 15 | class TestActionGoal : public ros::Msg 16 | { 17 | public: 18 | std_msgs::Header header; 19 | actionlib_msgs::GoalID goal_id; 20 | actionlib::TestGoal goal; 21 | 22 | virtual int serialize(unsigned char *outbuffer) const 23 | { 24 | int offset = 0; 25 | offset += this->header.serialize(outbuffer + offset); 26 | offset += this->goal_id.serialize(outbuffer + offset); 27 | offset += this->goal.serialize(outbuffer + offset); 28 | return offset; 29 | } 30 | 31 | virtual int deserialize(unsigned char *inbuffer) 32 | { 33 | int offset = 0; 34 | offset += this->header.deserialize(inbuffer + offset); 35 | offset += this->goal_id.deserialize(inbuffer + offset); 36 | offset += this->goal.deserialize(inbuffer + offset); 37 | return offset; 38 | } 39 | 40 | const char * getType(){ return "actionlib/TestActionGoal"; }; 41 | const char * getMD5(){ return "348369c5b403676156094e8c159720bf"; }; 42 | 43 | }; 44 | 45 | } 46 | #endif -------------------------------------------------------------------------------- /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 | virtual int serialize(unsigned char *outbuffer) const 18 | { 19 | int offset = 0; 20 | uint32_t length_data = strlen(this->data); 21 | memcpy(outbuffer + offset, &length_data, sizeof(uint32_t)); 22 | offset += 4; 23 | memcpy(outbuffer + offset, this->data, length_data); 24 | offset += length_data; 25 | return offset; 26 | } 27 | 28 | virtual int deserialize(unsigned char *inbuffer) 29 | { 30 | int offset = 0; 31 | uint32_t length_data; 32 | memcpy(&length_data, (inbuffer + offset), sizeof(uint32_t)); 33 | offset += 4; 34 | for(unsigned int k= offset; k< offset+length_data; ++k){ 35 | inbuffer[k-1]=inbuffer[k]; 36 | } 37 | inbuffer[offset+length_data-1]=0; 38 | this->data = (char *)(inbuffer + offset-1); 39 | offset += length_data; 40 | return offset; 41 | } 42 | 43 | const char * getType(){ return "std_msgs/String"; }; 44 | const char * getMD5(){ return "992ce8a1687cec8c8bd883ec73ca41d1"; }; 45 | 46 | }; 47 | 48 | } 49 | #endif -------------------------------------------------------------------------------- /lib/ros_lib/nav_msgs/GetMapActionGoal.h: -------------------------------------------------------------------------------- 1 | #ifndef _ROS_nav_msgs_GetMapActionGoal_h 2 | #define _ROS_nav_msgs_GetMapActionGoal_h 3 | 4 | #include 5 | #include 6 | #include 7 | #include "ros/msg.h" 8 | #include "std_msgs/Header.h" 9 | #include "actionlib_msgs/GoalID.h" 10 | #include "nav_msgs/GetMapGoal.h" 11 | 12 | namespace nav_msgs 13 | { 14 | 15 | class GetMapActionGoal : public ros::Msg 16 | { 17 | public: 18 | std_msgs::Header header; 19 | actionlib_msgs::GoalID goal_id; 20 | nav_msgs::GetMapGoal goal; 21 | 22 | virtual int serialize(unsigned char *outbuffer) const 23 | { 24 | int offset = 0; 25 | offset += this->header.serialize(outbuffer + offset); 26 | offset += this->goal_id.serialize(outbuffer + offset); 27 | offset += this->goal.serialize(outbuffer + offset); 28 | return offset; 29 | } 30 | 31 | virtual int deserialize(unsigned char *inbuffer) 32 | { 33 | int offset = 0; 34 | offset += this->header.deserialize(inbuffer + offset); 35 | offset += this->goal_id.deserialize(inbuffer + offset); 36 | offset += this->goal.deserialize(inbuffer + offset); 37 | return offset; 38 | } 39 | 40 | const char * getType(){ return "nav_msgs/GetMapActionGoal"; }; 41 | const char * getMD5(){ return "4b30be6cd12b9e72826df56b481f40e0"; }; 42 | 43 | }; 44 | 45 | } 46 | #endif -------------------------------------------------------------------------------- /lib/ros_lib/turtlesim/Color.h: -------------------------------------------------------------------------------- 1 | #ifndef _ROS_turtlesim_Color_h 2 | #define _ROS_turtlesim_Color_h 3 | 4 | #include 5 | #include 6 | #include 7 | #include "ros/msg.h" 8 | 9 | namespace turtlesim 10 | { 11 | 12 | class Color : public ros::Msg 13 | { 14 | public: 15 | uint8_t r; 16 | uint8_t g; 17 | uint8_t b; 18 | 19 | virtual int serialize(unsigned char *outbuffer) const 20 | { 21 | int offset = 0; 22 | *(outbuffer + offset + 0) = (this->r >> (8 * 0)) & 0xFF; 23 | offset += sizeof(this->r); 24 | *(outbuffer + offset + 0) = (this->g >> (8 * 0)) & 0xFF; 25 | offset += sizeof(this->g); 26 | *(outbuffer + offset + 0) = (this->b >> (8 * 0)) & 0xFF; 27 | offset += sizeof(this->b); 28 | return offset; 29 | } 30 | 31 | virtual int deserialize(unsigned char *inbuffer) 32 | { 33 | int offset = 0; 34 | this->r = ((uint8_t) (*(inbuffer + offset))); 35 | offset += sizeof(this->r); 36 | this->g = ((uint8_t) (*(inbuffer + offset))); 37 | offset += sizeof(this->g); 38 | this->b = ((uint8_t) (*(inbuffer + offset))); 39 | offset += sizeof(this->b); 40 | return offset; 41 | } 42 | 43 | const char * getType(){ return "turtlesim/Color"; }; 44 | const char * getMD5(){ return "353891e354491c51aabe32df673fb446"; }; 45 | 46 | }; 47 | 48 | } 49 | #endif -------------------------------------------------------------------------------- /lib/ros_lib/actionlib/TwoIntsActionGoal.h: -------------------------------------------------------------------------------- 1 | #ifndef _ROS_actionlib_TwoIntsActionGoal_h 2 | #define _ROS_actionlib_TwoIntsActionGoal_h 3 | 4 | #include 5 | #include 6 | #include 7 | #include "ros/msg.h" 8 | #include "std_msgs/Header.h" 9 | #include "actionlib_msgs/GoalID.h" 10 | #include "actionlib/TwoIntsGoal.h" 11 | 12 | namespace actionlib 13 | { 14 | 15 | class TwoIntsActionGoal : public ros::Msg 16 | { 17 | public: 18 | std_msgs::Header header; 19 | actionlib_msgs::GoalID goal_id; 20 | actionlib::TwoIntsGoal goal; 21 | 22 | virtual int serialize(unsigned char *outbuffer) const 23 | { 24 | int offset = 0; 25 | offset += this->header.serialize(outbuffer + offset); 26 | offset += this->goal_id.serialize(outbuffer + offset); 27 | offset += this->goal.serialize(outbuffer + offset); 28 | return offset; 29 | } 30 | 31 | virtual int deserialize(unsigned char *inbuffer) 32 | { 33 | int offset = 0; 34 | offset += this->header.deserialize(inbuffer + offset); 35 | offset += this->goal_id.deserialize(inbuffer + offset); 36 | offset += this->goal.deserialize(inbuffer + offset); 37 | return offset; 38 | } 39 | 40 | const char * getType(){ return "actionlib/TwoIntsActionGoal"; }; 41 | const char * getMD5(){ return "684a2db55d6ffb8046fb9d6764ce0860"; }; 42 | 43 | }; 44 | 45 | } 46 | #endif -------------------------------------------------------------------------------- /lib/ros_lib/roscpp/Empty.h: -------------------------------------------------------------------------------- 1 | #ifndef _ROS_SERVICE_Empty_h 2 | #define _ROS_SERVICE_Empty_h 3 | #include 4 | #include 5 | #include 6 | #include "ros/msg.h" 7 | 8 | namespace roscpp 9 | { 10 | 11 | static const char EMPTY[] = "roscpp/Empty"; 12 | 13 | class EmptyRequest : public ros::Msg 14 | { 15 | public: 16 | 17 | virtual int serialize(unsigned char *outbuffer) const 18 | { 19 | int offset = 0; 20 | return offset; 21 | } 22 | 23 | virtual int deserialize(unsigned char *inbuffer) 24 | { 25 | int offset = 0; 26 | return offset; 27 | } 28 | 29 | const char * getType(){ return EMPTY; }; 30 | const char * getMD5(){ return "d41d8cd98f00b204e9800998ecf8427e"; }; 31 | 32 | }; 33 | 34 | class EmptyResponse : public ros::Msg 35 | { 36 | public: 37 | 38 | virtual int serialize(unsigned char *outbuffer) const 39 | { 40 | int offset = 0; 41 | return offset; 42 | } 43 | 44 | virtual int deserialize(unsigned char *inbuffer) 45 | { 46 | int offset = 0; 47 | return offset; 48 | } 49 | 50 | const char * getType(){ return EMPTY; }; 51 | const char * getMD5(){ return "d41d8cd98f00b204e9800998ecf8427e"; }; 52 | 53 | }; 54 | 55 | class Empty { 56 | public: 57 | typedef EmptyRequest Request; 58 | typedef EmptyResponse Response; 59 | }; 60 | 61 | } 62 | #endif 63 | -------------------------------------------------------------------------------- /lib/ros_lib/actionlib/TestActionResult.h: -------------------------------------------------------------------------------- 1 | #ifndef _ROS_actionlib_TestActionResult_h 2 | #define _ROS_actionlib_TestActionResult_h 3 | 4 | #include 5 | #include 6 | #include 7 | #include "ros/msg.h" 8 | #include "std_msgs/Header.h" 9 | #include "actionlib_msgs/GoalStatus.h" 10 | #include "actionlib/TestResult.h" 11 | 12 | namespace actionlib 13 | { 14 | 15 | class TestActionResult : public ros::Msg 16 | { 17 | public: 18 | std_msgs::Header header; 19 | actionlib_msgs::GoalStatus status; 20 | actionlib::TestResult result; 21 | 22 | virtual int serialize(unsigned char *outbuffer) const 23 | { 24 | int offset = 0; 25 | offset += this->header.serialize(outbuffer + offset); 26 | offset += this->status.serialize(outbuffer + offset); 27 | offset += this->result.serialize(outbuffer + offset); 28 | return offset; 29 | } 30 | 31 | virtual int deserialize(unsigned char *inbuffer) 32 | { 33 | int offset = 0; 34 | offset += this->header.deserialize(inbuffer + offset); 35 | offset += this->status.deserialize(inbuffer + offset); 36 | offset += this->result.deserialize(inbuffer + offset); 37 | return offset; 38 | } 39 | 40 | const char * getType(){ return "actionlib/TestActionResult"; }; 41 | const char * getMD5(){ return "3d669e3a63aa986c667ea7b0f46ce85e"; }; 42 | 43 | }; 44 | 45 | } 46 | #endif -------------------------------------------------------------------------------- /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 | virtual int serialize(unsigned char *outbuffer) const 18 | { 19 | int offset = 0; 20 | *(outbuffer + offset + 0) = (this->data >> (8 * 0)) & 0xFF; 21 | *(outbuffer + offset + 1) = (this->data >> (8 * 1)) & 0xFF; 22 | *(outbuffer + offset + 2) = (this->data >> (8 * 2)) & 0xFF; 23 | *(outbuffer + offset + 3) = (this->data >> (8 * 3)) & 0xFF; 24 | offset += sizeof(this->data); 25 | return offset; 26 | } 27 | 28 | virtual int deserialize(unsigned char *inbuffer) 29 | { 30 | int offset = 0; 31 | this->data = ((uint32_t) (*(inbuffer + offset))); 32 | this->data |= ((uint32_t) (*(inbuffer + offset + 1))) << (8 * 1); 33 | this->data |= ((uint32_t) (*(inbuffer + offset + 2))) << (8 * 2); 34 | this->data |= ((uint32_t) (*(inbuffer + offset + 3))) << (8 * 3); 35 | offset += sizeof(this->data); 36 | return offset; 37 | } 38 | 39 | const char * getType(){ return "std_msgs/UInt32"; }; 40 | const char * getMD5(){ return "304a39449588c7f8ce2df6e8001c5fce"; }; 41 | 42 | }; 43 | 44 | } 45 | #endif -------------------------------------------------------------------------------- /lib/ros_lib/std_srvs/Empty.h: -------------------------------------------------------------------------------- 1 | #ifndef _ROS_SERVICE_Empty_h 2 | #define _ROS_SERVICE_Empty_h 3 | #include 4 | #include 5 | #include 6 | #include "ros/msg.h" 7 | 8 | namespace std_srvs 9 | { 10 | 11 | static const char EMPTY[] = "std_srvs/Empty"; 12 | 13 | class EmptyRequest : public ros::Msg 14 | { 15 | public: 16 | 17 | virtual int serialize(unsigned char *outbuffer) const 18 | { 19 | int offset = 0; 20 | return offset; 21 | } 22 | 23 | virtual int deserialize(unsigned char *inbuffer) 24 | { 25 | int offset = 0; 26 | return offset; 27 | } 28 | 29 | const char * getType(){ return EMPTY; }; 30 | const char * getMD5(){ return "d41d8cd98f00b204e9800998ecf8427e"; }; 31 | 32 | }; 33 | 34 | class EmptyResponse : public ros::Msg 35 | { 36 | public: 37 | 38 | virtual int serialize(unsigned char *outbuffer) const 39 | { 40 | int offset = 0; 41 | return offset; 42 | } 43 | 44 | virtual int deserialize(unsigned char *inbuffer) 45 | { 46 | int offset = 0; 47 | return offset; 48 | } 49 | 50 | const char * getType(){ return EMPTY; }; 51 | const char * getMD5(){ return "d41d8cd98f00b204e9800998ecf8427e"; }; 52 | 53 | }; 54 | 55 | class Empty { 56 | public: 57 | typedef EmptyRequest Request; 58 | typedef EmptyResponse Response; 59 | }; 60 | 61 | } 62 | #endif 63 | -------------------------------------------------------------------------------- /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 | virtual int serialize(unsigned char *outbuffer) const 18 | { 19 | int offset = 0; 20 | union { 21 | int16_t real; 22 | uint16_t base; 23 | } u_data; 24 | u_data.real = this->data; 25 | *(outbuffer + offset + 0) = (u_data.base >> (8 * 0)) & 0xFF; 26 | *(outbuffer + offset + 1) = (u_data.base >> (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 | union { 35 | int16_t real; 36 | uint16_t base; 37 | } u_data; 38 | u_data.base = 0; 39 | u_data.base |= ((uint16_t) (*(inbuffer + offset + 0))) << (8 * 0); 40 | u_data.base |= ((uint16_t) (*(inbuffer + offset + 1))) << (8 * 1); 41 | this->data = u_data.real; 42 | offset += sizeof(this->data); 43 | return offset; 44 | } 45 | 46 | const char * getType(){ return "std_msgs/Int16"; }; 47 | const char * getMD5(){ return "8524586e34fbd7cb1c08c5f5f1ca0e57"; }; 48 | 49 | }; 50 | 51 | } 52 | #endif -------------------------------------------------------------------------------- /lib/ros_lib/nav_msgs/GetMapActionResult.h: -------------------------------------------------------------------------------- 1 | #ifndef _ROS_nav_msgs_GetMapActionResult_h 2 | #define _ROS_nav_msgs_GetMapActionResult_h 3 | 4 | #include 5 | #include 6 | #include 7 | #include "ros/msg.h" 8 | #include "std_msgs/Header.h" 9 | #include "actionlib_msgs/GoalStatus.h" 10 | #include "nav_msgs/GetMapResult.h" 11 | 12 | namespace nav_msgs 13 | { 14 | 15 | class GetMapActionResult : public ros::Msg 16 | { 17 | public: 18 | std_msgs::Header header; 19 | actionlib_msgs::GoalStatus status; 20 | nav_msgs::GetMapResult result; 21 | 22 | virtual int serialize(unsigned char *outbuffer) const 23 | { 24 | int offset = 0; 25 | offset += this->header.serialize(outbuffer + offset); 26 | offset += this->status.serialize(outbuffer + offset); 27 | offset += this->result.serialize(outbuffer + offset); 28 | return offset; 29 | } 30 | 31 | virtual int deserialize(unsigned char *inbuffer) 32 | { 33 | int offset = 0; 34 | offset += this->header.deserialize(inbuffer + offset); 35 | offset += this->status.deserialize(inbuffer + offset); 36 | offset += this->result.deserialize(inbuffer + offset); 37 | return offset; 38 | } 39 | 40 | const char * getType(){ return "nav_msgs/GetMapActionResult"; }; 41 | const char * getMD5(){ return "ac66e5b9a79bb4bbd33dab245236c892"; }; 42 | 43 | }; 44 | 45 | } 46 | #endif -------------------------------------------------------------------------------- /lib/ros_lib/actionlib/TestActionFeedback.h: -------------------------------------------------------------------------------- 1 | #ifndef _ROS_actionlib_TestActionFeedback_h 2 | #define _ROS_actionlib_TestActionFeedback_h 3 | 4 | #include 5 | #include 6 | #include 7 | #include "ros/msg.h" 8 | #include "std_msgs/Header.h" 9 | #include "actionlib_msgs/GoalStatus.h" 10 | #include "actionlib/TestFeedback.h" 11 | 12 | namespace actionlib 13 | { 14 | 15 | class TestActionFeedback : public ros::Msg 16 | { 17 | public: 18 | std_msgs::Header header; 19 | actionlib_msgs::GoalStatus status; 20 | actionlib::TestFeedback feedback; 21 | 22 | virtual int serialize(unsigned char *outbuffer) const 23 | { 24 | int offset = 0; 25 | offset += this->header.serialize(outbuffer + offset); 26 | offset += this->status.serialize(outbuffer + offset); 27 | offset += this->feedback.serialize(outbuffer + offset); 28 | return offset; 29 | } 30 | 31 | virtual int deserialize(unsigned char *inbuffer) 32 | { 33 | int offset = 0; 34 | offset += this->header.deserialize(inbuffer + offset); 35 | offset += this->status.deserialize(inbuffer + offset); 36 | offset += this->feedback.deserialize(inbuffer + offset); 37 | return offset; 38 | } 39 | 40 | const char * getType(){ return "actionlib/TestActionFeedback"; }; 41 | const char * getMD5(){ return "6d3d0bf7fb3dda24779c010a9f3eb7cb"; }; 42 | 43 | }; 44 | 45 | } 46 | #endif -------------------------------------------------------------------------------- /lib/ros_lib/actionlib/TwoIntsActionResult.h: -------------------------------------------------------------------------------- 1 | #ifndef _ROS_actionlib_TwoIntsActionResult_h 2 | #define _ROS_actionlib_TwoIntsActionResult_h 3 | 4 | #include 5 | #include 6 | #include 7 | #include "ros/msg.h" 8 | #include "std_msgs/Header.h" 9 | #include "actionlib_msgs/GoalStatus.h" 10 | #include "actionlib/TwoIntsResult.h" 11 | 12 | namespace actionlib 13 | { 14 | 15 | class TwoIntsActionResult : public ros::Msg 16 | { 17 | public: 18 | std_msgs::Header header; 19 | actionlib_msgs::GoalStatus status; 20 | actionlib::TwoIntsResult result; 21 | 22 | virtual int serialize(unsigned char *outbuffer) const 23 | { 24 | int offset = 0; 25 | offset += this->header.serialize(outbuffer + offset); 26 | offset += this->status.serialize(outbuffer + offset); 27 | offset += this->result.serialize(outbuffer + offset); 28 | return offset; 29 | } 30 | 31 | virtual int deserialize(unsigned char *inbuffer) 32 | { 33 | int offset = 0; 34 | offset += this->header.deserialize(inbuffer + offset); 35 | offset += this->status.deserialize(inbuffer + offset); 36 | offset += this->result.deserialize(inbuffer + offset); 37 | return offset; 38 | } 39 | 40 | const char * getType(){ return "actionlib/TwoIntsActionResult"; }; 41 | const char * getMD5(){ return "3ba7dea8b8cddcae4528ade4ef74b6e7"; }; 42 | 43 | }; 44 | 45 | } 46 | #endif -------------------------------------------------------------------------------- /lib/ros_lib/actionlib/TestRequestActionGoal.h: -------------------------------------------------------------------------------- 1 | #ifndef _ROS_actionlib_TestRequestActionGoal_h 2 | #define _ROS_actionlib_TestRequestActionGoal_h 3 | 4 | #include 5 | #include 6 | #include 7 | #include "ros/msg.h" 8 | #include "std_msgs/Header.h" 9 | #include "actionlib_msgs/GoalID.h" 10 | #include "actionlib/TestRequestGoal.h" 11 | 12 | namespace actionlib 13 | { 14 | 15 | class TestRequestActionGoal : public ros::Msg 16 | { 17 | public: 18 | std_msgs::Header header; 19 | actionlib_msgs::GoalID goal_id; 20 | actionlib::TestRequestGoal goal; 21 | 22 | virtual int serialize(unsigned char *outbuffer) const 23 | { 24 | int offset = 0; 25 | offset += this->header.serialize(outbuffer + offset); 26 | offset += this->goal_id.serialize(outbuffer + offset); 27 | offset += this->goal.serialize(outbuffer + offset); 28 | return offset; 29 | } 30 | 31 | virtual int deserialize(unsigned char *inbuffer) 32 | { 33 | int offset = 0; 34 | offset += this->header.deserialize(inbuffer + offset); 35 | offset += this->goal_id.deserialize(inbuffer + offset); 36 | offset += this->goal.deserialize(inbuffer + offset); 37 | return offset; 38 | } 39 | 40 | const char * getType(){ return "actionlib/TestRequestActionGoal"; }; 41 | const char * getMD5(){ return "1889556d3fef88f821c7cb004e4251f3"; }; 42 | 43 | }; 44 | 45 | } 46 | #endif -------------------------------------------------------------------------------- /lib/ros_lib/control_msgs/PointHeadActionGoal.h: -------------------------------------------------------------------------------- 1 | #ifndef _ROS_control_msgs_PointHeadActionGoal_h 2 | #define _ROS_control_msgs_PointHeadActionGoal_h 3 | 4 | #include 5 | #include 6 | #include 7 | #include "ros/msg.h" 8 | #include "std_msgs/Header.h" 9 | #include "actionlib_msgs/GoalID.h" 10 | #include "control_msgs/PointHeadGoal.h" 11 | 12 | namespace control_msgs 13 | { 14 | 15 | class PointHeadActionGoal : public ros::Msg 16 | { 17 | public: 18 | std_msgs::Header header; 19 | actionlib_msgs::GoalID goal_id; 20 | control_msgs::PointHeadGoal goal; 21 | 22 | virtual int serialize(unsigned char *outbuffer) const 23 | { 24 | int offset = 0; 25 | offset += this->header.serialize(outbuffer + offset); 26 | offset += this->goal_id.serialize(outbuffer + offset); 27 | offset += this->goal.serialize(outbuffer + offset); 28 | return offset; 29 | } 30 | 31 | virtual int deserialize(unsigned char *inbuffer) 32 | { 33 | int offset = 0; 34 | offset += this->header.deserialize(inbuffer + offset); 35 | offset += this->goal_id.deserialize(inbuffer + offset); 36 | offset += this->goal.deserialize(inbuffer + offset); 37 | return offset; 38 | } 39 | 40 | const char * getType(){ return "control_msgs/PointHeadActionGoal"; }; 41 | const char * getMD5(){ return "b53a8323d0ba7b310ba17a2d3a82a6b8"; }; 42 | 43 | }; 44 | 45 | } 46 | #endif -------------------------------------------------------------------------------- /lib/ros_lib/nav_msgs/GetMapActionFeedback.h: -------------------------------------------------------------------------------- 1 | #ifndef _ROS_nav_msgs_GetMapActionFeedback_h 2 | #define _ROS_nav_msgs_GetMapActionFeedback_h 3 | 4 | #include 5 | #include 6 | #include 7 | #include "ros/msg.h" 8 | #include "std_msgs/Header.h" 9 | #include "actionlib_msgs/GoalStatus.h" 10 | #include "nav_msgs/GetMapFeedback.h" 11 | 12 | namespace nav_msgs 13 | { 14 | 15 | class GetMapActionFeedback : public ros::Msg 16 | { 17 | public: 18 | std_msgs::Header header; 19 | actionlib_msgs::GoalStatus status; 20 | nav_msgs::GetMapFeedback feedback; 21 | 22 | virtual int serialize(unsigned char *outbuffer) const 23 | { 24 | int offset = 0; 25 | offset += this->header.serialize(outbuffer + offset); 26 | offset += this->status.serialize(outbuffer + offset); 27 | offset += this->feedback.serialize(outbuffer + offset); 28 | return offset; 29 | } 30 | 31 | virtual int deserialize(unsigned char *inbuffer) 32 | { 33 | int offset = 0; 34 | offset += this->header.deserialize(inbuffer + offset); 35 | offset += this->status.deserialize(inbuffer + offset); 36 | offset += this->feedback.deserialize(inbuffer + offset); 37 | return offset; 38 | } 39 | 40 | const char * getType(){ return "nav_msgs/GetMapActionFeedback"; }; 41 | const char * getMD5(){ return "aae20e09065c3809e8a8e87c4c8953fd"; }; 42 | 43 | }; 44 | 45 | } 46 | #endif -------------------------------------------------------------------------------- /lib/ros_lib/turtle_actionlib/ShapeActionGoal.h: -------------------------------------------------------------------------------- 1 | #ifndef _ROS_turtle_actionlib_ShapeActionGoal_h 2 | #define _ROS_turtle_actionlib_ShapeActionGoal_h 3 | 4 | #include 5 | #include 6 | #include 7 | #include "ros/msg.h" 8 | #include "std_msgs/Header.h" 9 | #include "actionlib_msgs/GoalID.h" 10 | #include "turtle_actionlib/ShapeGoal.h" 11 | 12 | namespace turtle_actionlib 13 | { 14 | 15 | class ShapeActionGoal : public ros::Msg 16 | { 17 | public: 18 | std_msgs::Header header; 19 | actionlib_msgs::GoalID goal_id; 20 | turtle_actionlib::ShapeGoal goal; 21 | 22 | virtual int serialize(unsigned char *outbuffer) const 23 | { 24 | int offset = 0; 25 | offset += this->header.serialize(outbuffer + offset); 26 | offset += this->goal_id.serialize(outbuffer + offset); 27 | offset += this->goal.serialize(outbuffer + offset); 28 | return offset; 29 | } 30 | 31 | virtual int deserialize(unsigned char *inbuffer) 32 | { 33 | int offset = 0; 34 | offset += this->header.deserialize(inbuffer + offset); 35 | offset += this->goal_id.deserialize(inbuffer + offset); 36 | offset += this->goal.deserialize(inbuffer + offset); 37 | return offset; 38 | } 39 | 40 | const char * getType(){ return "turtle_actionlib/ShapeActionGoal"; }; 41 | const char * getMD5(){ return "dbfccd187f2ec9c593916447ffd6cc77"; }; 42 | 43 | }; 44 | 45 | } 46 | #endif -------------------------------------------------------------------------------- /lib/ros_lib/actionlib/TwoIntsActionFeedback.h: -------------------------------------------------------------------------------- 1 | #ifndef _ROS_actionlib_TwoIntsActionFeedback_h 2 | #define _ROS_actionlib_TwoIntsActionFeedback_h 3 | 4 | #include 5 | #include 6 | #include 7 | #include "ros/msg.h" 8 | #include "std_msgs/Header.h" 9 | #include "actionlib_msgs/GoalStatus.h" 10 | #include "actionlib/TwoIntsFeedback.h" 11 | 12 | namespace actionlib 13 | { 14 | 15 | class TwoIntsActionFeedback : public ros::Msg 16 | { 17 | public: 18 | std_msgs::Header header; 19 | actionlib_msgs::GoalStatus status; 20 | actionlib::TwoIntsFeedback feedback; 21 | 22 | virtual int serialize(unsigned char *outbuffer) const 23 | { 24 | int offset = 0; 25 | offset += this->header.serialize(outbuffer + offset); 26 | offset += this->status.serialize(outbuffer + offset); 27 | offset += this->feedback.serialize(outbuffer + offset); 28 | return offset; 29 | } 30 | 31 | virtual int deserialize(unsigned char *inbuffer) 32 | { 33 | int offset = 0; 34 | offset += this->header.deserialize(inbuffer + offset); 35 | offset += this->status.deserialize(inbuffer + offset); 36 | offset += this->feedback.deserialize(inbuffer + offset); 37 | return offset; 38 | } 39 | 40 | const char * getType(){ return "actionlib/TwoIntsActionFeedback"; }; 41 | const char * getMD5(){ return "aae20e09065c3809e8a8e87c4c8953fd"; }; 42 | 43 | }; 44 | 45 | } 46 | #endif -------------------------------------------------------------------------------- /lib/ros_lib/tf2_msgs/LookupTransformActionGoal.h: -------------------------------------------------------------------------------- 1 | #ifndef _ROS_tf2_msgs_LookupTransformActionGoal_h 2 | #define _ROS_tf2_msgs_LookupTransformActionGoal_h 3 | 4 | #include 5 | #include 6 | #include 7 | #include "ros/msg.h" 8 | #include "std_msgs/Header.h" 9 | #include "actionlib_msgs/GoalID.h" 10 | #include "tf2_msgs/LookupTransformGoal.h" 11 | 12 | namespace tf2_msgs 13 | { 14 | 15 | class LookupTransformActionGoal : public ros::Msg 16 | { 17 | public: 18 | std_msgs::Header header; 19 | actionlib_msgs::GoalID goal_id; 20 | tf2_msgs::LookupTransformGoal goal; 21 | 22 | virtual int serialize(unsigned char *outbuffer) const 23 | { 24 | int offset = 0; 25 | offset += this->header.serialize(outbuffer + offset); 26 | offset += this->goal_id.serialize(outbuffer + offset); 27 | offset += this->goal.serialize(outbuffer + offset); 28 | return offset; 29 | } 30 | 31 | virtual int deserialize(unsigned char *inbuffer) 32 | { 33 | int offset = 0; 34 | offset += this->header.deserialize(inbuffer + offset); 35 | offset += this->goal_id.deserialize(inbuffer + offset); 36 | offset += this->goal.deserialize(inbuffer + offset); 37 | return offset; 38 | } 39 | 40 | const char * getType(){ return "tf2_msgs/LookupTransformActionGoal"; }; 41 | const char * getMD5(){ return "f2e7bcdb75c847978d0351a13e699da5"; }; 42 | 43 | }; 44 | 45 | } 46 | #endif -------------------------------------------------------------------------------- /lib/ros_lib/actionlib/TestRequestActionResult.h: -------------------------------------------------------------------------------- 1 | #ifndef _ROS_actionlib_TestRequestActionResult_h 2 | #define _ROS_actionlib_TestRequestActionResult_h 3 | 4 | #include 5 | #include 6 | #include 7 | #include "ros/msg.h" 8 | #include "std_msgs/Header.h" 9 | #include "actionlib_msgs/GoalStatus.h" 10 | #include "actionlib/TestRequestResult.h" 11 | 12 | namespace actionlib 13 | { 14 | 15 | class TestRequestActionResult : public ros::Msg 16 | { 17 | public: 18 | std_msgs::Header header; 19 | actionlib_msgs::GoalStatus status; 20 | actionlib::TestRequestResult result; 21 | 22 | virtual int serialize(unsigned char *outbuffer) const 23 | { 24 | int offset = 0; 25 | offset += this->header.serialize(outbuffer + offset); 26 | offset += this->status.serialize(outbuffer + offset); 27 | offset += this->result.serialize(outbuffer + offset); 28 | return offset; 29 | } 30 | 31 | virtual int deserialize(unsigned char *inbuffer) 32 | { 33 | int offset = 0; 34 | offset += this->header.deserialize(inbuffer + offset); 35 | offset += this->status.deserialize(inbuffer + offset); 36 | offset += this->result.deserialize(inbuffer + offset); 37 | return offset; 38 | } 39 | 40 | const char * getType(){ return "actionlib/TestRequestActionResult"; }; 41 | const char * getMD5(){ return "0476d1fdf437a3a6e7d6d0e9f5561298"; }; 42 | 43 | }; 44 | 45 | } 46 | #endif -------------------------------------------------------------------------------- /lib/ros_lib/control_msgs/PointHeadActionResult.h: -------------------------------------------------------------------------------- 1 | #ifndef _ROS_control_msgs_PointHeadActionResult_h 2 | #define _ROS_control_msgs_PointHeadActionResult_h 3 | 4 | #include 5 | #include 6 | #include 7 | #include "ros/msg.h" 8 | #include "std_msgs/Header.h" 9 | #include "actionlib_msgs/GoalStatus.h" 10 | #include "control_msgs/PointHeadResult.h" 11 | 12 | namespace control_msgs 13 | { 14 | 15 | class PointHeadActionResult : public ros::Msg 16 | { 17 | public: 18 | std_msgs::Header header; 19 | actionlib_msgs::GoalStatus status; 20 | control_msgs::PointHeadResult result; 21 | 22 | virtual int serialize(unsigned char *outbuffer) const 23 | { 24 | int offset = 0; 25 | offset += this->header.serialize(outbuffer + offset); 26 | offset += this->status.serialize(outbuffer + offset); 27 | offset += this->result.serialize(outbuffer + offset); 28 | return offset; 29 | } 30 | 31 | virtual int deserialize(unsigned char *inbuffer) 32 | { 33 | int offset = 0; 34 | offset += this->header.deserialize(inbuffer + offset); 35 | offset += this->status.deserialize(inbuffer + offset); 36 | offset += this->result.deserialize(inbuffer + offset); 37 | return offset; 38 | } 39 | 40 | const char * getType(){ return "control_msgs/PointHeadActionResult"; }; 41 | const char * getMD5(){ return "1eb06eeff08fa7ea874431638cb52332"; }; 42 | 43 | }; 44 | 45 | } 46 | #endif -------------------------------------------------------------------------------- /lib/ros_lib/turtle_actionlib/ShapeActionResult.h: -------------------------------------------------------------------------------- 1 | #ifndef _ROS_turtle_actionlib_ShapeActionResult_h 2 | #define _ROS_turtle_actionlib_ShapeActionResult_h 3 | 4 | #include 5 | #include 6 | #include 7 | #include "ros/msg.h" 8 | #include "std_msgs/Header.h" 9 | #include "actionlib_msgs/GoalStatus.h" 10 | #include "turtle_actionlib/ShapeResult.h" 11 | 12 | namespace turtle_actionlib 13 | { 14 | 15 | class ShapeActionResult : public ros::Msg 16 | { 17 | public: 18 | std_msgs::Header header; 19 | actionlib_msgs::GoalStatus status; 20 | turtle_actionlib::ShapeResult result; 21 | 22 | virtual int serialize(unsigned char *outbuffer) const 23 | { 24 | int offset = 0; 25 | offset += this->header.serialize(outbuffer + offset); 26 | offset += this->status.serialize(outbuffer + offset); 27 | offset += this->result.serialize(outbuffer + offset); 28 | return offset; 29 | } 30 | 31 | virtual int deserialize(unsigned char *inbuffer) 32 | { 33 | int offset = 0; 34 | offset += this->header.deserialize(inbuffer + offset); 35 | offset += this->status.deserialize(inbuffer + offset); 36 | offset += this->result.deserialize(inbuffer + offset); 37 | return offset; 38 | } 39 | 40 | const char * getType(){ return "turtle_actionlib/ShapeActionResult"; }; 41 | const char * getMD5(){ return "c8d13d5d140f1047a2e4d3bf5c045822"; }; 42 | 43 | }; 44 | 45 | } 46 | #endif -------------------------------------------------------------------------------- /lib/ros_lib/control_msgs/GripperCommandActionGoal.h: -------------------------------------------------------------------------------- 1 | #ifndef _ROS_control_msgs_GripperCommandActionGoal_h 2 | #define _ROS_control_msgs_GripperCommandActionGoal_h 3 | 4 | #include 5 | #include 6 | #include 7 | #include "ros/msg.h" 8 | #include "std_msgs/Header.h" 9 | #include "actionlib_msgs/GoalID.h" 10 | #include "control_msgs/GripperCommandGoal.h" 11 | 12 | namespace control_msgs 13 | { 14 | 15 | class GripperCommandActionGoal : public ros::Msg 16 | { 17 | public: 18 | std_msgs::Header header; 19 | actionlib_msgs::GoalID goal_id; 20 | control_msgs::GripperCommandGoal goal; 21 | 22 | virtual int serialize(unsigned char *outbuffer) const 23 | { 24 | int offset = 0; 25 | offset += this->header.serialize(outbuffer + offset); 26 | offset += this->goal_id.serialize(outbuffer + offset); 27 | offset += this->goal.serialize(outbuffer + offset); 28 | return offset; 29 | } 30 | 31 | virtual int deserialize(unsigned char *inbuffer) 32 | { 33 | int offset = 0; 34 | offset += this->header.deserialize(inbuffer + offset); 35 | offset += this->goal_id.deserialize(inbuffer + offset); 36 | offset += this->goal.deserialize(inbuffer + offset); 37 | return offset; 38 | } 39 | 40 | const char * getType(){ return "control_msgs/GripperCommandActionGoal"; }; 41 | const char * getMD5(){ return "aa581f648a35ed681db2ec0bf7a82bea"; }; 42 | 43 | }; 44 | 45 | } 46 | #endif -------------------------------------------------------------------------------- /lib/ros_lib/actionlib/TestAction.h: -------------------------------------------------------------------------------- 1 | #ifndef _ROS_actionlib_TestAction_h 2 | #define _ROS_actionlib_TestAction_h 3 | 4 | #include 5 | #include 6 | #include 7 | #include "ros/msg.h" 8 | #include "actionlib/TestActionGoal.h" 9 | #include "actionlib/TestActionResult.h" 10 | #include "actionlib/TestActionFeedback.h" 11 | 12 | namespace actionlib 13 | { 14 | 15 | class TestAction : public ros::Msg 16 | { 17 | public: 18 | actionlib::TestActionGoal action_goal; 19 | actionlib::TestActionResult action_result; 20 | actionlib::TestActionFeedback action_feedback; 21 | 22 | virtual int serialize(unsigned char *outbuffer) const 23 | { 24 | int offset = 0; 25 | offset += this->action_goal.serialize(outbuffer + offset); 26 | offset += this->action_result.serialize(outbuffer + offset); 27 | offset += this->action_feedback.serialize(outbuffer + offset); 28 | return offset; 29 | } 30 | 31 | virtual int deserialize(unsigned char *inbuffer) 32 | { 33 | int offset = 0; 34 | offset += this->action_goal.deserialize(inbuffer + offset); 35 | offset += this->action_result.deserialize(inbuffer + offset); 36 | offset += this->action_feedback.deserialize(inbuffer + offset); 37 | return offset; 38 | } 39 | 40 | const char * getType(){ return "actionlib/TestAction"; }; 41 | const char * getMD5(){ return "991e87a72802262dfbe5d1b3cf6efc9a"; }; 42 | 43 | }; 44 | 45 | } 46 | #endif -------------------------------------------------------------------------------- /lib/ros_lib/actionlib/TestRequestActionFeedback.h: -------------------------------------------------------------------------------- 1 | #ifndef _ROS_actionlib_TestRequestActionFeedback_h 2 | #define _ROS_actionlib_TestRequestActionFeedback_h 3 | 4 | #include 5 | #include 6 | #include 7 | #include "ros/msg.h" 8 | #include "std_msgs/Header.h" 9 | #include "actionlib_msgs/GoalStatus.h" 10 | #include "actionlib/TestRequestFeedback.h" 11 | 12 | namespace actionlib 13 | { 14 | 15 | class TestRequestActionFeedback : public ros::Msg 16 | { 17 | public: 18 | std_msgs::Header header; 19 | actionlib_msgs::GoalStatus status; 20 | actionlib::TestRequestFeedback feedback; 21 | 22 | virtual int serialize(unsigned char *outbuffer) const 23 | { 24 | int offset = 0; 25 | offset += this->header.serialize(outbuffer + offset); 26 | offset += this->status.serialize(outbuffer + offset); 27 | offset += this->feedback.serialize(outbuffer + offset); 28 | return offset; 29 | } 30 | 31 | virtual int deserialize(unsigned char *inbuffer) 32 | { 33 | int offset = 0; 34 | offset += this->header.deserialize(inbuffer + offset); 35 | offset += this->status.deserialize(inbuffer + offset); 36 | offset += this->feedback.deserialize(inbuffer + offset); 37 | return offset; 38 | } 39 | 40 | const char * getType(){ return "actionlib/TestRequestActionFeedback"; }; 41 | const char * getMD5(){ return "aae20e09065c3809e8a8e87c4c8953fd"; }; 42 | 43 | }; 44 | 45 | } 46 | #endif -------------------------------------------------------------------------------- /lib/ros_lib/tf2_msgs/LookupTransformActionResult.h: -------------------------------------------------------------------------------- 1 | #ifndef _ROS_tf2_msgs_LookupTransformActionResult_h 2 | #define _ROS_tf2_msgs_LookupTransformActionResult_h 3 | 4 | #include 5 | #include 6 | #include 7 | #include "ros/msg.h" 8 | #include "std_msgs/Header.h" 9 | #include "actionlib_msgs/GoalStatus.h" 10 | #include "tf2_msgs/LookupTransformResult.h" 11 | 12 | namespace tf2_msgs 13 | { 14 | 15 | class LookupTransformActionResult : public ros::Msg 16 | { 17 | public: 18 | std_msgs::Header header; 19 | actionlib_msgs::GoalStatus status; 20 | tf2_msgs::LookupTransformResult result; 21 | 22 | virtual int serialize(unsigned char *outbuffer) const 23 | { 24 | int offset = 0; 25 | offset += this->header.serialize(outbuffer + offset); 26 | offset += this->status.serialize(outbuffer + offset); 27 | offset += this->result.serialize(outbuffer + offset); 28 | return offset; 29 | } 30 | 31 | virtual int deserialize(unsigned char *inbuffer) 32 | { 33 | int offset = 0; 34 | offset += this->header.deserialize(inbuffer + offset); 35 | offset += this->status.deserialize(inbuffer + offset); 36 | offset += this->result.deserialize(inbuffer + offset); 37 | return offset; 38 | } 39 | 40 | const char * getType(){ return "tf2_msgs/LookupTransformActionResult"; }; 41 | const char * getMD5(){ return "ac26ce75a41384fa8bb4dc10f491ab90"; }; 42 | 43 | }; 44 | 45 | } 46 | #endif -------------------------------------------------------------------------------- /lib/ros_lib/control_msgs/JointTrajectoryActionGoal.h: -------------------------------------------------------------------------------- 1 | #ifndef _ROS_control_msgs_JointTrajectoryActionGoal_h 2 | #define _ROS_control_msgs_JointTrajectoryActionGoal_h 3 | 4 | #include 5 | #include 6 | #include 7 | #include "ros/msg.h" 8 | #include "std_msgs/Header.h" 9 | #include "actionlib_msgs/GoalID.h" 10 | #include "control_msgs/JointTrajectoryGoal.h" 11 | 12 | namespace control_msgs 13 | { 14 | 15 | class JointTrajectoryActionGoal : public ros::Msg 16 | { 17 | public: 18 | std_msgs::Header header; 19 | actionlib_msgs::GoalID goal_id; 20 | control_msgs::JointTrajectoryGoal goal; 21 | 22 | virtual int serialize(unsigned char *outbuffer) const 23 | { 24 | int offset = 0; 25 | offset += this->header.serialize(outbuffer + offset); 26 | offset += this->goal_id.serialize(outbuffer + offset); 27 | offset += this->goal.serialize(outbuffer + offset); 28 | return offset; 29 | } 30 | 31 | virtual int deserialize(unsigned char *inbuffer) 32 | { 33 | int offset = 0; 34 | offset += this->header.deserialize(inbuffer + offset); 35 | offset += this->goal_id.deserialize(inbuffer + offset); 36 | offset += this->goal.deserialize(inbuffer + offset); 37 | return offset; 38 | } 39 | 40 | const char * getType(){ return "control_msgs/JointTrajectoryActionGoal"; }; 41 | const char * getMD5(){ return "a99e83ef6185f9fdd7693efe99623a86"; }; 42 | 43 | }; 44 | 45 | } 46 | #endif -------------------------------------------------------------------------------- /lib/ros_lib/control_msgs/PointHeadActionFeedback.h: -------------------------------------------------------------------------------- 1 | #ifndef _ROS_control_msgs_PointHeadActionFeedback_h 2 | #define _ROS_control_msgs_PointHeadActionFeedback_h 3 | 4 | #include 5 | #include 6 | #include 7 | #include "ros/msg.h" 8 | #include "std_msgs/Header.h" 9 | #include "actionlib_msgs/GoalStatus.h" 10 | #include "control_msgs/PointHeadFeedback.h" 11 | 12 | namespace control_msgs 13 | { 14 | 15 | class PointHeadActionFeedback : public ros::Msg 16 | { 17 | public: 18 | std_msgs::Header header; 19 | actionlib_msgs::GoalStatus status; 20 | control_msgs::PointHeadFeedback feedback; 21 | 22 | virtual int serialize(unsigned char *outbuffer) const 23 | { 24 | int offset = 0; 25 | offset += this->header.serialize(outbuffer + offset); 26 | offset += this->status.serialize(outbuffer + offset); 27 | offset += this->feedback.serialize(outbuffer + offset); 28 | return offset; 29 | } 30 | 31 | virtual int deserialize(unsigned char *inbuffer) 32 | { 33 | int offset = 0; 34 | offset += this->header.deserialize(inbuffer + offset); 35 | offset += this->status.deserialize(inbuffer + offset); 36 | offset += this->feedback.deserialize(inbuffer + offset); 37 | return offset; 38 | } 39 | 40 | const char * getType(){ return "control_msgs/PointHeadActionFeedback"; }; 41 | const char * getMD5(){ return "33c9244957176bbba97dd641119e8460"; }; 42 | 43 | }; 44 | 45 | } 46 | #endif -------------------------------------------------------------------------------- /lib/ros_lib/turtle_actionlib/ShapeActionFeedback.h: -------------------------------------------------------------------------------- 1 | #ifndef _ROS_turtle_actionlib_ShapeActionFeedback_h 2 | #define _ROS_turtle_actionlib_ShapeActionFeedback_h 3 | 4 | #include 5 | #include 6 | #include 7 | #include "ros/msg.h" 8 | #include "std_msgs/Header.h" 9 | #include "actionlib_msgs/GoalStatus.h" 10 | #include "turtle_actionlib/ShapeFeedback.h" 11 | 12 | namespace turtle_actionlib 13 | { 14 | 15 | class ShapeActionFeedback : public ros::Msg 16 | { 17 | public: 18 | std_msgs::Header header; 19 | actionlib_msgs::GoalStatus status; 20 | turtle_actionlib::ShapeFeedback feedback; 21 | 22 | virtual int serialize(unsigned char *outbuffer) const 23 | { 24 | int offset = 0; 25 | offset += this->header.serialize(outbuffer + offset); 26 | offset += this->status.serialize(outbuffer + offset); 27 | offset += this->feedback.serialize(outbuffer + offset); 28 | return offset; 29 | } 30 | 31 | virtual int deserialize(unsigned char *inbuffer) 32 | { 33 | int offset = 0; 34 | offset += this->header.deserialize(inbuffer + offset); 35 | offset += this->status.deserialize(inbuffer + offset); 36 | offset += this->feedback.deserialize(inbuffer + offset); 37 | return offset; 38 | } 39 | 40 | const char * getType(){ return "turtle_actionlib/ShapeActionFeedback"; }; 41 | const char * getMD5(){ return "aae20e09065c3809e8a8e87c4c8953fd"; }; 42 | 43 | }; 44 | 45 | } 46 | #endif -------------------------------------------------------------------------------- /lib/ros_lib/actionlib_tutorials/AveragingActionGoal.h: -------------------------------------------------------------------------------- 1 | #ifndef _ROS_actionlib_tutorials_AveragingActionGoal_h 2 | #define _ROS_actionlib_tutorials_AveragingActionGoal_h 3 | 4 | #include 5 | #include 6 | #include 7 | #include "ros/msg.h" 8 | #include "std_msgs/Header.h" 9 | #include "actionlib_msgs/GoalID.h" 10 | #include "actionlib_tutorials/AveragingGoal.h" 11 | 12 | namespace actionlib_tutorials 13 | { 14 | 15 | class AveragingActionGoal : public ros::Msg 16 | { 17 | public: 18 | std_msgs::Header header; 19 | actionlib_msgs::GoalID goal_id; 20 | actionlib_tutorials::AveragingGoal goal; 21 | 22 | virtual int serialize(unsigned char *outbuffer) const 23 | { 24 | int offset = 0; 25 | offset += this->header.serialize(outbuffer + offset); 26 | offset += this->goal_id.serialize(outbuffer + offset); 27 | offset += this->goal.serialize(outbuffer + offset); 28 | return offset; 29 | } 30 | 31 | virtual int deserialize(unsigned char *inbuffer) 32 | { 33 | int offset = 0; 34 | offset += this->header.deserialize(inbuffer + offset); 35 | offset += this->goal_id.deserialize(inbuffer + offset); 36 | offset += this->goal.deserialize(inbuffer + offset); 37 | return offset; 38 | } 39 | 40 | const char * getType(){ return "actionlib_tutorials/AveragingActionGoal"; }; 41 | const char * getMD5(){ return "1561825b734ebd6039851c501e3fb570"; }; 42 | 43 | }; 44 | 45 | } 46 | #endif -------------------------------------------------------------------------------- /lib/ros_lib/actionlib_tutorials/FibonacciActionGoal.h: -------------------------------------------------------------------------------- 1 | #ifndef _ROS_actionlib_tutorials_FibonacciActionGoal_h 2 | #define _ROS_actionlib_tutorials_FibonacciActionGoal_h 3 | 4 | #include 5 | #include 6 | #include 7 | #include "ros/msg.h" 8 | #include "std_msgs/Header.h" 9 | #include "actionlib_msgs/GoalID.h" 10 | #include "actionlib_tutorials/FibonacciGoal.h" 11 | 12 | namespace actionlib_tutorials 13 | { 14 | 15 | class FibonacciActionGoal : public ros::Msg 16 | { 17 | public: 18 | std_msgs::Header header; 19 | actionlib_msgs::GoalID goal_id; 20 | actionlib_tutorials::FibonacciGoal goal; 21 | 22 | virtual int serialize(unsigned char *outbuffer) const 23 | { 24 | int offset = 0; 25 | offset += this->header.serialize(outbuffer + offset); 26 | offset += this->goal_id.serialize(outbuffer + offset); 27 | offset += this->goal.serialize(outbuffer + offset); 28 | return offset; 29 | } 30 | 31 | virtual int deserialize(unsigned char *inbuffer) 32 | { 33 | int offset = 0; 34 | offset += this->header.deserialize(inbuffer + offset); 35 | offset += this->goal_id.deserialize(inbuffer + offset); 36 | offset += this->goal.deserialize(inbuffer + offset); 37 | return offset; 38 | } 39 | 40 | const char * getType(){ return "actionlib_tutorials/FibonacciActionGoal"; }; 41 | const char * getMD5(){ return "006871c7fa1d0e3d5fe2226bf17b2a94"; }; 42 | 43 | }; 44 | 45 | } 46 | #endif -------------------------------------------------------------------------------- /lib/ros_lib/nav_msgs/GetMapAction.h: -------------------------------------------------------------------------------- 1 | #ifndef _ROS_nav_msgs_GetMapAction_h 2 | #define _ROS_nav_msgs_GetMapAction_h 3 | 4 | #include 5 | #include 6 | #include 7 | #include "ros/msg.h" 8 | #include "nav_msgs/GetMapActionGoal.h" 9 | #include "nav_msgs/GetMapActionResult.h" 10 | #include "nav_msgs/GetMapActionFeedback.h" 11 | 12 | namespace nav_msgs 13 | { 14 | 15 | class GetMapAction : public ros::Msg 16 | { 17 | public: 18 | nav_msgs::GetMapActionGoal action_goal; 19 | nav_msgs::GetMapActionResult action_result; 20 | nav_msgs::GetMapActionFeedback action_feedback; 21 | 22 | virtual int serialize(unsigned char *outbuffer) const 23 | { 24 | int offset = 0; 25 | offset += this->action_goal.serialize(outbuffer + offset); 26 | offset += this->action_result.serialize(outbuffer + offset); 27 | offset += this->action_feedback.serialize(outbuffer + offset); 28 | return offset; 29 | } 30 | 31 | virtual int deserialize(unsigned char *inbuffer) 32 | { 33 | int offset = 0; 34 | offset += this->action_goal.deserialize(inbuffer + offset); 35 | offset += this->action_result.deserialize(inbuffer + offset); 36 | offset += this->action_feedback.deserialize(inbuffer + offset); 37 | return offset; 38 | } 39 | 40 | const char * getType(){ return "nav_msgs/GetMapAction"; }; 41 | const char * getMD5(){ return "e611ad23fbf237c031b7536416dc7cd7"; }; 42 | 43 | }; 44 | 45 | } 46 | #endif -------------------------------------------------------------------------------- /lib/ros_lib/control_msgs/GripperCommandActionResult.h: -------------------------------------------------------------------------------- 1 | #ifndef _ROS_control_msgs_GripperCommandActionResult_h 2 | #define _ROS_control_msgs_GripperCommandActionResult_h 3 | 4 | #include 5 | #include 6 | #include 7 | #include "ros/msg.h" 8 | #include "std_msgs/Header.h" 9 | #include "actionlib_msgs/GoalStatus.h" 10 | #include "control_msgs/GripperCommandResult.h" 11 | 12 | namespace control_msgs 13 | { 14 | 15 | class GripperCommandActionResult : public ros::Msg 16 | { 17 | public: 18 | std_msgs::Header header; 19 | actionlib_msgs::GoalStatus status; 20 | control_msgs::GripperCommandResult result; 21 | 22 | virtual int serialize(unsigned char *outbuffer) const 23 | { 24 | int offset = 0; 25 | offset += this->header.serialize(outbuffer + offset); 26 | offset += this->status.serialize(outbuffer + offset); 27 | offset += this->result.serialize(outbuffer + offset); 28 | return offset; 29 | } 30 | 31 | virtual int deserialize(unsigned char *inbuffer) 32 | { 33 | int offset = 0; 34 | offset += this->header.deserialize(inbuffer + offset); 35 | offset += this->status.deserialize(inbuffer + offset); 36 | offset += this->result.deserialize(inbuffer + offset); 37 | return offset; 38 | } 39 | 40 | const char * getType(){ return "control_msgs/GripperCommandActionResult"; }; 41 | const char * getMD5(){ return "143702cb2df0f163c5283cedc5efc6b6"; }; 42 | 43 | }; 44 | 45 | } 46 | #endif -------------------------------------------------------------------------------- /lib/ros_lib/examples/button_example/button_example.pde: -------------------------------------------------------------------------------- 1 | /* 2 | * Button Example for Rosserial 3 | */ 4 | 5 | #include 6 | #include 7 | 8 | 9 | ros::NodeHandle nh; 10 | 11 | std_msgs::Bool pushed_msg; 12 | ros::Publisher pub_button("pushed", &pushed_msg); 13 | 14 | const int button_pin = 7; 15 | const int led_pin = 13; 16 | 17 | bool last_reading; 18 | long last_debounce_time=0; 19 | long debounce_delay=50; 20 | bool published = true; 21 | 22 | void setup() 23 | { 24 | nh.initNode(); 25 | nh.advertise(pub_button); 26 | 27 | //initialize an LED output pin 28 | //and a input pin for our push button 29 | pinMode(led_pin, OUTPUT); 30 | pinMode(button_pin, INPUT); 31 | 32 | //Enable the pullup resistor on the button 33 | digitalWrite(button_pin, HIGH); 34 | 35 | //The button is a normally button 36 | last_reading = ! digitalRead(button_pin); 37 | 38 | } 39 | 40 | void loop() 41 | { 42 | 43 | bool reading = ! digitalRead(button_pin); 44 | 45 | if (last_reading!= reading){ 46 | last_debounce_time = millis(); 47 | published = false; 48 | } 49 | 50 | //if the button value has not changed for the debounce delay, we know its stable 51 | if ( !published && (millis() - last_debounce_time) > debounce_delay) { 52 | digitalWrite(led_pin, reading); 53 | pushed_msg.data = reading; 54 | pub_button.publish(&pushed_msg); 55 | published = true; 56 | } 57 | 58 | last_reading = reading; 59 | 60 | nh.spinOnce(); 61 | } 62 | -------------------------------------------------------------------------------- /lib/ros_lib/tf2_msgs/LookupTransformActionFeedback.h: -------------------------------------------------------------------------------- 1 | #ifndef _ROS_tf2_msgs_LookupTransformActionFeedback_h 2 | #define _ROS_tf2_msgs_LookupTransformActionFeedback_h 3 | 4 | #include 5 | #include 6 | #include 7 | #include "ros/msg.h" 8 | #include "std_msgs/Header.h" 9 | #include "actionlib_msgs/GoalStatus.h" 10 | #include "tf2_msgs/LookupTransformFeedback.h" 11 | 12 | namespace tf2_msgs 13 | { 14 | 15 | class LookupTransformActionFeedback : public ros::Msg 16 | { 17 | public: 18 | std_msgs::Header header; 19 | actionlib_msgs::GoalStatus status; 20 | tf2_msgs::LookupTransformFeedback feedback; 21 | 22 | virtual int serialize(unsigned char *outbuffer) const 23 | { 24 | int offset = 0; 25 | offset += this->header.serialize(outbuffer + offset); 26 | offset += this->status.serialize(outbuffer + offset); 27 | offset += this->feedback.serialize(outbuffer + offset); 28 | return offset; 29 | } 30 | 31 | virtual int deserialize(unsigned char *inbuffer) 32 | { 33 | int offset = 0; 34 | offset += this->header.deserialize(inbuffer + offset); 35 | offset += this->status.deserialize(inbuffer + offset); 36 | offset += this->feedback.deserialize(inbuffer + offset); 37 | return offset; 38 | } 39 | 40 | const char * getType(){ return "tf2_msgs/LookupTransformActionFeedback"; }; 41 | const char * getMD5(){ return "aae20e09065c3809e8a8e87c4c8953fd"; }; 42 | 43 | }; 44 | 45 | } 46 | #endif -------------------------------------------------------------------------------- /lib/ros_lib/control_msgs/JointTrajectoryActionResult.h: -------------------------------------------------------------------------------- 1 | #ifndef _ROS_control_msgs_JointTrajectoryActionResult_h 2 | #define _ROS_control_msgs_JointTrajectoryActionResult_h 3 | 4 | #include 5 | #include 6 | #include 7 | #include "ros/msg.h" 8 | #include "std_msgs/Header.h" 9 | #include "actionlib_msgs/GoalStatus.h" 10 | #include "control_msgs/JointTrajectoryResult.h" 11 | 12 | namespace control_msgs 13 | { 14 | 15 | class JointTrajectoryActionResult : public ros::Msg 16 | { 17 | public: 18 | std_msgs::Header header; 19 | actionlib_msgs::GoalStatus status; 20 | control_msgs::JointTrajectoryResult result; 21 | 22 | virtual int serialize(unsigned char *outbuffer) const 23 | { 24 | int offset = 0; 25 | offset += this->header.serialize(outbuffer + offset); 26 | offset += this->status.serialize(outbuffer + offset); 27 | offset += this->result.serialize(outbuffer + offset); 28 | return offset; 29 | } 30 | 31 | virtual int deserialize(unsigned char *inbuffer) 32 | { 33 | int offset = 0; 34 | offset += this->header.deserialize(inbuffer + offset); 35 | offset += this->status.deserialize(inbuffer + offset); 36 | offset += this->result.deserialize(inbuffer + offset); 37 | return offset; 38 | } 39 | 40 | const char * getType(){ return "control_msgs/JointTrajectoryActionResult"; }; 41 | const char * getMD5(){ return "1eb06eeff08fa7ea874431638cb52332"; }; 42 | 43 | }; 44 | 45 | } 46 | #endif -------------------------------------------------------------------------------- /lib/ros_lib/control_msgs/SingleJointPositionActionGoal.h: -------------------------------------------------------------------------------- 1 | #ifndef _ROS_control_msgs_SingleJointPositionActionGoal_h 2 | #define _ROS_control_msgs_SingleJointPositionActionGoal_h 3 | 4 | #include 5 | #include 6 | #include 7 | #include "ros/msg.h" 8 | #include "std_msgs/Header.h" 9 | #include "actionlib_msgs/GoalID.h" 10 | #include "control_msgs/SingleJointPositionGoal.h" 11 | 12 | namespace control_msgs 13 | { 14 | 15 | class SingleJointPositionActionGoal : public ros::Msg 16 | { 17 | public: 18 | std_msgs::Header header; 19 | actionlib_msgs::GoalID goal_id; 20 | control_msgs::SingleJointPositionGoal goal; 21 | 22 | virtual int serialize(unsigned char *outbuffer) const 23 | { 24 | int offset = 0; 25 | offset += this->header.serialize(outbuffer + offset); 26 | offset += this->goal_id.serialize(outbuffer + offset); 27 | offset += this->goal.serialize(outbuffer + offset); 28 | return offset; 29 | } 30 | 31 | virtual int deserialize(unsigned char *inbuffer) 32 | { 33 | int offset = 0; 34 | offset += this->header.deserialize(inbuffer + offset); 35 | offset += this->goal_id.deserialize(inbuffer + offset); 36 | offset += this->goal.deserialize(inbuffer + offset); 37 | return offset; 38 | } 39 | 40 | const char * getType(){ return "control_msgs/SingleJointPositionActionGoal"; }; 41 | const char * getMD5(){ return "4b0d3d091471663e17749c1d0db90f61"; }; 42 | 43 | }; 44 | 45 | } 46 | #endif -------------------------------------------------------------------------------- /lib/ros_lib/actionlib/TwoIntsAction.h: -------------------------------------------------------------------------------- 1 | #ifndef _ROS_actionlib_TwoIntsAction_h 2 | #define _ROS_actionlib_TwoIntsAction_h 3 | 4 | #include 5 | #include 6 | #include 7 | #include "ros/msg.h" 8 | #include "actionlib/TwoIntsActionGoal.h" 9 | #include "actionlib/TwoIntsActionResult.h" 10 | #include "actionlib/TwoIntsActionFeedback.h" 11 | 12 | namespace actionlib 13 | { 14 | 15 | class TwoIntsAction : public ros::Msg 16 | { 17 | public: 18 | actionlib::TwoIntsActionGoal action_goal; 19 | actionlib::TwoIntsActionResult action_result; 20 | actionlib::TwoIntsActionFeedback action_feedback; 21 | 22 | virtual int serialize(unsigned char *outbuffer) const 23 | { 24 | int offset = 0; 25 | offset += this->action_goal.serialize(outbuffer + offset); 26 | offset += this->action_result.serialize(outbuffer + offset); 27 | offset += this->action_feedback.serialize(outbuffer + offset); 28 | return offset; 29 | } 30 | 31 | virtual int deserialize(unsigned char *inbuffer) 32 | { 33 | int offset = 0; 34 | offset += this->action_goal.deserialize(inbuffer + offset); 35 | offset += this->action_result.deserialize(inbuffer + offset); 36 | offset += this->action_feedback.deserialize(inbuffer + offset); 37 | return offset; 38 | } 39 | 40 | const char * getType(){ return "actionlib/TwoIntsAction"; }; 41 | const char * getMD5(){ return "6d1aa538c4bd6183a2dfb7fcac41ee50"; }; 42 | 43 | }; 44 | 45 | } 46 | #endif -------------------------------------------------------------------------------- /lib/ros_lib/actionlib_tutorials/AveragingActionResult.h: -------------------------------------------------------------------------------- 1 | #ifndef _ROS_actionlib_tutorials_AveragingActionResult_h 2 | #define _ROS_actionlib_tutorials_AveragingActionResult_h 3 | 4 | #include 5 | #include 6 | #include 7 | #include "ros/msg.h" 8 | #include "std_msgs/Header.h" 9 | #include "actionlib_msgs/GoalStatus.h" 10 | #include "actionlib_tutorials/AveragingResult.h" 11 | 12 | namespace actionlib_tutorials 13 | { 14 | 15 | class AveragingActionResult : public ros::Msg 16 | { 17 | public: 18 | std_msgs::Header header; 19 | actionlib_msgs::GoalStatus status; 20 | actionlib_tutorials::AveragingResult result; 21 | 22 | virtual int serialize(unsigned char *outbuffer) const 23 | { 24 | int offset = 0; 25 | offset += this->header.serialize(outbuffer + offset); 26 | offset += this->status.serialize(outbuffer + offset); 27 | offset += this->result.serialize(outbuffer + offset); 28 | return offset; 29 | } 30 | 31 | virtual int deserialize(unsigned char *inbuffer) 32 | { 33 | int offset = 0; 34 | offset += this->header.deserialize(inbuffer + offset); 35 | offset += this->status.deserialize(inbuffer + offset); 36 | offset += this->result.deserialize(inbuffer + offset); 37 | return offset; 38 | } 39 | 40 | const char * getType(){ return "actionlib_tutorials/AveragingActionResult"; }; 41 | const char * getMD5(){ return "8672cb489d347580acdcd05c5d497497"; }; 42 | 43 | }; 44 | 45 | } 46 | #endif -------------------------------------------------------------------------------- /lib/ros_lib/actionlib_tutorials/FibonacciActionResult.h: -------------------------------------------------------------------------------- 1 | #ifndef _ROS_actionlib_tutorials_FibonacciActionResult_h 2 | #define _ROS_actionlib_tutorials_FibonacciActionResult_h 3 | 4 | #include 5 | #include 6 | #include 7 | #include "ros/msg.h" 8 | #include "std_msgs/Header.h" 9 | #include "actionlib_msgs/GoalStatus.h" 10 | #include "actionlib_tutorials/FibonacciResult.h" 11 | 12 | namespace actionlib_tutorials 13 | { 14 | 15 | class FibonacciActionResult : public ros::Msg 16 | { 17 | public: 18 | std_msgs::Header header; 19 | actionlib_msgs::GoalStatus status; 20 | actionlib_tutorials::FibonacciResult result; 21 | 22 | virtual int serialize(unsigned char *outbuffer) const 23 | { 24 | int offset = 0; 25 | offset += this->header.serialize(outbuffer + offset); 26 | offset += this->status.serialize(outbuffer + offset); 27 | offset += this->result.serialize(outbuffer + offset); 28 | return offset; 29 | } 30 | 31 | virtual int deserialize(unsigned char *inbuffer) 32 | { 33 | int offset = 0; 34 | offset += this->header.deserialize(inbuffer + offset); 35 | offset += this->status.deserialize(inbuffer + offset); 36 | offset += this->result.deserialize(inbuffer + offset); 37 | return offset; 38 | } 39 | 40 | const char * getType(){ return "actionlib_tutorials/FibonacciActionResult"; }; 41 | const char * getMD5(){ return "bee73a9fe29ae25e966e105f5553dd03"; }; 42 | 43 | }; 44 | 45 | } 46 | #endif -------------------------------------------------------------------------------- /lib/ros_lib/control_msgs/GripperCommandActionFeedback.h: -------------------------------------------------------------------------------- 1 | #ifndef _ROS_control_msgs_GripperCommandActionFeedback_h 2 | #define _ROS_control_msgs_GripperCommandActionFeedback_h 3 | 4 | #include 5 | #include 6 | #include 7 | #include "ros/msg.h" 8 | #include "std_msgs/Header.h" 9 | #include "actionlib_msgs/GoalStatus.h" 10 | #include "control_msgs/GripperCommandFeedback.h" 11 | 12 | namespace control_msgs 13 | { 14 | 15 | class GripperCommandActionFeedback : public ros::Msg 16 | { 17 | public: 18 | std_msgs::Header header; 19 | actionlib_msgs::GoalStatus status; 20 | control_msgs::GripperCommandFeedback feedback; 21 | 22 | virtual int serialize(unsigned char *outbuffer) const 23 | { 24 | int offset = 0; 25 | offset += this->header.serialize(outbuffer + offset); 26 | offset += this->status.serialize(outbuffer + offset); 27 | offset += this->feedback.serialize(outbuffer + offset); 28 | return offset; 29 | } 30 | 31 | virtual int deserialize(unsigned char *inbuffer) 32 | { 33 | int offset = 0; 34 | offset += this->header.deserialize(inbuffer + offset); 35 | offset += this->status.deserialize(inbuffer + offset); 36 | offset += this->feedback.deserialize(inbuffer + offset); 37 | return offset; 38 | } 39 | 40 | const char * getType(){ return "control_msgs/GripperCommandActionFeedback"; }; 41 | const char * getMD5(){ return "653dff30c045f5e6ff3feb3409f4558d"; }; 42 | 43 | }; 44 | 45 | } 46 | #endif -------------------------------------------------------------------------------- /lib/ros_lib/examples/Ultrasound/Ultrasound.pde: -------------------------------------------------------------------------------- 1 | /* 2 | * rosserial Ultrasound Example 3 | * 4 | * This example is for the Maxbotix Ultrasound rangers. 5 | */ 6 | 7 | #include 8 | #include 9 | #include 10 | 11 | ros::NodeHandle nh; 12 | 13 | sensor_msgs::Range range_msg; 14 | ros::Publisher pub_range( "/ultrasound", &range_msg); 15 | 16 | const int adc_pin = 0; 17 | 18 | char frameid[] = "/ultrasound"; 19 | 20 | float getRange_Ultrasound(int pin_num){ 21 | int val = 0; 22 | for(int i=0; i<4; i++) val += analogRead(pin_num); 23 | float range = val; 24 | return range /322.519685; // (0.0124023437 /4) ; //cvt to meters 25 | } 26 | 27 | void setup() 28 | { 29 | nh.initNode(); 30 | nh.advertise(pub_range); 31 | 32 | 33 | range_msg.radiation_type = sensor_msgs::Range::ULTRASOUND; 34 | range_msg.header.frame_id = frameid; 35 | range_msg.field_of_view = 0.1; // fake 36 | range_msg.min_range = 0.0; 37 | range_msg.max_range = 6.47; 38 | 39 | pinMode(8,OUTPUT); 40 | digitalWrite(8, LOW); 41 | } 42 | 43 | 44 | long range_time; 45 | 46 | void loop() 47 | { 48 | 49 | //publish the adc value every 50 milliseconds 50 | //since it takes that long for the sensor to stablize 51 | if ( millis() >= range_time ){ 52 | int r =0; 53 | 54 | range_msg.range = getRange_Ultrasound(5); 55 | range_msg.header.stamp = nh.now(); 56 | pub_range.publish(&range_msg); 57 | range_time = millis() + 50; 58 | } 59 | 60 | nh.spinOnce(); 61 | } 62 | -------------------------------------------------------------------------------- /lib/ros_lib/control_msgs/FollowJointTrajectoryActionGoal.h: -------------------------------------------------------------------------------- 1 | #ifndef _ROS_control_msgs_FollowJointTrajectoryActionGoal_h 2 | #define _ROS_control_msgs_FollowJointTrajectoryActionGoal_h 3 | 4 | #include 5 | #include 6 | #include 7 | #include "ros/msg.h" 8 | #include "std_msgs/Header.h" 9 | #include "actionlib_msgs/GoalID.h" 10 | #include "control_msgs/FollowJointTrajectoryGoal.h" 11 | 12 | namespace control_msgs 13 | { 14 | 15 | class FollowJointTrajectoryActionGoal : public ros::Msg 16 | { 17 | public: 18 | std_msgs::Header header; 19 | actionlib_msgs::GoalID goal_id; 20 | control_msgs::FollowJointTrajectoryGoal goal; 21 | 22 | virtual int serialize(unsigned char *outbuffer) const 23 | { 24 | int offset = 0; 25 | offset += this->header.serialize(outbuffer + offset); 26 | offset += this->goal_id.serialize(outbuffer + offset); 27 | offset += this->goal.serialize(outbuffer + offset); 28 | return offset; 29 | } 30 | 31 | virtual int deserialize(unsigned char *inbuffer) 32 | { 33 | int offset = 0; 34 | offset += this->header.deserialize(inbuffer + offset); 35 | offset += this->goal_id.deserialize(inbuffer + offset); 36 | offset += this->goal.deserialize(inbuffer + offset); 37 | return offset; 38 | } 39 | 40 | const char * getType(){ return "control_msgs/FollowJointTrajectoryActionGoal"; }; 41 | const char * getMD5(){ return "cff5c1d533bf2f82dd0138d57f4304bb"; }; 42 | 43 | }; 44 | 45 | } 46 | #endif -------------------------------------------------------------------------------- /lib/ros_lib/control_msgs/JointTrajectoryActionFeedback.h: -------------------------------------------------------------------------------- 1 | #ifndef _ROS_control_msgs_JointTrajectoryActionFeedback_h 2 | #define _ROS_control_msgs_JointTrajectoryActionFeedback_h 3 | 4 | #include 5 | #include 6 | #include 7 | #include "ros/msg.h" 8 | #include "std_msgs/Header.h" 9 | #include "actionlib_msgs/GoalStatus.h" 10 | #include "control_msgs/JointTrajectoryFeedback.h" 11 | 12 | namespace control_msgs 13 | { 14 | 15 | class JointTrajectoryActionFeedback : public ros::Msg 16 | { 17 | public: 18 | std_msgs::Header header; 19 | actionlib_msgs::GoalStatus status; 20 | control_msgs::JointTrajectoryFeedback feedback; 21 | 22 | virtual int serialize(unsigned char *outbuffer) const 23 | { 24 | int offset = 0; 25 | offset += this->header.serialize(outbuffer + offset); 26 | offset += this->status.serialize(outbuffer + offset); 27 | offset += this->feedback.serialize(outbuffer + offset); 28 | return offset; 29 | } 30 | 31 | virtual int deserialize(unsigned char *inbuffer) 32 | { 33 | int offset = 0; 34 | offset += this->header.deserialize(inbuffer + offset); 35 | offset += this->status.deserialize(inbuffer + offset); 36 | offset += this->feedback.deserialize(inbuffer + offset); 37 | return offset; 38 | } 39 | 40 | const char * getType(){ return "control_msgs/JointTrajectoryActionFeedback"; }; 41 | const char * getMD5(){ return "aae20e09065c3809e8a8e87c4c8953fd"; }; 42 | 43 | }; 44 | 45 | } 46 | #endif -------------------------------------------------------------------------------- /lib/ros_lib/actionlib_tutorials/AveragingActionFeedback.h: -------------------------------------------------------------------------------- 1 | #ifndef _ROS_actionlib_tutorials_AveragingActionFeedback_h 2 | #define _ROS_actionlib_tutorials_AveragingActionFeedback_h 3 | 4 | #include 5 | #include 6 | #include 7 | #include "ros/msg.h" 8 | #include "std_msgs/Header.h" 9 | #include "actionlib_msgs/GoalStatus.h" 10 | #include "actionlib_tutorials/AveragingFeedback.h" 11 | 12 | namespace actionlib_tutorials 13 | { 14 | 15 | class AveragingActionFeedback : public ros::Msg 16 | { 17 | public: 18 | std_msgs::Header header; 19 | actionlib_msgs::GoalStatus status; 20 | actionlib_tutorials::AveragingFeedback feedback; 21 | 22 | virtual int serialize(unsigned char *outbuffer) const 23 | { 24 | int offset = 0; 25 | offset += this->header.serialize(outbuffer + offset); 26 | offset += this->status.serialize(outbuffer + offset); 27 | offset += this->feedback.serialize(outbuffer + offset); 28 | return offset; 29 | } 30 | 31 | virtual int deserialize(unsigned char *inbuffer) 32 | { 33 | int offset = 0; 34 | offset += this->header.deserialize(inbuffer + offset); 35 | offset += this->status.deserialize(inbuffer + offset); 36 | offset += this->feedback.deserialize(inbuffer + offset); 37 | return offset; 38 | } 39 | 40 | const char * getType(){ return "actionlib_tutorials/AveragingActionFeedback"; }; 41 | const char * getMD5(){ return "78a4a09241b1791069223ae7ebd5b16b"; }; 42 | 43 | }; 44 | 45 | } 46 | #endif -------------------------------------------------------------------------------- /lib/ros_lib/actionlib_tutorials/FibonacciActionFeedback.h: -------------------------------------------------------------------------------- 1 | #ifndef _ROS_actionlib_tutorials_FibonacciActionFeedback_h 2 | #define _ROS_actionlib_tutorials_FibonacciActionFeedback_h 3 | 4 | #include 5 | #include 6 | #include 7 | #include "ros/msg.h" 8 | #include "std_msgs/Header.h" 9 | #include "actionlib_msgs/GoalStatus.h" 10 | #include "actionlib_tutorials/FibonacciFeedback.h" 11 | 12 | namespace actionlib_tutorials 13 | { 14 | 15 | class FibonacciActionFeedback : public ros::Msg 16 | { 17 | public: 18 | std_msgs::Header header; 19 | actionlib_msgs::GoalStatus status; 20 | actionlib_tutorials::FibonacciFeedback feedback; 21 | 22 | virtual int serialize(unsigned char *outbuffer) const 23 | { 24 | int offset = 0; 25 | offset += this->header.serialize(outbuffer + offset); 26 | offset += this->status.serialize(outbuffer + offset); 27 | offset += this->feedback.serialize(outbuffer + offset); 28 | return offset; 29 | } 30 | 31 | virtual int deserialize(unsigned char *inbuffer) 32 | { 33 | int offset = 0; 34 | offset += this->header.deserialize(inbuffer + offset); 35 | offset += this->status.deserialize(inbuffer + offset); 36 | offset += this->feedback.deserialize(inbuffer + offset); 37 | return offset; 38 | } 39 | 40 | const char * getType(){ return "actionlib_tutorials/FibonacciActionFeedback"; }; 41 | const char * getMD5(){ return "73b8497a9f629a31c0020900e4148f07"; }; 42 | 43 | }; 44 | 45 | } 46 | #endif -------------------------------------------------------------------------------- /lib/ros_lib/control_msgs/SingleJointPositionActionResult.h: -------------------------------------------------------------------------------- 1 | #ifndef _ROS_control_msgs_SingleJointPositionActionResult_h 2 | #define _ROS_control_msgs_SingleJointPositionActionResult_h 3 | 4 | #include 5 | #include 6 | #include 7 | #include "ros/msg.h" 8 | #include "std_msgs/Header.h" 9 | #include "actionlib_msgs/GoalStatus.h" 10 | #include "control_msgs/SingleJointPositionResult.h" 11 | 12 | namespace control_msgs 13 | { 14 | 15 | class SingleJointPositionActionResult : public ros::Msg 16 | { 17 | public: 18 | std_msgs::Header header; 19 | actionlib_msgs::GoalStatus status; 20 | control_msgs::SingleJointPositionResult result; 21 | 22 | virtual int serialize(unsigned char *outbuffer) const 23 | { 24 | int offset = 0; 25 | offset += this->header.serialize(outbuffer + offset); 26 | offset += this->status.serialize(outbuffer + offset); 27 | offset += this->result.serialize(outbuffer + offset); 28 | return offset; 29 | } 30 | 31 | virtual int deserialize(unsigned char *inbuffer) 32 | { 33 | int offset = 0; 34 | offset += this->header.deserialize(inbuffer + offset); 35 | offset += this->status.deserialize(inbuffer + offset); 36 | offset += this->result.deserialize(inbuffer + offset); 37 | return offset; 38 | } 39 | 40 | const char * getType(){ return "control_msgs/SingleJointPositionActionResult"; }; 41 | const char * getMD5(){ return "1eb06eeff08fa7ea874431638cb52332"; }; 42 | 43 | }; 44 | 45 | } 46 | #endif -------------------------------------------------------------------------------- /lib/ros_lib/control_msgs/FollowJointTrajectoryActionResult.h: -------------------------------------------------------------------------------- 1 | #ifndef _ROS_control_msgs_FollowJointTrajectoryActionResult_h 2 | #define _ROS_control_msgs_FollowJointTrajectoryActionResult_h 3 | 4 | #include 5 | #include 6 | #include 7 | #include "ros/msg.h" 8 | #include "std_msgs/Header.h" 9 | #include "actionlib_msgs/GoalStatus.h" 10 | #include "control_msgs/FollowJointTrajectoryResult.h" 11 | 12 | namespace control_msgs 13 | { 14 | 15 | class FollowJointTrajectoryActionResult : public ros::Msg 16 | { 17 | public: 18 | std_msgs::Header header; 19 | actionlib_msgs::GoalStatus status; 20 | control_msgs::FollowJointTrajectoryResult result; 21 | 22 | virtual int serialize(unsigned char *outbuffer) const 23 | { 24 | int offset = 0; 25 | offset += this->header.serialize(outbuffer + offset); 26 | offset += this->status.serialize(outbuffer + offset); 27 | offset += this->result.serialize(outbuffer + offset); 28 | return offset; 29 | } 30 | 31 | virtual int deserialize(unsigned char *inbuffer) 32 | { 33 | int offset = 0; 34 | offset += this->header.deserialize(inbuffer + offset); 35 | offset += this->status.deserialize(inbuffer + offset); 36 | offset += this->result.deserialize(inbuffer + offset); 37 | return offset; 38 | } 39 | 40 | const char * getType(){ return "control_msgs/FollowJointTrajectoryActionResult"; }; 41 | const char * getMD5(){ return "c4fb3b000dc9da4fd99699380efcc5d9"; }; 42 | 43 | }; 44 | 45 | } 46 | #endif -------------------------------------------------------------------------------- /lib/ros_lib/actionlib/TestRequestAction.h: -------------------------------------------------------------------------------- 1 | #ifndef _ROS_actionlib_TestRequestAction_h 2 | #define _ROS_actionlib_TestRequestAction_h 3 | 4 | #include 5 | #include 6 | #include 7 | #include "ros/msg.h" 8 | #include "actionlib/TestRequestActionGoal.h" 9 | #include "actionlib/TestRequestActionResult.h" 10 | #include "actionlib/TestRequestActionFeedback.h" 11 | 12 | namespace actionlib 13 | { 14 | 15 | class TestRequestAction : public ros::Msg 16 | { 17 | public: 18 | actionlib::TestRequestActionGoal action_goal; 19 | actionlib::TestRequestActionResult action_result; 20 | actionlib::TestRequestActionFeedback action_feedback; 21 | 22 | virtual int serialize(unsigned char *outbuffer) const 23 | { 24 | int offset = 0; 25 | offset += this->action_goal.serialize(outbuffer + offset); 26 | offset += this->action_result.serialize(outbuffer + offset); 27 | offset += this->action_feedback.serialize(outbuffer + offset); 28 | return offset; 29 | } 30 | 31 | virtual int deserialize(unsigned char *inbuffer) 32 | { 33 | int offset = 0; 34 | offset += this->action_goal.deserialize(inbuffer + offset); 35 | offset += this->action_result.deserialize(inbuffer + offset); 36 | offset += this->action_feedback.deserialize(inbuffer + offset); 37 | return offset; 38 | } 39 | 40 | const char * getType(){ return "actionlib/TestRequestAction"; }; 41 | const char * getMD5(){ return "dc44b1f4045dbf0d1db54423b3b86b30"; }; 42 | 43 | }; 44 | 45 | } 46 | #endif -------------------------------------------------------------------------------- /lib/ros_lib/control_msgs/SingleJointPositionActionFeedback.h: -------------------------------------------------------------------------------- 1 | #ifndef _ROS_control_msgs_SingleJointPositionActionFeedback_h 2 | #define _ROS_control_msgs_SingleJointPositionActionFeedback_h 3 | 4 | #include 5 | #include 6 | #include 7 | #include "ros/msg.h" 8 | #include "std_msgs/Header.h" 9 | #include "actionlib_msgs/GoalStatus.h" 10 | #include "control_msgs/SingleJointPositionFeedback.h" 11 | 12 | namespace control_msgs 13 | { 14 | 15 | class SingleJointPositionActionFeedback : public ros::Msg 16 | { 17 | public: 18 | std_msgs::Header header; 19 | actionlib_msgs::GoalStatus status; 20 | control_msgs::SingleJointPositionFeedback feedback; 21 | 22 | virtual int serialize(unsigned char *outbuffer) const 23 | { 24 | int offset = 0; 25 | offset += this->header.serialize(outbuffer + offset); 26 | offset += this->status.serialize(outbuffer + offset); 27 | offset += this->feedback.serialize(outbuffer + offset); 28 | return offset; 29 | } 30 | 31 | virtual int deserialize(unsigned char *inbuffer) 32 | { 33 | int offset = 0; 34 | offset += this->header.deserialize(inbuffer + offset); 35 | offset += this->status.deserialize(inbuffer + offset); 36 | offset += this->feedback.deserialize(inbuffer + offset); 37 | return offset; 38 | } 39 | 40 | const char * getType(){ return "control_msgs/SingleJointPositionActionFeedback"; }; 41 | const char * getMD5(){ return "3503b7cf8972f90d245850a5d8796cfa"; }; 42 | 43 | }; 44 | 45 | } 46 | #endif -------------------------------------------------------------------------------- /lib/ros_lib/control_msgs/FollowJointTrajectoryActionFeedback.h: -------------------------------------------------------------------------------- 1 | #ifndef _ROS_control_msgs_FollowJointTrajectoryActionFeedback_h 2 | #define _ROS_control_msgs_FollowJointTrajectoryActionFeedback_h 3 | 4 | #include 5 | #include 6 | #include 7 | #include "ros/msg.h" 8 | #include "std_msgs/Header.h" 9 | #include "actionlib_msgs/GoalStatus.h" 10 | #include "control_msgs/FollowJointTrajectoryFeedback.h" 11 | 12 | namespace control_msgs 13 | { 14 | 15 | class FollowJointTrajectoryActionFeedback : public ros::Msg 16 | { 17 | public: 18 | std_msgs::Header header; 19 | actionlib_msgs::GoalStatus status; 20 | control_msgs::FollowJointTrajectoryFeedback feedback; 21 | 22 | virtual int serialize(unsigned char *outbuffer) const 23 | { 24 | int offset = 0; 25 | offset += this->header.serialize(outbuffer + offset); 26 | offset += this->status.serialize(outbuffer + offset); 27 | offset += this->feedback.serialize(outbuffer + offset); 28 | return offset; 29 | } 30 | 31 | virtual int deserialize(unsigned char *inbuffer) 32 | { 33 | int offset = 0; 34 | offset += this->header.deserialize(inbuffer + offset); 35 | offset += this->status.deserialize(inbuffer + offset); 36 | offset += this->feedback.deserialize(inbuffer + offset); 37 | return offset; 38 | } 39 | 40 | const char * getType(){ return "control_msgs/FollowJointTrajectoryActionFeedback"; }; 41 | const char * getMD5(){ return "d8920dc4eae9fc107e00999cce4be641"; }; 42 | 43 | }; 44 | 45 | } 46 | #endif -------------------------------------------------------------------------------- /lib/ros_lib/control_msgs/PointHeadAction.h: -------------------------------------------------------------------------------- 1 | #ifndef _ROS_control_msgs_PointHeadAction_h 2 | #define _ROS_control_msgs_PointHeadAction_h 3 | 4 | #include 5 | #include 6 | #include 7 | #include "ros/msg.h" 8 | #include "control_msgs/PointHeadActionGoal.h" 9 | #include "control_msgs/PointHeadActionResult.h" 10 | #include "control_msgs/PointHeadActionFeedback.h" 11 | 12 | namespace control_msgs 13 | { 14 | 15 | class PointHeadAction : public ros::Msg 16 | { 17 | public: 18 | control_msgs::PointHeadActionGoal action_goal; 19 | control_msgs::PointHeadActionResult action_result; 20 | control_msgs::PointHeadActionFeedback action_feedback; 21 | 22 | virtual int serialize(unsigned char *outbuffer) const 23 | { 24 | int offset = 0; 25 | offset += this->action_goal.serialize(outbuffer + offset); 26 | offset += this->action_result.serialize(outbuffer + offset); 27 | offset += this->action_feedback.serialize(outbuffer + offset); 28 | return offset; 29 | } 30 | 31 | virtual int deserialize(unsigned char *inbuffer) 32 | { 33 | int offset = 0; 34 | offset += this->action_goal.deserialize(inbuffer + offset); 35 | offset += this->action_result.deserialize(inbuffer + offset); 36 | offset += this->action_feedback.deserialize(inbuffer + offset); 37 | return offset; 38 | } 39 | 40 | const char * getType(){ return "control_msgs/PointHeadAction"; }; 41 | const char * getMD5(){ return "7252920f1243de1b741f14f214125371"; }; 42 | 43 | }; 44 | 45 | } 46 | #endif -------------------------------------------------------------------------------- /lib/ros_lib/turtle_actionlib/ShapeAction.h: -------------------------------------------------------------------------------- 1 | #ifndef _ROS_turtle_actionlib_ShapeAction_h 2 | #define _ROS_turtle_actionlib_ShapeAction_h 3 | 4 | #include 5 | #include 6 | #include 7 | #include "ros/msg.h" 8 | #include "turtle_actionlib/ShapeActionGoal.h" 9 | #include "turtle_actionlib/ShapeActionResult.h" 10 | #include "turtle_actionlib/ShapeActionFeedback.h" 11 | 12 | namespace turtle_actionlib 13 | { 14 | 15 | class ShapeAction : public ros::Msg 16 | { 17 | public: 18 | turtle_actionlib::ShapeActionGoal action_goal; 19 | turtle_actionlib::ShapeActionResult action_result; 20 | turtle_actionlib::ShapeActionFeedback action_feedback; 21 | 22 | virtual int serialize(unsigned char *outbuffer) const 23 | { 24 | int offset = 0; 25 | offset += this->action_goal.serialize(outbuffer + offset); 26 | offset += this->action_result.serialize(outbuffer + offset); 27 | offset += this->action_feedback.serialize(outbuffer + offset); 28 | return offset; 29 | } 30 | 31 | virtual int deserialize(unsigned char *inbuffer) 32 | { 33 | int offset = 0; 34 | offset += this->action_goal.deserialize(inbuffer + offset); 35 | offset += this->action_result.deserialize(inbuffer + offset); 36 | offset += this->action_feedback.deserialize(inbuffer + offset); 37 | return offset; 38 | } 39 | 40 | const char * getType(){ return "turtle_actionlib/ShapeAction"; }; 41 | const char * getMD5(){ return "d73b17d6237a925511f5d7727a1dc903"; }; 42 | 43 | }; 44 | 45 | } 46 | #endif -------------------------------------------------------------------------------- /LICENSE: -------------------------------------------------------------------------------- 1 | Copyright (c) 2014, spiralray 2 | All rights reserved. 3 | 4 | Redistribution and use in source and binary forms, with or without 5 | modification, are permitted provided that the following conditions are met: 6 | 7 | * Redistributions of source code must retain the above copyright notice, this 8 | list of conditions and the following disclaimer. 9 | 10 | * Redistributions in binary form must reproduce the above copyright notice, 11 | this list of conditions and the following disclaimer in the documentation 12 | and/or other materials provided with the distribution. 13 | 14 | * Neither the name of stm32f1_rosserial nor the names of its 15 | contributors may be used to endorse or promote products derived from 16 | this software without specific prior written permission. 17 | 18 | THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" 19 | AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE 20 | IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE 21 | DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE 22 | FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL 23 | DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR 24 | SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER 25 | CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, 26 | OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE 27 | OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. 28 | 29 | -------------------------------------------------------------------------------- /lib/ros_lib/tf2_msgs/LookupTransformAction.h: -------------------------------------------------------------------------------- 1 | #ifndef _ROS_tf2_msgs_LookupTransformAction_h 2 | #define _ROS_tf2_msgs_LookupTransformAction_h 3 | 4 | #include 5 | #include 6 | #include 7 | #include "ros/msg.h" 8 | #include "tf2_msgs/LookupTransformActionGoal.h" 9 | #include "tf2_msgs/LookupTransformActionResult.h" 10 | #include "tf2_msgs/LookupTransformActionFeedback.h" 11 | 12 | namespace tf2_msgs 13 | { 14 | 15 | class LookupTransformAction : public ros::Msg 16 | { 17 | public: 18 | tf2_msgs::LookupTransformActionGoal action_goal; 19 | tf2_msgs::LookupTransformActionResult action_result; 20 | tf2_msgs::LookupTransformActionFeedback action_feedback; 21 | 22 | virtual int serialize(unsigned char *outbuffer) const 23 | { 24 | int offset = 0; 25 | offset += this->action_goal.serialize(outbuffer + offset); 26 | offset += this->action_result.serialize(outbuffer + offset); 27 | offset += this->action_feedback.serialize(outbuffer + offset); 28 | return offset; 29 | } 30 | 31 | virtual int deserialize(unsigned char *inbuffer) 32 | { 33 | int offset = 0; 34 | offset += this->action_goal.deserialize(inbuffer + offset); 35 | offset += this->action_result.deserialize(inbuffer + offset); 36 | offset += this->action_feedback.deserialize(inbuffer + offset); 37 | return offset; 38 | } 39 | 40 | const char * getType(){ return "tf2_msgs/LookupTransformAction"; }; 41 | const char * getMD5(){ return "7ee01ba91a56c2245c610992dbaa3c37"; }; 42 | 43 | }; 44 | 45 | } 46 | #endif -------------------------------------------------------------------------------- /src/main.cpp: -------------------------------------------------------------------------------- 1 | /* Includes ------------------------------------------------------------------*/ 2 | #include "config/stm32plus.h" 3 | #include "config/gpio.h" 4 | #include "config/timing.h" 5 | 6 | #include "ros.h" 7 | #include 8 | 9 | 10 | using namespace stm32plus; 11 | 12 | /* Defines -------------------------------------------------------------------*/ 13 | 14 | /* Variables -----------------------------------------------------------------*/ 15 | 16 | /* Constants -----------------------------------------------------------------*/ 17 | 18 | /* Function prototypes -------------------------------------------------------*/ 19 | 20 | /* Functions -----------------------------------------------------------------*/ 21 | 22 | /**************************************************************************/ 23 | /*! 24 | @brief Main Program. 25 | @param None. 26 | @retval None. 27 | */ 28 | /**************************************************************************/ 29 | int main(void) 30 | { 31 | //Initialise Systick 32 | MillisecondTimer::initialise(); 33 | Nvic::initialise(); 34 | 35 | ros::NodeHandle nh; 36 | 37 | std_msgs::String str_msg; 38 | ros::Publisher chatter("chatter", &str_msg); 39 | 40 | char hello[13] = "hello world!"; 41 | 42 | nh.initNode(); 43 | nh.advertise(chatter); 44 | while(1){ 45 | str_msg.data = hello; 46 | chatter.publish( &str_msg ); 47 | nh.spinOnce(); 48 | MillisecondTimer::delay(1000); 49 | } 50 | 51 | } 52 | 53 | /* End Of File ---------------------------------------------------------------*/ 54 | 55 | -------------------------------------------------------------------------------- /lib/ros_lib/control_msgs/GripperCommandAction.h: -------------------------------------------------------------------------------- 1 | #ifndef _ROS_control_msgs_GripperCommandAction_h 2 | #define _ROS_control_msgs_GripperCommandAction_h 3 | 4 | #include 5 | #include 6 | #include 7 | #include "ros/msg.h" 8 | #include "control_msgs/GripperCommandActionGoal.h" 9 | #include "control_msgs/GripperCommandActionResult.h" 10 | #include "control_msgs/GripperCommandActionFeedback.h" 11 | 12 | namespace control_msgs 13 | { 14 | 15 | class GripperCommandAction : public ros::Msg 16 | { 17 | public: 18 | control_msgs::GripperCommandActionGoal action_goal; 19 | control_msgs::GripperCommandActionResult action_result; 20 | control_msgs::GripperCommandActionFeedback action_feedback; 21 | 22 | virtual int serialize(unsigned char *outbuffer) const 23 | { 24 | int offset = 0; 25 | offset += this->action_goal.serialize(outbuffer + offset); 26 | offset += this->action_result.serialize(outbuffer + offset); 27 | offset += this->action_feedback.serialize(outbuffer + offset); 28 | return offset; 29 | } 30 | 31 | virtual int deserialize(unsigned char *inbuffer) 32 | { 33 | int offset = 0; 34 | offset += this->action_goal.deserialize(inbuffer + offset); 35 | offset += this->action_result.deserialize(inbuffer + offset); 36 | offset += this->action_feedback.deserialize(inbuffer + offset); 37 | return offset; 38 | } 39 | 40 | const char * getType(){ return "control_msgs/GripperCommandAction"; }; 41 | const char * getMD5(){ return "950b2a6ebe831f5d4f4ceaba3d8be01e"; }; 42 | 43 | }; 44 | 45 | } 46 | #endif -------------------------------------------------------------------------------- /lib/ros_lib/nav_msgs/GetMap.h: -------------------------------------------------------------------------------- 1 | #ifndef _ROS_SERVICE_GetMap_h 2 | #define _ROS_SERVICE_GetMap_h 3 | #include 4 | #include 5 | #include 6 | #include "ros/msg.h" 7 | #include "nav_msgs/OccupancyGrid.h" 8 | 9 | namespace nav_msgs 10 | { 11 | 12 | static const char GETMAP[] = "nav_msgs/GetMap"; 13 | 14 | class GetMapRequest : public ros::Msg 15 | { 16 | public: 17 | 18 | virtual int serialize(unsigned char *outbuffer) const 19 | { 20 | int offset = 0; 21 | return offset; 22 | } 23 | 24 | virtual int deserialize(unsigned char *inbuffer) 25 | { 26 | int offset = 0; 27 | return offset; 28 | } 29 | 30 | const char * getType(){ return GETMAP; }; 31 | const char * getMD5(){ return "d41d8cd98f00b204e9800998ecf8427e"; }; 32 | 33 | }; 34 | 35 | class GetMapResponse : public ros::Msg 36 | { 37 | public: 38 | nav_msgs::OccupancyGrid map; 39 | 40 | virtual int serialize(unsigned char *outbuffer) const 41 | { 42 | int offset = 0; 43 | offset += this->map.serialize(outbuffer + offset); 44 | return offset; 45 | } 46 | 47 | virtual int deserialize(unsigned char *inbuffer) 48 | { 49 | int offset = 0; 50 | offset += this->map.deserialize(inbuffer + offset); 51 | return offset; 52 | } 53 | 54 | const char * getType(){ return GETMAP; }; 55 | const char * getMD5(){ return "6cdd0a18e0aff5b0a3ca2326a89b54ff"; }; 56 | 57 | }; 58 | 59 | class GetMap { 60 | public: 61 | typedef GetMapRequest Request; 62 | typedef GetMapResponse Response; 63 | }; 64 | 65 | } 66 | #endif 67 | -------------------------------------------------------------------------------- /lib/ros_lib/control_msgs/JointTrajectoryAction.h: -------------------------------------------------------------------------------- 1 | #ifndef _ROS_control_msgs_JointTrajectoryAction_h 2 | #define _ROS_control_msgs_JointTrajectoryAction_h 3 | 4 | #include 5 | #include 6 | #include 7 | #include "ros/msg.h" 8 | #include "control_msgs/JointTrajectoryActionGoal.h" 9 | #include "control_msgs/JointTrajectoryActionResult.h" 10 | #include "control_msgs/JointTrajectoryActionFeedback.h" 11 | 12 | namespace control_msgs 13 | { 14 | 15 | class JointTrajectoryAction : public ros::Msg 16 | { 17 | public: 18 | control_msgs::JointTrajectoryActionGoal action_goal; 19 | control_msgs::JointTrajectoryActionResult action_result; 20 | control_msgs::JointTrajectoryActionFeedback action_feedback; 21 | 22 | virtual int serialize(unsigned char *outbuffer) const 23 | { 24 | int offset = 0; 25 | offset += this->action_goal.serialize(outbuffer + offset); 26 | offset += this->action_result.serialize(outbuffer + offset); 27 | offset += this->action_feedback.serialize(outbuffer + offset); 28 | return offset; 29 | } 30 | 31 | virtual int deserialize(unsigned char *inbuffer) 32 | { 33 | int offset = 0; 34 | offset += this->action_goal.deserialize(inbuffer + offset); 35 | offset += this->action_result.deserialize(inbuffer + offset); 36 | offset += this->action_feedback.deserialize(inbuffer + offset); 37 | return offset; 38 | } 39 | 40 | const char * getType(){ return "control_msgs/JointTrajectoryAction"; }; 41 | const char * getMD5(){ return "a04ba3ee8f6a2d0985a6aeaf23d9d7ad"; }; 42 | 43 | }; 44 | 45 | } 46 | #endif -------------------------------------------------------------------------------- /lib/ros_lib/actionlib_tutorials/AveragingAction.h: -------------------------------------------------------------------------------- 1 | #ifndef _ROS_actionlib_tutorials_AveragingAction_h 2 | #define _ROS_actionlib_tutorials_AveragingAction_h 3 | 4 | #include 5 | #include 6 | #include 7 | #include "ros/msg.h" 8 | #include "actionlib_tutorials/AveragingActionGoal.h" 9 | #include "actionlib_tutorials/AveragingActionResult.h" 10 | #include "actionlib_tutorials/AveragingActionFeedback.h" 11 | 12 | namespace actionlib_tutorials 13 | { 14 | 15 | class AveragingAction : public ros::Msg 16 | { 17 | public: 18 | actionlib_tutorials::AveragingActionGoal action_goal; 19 | actionlib_tutorials::AveragingActionResult action_result; 20 | actionlib_tutorials::AveragingActionFeedback action_feedback; 21 | 22 | virtual int serialize(unsigned char *outbuffer) const 23 | { 24 | int offset = 0; 25 | offset += this->action_goal.serialize(outbuffer + offset); 26 | offset += this->action_result.serialize(outbuffer + offset); 27 | offset += this->action_feedback.serialize(outbuffer + offset); 28 | return offset; 29 | } 30 | 31 | virtual int deserialize(unsigned char *inbuffer) 32 | { 33 | int offset = 0; 34 | offset += this->action_goal.deserialize(inbuffer + offset); 35 | offset += this->action_result.deserialize(inbuffer + offset); 36 | offset += this->action_feedback.deserialize(inbuffer + offset); 37 | return offset; 38 | } 39 | 40 | const char * getType(){ return "actionlib_tutorials/AveragingAction"; }; 41 | const char * getMD5(){ return "628678f2b4fa6a5951746a4a2d39e716"; }; 42 | 43 | }; 44 | 45 | } 46 | #endif -------------------------------------------------------------------------------- /lib/ros_lib/actionlib_tutorials/FibonacciAction.h: -------------------------------------------------------------------------------- 1 | #ifndef _ROS_actionlib_tutorials_FibonacciAction_h 2 | #define _ROS_actionlib_tutorials_FibonacciAction_h 3 | 4 | #include 5 | #include 6 | #include 7 | #include "ros/msg.h" 8 | #include "actionlib_tutorials/FibonacciActionGoal.h" 9 | #include "actionlib_tutorials/FibonacciActionResult.h" 10 | #include "actionlib_tutorials/FibonacciActionFeedback.h" 11 | 12 | namespace actionlib_tutorials 13 | { 14 | 15 | class FibonacciAction : public ros::Msg 16 | { 17 | public: 18 | actionlib_tutorials::FibonacciActionGoal action_goal; 19 | actionlib_tutorials::FibonacciActionResult action_result; 20 | actionlib_tutorials::FibonacciActionFeedback action_feedback; 21 | 22 | virtual int serialize(unsigned char *outbuffer) const 23 | { 24 | int offset = 0; 25 | offset += this->action_goal.serialize(outbuffer + offset); 26 | offset += this->action_result.serialize(outbuffer + offset); 27 | offset += this->action_feedback.serialize(outbuffer + offset); 28 | return offset; 29 | } 30 | 31 | virtual int deserialize(unsigned char *inbuffer) 32 | { 33 | int offset = 0; 34 | offset += this->action_goal.deserialize(inbuffer + offset); 35 | offset += this->action_result.deserialize(inbuffer + offset); 36 | offset += this->action_feedback.deserialize(inbuffer + offset); 37 | return offset; 38 | } 39 | 40 | const char * getType(){ return "actionlib_tutorials/FibonacciAction"; }; 41 | const char * getMD5(){ return "f59df5767bf7634684781c92598b2406"; }; 42 | 43 | }; 44 | 45 | } 46 | #endif -------------------------------------------------------------------------------- /lib/ros_lib/rospy_tutorials/HeaderString.h: -------------------------------------------------------------------------------- 1 | #ifndef _ROS_rospy_tutorials_HeaderString_h 2 | #define _ROS_rospy_tutorials_HeaderString_h 3 | 4 | #include 5 | #include 6 | #include 7 | #include "ros/msg.h" 8 | #include "std_msgs/Header.h" 9 | 10 | namespace rospy_tutorials 11 | { 12 | 13 | class HeaderString : public ros::Msg 14 | { 15 | public: 16 | std_msgs::Header header; 17 | const char* data; 18 | 19 | virtual int serialize(unsigned char *outbuffer) const 20 | { 21 | int offset = 0; 22 | offset += this->header.serialize(outbuffer + offset); 23 | uint32_t length_data = strlen(this->data); 24 | memcpy(outbuffer + offset, &length_data, sizeof(uint32_t)); 25 | offset += 4; 26 | memcpy(outbuffer + offset, this->data, length_data); 27 | offset += length_data; 28 | return offset; 29 | } 30 | 31 | virtual int deserialize(unsigned char *inbuffer) 32 | { 33 | int offset = 0; 34 | offset += this->header.deserialize(inbuffer + offset); 35 | uint32_t length_data; 36 | memcpy(&length_data, (inbuffer + offset), sizeof(uint32_t)); 37 | offset += 4; 38 | for(unsigned int k= offset; k< offset+length_data; ++k){ 39 | inbuffer[k-1]=inbuffer[k]; 40 | } 41 | inbuffer[offset+length_data-1]=0; 42 | this->data = (char *)(inbuffer + offset-1); 43 | offset += length_data; 44 | return offset; 45 | } 46 | 47 | const char * getType(){ return "rospy_tutorials/HeaderString"; }; 48 | const char * getMD5(){ return "c99a9440709e4d4a9716d55b8270d5e7"; }; 49 | 50 | }; 51 | 52 | } 53 | #endif -------------------------------------------------------------------------------- /lib/ros_lib/control_msgs/SingleJointPositionAction.h: -------------------------------------------------------------------------------- 1 | #ifndef _ROS_control_msgs_SingleJointPositionAction_h 2 | #define _ROS_control_msgs_SingleJointPositionAction_h 3 | 4 | #include 5 | #include 6 | #include 7 | #include "ros/msg.h" 8 | #include "control_msgs/SingleJointPositionActionGoal.h" 9 | #include "control_msgs/SingleJointPositionActionResult.h" 10 | #include "control_msgs/SingleJointPositionActionFeedback.h" 11 | 12 | namespace control_msgs 13 | { 14 | 15 | class SingleJointPositionAction : public ros::Msg 16 | { 17 | public: 18 | control_msgs::SingleJointPositionActionGoal action_goal; 19 | control_msgs::SingleJointPositionActionResult action_result; 20 | control_msgs::SingleJointPositionActionFeedback action_feedback; 21 | 22 | virtual int serialize(unsigned char *outbuffer) const 23 | { 24 | int offset = 0; 25 | offset += this->action_goal.serialize(outbuffer + offset); 26 | offset += this->action_result.serialize(outbuffer + offset); 27 | offset += this->action_feedback.serialize(outbuffer + offset); 28 | return offset; 29 | } 30 | 31 | virtual int deserialize(unsigned char *inbuffer) 32 | { 33 | int offset = 0; 34 | offset += this->action_goal.deserialize(inbuffer + offset); 35 | offset += this->action_result.deserialize(inbuffer + offset); 36 | offset += this->action_feedback.deserialize(inbuffer + offset); 37 | return offset; 38 | } 39 | 40 | const char * getType(){ return "control_msgs/SingleJointPositionAction"; }; 41 | const char * getMD5(){ return "c4a786b7d53e5d0983decf967a5a779e"; }; 42 | 43 | }; 44 | 45 | } 46 | #endif -------------------------------------------------------------------------------- /lib/ros_lib/control_msgs/FollowJointTrajectoryAction.h: -------------------------------------------------------------------------------- 1 | #ifndef _ROS_control_msgs_FollowJointTrajectoryAction_h 2 | #define _ROS_control_msgs_FollowJointTrajectoryAction_h 3 | 4 | #include 5 | #include 6 | #include 7 | #include "ros/msg.h" 8 | #include "control_msgs/FollowJointTrajectoryActionGoal.h" 9 | #include "control_msgs/FollowJointTrajectoryActionResult.h" 10 | #include "control_msgs/FollowJointTrajectoryActionFeedback.h" 11 | 12 | namespace control_msgs 13 | { 14 | 15 | class FollowJointTrajectoryAction : public ros::Msg 16 | { 17 | public: 18 | control_msgs::FollowJointTrajectoryActionGoal action_goal; 19 | control_msgs::FollowJointTrajectoryActionResult action_result; 20 | control_msgs::FollowJointTrajectoryActionFeedback action_feedback; 21 | 22 | virtual int serialize(unsigned char *outbuffer) const 23 | { 24 | int offset = 0; 25 | offset += this->action_goal.serialize(outbuffer + offset); 26 | offset += this->action_result.serialize(outbuffer + offset); 27 | offset += this->action_feedback.serialize(outbuffer + offset); 28 | return offset; 29 | } 30 | 31 | virtual int deserialize(unsigned char *inbuffer) 32 | { 33 | int offset = 0; 34 | offset += this->action_goal.deserialize(inbuffer + offset); 35 | offset += this->action_result.deserialize(inbuffer + offset); 36 | offset += this->action_feedback.deserialize(inbuffer + offset); 37 | return offset; 38 | } 39 | 40 | const char * getType(){ return "control_msgs/FollowJointTrajectoryAction"; }; 41 | const char * getMD5(){ return "bc4f9b743838566551c0390c65f1a248"; }; 42 | 43 | }; 44 | 45 | } 46 | #endif -------------------------------------------------------------------------------- /lib/ros_lib/shape_msgs/MeshTriangle.h: -------------------------------------------------------------------------------- 1 | #ifndef _ROS_shape_msgs_MeshTriangle_h 2 | #define _ROS_shape_msgs_MeshTriangle_h 3 | 4 | #include 5 | #include 6 | #include 7 | #include "ros/msg.h" 8 | 9 | namespace shape_msgs 10 | { 11 | 12 | class MeshTriangle : public ros::Msg 13 | { 14 | public: 15 | uint32_t vertex_indices[3]; 16 | 17 | virtual int serialize(unsigned char *outbuffer) const 18 | { 19 | int offset = 0; 20 | for( uint8_t i = 0; i < 3; i++){ 21 | *(outbuffer + offset + 0) = (this->vertex_indices[i] >> (8 * 0)) & 0xFF; 22 | *(outbuffer + offset + 1) = (this->vertex_indices[i] >> (8 * 1)) & 0xFF; 23 | *(outbuffer + offset + 2) = (this->vertex_indices[i] >> (8 * 2)) & 0xFF; 24 | *(outbuffer + offset + 3) = (this->vertex_indices[i] >> (8 * 3)) & 0xFF; 25 | offset += sizeof(this->vertex_indices[i]); 26 | } 27 | return offset; 28 | } 29 | 30 | virtual int deserialize(unsigned char *inbuffer) 31 | { 32 | int offset = 0; 33 | for( uint8_t i = 0; i < 3; i++){ 34 | this->vertex_indices[i] = ((uint32_t) (*(inbuffer + offset))); 35 | this->vertex_indices[i] |= ((uint32_t) (*(inbuffer + offset + 1))) << (8 * 1); 36 | this->vertex_indices[i] |= ((uint32_t) (*(inbuffer + offset + 2))) << (8 * 2); 37 | this->vertex_indices[i] |= ((uint32_t) (*(inbuffer + offset + 3))) << (8 * 3); 38 | offset += sizeof(this->vertex_indices[i]); 39 | } 40 | return offset; 41 | } 42 | 43 | const char * getType(){ return "shape_msgs/MeshTriangle"; }; 44 | const char * getMD5(){ return "23688b2e6d2de3d32fe8af104a903253"; }; 45 | 46 | }; 47 | 48 | } 49 | #endif -------------------------------------------------------------------------------- /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 | virtual int serialize(unsigned char *outbuffer) const 18 | { 19 | int offset = 0; 20 | union { 21 | int32_t real; 22 | uint32_t base; 23 | } u_data; 24 | u_data.real = this->data; 25 | *(outbuffer + offset + 0) = (u_data.base >> (8 * 0)) & 0xFF; 26 | *(outbuffer + offset + 1) = (u_data.base >> (8 * 1)) & 0xFF; 27 | *(outbuffer + offset + 2) = (u_data.base >> (8 * 2)) & 0xFF; 28 | *(outbuffer + offset + 3) = (u_data.base >> (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 | union { 37 | int32_t real; 38 | uint32_t base; 39 | } u_data; 40 | u_data.base = 0; 41 | u_data.base |= ((uint32_t) (*(inbuffer + offset + 0))) << (8 * 0); 42 | u_data.base |= ((uint32_t) (*(inbuffer + offset + 1))) << (8 * 1); 43 | u_data.base |= ((uint32_t) (*(inbuffer + offset + 2))) << (8 * 2); 44 | u_data.base |= ((uint32_t) (*(inbuffer + offset + 3))) << (8 * 3); 45 | this->data = u_data.real; 46 | offset += sizeof(this->data); 47 | return offset; 48 | } 49 | 50 | const char * getType(){ return "std_msgs/Int32"; }; 51 | const char * getMD5(){ return "da5909fbe378aeaf85e547e830cc1bb7"; }; 52 | 53 | }; 54 | 55 | } 56 | #endif -------------------------------------------------------------------------------- /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 | virtual int serialize(unsigned char *outbuffer) const 18 | { 19 | int offset = 0; 20 | union { 21 | float real; 22 | uint32_t base; 23 | } u_data; 24 | u_data.real = this->data; 25 | *(outbuffer + offset + 0) = (u_data.base >> (8 * 0)) & 0xFF; 26 | *(outbuffer + offset + 1) = (u_data.base >> (8 * 1)) & 0xFF; 27 | *(outbuffer + offset + 2) = (u_data.base >> (8 * 2)) & 0xFF; 28 | *(outbuffer + offset + 3) = (u_data.base >> (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 | union { 37 | float real; 38 | uint32_t base; 39 | } u_data; 40 | u_data.base = 0; 41 | u_data.base |= ((uint32_t) (*(inbuffer + offset + 0))) << (8 * 0); 42 | u_data.base |= ((uint32_t) (*(inbuffer + offset + 1))) << (8 * 1); 43 | u_data.base |= ((uint32_t) (*(inbuffer + offset + 2))) << (8 * 2); 44 | u_data.base |= ((uint32_t) (*(inbuffer + offset + 3))) << (8 * 3); 45 | this->data = u_data.real; 46 | offset += sizeof(this->data); 47 | return offset; 48 | } 49 | 50 | const char * getType(){ return "std_msgs/Float32"; }; 51 | const char * getMD5(){ return "73fcbf46b49191e672908e50842a83d4"; }; 52 | 53 | }; 54 | 55 | } 56 | #endif -------------------------------------------------------------------------------- /lib/ros_lib/examples/IrRanger/IrRanger.pde: -------------------------------------------------------------------------------- 1 | /* 2 | * rosserial IR Ranger Example 3 | * 4 | * This example is calibrated for the Sharp GP2D120XJ00F. 5 | */ 6 | 7 | #include 8 | #include 9 | #include 10 | 11 | ros::NodeHandle nh; 12 | 13 | 14 | sensor_msgs::Range range_msg; 15 | ros::Publisher pub_range( "range_data", &range_msg); 16 | 17 | const int analog_pin = 0; 18 | unsigned long range_timer; 19 | 20 | /* 21 | * getRange() - samples the analog input from the ranger 22 | * and converts it into meters. 23 | */ 24 | float getRange(int pin_num){ 25 | int sample; 26 | // Get data 27 | sample = analogRead(pin_num)/4; 28 | // if the ADC reading is too low, 29 | // then we are really far away from anything 30 | if(sample < 10) 31 | return 254; // max range 32 | // Magic numbers to get cm 33 | sample= 1309/(sample-3); 34 | return (sample - 1)/100; //convert to meters 35 | } 36 | 37 | char frameid[] = "/ir_ranger"; 38 | 39 | void setup() 40 | { 41 | nh.initNode(); 42 | nh.advertise(pub_range); 43 | 44 | range_msg.radiation_type = sensor_msgs::Range::INFRARED; 45 | range_msg.header.frame_id = frameid; 46 | range_msg.field_of_view = 0.01; 47 | range_msg.min_range = 0.03; 48 | range_msg.max_range = 0.4; 49 | 50 | } 51 | 52 | void loop() 53 | { 54 | // publish the range value every 50 milliseconds 55 | // since it takes that long for the sensor to stabilize 56 | if ( (millis()-range_timer) > 50){ 57 | range_msg.range = getRange(analog_pin); 58 | range_msg.header.stamp = nh.now(); 59 | pub_range.publish(&range_msg); 60 | range_timer = millis() + 50; 61 | } 62 | nh.spinOnce(); 63 | } 64 | 65 | -------------------------------------------------------------------------------- /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 | virtual int serialize(unsigned char *outbuffer) const 18 | { 19 | int offset = 0; 20 | union { 21 | uint64_t real; 22 | uint32_t base; 23 | } u_data; 24 | u_data.real = this->data; 25 | *(outbuffer + offset + 0) = (u_data.base >> (8 * 0)) & 0xFF; 26 | *(outbuffer + offset + 1) = (u_data.base >> (8 * 1)) & 0xFF; 27 | *(outbuffer + offset + 2) = (u_data.base >> (8 * 2)) & 0xFF; 28 | *(outbuffer + offset + 3) = (u_data.base >> (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 | union { 37 | uint64_t real; 38 | uint32_t base; 39 | } u_data; 40 | u_data.base = 0; 41 | u_data.base |= ((uint32_t) (*(inbuffer + offset + 0))) << (8 * 0); 42 | u_data.base |= ((uint32_t) (*(inbuffer + offset + 1))) << (8 * 1); 43 | u_data.base |= ((uint32_t) (*(inbuffer + offset + 2))) << (8 * 2); 44 | u_data.base |= ((uint32_t) (*(inbuffer + offset + 3))) << (8 * 3); 45 | this->data = u_data.real; 46 | offset += sizeof(this->data); 47 | return offset; 48 | } 49 | 50 | const char * getType(){ return "std_msgs/UInt64"; }; 51 | const char * getMD5(){ return "1b2a79973e8bf53d7b53acb71299cb57"; }; 52 | 53 | }; 54 | 55 | } 56 | #endif -------------------------------------------------------------------------------- /lib/ros_lib/actionlib/TestGoal.h: -------------------------------------------------------------------------------- 1 | #ifndef _ROS_actionlib_TestGoal_h 2 | #define _ROS_actionlib_TestGoal_h 3 | 4 | #include 5 | #include 6 | #include 7 | #include "ros/msg.h" 8 | 9 | namespace actionlib 10 | { 11 | 12 | class TestGoal : public ros::Msg 13 | { 14 | public: 15 | int32_t goal; 16 | 17 | virtual int serialize(unsigned char *outbuffer) const 18 | { 19 | int offset = 0; 20 | union { 21 | int32_t real; 22 | uint32_t base; 23 | } u_goal; 24 | u_goal.real = this->goal; 25 | *(outbuffer + offset + 0) = (u_goal.base >> (8 * 0)) & 0xFF; 26 | *(outbuffer + offset + 1) = (u_goal.base >> (8 * 1)) & 0xFF; 27 | *(outbuffer + offset + 2) = (u_goal.base >> (8 * 2)) & 0xFF; 28 | *(outbuffer + offset + 3) = (u_goal.base >> (8 * 3)) & 0xFF; 29 | offset += sizeof(this->goal); 30 | return offset; 31 | } 32 | 33 | virtual int deserialize(unsigned char *inbuffer) 34 | { 35 | int offset = 0; 36 | union { 37 | int32_t real; 38 | uint32_t base; 39 | } u_goal; 40 | u_goal.base = 0; 41 | u_goal.base |= ((uint32_t) (*(inbuffer + offset + 0))) << (8 * 0); 42 | u_goal.base |= ((uint32_t) (*(inbuffer + offset + 1))) << (8 * 1); 43 | u_goal.base |= ((uint32_t) (*(inbuffer + offset + 2))) << (8 * 2); 44 | u_goal.base |= ((uint32_t) (*(inbuffer + offset + 3))) << (8 * 3); 45 | this->goal = u_goal.real; 46 | offset += sizeof(this->goal); 47 | return offset; 48 | } 49 | 50 | const char * getType(){ return "actionlib/TestGoal"; }; 51 | const char * getMD5(){ return "18df0149936b7aa95588e3862476ebde"; }; 52 | 53 | }; 54 | 55 | } 56 | #endif -------------------------------------------------------------------------------- /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 | virtual int serialize(unsigned char *outbuffer) const 21 | { 22 | int offset = 0; 23 | *(outbuffer + offset++) = points_length; 24 | *(outbuffer + offset++) = 0; 25 | *(outbuffer + offset++) = 0; 26 | *(outbuffer + offset++) = 0; 27 | for( uint8_t i = 0; i < points_length; i++){ 28 | offset += this->points[i].serialize(outbuffer + offset); 29 | } 30 | return offset; 31 | } 32 | 33 | virtual int deserialize(unsigned char *inbuffer) 34 | { 35 | int offset = 0; 36 | uint8_t points_lengthT = *(inbuffer + offset++); 37 | if(points_lengthT > points_length) 38 | this->points = (geometry_msgs::Point32*)realloc(this->points, points_lengthT * sizeof(geometry_msgs::Point32)); 39 | offset += 3; 40 | points_length = points_lengthT; 41 | for( uint8_t i = 0; i < points_length; i++){ 42 | offset += this->st_points.deserialize(inbuffer + offset); 43 | memcpy( &(this->points[i]), &(this->st_points), sizeof(geometry_msgs::Point32)); 44 | } 45 | return offset; 46 | } 47 | 48 | const char * getType(){ return "geometry_msgs/Polygon"; }; 49 | const char * getMD5(){ return "cd60a26494a087f577976f0329fa120e"; }; 50 | 51 | }; 52 | 53 | } 54 | #endif -------------------------------------------------------------------------------- /lib/ros_lib/actionlib/TestResult.h: -------------------------------------------------------------------------------- 1 | #ifndef _ROS_actionlib_TestResult_h 2 | #define _ROS_actionlib_TestResult_h 3 | 4 | #include 5 | #include 6 | #include 7 | #include "ros/msg.h" 8 | 9 | namespace actionlib 10 | { 11 | 12 | class TestResult : public ros::Msg 13 | { 14 | public: 15 | int32_t result; 16 | 17 | virtual int serialize(unsigned char *outbuffer) const 18 | { 19 | int offset = 0; 20 | union { 21 | int32_t real; 22 | uint32_t base; 23 | } u_result; 24 | u_result.real = this->result; 25 | *(outbuffer + offset + 0) = (u_result.base >> (8 * 0)) & 0xFF; 26 | *(outbuffer + offset + 1) = (u_result.base >> (8 * 1)) & 0xFF; 27 | *(outbuffer + offset + 2) = (u_result.base >> (8 * 2)) & 0xFF; 28 | *(outbuffer + offset + 3) = (u_result.base >> (8 * 3)) & 0xFF; 29 | offset += sizeof(this->result); 30 | return offset; 31 | } 32 | 33 | virtual int deserialize(unsigned char *inbuffer) 34 | { 35 | int offset = 0; 36 | union { 37 | int32_t real; 38 | uint32_t base; 39 | } u_result; 40 | u_result.base = 0; 41 | u_result.base |= ((uint32_t) (*(inbuffer + offset + 0))) << (8 * 0); 42 | u_result.base |= ((uint32_t) (*(inbuffer + offset + 1))) << (8 * 1); 43 | u_result.base |= ((uint32_t) (*(inbuffer + offset + 2))) << (8 * 2); 44 | u_result.base |= ((uint32_t) (*(inbuffer + offset + 3))) << (8 * 3); 45 | this->result = u_result.real; 46 | offset += sizeof(this->result); 47 | return offset; 48 | } 49 | 50 | const char * getType(){ return "actionlib/TestResult"; }; 51 | const char * getMD5(){ return "034a8e20d6a306665e3a5b340fab3f09"; }; 52 | 53 | }; 54 | 55 | } 56 | #endif -------------------------------------------------------------------------------- /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 | virtual int serialize(unsigned char *outbuffer) const 24 | { 25 | int offset = 0; 26 | *(outbuffer + offset + 0) = (this->level >> (8 * 0)) & 0xFF; 27 | offset += sizeof(this->level); 28 | uint32_t length_msg = strlen(this->msg); 29 | memcpy(outbuffer + offset, &length_msg, sizeof(uint32_t)); 30 | offset += 4; 31 | memcpy(outbuffer + offset, this->msg, length_msg); 32 | offset += length_msg; 33 | return offset; 34 | } 35 | 36 | virtual int deserialize(unsigned char *inbuffer) 37 | { 38 | int offset = 0; 39 | this->level = ((uint8_t) (*(inbuffer + offset))); 40 | offset += sizeof(this->level); 41 | uint32_t length_msg; 42 | memcpy(&length_msg, (inbuffer + offset), sizeof(uint32_t)); 43 | offset += 4; 44 | for(unsigned int k= offset; k< offset+length_msg; ++k){ 45 | inbuffer[k-1]=inbuffer[k]; 46 | } 47 | inbuffer[offset+length_msg-1]=0; 48 | this->msg = (char *)(inbuffer + offset-1); 49 | offset += length_msg; 50 | return offset; 51 | } 52 | 53 | const char * getType(){ return "rosserial_msgs/Log"; }; 54 | const char * getMD5(){ return "11abd731c25933261cd6183bd12d6295"; }; 55 | 56 | }; 57 | 58 | } 59 | #endif -------------------------------------------------------------------------------- /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 | virtual int serialize(unsigned char *outbuffer) const 21 | { 22 | int offset = 0; 23 | *(outbuffer + offset++) = array_length; 24 | *(outbuffer + offset++) = 0; 25 | *(outbuffer + offset++) = 0; 26 | *(outbuffer + offset++) = 0; 27 | for( uint8_t i = 0; i < array_length; i++){ 28 | offset += this->array[i].serialize(outbuffer + offset); 29 | } 30 | return offset; 31 | } 32 | 33 | virtual int deserialize(unsigned char *inbuffer) 34 | { 35 | int offset = 0; 36 | uint8_t array_lengthT = *(inbuffer + offset++); 37 | if(array_lengthT > array_length) 38 | this->array = (sensor_msgs::JoyFeedback*)realloc(this->array, array_lengthT * sizeof(sensor_msgs::JoyFeedback)); 39 | offset += 3; 40 | array_length = array_lengthT; 41 | for( uint8_t i = 0; i < array_length; i++){ 42 | offset += this->st_array.deserialize(inbuffer + offset); 43 | memcpy( &(this->array[i]), &(this->st_array), sizeof(sensor_msgs::JoyFeedback)); 44 | } 45 | return offset; 46 | } 47 | 48 | const char * getType(){ return "sensor_msgs/JoyFeedbackArray"; }; 49 | const char * getMD5(){ return "cde5730a895b1fc4dee6f91b754b213d"; }; 50 | 51 | }; 52 | 53 | } 54 | #endif -------------------------------------------------------------------------------- /lib/ros_lib/hector_nav_msgs/GetRobotTrajectory.h: -------------------------------------------------------------------------------- 1 | #ifndef _ROS_SERVICE_GetRobotTrajectory_h 2 | #define _ROS_SERVICE_GetRobotTrajectory_h 3 | #include 4 | #include 5 | #include 6 | #include "ros/msg.h" 7 | #include "nav_msgs/Path.h" 8 | 9 | namespace hector_nav_msgs 10 | { 11 | 12 | static const char GETROBOTTRAJECTORY[] = "hector_nav_msgs/GetRobotTrajectory"; 13 | 14 | class GetRobotTrajectoryRequest : public ros::Msg 15 | { 16 | public: 17 | 18 | virtual int serialize(unsigned char *outbuffer) const 19 | { 20 | int offset = 0; 21 | return offset; 22 | } 23 | 24 | virtual int deserialize(unsigned char *inbuffer) 25 | { 26 | int offset = 0; 27 | return offset; 28 | } 29 | 30 | const char * getType(){ return GETROBOTTRAJECTORY; }; 31 | const char * getMD5(){ return "d41d8cd98f00b204e9800998ecf8427e"; }; 32 | 33 | }; 34 | 35 | class GetRobotTrajectoryResponse : public ros::Msg 36 | { 37 | public: 38 | nav_msgs::Path trajectory; 39 | 40 | virtual int serialize(unsigned char *outbuffer) const 41 | { 42 | int offset = 0; 43 | offset += this->trajectory.serialize(outbuffer + offset); 44 | return offset; 45 | } 46 | 47 | virtual int deserialize(unsigned char *inbuffer) 48 | { 49 | int offset = 0; 50 | offset += this->trajectory.deserialize(inbuffer + offset); 51 | return offset; 52 | } 53 | 54 | const char * getType(){ return GETROBOTTRAJECTORY; }; 55 | const char * getMD5(){ return "c7bd40129c5786fc26351edbd33b8d33"; }; 56 | 57 | }; 58 | 59 | class GetRobotTrajectory { 60 | public: 61 | typedef GetRobotTrajectoryRequest Request; 62 | typedef GetRobotTrajectoryResponse Response; 63 | }; 64 | 65 | } 66 | #endif 67 | -------------------------------------------------------------------------------- /lib/ros_lib/actionlib/TestFeedback.h: -------------------------------------------------------------------------------- 1 | #ifndef _ROS_actionlib_TestFeedback_h 2 | #define _ROS_actionlib_TestFeedback_h 3 | 4 | #include 5 | #include 6 | #include 7 | #include "ros/msg.h" 8 | 9 | namespace actionlib 10 | { 11 | 12 | class TestFeedback : public ros::Msg 13 | { 14 | public: 15 | int32_t feedback; 16 | 17 | virtual int serialize(unsigned char *outbuffer) const 18 | { 19 | int offset = 0; 20 | union { 21 | int32_t real; 22 | uint32_t base; 23 | } u_feedback; 24 | u_feedback.real = this->feedback; 25 | *(outbuffer + offset + 0) = (u_feedback.base >> (8 * 0)) & 0xFF; 26 | *(outbuffer + offset + 1) = (u_feedback.base >> (8 * 1)) & 0xFF; 27 | *(outbuffer + offset + 2) = (u_feedback.base >> (8 * 2)) & 0xFF; 28 | *(outbuffer + offset + 3) = (u_feedback.base >> (8 * 3)) & 0xFF; 29 | offset += sizeof(this->feedback); 30 | return offset; 31 | } 32 | 33 | virtual int deserialize(unsigned char *inbuffer) 34 | { 35 | int offset = 0; 36 | union { 37 | int32_t real; 38 | uint32_t base; 39 | } u_feedback; 40 | u_feedback.base = 0; 41 | u_feedback.base |= ((uint32_t) (*(inbuffer + offset + 0))) << (8 * 0); 42 | u_feedback.base |= ((uint32_t) (*(inbuffer + offset + 1))) << (8 * 1); 43 | u_feedback.base |= ((uint32_t) (*(inbuffer + offset + 2))) << (8 * 2); 44 | u_feedback.base |= ((uint32_t) (*(inbuffer + offset + 3))) << (8 * 3); 45 | this->feedback = u_feedback.real; 46 | offset += sizeof(this->feedback); 47 | return offset; 48 | } 49 | 50 | const char * getType(){ return "actionlib/TestFeedback"; }; 51 | const char * getMD5(){ return "49ceb5b32ea3af22073ede4a0328249e"; }; 52 | 53 | }; 54 | 55 | } 56 | #endif -------------------------------------------------------------------------------- /lib/ros_lib/actionlib_tutorials/FibonacciGoal.h: -------------------------------------------------------------------------------- 1 | #ifndef _ROS_actionlib_tutorials_FibonacciGoal_h 2 | #define _ROS_actionlib_tutorials_FibonacciGoal_h 3 | 4 | #include 5 | #include 6 | #include 7 | #include "ros/msg.h" 8 | 9 | namespace actionlib_tutorials 10 | { 11 | 12 | class FibonacciGoal : public ros::Msg 13 | { 14 | public: 15 | int32_t order; 16 | 17 | virtual int serialize(unsigned char *outbuffer) const 18 | { 19 | int offset = 0; 20 | union { 21 | int32_t real; 22 | uint32_t base; 23 | } u_order; 24 | u_order.real = this->order; 25 | *(outbuffer + offset + 0) = (u_order.base >> (8 * 0)) & 0xFF; 26 | *(outbuffer + offset + 1) = (u_order.base >> (8 * 1)) & 0xFF; 27 | *(outbuffer + offset + 2) = (u_order.base >> (8 * 2)) & 0xFF; 28 | *(outbuffer + offset + 3) = (u_order.base >> (8 * 3)) & 0xFF; 29 | offset += sizeof(this->order); 30 | return offset; 31 | } 32 | 33 | virtual int deserialize(unsigned char *inbuffer) 34 | { 35 | int offset = 0; 36 | union { 37 | int32_t real; 38 | uint32_t base; 39 | } u_order; 40 | u_order.base = 0; 41 | u_order.base |= ((uint32_t) (*(inbuffer + offset + 0))) << (8 * 0); 42 | u_order.base |= ((uint32_t) (*(inbuffer + offset + 1))) << (8 * 1); 43 | u_order.base |= ((uint32_t) (*(inbuffer + offset + 2))) << (8 * 2); 44 | u_order.base |= ((uint32_t) (*(inbuffer + offset + 3))) << (8 * 3); 45 | this->order = u_order.real; 46 | offset += sizeof(this->order); 47 | return offset; 48 | } 49 | 50 | const char * getType(){ return "actionlib_tutorials/FibonacciGoal"; }; 51 | const char * getMD5(){ return "6889063349a00b249bd1661df429d822"; }; 52 | 53 | }; 54 | 55 | } 56 | #endif -------------------------------------------------------------------------------- /lib/ros_lib/tf/tfMessage.h: -------------------------------------------------------------------------------- 1 | #ifndef _ROS_tf_tfMessage_h 2 | #define _ROS_tf_tfMessage_h 3 | 4 | #include 5 | #include 6 | #include 7 | #include "ros/msg.h" 8 | #include "geometry_msgs/TransformStamped.h" 9 | 10 | namespace tf 11 | { 12 | 13 | class tfMessage : public ros::Msg 14 | { 15 | public: 16 | uint8_t transforms_length; 17 | geometry_msgs::TransformStamped st_transforms; 18 | geometry_msgs::TransformStamped * transforms; 19 | 20 | virtual int serialize(unsigned char *outbuffer) const 21 | { 22 | int offset = 0; 23 | *(outbuffer + offset++) = transforms_length; 24 | *(outbuffer + offset++) = 0; 25 | *(outbuffer + offset++) = 0; 26 | *(outbuffer + offset++) = 0; 27 | for( uint8_t i = 0; i < transforms_length; i++){ 28 | offset += this->transforms[i].serialize(outbuffer + offset); 29 | } 30 | return offset; 31 | } 32 | 33 | virtual int deserialize(unsigned char *inbuffer) 34 | { 35 | int offset = 0; 36 | uint8_t transforms_lengthT = *(inbuffer + offset++); 37 | if(transforms_lengthT > transforms_length) 38 | this->transforms = (geometry_msgs::TransformStamped*)realloc(this->transforms, transforms_lengthT * sizeof(geometry_msgs::TransformStamped)); 39 | offset += 3; 40 | transforms_length = transforms_lengthT; 41 | for( uint8_t i = 0; i < transforms_length; i++){ 42 | offset += this->st_transforms.deserialize(inbuffer + offset); 43 | memcpy( &(this->transforms[i]), &(this->st_transforms), sizeof(geometry_msgs::TransformStamped)); 44 | } 45 | return offset; 46 | } 47 | 48 | const char * getType(){ return "tf/tfMessage"; }; 49 | const char * getMD5(){ return "94810edda583a504dfda3829e70d7eec"; }; 50 | 51 | }; 52 | 53 | } 54 | #endif -------------------------------------------------------------------------------- /lib/ros_lib/visualization_msgs/MarkerArray.h: -------------------------------------------------------------------------------- 1 | #ifndef _ROS_visualization_msgs_MarkerArray_h 2 | #define _ROS_visualization_msgs_MarkerArray_h 3 | 4 | #include 5 | #include 6 | #include 7 | #include "ros/msg.h" 8 | #include "visualization_msgs/Marker.h" 9 | 10 | namespace visualization_msgs 11 | { 12 | 13 | class MarkerArray : public ros::Msg 14 | { 15 | public: 16 | uint8_t markers_length; 17 | visualization_msgs::Marker st_markers; 18 | visualization_msgs::Marker * markers; 19 | 20 | virtual int serialize(unsigned char *outbuffer) const 21 | { 22 | int offset = 0; 23 | *(outbuffer + offset++) = markers_length; 24 | *(outbuffer + offset++) = 0; 25 | *(outbuffer + offset++) = 0; 26 | *(outbuffer + offset++) = 0; 27 | for( uint8_t i = 0; i < markers_length; i++){ 28 | offset += this->markers[i].serialize(outbuffer + offset); 29 | } 30 | return offset; 31 | } 32 | 33 | virtual int deserialize(unsigned char *inbuffer) 34 | { 35 | int offset = 0; 36 | uint8_t markers_lengthT = *(inbuffer + offset++); 37 | if(markers_lengthT > markers_length) 38 | this->markers = (visualization_msgs::Marker*)realloc(this->markers, markers_lengthT * sizeof(visualization_msgs::Marker)); 39 | offset += 3; 40 | markers_length = markers_lengthT; 41 | for( uint8_t i = 0; i < markers_length; i++){ 42 | offset += this->st_markers.deserialize(inbuffer + offset); 43 | memcpy( &(this->markers[i]), &(this->st_markers), sizeof(visualization_msgs::Marker)); 44 | } 45 | return offset; 46 | } 47 | 48 | const char * getType(){ return "visualization_msgs/MarkerArray"; }; 49 | const char * getMD5(){ return "90da67007c26525f655c1c269094e39f"; }; 50 | 51 | }; 52 | 53 | } 54 | #endif -------------------------------------------------------------------------------- /lib/ros_lib/actionlib_tutorials/AveragingGoal.h: -------------------------------------------------------------------------------- 1 | #ifndef _ROS_actionlib_tutorials_AveragingGoal_h 2 | #define _ROS_actionlib_tutorials_AveragingGoal_h 3 | 4 | #include 5 | #include 6 | #include 7 | #include "ros/msg.h" 8 | 9 | namespace actionlib_tutorials 10 | { 11 | 12 | class AveragingGoal : public ros::Msg 13 | { 14 | public: 15 | int32_t samples; 16 | 17 | virtual int serialize(unsigned char *outbuffer) const 18 | { 19 | int offset = 0; 20 | union { 21 | int32_t real; 22 | uint32_t base; 23 | } u_samples; 24 | u_samples.real = this->samples; 25 | *(outbuffer + offset + 0) = (u_samples.base >> (8 * 0)) & 0xFF; 26 | *(outbuffer + offset + 1) = (u_samples.base >> (8 * 1)) & 0xFF; 27 | *(outbuffer + offset + 2) = (u_samples.base >> (8 * 2)) & 0xFF; 28 | *(outbuffer + offset + 3) = (u_samples.base >> (8 * 3)) & 0xFF; 29 | offset += sizeof(this->samples); 30 | return offset; 31 | } 32 | 33 | virtual int deserialize(unsigned char *inbuffer) 34 | { 35 | int offset = 0; 36 | union { 37 | int32_t real; 38 | uint32_t base; 39 | } u_samples; 40 | u_samples.base = 0; 41 | u_samples.base |= ((uint32_t) (*(inbuffer + offset + 0))) << (8 * 0); 42 | u_samples.base |= ((uint32_t) (*(inbuffer + offset + 1))) << (8 * 1); 43 | u_samples.base |= ((uint32_t) (*(inbuffer + offset + 2))) << (8 * 2); 44 | u_samples.base |= ((uint32_t) (*(inbuffer + offset + 3))) << (8 * 3); 45 | this->samples = u_samples.real; 46 | offset += sizeof(this->samples); 47 | return offset; 48 | } 49 | 50 | const char * getType(){ return "actionlib_tutorials/AveragingGoal"; }; 51 | const char * getMD5(){ return "32c9b10ef9b253faa93b93f564762c8f"; }; 52 | 53 | }; 54 | 55 | } 56 | #endif -------------------------------------------------------------------------------- /lib/ros_lib/tf2_msgs/TFMessage.h: -------------------------------------------------------------------------------- 1 | #ifndef _ROS_tf2_msgs_TFMessage_h 2 | #define _ROS_tf2_msgs_TFMessage_h 3 | 4 | #include 5 | #include 6 | #include 7 | #include "ros/msg.h" 8 | #include "geometry_msgs/TransformStamped.h" 9 | 10 | namespace tf2_msgs 11 | { 12 | 13 | class TFMessage : public ros::Msg 14 | { 15 | public: 16 | uint8_t transforms_length; 17 | geometry_msgs::TransformStamped st_transforms; 18 | geometry_msgs::TransformStamped * transforms; 19 | 20 | virtual int serialize(unsigned char *outbuffer) const 21 | { 22 | int offset = 0; 23 | *(outbuffer + offset++) = transforms_length; 24 | *(outbuffer + offset++) = 0; 25 | *(outbuffer + offset++) = 0; 26 | *(outbuffer + offset++) = 0; 27 | for( uint8_t i = 0; i < transforms_length; i++){ 28 | offset += this->transforms[i].serialize(outbuffer + offset); 29 | } 30 | return offset; 31 | } 32 | 33 | virtual int deserialize(unsigned char *inbuffer) 34 | { 35 | int offset = 0; 36 | uint8_t transforms_lengthT = *(inbuffer + offset++); 37 | if(transforms_lengthT > transforms_length) 38 | this->transforms = (geometry_msgs::TransformStamped*)realloc(this->transforms, transforms_lengthT * sizeof(geometry_msgs::TransformStamped)); 39 | offset += 3; 40 | transforms_length = transforms_lengthT; 41 | for( uint8_t i = 0; i < transforms_length; i++){ 42 | offset += this->st_transforms.deserialize(inbuffer + offset); 43 | memcpy( &(this->transforms[i]), &(this->st_transforms), sizeof(geometry_msgs::TransformStamped)); 44 | } 45 | return offset; 46 | } 47 | 48 | const char * getType(){ return "tf2_msgs/TFMessage"; }; 49 | const char * getMD5(){ return "94810edda583a504dfda3829e70d7eec"; }; 50 | 51 | }; 52 | 53 | } 54 | #endif -------------------------------------------------------------------------------- /lib/ros_lib/hector_mapping/HectorDebugInfo.h: -------------------------------------------------------------------------------- 1 | #ifndef _ROS_hector_mapping_HectorDebugInfo_h 2 | #define _ROS_hector_mapping_HectorDebugInfo_h 3 | 4 | #include 5 | #include 6 | #include 7 | #include "ros/msg.h" 8 | #include "hector_mapping/HectorIterData.h" 9 | 10 | namespace hector_mapping 11 | { 12 | 13 | class HectorDebugInfo : public ros::Msg 14 | { 15 | public: 16 | uint8_t iterData_length; 17 | hector_mapping::HectorIterData st_iterData; 18 | hector_mapping::HectorIterData * iterData; 19 | 20 | virtual int serialize(unsigned char *outbuffer) const 21 | { 22 | int offset = 0; 23 | *(outbuffer + offset++) = iterData_length; 24 | *(outbuffer + offset++) = 0; 25 | *(outbuffer + offset++) = 0; 26 | *(outbuffer + offset++) = 0; 27 | for( uint8_t i = 0; i < iterData_length; i++){ 28 | offset += this->iterData[i].serialize(outbuffer + offset); 29 | } 30 | return offset; 31 | } 32 | 33 | virtual int deserialize(unsigned char *inbuffer) 34 | { 35 | int offset = 0; 36 | uint8_t iterData_lengthT = *(inbuffer + offset++); 37 | if(iterData_lengthT > iterData_length) 38 | this->iterData = (hector_mapping::HectorIterData*)realloc(this->iterData, iterData_lengthT * sizeof(hector_mapping::HectorIterData)); 39 | offset += 3; 40 | iterData_length = iterData_lengthT; 41 | for( uint8_t i = 0; i < iterData_length; i++){ 42 | offset += this->st_iterData.deserialize(inbuffer + offset); 43 | memcpy( &(this->iterData[i]), &(this->st_iterData), sizeof(hector_mapping::HectorIterData)); 44 | } 45 | return offset; 46 | } 47 | 48 | const char * getType(){ return "hector_mapping/HectorDebugInfo"; }; 49 | const char * getMD5(){ return "4d33c0696c0c536f5c1447c260756674"; }; 50 | 51 | }; 52 | 53 | } 54 | #endif -------------------------------------------------------------------------------- /lib/ros_lib/visualization_msgs/InteractiveMarkerPose.h: -------------------------------------------------------------------------------- 1 | #ifndef _ROS_visualization_msgs_InteractiveMarkerPose_h 2 | #define _ROS_visualization_msgs_InteractiveMarkerPose_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 visualization_msgs 12 | { 13 | 14 | class InteractiveMarkerPose : public ros::Msg 15 | { 16 | public: 17 | std_msgs::Header header; 18 | geometry_msgs::Pose pose; 19 | const char* name; 20 | 21 | virtual int serialize(unsigned char *outbuffer) const 22 | { 23 | int offset = 0; 24 | offset += this->header.serialize(outbuffer + offset); 25 | offset += this->pose.serialize(outbuffer + offset); 26 | uint32_t length_name = strlen(this->name); 27 | memcpy(outbuffer + offset, &length_name, sizeof(uint32_t)); 28 | offset += 4; 29 | memcpy(outbuffer + offset, this->name, length_name); 30 | offset += length_name; 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 | uint32_t length_name; 40 | memcpy(&length_name, (inbuffer + offset), sizeof(uint32_t)); 41 | offset += 4; 42 | for(unsigned int k= offset; k< offset+length_name; ++k){ 43 | inbuffer[k-1]=inbuffer[k]; 44 | } 45 | inbuffer[offset+length_name-1]=0; 46 | this->name = (char *)(inbuffer + offset-1); 47 | offset += length_name; 48 | return offset; 49 | } 50 | 51 | const char * getType(){ return "visualization_msgs/InteractiveMarkerPose"; }; 52 | const char * getMD5(){ return "a6e6833209a196a38d798dadb02c81f8"; }; 53 | 54 | }; 55 | 56 | } 57 | #endif -------------------------------------------------------------------------------- /lib/ros_lib/dynamic_reconfigure/Reconfigure.h: -------------------------------------------------------------------------------- 1 | #ifndef _ROS_SERVICE_Reconfigure_h 2 | #define _ROS_SERVICE_Reconfigure_h 3 | #include 4 | #include 5 | #include 6 | #include "ros/msg.h" 7 | #include "dynamic_reconfigure/Config.h" 8 | 9 | namespace dynamic_reconfigure 10 | { 11 | 12 | static const char RECONFIGURE[] = "dynamic_reconfigure/Reconfigure"; 13 | 14 | class ReconfigureRequest : public ros::Msg 15 | { 16 | public: 17 | dynamic_reconfigure::Config config; 18 | 19 | virtual int serialize(unsigned char *outbuffer) const 20 | { 21 | int offset = 0; 22 | offset += this->config.serialize(outbuffer + offset); 23 | return offset; 24 | } 25 | 26 | virtual int deserialize(unsigned char *inbuffer) 27 | { 28 | int offset = 0; 29 | offset += this->config.deserialize(inbuffer + offset); 30 | return offset; 31 | } 32 | 33 | const char * getType(){ return RECONFIGURE; }; 34 | const char * getMD5(){ return "ac41a77620a4a0348b7001641796a8a1"; }; 35 | 36 | }; 37 | 38 | class ReconfigureResponse : public ros::Msg 39 | { 40 | public: 41 | dynamic_reconfigure::Config config; 42 | 43 | virtual int serialize(unsigned char *outbuffer) const 44 | { 45 | int offset = 0; 46 | offset += this->config.serialize(outbuffer + offset); 47 | return offset; 48 | } 49 | 50 | virtual int deserialize(unsigned char *inbuffer) 51 | { 52 | int offset = 0; 53 | offset += this->config.deserialize(inbuffer + offset); 54 | return offset; 55 | } 56 | 57 | const char * getType(){ return RECONFIGURE; }; 58 | const char * getMD5(){ return "ac41a77620a4a0348b7001641796a8a1"; }; 59 | 60 | }; 61 | 62 | class Reconfigure { 63 | public: 64 | typedef ReconfigureRequest Request; 65 | typedef ReconfigureResponse Response; 66 | }; 67 | 68 | } 69 | #endif 70 | -------------------------------------------------------------------------------- /lib/ros_lib/nav_msgs/Path.h: -------------------------------------------------------------------------------- 1 | #ifndef _ROS_nav_msgs_Path_h 2 | #define _ROS_nav_msgs_Path_h 3 | 4 | #include 5 | #include 6 | #include 7 | #include "ros/msg.h" 8 | #include "std_msgs/Header.h" 9 | #include "geometry_msgs/PoseStamped.h" 10 | 11 | namespace nav_msgs 12 | { 13 | 14 | class Path : public ros::Msg 15 | { 16 | public: 17 | std_msgs::Header header; 18 | uint8_t poses_length; 19 | geometry_msgs::PoseStamped st_poses; 20 | geometry_msgs::PoseStamped * poses; 21 | 22 | virtual int serialize(unsigned char *outbuffer) const 23 | { 24 | int offset = 0; 25 | offset += this->header.serialize(outbuffer + offset); 26 | *(outbuffer + offset++) = poses_length; 27 | *(outbuffer + offset++) = 0; 28 | *(outbuffer + offset++) = 0; 29 | *(outbuffer + offset++) = 0; 30 | for( uint8_t i = 0; i < poses_length; i++){ 31 | offset += this->poses[i].serialize(outbuffer + offset); 32 | } 33 | return offset; 34 | } 35 | 36 | virtual int deserialize(unsigned char *inbuffer) 37 | { 38 | int offset = 0; 39 | offset += this->header.deserialize(inbuffer + offset); 40 | uint8_t poses_lengthT = *(inbuffer + offset++); 41 | if(poses_lengthT > poses_length) 42 | this->poses = (geometry_msgs::PoseStamped*)realloc(this->poses, poses_lengthT * sizeof(geometry_msgs::PoseStamped)); 43 | offset += 3; 44 | poses_length = poses_lengthT; 45 | for( uint8_t i = 0; i < poses_length; i++){ 46 | offset += this->st_poses.deserialize(inbuffer + offset); 47 | memcpy( &(this->poses[i]), &(this->st_poses), sizeof(geometry_msgs::PoseStamped)); 48 | } 49 | return offset; 50 | } 51 | 52 | const char * getType(){ return "nav_msgs/Path"; }; 53 | const char * getMD5(){ return "6227e2b7e9cce15051f669a5e197bbf7"; }; 54 | 55 | }; 56 | 57 | } 58 | #endif -------------------------------------------------------------------------------- /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 | virtual int serialize(unsigned char *outbuffer) const 23 | { 24 | int offset = 0; 25 | offset += this->header.serialize(outbuffer + offset); 26 | *(outbuffer + offset++) = poses_length; 27 | *(outbuffer + offset++) = 0; 28 | *(outbuffer + offset++) = 0; 29 | *(outbuffer + offset++) = 0; 30 | for( uint8_t i = 0; i < poses_length; i++){ 31 | offset += this->poses[i].serialize(outbuffer + offset); 32 | } 33 | return offset; 34 | } 35 | 36 | virtual int deserialize(unsigned char *inbuffer) 37 | { 38 | int offset = 0; 39 | offset += this->header.deserialize(inbuffer + offset); 40 | uint8_t poses_lengthT = *(inbuffer + offset++); 41 | if(poses_lengthT > poses_length) 42 | this->poses = (geometry_msgs::Pose*)realloc(this->poses, poses_lengthT * sizeof(geometry_msgs::Pose)); 43 | offset += 3; 44 | poses_length = poses_lengthT; 45 | for( uint8_t i = 0; i < poses_length; i++){ 46 | offset += this->st_poses.deserialize(inbuffer + offset); 47 | memcpy( &(this->poses[i]), &(this->st_poses), sizeof(geometry_msgs::Pose)); 48 | } 49 | return offset; 50 | } 51 | 52 | const char * getType(){ return "geometry_msgs/PoseArray"; }; 53 | const char * getMD5(){ return "916c28c5764443f268b296bb671b9d97"; }; 54 | 55 | }; 56 | 57 | } 58 | #endif -------------------------------------------------------------------------------- /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 | virtual int serialize(unsigned char *outbuffer) const 22 | { 23 | int offset = 0; 24 | offset += this->layout.serialize(outbuffer + offset); 25 | *(outbuffer + offset++) = data_length; 26 | *(outbuffer + offset++) = 0; 27 | *(outbuffer + offset++) = 0; 28 | *(outbuffer + offset++) = 0; 29 | for( uint8_t i = 0; i < data_length; i++){ 30 | *(outbuffer + offset + 0) = (this->data[i] >> (8 * 0)) & 0xFF; 31 | offset += sizeof(this->data[i]); 32 | } 33 | return offset; 34 | } 35 | 36 | virtual int deserialize(unsigned char *inbuffer) 37 | { 38 | int offset = 0; 39 | offset += this->layout.deserialize(inbuffer + offset); 40 | uint8_t data_lengthT = *(inbuffer + offset++); 41 | if(data_lengthT > data_length) 42 | this->data = (uint8_t*)realloc(this->data, data_lengthT * sizeof(uint8_t)); 43 | offset += 3; 44 | data_length = data_lengthT; 45 | for( uint8_t i = 0; i < data_length; i++){ 46 | this->st_data = ((uint8_t) (*(inbuffer + offset))); 47 | offset += sizeof(this->st_data); 48 | memcpy( &(this->data[i]), &(this->st_data), sizeof(uint8_t)); 49 | } 50 | return offset; 51 | } 52 | 53 | const char * getType(){ return "std_msgs/UInt8MultiArray"; }; 54 | const char * getMD5(){ return "82373f1612381bb6ee473b5cd6f5d89c"; }; 55 | 56 | }; 57 | 58 | } 59 | #endif -------------------------------------------------------------------------------- /lib/ros_lib/gazebo_msgs/ContactsState.h: -------------------------------------------------------------------------------- 1 | #ifndef _ROS_gazebo_msgs_ContactsState_h 2 | #define _ROS_gazebo_msgs_ContactsState_h 3 | 4 | #include 5 | #include 6 | #include 7 | #include "ros/msg.h" 8 | #include "std_msgs/Header.h" 9 | #include "gazebo_msgs/ContactState.h" 10 | 11 | namespace gazebo_msgs 12 | { 13 | 14 | class ContactsState : public ros::Msg 15 | { 16 | public: 17 | std_msgs::Header header; 18 | uint8_t states_length; 19 | gazebo_msgs::ContactState st_states; 20 | gazebo_msgs::ContactState * states; 21 | 22 | virtual int serialize(unsigned char *outbuffer) const 23 | { 24 | int offset = 0; 25 | offset += this->header.serialize(outbuffer + offset); 26 | *(outbuffer + offset++) = states_length; 27 | *(outbuffer + offset++) = 0; 28 | *(outbuffer + offset++) = 0; 29 | *(outbuffer + offset++) = 0; 30 | for( uint8_t i = 0; i < states_length; i++){ 31 | offset += this->states[i].serialize(outbuffer + offset); 32 | } 33 | return offset; 34 | } 35 | 36 | virtual int deserialize(unsigned char *inbuffer) 37 | { 38 | int offset = 0; 39 | offset += this->header.deserialize(inbuffer + offset); 40 | uint8_t states_lengthT = *(inbuffer + offset++); 41 | if(states_lengthT > states_length) 42 | this->states = (gazebo_msgs::ContactState*)realloc(this->states, states_lengthT * sizeof(gazebo_msgs::ContactState)); 43 | offset += 3; 44 | states_length = states_lengthT; 45 | for( uint8_t i = 0; i < states_length; i++){ 46 | offset += this->st_states.deserialize(inbuffer + offset); 47 | memcpy( &(this->states[i]), &(this->st_states), sizeof(gazebo_msgs::ContactState)); 48 | } 49 | return offset; 50 | } 51 | 52 | const char * getType(){ return "gazebo_msgs/ContactsState"; }; 53 | const char * getMD5(){ return "acbcb1601a8e525bf72509f18e6f668d"; }; 54 | 55 | }; 56 | 57 | } 58 | #endif -------------------------------------------------------------------------------- /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 | virtual int serialize(unsigned char *outbuffer) const 22 | { 23 | int offset = 0; 24 | offset += this->header.serialize(outbuffer + offset); 25 | uint32_t length_child_frame_id = strlen(this->child_frame_id); 26 | memcpy(outbuffer + offset, &length_child_frame_id, sizeof(uint32_t)); 27 | offset += 4; 28 | memcpy(outbuffer + offset, this->child_frame_id, length_child_frame_id); 29 | offset += length_child_frame_id; 30 | offset += this->transform.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 | uint32_t length_child_frame_id; 39 | memcpy(&length_child_frame_id, (inbuffer + offset), sizeof(uint32_t)); 40 | offset += 4; 41 | for(unsigned int k= offset; k< offset+length_child_frame_id; ++k){ 42 | inbuffer[k-1]=inbuffer[k]; 43 | } 44 | inbuffer[offset+length_child_frame_id-1]=0; 45 | this->child_frame_id = (char *)(inbuffer + offset-1); 46 | offset += length_child_frame_id; 47 | offset += this->transform.deserialize(inbuffer + offset); 48 | return offset; 49 | } 50 | 51 | const char * getType(){ return "geometry_msgs/TransformStamped"; }; 52 | const char * getMD5(){ return "b5764a33bfeb3588febc2682852579b0"; }; 53 | 54 | }; 55 | 56 | } 57 | #endif -------------------------------------------------------------------------------- /lib/ros_lib/ros.h: -------------------------------------------------------------------------------- 1 | /* 2 | * Software License Agreement (BSD License) 3 | * 4 | * Copyright (c) 2011, Willow Garage, Inc. 5 | * All rights reserved. 6 | * 7 | * Redistribution and use in source and binary forms, with or without 8 | * modification, are permitted provided that the following conditions 9 | * are met: 10 | * 11 | * * Redistributions of source code must retain the above copyright 12 | * notice, this list of conditions and the following disclaimer. 13 | * * Redistributions in binary form must reproduce the above 14 | * copyright notice, this list of conditions and the following 15 | * disclaimer in the documentation and/or other materials provided 16 | * with the distribution. 17 | * * Neither the name of Willow Garage, Inc. nor the names of its 18 | * contributors may be used to endorse or promote prducts derived 19 | * from this software without specific prior written permission. 20 | * 21 | * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS 22 | * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT 23 | * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS 24 | * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE 25 | * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, 26 | * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, 27 | * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; 28 | * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER 29 | * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT 30 | * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN 31 | * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE 32 | * POSSIBILITY OF SUCH DAMAGE. 33 | */ 34 | 35 | #ifndef _ROS_H_ 36 | #define _ROS_H_ 37 | 38 | #include "ros/node_handle.h" 39 | #include "STM32Hardware.h" 40 | 41 | namespace ros 42 | { 43 | typedef NodeHandle_ NodeHandle; 44 | } 45 | 46 | #endif 47 | -------------------------------------------------------------------------------- /lib/ros_lib/dynamic_reconfigure/BoolParameter.h: -------------------------------------------------------------------------------- 1 | #ifndef _ROS_dynamic_reconfigure_BoolParameter_h 2 | #define _ROS_dynamic_reconfigure_BoolParameter_h 3 | 4 | #include 5 | #include 6 | #include 7 | #include "ros/msg.h" 8 | 9 | namespace dynamic_reconfigure 10 | { 11 | 12 | class BoolParameter : public ros::Msg 13 | { 14 | public: 15 | const char* name; 16 | bool value; 17 | 18 | virtual int serialize(unsigned char *outbuffer) const 19 | { 20 | int offset = 0; 21 | uint32_t length_name = strlen(this->name); 22 | memcpy(outbuffer + offset, &length_name, sizeof(uint32_t)); 23 | offset += 4; 24 | memcpy(outbuffer + offset, this->name, length_name); 25 | offset += length_name; 26 | union { 27 | bool real; 28 | uint8_t base; 29 | } u_value; 30 | u_value.real = this->value; 31 | *(outbuffer + offset + 0) = (u_value.base >> (8 * 0)) & 0xFF; 32 | offset += sizeof(this->value); 33 | return offset; 34 | } 35 | 36 | virtual int deserialize(unsigned char *inbuffer) 37 | { 38 | int offset = 0; 39 | uint32_t length_name; 40 | memcpy(&length_name, (inbuffer + offset), sizeof(uint32_t)); 41 | offset += 4; 42 | for(unsigned int k= offset; k< offset+length_name; ++k){ 43 | inbuffer[k-1]=inbuffer[k]; 44 | } 45 | inbuffer[offset+length_name-1]=0; 46 | this->name = (char *)(inbuffer + offset-1); 47 | offset += length_name; 48 | union { 49 | bool real; 50 | uint8_t base; 51 | } u_value; 52 | u_value.base = 0; 53 | u_value.base |= ((uint8_t) (*(inbuffer + offset + 0))) << (8 * 0); 54 | this->value = u_value.real; 55 | offset += sizeof(this->value); 56 | return offset; 57 | } 58 | 59 | const char * getType(){ return "dynamic_reconfigure/BoolParameter"; }; 60 | const char * getMD5(){ return "23f05028c1a699fb83e22401228c3a9e"; }; 61 | 62 | }; 63 | 64 | } 65 | #endif -------------------------------------------------------------------------------- /lib/ros_lib/tf2_msgs/TF2Error.h: -------------------------------------------------------------------------------- 1 | #ifndef _ROS_tf2_msgs_TF2Error_h 2 | #define _ROS_tf2_msgs_TF2Error_h 3 | 4 | #include 5 | #include 6 | #include 7 | #include "ros/msg.h" 8 | 9 | namespace tf2_msgs 10 | { 11 | 12 | class TF2Error : public ros::Msg 13 | { 14 | public: 15 | uint8_t error; 16 | const char* error_string; 17 | enum { NO_ERROR = 0 }; 18 | enum { LOOKUP_ERROR = 1 }; 19 | enum { CONNECTIVITY_ERROR = 2 }; 20 | enum { EXTRAPOLATION_ERROR = 3 }; 21 | enum { INVALID_ARGUMENT_ERROR = 4 }; 22 | enum { TIMEOUT_ERROR = 5 }; 23 | enum { TRANSFORM_ERROR = 6 }; 24 | 25 | virtual int serialize(unsigned char *outbuffer) const 26 | { 27 | int offset = 0; 28 | *(outbuffer + offset + 0) = (this->error >> (8 * 0)) & 0xFF; 29 | offset += sizeof(this->error); 30 | uint32_t length_error_string = strlen(this->error_string); 31 | memcpy(outbuffer + offset, &length_error_string, sizeof(uint32_t)); 32 | offset += 4; 33 | memcpy(outbuffer + offset, this->error_string, length_error_string); 34 | offset += length_error_string; 35 | return offset; 36 | } 37 | 38 | virtual int deserialize(unsigned char *inbuffer) 39 | { 40 | int offset = 0; 41 | this->error = ((uint8_t) (*(inbuffer + offset))); 42 | offset += sizeof(this->error); 43 | uint32_t length_error_string; 44 | memcpy(&length_error_string, (inbuffer + offset), sizeof(uint32_t)); 45 | offset += 4; 46 | for(unsigned int k= offset; k< offset+length_error_string; ++k){ 47 | inbuffer[k-1]=inbuffer[k]; 48 | } 49 | inbuffer[offset+length_error_string-1]=0; 50 | this->error_string = (char *)(inbuffer + offset-1); 51 | offset += length_error_string; 52 | return offset; 53 | } 54 | 55 | const char * getType(){ return "tf2_msgs/TF2Error"; }; 56 | const char * getMD5(){ return "bc6848fd6fd750c92e38575618a4917d"; }; 57 | 58 | }; 59 | 60 | } 61 | #endif -------------------------------------------------------------------------------- /lib/ros_lib/roscpp/Logger.h: -------------------------------------------------------------------------------- 1 | #ifndef _ROS_roscpp_Logger_h 2 | #define _ROS_roscpp_Logger_h 3 | 4 | #include 5 | #include 6 | #include 7 | #include "ros/msg.h" 8 | 9 | namespace roscpp 10 | { 11 | 12 | class Logger : public ros::Msg 13 | { 14 | public: 15 | const char* name; 16 | const char* level; 17 | 18 | virtual int serialize(unsigned char *outbuffer) const 19 | { 20 | int offset = 0; 21 | uint32_t length_name = strlen(this->name); 22 | memcpy(outbuffer + offset, &length_name, sizeof(uint32_t)); 23 | offset += 4; 24 | memcpy(outbuffer + offset, this->name, length_name); 25 | offset += length_name; 26 | uint32_t length_level = strlen(this->level); 27 | memcpy(outbuffer + offset, &length_level, sizeof(uint32_t)); 28 | offset += 4; 29 | memcpy(outbuffer + offset, this->level, length_level); 30 | offset += length_level; 31 | return offset; 32 | } 33 | 34 | virtual int deserialize(unsigned char *inbuffer) 35 | { 36 | int offset = 0; 37 | uint32_t length_name; 38 | memcpy(&length_name, (inbuffer + offset), sizeof(uint32_t)); 39 | offset += 4; 40 | for(unsigned int k= offset; k< offset+length_name; ++k){ 41 | inbuffer[k-1]=inbuffer[k]; 42 | } 43 | inbuffer[offset+length_name-1]=0; 44 | this->name = (char *)(inbuffer + offset-1); 45 | offset += length_name; 46 | uint32_t length_level; 47 | memcpy(&length_level, (inbuffer + offset), sizeof(uint32_t)); 48 | offset += 4; 49 | for(unsigned int k= offset; k< offset+length_level; ++k){ 50 | inbuffer[k-1]=inbuffer[k]; 51 | } 52 | inbuffer[offset+length_level-1]=0; 53 | this->level = (char *)(inbuffer + offset-1); 54 | offset += length_level; 55 | return offset; 56 | } 57 | 58 | const char * getType(){ return "roscpp/Logger"; }; 59 | const char * getMD5(){ return "a6069a2ff40db7bd32143dd66e1f408e"; }; 60 | 61 | }; 62 | 63 | } 64 | #endif -------------------------------------------------------------------------------- /lib/ros_lib/turtlesim/Kill.h: -------------------------------------------------------------------------------- 1 | #ifndef _ROS_SERVICE_Kill_h 2 | #define _ROS_SERVICE_Kill_h 3 | #include 4 | #include 5 | #include 6 | #include "ros/msg.h" 7 | 8 | namespace turtlesim 9 | { 10 | 11 | static const char KILL[] = "turtlesim/Kill"; 12 | 13 | class KillRequest : public ros::Msg 14 | { 15 | public: 16 | const char* name; 17 | 18 | virtual int serialize(unsigned char *outbuffer) const 19 | { 20 | int offset = 0; 21 | uint32_t length_name = strlen(this->name); 22 | memcpy(outbuffer + offset, &length_name, sizeof(uint32_t)); 23 | offset += 4; 24 | memcpy(outbuffer + offset, this->name, length_name); 25 | offset += length_name; 26 | return offset; 27 | } 28 | 29 | virtual int deserialize(unsigned char *inbuffer) 30 | { 31 | int offset = 0; 32 | uint32_t length_name; 33 | memcpy(&length_name, (inbuffer + offset), sizeof(uint32_t)); 34 | offset += 4; 35 | for(unsigned int k= offset; k< offset+length_name; ++k){ 36 | inbuffer[k-1]=inbuffer[k]; 37 | } 38 | inbuffer[offset+length_name-1]=0; 39 | this->name = (char *)(inbuffer + offset-1); 40 | offset += length_name; 41 | return offset; 42 | } 43 | 44 | const char * getType(){ return KILL; }; 45 | const char * getMD5(){ return "c1f3d28f1b044c871e6eff2e9fc3c667"; }; 46 | 47 | }; 48 | 49 | class KillResponse : public ros::Msg 50 | { 51 | public: 52 | 53 | virtual int serialize(unsigned char *outbuffer) const 54 | { 55 | int offset = 0; 56 | return offset; 57 | } 58 | 59 | virtual int deserialize(unsigned char *inbuffer) 60 | { 61 | int offset = 0; 62 | return offset; 63 | } 64 | 65 | const char * getType(){ return KILL; }; 66 | const char * getMD5(){ return "d41d8cd98f00b204e9800998ecf8427e"; }; 67 | 68 | }; 69 | 70 | class Kill { 71 | public: 72 | typedef KillRequest Request; 73 | typedef KillResponse Response; 74 | }; 75 | 76 | } 77 | #endif 78 | -------------------------------------------------------------------------------- /lib/ros_lib/diagnostic_msgs/DiagnosticArray.h: -------------------------------------------------------------------------------- 1 | #ifndef _ROS_diagnostic_msgs_DiagnosticArray_h 2 | #define _ROS_diagnostic_msgs_DiagnosticArray_h 3 | 4 | #include 5 | #include 6 | #include 7 | #include "ros/msg.h" 8 | #include "std_msgs/Header.h" 9 | #include "diagnostic_msgs/DiagnosticStatus.h" 10 | 11 | namespace diagnostic_msgs 12 | { 13 | 14 | class DiagnosticArray : public ros::Msg 15 | { 16 | public: 17 | std_msgs::Header header; 18 | uint8_t status_length; 19 | diagnostic_msgs::DiagnosticStatus st_status; 20 | diagnostic_msgs::DiagnosticStatus * status; 21 | 22 | virtual int serialize(unsigned char *outbuffer) const 23 | { 24 | int offset = 0; 25 | offset += this->header.serialize(outbuffer + offset); 26 | *(outbuffer + offset++) = status_length; 27 | *(outbuffer + offset++) = 0; 28 | *(outbuffer + offset++) = 0; 29 | *(outbuffer + offset++) = 0; 30 | for( uint8_t i = 0; i < status_length; i++){ 31 | offset += this->status[i].serialize(outbuffer + offset); 32 | } 33 | return offset; 34 | } 35 | 36 | virtual int deserialize(unsigned char *inbuffer) 37 | { 38 | int offset = 0; 39 | offset += this->header.deserialize(inbuffer + offset); 40 | uint8_t status_lengthT = *(inbuffer + offset++); 41 | if(status_lengthT > status_length) 42 | this->status = (diagnostic_msgs::DiagnosticStatus*)realloc(this->status, status_lengthT * sizeof(diagnostic_msgs::DiagnosticStatus)); 43 | offset += 3; 44 | status_length = status_lengthT; 45 | for( uint8_t i = 0; i < status_length; i++){ 46 | offset += this->st_status.deserialize(inbuffer + offset); 47 | memcpy( &(this->status[i]), &(this->st_status), sizeof(diagnostic_msgs::DiagnosticStatus)); 48 | } 49 | return offset; 50 | } 51 | 52 | const char * getType(){ return "diagnostic_msgs/DiagnosticArray"; }; 53 | const char * getMD5(){ return "60810da900de1dd6ddd437c3503511da"; }; 54 | 55 | }; 56 | 57 | } 58 | #endif -------------------------------------------------------------------------------- /lib/ros_lib/diagnostic_msgs/KeyValue.h: -------------------------------------------------------------------------------- 1 | #ifndef _ROS_diagnostic_msgs_KeyValue_h 2 | #define _ROS_diagnostic_msgs_KeyValue_h 3 | 4 | #include 5 | #include 6 | #include 7 | #include "ros/msg.h" 8 | 9 | namespace diagnostic_msgs 10 | { 11 | 12 | class KeyValue : public ros::Msg 13 | { 14 | public: 15 | const char* key; 16 | const char* value; 17 | 18 | virtual int serialize(unsigned char *outbuffer) const 19 | { 20 | int offset = 0; 21 | uint32_t length_key = strlen(this->key); 22 | memcpy(outbuffer + offset, &length_key, sizeof(uint32_t)); 23 | offset += 4; 24 | memcpy(outbuffer + offset, this->key, length_key); 25 | offset += length_key; 26 | uint32_t length_value = strlen(this->value); 27 | memcpy(outbuffer + offset, &length_value, sizeof(uint32_t)); 28 | offset += 4; 29 | memcpy(outbuffer + offset, this->value, length_value); 30 | offset += length_value; 31 | return offset; 32 | } 33 | 34 | virtual int deserialize(unsigned char *inbuffer) 35 | { 36 | int offset = 0; 37 | uint32_t length_key; 38 | memcpy(&length_key, (inbuffer + offset), sizeof(uint32_t)); 39 | offset += 4; 40 | for(unsigned int k= offset; k< offset+length_key; ++k){ 41 | inbuffer[k-1]=inbuffer[k]; 42 | } 43 | inbuffer[offset+length_key-1]=0; 44 | this->key = (char *)(inbuffer + offset-1); 45 | offset += length_key; 46 | uint32_t length_value; 47 | memcpy(&length_value, (inbuffer + offset), sizeof(uint32_t)); 48 | offset += 4; 49 | for(unsigned int k= offset; k< offset+length_value; ++k){ 50 | inbuffer[k-1]=inbuffer[k]; 51 | } 52 | inbuffer[offset+length_value-1]=0; 53 | this->value = (char *)(inbuffer + offset-1); 54 | offset += length_value; 55 | return offset; 56 | } 57 | 58 | const char * getType(){ return "diagnostic_msgs/KeyValue"; }; 59 | const char * getMD5(){ return "cf57fdc6617a881a88c16e768132149c"; }; 60 | 61 | }; 62 | 63 | } 64 | #endif -------------------------------------------------------------------------------- /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 | virtual int serialize(unsigned char *outbuffer) const 22 | { 23 | int offset = 0; 24 | offset += this->layout.serialize(outbuffer + offset); 25 | *(outbuffer + offset++) = data_length; 26 | *(outbuffer + offset++) = 0; 27 | *(outbuffer + offset++) = 0; 28 | *(outbuffer + offset++) = 0; 29 | for( uint8_t i = 0; i < data_length; i++){ 30 | *(outbuffer + offset + 0) = (this->data[i] >> (8 * 0)) & 0xFF; 31 | *(outbuffer + offset + 1) = (this->data[i] >> (8 * 1)) & 0xFF; 32 | offset += sizeof(this->data[i]); 33 | } 34 | return offset; 35 | } 36 | 37 | virtual int deserialize(unsigned char *inbuffer) 38 | { 39 | int offset = 0; 40 | offset += this->layout.deserialize(inbuffer + offset); 41 | uint8_t data_lengthT = *(inbuffer + offset++); 42 | if(data_lengthT > data_length) 43 | this->data = (uint16_t*)realloc(this->data, data_lengthT * sizeof(uint16_t)); 44 | offset += 3; 45 | data_length = data_lengthT; 46 | for( uint8_t i = 0; i < data_length; i++){ 47 | this->st_data = ((uint16_t) (*(inbuffer + offset))); 48 | this->st_data |= ((uint16_t) (*(inbuffer + offset + 1))) << (8 * 1); 49 | offset += sizeof(this->st_data); 50 | memcpy( &(this->data[i]), &(this->st_data), sizeof(uint16_t)); 51 | } 52 | return offset; 53 | } 54 | 55 | const char * getType(){ return "std_msgs/UInt16MultiArray"; }; 56 | const char * getMD5(){ return "52f264f1c973c4b73790d384c6cb4484"; }; 57 | 58 | }; 59 | 60 | } 61 | #endif --------------------------------------------------------------------------------