diff --git a/.gitignore b/.gitignore index 52e7447a..40dd6711 100644 --- a/.gitignore +++ b/.gitignore @@ -57,3 +57,4 @@ build-*/ *.pyc *.dat *.txt.user +*.gch diff --git a/turtlebot3_core/libraries/ros_lib/ArduinoHardware.h b/turtlebot3_core/libraries/ros_lib/ArduinoHardware.h new file mode 100644 index 00000000..437e45d3 --- /dev/null +++ b/turtlebot3_core/libraries/ros_lib/ArduinoHardware.h @@ -0,0 +1,112 @@ +/* + * Software License Agreement (BSD License) + * + * Copyright (c) 2011, Willow Garage, Inc. + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above + * copyright notice, this list of conditions and the following + * disclaimer in the documentation and/or other materials provided + * with the distribution. + * * Neither the name of Willow Garage, Inc. nor the names of its + * contributors may be used to endorse or promote prducts derived + * from this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; + * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + */ + +#ifndef ROS_ARDUINO_HARDWARE_H_ +#define ROS_ARDUINO_HARDWARE_H_ + +#if ARDUINO>=100 + #include // Arduino 1.0 +#else + #include // Arduino 0022 +#endif + +#if defined(__MK20DX128__) || defined(__MK20DX256__) + #if defined(USE_TEENSY_HW_SERIAL) + #define SERIAL_CLASS HardwareSerial // Teensy HW Serial + #else + #include // Teensy 3.0 and 3.1 + #define SERIAL_CLASS usb_serial_class + #endif +#elif defined(_SAM3XA_) + #include // Arduino Due + #define SERIAL_CLASS UARTClass +#elif defined(USE_USBCON) + // Arduino Leonardo USB Serial Port + #define SERIAL_CLASS Serial_ +#else + #include // Arduino AVR + #define SERIAL_CLASS USBSerial // UARTClass (Bluetooth Device) +#endif + +class ArduinoHardware { + public: + ArduinoHardware(SERIAL_CLASS* io , long baud= 57600){ + iostream = io; + baud_ = baud; + } + ArduinoHardware() + { +#if defined(USBCON) and !(defined(USE_USBCON)) + /* Leonardo support */ + iostream = &Serial2; +#elif defined(USE_TEENSY_HW_SERIAL) + iostream = &Serial2; +#else + iostream = &Serial; +#endif + baud_ = 57600; + } + ArduinoHardware(ArduinoHardware& h){ + this->iostream = iostream; + this->baud_ = h.baud_; + } + + void setBaud(long baud){ + this->baud_= baud; + } + + int getBaud(){return baud_;} + + void init(){ +#if defined(USE_USBCON) + // Startup delay as a fail-safe to upload a new sketch + delay(3000); +#endif + iostream->begin(baud_); + } + + int read(){return iostream->read();}; + void write(uint8_t* data, int length){ + for(int i=0; iwrite(data[i]); + } + + unsigned long time(){return millis();} + + protected: + SERIAL_CLASS* iostream; + long baud_; +}; + +#endif diff --git a/turtlebot3_core/libraries/ros_lib/duration.cpp b/turtlebot3_core/libraries/ros_lib/duration.cpp new file mode 100644 index 00000000..f471c33d --- /dev/null +++ b/turtlebot3_core/libraries/ros_lib/duration.cpp @@ -0,0 +1,81 @@ +/* + * Software License Agreement (BSD License) + * + * Copyright (c) 2011, Willow Garage, Inc. + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above + * copyright notice, this list of conditions and the following + * disclaimer in the documentation and/or other materials provided + * with the distribution. + * * Neither the name of Willow Garage, Inc. nor the names of its + * contributors may be used to endorse or promote prducts derived + * from this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; + * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + */ + +#include +#include "ros/duration.h" + +namespace ros +{ + void normalizeSecNSecSigned(int32_t &sec, int32_t &nsec) + { + int32_t nsec_part = nsec; + int32_t sec_part = sec; + + while (nsec_part > 1000000000L) + { + nsec_part -= 1000000000L; + ++sec_part; + } + while (nsec_part < 0) + { + nsec_part += 1000000000L; + --sec_part; + } + sec = sec_part; + nsec = nsec_part; + } + + Duration& Duration::operator+=(const Duration &rhs) + { + sec += rhs.sec; + nsec += rhs.nsec; + normalizeSecNSecSigned(sec, nsec); + return *this; + } + + Duration& Duration::operator-=(const Duration &rhs){ + sec += -rhs.sec; + nsec += -rhs.nsec; + normalizeSecNSecSigned(sec, nsec); + return *this; + } + + Duration& Duration::operator*=(double scale){ + sec *= scale; + nsec *= scale; + normalizeSecNSecSigned(sec, nsec); + return *this; + } + +} diff --git a/turtlebot3_core/libraries/ros_lib/geometry_msgs/Accel.h b/turtlebot3_core/libraries/ros_lib/geometry_msgs/Accel.h new file mode 100644 index 00000000..c3bedcfe --- /dev/null +++ b/turtlebot3_core/libraries/ros_lib/geometry_msgs/Accel.h @@ -0,0 +1,47 @@ +#ifndef _ROS_geometry_msgs_Accel_h +#define _ROS_geometry_msgs_Accel_h + +#include +#include +#include +#include "ros/msg.h" +#include "geometry_msgs/Vector3.h" + +namespace geometry_msgs +{ + + class Accel : public ros::Msg + { + public: + geometry_msgs::Vector3 linear; + geometry_msgs::Vector3 angular; + + Accel(): + linear(), + angular() + { + } + + virtual int serialize(unsigned char *outbuffer) const + { + int offset = 0; + offset += this->linear.serialize(outbuffer + offset); + offset += this->angular.serialize(outbuffer + offset); + return offset; + } + + virtual int deserialize(unsigned char *inbuffer) + { + int offset = 0; + offset += this->linear.deserialize(inbuffer + offset); + offset += this->angular.deserialize(inbuffer + offset); + return offset; + } + + const char * getType(){ return "geometry_msgs/Accel"; }; + const char * getMD5(){ return "9f195f881246fdfa2798d1d3eebca84a"; }; + + }; + +} +#endif \ No newline at end of file diff --git a/turtlebot3_core/libraries/ros_lib/geometry_msgs/AccelStamped.h b/turtlebot3_core/libraries/ros_lib/geometry_msgs/AccelStamped.h new file mode 100644 index 00000000..121060d8 --- /dev/null +++ b/turtlebot3_core/libraries/ros_lib/geometry_msgs/AccelStamped.h @@ -0,0 +1,48 @@ +#ifndef _ROS_geometry_msgs_AccelStamped_h +#define _ROS_geometry_msgs_AccelStamped_h + +#include +#include +#include +#include "ros/msg.h" +#include "std_msgs/Header.h" +#include "geometry_msgs/Accel.h" + +namespace geometry_msgs +{ + + class AccelStamped : public ros::Msg + { + public: + std_msgs::Header header; + geometry_msgs::Accel accel; + + AccelStamped(): + header(), + accel() + { + } + + virtual int serialize(unsigned char *outbuffer) const + { + int offset = 0; + offset += this->header.serialize(outbuffer + offset); + offset += this->accel.serialize(outbuffer + offset); + return offset; + } + + virtual int deserialize(unsigned char *inbuffer) + { + int offset = 0; + offset += this->header.deserialize(inbuffer + offset); + offset += this->accel.deserialize(inbuffer + offset); + return offset; + } + + const char * getType(){ return "geometry_msgs/AccelStamped"; }; + const char * getMD5(){ return "d8a98a5d81351b6eb0578c78557e7659"; }; + + }; + +} +#endif \ No newline at end of file diff --git a/turtlebot3_core/libraries/ros_lib/geometry_msgs/AccelWithCovariance.h b/turtlebot3_core/libraries/ros_lib/geometry_msgs/AccelWithCovariance.h new file mode 100644 index 00000000..867131dd --- /dev/null +++ b/turtlebot3_core/libraries/ros_lib/geometry_msgs/AccelWithCovariance.h @@ -0,0 +1,51 @@ +#ifndef _ROS_geometry_msgs_AccelWithCovariance_h +#define _ROS_geometry_msgs_AccelWithCovariance_h + +#include +#include +#include +#include "ros/msg.h" +#include "geometry_msgs/Accel.h" + +namespace geometry_msgs +{ + + class AccelWithCovariance : public ros::Msg + { + public: + geometry_msgs::Accel accel; + float covariance[36]; + + AccelWithCovariance(): + accel(), + covariance() + { + } + + virtual int serialize(unsigned char *outbuffer) const + { + int offset = 0; + offset += this->accel.serialize(outbuffer + offset); + for( uint32_t i = 0; i < 36; i++){ + offset += serializeAvrFloat64(outbuffer + offset, this->covariance[i]); + } + return offset; + } + + virtual int deserialize(unsigned char *inbuffer) + { + int offset = 0; + offset += this->accel.deserialize(inbuffer + offset); + for( uint32_t i = 0; i < 36; i++){ + offset += deserializeAvrFloat64(inbuffer + offset, &(this->covariance[i])); + } + return offset; + } + + const char * getType(){ return "geometry_msgs/AccelWithCovariance"; }; + const char * getMD5(){ return "ad5a718d699c6be72a02b8d6a139f334"; }; + + }; + +} +#endif \ No newline at end of file diff --git a/turtlebot3_core/libraries/ros_lib/geometry_msgs/AccelWithCovarianceStamped.h b/turtlebot3_core/libraries/ros_lib/geometry_msgs/AccelWithCovarianceStamped.h new file mode 100644 index 00000000..d5481394 --- /dev/null +++ b/turtlebot3_core/libraries/ros_lib/geometry_msgs/AccelWithCovarianceStamped.h @@ -0,0 +1,48 @@ +#ifndef _ROS_geometry_msgs_AccelWithCovarianceStamped_h +#define _ROS_geometry_msgs_AccelWithCovarianceStamped_h + +#include +#include +#include +#include "ros/msg.h" +#include "std_msgs/Header.h" +#include "geometry_msgs/AccelWithCovariance.h" + +namespace geometry_msgs +{ + + class AccelWithCovarianceStamped : public ros::Msg + { + public: + std_msgs::Header header; + geometry_msgs::AccelWithCovariance accel; + + AccelWithCovarianceStamped(): + header(), + accel() + { + } + + virtual int serialize(unsigned char *outbuffer) const + { + int offset = 0; + offset += this->header.serialize(outbuffer + offset); + offset += this->accel.serialize(outbuffer + offset); + return offset; + } + + virtual int deserialize(unsigned char *inbuffer) + { + int offset = 0; + offset += this->header.deserialize(inbuffer + offset); + offset += this->accel.deserialize(inbuffer + offset); + return offset; + } + + const char * getType(){ return "geometry_msgs/AccelWithCovarianceStamped"; }; + const char * getMD5(){ return "96adb295225031ec8d57fb4251b0a886"; }; + + }; + +} +#endif \ No newline at end of file diff --git a/turtlebot3_core/libraries/ros_lib/geometry_msgs/Inertia.h b/turtlebot3_core/libraries/ros_lib/geometry_msgs/Inertia.h new file mode 100644 index 00000000..c7f719a0 --- /dev/null +++ b/turtlebot3_core/libraries/ros_lib/geometry_msgs/Inertia.h @@ -0,0 +1,71 @@ +#ifndef _ROS_geometry_msgs_Inertia_h +#define _ROS_geometry_msgs_Inertia_h + +#include +#include +#include +#include "ros/msg.h" +#include "geometry_msgs/Vector3.h" + +namespace geometry_msgs +{ + + class Inertia : public ros::Msg + { + public: + float m; + geometry_msgs::Vector3 com; + float ixx; + float ixy; + float ixz; + float iyy; + float iyz; + float izz; + + Inertia(): + m(0), + com(), + ixx(0), + ixy(0), + ixz(0), + iyy(0), + iyz(0), + izz(0) + { + } + + virtual int serialize(unsigned char *outbuffer) const + { + int offset = 0; + offset += serializeAvrFloat64(outbuffer + offset, this->m); + offset += this->com.serialize(outbuffer + offset); + offset += serializeAvrFloat64(outbuffer + offset, this->ixx); + offset += serializeAvrFloat64(outbuffer + offset, this->ixy); + offset += serializeAvrFloat64(outbuffer + offset, this->ixz); + offset += serializeAvrFloat64(outbuffer + offset, this->iyy); + offset += serializeAvrFloat64(outbuffer + offset, this->iyz); + offset += serializeAvrFloat64(outbuffer + offset, this->izz); + return offset; + } + + virtual int deserialize(unsigned char *inbuffer) + { + int offset = 0; + offset += deserializeAvrFloat64(inbuffer + offset, &(this->m)); + offset += this->com.deserialize(inbuffer + offset); + offset += deserializeAvrFloat64(inbuffer + offset, &(this->ixx)); + offset += deserializeAvrFloat64(inbuffer + offset, &(this->ixy)); + offset += deserializeAvrFloat64(inbuffer + offset, &(this->ixz)); + offset += deserializeAvrFloat64(inbuffer + offset, &(this->iyy)); + offset += deserializeAvrFloat64(inbuffer + offset, &(this->iyz)); + offset += deserializeAvrFloat64(inbuffer + offset, &(this->izz)); + return offset; + } + + const char * getType(){ return "geometry_msgs/Inertia"; }; + const char * getMD5(){ return "1d26e4bb6c83ff141c5cf0d883c2b0fe"; }; + + }; + +} +#endif \ No newline at end of file diff --git a/turtlebot3_core/libraries/ros_lib/geometry_msgs/InertiaStamped.h b/turtlebot3_core/libraries/ros_lib/geometry_msgs/InertiaStamped.h new file mode 100644 index 00000000..33cb7565 --- /dev/null +++ b/turtlebot3_core/libraries/ros_lib/geometry_msgs/InertiaStamped.h @@ -0,0 +1,48 @@ +#ifndef _ROS_geometry_msgs_InertiaStamped_h +#define _ROS_geometry_msgs_InertiaStamped_h + +#include +#include +#include +#include "ros/msg.h" +#include "std_msgs/Header.h" +#include "geometry_msgs/Inertia.h" + +namespace geometry_msgs +{ + + class InertiaStamped : public ros::Msg + { + public: + std_msgs::Header header; + geometry_msgs::Inertia inertia; + + InertiaStamped(): + header(), + inertia() + { + } + + virtual int serialize(unsigned char *outbuffer) const + { + int offset = 0; + offset += this->header.serialize(outbuffer + offset); + offset += this->inertia.serialize(outbuffer + offset); + return offset; + } + + virtual int deserialize(unsigned char *inbuffer) + { + int offset = 0; + offset += this->header.deserialize(inbuffer + offset); + offset += this->inertia.deserialize(inbuffer + offset); + return offset; + } + + const char * getType(){ return "geometry_msgs/InertiaStamped"; }; + const char * getMD5(){ return "ddee48caeab5a966c5e8d166654a9ac7"; }; + + }; + +} +#endif \ No newline at end of file diff --git a/turtlebot3_core/libraries/ros_lib/geometry_msgs/Point.h b/turtlebot3_core/libraries/ros_lib/geometry_msgs/Point.h new file mode 100644 index 00000000..92e1b429 --- /dev/null +++ b/turtlebot3_core/libraries/ros_lib/geometry_msgs/Point.h @@ -0,0 +1,50 @@ +#ifndef _ROS_geometry_msgs_Point_h +#define _ROS_geometry_msgs_Point_h + +#include +#include +#include +#include "ros/msg.h" + +namespace geometry_msgs +{ + + class Point : public ros::Msg + { + public: + float x; + float y; + float z; + + Point(): + x(0), + y(0), + z(0) + { + } + + virtual int serialize(unsigned char *outbuffer) const + { + int offset = 0; + offset += serializeAvrFloat64(outbuffer + offset, this->x); + offset += serializeAvrFloat64(outbuffer + offset, this->y); + offset += serializeAvrFloat64(outbuffer + offset, this->z); + return offset; + } + + virtual int deserialize(unsigned char *inbuffer) + { + int offset = 0; + offset += deserializeAvrFloat64(inbuffer + offset, &(this->x)); + offset += deserializeAvrFloat64(inbuffer + offset, &(this->y)); + offset += deserializeAvrFloat64(inbuffer + offset, &(this->z)); + return offset; + } + + const char * getType(){ return "geometry_msgs/Point"; }; + const char * getMD5(){ return "4a842b65f413084dc2b10fb484ea7f17"; }; + + }; + +} +#endif \ No newline at end of file diff --git a/turtlebot3_core/libraries/ros_lib/geometry_msgs/Point32.h b/turtlebot3_core/libraries/ros_lib/geometry_msgs/Point32.h new file mode 100644 index 00000000..15be573e --- /dev/null +++ b/turtlebot3_core/libraries/ros_lib/geometry_msgs/Point32.h @@ -0,0 +1,107 @@ +#ifndef _ROS_geometry_msgs_Point32_h +#define _ROS_geometry_msgs_Point32_h + +#include +#include +#include +#include "ros/msg.h" + +namespace geometry_msgs +{ + + class Point32 : public ros::Msg + { + public: + float x; + float y; + float z; + + Point32(): + x(0), + y(0), + z(0) + { + } + + virtual int serialize(unsigned char *outbuffer) const + { + int offset = 0; + union { + float real; + uint32_t base; + } u_x; + u_x.real = this->x; + *(outbuffer + offset + 0) = (u_x.base >> (8 * 0)) & 0xFF; + *(outbuffer + offset + 1) = (u_x.base >> (8 * 1)) & 0xFF; + *(outbuffer + offset + 2) = (u_x.base >> (8 * 2)) & 0xFF; + *(outbuffer + offset + 3) = (u_x.base >> (8 * 3)) & 0xFF; + offset += sizeof(this->x); + union { + float real; + uint32_t base; + } u_y; + u_y.real = this->y; + *(outbuffer + offset + 0) = (u_y.base >> (8 * 0)) & 0xFF; + *(outbuffer + offset + 1) = (u_y.base >> (8 * 1)) & 0xFF; + *(outbuffer + offset + 2) = (u_y.base >> (8 * 2)) & 0xFF; + *(outbuffer + offset + 3) = (u_y.base >> (8 * 3)) & 0xFF; + offset += sizeof(this->y); + union { + float real; + uint32_t base; + } u_z; + u_z.real = this->z; + *(outbuffer + offset + 0) = (u_z.base >> (8 * 0)) & 0xFF; + *(outbuffer + offset + 1) = (u_z.base >> (8 * 1)) & 0xFF; + *(outbuffer + offset + 2) = (u_z.base >> (8 * 2)) & 0xFF; + *(outbuffer + offset + 3) = (u_z.base >> (8 * 3)) & 0xFF; + offset += sizeof(this->z); + return offset; + } + + virtual int deserialize(unsigned char *inbuffer) + { + int offset = 0; + union { + float real; + uint32_t base; + } u_x; + u_x.base = 0; + u_x.base |= ((uint32_t) (*(inbuffer + offset + 0))) << (8 * 0); + u_x.base |= ((uint32_t) (*(inbuffer + offset + 1))) << (8 * 1); + u_x.base |= ((uint32_t) (*(inbuffer + offset + 2))) << (8 * 2); + u_x.base |= ((uint32_t) (*(inbuffer + offset + 3))) << (8 * 3); + this->x = u_x.real; + offset += sizeof(this->x); + union { + float real; + uint32_t base; + } u_y; + u_y.base = 0; + u_y.base |= ((uint32_t) (*(inbuffer + offset + 0))) << (8 * 0); + u_y.base |= ((uint32_t) (*(inbuffer + offset + 1))) << (8 * 1); + u_y.base |= ((uint32_t) (*(inbuffer + offset + 2))) << (8 * 2); + u_y.base |= ((uint32_t) (*(inbuffer + offset + 3))) << (8 * 3); + this->y = u_y.real; + offset += sizeof(this->y); + union { + float real; + uint32_t base; + } u_z; + u_z.base = 0; + u_z.base |= ((uint32_t) (*(inbuffer + offset + 0))) << (8 * 0); + u_z.base |= ((uint32_t) (*(inbuffer + offset + 1))) << (8 * 1); + u_z.base |= ((uint32_t) (*(inbuffer + offset + 2))) << (8 * 2); + u_z.base |= ((uint32_t) (*(inbuffer + offset + 3))) << (8 * 3); + this->z = u_z.real; + offset += sizeof(this->z); + return offset; + } + + const char * getType(){ return "geometry_msgs/Point32"; }; + const char * getMD5(){ return "cc153912f1453b708d221682bc23d9ac"; }; + + }; + +} +#endif \ No newline at end of file diff --git a/turtlebot3_core/libraries/ros_lib/geometry_msgs/PointStamped.h b/turtlebot3_core/libraries/ros_lib/geometry_msgs/PointStamped.h new file mode 100644 index 00000000..76cb5884 --- /dev/null +++ b/turtlebot3_core/libraries/ros_lib/geometry_msgs/PointStamped.h @@ -0,0 +1,48 @@ +#ifndef _ROS_geometry_msgs_PointStamped_h +#define _ROS_geometry_msgs_PointStamped_h + +#include +#include +#include +#include "ros/msg.h" +#include "std_msgs/Header.h" +#include "geometry_msgs/Point.h" + +namespace geometry_msgs +{ + + class PointStamped : public ros::Msg + { + public: + std_msgs::Header header; + geometry_msgs::Point point; + + PointStamped(): + header(), + point() + { + } + + virtual int serialize(unsigned char *outbuffer) const + { + int offset = 0; + offset += this->header.serialize(outbuffer + offset); + offset += this->point.serialize(outbuffer + offset); + return offset; + } + + virtual int deserialize(unsigned char *inbuffer) + { + int offset = 0; + offset += this->header.deserialize(inbuffer + offset); + offset += this->point.deserialize(inbuffer + offset); + return offset; + } + + const char * getType(){ return "geometry_msgs/PointStamped"; }; + const char * getMD5(){ return "c63aecb41bfdfd6b7e1fac37c7cbe7bf"; }; + + }; + +} +#endif \ No newline at end of file diff --git a/turtlebot3_core/libraries/ros_lib/geometry_msgs/Polygon.h b/turtlebot3_core/libraries/ros_lib/geometry_msgs/Polygon.h new file mode 100644 index 00000000..1ba3a549 --- /dev/null +++ b/turtlebot3_core/libraries/ros_lib/geometry_msgs/Polygon.h @@ -0,0 +1,63 @@ +#ifndef _ROS_geometry_msgs_Polygon_h +#define _ROS_geometry_msgs_Polygon_h + +#include +#include +#include +#include "ros/msg.h" +#include "geometry_msgs/Point32.h" + +namespace geometry_msgs +{ + + class Polygon : public ros::Msg + { + public: + uint32_t points_length; + geometry_msgs::Point32 st_points; + geometry_msgs::Point32 * points; + + Polygon(): + points_length(0), points(NULL) + { + } + + virtual int serialize(unsigned char *outbuffer) const + { + int offset = 0; + *(outbuffer + offset + 0) = (this->points_length >> (8 * 0)) & 0xFF; + *(outbuffer + offset + 1) = (this->points_length >> (8 * 1)) & 0xFF; + *(outbuffer + offset + 2) = (this->points_length >> (8 * 2)) & 0xFF; + *(outbuffer + offset + 3) = (this->points_length >> (8 * 3)) & 0xFF; + offset += sizeof(this->points_length); + for( uint32_t i = 0; i < points_length; i++){ + offset += this->points[i].serialize(outbuffer + offset); + } + return offset; + } + + virtual int deserialize(unsigned char *inbuffer) + { + int offset = 0; + uint32_t points_lengthT = ((uint32_t) (*(inbuffer + offset))); + points_lengthT |= ((uint32_t) (*(inbuffer + offset + 1))) << (8 * 1); + points_lengthT |= ((uint32_t) (*(inbuffer + offset + 2))) << (8 * 2); + points_lengthT |= ((uint32_t) (*(inbuffer + offset + 3))) << (8 * 3); + offset += sizeof(this->points_length); + if(points_lengthT > points_length) + this->points = (geometry_msgs::Point32*)realloc(this->points, points_lengthT * sizeof(geometry_msgs::Point32)); + points_length = points_lengthT; + for( uint32_t i = 0; i < points_length; i++){ + offset += this->st_points.deserialize(inbuffer + offset); + memcpy( &(this->points[i]), &(this->st_points), sizeof(geometry_msgs::Point32)); + } + return offset; + } + + const char * getType(){ return "geometry_msgs/Polygon"; }; + const char * getMD5(){ return "cd60a26494a087f577976f0329fa120e"; }; + + }; + +} +#endif \ No newline at end of file diff --git a/turtlebot3_core/libraries/ros_lib/geometry_msgs/PolygonStamped.h b/turtlebot3_core/libraries/ros_lib/geometry_msgs/PolygonStamped.h new file mode 100644 index 00000000..1f90c35a --- /dev/null +++ b/turtlebot3_core/libraries/ros_lib/geometry_msgs/PolygonStamped.h @@ -0,0 +1,48 @@ +#ifndef _ROS_geometry_msgs_PolygonStamped_h +#define _ROS_geometry_msgs_PolygonStamped_h + +#include +#include +#include +#include "ros/msg.h" +#include "std_msgs/Header.h" +#include "geometry_msgs/Polygon.h" + +namespace geometry_msgs +{ + + class PolygonStamped : public ros::Msg + { + public: + std_msgs::Header header; + geometry_msgs::Polygon polygon; + + PolygonStamped(): + header(), + polygon() + { + } + + virtual int serialize(unsigned char *outbuffer) const + { + int offset = 0; + offset += this->header.serialize(outbuffer + offset); + offset += this->polygon.serialize(outbuffer + offset); + return offset; + } + + virtual int deserialize(unsigned char *inbuffer) + { + int offset = 0; + offset += this->header.deserialize(inbuffer + offset); + offset += this->polygon.deserialize(inbuffer + offset); + return offset; + } + + const char * getType(){ return "geometry_msgs/PolygonStamped"; }; + const char * getMD5(){ return "c6be8f7dc3bee7fe9e8d296070f53340"; }; + + }; + +} +#endif \ No newline at end of file diff --git a/turtlebot3_core/libraries/ros_lib/geometry_msgs/Pose.h b/turtlebot3_core/libraries/ros_lib/geometry_msgs/Pose.h new file mode 100644 index 00000000..facec0f8 --- /dev/null +++ b/turtlebot3_core/libraries/ros_lib/geometry_msgs/Pose.h @@ -0,0 +1,48 @@ +#ifndef _ROS_geometry_msgs_Pose_h +#define _ROS_geometry_msgs_Pose_h + +#include +#include +#include +#include "ros/msg.h" +#include "geometry_msgs/Point.h" +#include "geometry_msgs/Quaternion.h" + +namespace geometry_msgs +{ + + class Pose : public ros::Msg + { + public: + geometry_msgs::Point position; + geometry_msgs::Quaternion orientation; + + Pose(): + position(), + orientation() + { + } + + virtual int serialize(unsigned char *outbuffer) const + { + int offset = 0; + offset += this->position.serialize(outbuffer + offset); + offset += this->orientation.serialize(outbuffer + offset); + return offset; + } + + virtual int deserialize(unsigned char *inbuffer) + { + int offset = 0; + offset += this->position.deserialize(inbuffer + offset); + offset += this->orientation.deserialize(inbuffer + offset); + return offset; + } + + const char * getType(){ return "geometry_msgs/Pose"; }; + const char * getMD5(){ return "e45d45a5a1ce597b249e23fb30fc871f"; }; + + }; + +} +#endif \ No newline at end of file diff --git a/turtlebot3_core/libraries/ros_lib/geometry_msgs/Pose2D.h b/turtlebot3_core/libraries/ros_lib/geometry_msgs/Pose2D.h new file mode 100644 index 00000000..a0117180 --- /dev/null +++ b/turtlebot3_core/libraries/ros_lib/geometry_msgs/Pose2D.h @@ -0,0 +1,50 @@ +#ifndef _ROS_geometry_msgs_Pose2D_h +#define _ROS_geometry_msgs_Pose2D_h + +#include +#include +#include +#include "ros/msg.h" + +namespace geometry_msgs +{ + + class Pose2D : public ros::Msg + { + public: + float x; + float y; + float theta; + + Pose2D(): + x(0), + y(0), + theta(0) + { + } + + virtual int serialize(unsigned char *outbuffer) const + { + int offset = 0; + offset += serializeAvrFloat64(outbuffer + offset, this->x); + offset += serializeAvrFloat64(outbuffer + offset, this->y); + offset += serializeAvrFloat64(outbuffer + offset, this->theta); + return offset; + } + + virtual int deserialize(unsigned char *inbuffer) + { + int offset = 0; + offset += deserializeAvrFloat64(inbuffer + offset, &(this->x)); + offset += deserializeAvrFloat64(inbuffer + offset, &(this->y)); + offset += deserializeAvrFloat64(inbuffer + offset, &(this->theta)); + return offset; + } + + const char * getType(){ return "geometry_msgs/Pose2D"; }; + const char * getMD5(){ return "938fa65709584ad8e77d238529be13b8"; }; + + }; + +} +#endif \ No newline at end of file diff --git a/turtlebot3_core/libraries/ros_lib/geometry_msgs/PoseArray.h b/turtlebot3_core/libraries/ros_lib/geometry_msgs/PoseArray.h new file mode 100644 index 00000000..c5773c99 --- /dev/null +++ b/turtlebot3_core/libraries/ros_lib/geometry_msgs/PoseArray.h @@ -0,0 +1,68 @@ +#ifndef _ROS_geometry_msgs_PoseArray_h +#define _ROS_geometry_msgs_PoseArray_h + +#include +#include +#include +#include "ros/msg.h" +#include "std_msgs/Header.h" +#include "geometry_msgs/Pose.h" + +namespace geometry_msgs +{ + + class PoseArray : public ros::Msg + { + public: + std_msgs::Header header; + uint32_t poses_length; + geometry_msgs::Pose st_poses; + geometry_msgs::Pose * poses; + + PoseArray(): + header(), + poses_length(0), poses(NULL) + { + } + + virtual int serialize(unsigned char *outbuffer) const + { + int offset = 0; + offset += this->header.serialize(outbuffer + offset); + *(outbuffer + offset + 0) = (this->poses_length >> (8 * 0)) & 0xFF; + *(outbuffer + offset + 1) = (this->poses_length >> (8 * 1)) & 0xFF; + *(outbuffer + offset + 2) = (this->poses_length >> (8 * 2)) & 0xFF; + *(outbuffer + offset + 3) = (this->poses_length >> (8 * 3)) & 0xFF; + offset += sizeof(this->poses_length); + for( uint32_t i = 0; i < poses_length; i++){ + offset += this->poses[i].serialize(outbuffer + offset); + } + return offset; + } + + virtual int deserialize(unsigned char *inbuffer) + { + int offset = 0; + offset += this->header.deserialize(inbuffer + offset); + uint32_t poses_lengthT = ((uint32_t) (*(inbuffer + offset))); + poses_lengthT |= ((uint32_t) (*(inbuffer + offset + 1))) << (8 * 1); + poses_lengthT |= ((uint32_t) (*(inbuffer + offset + 2))) << (8 * 2); + poses_lengthT |= ((uint32_t) (*(inbuffer + offset + 3))) << (8 * 3); + offset += sizeof(this->poses_length); + if(poses_lengthT > poses_length) + this->poses = (geometry_msgs::Pose*)realloc(this->poses, poses_lengthT * sizeof(geometry_msgs::Pose)); + poses_length = poses_lengthT; + for( uint32_t i = 0; i < poses_length; i++){ + offset += this->st_poses.deserialize(inbuffer + offset); + memcpy( &(this->poses[i]), &(this->st_poses), sizeof(geometry_msgs::Pose)); + } + return offset; + } + + const char * getType(){ return "geometry_msgs/PoseArray"; }; + const char * getMD5(){ return "916c28c5764443f268b296bb671b9d97"; }; + + }; + +} +#endif \ No newline at end of file diff --git a/turtlebot3_core/libraries/ros_lib/geometry_msgs/PoseStamped.h b/turtlebot3_core/libraries/ros_lib/geometry_msgs/PoseStamped.h new file mode 100644 index 00000000..ae466abc --- /dev/null +++ b/turtlebot3_core/libraries/ros_lib/geometry_msgs/PoseStamped.h @@ -0,0 +1,48 @@ +#ifndef _ROS_geometry_msgs_PoseStamped_h +#define _ROS_geometry_msgs_PoseStamped_h + +#include +#include +#include +#include "ros/msg.h" +#include "std_msgs/Header.h" +#include "geometry_msgs/Pose.h" + +namespace geometry_msgs +{ + + class PoseStamped : public ros::Msg + { + public: + std_msgs::Header header; + geometry_msgs::Pose pose; + + PoseStamped(): + header(), + pose() + { + } + + virtual int serialize(unsigned char *outbuffer) const + { + int offset = 0; + offset += this->header.serialize(outbuffer + offset); + offset += this->pose.serialize(outbuffer + offset); + return offset; + } + + virtual int deserialize(unsigned char *inbuffer) + { + int offset = 0; + offset += this->header.deserialize(inbuffer + offset); + offset += this->pose.deserialize(inbuffer + offset); + return offset; + } + + const char * getType(){ return "geometry_msgs/PoseStamped"; }; + const char * getMD5(){ return "d3812c3cbc69362b77dc0b19b345f8f5"; }; + + }; + +} +#endif \ No newline at end of file diff --git a/turtlebot3_core/libraries/ros_lib/geometry_msgs/PoseWithCovariance.h b/turtlebot3_core/libraries/ros_lib/geometry_msgs/PoseWithCovariance.h new file mode 100644 index 00000000..d6f24610 --- /dev/null +++ b/turtlebot3_core/libraries/ros_lib/geometry_msgs/PoseWithCovariance.h @@ -0,0 +1,51 @@ +#ifndef _ROS_geometry_msgs_PoseWithCovariance_h +#define _ROS_geometry_msgs_PoseWithCovariance_h + +#include +#include +#include +#include "ros/msg.h" +#include "geometry_msgs/Pose.h" + +namespace geometry_msgs +{ + + class PoseWithCovariance : public ros::Msg + { + public: + geometry_msgs::Pose pose; + float covariance[36]; + + PoseWithCovariance(): + pose(), + covariance() + { + } + + virtual int serialize(unsigned char *outbuffer) const + { + int offset = 0; + offset += this->pose.serialize(outbuffer + offset); + for( uint32_t i = 0; i < 36; i++){ + offset += serializeAvrFloat64(outbuffer + offset, this->covariance[i]); + } + return offset; + } + + virtual int deserialize(unsigned char *inbuffer) + { + int offset = 0; + offset += this->pose.deserialize(inbuffer + offset); + for( uint32_t i = 0; i < 36; i++){ + offset += deserializeAvrFloat64(inbuffer + offset, &(this->covariance[i])); + } + return offset; + } + + const char * getType(){ return "geometry_msgs/PoseWithCovariance"; }; + const char * getMD5(){ return "c23e848cf1b7533a8d7c259073a97e6f"; }; + + }; + +} +#endif \ No newline at end of file diff --git a/turtlebot3_core/libraries/ros_lib/geometry_msgs/PoseWithCovarianceStamped.h b/turtlebot3_core/libraries/ros_lib/geometry_msgs/PoseWithCovarianceStamped.h new file mode 100644 index 00000000..586a9303 --- /dev/null +++ b/turtlebot3_core/libraries/ros_lib/geometry_msgs/PoseWithCovarianceStamped.h @@ -0,0 +1,48 @@ +#ifndef _ROS_geometry_msgs_PoseWithCovarianceStamped_h +#define _ROS_geometry_msgs_PoseWithCovarianceStamped_h + +#include +#include +#include +#include "ros/msg.h" +#include "std_msgs/Header.h" +#include "geometry_msgs/PoseWithCovariance.h" + +namespace geometry_msgs +{ + + class PoseWithCovarianceStamped : public ros::Msg + { + public: + std_msgs::Header header; + geometry_msgs::PoseWithCovariance pose; + + PoseWithCovarianceStamped(): + header(), + pose() + { + } + + virtual int serialize(unsigned char *outbuffer) const + { + int offset = 0; + offset += this->header.serialize(outbuffer + offset); + offset += this->pose.serialize(outbuffer + offset); + return offset; + } + + virtual int deserialize(unsigned char *inbuffer) + { + int offset = 0; + offset += this->header.deserialize(inbuffer + offset); + offset += this->pose.deserialize(inbuffer + offset); + return offset; + } + + const char * getType(){ return "geometry_msgs/PoseWithCovarianceStamped"; }; + const char * getMD5(){ return "953b798c0f514ff060a53a3498ce6246"; }; + + }; + +} +#endif \ No newline at end of file diff --git a/turtlebot3_core/libraries/ros_lib/geometry_msgs/Quaternion.h b/turtlebot3_core/libraries/ros_lib/geometry_msgs/Quaternion.h new file mode 100644 index 00000000..69cddbe0 --- /dev/null +++ b/turtlebot3_core/libraries/ros_lib/geometry_msgs/Quaternion.h @@ -0,0 +1,54 @@ +#ifndef _ROS_geometry_msgs_Quaternion_h +#define _ROS_geometry_msgs_Quaternion_h + +#include +#include +#include +#include "ros/msg.h" + +namespace geometry_msgs +{ + + class Quaternion : public ros::Msg + { + public: + float x; + float y; + float z; + float w; + + Quaternion(): + x(0), + y(0), + z(0), + w(0) + { + } + + virtual int serialize(unsigned char *outbuffer) const + { + int offset = 0; + offset += serializeAvrFloat64(outbuffer + offset, this->x); + offset += serializeAvrFloat64(outbuffer + offset, this->y); + offset += serializeAvrFloat64(outbuffer + offset, this->z); + offset += serializeAvrFloat64(outbuffer + offset, this->w); + return offset; + } + + virtual int deserialize(unsigned char *inbuffer) + { + int offset = 0; + offset += deserializeAvrFloat64(inbuffer + offset, &(this->x)); + offset += deserializeAvrFloat64(inbuffer + offset, &(this->y)); + offset += deserializeAvrFloat64(inbuffer + offset, &(this->z)); + offset += deserializeAvrFloat64(inbuffer + offset, &(this->w)); + return offset; + } + + const char * getType(){ return "geometry_msgs/Quaternion"; }; + const char * getMD5(){ return "a779879fadf0160734f906b8c19c7004"; }; + + }; + +} +#endif \ No newline at end of file diff --git a/turtlebot3_core/libraries/ros_lib/geometry_msgs/QuaternionStamped.h b/turtlebot3_core/libraries/ros_lib/geometry_msgs/QuaternionStamped.h new file mode 100644 index 00000000..c2e0fcfe --- /dev/null +++ b/turtlebot3_core/libraries/ros_lib/geometry_msgs/QuaternionStamped.h @@ -0,0 +1,48 @@ +#ifndef _ROS_geometry_msgs_QuaternionStamped_h +#define _ROS_geometry_msgs_QuaternionStamped_h + +#include +#include +#include +#include "ros/msg.h" +#include "std_msgs/Header.h" +#include "geometry_msgs/Quaternion.h" + +namespace geometry_msgs +{ + + class QuaternionStamped : public ros::Msg + { + public: + std_msgs::Header header; + geometry_msgs::Quaternion quaternion; + + QuaternionStamped(): + header(), + quaternion() + { + } + + virtual int serialize(unsigned char *outbuffer) const + { + int offset = 0; + offset += this->header.serialize(outbuffer + offset); + offset += this->quaternion.serialize(outbuffer + offset); + return offset; + } + + virtual int deserialize(unsigned char *inbuffer) + { + int offset = 0; + offset += this->header.deserialize(inbuffer + offset); + offset += this->quaternion.deserialize(inbuffer + offset); + return offset; + } + + const char * getType(){ return "geometry_msgs/QuaternionStamped"; }; + const char * getMD5(){ return "e57f1e547e0e1fd13504588ffc8334e2"; }; + + }; + +} +#endif \ No newline at end of file diff --git a/turtlebot3_core/libraries/ros_lib/geometry_msgs/Transform.h b/turtlebot3_core/libraries/ros_lib/geometry_msgs/Transform.h new file mode 100644 index 00000000..94cf7381 --- /dev/null +++ b/turtlebot3_core/libraries/ros_lib/geometry_msgs/Transform.h @@ -0,0 +1,48 @@ +#ifndef _ROS_geometry_msgs_Transform_h +#define _ROS_geometry_msgs_Transform_h + +#include +#include +#include +#include "ros/msg.h" +#include "geometry_msgs/Vector3.h" +#include "geometry_msgs/Quaternion.h" + +namespace geometry_msgs +{ + + class Transform : public ros::Msg + { + public: + geometry_msgs::Vector3 translation; + geometry_msgs::Quaternion rotation; + + Transform(): + translation(), + rotation() + { + } + + virtual int serialize(unsigned char *outbuffer) const + { + int offset = 0; + offset += this->translation.serialize(outbuffer + offset); + offset += this->rotation.serialize(outbuffer + offset); + return offset; + } + + virtual int deserialize(unsigned char *inbuffer) + { + int offset = 0; + offset += this->translation.deserialize(inbuffer + offset); + offset += this->rotation.deserialize(inbuffer + offset); + return offset; + } + + const char * getType(){ return "geometry_msgs/Transform"; }; + const char * getMD5(){ return "ac9eff44abf714214112b05d54a3cf9b"; }; + + }; + +} +#endif \ No newline at end of file diff --git a/turtlebot3_core/libraries/ros_lib/geometry_msgs/TransformStamped.h b/turtlebot3_core/libraries/ros_lib/geometry_msgs/TransformStamped.h new file mode 100644 index 00000000..d42125e2 --- /dev/null +++ b/turtlebot3_core/libraries/ros_lib/geometry_msgs/TransformStamped.h @@ -0,0 +1,64 @@ +#ifndef _ROS_geometry_msgs_TransformStamped_h +#define _ROS_geometry_msgs_TransformStamped_h + +#include +#include +#include +#include "ros/msg.h" +#include "std_msgs/Header.h" +#include "geometry_msgs/Transform.h" + +namespace geometry_msgs +{ + + class TransformStamped : public ros::Msg + { + public: + std_msgs::Header header; + const char* child_frame_id; + geometry_msgs::Transform transform; + + TransformStamped(): + header(), + child_frame_id(""), + transform() + { + } + + virtual int serialize(unsigned char *outbuffer) const + { + int offset = 0; + offset += this->header.serialize(outbuffer + offset); + uint32_t length_child_frame_id = strlen(this->child_frame_id); + memcpy(outbuffer + offset, &length_child_frame_id, sizeof(uint32_t)); + offset += 4; + memcpy(outbuffer + offset, this->child_frame_id, length_child_frame_id); + offset += length_child_frame_id; + offset += this->transform.serialize(outbuffer + offset); + return offset; + } + + virtual int deserialize(unsigned char *inbuffer) + { + int offset = 0; + offset += this->header.deserialize(inbuffer + offset); + uint32_t length_child_frame_id; + memcpy(&length_child_frame_id, (inbuffer + offset), sizeof(uint32_t)); + offset += 4; + for(unsigned int k= offset; k< offset+length_child_frame_id; ++k){ + inbuffer[k-1]=inbuffer[k]; + } + inbuffer[offset+length_child_frame_id-1]=0; + this->child_frame_id = (char *)(inbuffer + offset-1); + offset += length_child_frame_id; + offset += this->transform.deserialize(inbuffer + offset); + return offset; + } + + const char * getType(){ return "geometry_msgs/TransformStamped"; }; + const char * getMD5(){ return "b5764a33bfeb3588febc2682852579b0"; }; + + }; + +} +#endif \ No newline at end of file diff --git a/turtlebot3_core/libraries/ros_lib/geometry_msgs/Twist.h b/turtlebot3_core/libraries/ros_lib/geometry_msgs/Twist.h new file mode 100644 index 00000000..6133c64a --- /dev/null +++ b/turtlebot3_core/libraries/ros_lib/geometry_msgs/Twist.h @@ -0,0 +1,47 @@ +#ifndef _ROS_geometry_msgs_Twist_h +#define _ROS_geometry_msgs_Twist_h + +#include +#include +#include +#include "ros/msg.h" +#include "geometry_msgs/Vector3.h" + +namespace geometry_msgs +{ + + class Twist : public ros::Msg + { + public: + geometry_msgs::Vector3 linear; + geometry_msgs::Vector3 angular; + + Twist(): + linear(), + angular() + { + } + + virtual int serialize(unsigned char *outbuffer) const + { + int offset = 0; + offset += this->linear.serialize(outbuffer + offset); + offset += this->angular.serialize(outbuffer + offset); + return offset; + } + + virtual int deserialize(unsigned char *inbuffer) + { + int offset = 0; + offset += this->linear.deserialize(inbuffer + offset); + offset += this->angular.deserialize(inbuffer + offset); + return offset; + } + + const char * getType(){ return "geometry_msgs/Twist"; }; + const char * getMD5(){ return "9f195f881246fdfa2798d1d3eebca84a"; }; + + }; + +} +#endif \ No newline at end of file diff --git a/turtlebot3_core/libraries/ros_lib/geometry_msgs/TwistStamped.h b/turtlebot3_core/libraries/ros_lib/geometry_msgs/TwistStamped.h new file mode 100644 index 00000000..bf399406 --- /dev/null +++ b/turtlebot3_core/libraries/ros_lib/geometry_msgs/TwistStamped.h @@ -0,0 +1,48 @@ +#ifndef _ROS_geometry_msgs_TwistStamped_h +#define _ROS_geometry_msgs_TwistStamped_h + +#include +#include +#include +#include "ros/msg.h" +#include "std_msgs/Header.h" +#include "geometry_msgs/Twist.h" + +namespace geometry_msgs +{ + + class TwistStamped : public ros::Msg + { + public: + std_msgs::Header header; + geometry_msgs::Twist twist; + + TwistStamped(): + header(), + twist() + { + } + + virtual int serialize(unsigned char *outbuffer) const + { + int offset = 0; + offset += this->header.serialize(outbuffer + offset); + offset += this->twist.serialize(outbuffer + offset); + return offset; + } + + virtual int deserialize(unsigned char *inbuffer) + { + int offset = 0; + offset += this->header.deserialize(inbuffer + offset); + offset += this->twist.deserialize(inbuffer + offset); + return offset; + } + + const char * getType(){ return "geometry_msgs/TwistStamped"; }; + const char * getMD5(){ return "98d34b0043a2093cf9d9345ab6eef12e"; }; + + }; + +} +#endif \ No newline at end of file diff --git a/turtlebot3_core/libraries/ros_lib/geometry_msgs/TwistWithCovariance.h b/turtlebot3_core/libraries/ros_lib/geometry_msgs/TwistWithCovariance.h new file mode 100644 index 00000000..3618841e --- /dev/null +++ b/turtlebot3_core/libraries/ros_lib/geometry_msgs/TwistWithCovariance.h @@ -0,0 +1,51 @@ +#ifndef _ROS_geometry_msgs_TwistWithCovariance_h +#define _ROS_geometry_msgs_TwistWithCovariance_h + +#include +#include +#include +#include "ros/msg.h" +#include "geometry_msgs/Twist.h" + +namespace geometry_msgs +{ + + class TwistWithCovariance : public ros::Msg + { + public: + geometry_msgs::Twist twist; + float covariance[36]; + + TwistWithCovariance(): + twist(), + covariance() + { + } + + virtual int serialize(unsigned char *outbuffer) const + { + int offset = 0; + offset += this->twist.serialize(outbuffer + offset); + for( uint32_t i = 0; i < 36; i++){ + offset += serializeAvrFloat64(outbuffer + offset, this->covariance[i]); + } + return offset; + } + + virtual int deserialize(unsigned char *inbuffer) + { + int offset = 0; + offset += this->twist.deserialize(inbuffer + offset); + for( uint32_t i = 0; i < 36; i++){ + offset += deserializeAvrFloat64(inbuffer + offset, &(this->covariance[i])); + } + return offset; + } + + const char * getType(){ return "geometry_msgs/TwistWithCovariance"; }; + const char * getMD5(){ return "1fe8a28e6890a4cc3ae4c3ca5c7d82e6"; }; + + }; + +} +#endif \ No newline at end of file diff --git a/turtlebot3_core/libraries/ros_lib/geometry_msgs/TwistWithCovarianceStamped.h b/turtlebot3_core/libraries/ros_lib/geometry_msgs/TwistWithCovarianceStamped.h new file mode 100644 index 00000000..5ea6c8e4 --- /dev/null +++ b/turtlebot3_core/libraries/ros_lib/geometry_msgs/TwistWithCovarianceStamped.h @@ -0,0 +1,48 @@ +#ifndef _ROS_geometry_msgs_TwistWithCovarianceStamped_h +#define _ROS_geometry_msgs_TwistWithCovarianceStamped_h + +#include +#include +#include +#include "ros/msg.h" +#include "std_msgs/Header.h" +#include "geometry_msgs/TwistWithCovariance.h" + +namespace geometry_msgs +{ + + class TwistWithCovarianceStamped : public ros::Msg + { + public: + std_msgs::Header header; + geometry_msgs::TwistWithCovariance twist; + + TwistWithCovarianceStamped(): + header(), + twist() + { + } + + virtual int serialize(unsigned char *outbuffer) const + { + int offset = 0; + offset += this->header.serialize(outbuffer + offset); + offset += this->twist.serialize(outbuffer + offset); + return offset; + } + + virtual int deserialize(unsigned char *inbuffer) + { + int offset = 0; + offset += this->header.deserialize(inbuffer + offset); + offset += this->twist.deserialize(inbuffer + offset); + return offset; + } + + const char * getType(){ return "geometry_msgs/TwistWithCovarianceStamped"; }; + const char * getMD5(){ return "8927a1a12fb2607ceea095b2dc440a96"; }; + + }; + +} +#endif \ No newline at end of file diff --git a/turtlebot3_core/libraries/ros_lib/geometry_msgs/Vector3.h b/turtlebot3_core/libraries/ros_lib/geometry_msgs/Vector3.h new file mode 100644 index 00000000..af3f2538 --- /dev/null +++ b/turtlebot3_core/libraries/ros_lib/geometry_msgs/Vector3.h @@ -0,0 +1,50 @@ +#ifndef _ROS_geometry_msgs_Vector3_h +#define _ROS_geometry_msgs_Vector3_h + +#include +#include +#include +#include "ros/msg.h" + +namespace geometry_msgs +{ + + class Vector3 : public ros::Msg + { + public: + float x; + float y; + float z; + + Vector3(): + x(0), + y(0), + z(0) + { + } + + virtual int serialize(unsigned char *outbuffer) const + { + int offset = 0; + offset += serializeAvrFloat64(outbuffer + offset, this->x); + offset += serializeAvrFloat64(outbuffer + offset, this->y); + offset += serializeAvrFloat64(outbuffer + offset, this->z); + return offset; + } + + virtual int deserialize(unsigned char *inbuffer) + { + int offset = 0; + offset += deserializeAvrFloat64(inbuffer + offset, &(this->x)); + offset += deserializeAvrFloat64(inbuffer + offset, &(this->y)); + offset += deserializeAvrFloat64(inbuffer + offset, &(this->z)); + return offset; + } + + const char * getType(){ return "geometry_msgs/Vector3"; }; + const char * getMD5(){ return "4a842b65f413084dc2b10fb484ea7f17"; }; + + }; + +} +#endif \ No newline at end of file diff --git a/turtlebot3_core/libraries/ros_lib/geometry_msgs/Vector3Stamped.h b/turtlebot3_core/libraries/ros_lib/geometry_msgs/Vector3Stamped.h new file mode 100644 index 00000000..589d5712 --- /dev/null +++ b/turtlebot3_core/libraries/ros_lib/geometry_msgs/Vector3Stamped.h @@ -0,0 +1,48 @@ +#ifndef _ROS_geometry_msgs_Vector3Stamped_h +#define _ROS_geometry_msgs_Vector3Stamped_h + +#include +#include +#include +#include "ros/msg.h" +#include "std_msgs/Header.h" +#include "geometry_msgs/Vector3.h" + +namespace geometry_msgs +{ + + class Vector3Stamped : public ros::Msg + { + public: + std_msgs::Header header; + geometry_msgs::Vector3 vector; + + Vector3Stamped(): + header(), + vector() + { + } + + virtual int serialize(unsigned char *outbuffer) const + { + int offset = 0; + offset += this->header.serialize(outbuffer + offset); + offset += this->vector.serialize(outbuffer + offset); + return offset; + } + + virtual int deserialize(unsigned char *inbuffer) + { + int offset = 0; + offset += this->header.deserialize(inbuffer + offset); + offset += this->vector.deserialize(inbuffer + offset); + return offset; + } + + const char * getType(){ return "geometry_msgs/Vector3Stamped"; }; + const char * getMD5(){ return "7b324c7325e683bf02a9b14b01090ec7"; }; + + }; + +} +#endif \ No newline at end of file diff --git a/turtlebot3_core/libraries/ros_lib/geometry_msgs/Wrench.h b/turtlebot3_core/libraries/ros_lib/geometry_msgs/Wrench.h new file mode 100644 index 00000000..d95bf1b8 --- /dev/null +++ b/turtlebot3_core/libraries/ros_lib/geometry_msgs/Wrench.h @@ -0,0 +1,47 @@ +#ifndef _ROS_geometry_msgs_Wrench_h +#define _ROS_geometry_msgs_Wrench_h + +#include +#include +#include +#include "ros/msg.h" +#include "geometry_msgs/Vector3.h" + +namespace geometry_msgs +{ + + class Wrench : public ros::Msg + { + public: + geometry_msgs::Vector3 force; + geometry_msgs::Vector3 torque; + + Wrench(): + force(), + torque() + { + } + + virtual int serialize(unsigned char *outbuffer) const + { + int offset = 0; + offset += this->force.serialize(outbuffer + offset); + offset += this->torque.serialize(outbuffer + offset); + return offset; + } + + virtual int deserialize(unsigned char *inbuffer) + { + int offset = 0; + offset += this->force.deserialize(inbuffer + offset); + offset += this->torque.deserialize(inbuffer + offset); + return offset; + } + + const char * getType(){ return "geometry_msgs/Wrench"; }; + const char * getMD5(){ return "4f539cf138b23283b520fd271b567936"; }; + + }; + +} +#endif \ No newline at end of file diff --git a/turtlebot3_core/libraries/ros_lib/geometry_msgs/WrenchStamped.h b/turtlebot3_core/libraries/ros_lib/geometry_msgs/WrenchStamped.h new file mode 100644 index 00000000..86becdcf --- /dev/null +++ b/turtlebot3_core/libraries/ros_lib/geometry_msgs/WrenchStamped.h @@ -0,0 +1,48 @@ +#ifndef _ROS_geometry_msgs_WrenchStamped_h +#define _ROS_geometry_msgs_WrenchStamped_h + +#include +#include +#include +#include "ros/msg.h" +#include "std_msgs/Header.h" +#include "geometry_msgs/Wrench.h" + +namespace geometry_msgs +{ + + class WrenchStamped : public ros::Msg + { + public: + std_msgs::Header header; + geometry_msgs::Wrench wrench; + + WrenchStamped(): + header(), + wrench() + { + } + + virtual int serialize(unsigned char *outbuffer) const + { + int offset = 0; + offset += this->header.serialize(outbuffer + offset); + offset += this->wrench.serialize(outbuffer + offset); + return offset; + } + + virtual int deserialize(unsigned char *inbuffer) + { + int offset = 0; + offset += this->header.deserialize(inbuffer + offset); + offset += this->wrench.deserialize(inbuffer + offset); + return offset; + } + + const char * getType(){ return "geometry_msgs/WrenchStamped"; }; + const char * getMD5(){ return "d78d3cb249ce23087ade7e7d0c40cfa7"; }; + + }; + +} +#endif \ No newline at end of file diff --git a/turtlebot3_core/libraries/ros_lib/nav_msgs/GetMap.h b/turtlebot3_core/libraries/ros_lib/nav_msgs/GetMap.h new file mode 100644 index 00000000..5fa87e87 --- /dev/null +++ b/turtlebot3_core/libraries/ros_lib/nav_msgs/GetMap.h @@ -0,0 +1,75 @@ +#ifndef _ROS_SERVICE_GetMap_h +#define _ROS_SERVICE_GetMap_h +#include +#include +#include +#include "ros/msg.h" +#include "nav_msgs/OccupancyGrid.h" + +namespace nav_msgs +{ + +static const char GETMAP[] = "nav_msgs/GetMap"; + + class GetMapRequest : public ros::Msg + { + public: + + GetMapRequest() + { + } + + virtual int serialize(unsigned char *outbuffer) const + { + int offset = 0; + return offset; + } + + virtual int deserialize(unsigned char *inbuffer) + { + int offset = 0; + return offset; + } + + const char * getType(){ return GETMAP; }; + const char * getMD5(){ return "d41d8cd98f00b204e9800998ecf8427e"; }; + + }; + + class GetMapResponse : public ros::Msg + { + public: + nav_msgs::OccupancyGrid map; + + GetMapResponse(): + map() + { + } + + virtual int serialize(unsigned char *outbuffer) const + { + int offset = 0; + offset += this->map.serialize(outbuffer + offset); + return offset; + } + + virtual int deserialize(unsigned char *inbuffer) + { + int offset = 0; + offset += this->map.deserialize(inbuffer + offset); + return offset; + } + + const char * getType(){ return GETMAP; }; + const char * getMD5(){ return "6cdd0a18e0aff5b0a3ca2326a89b54ff"; }; + + }; + + class GetMap { + public: + typedef GetMapRequest Request; + typedef GetMapResponse Response; + }; + +} +#endif diff --git a/turtlebot3_core/libraries/ros_lib/nav_msgs/GetMapAction.h b/turtlebot3_core/libraries/ros_lib/nav_msgs/GetMapAction.h new file mode 100644 index 00000000..774101b3 --- /dev/null +++ b/turtlebot3_core/libraries/ros_lib/nav_msgs/GetMapAction.h @@ -0,0 +1,53 @@ +#ifndef _ROS_nav_msgs_GetMapAction_h +#define _ROS_nav_msgs_GetMapAction_h + +#include +#include +#include +#include "ros/msg.h" +#include "nav_msgs/GetMapActionGoal.h" +#include "nav_msgs/GetMapActionResult.h" +#include "nav_msgs/GetMapActionFeedback.h" + +namespace nav_msgs +{ + + class GetMapAction : public ros::Msg + { + public: + nav_msgs::GetMapActionGoal action_goal; + nav_msgs::GetMapActionResult action_result; + nav_msgs::GetMapActionFeedback action_feedback; + + GetMapAction(): + action_goal(), + action_result(), + action_feedback() + { + } + + virtual int serialize(unsigned char *outbuffer) const + { + int offset = 0; + offset += this->action_goal.serialize(outbuffer + offset); + offset += this->action_result.serialize(outbuffer + offset); + offset += this->action_feedback.serialize(outbuffer + offset); + return offset; + } + + virtual int deserialize(unsigned char *inbuffer) + { + int offset = 0; + offset += this->action_goal.deserialize(inbuffer + offset); + offset += this->action_result.deserialize(inbuffer + offset); + offset += this->action_feedback.deserialize(inbuffer + offset); + return offset; + } + + const char * getType(){ return "nav_msgs/GetMapAction"; }; + const char * getMD5(){ return "e611ad23fbf237c031b7536416dc7cd7"; }; + + }; + +} +#endif \ No newline at end of file diff --git a/turtlebot3_core/libraries/ros_lib/nav_msgs/GetMapActionFeedback.h b/turtlebot3_core/libraries/ros_lib/nav_msgs/GetMapActionFeedback.h new file mode 100644 index 00000000..e5445b34 --- /dev/null +++ b/turtlebot3_core/libraries/ros_lib/nav_msgs/GetMapActionFeedback.h @@ -0,0 +1,53 @@ +#ifndef _ROS_nav_msgs_GetMapActionFeedback_h +#define _ROS_nav_msgs_GetMapActionFeedback_h + +#include +#include +#include +#include "ros/msg.h" +#include "std_msgs/Header.h" +#include "actionlib_msgs/GoalStatus.h" +#include "nav_msgs/GetMapFeedback.h" + +namespace nav_msgs +{ + + class GetMapActionFeedback : public ros::Msg + { + public: + std_msgs::Header header; + actionlib_msgs::GoalStatus status; + nav_msgs::GetMapFeedback feedback; + + GetMapActionFeedback(): + header(), + status(), + feedback() + { + } + + virtual int serialize(unsigned char *outbuffer) const + { + int offset = 0; + offset += this->header.serialize(outbuffer + offset); + offset += this->status.serialize(outbuffer + offset); + offset += this->feedback.serialize(outbuffer + offset); + return offset; + } + + virtual int deserialize(unsigned char *inbuffer) + { + int offset = 0; + offset += this->header.deserialize(inbuffer + offset); + offset += this->status.deserialize(inbuffer + offset); + offset += this->feedback.deserialize(inbuffer + offset); + return offset; + } + + const char * getType(){ return "nav_msgs/GetMapActionFeedback"; }; + const char * getMD5(){ return "aae20e09065c3809e8a8e87c4c8953fd"; }; + + }; + +} +#endif \ No newline at end of file diff --git a/turtlebot3_core/libraries/ros_lib/nav_msgs/GetMapActionGoal.h b/turtlebot3_core/libraries/ros_lib/nav_msgs/GetMapActionGoal.h new file mode 100644 index 00000000..644be5ca --- /dev/null +++ b/turtlebot3_core/libraries/ros_lib/nav_msgs/GetMapActionGoal.h @@ -0,0 +1,53 @@ +#ifndef _ROS_nav_msgs_GetMapActionGoal_h +#define _ROS_nav_msgs_GetMapActionGoal_h + +#include +#include +#include +#include "ros/msg.h" +#include "std_msgs/Header.h" +#include "actionlib_msgs/GoalID.h" +#include "nav_msgs/GetMapGoal.h" + +namespace nav_msgs +{ + + class GetMapActionGoal : public ros::Msg + { + public: + std_msgs::Header header; + actionlib_msgs::GoalID goal_id; + nav_msgs::GetMapGoal goal; + + GetMapActionGoal(): + header(), + goal_id(), + goal() + { + } + + virtual int serialize(unsigned char *outbuffer) const + { + int offset = 0; + offset += this->header.serialize(outbuffer + offset); + offset += this->goal_id.serialize(outbuffer + offset); + offset += this->goal.serialize(outbuffer + offset); + return offset; + } + + virtual int deserialize(unsigned char *inbuffer) + { + int offset = 0; + offset += this->header.deserialize(inbuffer + offset); + offset += this->goal_id.deserialize(inbuffer + offset); + offset += this->goal.deserialize(inbuffer + offset); + return offset; + } + + const char * getType(){ return "nav_msgs/GetMapActionGoal"; }; + const char * getMD5(){ return "4b30be6cd12b9e72826df56b481f40e0"; }; + + }; + +} +#endif \ No newline at end of file diff --git a/turtlebot3_core/libraries/ros_lib/nav_msgs/GetMapActionResult.h b/turtlebot3_core/libraries/ros_lib/nav_msgs/GetMapActionResult.h new file mode 100644 index 00000000..b635cfdf --- /dev/null +++ b/turtlebot3_core/libraries/ros_lib/nav_msgs/GetMapActionResult.h @@ -0,0 +1,53 @@ +#ifndef _ROS_nav_msgs_GetMapActionResult_h +#define _ROS_nav_msgs_GetMapActionResult_h + +#include +#include +#include +#include "ros/msg.h" +#include "std_msgs/Header.h" +#include "actionlib_msgs/GoalStatus.h" +#include "nav_msgs/GetMapResult.h" + +namespace nav_msgs +{ + + class GetMapActionResult : public ros::Msg + { + public: + std_msgs::Header header; + actionlib_msgs::GoalStatus status; + nav_msgs::GetMapResult result; + + GetMapActionResult(): + header(), + status(), + result() + { + } + + virtual int serialize(unsigned char *outbuffer) const + { + int offset = 0; + offset += this->header.serialize(outbuffer + offset); + offset += this->status.serialize(outbuffer + offset); + offset += this->result.serialize(outbuffer + offset); + return offset; + } + + virtual int deserialize(unsigned char *inbuffer) + { + int offset = 0; + offset += this->header.deserialize(inbuffer + offset); + offset += this->status.deserialize(inbuffer + offset); + offset += this->result.deserialize(inbuffer + offset); + return offset; + } + + const char * getType(){ return "nav_msgs/GetMapActionResult"; }; + const char * getMD5(){ return "ac66e5b9a79bb4bbd33dab245236c892"; }; + + }; + +} +#endif \ No newline at end of file diff --git a/turtlebot3_core/libraries/ros_lib/nav_msgs/GetMapFeedback.h b/turtlebot3_core/libraries/ros_lib/nav_msgs/GetMapFeedback.h new file mode 100644 index 00000000..e3b4560a --- /dev/null +++ b/turtlebot3_core/libraries/ros_lib/nav_msgs/GetMapFeedback.h @@ -0,0 +1,38 @@ +#ifndef _ROS_nav_msgs_GetMapFeedback_h +#define _ROS_nav_msgs_GetMapFeedback_h + +#include +#include +#include +#include "ros/msg.h" + +namespace nav_msgs +{ + + class GetMapFeedback : public ros::Msg + { + public: + + GetMapFeedback() + { + } + + virtual int serialize(unsigned char *outbuffer) const + { + int offset = 0; + return offset; + } + + virtual int deserialize(unsigned char *inbuffer) + { + int offset = 0; + return offset; + } + + const char * getType(){ return "nav_msgs/GetMapFeedback"; }; + const char * getMD5(){ return "d41d8cd98f00b204e9800998ecf8427e"; }; + + }; + +} +#endif \ No newline at end of file diff --git a/turtlebot3_core/libraries/ros_lib/nav_msgs/GetMapGoal.h b/turtlebot3_core/libraries/ros_lib/nav_msgs/GetMapGoal.h new file mode 100644 index 00000000..88a17c5e --- /dev/null +++ b/turtlebot3_core/libraries/ros_lib/nav_msgs/GetMapGoal.h @@ -0,0 +1,38 @@ +#ifndef _ROS_nav_msgs_GetMapGoal_h +#define _ROS_nav_msgs_GetMapGoal_h + +#include +#include +#include +#include "ros/msg.h" + +namespace nav_msgs +{ + + class GetMapGoal : public ros::Msg + { + public: + + GetMapGoal() + { + } + + virtual int serialize(unsigned char *outbuffer) const + { + int offset = 0; + return offset; + } + + virtual int deserialize(unsigned char *inbuffer) + { + int offset = 0; + return offset; + } + + const char * getType(){ return "nav_msgs/GetMapGoal"; }; + const char * getMD5(){ return "d41d8cd98f00b204e9800998ecf8427e"; }; + + }; + +} +#endif \ No newline at end of file diff --git a/turtlebot3_core/libraries/ros_lib/nav_msgs/GetMapResult.h b/turtlebot3_core/libraries/ros_lib/nav_msgs/GetMapResult.h new file mode 100644 index 00000000..005b6398 --- /dev/null +++ b/turtlebot3_core/libraries/ros_lib/nav_msgs/GetMapResult.h @@ -0,0 +1,43 @@ +#ifndef _ROS_nav_msgs_GetMapResult_h +#define _ROS_nav_msgs_GetMapResult_h + +#include +#include +#include +#include "ros/msg.h" +#include "nav_msgs/OccupancyGrid.h" + +namespace nav_msgs +{ + + class GetMapResult : public ros::Msg + { + public: + nav_msgs::OccupancyGrid map; + + GetMapResult(): + map() + { + } + + virtual int serialize(unsigned char *outbuffer) const + { + int offset = 0; + offset += this->map.serialize(outbuffer + offset); + return offset; + } + + virtual int deserialize(unsigned char *inbuffer) + { + int offset = 0; + offset += this->map.deserialize(inbuffer + offset); + return offset; + } + + const char * getType(){ return "nav_msgs/GetMapResult"; }; + const char * getMD5(){ return "6cdd0a18e0aff5b0a3ca2326a89b54ff"; }; + + }; + +} +#endif \ No newline at end of file diff --git a/turtlebot3_core/libraries/ros_lib/nav_msgs/GetPlan.h b/turtlebot3_core/libraries/ros_lib/nav_msgs/GetPlan.h new file mode 100644 index 00000000..1ea3f325 --- /dev/null +++ b/turtlebot3_core/libraries/ros_lib/nav_msgs/GetPlan.h @@ -0,0 +1,107 @@ +#ifndef _ROS_SERVICE_GetPlan_h +#define _ROS_SERVICE_GetPlan_h +#include +#include +#include +#include "ros/msg.h" +#include "geometry_msgs/PoseStamped.h" +#include "nav_msgs/Path.h" + +namespace nav_msgs +{ + +static const char GETPLAN[] = "nav_msgs/GetPlan"; + + class GetPlanRequest : public ros::Msg + { + public: + geometry_msgs::PoseStamped start; + geometry_msgs::PoseStamped goal; + float tolerance; + + GetPlanRequest(): + start(), + goal(), + tolerance(0) + { + } + + virtual int serialize(unsigned char *outbuffer) const + { + int offset = 0; + offset += this->start.serialize(outbuffer + offset); + offset += this->goal.serialize(outbuffer + offset); + union { + float real; + uint32_t base; + } u_tolerance; + u_tolerance.real = this->tolerance; + *(outbuffer + offset + 0) = (u_tolerance.base >> (8 * 0)) & 0xFF; + *(outbuffer + offset + 1) = (u_tolerance.base >> (8 * 1)) & 0xFF; + *(outbuffer + offset + 2) = (u_tolerance.base >> (8 * 2)) & 0xFF; + *(outbuffer + offset + 3) = (u_tolerance.base >> (8 * 3)) & 0xFF; + offset += sizeof(this->tolerance); + return offset; + } + + virtual int deserialize(unsigned char *inbuffer) + { + int offset = 0; + offset += this->start.deserialize(inbuffer + offset); + offset += this->goal.deserialize(inbuffer + offset); + union { + float real; + uint32_t base; + } u_tolerance; + u_tolerance.base = 0; + u_tolerance.base |= ((uint32_t) (*(inbuffer + offset + 0))) << (8 * 0); + u_tolerance.base |= ((uint32_t) (*(inbuffer + offset + 1))) << (8 * 1); + u_tolerance.base |= ((uint32_t) (*(inbuffer + offset + 2))) << (8 * 2); + u_tolerance.base |= ((uint32_t) (*(inbuffer + offset + 3))) << (8 * 3); + this->tolerance = u_tolerance.real; + offset += sizeof(this->tolerance); + return offset; + } + + const char * getType(){ return GETPLAN; }; + const char * getMD5(){ return "e25a43e0752bcca599a8c2eef8282df8"; }; + + }; + + class GetPlanResponse : public ros::Msg + { + public: + nav_msgs::Path plan; + + GetPlanResponse(): + plan() + { + } + + virtual int serialize(unsigned char *outbuffer) const + { + int offset = 0; + offset += this->plan.serialize(outbuffer + offset); + return offset; + } + + virtual int deserialize(unsigned char *inbuffer) + { + int offset = 0; + offset += this->plan.deserialize(inbuffer + offset); + return offset; + } + + const char * getType(){ return GETPLAN; }; + const char * getMD5(){ return "0002bc113c0259d71f6cf8cbc9430e18"; }; + + }; + + class GetPlan { + public: + typedef GetPlanRequest Request; + typedef GetPlanResponse Response; + }; + +} +#endif diff --git a/turtlebot3_core/libraries/ros_lib/nav_msgs/GridCells.h b/turtlebot3_core/libraries/ros_lib/nav_msgs/GridCells.h new file mode 100644 index 00000000..3785242b --- /dev/null +++ b/turtlebot3_core/libraries/ros_lib/nav_msgs/GridCells.h @@ -0,0 +1,114 @@ +#ifndef _ROS_nav_msgs_GridCells_h +#define _ROS_nav_msgs_GridCells_h + +#include +#include +#include +#include "ros/msg.h" +#include "std_msgs/Header.h" +#include "geometry_msgs/Point.h" + +namespace nav_msgs +{ + + class GridCells : public ros::Msg + { + public: + std_msgs::Header header; + float cell_width; + float cell_height; + uint32_t cells_length; + geometry_msgs::Point st_cells; + geometry_msgs::Point * cells; + + GridCells(): + header(), + cell_width(0), + cell_height(0), + cells_length(0), cells(NULL) + { + } + + virtual int serialize(unsigned char *outbuffer) const + { + int offset = 0; + offset += this->header.serialize(outbuffer + offset); + union { + float real; + uint32_t base; + } u_cell_width; + u_cell_width.real = this->cell_width; + *(outbuffer + offset + 0) = (u_cell_width.base >> (8 * 0)) & 0xFF; + *(outbuffer + offset + 1) = (u_cell_width.base >> (8 * 1)) & 0xFF; + *(outbuffer + offset + 2) = (u_cell_width.base >> (8 * 2)) & 0xFF; + *(outbuffer + offset + 3) = (u_cell_width.base >> (8 * 3)) & 0xFF; + offset += sizeof(this->cell_width); + union { + float real; + uint32_t base; + } u_cell_height; + u_cell_height.real = this->cell_height; + *(outbuffer + offset + 0) = (u_cell_height.base >> (8 * 0)) & 0xFF; + *(outbuffer + offset + 1) = (u_cell_height.base >> (8 * 1)) & 0xFF; + *(outbuffer + offset + 2) = (u_cell_height.base >> (8 * 2)) & 0xFF; + *(outbuffer + offset + 3) = (u_cell_height.base >> (8 * 3)) & 0xFF; + offset += sizeof(this->cell_height); + *(outbuffer + offset + 0) = (this->cells_length >> (8 * 0)) & 0xFF; + *(outbuffer + offset + 1) = (this->cells_length >> (8 * 1)) & 0xFF; + *(outbuffer + offset + 2) = (this->cells_length >> (8 * 2)) & 0xFF; + *(outbuffer + offset + 3) = (this->cells_length >> (8 * 3)) & 0xFF; + offset += sizeof(this->cells_length); + for( uint32_t i = 0; i < cells_length; i++){ + offset += this->cells[i].serialize(outbuffer + offset); + } + return offset; + } + + virtual int deserialize(unsigned char *inbuffer) + { + int offset = 0; + offset += this->header.deserialize(inbuffer + offset); + union { + float real; + uint32_t base; + } u_cell_width; + u_cell_width.base = 0; + u_cell_width.base |= ((uint32_t) (*(inbuffer + offset + 0))) << (8 * 0); + u_cell_width.base |= ((uint32_t) (*(inbuffer + offset + 1))) << (8 * 1); + u_cell_width.base |= ((uint32_t) (*(inbuffer + offset + 2))) << (8 * 2); + u_cell_width.base |= ((uint32_t) (*(inbuffer + offset + 3))) << (8 * 3); + this->cell_width = u_cell_width.real; + offset += sizeof(this->cell_width); + union { + float real; + uint32_t base; + } u_cell_height; + u_cell_height.base = 0; + u_cell_height.base |= ((uint32_t) (*(inbuffer + offset + 0))) << (8 * 0); + u_cell_height.base |= ((uint32_t) (*(inbuffer + offset + 1))) << (8 * 1); + u_cell_height.base |= ((uint32_t) (*(inbuffer + offset + 2))) << (8 * 2); + u_cell_height.base |= ((uint32_t) (*(inbuffer + offset + 3))) << (8 * 3); + this->cell_height = u_cell_height.real; + offset += sizeof(this->cell_height); + uint32_t cells_lengthT = ((uint32_t) (*(inbuffer + offset))); + cells_lengthT |= ((uint32_t) (*(inbuffer + offset + 1))) << (8 * 1); + cells_lengthT |= ((uint32_t) (*(inbuffer + offset + 2))) << (8 * 2); + cells_lengthT |= ((uint32_t) (*(inbuffer + offset + 3))) << (8 * 3); + offset += sizeof(this->cells_length); + if(cells_lengthT > cells_length) + this->cells = (geometry_msgs::Point*)realloc(this->cells, cells_lengthT * sizeof(geometry_msgs::Point)); + cells_length = cells_lengthT; + for( uint32_t i = 0; i < cells_length; i++){ + offset += this->st_cells.deserialize(inbuffer + offset); + memcpy( &(this->cells[i]), &(this->st_cells), sizeof(geometry_msgs::Point)); + } + return offset; + } + + const char * getType(){ return "nav_msgs/GridCells"; }; + const char * getMD5(){ return "b9e4f5df6d28e272ebde00a3994830f5"; }; + + }; + +} +#endif \ No newline at end of file diff --git a/turtlebot3_core/libraries/ros_lib/nav_msgs/MapMetaData.h b/turtlebot3_core/libraries/ros_lib/nav_msgs/MapMetaData.h new file mode 100644 index 00000000..e9e73d8e --- /dev/null +++ b/turtlebot3_core/libraries/ros_lib/nav_msgs/MapMetaData.h @@ -0,0 +1,113 @@ +#ifndef _ROS_nav_msgs_MapMetaData_h +#define _ROS_nav_msgs_MapMetaData_h + +#include +#include +#include +#include "ros/msg.h" +#include "ros/time.h" +#include "geometry_msgs/Pose.h" + +namespace nav_msgs +{ + + class MapMetaData : public ros::Msg + { + public: + ros::Time map_load_time; + float resolution; + uint32_t width; + uint32_t height; + geometry_msgs::Pose origin; + + MapMetaData(): + map_load_time(), + resolution(0), + width(0), + height(0), + origin() + { + } + + virtual int serialize(unsigned char *outbuffer) const + { + int offset = 0; + *(outbuffer + offset + 0) = (this->map_load_time.sec >> (8 * 0)) & 0xFF; + *(outbuffer + offset + 1) = (this->map_load_time.sec >> (8 * 1)) & 0xFF; + *(outbuffer + offset + 2) = (this->map_load_time.sec >> (8 * 2)) & 0xFF; + *(outbuffer + offset + 3) = (this->map_load_time.sec >> (8 * 3)) & 0xFF; + offset += sizeof(this->map_load_time.sec); + *(outbuffer + offset + 0) = (this->map_load_time.nsec >> (8 * 0)) & 0xFF; + *(outbuffer + offset + 1) = (this->map_load_time.nsec >> (8 * 1)) & 0xFF; + *(outbuffer + offset + 2) = (this->map_load_time.nsec >> (8 * 2)) & 0xFF; + *(outbuffer + offset + 3) = (this->map_load_time.nsec >> (8 * 3)) & 0xFF; + offset += sizeof(this->map_load_time.nsec); + union { + float real; + uint32_t base; + } u_resolution; + u_resolution.real = this->resolution; + *(outbuffer + offset + 0) = (u_resolution.base >> (8 * 0)) & 0xFF; + *(outbuffer + offset + 1) = (u_resolution.base >> (8 * 1)) & 0xFF; + *(outbuffer + offset + 2) = (u_resolution.base >> (8 * 2)) & 0xFF; + *(outbuffer + offset + 3) = (u_resolution.base >> (8 * 3)) & 0xFF; + offset += sizeof(this->resolution); + *(outbuffer + offset + 0) = (this->width >> (8 * 0)) & 0xFF; + *(outbuffer + offset + 1) = (this->width >> (8 * 1)) & 0xFF; + *(outbuffer + offset + 2) = (this->width >> (8 * 2)) & 0xFF; + *(outbuffer + offset + 3) = (this->width >> (8 * 3)) & 0xFF; + offset += sizeof(this->width); + *(outbuffer + offset + 0) = (this->height >> (8 * 0)) & 0xFF; + *(outbuffer + offset + 1) = (this->height >> (8 * 1)) & 0xFF; + *(outbuffer + offset + 2) = (this->height >> (8 * 2)) & 0xFF; + *(outbuffer + offset + 3) = (this->height >> (8 * 3)) & 0xFF; + offset += sizeof(this->height); + offset += this->origin.serialize(outbuffer + offset); + return offset; + } + + virtual int deserialize(unsigned char *inbuffer) + { + int offset = 0; + this->map_load_time.sec = ((uint32_t) (*(inbuffer + offset))); + this->map_load_time.sec |= ((uint32_t) (*(inbuffer + offset + 1))) << (8 * 1); + this->map_load_time.sec |= ((uint32_t) (*(inbuffer + offset + 2))) << (8 * 2); + this->map_load_time.sec |= ((uint32_t) (*(inbuffer + offset + 3))) << (8 * 3); + offset += sizeof(this->map_load_time.sec); + this->map_load_time.nsec = ((uint32_t) (*(inbuffer + offset))); + this->map_load_time.nsec |= ((uint32_t) (*(inbuffer + offset + 1))) << (8 * 1); + this->map_load_time.nsec |= ((uint32_t) (*(inbuffer + offset + 2))) << (8 * 2); + this->map_load_time.nsec |= ((uint32_t) (*(inbuffer + offset + 3))) << (8 * 3); + offset += sizeof(this->map_load_time.nsec); + union { + float real; + uint32_t base; + } u_resolution; + u_resolution.base = 0; + u_resolution.base |= ((uint32_t) (*(inbuffer + offset + 0))) << (8 * 0); + u_resolution.base |= ((uint32_t) (*(inbuffer + offset + 1))) << (8 * 1); + u_resolution.base |= ((uint32_t) (*(inbuffer + offset + 2))) << (8 * 2); + u_resolution.base |= ((uint32_t) (*(inbuffer + offset + 3))) << (8 * 3); + this->resolution = u_resolution.real; + offset += sizeof(this->resolution); + this->width = ((uint32_t) (*(inbuffer + offset))); + this->width |= ((uint32_t) (*(inbuffer + offset + 1))) << (8 * 1); + this->width |= ((uint32_t) (*(inbuffer + offset + 2))) << (8 * 2); + this->width |= ((uint32_t) (*(inbuffer + offset + 3))) << (8 * 3); + offset += sizeof(this->width); + this->height = ((uint32_t) (*(inbuffer + offset))); + this->height |= ((uint32_t) (*(inbuffer + offset + 1))) << (8 * 1); + this->height |= ((uint32_t) (*(inbuffer + offset + 2))) << (8 * 2); + this->height |= ((uint32_t) (*(inbuffer + offset + 3))) << (8 * 3); + offset += sizeof(this->height); + offset += this->origin.deserialize(inbuffer + offset); + return offset; + } + + const char * getType(){ return "nav_msgs/MapMetaData"; }; + const char * getMD5(){ return "10cfc8a2818024d3248802c00c95f11b"; }; + + }; + +} +#endif \ No newline at end of file diff --git a/turtlebot3_core/libraries/ros_lib/nav_msgs/OccupancyGrid.h b/turtlebot3_core/libraries/ros_lib/nav_msgs/OccupancyGrid.h new file mode 100644 index 00000000..1ea58b9e --- /dev/null +++ b/turtlebot3_core/libraries/ros_lib/nav_msgs/OccupancyGrid.h @@ -0,0 +1,85 @@ +#ifndef _ROS_nav_msgs_OccupancyGrid_h +#define _ROS_nav_msgs_OccupancyGrid_h + +#include +#include +#include +#include "ros/msg.h" +#include "std_msgs/Header.h" +#include "nav_msgs/MapMetaData.h" + +namespace nav_msgs +{ + + class OccupancyGrid : public ros::Msg + { + public: + std_msgs::Header header; + nav_msgs::MapMetaData info; + uint32_t data_length; + int8_t st_data; + int8_t * data; + + OccupancyGrid(): + header(), + info(), + data_length(0), data(NULL) + { + } + + virtual int serialize(unsigned char *outbuffer) const + { + int offset = 0; + offset += this->header.serialize(outbuffer + offset); + offset += this->info.serialize(outbuffer + offset); + *(outbuffer + offset + 0) = (this->data_length >> (8 * 0)) & 0xFF; + *(outbuffer + offset + 1) = (this->data_length >> (8 * 1)) & 0xFF; + *(outbuffer + offset + 2) = (this->data_length >> (8 * 2)) & 0xFF; + *(outbuffer + offset + 3) = (this->data_length >> (8 * 3)) & 0xFF; + offset += sizeof(this->data_length); + for( uint32_t i = 0; i < data_length; i++){ + union { + int8_t real; + uint8_t base; + } u_datai; + u_datai.real = this->data[i]; + *(outbuffer + offset + 0) = (u_datai.base >> (8 * 0)) & 0xFF; + offset += sizeof(this->data[i]); + } + return offset; + } + + virtual int deserialize(unsigned char *inbuffer) + { + int offset = 0; + offset += this->header.deserialize(inbuffer + offset); + offset += this->info.deserialize(inbuffer + offset); + uint32_t data_lengthT = ((uint32_t) (*(inbuffer + offset))); + data_lengthT |= ((uint32_t) (*(inbuffer + offset + 1))) << (8 * 1); + data_lengthT |= ((uint32_t) (*(inbuffer + offset + 2))) << (8 * 2); + data_lengthT |= ((uint32_t) (*(inbuffer + offset + 3))) << (8 * 3); + offset += sizeof(this->data_length); + if(data_lengthT > data_length) + this->data = (int8_t*)realloc(this->data, data_lengthT * sizeof(int8_t)); + data_length = data_lengthT; + for( uint32_t i = 0; i < data_length; i++){ + union { + int8_t real; + uint8_t base; + } u_st_data; + u_st_data.base = 0; + u_st_data.base |= ((uint8_t) (*(inbuffer + offset + 0))) << (8 * 0); + this->st_data = u_st_data.real; + offset += sizeof(this->st_data); + memcpy( &(this->data[i]), &(this->st_data), sizeof(int8_t)); + } + return offset; + } + + const char * getType(){ return "nav_msgs/OccupancyGrid"; }; + const char * getMD5(){ return "3381f2d731d4076ec5c71b0759edbe4e"; }; + + }; + +} +#endif \ No newline at end of file diff --git a/turtlebot3_core/libraries/ros_lib/nav_msgs/Odometry.h b/turtlebot3_core/libraries/ros_lib/nav_msgs/Odometry.h new file mode 100644 index 00000000..7966ea58 --- /dev/null +++ b/turtlebot3_core/libraries/ros_lib/nav_msgs/Odometry.h @@ -0,0 +1,69 @@ +#ifndef _ROS_nav_msgs_Odometry_h +#define _ROS_nav_msgs_Odometry_h + +#include +#include +#include +#include "ros/msg.h" +#include "std_msgs/Header.h" +#include "geometry_msgs/PoseWithCovariance.h" +#include "geometry_msgs/TwistWithCovariance.h" + +namespace nav_msgs +{ + + class Odometry : public ros::Msg + { + public: + std_msgs::Header header; + const char* child_frame_id; + geometry_msgs::PoseWithCovariance pose; + geometry_msgs::TwistWithCovariance twist; + + Odometry(): + header(), + child_frame_id(""), + pose(), + twist() + { + } + + virtual int serialize(unsigned char *outbuffer) const + { + int offset = 0; + offset += this->header.serialize(outbuffer + offset); + uint32_t length_child_frame_id = strlen(this->child_frame_id); + memcpy(outbuffer + offset, &length_child_frame_id, sizeof(uint32_t)); + offset += 4; + memcpy(outbuffer + offset, this->child_frame_id, length_child_frame_id); + offset += length_child_frame_id; + offset += this->pose.serialize(outbuffer + offset); + offset += this->twist.serialize(outbuffer + offset); + return offset; + } + + virtual int deserialize(unsigned char *inbuffer) + { + int offset = 0; + offset += this->header.deserialize(inbuffer + offset); + uint32_t length_child_frame_id; + memcpy(&length_child_frame_id, (inbuffer + offset), sizeof(uint32_t)); + offset += 4; + for(unsigned int k= offset; k< offset+length_child_frame_id; ++k){ + inbuffer[k-1]=inbuffer[k]; + } + inbuffer[offset+length_child_frame_id-1]=0; + this->child_frame_id = (char *)(inbuffer + offset-1); + offset += length_child_frame_id; + offset += this->pose.deserialize(inbuffer + offset); + offset += this->twist.deserialize(inbuffer + offset); + return offset; + } + + const char * getType(){ return "nav_msgs/Odometry"; }; + const char * getMD5(){ return "cd5e73d190d741a2f92e81eda573aca7"; }; + + }; + +} +#endif \ No newline at end of file diff --git a/turtlebot3_core/libraries/ros_lib/nav_msgs/Path.h b/turtlebot3_core/libraries/ros_lib/nav_msgs/Path.h new file mode 100644 index 00000000..6c22664a --- /dev/null +++ b/turtlebot3_core/libraries/ros_lib/nav_msgs/Path.h @@ -0,0 +1,68 @@ +#ifndef _ROS_nav_msgs_Path_h +#define _ROS_nav_msgs_Path_h + +#include +#include +#include +#include "ros/msg.h" +#include "std_msgs/Header.h" +#include "geometry_msgs/PoseStamped.h" + +namespace nav_msgs +{ + + class Path : public ros::Msg + { + public: + std_msgs::Header header; + uint32_t poses_length; + geometry_msgs::PoseStamped st_poses; + geometry_msgs::PoseStamped * poses; + + Path(): + header(), + poses_length(0), poses(NULL) + { + } + + virtual int serialize(unsigned char *outbuffer) const + { + int offset = 0; + offset += this->header.serialize(outbuffer + offset); + *(outbuffer + offset + 0) = (this->poses_length >> (8 * 0)) & 0xFF; + *(outbuffer + offset + 1) = (this->poses_length >> (8 * 1)) & 0xFF; + *(outbuffer + offset + 2) = (this->poses_length >> (8 * 2)) & 0xFF; + *(outbuffer + offset + 3) = (this->poses_length >> (8 * 3)) & 0xFF; + offset += sizeof(this->poses_length); + for( uint32_t i = 0; i < poses_length; i++){ + offset += this->poses[i].serialize(outbuffer + offset); + } + return offset; + } + + virtual int deserialize(unsigned char *inbuffer) + { + int offset = 0; + offset += this->header.deserialize(inbuffer + offset); + uint32_t poses_lengthT = ((uint32_t) (*(inbuffer + offset))); + poses_lengthT |= ((uint32_t) (*(inbuffer + offset + 1))) << (8 * 1); + poses_lengthT |= ((uint32_t) (*(inbuffer + offset + 2))) << (8 * 2); + poses_lengthT |= ((uint32_t) (*(inbuffer + offset + 3))) << (8 * 3); + offset += sizeof(this->poses_length); + if(poses_lengthT > poses_length) + this->poses = (geometry_msgs::PoseStamped*)realloc(this->poses, poses_lengthT * sizeof(geometry_msgs::PoseStamped)); + poses_length = poses_lengthT; + for( uint32_t i = 0; i < poses_length; i++){ + offset += this->st_poses.deserialize(inbuffer + offset); + memcpy( &(this->poses[i]), &(this->st_poses), sizeof(geometry_msgs::PoseStamped)); + } + return offset; + } + + const char * getType(){ return "nav_msgs/Path"; }; + const char * getMD5(){ return "6227e2b7e9cce15051f669a5e197bbf7"; }; + + }; + +} +#endif \ No newline at end of file diff --git a/turtlebot3_core/libraries/ros_lib/nav_msgs/SetMap.h b/turtlebot3_core/libraries/ros_lib/nav_msgs/SetMap.h new file mode 100644 index 00000000..436f95de --- /dev/null +++ b/turtlebot3_core/libraries/ros_lib/nav_msgs/SetMap.h @@ -0,0 +1,97 @@ +#ifndef _ROS_SERVICE_SetMap_h +#define _ROS_SERVICE_SetMap_h +#include +#include +#include +#include "ros/msg.h" +#include "nav_msgs/OccupancyGrid.h" +#include "geometry_msgs/PoseWithCovarianceStamped.h" + +namespace nav_msgs +{ + +static const char SETMAP[] = "nav_msgs/SetMap"; + + class SetMapRequest : public ros::Msg + { + public: + nav_msgs::OccupancyGrid map; + geometry_msgs::PoseWithCovarianceStamped initial_pose; + + SetMapRequest(): + map(), + initial_pose() + { + } + + virtual int serialize(unsigned char *outbuffer) const + { + int offset = 0; + offset += this->map.serialize(outbuffer + offset); + offset += this->initial_pose.serialize(outbuffer + offset); + return offset; + } + + virtual int deserialize(unsigned char *inbuffer) + { + int offset = 0; + offset += this->map.deserialize(inbuffer + offset); + offset += this->initial_pose.deserialize(inbuffer + offset); + return offset; + } + + const char * getType(){ return SETMAP; }; + const char * getMD5(){ return "91149a20d7be299b87c340df8cc94fd4"; }; + + }; + + class SetMapResponse : public ros::Msg + { + public: + bool success; + + SetMapResponse(): + success(0) + { + } + + virtual int serialize(unsigned char *outbuffer) const + { + int offset = 0; + union { + bool real; + uint8_t base; + } u_success; + u_success.real = this->success; + *(outbuffer + offset + 0) = (u_success.base >> (8 * 0)) & 0xFF; + offset += sizeof(this->success); + return offset; + } + + virtual int deserialize(unsigned char *inbuffer) + { + int offset = 0; + union { + bool real; + uint8_t base; + } u_success; + u_success.base = 0; + u_success.base |= ((uint8_t) (*(inbuffer + offset + 0))) << (8 * 0); + this->success = u_success.real; + offset += sizeof(this->success); + return offset; + } + + const char * getType(){ return SETMAP; }; + const char * getMD5(){ return "358e233cde0c8a8bcfea4ce193f8fc15"; }; + + }; + + class SetMap { + public: + typedef SetMapRequest Request; + typedef SetMapResponse Response; + }; + +} +#endif diff --git a/turtlebot3_core/libraries/ros_lib/ros.h b/turtlebot3_core/libraries/ros_lib/ros.h new file mode 100644 index 00000000..7449f399 --- /dev/null +++ b/turtlebot3_core/libraries/ros_lib/ros.h @@ -0,0 +1,58 @@ +/* + * Software License Agreement (BSD License) + * + * Copyright (c) 2011, Willow Garage, Inc. + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above + * copyright notice, this list of conditions and the following + * disclaimer in the documentation and/or other materials provided + * with the distribution. + * * Neither the name of Willow Garage, Inc. nor the names of its + * contributors may be used to endorse or promote prducts derived + * from this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; + * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + */ + +#ifndef _ROS_H_ +#define _ROS_H_ + +#include "ros/node_handle.h" +#include "ArduinoHardware.h" + +namespace ros +{ +#if defined(__AVR_ATmega8__) || defined(__AVR_ATmega168__) + /* downsize our buffers */ + typedef NodeHandle_ NodeHandle; + +#elif defined(__AVR_ATmega328P__) + + typedef NodeHandle_ NodeHandle; + +#else + + typedef NodeHandle_ NodeHandle; + +#endif +} + +#endif diff --git a/turtlebot3_core/libraries/ros_lib/ros/duration.h b/turtlebot3_core/libraries/ros_lib/ros/duration.h new file mode 100644 index 00000000..ab889cdd --- /dev/null +++ b/turtlebot3_core/libraries/ros_lib/ros/duration.h @@ -0,0 +1,67 @@ +/* + * Software License Agreement (BSD License) + * + * Copyright (c) 2011, Willow Garage, Inc. + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above + * copyright notice, this list of conditions and the following + * disclaimer in the documentation and/or other materials provided + * with the distribution. + * * Neither the name of Willow Garage, Inc. nor the names of its + * contributors may be used to endorse or promote prducts derived + * from this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; + * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + */ + +#ifndef _ROS_DURATION_H_ +#define _ROS_DURATION_H_ + +#include +#include + +namespace ros { + + void normalizeSecNSecSigned(int32_t& sec, int32_t& nsec); + + class Duration + { + public: + int32_t sec, nsec; + + Duration() : sec(0), nsec(0) {} + Duration(int32_t _sec, int32_t _nsec) : sec(_sec), nsec(_nsec) + { + normalizeSecNSecSigned(sec, nsec); + } + + double toSec() const { return (double)sec + 1e-9*(double)nsec; }; + void fromSec(double t) { sec = (uint32_t) floor(t); nsec = (uint32_t) round((t-sec) * 1e9); }; + + Duration& operator+=(const Duration &rhs); + Duration& operator-=(const Duration &rhs); + Duration& operator*=(double scale); + }; + +} + +#endif + diff --git a/turtlebot3_core/libraries/ros_lib/ros/msg.h b/turtlebot3_core/libraries/ros_lib/ros/msg.h new file mode 100644 index 00000000..87f2e5c9 --- /dev/null +++ b/turtlebot3_core/libraries/ros_lib/ros/msg.h @@ -0,0 +1,129 @@ +/* + * Software License Agreement (BSD License) + * + * Copyright (c) 2011, Willow Garage, Inc. + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above + * copyright notice, this list of conditions and the following + * disclaimer in the documentation and/or other materials provided + * with the distribution. + * * Neither the name of Willow Garage, Inc. nor the names of its + * contributors may be used to endorse or promote prducts derived + * from this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; + * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + */ + +#ifndef _ROS_MSG_H_ +#define _ROS_MSG_H_ + +#include + +namespace ros { + +/* Base Message Type */ +class Msg +{ +public: + virtual int serialize(unsigned char *outbuffer) const = 0; + virtual int deserialize(unsigned char *data) = 0; + virtual const char * getType() = 0; + virtual const char * getMD5() = 0; + + /** + * @brief This tricky function handles promoting a 32bit float to a 64bit + * double, so that AVR can publish messages containing float64 + * fields, despite AVV having no native support for double. + * + * @param[out] outbuffer pointer for buffer to serialize to. + * @param[in] f value to serialize. + * + * @return number of bytes to advance the buffer pointer. + * + */ + static int serializeAvrFloat64(unsigned char* outbuffer, const float f) + { + const int32_t* val = (int32_t*) &f; + int32_t exp = ((*val >> 23) & 255); + if (exp != 0) + { + exp += 1023 - 127; + } + + int32_t sig = *val; + *(outbuffer++) = 0; + *(outbuffer++) = 0; + *(outbuffer++) = 0; + *(outbuffer++) = (sig << 5) & 0xff; + *(outbuffer++) = (sig >> 3) & 0xff; + *(outbuffer++) = (sig >> 11) & 0xff; + *(outbuffer++) = ((exp << 4) & 0xF0) | ((sig >> 19) & 0x0F); + *(outbuffer++) = (exp >> 4) & 0x7F; + + // Mark negative bit as necessary. + if (f < 0) + { + *(outbuffer - 1) |= 0x80; + } + + return 8; + } + + /** + * @brief This tricky function handles demoting a 64bit double to a + * 32bit float, so that AVR can understand messages containing + * float64 fields, despite AVR having no native support for double. + * + * @param[in] inbuffer pointer for buffer to deserialize from. + * @param[out] f pointer to place the deserialized value in. + * + * @return number of bytes to advance the buffer pointer. + */ + static int deserializeAvrFloat64(const unsigned char* inbuffer, float* f) + { + uint32_t* val = (uint32_t*)f; + inbuffer += 3; + + // Copy truncated mantissa. + *val = ((uint32_t)(*(inbuffer++)) >> 5 & 0x07); + *val |= ((uint32_t)(*(inbuffer++)) & 0xff) << 3; + *val |= ((uint32_t)(*(inbuffer++)) & 0xff) << 11; + *val |= ((uint32_t)(*inbuffer) & 0x0f) << 19; + + // Copy truncated exponent. + uint32_t exp = ((uint32_t)(*(inbuffer++)) & 0xf0)>>4; + exp |= ((uint32_t)(*inbuffer) & 0x7f) << 4; + if (exp != 0) + { + *val |= ((exp) - 1023 + 127) << 23; + } + + // Copy negative sign. + *val |= ((uint32_t)(*(inbuffer++)) & 0x80) << 24; + + return 8; + } + +}; + +} // namespace ros + +#endif diff --git a/turtlebot3_core/libraries/ros_lib/ros/node_handle.h b/turtlebot3_core/libraries/ros_lib/ros/node_handle.h new file mode 100644 index 00000000..81974319 --- /dev/null +++ b/turtlebot3_core/libraries/ros_lib/ros/node_handle.h @@ -0,0 +1,543 @@ +/* + * Software License Agreement (BSD License) + * + * Copyright (c) 2011, Willow Garage, Inc. + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above + * copyright notice, this list of conditions and the following + * disclaimer in the documentation and/or other materials provided + * with the distribution. + * * Neither the name of Willow Garage, Inc. nor the names of its + * contributors may be used to endorse or promote prducts derived + * from this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; + * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + */ + +#ifndef ROS_NODE_HANDLE_H_ +#define ROS_NODE_HANDLE_H_ + +#include + +#include "std_msgs/Time.h" +#include "rosserial_msgs/TopicInfo.h" +#include "rosserial_msgs/Log.h" +#include "rosserial_msgs/RequestParam.h" + +#define SYNC_SECONDS 5 + +#define MODE_FIRST_FF 0 +/* + * The second sync byte is a protocol version. It's value is 0xff for the first + * version of the rosserial protocol (used up to hydro), 0xfe for the second version + * (introduced in hydro), 0xfd for the next, and so on. Its purpose is to enable + * detection of mismatched protocol versions (e.g. hydro rosserial_python with groovy + * rosserial_arduino. It must be changed in both this file and in + * rosserial_python/src/rosserial_python/SerialClient.py + */ +#define MODE_PROTOCOL_VER 1 +#define PROTOCOL_VER1 0xff // through groovy +#define PROTOCOL_VER2 0xfe // in hydro +#define PROTOCOL_VER PROTOCOL_VER2 +#define MODE_SIZE_L 2 +#define MODE_SIZE_H 3 +#define MODE_SIZE_CHECKSUM 4 // checksum for msg size received from size L and H +#define MODE_TOPIC_L 5 // waiting for topic id +#define MODE_TOPIC_H 6 +#define MODE_MESSAGE 7 +#define MODE_MSG_CHECKSUM 8 // checksum for msg and topic id + + +#define MSG_TIMEOUT 20 //20 milliseconds to recieve all of message data + +#include "ros/msg.h" + +namespace ros { + + class NodeHandleBase_{ + public: + virtual int publish(int id, const Msg* msg)=0; + virtual int spinOnce()=0; + virtual bool connected()=0; + }; +} + +#include "ros/publisher.h" +#include "ros/subscriber.h" +#include "ros/service_server.h" +#include "ros/service_client.h" + +namespace ros { + + using rosserial_msgs::TopicInfo; + + /* Node Handle */ + template + class NodeHandle_ : public NodeHandleBase_ + { + protected: + Hardware hardware_; + + /* time used for syncing */ + uint32_t rt_time; + + /* used for computing current time */ + uint32_t sec_offset, nsec_offset; + + uint8_t message_in[INPUT_SIZE]; + uint8_t message_out[OUTPUT_SIZE]; + + Publisher * publishers[MAX_PUBLISHERS]; + Subscriber_ * subscribers[MAX_SUBSCRIBERS]; + + /* + * Setup Functions + */ + public: + NodeHandle_() : configured_(false) { + + for(unsigned int i=0; i< MAX_PUBLISHERS; i++) + publishers[i] = 0; + + for(unsigned int i=0; i< MAX_SUBSCRIBERS; i++) + subscribers[i] = 0; + + for(unsigned int i=0; i< INPUT_SIZE; i++) + message_in[i] = 0; + + for(unsigned int i=0; i< OUTPUT_SIZE; i++) + message_out[i] = 0; + + req_param_resp.ints_length = 0; + req_param_resp.ints = NULL; + req_param_resp.floats_length = 0; + req_param_resp.floats = NULL; + req_param_resp.ints_length = 0; + req_param_resp.ints = NULL; + } + + Hardware* getHardware(){ + return &hardware_; + } + + /* Start serial, initialize buffers */ + void initNode(){ + hardware_.init(); + mode_ = 0; + bytes_ = 0; + index_ = 0; + topic_ = 0; + }; + + /* Start a named port, which may be network server IP, initialize buffers */ + void initNode(char *portName){ + hardware_.init(portName); + mode_ = 0; + bytes_ = 0; + index_ = 0; + topic_ = 0; + }; + + protected: + //State machine variables for spinOnce + int mode_; + int bytes_; + int topic_; + int index_; + int checksum_; + + bool configured_; + + /* used for syncing the time */ + uint32_t last_sync_time; + uint32_t last_sync_receive_time; + uint32_t last_msg_timeout_time; + + public: + /* This function goes in your loop() function, it handles + * serial input and callbacks for subscribers. + */ + + + virtual int spinOnce(){ + + /* restart if timed out */ + uint32_t c_time = hardware_.time(); + if( (c_time - last_sync_receive_time) > (SYNC_SECONDS*2200) ){ + configured_ = false; + } + + /* reset if message has timed out */ + if ( mode_ != MODE_FIRST_FF){ + if (c_time > last_msg_timeout_time){ + mode_ = MODE_FIRST_FF; + } + } + + /* while available buffer, read data */ + while( true ) + { + int data = hardware_.read(); + if( data < 0 ) + break; + checksum_ += data; + if( mode_ == MODE_MESSAGE ){ /* message data being recieved */ + message_in[index_++] = data; + bytes_--; + if(bytes_ == 0) /* is message complete? if so, checksum */ + mode_ = MODE_MSG_CHECKSUM; + }else if( mode_ == MODE_FIRST_FF ){ + if(data == 0xff){ + mode_++; + last_msg_timeout_time = c_time + MSG_TIMEOUT; + } + else if( hardware_.time() - c_time > (SYNC_SECONDS)){ + /* We have been stuck in spinOnce too long, return error */ + configured_=false; + return -2; + } + }else if( mode_ == MODE_PROTOCOL_VER ){ + if(data == PROTOCOL_VER){ + mode_++; + }else{ + mode_ = MODE_FIRST_FF; + if (configured_ == false) + requestSyncTime(); /* send a msg back showing our protocol version */ + } + }else if( mode_ == MODE_SIZE_L ){ /* bottom half of message size */ + bytes_ = data; + index_ = 0; + mode_++; + checksum_ = data; /* first byte for calculating size checksum */ + }else if( mode_ == MODE_SIZE_H ){ /* top half of message size */ + bytes_ += data<<8; + mode_++; + }else if( mode_ == MODE_SIZE_CHECKSUM ){ + if( (checksum_%256) == 255) + mode_++; + else + mode_ = MODE_FIRST_FF; /* Abandon the frame if the msg len is wrong */ + }else if( mode_ == MODE_TOPIC_L ){ /* bottom half of topic id */ + topic_ = data; + mode_++; + checksum_ = data; /* first byte included in checksum */ + }else if( mode_ == MODE_TOPIC_H ){ /* top half of topic id */ + topic_ += data<<8; + mode_ = MODE_MESSAGE; + if(bytes_ == 0) + mode_ = MODE_MSG_CHECKSUM; + }else if( mode_ == MODE_MSG_CHECKSUM ){ /* do checksum */ + mode_ = MODE_FIRST_FF; + if( (checksum_%256) == 255){ + if(topic_ == TopicInfo::ID_PUBLISHER){ + requestSyncTime(); + negotiateTopics(); + last_sync_time = c_time; + last_sync_receive_time = c_time; + return -1; + }else if(topic_ == TopicInfo::ID_TIME){ + syncTime(message_in); + }else if (topic_ == TopicInfo::ID_PARAMETER_REQUEST){ + req_param_resp.deserialize(message_in); + param_recieved= true; + }else if(topic_ == TopicInfo::ID_TX_STOP){ + configured_ = false; + }else{ + if(subscribers[topic_-100]) + subscribers[topic_-100]->callback( message_in ); + } + } + } + } + + /* occasionally sync time */ + if( configured_ && ((c_time-last_sync_time) > (SYNC_SECONDS*500) )){ + requestSyncTime(); + last_sync_time = c_time; + } + + return 0; + } + + + /* Are we connected to the PC? */ + virtual bool connected() { + return configured_; + }; + + /******************************************************************** + * Time functions + */ + + void requestSyncTime() + { + std_msgs::Time t; + publish(TopicInfo::ID_TIME, &t); + rt_time = hardware_.time(); + } + + void syncTime(uint8_t * data) + { + std_msgs::Time t; + uint32_t offset = hardware_.time() - rt_time; + + t.deserialize(data); + t.data.sec += offset/1000; + t.data.nsec += (offset%1000)*1000000UL; + + this->setNow(t.data); + last_sync_receive_time = hardware_.time(); + } + + Time now() + { + uint32_t ms = hardware_.time(); + Time current_time; + current_time.sec = ms/1000 + sec_offset; + current_time.nsec = (ms%1000)*1000000UL + nsec_offset; + normalizeSecNSec(current_time.sec, current_time.nsec); + return current_time; + } + + void setNow( Time & new_now ) + { + uint32_t ms = hardware_.time(); + sec_offset = new_now.sec - ms/1000 - 1; + nsec_offset = new_now.nsec - (ms%1000)*1000000UL + 1000000000UL; + normalizeSecNSec(sec_offset, nsec_offset); + } + + /******************************************************************** + * Topic Management + */ + + /* Register a new publisher */ + bool advertise(Publisher & p) + { + for(int i = 0; i < MAX_PUBLISHERS; i++){ + if(publishers[i] == 0){ // empty slot + publishers[i] = &p; + p.id_ = i+100+MAX_SUBSCRIBERS; + p.nh_ = this; + return true; + } + } + return false; + } + + /* Register a new subscriber */ + template + bool subscribe(SubscriberT& s){ + for(int i = 0; i < MAX_SUBSCRIBERS; i++){ + if(subscribers[i] == 0){ // empty slot + subscribers[i] = static_cast(&s); + s.id_ = i+100; + return true; + } + } + return false; + } + + /* Register a new Service Server */ + template + bool advertiseService(ServiceServer& srv){ + bool v = advertise(srv.pub); + for(int i = 0; i < MAX_SUBSCRIBERS; i++){ + if(subscribers[i] == 0){ // empty slot + subscribers[i] = static_cast(&srv); + srv.id_ = i+100; + return v; + } + } + return false; + } + + /* Register a new Service Client */ + template + bool serviceClient(ServiceClient& srv){ + bool v = advertise(srv.pub); + for(int i = 0; i < MAX_SUBSCRIBERS; i++){ + if(subscribers[i] == 0){ // empty slot + subscribers[i] = static_cast(&srv); + srv.id_ = i+100; + return v; + } + } + return false; + } + + void negotiateTopics() + { + rosserial_msgs::TopicInfo ti; + int i; + for(i = 0; i < MAX_PUBLISHERS; i++) + { + if(publishers[i] != 0) // non-empty slot + { + ti.topic_id = publishers[i]->id_; + ti.topic_name = (char *) publishers[i]->topic_; + ti.message_type = (char *) publishers[i]->msg_->getType(); + ti.md5sum = (char *) publishers[i]->msg_->getMD5(); + ti.buffer_size = OUTPUT_SIZE; + publish( publishers[i]->getEndpointType(), &ti ); + } + } + for(i = 0; i < MAX_SUBSCRIBERS; i++) + { + if(subscribers[i] != 0) // non-empty slot + { + ti.topic_id = subscribers[i]->id_; + ti.topic_name = (char *) subscribers[i]->topic_; + ti.message_type = (char *) subscribers[i]->getMsgType(); + ti.md5sum = (char *) subscribers[i]->getMsgMD5(); + ti.buffer_size = INPUT_SIZE; + publish( subscribers[i]->getEndpointType(), &ti ); + } + } + configured_ = true; + } + + virtual int publish(int id, const Msg * msg) + { + if(id >= 100 && !configured_) + return 0; + + /* serialize message */ + uint16_t l = msg->serialize(message_out+7); + + /* setup the header */ + message_out[0] = 0xff; + message_out[1] = PROTOCOL_VER; + message_out[2] = (uint8_t) ((uint16_t)l&255); + message_out[3] = (uint8_t) ((uint16_t)l>>8); + message_out[4] = 255 - ((message_out[2] + message_out[3])%256); + message_out[5] = (uint8_t) ((int16_t)id&255); + message_out[6] = (uint8_t) ((int16_t)id>>8); + + /* calculate checksum */ + int chk = 0; + for(int i =5; i end_time) return false; + } + return true; + } + + public: + bool getParam(const char* name, int* param, int length =1){ + if (requestParam(name) ){ + if (length == req_param_resp.ints_length){ + //copy it over + for(int i=0; ipublish(id_, msg); }; + int getEndpointType(){ return endpoint_; } + + const char * topic_; + Msg *msg_; + // id_ and no_ are set by NodeHandle when we advertise + int id_; + NodeHandleBase_* nh_; + + private: + int endpoint_; + }; + +} + +#endif diff --git a/turtlebot3_core/libraries/ros_lib/ros/service_client.h b/turtlebot3_core/libraries/ros_lib/ros/service_client.h new file mode 100644 index 00000000..2031fb17 --- /dev/null +++ b/turtlebot3_core/libraries/ros_lib/ros/service_client.h @@ -0,0 +1,83 @@ +/* + * Software License Agreement (BSD License) + * + * Copyright (c) 2011, Willow Garage, Inc. + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above + * copyright notice, this list of conditions and the following + * disclaimer in the documentation and/or other materials provided + * with the distribution. + * * Neither the name of Willow Garage, Inc. nor the names of its + * contributors may be used to endorse or promote prducts derived + * from this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; + * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + */ + +#ifndef _ROS_SERVICE_CLIENT_H_ +#define _ROS_SERVICE_CLIENT_H_ + +#include "rosserial_msgs/TopicInfo.h" + +#include "ros/publisher.h" +#include "ros/subscriber.h" + +namespace ros { + + template + class ServiceClient : public Subscriber_ { + public: + ServiceClient(const char* topic_name) : + pub(topic_name, &req, rosserial_msgs::TopicInfo::ID_SERVICE_CLIENT + rosserial_msgs::TopicInfo::ID_PUBLISHER) + { + this->topic_ = topic_name; + this->waiting = true; + } + + virtual void call(const MReq & request, MRes & response) + { + if(!pub.nh_->connected()) return; + ret = &response; + waiting = true; + pub.publish(&request); + while(waiting && pub.nh_->connected()) + if(pub.nh_->spinOnce() < 0) break; + } + + // these refer to the subscriber + virtual void callback(unsigned char *data){ + ret->deserialize(data); + waiting = false; + } + virtual const char * getMsgType(){ return this->resp.getType(); } + virtual const char * getMsgMD5(){ return this->resp.getMD5(); } + virtual int getEndpointType(){ return rosserial_msgs::TopicInfo::ID_SERVICE_CLIENT + rosserial_msgs::TopicInfo::ID_SUBSCRIBER; } + + MReq req; + MRes resp; + MRes * ret; + bool waiting; + Publisher pub; + }; + +} + +#endif diff --git a/turtlebot3_core/libraries/ros_lib/ros/service_server.h b/turtlebot3_core/libraries/ros_lib/ros/service_server.h new file mode 100644 index 00000000..8d087388 --- /dev/null +++ b/turtlebot3_core/libraries/ros_lib/ros/service_server.h @@ -0,0 +1,76 @@ +/* + * Software License Agreement (BSD License) + * + * Copyright (c) 2011, Willow Garage, Inc. + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above + * copyright notice, this list of conditions and the following + * disclaimer in the documentation and/or other materials provided + * with the distribution. + * * Neither the name of Willow Garage, Inc. nor the names of its + * contributors may be used to endorse or promote prducts derived + * from this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; + * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + */ + +#ifndef _ROS_SERVICE_SERVER_H_ +#define _ROS_SERVICE_SERVER_H_ + +#include "rosserial_msgs/TopicInfo.h" + +#include "ros/publisher.h" +#include "ros/subscriber.h" + +namespace ros { + + template + class ServiceServer : public Subscriber_ { + public: + typedef void(*CallbackT)(const MReq&, MRes&); + + ServiceServer(const char* topic_name, CallbackT cb) : + pub(topic_name, &resp, rosserial_msgs::TopicInfo::ID_SERVICE_SERVER + rosserial_msgs::TopicInfo::ID_PUBLISHER) + { + this->topic_ = topic_name; + this->cb_ = cb; + } + + // these refer to the subscriber + virtual void callback(unsigned char *data){ + req.deserialize(data); + cb_(req,resp); + pub.publish(&resp); + } + virtual const char * getMsgType(){ return this->req.getType(); } + virtual const char * getMsgMD5(){ return this->req.getMD5(); } + virtual int getEndpointType(){ return rosserial_msgs::TopicInfo::ID_SERVICE_SERVER + rosserial_msgs::TopicInfo::ID_SUBSCRIBER; } + + MReq req; + MRes resp; + Publisher pub; + private: + CallbackT cb_; + }; + +} + +#endif diff --git a/turtlebot3_core/libraries/ros_lib/ros/subscriber.h b/turtlebot3_core/libraries/ros_lib/ros/subscriber.h new file mode 100644 index 00000000..7b6db4c4 --- /dev/null +++ b/turtlebot3_core/libraries/ros_lib/ros/subscriber.h @@ -0,0 +1,121 @@ +/* + * Software License Agreement (BSD License) + * + * Copyright (c) 2011, Willow Garage, Inc. + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above + * copyright notice, this list of conditions and the following + * disclaimer in the documentation and/or other materials provided + * with the distribution. + * * Neither the name of Willow Garage, Inc. nor the names of its + * contributors may be used to endorse or promote prducts derived + * from this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; + * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + */ + +#ifndef ROS_SUBSCRIBER_H_ +#define ROS_SUBSCRIBER_H_ + +#include "rosserial_msgs/TopicInfo.h" + +namespace ros { + + /* Base class for objects subscribers. */ + class Subscriber_ + { + public: + virtual void callback(unsigned char *data)=0; + virtual int getEndpointType()=0; + + // id_ is set by NodeHandle when we advertise + int id_; + + virtual const char * getMsgType()=0; + virtual const char * getMsgMD5()=0; + const char * topic_; + }; + + /* Bound function subscriber. */ + template + class Subscriber: public Subscriber_ + { + public: + typedef void(ObjT::*CallbackT)(const MsgT&); + MsgT msg; + + Subscriber(const char * topic_name, CallbackT cb, ObjT* obj, int endpoint=rosserial_msgs::TopicInfo::ID_SUBSCRIBER) : + cb_(cb), + obj_(obj), + endpoint_(endpoint) + { + topic_ = topic_name; + }; + + virtual void callback(unsigned char* data) + { + msg.deserialize(data); + (obj_->*cb_)(msg); + } + + virtual const char * getMsgType() { return this->msg.getType(); } + virtual const char * getMsgMD5() { return this->msg.getMD5(); } + virtual int getEndpointType() { return endpoint_; } + + private: + CallbackT cb_; + ObjT* obj_; + int endpoint_; + }; + + /* Standalone function subscriber. */ + template + class Subscriber: public Subscriber_ + { + public: + typedef void(*CallbackT)(const MsgT&); + MsgT msg; + + Subscriber(const char * topic_name, CallbackT cb, int endpoint=rosserial_msgs::TopicInfo::ID_SUBSCRIBER) : + cb_(cb), + endpoint_(endpoint) + { + topic_ = topic_name; + }; + + virtual void callback(unsigned char* data) + { + msg.deserialize(data); + this->cb_(msg); + } + + virtual const char * getMsgType() { return this->msg.getType(); } + virtual const char * getMsgMD5() { return this->msg.getMD5(); } + virtual int getEndpointType() { return endpoint_; } + + private: + CallbackT cb_; + int endpoint_; + }; + +} + +#endif diff --git a/turtlebot3_core/libraries/ros_lib/ros/time.h b/turtlebot3_core/libraries/ros_lib/ros/time.h new file mode 100644 index 00000000..24958132 --- /dev/null +++ b/turtlebot3_core/libraries/ros_lib/ros/time.h @@ -0,0 +1,72 @@ +/* + * Software License Agreement (BSD License) + * + * Copyright (c) 2011, Willow Garage, Inc. + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above + * copyright notice, this list of conditions and the following + * disclaimer in the documentation and/or other materials provided + * with the distribution. + * * Neither the name of Willow Garage, Inc. nor the names of its + * contributors may be used to endorse or promote prducts derived + * from this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; + * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + */ + +#ifndef ROS_TIME_H_ +#define ROS_TIME_H_ + +#include "ros/duration.h" +#include +#include + +namespace ros +{ + void normalizeSecNSec(uint32_t &sec, uint32_t &nsec); + + class Time + { + public: + uint32_t sec, nsec; + + Time() : sec(0), nsec(0) {} + Time(uint32_t _sec, uint32_t _nsec) : sec(_sec), nsec(_nsec) + { + normalizeSecNSec(sec, nsec); + } + + double toSec() const { return (double)sec + 1e-9*(double)nsec; }; + void fromSec(double t) { sec = (uint32_t) floor(t); nsec = (uint32_t) round((t-sec) * 1e9); }; + + uint32_t toNsec() { return (uint32_t)sec*1000000000ull + (uint32_t)nsec; }; + Time& fromNSec(int32_t t); + + Time& operator +=(const Duration &rhs); + Time& operator -=(const Duration &rhs); + + static Time now(); + static void setNow( Time & new_now); + }; + +} + +#endif diff --git a/turtlebot3_core/libraries/ros_lib/roscpp/Empty.h b/turtlebot3_core/libraries/ros_lib/roscpp/Empty.h new file mode 100644 index 00000000..df021b79 --- /dev/null +++ b/turtlebot3_core/libraries/ros_lib/roscpp/Empty.h @@ -0,0 +1,70 @@ +#ifndef _ROS_SERVICE_Empty_h +#define _ROS_SERVICE_Empty_h +#include +#include +#include +#include "ros/msg.h" + +namespace roscpp +{ + +static const char EMPTY[] = "roscpp/Empty"; + + class EmptyRequest : public ros::Msg + { + public: + + EmptyRequest() + { + } + + virtual int serialize(unsigned char *outbuffer) const + { + int offset = 0; + return offset; + } + + virtual int deserialize(unsigned char *inbuffer) + { + int offset = 0; + return offset; + } + + const char * getType(){ return EMPTY; }; + const char * getMD5(){ return "d41d8cd98f00b204e9800998ecf8427e"; }; + + }; + + class EmptyResponse : public ros::Msg + { + public: + + EmptyResponse() + { + } + + virtual int serialize(unsigned char *outbuffer) const + { + int offset = 0; + return offset; + } + + virtual int deserialize(unsigned char *inbuffer) + { + int offset = 0; + return offset; + } + + const char * getType(){ return EMPTY; }; + const char * getMD5(){ return "d41d8cd98f00b204e9800998ecf8427e"; }; + + }; + + class Empty { + public: + typedef EmptyRequest Request; + typedef EmptyResponse Response; + }; + +} +#endif diff --git a/turtlebot3_core/libraries/ros_lib/roscpp/GetLoggers.h b/turtlebot3_core/libraries/ros_lib/roscpp/GetLoggers.h new file mode 100644 index 00000000..d0a69894 --- /dev/null +++ b/turtlebot3_core/libraries/ros_lib/roscpp/GetLoggers.h @@ -0,0 +1,95 @@ +#ifndef _ROS_SERVICE_GetLoggers_h +#define _ROS_SERVICE_GetLoggers_h +#include +#include +#include +#include "ros/msg.h" +#include "roscpp/Logger.h" + +namespace roscpp +{ + +static const char GETLOGGERS[] = "roscpp/GetLoggers"; + + class GetLoggersRequest : public ros::Msg + { + public: + + GetLoggersRequest() + { + } + + virtual int serialize(unsigned char *outbuffer) const + { + int offset = 0; + return offset; + } + + virtual int deserialize(unsigned char *inbuffer) + { + int offset = 0; + return offset; + } + + const char * getType(){ return GETLOGGERS; }; + const char * getMD5(){ return "d41d8cd98f00b204e9800998ecf8427e"; }; + + }; + + class GetLoggersResponse : public ros::Msg + { + public: + uint32_t loggers_length; + roscpp::Logger st_loggers; + roscpp::Logger * loggers; + + GetLoggersResponse(): + loggers_length(0), loggers(NULL) + { + } + + virtual int serialize(unsigned char *outbuffer) const + { + int offset = 0; + *(outbuffer + offset + 0) = (this->loggers_length >> (8 * 0)) & 0xFF; + *(outbuffer + offset + 1) = (this->loggers_length >> (8 * 1)) & 0xFF; + *(outbuffer + offset + 2) = (this->loggers_length >> (8 * 2)) & 0xFF; + *(outbuffer + offset + 3) = (this->loggers_length >> (8 * 3)) & 0xFF; + offset += sizeof(this->loggers_length); + for( uint32_t i = 0; i < loggers_length; i++){ + offset += this->loggers[i].serialize(outbuffer + offset); + } + return offset; + } + + virtual int deserialize(unsigned char *inbuffer) + { + int offset = 0; + uint32_t loggers_lengthT = ((uint32_t) (*(inbuffer + offset))); + loggers_lengthT |= ((uint32_t) (*(inbuffer + offset + 1))) << (8 * 1); + loggers_lengthT |= ((uint32_t) (*(inbuffer + offset + 2))) << (8 * 2); + loggers_lengthT |= ((uint32_t) (*(inbuffer + offset + 3))) << (8 * 3); + offset += sizeof(this->loggers_length); + if(loggers_lengthT > loggers_length) + this->loggers = (roscpp::Logger*)realloc(this->loggers, loggers_lengthT * sizeof(roscpp::Logger)); + loggers_length = loggers_lengthT; + for( uint32_t i = 0; i < loggers_length; i++){ + offset += this->st_loggers.deserialize(inbuffer + offset); + memcpy( &(this->loggers[i]), &(this->st_loggers), sizeof(roscpp::Logger)); + } + return offset; + } + + const char * getType(){ return GETLOGGERS; }; + const char * getMD5(){ return "32e97e85527d4678a8f9279894bb64b0"; }; + + }; + + class GetLoggers { + public: + typedef GetLoggersRequest Request; + typedef GetLoggersResponse Response; + }; + +} +#endif diff --git a/turtlebot3_core/libraries/ros_lib/roscpp/Logger.h b/turtlebot3_core/libraries/ros_lib/roscpp/Logger.h new file mode 100644 index 00000000..a67fb517 --- /dev/null +++ b/turtlebot3_core/libraries/ros_lib/roscpp/Logger.h @@ -0,0 +1,70 @@ +#ifndef _ROS_roscpp_Logger_h +#define _ROS_roscpp_Logger_h + +#include +#include +#include +#include "ros/msg.h" + +namespace roscpp +{ + + class Logger : public ros::Msg + { + public: + const char* name; + const char* level; + + Logger(): + name(""), + level("") + { + } + + virtual int serialize(unsigned char *outbuffer) const + { + int offset = 0; + uint32_t length_name = strlen(this->name); + memcpy(outbuffer + offset, &length_name, sizeof(uint32_t)); + offset += 4; + memcpy(outbuffer + offset, this->name, length_name); + offset += length_name; + uint32_t length_level = strlen(this->level); + memcpy(outbuffer + offset, &length_level, sizeof(uint32_t)); + offset += 4; + memcpy(outbuffer + offset, this->level, length_level); + offset += length_level; + return offset; + } + + virtual int deserialize(unsigned char *inbuffer) + { + int offset = 0; + uint32_t length_name; + memcpy(&length_name, (inbuffer + offset), sizeof(uint32_t)); + offset += 4; + for(unsigned int k= offset; k< offset+length_name; ++k){ + inbuffer[k-1]=inbuffer[k]; + } + inbuffer[offset+length_name-1]=0; + this->name = (char *)(inbuffer + offset-1); + offset += length_name; + uint32_t length_level; + memcpy(&length_level, (inbuffer + offset), sizeof(uint32_t)); + offset += 4; + for(unsigned int k= offset; k< offset+length_level; ++k){ + inbuffer[k-1]=inbuffer[k]; + } + inbuffer[offset+length_level-1]=0; + this->level = (char *)(inbuffer + offset-1); + offset += length_level; + return offset; + } + + const char * getType(){ return "roscpp/Logger"; }; + const char * getMD5(){ return "a6069a2ff40db7bd32143dd66e1f408e"; }; + + }; + +} +#endif \ No newline at end of file diff --git a/turtlebot3_core/libraries/ros_lib/roscpp/SetLoggerLevel.h b/turtlebot3_core/libraries/ros_lib/roscpp/SetLoggerLevel.h new file mode 100644 index 00000000..dddee2e9 --- /dev/null +++ b/turtlebot3_core/libraries/ros_lib/roscpp/SetLoggerLevel.h @@ -0,0 +1,102 @@ +#ifndef _ROS_SERVICE_SetLoggerLevel_h +#define _ROS_SERVICE_SetLoggerLevel_h +#include +#include +#include +#include "ros/msg.h" + +namespace roscpp +{ + +static const char SETLOGGERLEVEL[] = "roscpp/SetLoggerLevel"; + + class SetLoggerLevelRequest : public ros::Msg + { + public: + const char* logger; + const char* level; + + SetLoggerLevelRequest(): + logger(""), + level("") + { + } + + virtual int serialize(unsigned char *outbuffer) const + { + int offset = 0; + uint32_t length_logger = strlen(this->logger); + memcpy(outbuffer + offset, &length_logger, sizeof(uint32_t)); + offset += 4; + memcpy(outbuffer + offset, this->logger, length_logger); + offset += length_logger; + uint32_t length_level = strlen(this->level); + memcpy(outbuffer + offset, &length_level, sizeof(uint32_t)); + offset += 4; + memcpy(outbuffer + offset, this->level, length_level); + offset += length_level; + return offset; + } + + virtual int deserialize(unsigned char *inbuffer) + { + int offset = 0; + uint32_t length_logger; + memcpy(&length_logger, (inbuffer + offset), sizeof(uint32_t)); + offset += 4; + for(unsigned int k= offset; k< offset+length_logger; ++k){ + inbuffer[k-1]=inbuffer[k]; + } + inbuffer[offset+length_logger-1]=0; + this->logger = (char *)(inbuffer + offset-1); + offset += length_logger; + uint32_t length_level; + memcpy(&length_level, (inbuffer + offset), sizeof(uint32_t)); + offset += 4; + for(unsigned int k= offset; k< offset+length_level; ++k){ + inbuffer[k-1]=inbuffer[k]; + } + inbuffer[offset+length_level-1]=0; + this->level = (char *)(inbuffer + offset-1); + offset += length_level; + return offset; + } + + const char * getType(){ return SETLOGGERLEVEL; }; + const char * getMD5(){ return "51da076440d78ca1684d36c868df61ea"; }; + + }; + + class SetLoggerLevelResponse : public ros::Msg + { + public: + + SetLoggerLevelResponse() + { + } + + virtual int serialize(unsigned char *outbuffer) const + { + int offset = 0; + return offset; + } + + virtual int deserialize(unsigned char *inbuffer) + { + int offset = 0; + return offset; + } + + const char * getType(){ return SETLOGGERLEVEL; }; + const char * getMD5(){ return "d41d8cd98f00b204e9800998ecf8427e"; }; + + }; + + class SetLoggerLevel { + public: + typedef SetLoggerLevelRequest Request; + typedef SetLoggerLevelResponse Response; + }; + +} +#endif diff --git a/turtlebot3_core/libraries/ros_lib/rosserial_arduino/Adc.h b/turtlebot3_core/libraries/ros_lib/rosserial_arduino/Adc.h new file mode 100644 index 00000000..21114c7e --- /dev/null +++ b/turtlebot3_core/libraries/ros_lib/rosserial_arduino/Adc.h @@ -0,0 +1,86 @@ +#ifndef _ROS_rosserial_arduino_Adc_h +#define _ROS_rosserial_arduino_Adc_h + +#include +#include +#include +#include "ros/msg.h" + +namespace rosserial_arduino +{ + + class Adc : public ros::Msg + { + public: + uint16_t adc0; + uint16_t adc1; + uint16_t adc2; + uint16_t adc3; + uint16_t adc4; + uint16_t adc5; + + Adc(): + adc0(0), + adc1(0), + adc2(0), + adc3(0), + adc4(0), + adc5(0) + { + } + + virtual int serialize(unsigned char *outbuffer) const + { + int offset = 0; + *(outbuffer + offset + 0) = (this->adc0 >> (8 * 0)) & 0xFF; + *(outbuffer + offset + 1) = (this->adc0 >> (8 * 1)) & 0xFF; + offset += sizeof(this->adc0); + *(outbuffer + offset + 0) = (this->adc1 >> (8 * 0)) & 0xFF; + *(outbuffer + offset + 1) = (this->adc1 >> (8 * 1)) & 0xFF; + offset += sizeof(this->adc1); + *(outbuffer + offset + 0) = (this->adc2 >> (8 * 0)) & 0xFF; + *(outbuffer + offset + 1) = (this->adc2 >> (8 * 1)) & 0xFF; + offset += sizeof(this->adc2); + *(outbuffer + offset + 0) = (this->adc3 >> (8 * 0)) & 0xFF; + *(outbuffer + offset + 1) = (this->adc3 >> (8 * 1)) & 0xFF; + offset += sizeof(this->adc3); + *(outbuffer + offset + 0) = (this->adc4 >> (8 * 0)) & 0xFF; + *(outbuffer + offset + 1) = (this->adc4 >> (8 * 1)) & 0xFF; + offset += sizeof(this->adc4); + *(outbuffer + offset + 0) = (this->adc5 >> (8 * 0)) & 0xFF; + *(outbuffer + offset + 1) = (this->adc5 >> (8 * 1)) & 0xFF; + offset += sizeof(this->adc5); + return offset; + } + + virtual int deserialize(unsigned char *inbuffer) + { + int offset = 0; + this->adc0 = ((uint16_t) (*(inbuffer + offset))); + this->adc0 |= ((uint16_t) (*(inbuffer + offset + 1))) << (8 * 1); + offset += sizeof(this->adc0); + this->adc1 = ((uint16_t) (*(inbuffer + offset))); + this->adc1 |= ((uint16_t) (*(inbuffer + offset + 1))) << (8 * 1); + offset += sizeof(this->adc1); + this->adc2 = ((uint16_t) (*(inbuffer + offset))); + this->adc2 |= ((uint16_t) (*(inbuffer + offset + 1))) << (8 * 1); + offset += sizeof(this->adc2); + this->adc3 = ((uint16_t) (*(inbuffer + offset))); + this->adc3 |= ((uint16_t) (*(inbuffer + offset + 1))) << (8 * 1); + offset += sizeof(this->adc3); + this->adc4 = ((uint16_t) (*(inbuffer + offset))); + this->adc4 |= ((uint16_t) (*(inbuffer + offset + 1))) << (8 * 1); + offset += sizeof(this->adc4); + this->adc5 = ((uint16_t) (*(inbuffer + offset))); + this->adc5 |= ((uint16_t) (*(inbuffer + offset + 1))) << (8 * 1); + offset += sizeof(this->adc5); + return offset; + } + + const char * getType(){ return "rosserial_arduino/Adc"; }; + const char * getMD5(){ return "6d7853a614e2e821319068311f2af25b"; }; + + }; + +} +#endif \ No newline at end of file diff --git a/turtlebot3_core/libraries/ros_lib/rosserial_arduino/Test.h b/turtlebot3_core/libraries/ros_lib/rosserial_arduino/Test.h new file mode 100644 index 00000000..e1327289 --- /dev/null +++ b/turtlebot3_core/libraries/ros_lib/rosserial_arduino/Test.h @@ -0,0 +1,102 @@ +#ifndef _ROS_SERVICE_Test_h +#define _ROS_SERVICE_Test_h +#include +#include +#include +#include "ros/msg.h" + +namespace rosserial_arduino +{ + +static const char TEST[] = "rosserial_arduino/Test"; + + class TestRequest : public ros::Msg + { + public: + const char* input; + + TestRequest(): + input("") + { + } + + virtual int serialize(unsigned char *outbuffer) const + { + int offset = 0; + uint32_t length_input = strlen(this->input); + memcpy(outbuffer + offset, &length_input, sizeof(uint32_t)); + offset += 4; + memcpy(outbuffer + offset, this->input, length_input); + offset += length_input; + return offset; + } + + virtual int deserialize(unsigned char *inbuffer) + { + int offset = 0; + uint32_t length_input; + memcpy(&length_input, (inbuffer + offset), sizeof(uint32_t)); + offset += 4; + for(unsigned int k= offset; k< offset+length_input; ++k){ + inbuffer[k-1]=inbuffer[k]; + } + inbuffer[offset+length_input-1]=0; + this->input = (char *)(inbuffer + offset-1); + offset += length_input; + return offset; + } + + const char * getType(){ return TEST; }; + const char * getMD5(){ return "39e92f1778057359c64c7b8a7d7b19de"; }; + + }; + + class TestResponse : public ros::Msg + { + public: + const char* output; + + TestResponse(): + output("") + { + } + + virtual int serialize(unsigned char *outbuffer) const + { + int offset = 0; + uint32_t length_output = strlen(this->output); + memcpy(outbuffer + offset, &length_output, sizeof(uint32_t)); + offset += 4; + memcpy(outbuffer + offset, this->output, length_output); + offset += length_output; + return offset; + } + + virtual int deserialize(unsigned char *inbuffer) + { + int offset = 0; + uint32_t length_output; + memcpy(&length_output, (inbuffer + offset), sizeof(uint32_t)); + offset += 4; + for(unsigned int k= offset; k< offset+length_output; ++k){ + inbuffer[k-1]=inbuffer[k]; + } + inbuffer[offset+length_output-1]=0; + this->output = (char *)(inbuffer + offset-1); + offset += length_output; + return offset; + } + + const char * getType(){ return TEST; }; + const char * getMD5(){ return "0825d95fdfa2c8f4bbb4e9c74bccd3fd"; }; + + }; + + class Test { + public: + typedef TestRequest Request; + typedef TestResponse Response; + }; + +} +#endif diff --git a/turtlebot3_core/libraries/ros_lib/rosserial_msgs/Log.h b/turtlebot3_core/libraries/ros_lib/rosserial_msgs/Log.h new file mode 100644 index 00000000..bcd6fd62 --- /dev/null +++ b/turtlebot3_core/libraries/ros_lib/rosserial_msgs/Log.h @@ -0,0 +1,65 @@ +#ifndef _ROS_rosserial_msgs_Log_h +#define _ROS_rosserial_msgs_Log_h + +#include +#include +#include +#include "ros/msg.h" + +namespace rosserial_msgs +{ + + class Log : public ros::Msg + { + public: + uint8_t level; + const char* msg; + enum { ROSDEBUG = 0 }; + enum { INFO = 1 }; + enum { WARN = 2 }; + enum { ERROR = 3 }; + enum { FATAL = 4 }; + + Log(): + level(0), + msg("") + { + } + + virtual int serialize(unsigned char *outbuffer) const + { + int offset = 0; + *(outbuffer + offset + 0) = (this->level >> (8 * 0)) & 0xFF; + offset += sizeof(this->level); + uint32_t length_msg = strlen(this->msg); + memcpy(outbuffer + offset, &length_msg, sizeof(uint32_t)); + offset += 4; + memcpy(outbuffer + offset, this->msg, length_msg); + offset += length_msg; + return offset; + } + + virtual int deserialize(unsigned char *inbuffer) + { + int offset = 0; + this->level = ((uint8_t) (*(inbuffer + offset))); + offset += sizeof(this->level); + uint32_t length_msg; + memcpy(&length_msg, (inbuffer + offset), sizeof(uint32_t)); + offset += 4; + for(unsigned int k= offset; k< offset+length_msg; ++k){ + inbuffer[k-1]=inbuffer[k]; + } + inbuffer[offset+length_msg-1]=0; + this->msg = (char *)(inbuffer + offset-1); + offset += length_msg; + return offset; + } + + const char * getType(){ return "rosserial_msgs/Log"; }; + const char * getMD5(){ return "11abd731c25933261cd6183bd12d6295"; }; + + }; + +} +#endif \ No newline at end of file diff --git a/turtlebot3_core/libraries/ros_lib/rosserial_msgs/RequestMessageInfo.h b/turtlebot3_core/libraries/ros_lib/rosserial_msgs/RequestMessageInfo.h new file mode 100644 index 00000000..98af72e6 --- /dev/null +++ b/turtlebot3_core/libraries/ros_lib/rosserial_msgs/RequestMessageInfo.h @@ -0,0 +1,118 @@ +#ifndef _ROS_SERVICE_RequestMessageInfo_h +#define _ROS_SERVICE_RequestMessageInfo_h +#include +#include +#include +#include "ros/msg.h" + +namespace rosserial_msgs +{ + +static const char REQUESTMESSAGEINFO[] = "rosserial_msgs/RequestMessageInfo"; + + class RequestMessageInfoRequest : public ros::Msg + { + public: + const char* type; + + RequestMessageInfoRequest(): + type("") + { + } + + virtual int serialize(unsigned char *outbuffer) const + { + int offset = 0; + uint32_t length_type = strlen(this->type); + memcpy(outbuffer + offset, &length_type, sizeof(uint32_t)); + offset += 4; + memcpy(outbuffer + offset, this->type, length_type); + offset += length_type; + return offset; + } + + virtual int deserialize(unsigned char *inbuffer) + { + int offset = 0; + uint32_t length_type; + memcpy(&length_type, (inbuffer + offset), sizeof(uint32_t)); + offset += 4; + for(unsigned int k= offset; k< offset+length_type; ++k){ + inbuffer[k-1]=inbuffer[k]; + } + inbuffer[offset+length_type-1]=0; + this->type = (char *)(inbuffer + offset-1); + offset += length_type; + return offset; + } + + const char * getType(){ return REQUESTMESSAGEINFO; }; + const char * getMD5(){ return "dc67331de85cf97091b7d45e5c64ab75"; }; + + }; + + class RequestMessageInfoResponse : public ros::Msg + { + public: + const char* md5; + const char* definition; + + RequestMessageInfoResponse(): + md5(""), + definition("") + { + } + + virtual int serialize(unsigned char *outbuffer) const + { + int offset = 0; + uint32_t length_md5 = strlen(this->md5); + memcpy(outbuffer + offset, &length_md5, sizeof(uint32_t)); + offset += 4; + memcpy(outbuffer + offset, this->md5, length_md5); + offset += length_md5; + uint32_t length_definition = strlen(this->definition); + memcpy(outbuffer + offset, &length_definition, sizeof(uint32_t)); + offset += 4; + memcpy(outbuffer + offset, this->definition, length_definition); + offset += length_definition; + return offset; + } + + virtual int deserialize(unsigned char *inbuffer) + { + int offset = 0; + uint32_t length_md5; + memcpy(&length_md5, (inbuffer + offset), sizeof(uint32_t)); + offset += 4; + for(unsigned int k= offset; k< offset+length_md5; ++k){ + inbuffer[k-1]=inbuffer[k]; + } + inbuffer[offset+length_md5-1]=0; + this->md5 = (char *)(inbuffer + offset-1); + offset += length_md5; + uint32_t length_definition; + memcpy(&length_definition, (inbuffer + offset), sizeof(uint32_t)); + offset += 4; + for(unsigned int k= offset; k< offset+length_definition; ++k){ + inbuffer[k-1]=inbuffer[k]; + } + inbuffer[offset+length_definition-1]=0; + this->definition = (char *)(inbuffer + offset-1); + offset += length_definition; + return offset; + } + + const char * getType(){ return REQUESTMESSAGEINFO; }; + const char * getMD5(){ return "fe452186a069bed40f09b8628fe5eac8"; }; + + }; + + class RequestMessageInfo { + public: + typedef RequestMessageInfoRequest Request; + typedef RequestMessageInfoResponse Response; + }; + +} +#endif diff --git a/turtlebot3_core/libraries/ros_lib/rosserial_msgs/RequestParam.h b/turtlebot3_core/libraries/ros_lib/rosserial_msgs/RequestParam.h new file mode 100644 index 00000000..1ccf9e74 --- /dev/null +++ b/turtlebot3_core/libraries/ros_lib/rosserial_msgs/RequestParam.h @@ -0,0 +1,208 @@ +#ifndef _ROS_SERVICE_RequestParam_h +#define _ROS_SERVICE_RequestParam_h +#include +#include +#include +#include "ros/msg.h" + +namespace rosserial_msgs +{ + +static const char REQUESTPARAM[] = "rosserial_msgs/RequestParam"; + + class RequestParamRequest : public ros::Msg + { + public: + const char* name; + + RequestParamRequest(): + name("") + { + } + + virtual int serialize(unsigned char *outbuffer) const + { + int offset = 0; + uint32_t length_name = strlen(this->name); + memcpy(outbuffer + offset, &length_name, sizeof(uint32_t)); + offset += 4; + memcpy(outbuffer + offset, this->name, length_name); + offset += length_name; + return offset; + } + + virtual int deserialize(unsigned char *inbuffer) + { + int offset = 0; + uint32_t length_name; + memcpy(&length_name, (inbuffer + offset), sizeof(uint32_t)); + offset += 4; + for(unsigned int k= offset; k< offset+length_name; ++k){ + inbuffer[k-1]=inbuffer[k]; + } + inbuffer[offset+length_name-1]=0; + this->name = (char *)(inbuffer + offset-1); + offset += length_name; + return offset; + } + + const char * getType(){ return REQUESTPARAM; }; + const char * getMD5(){ return "c1f3d28f1b044c871e6eff2e9fc3c667"; }; + + }; + + class RequestParamResponse : public ros::Msg + { + public: + uint32_t ints_length; + int32_t st_ints; + int32_t * ints; + uint32_t floats_length; + float st_floats; + float * floats; + uint32_t strings_length; + char* st_strings; + char* * strings; + + RequestParamResponse(): + ints_length(0), ints(NULL), + floats_length(0), floats(NULL), + strings_length(0), strings(NULL) + { + } + + virtual int serialize(unsigned char *outbuffer) const + { + int offset = 0; + *(outbuffer + offset + 0) = (this->ints_length >> (8 * 0)) & 0xFF; + *(outbuffer + offset + 1) = (this->ints_length >> (8 * 1)) & 0xFF; + *(outbuffer + offset + 2) = (this->ints_length >> (8 * 2)) & 0xFF; + *(outbuffer + offset + 3) = (this->ints_length >> (8 * 3)) & 0xFF; + offset += sizeof(this->ints_length); + for( uint32_t i = 0; i < ints_length; i++){ + union { + int32_t real; + uint32_t base; + } u_intsi; + u_intsi.real = this->ints[i]; + *(outbuffer + offset + 0) = (u_intsi.base >> (8 * 0)) & 0xFF; + *(outbuffer + offset + 1) = (u_intsi.base >> (8 * 1)) & 0xFF; + *(outbuffer + offset + 2) = (u_intsi.base >> (8 * 2)) & 0xFF; + *(outbuffer + offset + 3) = (u_intsi.base >> (8 * 3)) & 0xFF; + offset += sizeof(this->ints[i]); + } + *(outbuffer + offset + 0) = (this->floats_length >> (8 * 0)) & 0xFF; + *(outbuffer + offset + 1) = (this->floats_length >> (8 * 1)) & 0xFF; + *(outbuffer + offset + 2) = (this->floats_length >> (8 * 2)) & 0xFF; + *(outbuffer + offset + 3) = (this->floats_length >> (8 * 3)) & 0xFF; + offset += sizeof(this->floats_length); + for( uint32_t i = 0; i < floats_length; i++){ + union { + float real; + uint32_t base; + } u_floatsi; + u_floatsi.real = this->floats[i]; + *(outbuffer + offset + 0) = (u_floatsi.base >> (8 * 0)) & 0xFF; + *(outbuffer + offset + 1) = (u_floatsi.base >> (8 * 1)) & 0xFF; + *(outbuffer + offset + 2) = (u_floatsi.base >> (8 * 2)) & 0xFF; + *(outbuffer + offset + 3) = (u_floatsi.base >> (8 * 3)) & 0xFF; + offset += sizeof(this->floats[i]); + } + *(outbuffer + offset + 0) = (this->strings_length >> (8 * 0)) & 0xFF; + *(outbuffer + offset + 1) = (this->strings_length >> (8 * 1)) & 0xFF; + *(outbuffer + offset + 2) = (this->strings_length >> (8 * 2)) & 0xFF; + *(outbuffer + offset + 3) = (this->strings_length >> (8 * 3)) & 0xFF; + offset += sizeof(this->strings_length); + for( uint32_t i = 0; i < strings_length; i++){ + uint32_t length_stringsi = strlen(this->strings[i]); + memcpy(outbuffer + offset, &length_stringsi, sizeof(uint32_t)); + offset += 4; + memcpy(outbuffer + offset, this->strings[i], length_stringsi); + offset += length_stringsi; + } + return offset; + } + + virtual int deserialize(unsigned char *inbuffer) + { + int offset = 0; + uint32_t ints_lengthT = ((uint32_t) (*(inbuffer + offset))); + ints_lengthT |= ((uint32_t) (*(inbuffer + offset + 1))) << (8 * 1); + ints_lengthT |= ((uint32_t) (*(inbuffer + offset + 2))) << (8 * 2); + ints_lengthT |= ((uint32_t) (*(inbuffer + offset + 3))) << (8 * 3); + offset += sizeof(this->ints_length); + if(ints_lengthT > ints_length) + this->ints = (int32_t*)realloc(this->ints, ints_lengthT * sizeof(int32_t)); + ints_length = ints_lengthT; + for( uint32_t i = 0; i < ints_length; i++){ + union { + int32_t real; + uint32_t base; + } u_st_ints; + u_st_ints.base = 0; + u_st_ints.base |= ((uint32_t) (*(inbuffer + offset + 0))) << (8 * 0); + u_st_ints.base |= ((uint32_t) (*(inbuffer + offset + 1))) << (8 * 1); + u_st_ints.base |= ((uint32_t) (*(inbuffer + offset + 2))) << (8 * 2); + u_st_ints.base |= ((uint32_t) (*(inbuffer + offset + 3))) << (8 * 3); + this->st_ints = u_st_ints.real; + offset += sizeof(this->st_ints); + memcpy( &(this->ints[i]), &(this->st_ints), sizeof(int32_t)); + } + uint32_t floats_lengthT = ((uint32_t) (*(inbuffer + offset))); + floats_lengthT |= ((uint32_t) (*(inbuffer + offset + 1))) << (8 * 1); + floats_lengthT |= ((uint32_t) (*(inbuffer + offset + 2))) << (8 * 2); + floats_lengthT |= ((uint32_t) (*(inbuffer + offset + 3))) << (8 * 3); + offset += sizeof(this->floats_length); + if(floats_lengthT > floats_length) + this->floats = (float*)realloc(this->floats, floats_lengthT * sizeof(float)); + floats_length = floats_lengthT; + for( uint32_t i = 0; i < floats_length; i++){ + union { + float real; + uint32_t base; + } u_st_floats; + u_st_floats.base = 0; + u_st_floats.base |= ((uint32_t) (*(inbuffer + offset + 0))) << (8 * 0); + u_st_floats.base |= ((uint32_t) (*(inbuffer + offset + 1))) << (8 * 1); + u_st_floats.base |= ((uint32_t) (*(inbuffer + offset + 2))) << (8 * 2); + u_st_floats.base |= ((uint32_t) (*(inbuffer + offset + 3))) << (8 * 3); + this->st_floats = u_st_floats.real; + offset += sizeof(this->st_floats); + memcpy( &(this->floats[i]), &(this->st_floats), sizeof(float)); + } + uint32_t strings_lengthT = ((uint32_t) (*(inbuffer + offset))); + strings_lengthT |= ((uint32_t) (*(inbuffer + offset + 1))) << (8 * 1); + strings_lengthT |= ((uint32_t) (*(inbuffer + offset + 2))) << (8 * 2); + strings_lengthT |= ((uint32_t) (*(inbuffer + offset + 3))) << (8 * 3); + offset += sizeof(this->strings_length); + if(strings_lengthT > strings_length) + this->strings = (char**)realloc(this->strings, strings_lengthT * sizeof(char*)); + strings_length = strings_lengthT; + for( uint32_t i = 0; i < strings_length; i++){ + uint32_t length_st_strings; + memcpy(&length_st_strings, (inbuffer + offset), sizeof(uint32_t)); + offset += 4; + for(unsigned int k= offset; k< offset+length_st_strings; ++k){ + inbuffer[k-1]=inbuffer[k]; + } + inbuffer[offset+length_st_strings-1]=0; + this->st_strings = (char *)(inbuffer + offset-1); + offset += length_st_strings; + memcpy( &(this->strings[i]), &(this->st_strings), sizeof(char*)); + } + return offset; + } + + const char * getType(){ return REQUESTPARAM; }; + const char * getMD5(){ return "9f0e98bda65981986ddf53afa7a40e49"; }; + + }; + + class RequestParam { + public: + typedef RequestParamRequest Request; + typedef RequestParamResponse Response; + }; + +} +#endif diff --git a/turtlebot3_core/libraries/ros_lib/rosserial_msgs/RequestServiceInfo.h b/turtlebot3_core/libraries/ros_lib/rosserial_msgs/RequestServiceInfo.h new file mode 100644 index 00000000..90b47c57 --- /dev/null +++ b/turtlebot3_core/libraries/ros_lib/rosserial_msgs/RequestServiceInfo.h @@ -0,0 +1,134 @@ +#ifndef _ROS_SERVICE_RequestServiceInfo_h +#define _ROS_SERVICE_RequestServiceInfo_h +#include +#include +#include +#include "ros/msg.h" + +namespace rosserial_msgs +{ + +static const char REQUESTSERVICEINFO[] = "rosserial_msgs/RequestServiceInfo"; + + class RequestServiceInfoRequest : public ros::Msg + { + public: + const char* service; + + RequestServiceInfoRequest(): + service("") + { + } + + virtual int serialize(unsigned char *outbuffer) const + { + int offset = 0; + uint32_t length_service = strlen(this->service); + memcpy(outbuffer + offset, &length_service, sizeof(uint32_t)); + offset += 4; + memcpy(outbuffer + offset, this->service, length_service); + offset += length_service; + return offset; + } + + virtual int deserialize(unsigned char *inbuffer) + { + int offset = 0; + uint32_t length_service; + memcpy(&length_service, (inbuffer + offset), sizeof(uint32_t)); + offset += 4; + for(unsigned int k= offset; k< offset+length_service; ++k){ + inbuffer[k-1]=inbuffer[k]; + } + inbuffer[offset+length_service-1]=0; + this->service = (char *)(inbuffer + offset-1); + offset += length_service; + return offset; + } + + const char * getType(){ return REQUESTSERVICEINFO; }; + const char * getMD5(){ return "1cbcfa13b08f6d36710b9af8741e6112"; }; + + }; + + class RequestServiceInfoResponse : public ros::Msg + { + public: + const char* service_md5; + const char* request_md5; + const char* response_md5; + + RequestServiceInfoResponse(): + service_md5(""), + request_md5(""), + response_md5("") + { + } + + virtual int serialize(unsigned char *outbuffer) const + { + int offset = 0; + uint32_t length_service_md5 = strlen(this->service_md5); + memcpy(outbuffer + offset, &length_service_md5, sizeof(uint32_t)); + offset += 4; + memcpy(outbuffer + offset, this->service_md5, length_service_md5); + offset += length_service_md5; + uint32_t length_request_md5 = strlen(this->request_md5); + memcpy(outbuffer + offset, &length_request_md5, sizeof(uint32_t)); + offset += 4; + memcpy(outbuffer + offset, this->request_md5, length_request_md5); + offset += length_request_md5; + uint32_t length_response_md5 = strlen(this->response_md5); + memcpy(outbuffer + offset, &length_response_md5, sizeof(uint32_t)); + offset += 4; + memcpy(outbuffer + offset, this->response_md5, length_response_md5); + offset += length_response_md5; + return offset; + } + + virtual int deserialize(unsigned char *inbuffer) + { + int offset = 0; + uint32_t length_service_md5; + memcpy(&length_service_md5, (inbuffer + offset), sizeof(uint32_t)); + offset += 4; + for(unsigned int k= offset; k< offset+length_service_md5; ++k){ + inbuffer[k-1]=inbuffer[k]; + } + inbuffer[offset+length_service_md5-1]=0; + this->service_md5 = (char *)(inbuffer + offset-1); + offset += length_service_md5; + uint32_t length_request_md5; + memcpy(&length_request_md5, (inbuffer + offset), sizeof(uint32_t)); + offset += 4; + for(unsigned int k= offset; k< offset+length_request_md5; ++k){ + inbuffer[k-1]=inbuffer[k]; + } + inbuffer[offset+length_request_md5-1]=0; + this->request_md5 = (char *)(inbuffer + offset-1); + offset += length_request_md5; + uint32_t length_response_md5; + memcpy(&length_response_md5, (inbuffer + offset), sizeof(uint32_t)); + offset += 4; + for(unsigned int k= offset; k< offset+length_response_md5; ++k){ + inbuffer[k-1]=inbuffer[k]; + } + inbuffer[offset+length_response_md5-1]=0; + this->response_md5 = (char *)(inbuffer + offset-1); + offset += length_response_md5; + return offset; + } + + const char * getType(){ return REQUESTSERVICEINFO; }; + const char * getMD5(){ return "c3d6dd25b909596479fbbc6559fa6874"; }; + + }; + + class RequestServiceInfo { + public: + typedef RequestServiceInfoRequest Request; + typedef RequestServiceInfoResponse Response; + }; + +} +#endif diff --git a/turtlebot3_core/libraries/ros_lib/rosserial_msgs/TopicInfo.h b/turtlebot3_core/libraries/ros_lib/rosserial_msgs/TopicInfo.h new file mode 100644 index 00000000..c4daf2d3 --- /dev/null +++ b/turtlebot3_core/libraries/ros_lib/rosserial_msgs/TopicInfo.h @@ -0,0 +1,125 @@ +#ifndef _ROS_rosserial_msgs_TopicInfo_h +#define _ROS_rosserial_msgs_TopicInfo_h + +#include +#include +#include +#include "ros/msg.h" + +namespace rosserial_msgs +{ + + class TopicInfo : public ros::Msg + { + public: + uint16_t topic_id; + const char* topic_name; + const char* message_type; + const char* md5sum; + int32_t buffer_size; + enum { ID_PUBLISHER = 0 }; + enum { ID_SUBSCRIBER = 1 }; + enum { ID_SERVICE_SERVER = 2 }; + enum { ID_SERVICE_CLIENT = 4 }; + enum { ID_PARAMETER_REQUEST = 6 }; + enum { ID_LOG = 7 }; + enum { ID_TIME = 10 }; + enum { ID_TX_STOP = 11 }; + + TopicInfo(): + topic_id(0), + topic_name(""), + message_type(""), + md5sum(""), + buffer_size(0) + { + } + + virtual int serialize(unsigned char *outbuffer) const + { + int offset = 0; + *(outbuffer + offset + 0) = (this->topic_id >> (8 * 0)) & 0xFF; + *(outbuffer + offset + 1) = (this->topic_id >> (8 * 1)) & 0xFF; + offset += sizeof(this->topic_id); + uint32_t length_topic_name = strlen(this->topic_name); + memcpy(outbuffer + offset, &length_topic_name, sizeof(uint32_t)); + offset += 4; + memcpy(outbuffer + offset, this->topic_name, length_topic_name); + offset += length_topic_name; + uint32_t length_message_type = strlen(this->message_type); + memcpy(outbuffer + offset, &length_message_type, sizeof(uint32_t)); + offset += 4; + memcpy(outbuffer + offset, this->message_type, length_message_type); + offset += length_message_type; + uint32_t length_md5sum = strlen(this->md5sum); + memcpy(outbuffer + offset, &length_md5sum, sizeof(uint32_t)); + offset += 4; + memcpy(outbuffer + offset, this->md5sum, length_md5sum); + offset += length_md5sum; + union { + int32_t real; + uint32_t base; + } u_buffer_size; + u_buffer_size.real = this->buffer_size; + *(outbuffer + offset + 0) = (u_buffer_size.base >> (8 * 0)) & 0xFF; + *(outbuffer + offset + 1) = (u_buffer_size.base >> (8 * 1)) & 0xFF; + *(outbuffer + offset + 2) = (u_buffer_size.base >> (8 * 2)) & 0xFF; + *(outbuffer + offset + 3) = (u_buffer_size.base >> (8 * 3)) & 0xFF; + offset += sizeof(this->buffer_size); + return offset; + } + + virtual int deserialize(unsigned char *inbuffer) + { + int offset = 0; + this->topic_id = ((uint16_t) (*(inbuffer + offset))); + this->topic_id |= ((uint16_t) (*(inbuffer + offset + 1))) << (8 * 1); + offset += sizeof(this->topic_id); + uint32_t length_topic_name; + memcpy(&length_topic_name, (inbuffer + offset), sizeof(uint32_t)); + offset += 4; + for(unsigned int k= offset; k< offset+length_topic_name; ++k){ + inbuffer[k-1]=inbuffer[k]; + } + inbuffer[offset+length_topic_name-1]=0; + this->topic_name = (char *)(inbuffer + offset-1); + offset += length_topic_name; + uint32_t length_message_type; + memcpy(&length_message_type, (inbuffer + offset), sizeof(uint32_t)); + offset += 4; + for(unsigned int k= offset; k< offset+length_message_type; ++k){ + inbuffer[k-1]=inbuffer[k]; + } + inbuffer[offset+length_message_type-1]=0; + this->message_type = (char *)(inbuffer + offset-1); + offset += length_message_type; + uint32_t length_md5sum; + memcpy(&length_md5sum, (inbuffer + offset), sizeof(uint32_t)); + offset += 4; + for(unsigned int k= offset; k< offset+length_md5sum; ++k){ + inbuffer[k-1]=inbuffer[k]; + } + inbuffer[offset+length_md5sum-1]=0; + this->md5sum = (char *)(inbuffer + offset-1); + offset += length_md5sum; + union { + int32_t real; + uint32_t base; + } u_buffer_size; + u_buffer_size.base = 0; + u_buffer_size.base |= ((uint32_t) (*(inbuffer + offset + 0))) << (8 * 0); + u_buffer_size.base |= ((uint32_t) (*(inbuffer + offset + 1))) << (8 * 1); + u_buffer_size.base |= ((uint32_t) (*(inbuffer + offset + 2))) << (8 * 2); + u_buffer_size.base |= ((uint32_t) (*(inbuffer + offset + 3))) << (8 * 3); + this->buffer_size = u_buffer_size.real; + offset += sizeof(this->buffer_size); + return offset; + } + + const char * getType(){ return "rosserial_msgs/TopicInfo"; }; + const char * getMD5(){ return "0ad51f88fc44892f8c10684077646005"; }; + + }; + +} +#endif \ No newline at end of file diff --git a/turtlebot3_core/libraries/ros_lib/sensor_msgs/BatteryState.h b/turtlebot3_core/libraries/ros_lib/sensor_msgs/BatteryState.h new file mode 100644 index 00000000..0a409b76 --- /dev/null +++ b/turtlebot3_core/libraries/ros_lib/sensor_msgs/BatteryState.h @@ -0,0 +1,312 @@ +#ifndef _ROS_sensor_msgs_BatteryState_h +#define _ROS_sensor_msgs_BatteryState_h + +#include +#include +#include +#include "ros/msg.h" +#include "std_msgs/Header.h" + +namespace sensor_msgs +{ + + class BatteryState : public ros::Msg + { + public: + std_msgs::Header header; + float voltage; + float current; + float charge; + float capacity; + float design_capacity; + float percentage; + uint8_t power_supply_status; + uint8_t power_supply_health; + uint8_t power_supply_technology; + bool present; + uint32_t cell_voltage_length; + float st_cell_voltage; + float * cell_voltage; + const char* location; + const char* serial_number; + enum { POWER_SUPPLY_STATUS_UNKNOWN = 0 }; + enum { POWER_SUPPLY_STATUS_CHARGING = 1 }; + enum { POWER_SUPPLY_STATUS_DISCHARGING = 2 }; + enum { POWER_SUPPLY_STATUS_NOT_CHARGING = 3 }; + enum { POWER_SUPPLY_STATUS_FULL = 4 }; + enum { POWER_SUPPLY_HEALTH_UNKNOWN = 0 }; + enum { POWER_SUPPLY_HEALTH_GOOD = 1 }; + enum { POWER_SUPPLY_HEALTH_OVERHEAT = 2 }; + enum { POWER_SUPPLY_HEALTH_DEAD = 3 }; + enum { POWER_SUPPLY_HEALTH_OVERVOLTAGE = 4 }; + enum { POWER_SUPPLY_HEALTH_UNSPEC_FAILURE = 5 }; + enum { POWER_SUPPLY_HEALTH_COLD = 6 }; + enum { POWER_SUPPLY_HEALTH_WATCHDOG_TIMER_EXPIRE = 7 }; + enum { POWER_SUPPLY_HEALTH_SAFETY_TIMER_EXPIRE = 8 }; + enum { POWER_SUPPLY_TECHNOLOGY_UNKNOWN = 0 }; + enum { POWER_SUPPLY_TECHNOLOGY_NIMH = 1 }; + enum { POWER_SUPPLY_TECHNOLOGY_LION = 2 }; + enum { POWER_SUPPLY_TECHNOLOGY_LIPO = 3 }; + enum { POWER_SUPPLY_TECHNOLOGY_LIFE = 4 }; + enum { POWER_SUPPLY_TECHNOLOGY_NICD = 5 }; + enum { POWER_SUPPLY_TECHNOLOGY_LIMN = 6 }; + + BatteryState(): + header(), + voltage(0), + current(0), + charge(0), + capacity(0), + design_capacity(0), + percentage(0), + power_supply_status(0), + power_supply_health(0), + power_supply_technology(0), + present(0), + cell_voltage_length(0), cell_voltage(NULL), + location(""), + serial_number("") + { + } + + virtual int serialize(unsigned char *outbuffer) const + { + int offset = 0; + offset += this->header.serialize(outbuffer + offset); + union { + float real; + uint32_t base; + } u_voltage; + u_voltage.real = this->voltage; + *(outbuffer + offset + 0) = (u_voltage.base >> (8 * 0)) & 0xFF; + *(outbuffer + offset + 1) = (u_voltage.base >> (8 * 1)) & 0xFF; + *(outbuffer + offset + 2) = (u_voltage.base >> (8 * 2)) & 0xFF; + *(outbuffer + offset + 3) = (u_voltage.base >> (8 * 3)) & 0xFF; + offset += sizeof(this->voltage); + union { + float real; + uint32_t base; + } u_current; + u_current.real = this->current; + *(outbuffer + offset + 0) = (u_current.base >> (8 * 0)) & 0xFF; + *(outbuffer + offset + 1) = (u_current.base >> (8 * 1)) & 0xFF; + *(outbuffer + offset + 2) = (u_current.base >> (8 * 2)) & 0xFF; + *(outbuffer + offset + 3) = (u_current.base >> (8 * 3)) & 0xFF; + offset += sizeof(this->current); + union { + float real; + uint32_t base; + } u_charge; + u_charge.real = this->charge; + *(outbuffer + offset + 0) = (u_charge.base >> (8 * 0)) & 0xFF; + *(outbuffer + offset + 1) = (u_charge.base >> (8 * 1)) & 0xFF; + *(outbuffer + offset + 2) = (u_charge.base >> (8 * 2)) & 0xFF; + *(outbuffer + offset + 3) = (u_charge.base >> (8 * 3)) & 0xFF; + offset += sizeof(this->charge); + union { + float real; + uint32_t base; + } u_capacity; + u_capacity.real = this->capacity; + *(outbuffer + offset + 0) = (u_capacity.base >> (8 * 0)) & 0xFF; + *(outbuffer + offset + 1) = (u_capacity.base >> (8 * 1)) & 0xFF; + *(outbuffer + offset + 2) = (u_capacity.base >> (8 * 2)) & 0xFF; + *(outbuffer + offset + 3) = (u_capacity.base >> (8 * 3)) & 0xFF; + offset += sizeof(this->capacity); + union { + float real; + uint32_t base; + } u_design_capacity; + u_design_capacity.real = this->design_capacity; + *(outbuffer + offset + 0) = (u_design_capacity.base >> (8 * 0)) & 0xFF; + *(outbuffer + offset + 1) = (u_design_capacity.base >> (8 * 1)) & 0xFF; + *(outbuffer + offset + 2) = (u_design_capacity.base >> (8 * 2)) & 0xFF; + *(outbuffer + offset + 3) = (u_design_capacity.base >> (8 * 3)) & 0xFF; + offset += sizeof(this->design_capacity); + union { + float real; + uint32_t base; + } u_percentage; + u_percentage.real = this->percentage; + *(outbuffer + offset + 0) = (u_percentage.base >> (8 * 0)) & 0xFF; + *(outbuffer + offset + 1) = (u_percentage.base >> (8 * 1)) & 0xFF; + *(outbuffer + offset + 2) = (u_percentage.base >> (8 * 2)) & 0xFF; + *(outbuffer + offset + 3) = (u_percentage.base >> (8 * 3)) & 0xFF; + offset += sizeof(this->percentage); + *(outbuffer + offset + 0) = (this->power_supply_status >> (8 * 0)) & 0xFF; + offset += sizeof(this->power_supply_status); + *(outbuffer + offset + 0) = (this->power_supply_health >> (8 * 0)) & 0xFF; + offset += sizeof(this->power_supply_health); + *(outbuffer + offset + 0) = (this->power_supply_technology >> (8 * 0)) & 0xFF; + offset += sizeof(this->power_supply_technology); + union { + bool real; + uint8_t base; + } u_present; + u_present.real = this->present; + *(outbuffer + offset + 0) = (u_present.base >> (8 * 0)) & 0xFF; + offset += sizeof(this->present); + *(outbuffer + offset + 0) = (this->cell_voltage_length >> (8 * 0)) & 0xFF; + *(outbuffer + offset + 1) = (this->cell_voltage_length >> (8 * 1)) & 0xFF; + *(outbuffer + offset + 2) = (this->cell_voltage_length >> (8 * 2)) & 0xFF; + *(outbuffer + offset + 3) = (this->cell_voltage_length >> (8 * 3)) & 0xFF; + offset += sizeof(this->cell_voltage_length); + for( uint32_t i = 0; i < cell_voltage_length; i++){ + union { + float real; + uint32_t base; + } u_cell_voltagei; + u_cell_voltagei.real = this->cell_voltage[i]; + *(outbuffer + offset + 0) = (u_cell_voltagei.base >> (8 * 0)) & 0xFF; + *(outbuffer + offset + 1) = (u_cell_voltagei.base >> (8 * 1)) & 0xFF; + *(outbuffer + offset + 2) = (u_cell_voltagei.base >> (8 * 2)) & 0xFF; + *(outbuffer + offset + 3) = (u_cell_voltagei.base >> (8 * 3)) & 0xFF; + offset += sizeof(this->cell_voltage[i]); + } + uint32_t length_location = strlen(this->location); + memcpy(outbuffer + offset, &length_location, sizeof(uint32_t)); + offset += 4; + memcpy(outbuffer + offset, this->location, length_location); + offset += length_location; + uint32_t length_serial_number = strlen(this->serial_number); + memcpy(outbuffer + offset, &length_serial_number, sizeof(uint32_t)); + offset += 4; + memcpy(outbuffer + offset, this->serial_number, length_serial_number); + offset += length_serial_number; + return offset; + } + + virtual int deserialize(unsigned char *inbuffer) + { + int offset = 0; + offset += this->header.deserialize(inbuffer + offset); + union { + float real; + uint32_t base; + } u_voltage; + u_voltage.base = 0; + u_voltage.base |= ((uint32_t) (*(inbuffer + offset + 0))) << (8 * 0); + u_voltage.base |= ((uint32_t) (*(inbuffer + offset + 1))) << (8 * 1); + u_voltage.base |= ((uint32_t) (*(inbuffer + offset + 2))) << (8 * 2); + u_voltage.base |= ((uint32_t) (*(inbuffer + offset + 3))) << (8 * 3); + this->voltage = u_voltage.real; + offset += sizeof(this->voltage); + union { + float real; + uint32_t base; + } u_current; + u_current.base = 0; + u_current.base |= ((uint32_t) (*(inbuffer + offset + 0))) << (8 * 0); + u_current.base |= ((uint32_t) (*(inbuffer + offset + 1))) << (8 * 1); + u_current.base |= ((uint32_t) (*(inbuffer + offset + 2))) << (8 * 2); + u_current.base |= ((uint32_t) (*(inbuffer + offset + 3))) << (8 * 3); + this->current = u_current.real; + offset += sizeof(this->current); + union { + float real; + uint32_t base; + } u_charge; + u_charge.base = 0; + u_charge.base |= ((uint32_t) (*(inbuffer + offset + 0))) << (8 * 0); + u_charge.base |= ((uint32_t) (*(inbuffer + offset + 1))) << (8 * 1); + u_charge.base |= ((uint32_t) (*(inbuffer + offset + 2))) << (8 * 2); + u_charge.base |= ((uint32_t) (*(inbuffer + offset + 3))) << (8 * 3); + this->charge = u_charge.real; + offset += sizeof(this->charge); + union { + float real; + uint32_t base; + } u_capacity; + u_capacity.base = 0; + u_capacity.base |= ((uint32_t) (*(inbuffer + offset + 0))) << (8 * 0); + u_capacity.base |= ((uint32_t) (*(inbuffer + offset + 1))) << (8 * 1); + u_capacity.base |= ((uint32_t) (*(inbuffer + offset + 2))) << (8 * 2); + u_capacity.base |= ((uint32_t) (*(inbuffer + offset + 3))) << (8 * 3); + this->capacity = u_capacity.real; + offset += sizeof(this->capacity); + union { + float real; + uint32_t base; + } u_design_capacity; + u_design_capacity.base = 0; + u_design_capacity.base |= ((uint32_t) (*(inbuffer + offset + 0))) << (8 * 0); + u_design_capacity.base |= ((uint32_t) (*(inbuffer + offset + 1))) << (8 * 1); + u_design_capacity.base |= ((uint32_t) (*(inbuffer + offset + 2))) << (8 * 2); + u_design_capacity.base |= ((uint32_t) (*(inbuffer + offset + 3))) << (8 * 3); + this->design_capacity = u_design_capacity.real; + offset += sizeof(this->design_capacity); + union { + float real; + uint32_t base; + } u_percentage; + u_percentage.base = 0; + u_percentage.base |= ((uint32_t) (*(inbuffer + offset + 0))) << (8 * 0); + u_percentage.base |= ((uint32_t) (*(inbuffer + offset + 1))) << (8 * 1); + u_percentage.base |= ((uint32_t) (*(inbuffer + offset + 2))) << (8 * 2); + u_percentage.base |= ((uint32_t) (*(inbuffer + offset + 3))) << (8 * 3); + this->percentage = u_percentage.real; + offset += sizeof(this->percentage); + this->power_supply_status = ((uint8_t) (*(inbuffer + offset))); + offset += sizeof(this->power_supply_status); + this->power_supply_health = ((uint8_t) (*(inbuffer + offset))); + offset += sizeof(this->power_supply_health); + this->power_supply_technology = ((uint8_t) (*(inbuffer + offset))); + offset += sizeof(this->power_supply_technology); + union { + bool real; + uint8_t base; + } u_present; + u_present.base = 0; + u_present.base |= ((uint8_t) (*(inbuffer + offset + 0))) << (8 * 0); + this->present = u_present.real; + offset += sizeof(this->present); + uint32_t cell_voltage_lengthT = ((uint32_t) (*(inbuffer + offset))); + cell_voltage_lengthT |= ((uint32_t) (*(inbuffer + offset + 1))) << (8 * 1); + cell_voltage_lengthT |= ((uint32_t) (*(inbuffer + offset + 2))) << (8 * 2); + cell_voltage_lengthT |= ((uint32_t) (*(inbuffer + offset + 3))) << (8 * 3); + offset += sizeof(this->cell_voltage_length); + if(cell_voltage_lengthT > cell_voltage_length) + this->cell_voltage = (float*)realloc(this->cell_voltage, cell_voltage_lengthT * sizeof(float)); + cell_voltage_length = cell_voltage_lengthT; + for( uint32_t i = 0; i < cell_voltage_length; i++){ + union { + float real; + uint32_t base; + } u_st_cell_voltage; + u_st_cell_voltage.base = 0; + u_st_cell_voltage.base |= ((uint32_t) (*(inbuffer + offset + 0))) << (8 * 0); + u_st_cell_voltage.base |= ((uint32_t) (*(inbuffer + offset + 1))) << (8 * 1); + u_st_cell_voltage.base |= ((uint32_t) (*(inbuffer + offset + 2))) << (8 * 2); + u_st_cell_voltage.base |= ((uint32_t) (*(inbuffer + offset + 3))) << (8 * 3); + this->st_cell_voltage = u_st_cell_voltage.real; + offset += sizeof(this->st_cell_voltage); + memcpy( &(this->cell_voltage[i]), &(this->st_cell_voltage), sizeof(float)); + } + uint32_t length_location; + memcpy(&length_location, (inbuffer + offset), sizeof(uint32_t)); + offset += 4; + for(unsigned int k= offset; k< offset+length_location; ++k){ + inbuffer[k-1]=inbuffer[k]; + } + inbuffer[offset+length_location-1]=0; + this->location = (char *)(inbuffer + offset-1); + offset += length_location; + uint32_t length_serial_number; + memcpy(&length_serial_number, (inbuffer + offset), sizeof(uint32_t)); + offset += 4; + for(unsigned int k= offset; k< offset+length_serial_number; ++k){ + inbuffer[k-1]=inbuffer[k]; + } + inbuffer[offset+length_serial_number-1]=0; + this->serial_number = (char *)(inbuffer + offset-1); + offset += length_serial_number; + return offset; + } + + const char * getType(){ return "sensor_msgs/BatteryState"; }; + const char * getMD5(){ return "476f837fa6771f6e16e3bf4ef96f8770"; }; + + }; + +} +#endif \ No newline at end of file diff --git a/turtlebot3_core/libraries/ros_lib/sensor_msgs/CameraInfo.h b/turtlebot3_core/libraries/ros_lib/sensor_msgs/CameraInfo.h new file mode 100644 index 00000000..9bc1c0b1 --- /dev/null +++ b/turtlebot3_core/libraries/ros_lib/sensor_msgs/CameraInfo.h @@ -0,0 +1,160 @@ +#ifndef _ROS_sensor_msgs_CameraInfo_h +#define _ROS_sensor_msgs_CameraInfo_h + +#include +#include +#include +#include "ros/msg.h" +#include "std_msgs/Header.h" +#include "sensor_msgs/RegionOfInterest.h" + +namespace sensor_msgs +{ + + class CameraInfo : public ros::Msg + { + public: + std_msgs::Header header; + uint32_t height; + uint32_t width; + const char* distortion_model; + uint32_t D_length; + float st_D; + float * D; + float K[9]; + float R[9]; + float P[12]; + uint32_t binning_x; + uint32_t binning_y; + sensor_msgs::RegionOfInterest roi; + + CameraInfo(): + header(), + height(0), + width(0), + distortion_model(""), + D_length(0), D(NULL), + K(), + R(), + P(), + binning_x(0), + binning_y(0), + roi() + { + } + + virtual int serialize(unsigned char *outbuffer) const + { + int offset = 0; + offset += this->header.serialize(outbuffer + offset); + *(outbuffer + offset + 0) = (this->height >> (8 * 0)) & 0xFF; + *(outbuffer + offset + 1) = (this->height >> (8 * 1)) & 0xFF; + *(outbuffer + offset + 2) = (this->height >> (8 * 2)) & 0xFF; + *(outbuffer + offset + 3) = (this->height >> (8 * 3)) & 0xFF; + offset += sizeof(this->height); + *(outbuffer + offset + 0) = (this->width >> (8 * 0)) & 0xFF; + *(outbuffer + offset + 1) = (this->width >> (8 * 1)) & 0xFF; + *(outbuffer + offset + 2) = (this->width >> (8 * 2)) & 0xFF; + *(outbuffer + offset + 3) = (this->width >> (8 * 3)) & 0xFF; + offset += sizeof(this->width); + uint32_t length_distortion_model = strlen(this->distortion_model); + memcpy(outbuffer + offset, &length_distortion_model, sizeof(uint32_t)); + offset += 4; + memcpy(outbuffer + offset, this->distortion_model, length_distortion_model); + offset += length_distortion_model; + *(outbuffer + offset + 0) = (this->D_length >> (8 * 0)) & 0xFF; + *(outbuffer + offset + 1) = (this->D_length >> (8 * 1)) & 0xFF; + *(outbuffer + offset + 2) = (this->D_length >> (8 * 2)) & 0xFF; + *(outbuffer + offset + 3) = (this->D_length >> (8 * 3)) & 0xFF; + offset += sizeof(this->D_length); + for( uint32_t i = 0; i < D_length; i++){ + offset += serializeAvrFloat64(outbuffer + offset, this->D[i]); + } + for( uint32_t i = 0; i < 9; i++){ + offset += serializeAvrFloat64(outbuffer + offset, this->K[i]); + } + for( uint32_t i = 0; i < 9; i++){ + offset += serializeAvrFloat64(outbuffer + offset, this->R[i]); + } + for( uint32_t i = 0; i < 12; i++){ + offset += serializeAvrFloat64(outbuffer + offset, this->P[i]); + } + *(outbuffer + offset + 0) = (this->binning_x >> (8 * 0)) & 0xFF; + *(outbuffer + offset + 1) = (this->binning_x >> (8 * 1)) & 0xFF; + *(outbuffer + offset + 2) = (this->binning_x >> (8 * 2)) & 0xFF; + *(outbuffer + offset + 3) = (this->binning_x >> (8 * 3)) & 0xFF; + offset += sizeof(this->binning_x); + *(outbuffer + offset + 0) = (this->binning_y >> (8 * 0)) & 0xFF; + *(outbuffer + offset + 1) = (this->binning_y >> (8 * 1)) & 0xFF; + *(outbuffer + offset + 2) = (this->binning_y >> (8 * 2)) & 0xFF; + *(outbuffer + offset + 3) = (this->binning_y >> (8 * 3)) & 0xFF; + offset += sizeof(this->binning_y); + offset += this->roi.serialize(outbuffer + offset); + return offset; + } + + virtual int deserialize(unsigned char *inbuffer) + { + int offset = 0; + offset += this->header.deserialize(inbuffer + offset); + this->height = ((uint32_t) (*(inbuffer + offset))); + this->height |= ((uint32_t) (*(inbuffer + offset + 1))) << (8 * 1); + this->height |= ((uint32_t) (*(inbuffer + offset + 2))) << (8 * 2); + this->height |= ((uint32_t) (*(inbuffer + offset + 3))) << (8 * 3); + offset += sizeof(this->height); + this->width = ((uint32_t) (*(inbuffer + offset))); + this->width |= ((uint32_t) (*(inbuffer + offset + 1))) << (8 * 1); + this->width |= ((uint32_t) (*(inbuffer + offset + 2))) << (8 * 2); + this->width |= ((uint32_t) (*(inbuffer + offset + 3))) << (8 * 3); + offset += sizeof(this->width); + uint32_t length_distortion_model; + memcpy(&length_distortion_model, (inbuffer + offset), sizeof(uint32_t)); + offset += 4; + for(unsigned int k= offset; k< offset+length_distortion_model; ++k){ + inbuffer[k-1]=inbuffer[k]; + } + inbuffer[offset+length_distortion_model-1]=0; + this->distortion_model = (char *)(inbuffer + offset-1); + offset += length_distortion_model; + uint32_t D_lengthT = ((uint32_t) (*(inbuffer + offset))); + D_lengthT |= ((uint32_t) (*(inbuffer + offset + 1))) << (8 * 1); + D_lengthT |= ((uint32_t) (*(inbuffer + offset + 2))) << (8 * 2); + D_lengthT |= ((uint32_t) (*(inbuffer + offset + 3))) << (8 * 3); + offset += sizeof(this->D_length); + if(D_lengthT > D_length) + this->D = (float*)realloc(this->D, D_lengthT * sizeof(float)); + D_length = D_lengthT; + for( uint32_t i = 0; i < D_length; i++){ + offset += deserializeAvrFloat64(inbuffer + offset, &(this->st_D)); + memcpy( &(this->D[i]), &(this->st_D), sizeof(float)); + } + for( uint32_t i = 0; i < 9; i++){ + offset += deserializeAvrFloat64(inbuffer + offset, &(this->K[i])); + } + for( uint32_t i = 0; i < 9; i++){ + offset += deserializeAvrFloat64(inbuffer + offset, &(this->R[i])); + } + for( uint32_t i = 0; i < 12; i++){ + offset += deserializeAvrFloat64(inbuffer + offset, &(this->P[i])); + } + this->binning_x = ((uint32_t) (*(inbuffer + offset))); + this->binning_x |= ((uint32_t) (*(inbuffer + offset + 1))) << (8 * 1); + this->binning_x |= ((uint32_t) (*(inbuffer + offset + 2))) << (8 * 2); + this->binning_x |= ((uint32_t) (*(inbuffer + offset + 3))) << (8 * 3); + offset += sizeof(this->binning_x); + this->binning_y = ((uint32_t) (*(inbuffer + offset))); + this->binning_y |= ((uint32_t) (*(inbuffer + offset + 1))) << (8 * 1); + this->binning_y |= ((uint32_t) (*(inbuffer + offset + 2))) << (8 * 2); + this->binning_y |= ((uint32_t) (*(inbuffer + offset + 3))) << (8 * 3); + offset += sizeof(this->binning_y); + offset += this->roi.deserialize(inbuffer + offset); + return offset; + } + + const char * getType(){ return "sensor_msgs/CameraInfo"; }; + const char * getMD5(){ return "c9a58c1b0b154e0e6da7578cb991d214"; }; + + }; + +} +#endif \ No newline at end of file diff --git a/turtlebot3_core/libraries/ros_lib/sensor_msgs/ChannelFloat32.h b/turtlebot3_core/libraries/ros_lib/sensor_msgs/ChannelFloat32.h new file mode 100644 index 00000000..60b733b9 --- /dev/null +++ b/turtlebot3_core/libraries/ros_lib/sensor_msgs/ChannelFloat32.h @@ -0,0 +1,97 @@ +#ifndef _ROS_sensor_msgs_ChannelFloat32_h +#define _ROS_sensor_msgs_ChannelFloat32_h + +#include +#include +#include +#include "ros/msg.h" + +namespace sensor_msgs +{ + + class ChannelFloat32 : public ros::Msg + { + public: + const char* name; + uint32_t values_length; + float st_values; + float * values; + + ChannelFloat32(): + name(""), + values_length(0), values(NULL) + { + } + + virtual int serialize(unsigned char *outbuffer) const + { + int offset = 0; + uint32_t length_name = strlen(this->name); + memcpy(outbuffer + offset, &length_name, sizeof(uint32_t)); + offset += 4; + memcpy(outbuffer + offset, this->name, length_name); + offset += length_name; + *(outbuffer + offset + 0) = (this->values_length >> (8 * 0)) & 0xFF; + *(outbuffer + offset + 1) = (this->values_length >> (8 * 1)) & 0xFF; + *(outbuffer + offset + 2) = (this->values_length >> (8 * 2)) & 0xFF; + *(outbuffer + offset + 3) = (this->values_length >> (8 * 3)) & 0xFF; + offset += sizeof(this->values_length); + for( uint32_t i = 0; i < values_length; i++){ + union { + float real; + uint32_t base; + } u_valuesi; + u_valuesi.real = this->values[i]; + *(outbuffer + offset + 0) = (u_valuesi.base >> (8 * 0)) & 0xFF; + *(outbuffer + offset + 1) = (u_valuesi.base >> (8 * 1)) & 0xFF; + *(outbuffer + offset + 2) = (u_valuesi.base >> (8 * 2)) & 0xFF; + *(outbuffer + offset + 3) = (u_valuesi.base >> (8 * 3)) & 0xFF; + offset += sizeof(this->values[i]); + } + return offset; + } + + virtual int deserialize(unsigned char *inbuffer) + { + int offset = 0; + uint32_t length_name; + memcpy(&length_name, (inbuffer + offset), sizeof(uint32_t)); + offset += 4; + for(unsigned int k= offset; k< offset+length_name; ++k){ + inbuffer[k-1]=inbuffer[k]; + } + inbuffer[offset+length_name-1]=0; + this->name = (char *)(inbuffer + offset-1); + offset += length_name; + uint32_t values_lengthT = ((uint32_t) (*(inbuffer + offset))); + values_lengthT |= ((uint32_t) (*(inbuffer + offset + 1))) << (8 * 1); + values_lengthT |= ((uint32_t) (*(inbuffer + offset + 2))) << (8 * 2); + values_lengthT |= ((uint32_t) (*(inbuffer + offset + 3))) << (8 * 3); + offset += sizeof(this->values_length); + if(values_lengthT > values_length) + this->values = (float*)realloc(this->values, values_lengthT * sizeof(float)); + values_length = values_lengthT; + for( uint32_t i = 0; i < values_length; i++){ + union { + float real; + uint32_t base; + } u_st_values; + u_st_values.base = 0; + u_st_values.base |= ((uint32_t) (*(inbuffer + offset + 0))) << (8 * 0); + u_st_values.base |= ((uint32_t) (*(inbuffer + offset + 1))) << (8 * 1); + u_st_values.base |= ((uint32_t) (*(inbuffer + offset + 2))) << (8 * 2); + u_st_values.base |= ((uint32_t) (*(inbuffer + offset + 3))) << (8 * 3); + this->st_values = u_st_values.real; + offset += sizeof(this->st_values); + memcpy( &(this->values[i]), &(this->st_values), sizeof(float)); + } + return offset; + } + + const char * getType(){ return "sensor_msgs/ChannelFloat32"; }; + const char * getMD5(){ return "3d40139cdd33dfedcb71ffeeeb42ae7f"; }; + + }; + +} +#endif \ No newline at end of file diff --git a/turtlebot3_core/libraries/ros_lib/sensor_msgs/CompressedImage.h b/turtlebot3_core/libraries/ros_lib/sensor_msgs/CompressedImage.h new file mode 100644 index 00000000..e023839a --- /dev/null +++ b/turtlebot3_core/libraries/ros_lib/sensor_msgs/CompressedImage.h @@ -0,0 +1,85 @@ +#ifndef _ROS_sensor_msgs_CompressedImage_h +#define _ROS_sensor_msgs_CompressedImage_h + +#include +#include +#include +#include "ros/msg.h" +#include "std_msgs/Header.h" + +namespace sensor_msgs +{ + + class CompressedImage : public ros::Msg + { + public: + std_msgs::Header header; + const char* format; + uint32_t data_length; + uint8_t st_data; + uint8_t * data; + + CompressedImage(): + header(), + format(""), + data_length(0), data(NULL) + { + } + + virtual int serialize(unsigned char *outbuffer) const + { + int offset = 0; + offset += this->header.serialize(outbuffer + offset); + uint32_t length_format = strlen(this->format); + memcpy(outbuffer + offset, &length_format, sizeof(uint32_t)); + offset += 4; + memcpy(outbuffer + offset, this->format, length_format); + offset += length_format; + *(outbuffer + offset + 0) = (this->data_length >> (8 * 0)) & 0xFF; + *(outbuffer + offset + 1) = (this->data_length >> (8 * 1)) & 0xFF; + *(outbuffer + offset + 2) = (this->data_length >> (8 * 2)) & 0xFF; + *(outbuffer + offset + 3) = (this->data_length >> (8 * 3)) & 0xFF; + offset += sizeof(this->data_length); + for( uint32_t i = 0; i < data_length; i++){ + *(outbuffer + offset + 0) = (this->data[i] >> (8 * 0)) & 0xFF; + offset += sizeof(this->data[i]); + } + return offset; + } + + virtual int deserialize(unsigned char *inbuffer) + { + int offset = 0; + offset += this->header.deserialize(inbuffer + offset); + uint32_t length_format; + memcpy(&length_format, (inbuffer + offset), sizeof(uint32_t)); + offset += 4; + for(unsigned int k= offset; k< offset+length_format; ++k){ + inbuffer[k-1]=inbuffer[k]; + } + inbuffer[offset+length_format-1]=0; + this->format = (char *)(inbuffer + offset-1); + offset += length_format; + uint32_t data_lengthT = ((uint32_t) (*(inbuffer + offset))); + data_lengthT |= ((uint32_t) (*(inbuffer + offset + 1))) << (8 * 1); + data_lengthT |= ((uint32_t) (*(inbuffer + offset + 2))) << (8 * 2); + data_lengthT |= ((uint32_t) (*(inbuffer + offset + 3))) << (8 * 3); + offset += sizeof(this->data_length); + if(data_lengthT > data_length) + this->data = (uint8_t*)realloc(this->data, data_lengthT * sizeof(uint8_t)); + data_length = data_lengthT; + for( uint32_t i = 0; i < data_length; i++){ + this->st_data = ((uint8_t) (*(inbuffer + offset))); + offset += sizeof(this->st_data); + memcpy( &(this->data[i]), &(this->st_data), sizeof(uint8_t)); + } + return offset; + } + + const char * getType(){ return "sensor_msgs/CompressedImage"; }; + const char * getMD5(){ return "8f7a12909da2c9d3332d540a0977563f"; }; + + }; + +} +#endif \ No newline at end of file diff --git a/turtlebot3_core/libraries/ros_lib/sensor_msgs/FluidPressure.h b/turtlebot3_core/libraries/ros_lib/sensor_msgs/FluidPressure.h new file mode 100644 index 00000000..b24e2115 --- /dev/null +++ b/turtlebot3_core/libraries/ros_lib/sensor_msgs/FluidPressure.h @@ -0,0 +1,51 @@ +#ifndef _ROS_sensor_msgs_FluidPressure_h +#define _ROS_sensor_msgs_FluidPressure_h + +#include +#include +#include +#include "ros/msg.h" +#include "std_msgs/Header.h" + +namespace sensor_msgs +{ + + class FluidPressure : public ros::Msg + { + public: + std_msgs::Header header; + float fluid_pressure; + float variance; + + FluidPressure(): + header(), + fluid_pressure(0), + variance(0) + { + } + + virtual int serialize(unsigned char *outbuffer) const + { + int offset = 0; + offset += this->header.serialize(outbuffer + offset); + offset += serializeAvrFloat64(outbuffer + offset, this->fluid_pressure); + offset += serializeAvrFloat64(outbuffer + offset, this->variance); + return offset; + } + + virtual int deserialize(unsigned char *inbuffer) + { + int offset = 0; + offset += this->header.deserialize(inbuffer + offset); + offset += deserializeAvrFloat64(inbuffer + offset, &(this->fluid_pressure)); + offset += deserializeAvrFloat64(inbuffer + offset, &(this->variance)); + return offset; + } + + const char * getType(){ return "sensor_msgs/FluidPressure"; }; + const char * getMD5(){ return "804dc5cea1c5306d6a2eb80b9833befe"; }; + + }; + +} +#endif \ No newline at end of file diff --git a/turtlebot3_core/libraries/ros_lib/sensor_msgs/Illuminance.h b/turtlebot3_core/libraries/ros_lib/sensor_msgs/Illuminance.h new file mode 100644 index 00000000..7e217953 --- /dev/null +++ b/turtlebot3_core/libraries/ros_lib/sensor_msgs/Illuminance.h @@ -0,0 +1,51 @@ +#ifndef _ROS_sensor_msgs_Illuminance_h +#define _ROS_sensor_msgs_Illuminance_h + +#include +#include +#include +#include "ros/msg.h" +#include "std_msgs/Header.h" + +namespace sensor_msgs +{ + + class Illuminance : public ros::Msg + { + public: + std_msgs::Header header; + float illuminance; + float variance; + + Illuminance(): + header(), + illuminance(0), + variance(0) + { + } + + virtual int serialize(unsigned char *outbuffer) const + { + int offset = 0; + offset += this->header.serialize(outbuffer + offset); + offset += serializeAvrFloat64(outbuffer + offset, this->illuminance); + offset += serializeAvrFloat64(outbuffer + offset, this->variance); + return offset; + } + + virtual int deserialize(unsigned char *inbuffer) + { + int offset = 0; + offset += this->header.deserialize(inbuffer + offset); + offset += deserializeAvrFloat64(inbuffer + offset, &(this->illuminance)); + offset += deserializeAvrFloat64(inbuffer + offset, &(this->variance)); + return offset; + } + + const char * getType(){ return "sensor_msgs/Illuminance"; }; + const char * getMD5(){ return "8cf5febb0952fca9d650c3d11a81a188"; }; + + }; + +} +#endif \ No newline at end of file diff --git a/turtlebot3_core/libraries/ros_lib/sensor_msgs/Image.h b/turtlebot3_core/libraries/ros_lib/sensor_msgs/Image.h new file mode 100644 index 00000000..2a0aced9 --- /dev/null +++ b/turtlebot3_core/libraries/ros_lib/sensor_msgs/Image.h @@ -0,0 +1,127 @@ +#ifndef _ROS_sensor_msgs_Image_h +#define _ROS_sensor_msgs_Image_h + +#include +#include +#include +#include "ros/msg.h" +#include "std_msgs/Header.h" + +namespace sensor_msgs +{ + + class Image : public ros::Msg + { + public: + std_msgs::Header header; + uint32_t height; + uint32_t width; + const char* encoding; + uint8_t is_bigendian; + uint32_t step; + uint32_t data_length; + uint8_t st_data; + uint8_t * data; + + Image(): + header(), + height(0), + width(0), + encoding(""), + is_bigendian(0), + step(0), + data_length(0), data(NULL) + { + } + + virtual int serialize(unsigned char *outbuffer) const + { + int offset = 0; + offset += this->header.serialize(outbuffer + offset); + *(outbuffer + offset + 0) = (this->height >> (8 * 0)) & 0xFF; + *(outbuffer + offset + 1) = (this->height >> (8 * 1)) & 0xFF; + *(outbuffer + offset + 2) = (this->height >> (8 * 2)) & 0xFF; + *(outbuffer + offset + 3) = (this->height >> (8 * 3)) & 0xFF; + offset += sizeof(this->height); + *(outbuffer + offset + 0) = (this->width >> (8 * 0)) & 0xFF; + *(outbuffer + offset + 1) = (this->width >> (8 * 1)) & 0xFF; + *(outbuffer + offset + 2) = (this->width >> (8 * 2)) & 0xFF; + *(outbuffer + offset + 3) = (this->width >> (8 * 3)) & 0xFF; + offset += sizeof(this->width); + uint32_t length_encoding = strlen(this->encoding); + memcpy(outbuffer + offset, &length_encoding, sizeof(uint32_t)); + offset += 4; + memcpy(outbuffer + offset, this->encoding, length_encoding); + offset += length_encoding; + *(outbuffer + offset + 0) = (this->is_bigendian >> (8 * 0)) & 0xFF; + offset += sizeof(this->is_bigendian); + *(outbuffer + offset + 0) = (this->step >> (8 * 0)) & 0xFF; + *(outbuffer + offset + 1) = (this->step >> (8 * 1)) & 0xFF; + *(outbuffer + offset + 2) = (this->step >> (8 * 2)) & 0xFF; + *(outbuffer + offset + 3) = (this->step >> (8 * 3)) & 0xFF; + offset += sizeof(this->step); + *(outbuffer + offset + 0) = (this->data_length >> (8 * 0)) & 0xFF; + *(outbuffer + offset + 1) = (this->data_length >> (8 * 1)) & 0xFF; + *(outbuffer + offset + 2) = (this->data_length >> (8 * 2)) & 0xFF; + *(outbuffer + offset + 3) = (this->data_length >> (8 * 3)) & 0xFF; + offset += sizeof(this->data_length); + for( uint32_t i = 0; i < data_length; i++){ + *(outbuffer + offset + 0) = (this->data[i] >> (8 * 0)) & 0xFF; + offset += sizeof(this->data[i]); + } + return offset; + } + + virtual int deserialize(unsigned char *inbuffer) + { + int offset = 0; + offset += this->header.deserialize(inbuffer + offset); + this->height = ((uint32_t) (*(inbuffer + offset))); + this->height |= ((uint32_t) (*(inbuffer + offset + 1))) << (8 * 1); + this->height |= ((uint32_t) (*(inbuffer + offset + 2))) << (8 * 2); + this->height |= ((uint32_t) (*(inbuffer + offset + 3))) << (8 * 3); + offset += sizeof(this->height); + this->width = ((uint32_t) (*(inbuffer + offset))); + this->width |= ((uint32_t) (*(inbuffer + offset + 1))) << (8 * 1); + this->width |= ((uint32_t) (*(inbuffer + offset + 2))) << (8 * 2); + this->width |= ((uint32_t) (*(inbuffer + offset + 3))) << (8 * 3); + offset += sizeof(this->width); + uint32_t length_encoding; + memcpy(&length_encoding, (inbuffer + offset), sizeof(uint32_t)); + offset += 4; + for(unsigned int k= offset; k< offset+length_encoding; ++k){ + inbuffer[k-1]=inbuffer[k]; + } + inbuffer[offset+length_encoding-1]=0; + this->encoding = (char *)(inbuffer + offset-1); + offset += length_encoding; + this->is_bigendian = ((uint8_t) (*(inbuffer + offset))); + offset += sizeof(this->is_bigendian); + this->step = ((uint32_t) (*(inbuffer + offset))); + this->step |= ((uint32_t) (*(inbuffer + offset + 1))) << (8 * 1); + this->step |= ((uint32_t) (*(inbuffer + offset + 2))) << (8 * 2); + this->step |= ((uint32_t) (*(inbuffer + offset + 3))) << (8 * 3); + offset += sizeof(this->step); + uint32_t data_lengthT = ((uint32_t) (*(inbuffer + offset))); + data_lengthT |= ((uint32_t) (*(inbuffer + offset + 1))) << (8 * 1); + data_lengthT |= ((uint32_t) (*(inbuffer + offset + 2))) << (8 * 2); + data_lengthT |= ((uint32_t) (*(inbuffer + offset + 3))) << (8 * 3); + offset += sizeof(this->data_length); + if(data_lengthT > data_length) + this->data = (uint8_t*)realloc(this->data, data_lengthT * sizeof(uint8_t)); + data_length = data_lengthT; + for( uint32_t i = 0; i < data_length; i++){ + this->st_data = ((uint8_t) (*(inbuffer + offset))); + offset += sizeof(this->st_data); + memcpy( &(this->data[i]), &(this->st_data), sizeof(uint8_t)); + } + return offset; + } + + const char * getType(){ return "sensor_msgs/Image"; }; + const char * getMD5(){ return "060021388200f6f0f447d0fcd9c64743"; }; + + }; + +} +#endif \ No newline at end of file diff --git a/turtlebot3_core/libraries/ros_lib/sensor_msgs/Imu.h b/turtlebot3_core/libraries/ros_lib/sensor_msgs/Imu.h new file mode 100644 index 00000000..1beada5f --- /dev/null +++ b/turtlebot3_core/libraries/ros_lib/sensor_msgs/Imu.h @@ -0,0 +1,81 @@ +#ifndef _ROS_sensor_msgs_Imu_h +#define _ROS_sensor_msgs_Imu_h + +#include +#include +#include +#include "ros/msg.h" +#include "std_msgs/Header.h" +#include "geometry_msgs/Quaternion.h" +#include "geometry_msgs/Vector3.h" + +namespace sensor_msgs +{ + + class Imu : public ros::Msg + { + public: + std_msgs::Header header; + geometry_msgs::Quaternion orientation; + float orientation_covariance[9]; + geometry_msgs::Vector3 angular_velocity; + float angular_velocity_covariance[9]; + geometry_msgs::Vector3 linear_acceleration; + float linear_acceleration_covariance[9]; + + Imu(): + header(), + orientation(), + orientation_covariance(), + angular_velocity(), + angular_velocity_covariance(), + linear_acceleration(), + linear_acceleration_covariance() + { + } + + virtual int serialize(unsigned char *outbuffer) const + { + int offset = 0; + offset += this->header.serialize(outbuffer + offset); + offset += this->orientation.serialize(outbuffer + offset); + for( uint32_t i = 0; i < 9; i++){ + offset += serializeAvrFloat64(outbuffer + offset, this->orientation_covariance[i]); + } + offset += this->angular_velocity.serialize(outbuffer + offset); + for( uint32_t i = 0; i < 9; i++){ + offset += serializeAvrFloat64(outbuffer + offset, this->angular_velocity_covariance[i]); + } + offset += this->linear_acceleration.serialize(outbuffer + offset); + for( uint32_t i = 0; i < 9; i++){ + offset += serializeAvrFloat64(outbuffer + offset, this->linear_acceleration_covariance[i]); + } + return offset; + } + + virtual int deserialize(unsigned char *inbuffer) + { + int offset = 0; + offset += this->header.deserialize(inbuffer + offset); + offset += this->orientation.deserialize(inbuffer + offset); + for( uint32_t i = 0; i < 9; i++){ + offset += deserializeAvrFloat64(inbuffer + offset, &(this->orientation_covariance[i])); + } + offset += this->angular_velocity.deserialize(inbuffer + offset); + for( uint32_t i = 0; i < 9; i++){ + offset += deserializeAvrFloat64(inbuffer + offset, &(this->angular_velocity_covariance[i])); + } + offset += this->linear_acceleration.deserialize(inbuffer + offset); + for( uint32_t i = 0; i < 9; i++){ + offset += deserializeAvrFloat64(inbuffer + offset, &(this->linear_acceleration_covariance[i])); + } + return offset; + } + + const char * getType(){ return "sensor_msgs/Imu"; }; + const char * getMD5(){ return "6a62c6daae103f4ff57a132d6f95cec2"; }; + + }; + +} +#endif \ No newline at end of file diff --git a/turtlebot3_core/libraries/ros_lib/sensor_msgs/JointState.h b/turtlebot3_core/libraries/ros_lib/sensor_msgs/JointState.h new file mode 100644 index 00000000..841d2d10 --- /dev/null +++ b/turtlebot3_core/libraries/ros_lib/sensor_msgs/JointState.h @@ -0,0 +1,151 @@ +#ifndef _ROS_sensor_msgs_JointState_h +#define _ROS_sensor_msgs_JointState_h + +#include +#include +#include +#include "ros/msg.h" +#include "std_msgs/Header.h" + +namespace sensor_msgs +{ + + class JointState : public ros::Msg + { + public: + std_msgs::Header header; + uint32_t name_length; + char* st_name; + char* * name; + uint32_t position_length; + float st_position; + float * position; + uint32_t velocity_length; + float st_velocity; + float * velocity; + uint32_t effort_length; + float st_effort; + float * effort; + + JointState(): + header(), + name_length(0), name(NULL), + position_length(0), position(NULL), + velocity_length(0), velocity(NULL), + effort_length(0), effort(NULL) + { + } + + virtual int serialize(unsigned char *outbuffer) const + { + int offset = 0; + offset += this->header.serialize(outbuffer + offset); + *(outbuffer + offset + 0) = (this->name_length >> (8 * 0)) & 0xFF; + *(outbuffer + offset + 1) = (this->name_length >> (8 * 1)) & 0xFF; + *(outbuffer + offset + 2) = (this->name_length >> (8 * 2)) & 0xFF; + *(outbuffer + offset + 3) = (this->name_length >> (8 * 3)) & 0xFF; + offset += sizeof(this->name_length); + for( uint32_t i = 0; i < name_length; i++){ + uint32_t length_namei = strlen(this->name[i]); + memcpy(outbuffer + offset, &length_namei, sizeof(uint32_t)); + offset += 4; + memcpy(outbuffer + offset, this->name[i], length_namei); + offset += length_namei; + } + *(outbuffer + offset + 0) = (this->position_length >> (8 * 0)) & 0xFF; + *(outbuffer + offset + 1) = (this->position_length >> (8 * 1)) & 0xFF; + *(outbuffer + offset + 2) = (this->position_length >> (8 * 2)) & 0xFF; + *(outbuffer + offset + 3) = (this->position_length >> (8 * 3)) & 0xFF; + offset += sizeof(this->position_length); + for( uint32_t i = 0; i < position_length; i++){ + offset += serializeAvrFloat64(outbuffer + offset, this->position[i]); + } + *(outbuffer + offset + 0) = (this->velocity_length >> (8 * 0)) & 0xFF; + *(outbuffer + offset + 1) = (this->velocity_length >> (8 * 1)) & 0xFF; + *(outbuffer + offset + 2) = (this->velocity_length >> (8 * 2)) & 0xFF; + *(outbuffer + offset + 3) = (this->velocity_length >> (8 * 3)) & 0xFF; + offset += sizeof(this->velocity_length); + for( uint32_t i = 0; i < velocity_length; i++){ + offset += serializeAvrFloat64(outbuffer + offset, this->velocity[i]); + } + *(outbuffer + offset + 0) = (this->effort_length >> (8 * 0)) & 0xFF; + *(outbuffer + offset + 1) = (this->effort_length >> (8 * 1)) & 0xFF; + *(outbuffer + offset + 2) = (this->effort_length >> (8 * 2)) & 0xFF; + *(outbuffer + offset + 3) = (this->effort_length >> (8 * 3)) & 0xFF; + offset += sizeof(this->effort_length); + for( uint32_t i = 0; i < effort_length; i++){ + offset += serializeAvrFloat64(outbuffer + offset, this->effort[i]); + } + return offset; + } + + virtual int deserialize(unsigned char *inbuffer) + { + int offset = 0; + offset += this->header.deserialize(inbuffer + offset); + uint32_t name_lengthT = ((uint32_t) (*(inbuffer + offset))); + name_lengthT |= ((uint32_t) (*(inbuffer + offset + 1))) << (8 * 1); + name_lengthT |= ((uint32_t) (*(inbuffer + offset + 2))) << (8 * 2); + name_lengthT |= ((uint32_t) (*(inbuffer + offset + 3))) << (8 * 3); + offset += sizeof(this->name_length); + if(name_lengthT > name_length) + this->name = (char**)realloc(this->name, name_lengthT * sizeof(char*)); + name_length = name_lengthT; + for( uint32_t i = 0; i < name_length; i++){ + uint32_t length_st_name; + memcpy(&length_st_name, (inbuffer + offset), sizeof(uint32_t)); + offset += 4; + for(unsigned int k= offset; k< offset+length_st_name; ++k){ + inbuffer[k-1]=inbuffer[k]; + } + inbuffer[offset+length_st_name-1]=0; + this->st_name = (char *)(inbuffer + offset-1); + offset += length_st_name; + memcpy( &(this->name[i]), &(this->st_name), sizeof(char*)); + } + uint32_t position_lengthT = ((uint32_t) (*(inbuffer + offset))); + position_lengthT |= ((uint32_t) (*(inbuffer + offset + 1))) << (8 * 1); + position_lengthT |= ((uint32_t) (*(inbuffer + offset + 2))) << (8 * 2); + position_lengthT |= ((uint32_t) (*(inbuffer + offset + 3))) << (8 * 3); + offset += sizeof(this->position_length); + if(position_lengthT > position_length) + this->position = (float*)realloc(this->position, position_lengthT * sizeof(float)); + position_length = position_lengthT; + for( uint32_t i = 0; i < position_length; i++){ + offset += deserializeAvrFloat64(inbuffer + offset, &(this->st_position)); + memcpy( &(this->position[i]), &(this->st_position), sizeof(float)); + } + uint32_t velocity_lengthT = ((uint32_t) (*(inbuffer + offset))); + velocity_lengthT |= ((uint32_t) (*(inbuffer + offset + 1))) << (8 * 1); + velocity_lengthT |= ((uint32_t) (*(inbuffer + offset + 2))) << (8 * 2); + velocity_lengthT |= ((uint32_t) (*(inbuffer + offset + 3))) << (8 * 3); + offset += sizeof(this->velocity_length); + if(velocity_lengthT > velocity_length) + this->velocity = (float*)realloc(this->velocity, velocity_lengthT * sizeof(float)); + velocity_length = velocity_lengthT; + for( uint32_t i = 0; i < velocity_length; i++){ + offset += deserializeAvrFloat64(inbuffer + offset, &(this->st_velocity)); + memcpy( &(this->velocity[i]), &(this->st_velocity), sizeof(float)); + } + uint32_t effort_lengthT = ((uint32_t) (*(inbuffer + offset))); + effort_lengthT |= ((uint32_t) (*(inbuffer + offset + 1))) << (8 * 1); + effort_lengthT |= ((uint32_t) (*(inbuffer + offset + 2))) << (8 * 2); + effort_lengthT |= ((uint32_t) (*(inbuffer + offset + 3))) << (8 * 3); + offset += sizeof(this->effort_length); + if(effort_lengthT > effort_length) + this->effort = (float*)realloc(this->effort, effort_lengthT * sizeof(float)); + effort_length = effort_lengthT; + for( uint32_t i = 0; i < effort_length; i++){ + offset += deserializeAvrFloat64(inbuffer + offset, &(this->st_effort)); + memcpy( &(this->effort[i]), &(this->st_effort), sizeof(float)); + } + return offset; + } + + const char * getType(){ return "sensor_msgs/JointState"; }; + const char * getMD5(){ return "3066dcd76a6cfaef579bd0f34173e9fd"; }; + + }; + +} +#endif \ No newline at end of file diff --git a/turtlebot3_core/libraries/ros_lib/sensor_msgs/Joy.h b/turtlebot3_core/libraries/ros_lib/sensor_msgs/Joy.h new file mode 100644 index 00000000..f186db0c --- /dev/null +++ b/turtlebot3_core/libraries/ros_lib/sensor_msgs/Joy.h @@ -0,0 +1,129 @@ +#ifndef _ROS_sensor_msgs_Joy_h +#define _ROS_sensor_msgs_Joy_h + +#include +#include +#include +#include "ros/msg.h" +#include "std_msgs/Header.h" + +namespace sensor_msgs +{ + + class Joy : public ros::Msg + { + public: + std_msgs::Header header; + uint32_t axes_length; + float st_axes; + float * axes; + uint32_t buttons_length; + int32_t st_buttons; + int32_t * buttons; + + Joy(): + header(), + axes_length(0), axes(NULL), + buttons_length(0), buttons(NULL) + { + } + + virtual int serialize(unsigned char *outbuffer) const + { + int offset = 0; + offset += this->header.serialize(outbuffer + offset); + *(outbuffer + offset + 0) = (this->axes_length >> (8 * 0)) & 0xFF; + *(outbuffer + offset + 1) = (this->axes_length >> (8 * 1)) & 0xFF; + *(outbuffer + offset + 2) = (this->axes_length >> (8 * 2)) & 0xFF; + *(outbuffer + offset + 3) = (this->axes_length >> (8 * 3)) & 0xFF; + offset += sizeof(this->axes_length); + for( uint32_t i = 0; i < axes_length; i++){ + union { + float real; + uint32_t base; + } u_axesi; + u_axesi.real = this->axes[i]; + *(outbuffer + offset + 0) = (u_axesi.base >> (8 * 0)) & 0xFF; + *(outbuffer + offset + 1) = (u_axesi.base >> (8 * 1)) & 0xFF; + *(outbuffer + offset + 2) = (u_axesi.base >> (8 * 2)) & 0xFF; + *(outbuffer + offset + 3) = (u_axesi.base >> (8 * 3)) & 0xFF; + offset += sizeof(this->axes[i]); + } + *(outbuffer + offset + 0) = (this->buttons_length >> (8 * 0)) & 0xFF; + *(outbuffer + offset + 1) = (this->buttons_length >> (8 * 1)) & 0xFF; + *(outbuffer + offset + 2) = (this->buttons_length >> (8 * 2)) & 0xFF; + *(outbuffer + offset + 3) = (this->buttons_length >> (8 * 3)) & 0xFF; + offset += sizeof(this->buttons_length); + for( uint32_t i = 0; i < buttons_length; i++){ + union { + int32_t real; + uint32_t base; + } u_buttonsi; + u_buttonsi.real = this->buttons[i]; + *(outbuffer + offset + 0) = (u_buttonsi.base >> (8 * 0)) & 0xFF; + *(outbuffer + offset + 1) = (u_buttonsi.base >> (8 * 1)) & 0xFF; + *(outbuffer + offset + 2) = (u_buttonsi.base >> (8 * 2)) & 0xFF; + *(outbuffer + offset + 3) = (u_buttonsi.base >> (8 * 3)) & 0xFF; + offset += sizeof(this->buttons[i]); + } + return offset; + } + + virtual int deserialize(unsigned char *inbuffer) + { + int offset = 0; + offset += this->header.deserialize(inbuffer + offset); + uint32_t axes_lengthT = ((uint32_t) (*(inbuffer + offset))); + axes_lengthT |= ((uint32_t) (*(inbuffer + offset + 1))) << (8 * 1); + axes_lengthT |= ((uint32_t) (*(inbuffer + offset + 2))) << (8 * 2); + axes_lengthT |= ((uint32_t) (*(inbuffer + offset + 3))) << (8 * 3); + offset += sizeof(this->axes_length); + if(axes_lengthT > axes_length) + this->axes = (float*)realloc(this->axes, axes_lengthT * sizeof(float)); + axes_length = axes_lengthT; + for( uint32_t i = 0; i < axes_length; i++){ + union { + float real; + uint32_t base; + } u_st_axes; + u_st_axes.base = 0; + u_st_axes.base |= ((uint32_t) (*(inbuffer + offset + 0))) << (8 * 0); + u_st_axes.base |= ((uint32_t) (*(inbuffer + offset + 1))) << (8 * 1); + u_st_axes.base |= ((uint32_t) (*(inbuffer + offset + 2))) << (8 * 2); + u_st_axes.base |= ((uint32_t) (*(inbuffer + offset + 3))) << (8 * 3); + this->st_axes = u_st_axes.real; + offset += sizeof(this->st_axes); + memcpy( &(this->axes[i]), &(this->st_axes), sizeof(float)); + } + uint32_t buttons_lengthT = ((uint32_t) (*(inbuffer + offset))); + buttons_lengthT |= ((uint32_t) (*(inbuffer + offset + 1))) << (8 * 1); + buttons_lengthT |= ((uint32_t) (*(inbuffer + offset + 2))) << (8 * 2); + buttons_lengthT |= ((uint32_t) (*(inbuffer + offset + 3))) << (8 * 3); + offset += sizeof(this->buttons_length); + if(buttons_lengthT > buttons_length) + this->buttons = (int32_t*)realloc(this->buttons, buttons_lengthT * sizeof(int32_t)); + buttons_length = buttons_lengthT; + for( uint32_t i = 0; i < buttons_length; i++){ + union { + int32_t real; + uint32_t base; + } u_st_buttons; + u_st_buttons.base = 0; + u_st_buttons.base |= ((uint32_t) (*(inbuffer + offset + 0))) << (8 * 0); + u_st_buttons.base |= ((uint32_t) (*(inbuffer + offset + 1))) << (8 * 1); + u_st_buttons.base |= ((uint32_t) (*(inbuffer + offset + 2))) << (8 * 2); + u_st_buttons.base |= ((uint32_t) (*(inbuffer + offset + 3))) << (8 * 3); + this->st_buttons = u_st_buttons.real; + offset += sizeof(this->st_buttons); + memcpy( &(this->buttons[i]), &(this->st_buttons), sizeof(int32_t)); + } + return offset; + } + + const char * getType(){ return "sensor_msgs/Joy"; }; + const char * getMD5(){ return "5a9ea5f83505693b71e785041e67a8bb"; }; + + }; + +} +#endif \ No newline at end of file diff --git a/turtlebot3_core/libraries/ros_lib/sensor_msgs/JoyFeedback.h b/turtlebot3_core/libraries/ros_lib/sensor_msgs/JoyFeedback.h new file mode 100644 index 00000000..dfea009e --- /dev/null +++ b/turtlebot3_core/libraries/ros_lib/sensor_msgs/JoyFeedback.h @@ -0,0 +1,76 @@ +#ifndef _ROS_sensor_msgs_JoyFeedback_h +#define _ROS_sensor_msgs_JoyFeedback_h + +#include +#include +#include +#include "ros/msg.h" + +namespace sensor_msgs +{ + + class JoyFeedback : public ros::Msg + { + public: + uint8_t type; + uint8_t id; + float intensity; + enum { TYPE_LED = 0 }; + enum { TYPE_RUMBLE = 1 }; + enum { TYPE_BUZZER = 2 }; + + JoyFeedback(): + type(0), + id(0), + intensity(0) + { + } + + virtual int serialize(unsigned char *outbuffer) const + { + int offset = 0; + *(outbuffer + offset + 0) = (this->type >> (8 * 0)) & 0xFF; + offset += sizeof(this->type); + *(outbuffer + offset + 0) = (this->id >> (8 * 0)) & 0xFF; + offset += sizeof(this->id); + union { + float real; + uint32_t base; + } u_intensity; + u_intensity.real = this->intensity; + *(outbuffer + offset + 0) = (u_intensity.base >> (8 * 0)) & 0xFF; + *(outbuffer + offset + 1) = (u_intensity.base >> (8 * 1)) & 0xFF; + *(outbuffer + offset + 2) = (u_intensity.base >> (8 * 2)) & 0xFF; + *(outbuffer + offset + 3) = (u_intensity.base >> (8 * 3)) & 0xFF; + offset += sizeof(this->intensity); + return offset; + } + + virtual int deserialize(unsigned char *inbuffer) + { + int offset = 0; + this->type = ((uint8_t) (*(inbuffer + offset))); + offset += sizeof(this->type); + this->id = ((uint8_t) (*(inbuffer + offset))); + offset += sizeof(this->id); + union { + float real; + uint32_t base; + } u_intensity; + u_intensity.base = 0; + u_intensity.base |= ((uint32_t) (*(inbuffer + offset + 0))) << (8 * 0); + u_intensity.base |= ((uint32_t) (*(inbuffer + offset + 1))) << (8 * 1); + u_intensity.base |= ((uint32_t) (*(inbuffer + offset + 2))) << (8 * 2); + u_intensity.base |= ((uint32_t) (*(inbuffer + offset + 3))) << (8 * 3); + this->intensity = u_intensity.real; + offset += sizeof(this->intensity); + return offset; + } + + const char * getType(){ return "sensor_msgs/JoyFeedback"; }; + const char * getMD5(){ return "f4dcd73460360d98f36e55ee7f2e46f1"; }; + + }; + +} +#endif \ No newline at end of file diff --git a/turtlebot3_core/libraries/ros_lib/sensor_msgs/JoyFeedbackArray.h b/turtlebot3_core/libraries/ros_lib/sensor_msgs/JoyFeedbackArray.h new file mode 100644 index 00000000..15319c95 --- /dev/null +++ b/turtlebot3_core/libraries/ros_lib/sensor_msgs/JoyFeedbackArray.h @@ -0,0 +1,63 @@ +#ifndef _ROS_sensor_msgs_JoyFeedbackArray_h +#define _ROS_sensor_msgs_JoyFeedbackArray_h + +#include +#include +#include +#include "ros/msg.h" +#include "sensor_msgs/JoyFeedback.h" + +namespace sensor_msgs +{ + + class JoyFeedbackArray : public ros::Msg + { + public: + uint32_t array_length; + sensor_msgs::JoyFeedback st_array; + sensor_msgs::JoyFeedback * array; + + JoyFeedbackArray(): + array_length(0), array(NULL) + { + } + + virtual int serialize(unsigned char *outbuffer) const + { + int offset = 0; + *(outbuffer + offset + 0) = (this->array_length >> (8 * 0)) & 0xFF; + *(outbuffer + offset + 1) = (this->array_length >> (8 * 1)) & 0xFF; + *(outbuffer + offset + 2) = (this->array_length >> (8 * 2)) & 0xFF; + *(outbuffer + offset + 3) = (this->array_length >> (8 * 3)) & 0xFF; + offset += sizeof(this->array_length); + for( uint32_t i = 0; i < array_length; i++){ + offset += this->array[i].serialize(outbuffer + offset); + } + return offset; + } + + virtual int deserialize(unsigned char *inbuffer) + { + int offset = 0; + uint32_t array_lengthT = ((uint32_t) (*(inbuffer + offset))); + array_lengthT |= ((uint32_t) (*(inbuffer + offset + 1))) << (8 * 1); + array_lengthT |= ((uint32_t) (*(inbuffer + offset + 2))) << (8 * 2); + array_lengthT |= ((uint32_t) (*(inbuffer + offset + 3))) << (8 * 3); + offset += sizeof(this->array_length); + if(array_lengthT > array_length) + this->array = (sensor_msgs::JoyFeedback*)realloc(this->array, array_lengthT * sizeof(sensor_msgs::JoyFeedback)); + array_length = array_lengthT; + for( uint32_t i = 0; i < array_length; i++){ + offset += this->st_array.deserialize(inbuffer + offset); + memcpy( &(this->array[i]), &(this->st_array), sizeof(sensor_msgs::JoyFeedback)); + } + return offset; + } + + const char * getType(){ return "sensor_msgs/JoyFeedbackArray"; }; + const char * getMD5(){ return "cde5730a895b1fc4dee6f91b754b213d"; }; + + }; + +} +#endif \ No newline at end of file diff --git a/turtlebot3_core/libraries/ros_lib/sensor_msgs/LaserEcho.h b/turtlebot3_core/libraries/ros_lib/sensor_msgs/LaserEcho.h new file mode 100644 index 00000000..8ceaad74 --- /dev/null +++ b/turtlebot3_core/libraries/ros_lib/sensor_msgs/LaserEcho.h @@ -0,0 +1,81 @@ +#ifndef _ROS_sensor_msgs_LaserEcho_h +#define _ROS_sensor_msgs_LaserEcho_h + +#include +#include +#include +#include "ros/msg.h" + +namespace sensor_msgs +{ + + class LaserEcho : public ros::Msg + { + public: + uint32_t echoes_length; + float st_echoes; + float * echoes; + + LaserEcho(): + echoes_length(0), echoes(NULL) + { + } + + virtual int serialize(unsigned char *outbuffer) const + { + int offset = 0; + *(outbuffer + offset + 0) = (this->echoes_length >> (8 * 0)) & 0xFF; + *(outbuffer + offset + 1) = (this->echoes_length >> (8 * 1)) & 0xFF; + *(outbuffer + offset + 2) = (this->echoes_length >> (8 * 2)) & 0xFF; + *(outbuffer + offset + 3) = (this->echoes_length >> (8 * 3)) & 0xFF; + offset += sizeof(this->echoes_length); + for( uint32_t i = 0; i < echoes_length; i++){ + union { + float real; + uint32_t base; + } u_echoesi; + u_echoesi.real = this->echoes[i]; + *(outbuffer + offset + 0) = (u_echoesi.base >> (8 * 0)) & 0xFF; + *(outbuffer + offset + 1) = (u_echoesi.base >> (8 * 1)) & 0xFF; + *(outbuffer + offset + 2) = (u_echoesi.base >> (8 * 2)) & 0xFF; + *(outbuffer + offset + 3) = (u_echoesi.base >> (8 * 3)) & 0xFF; + offset += sizeof(this->echoes[i]); + } + return offset; + } + + virtual int deserialize(unsigned char *inbuffer) + { + int offset = 0; + uint32_t echoes_lengthT = ((uint32_t) (*(inbuffer + offset))); + echoes_lengthT |= ((uint32_t) (*(inbuffer + offset + 1))) << (8 * 1); + echoes_lengthT |= ((uint32_t) (*(inbuffer + offset + 2))) << (8 * 2); + echoes_lengthT |= ((uint32_t) (*(inbuffer + offset + 3))) << (8 * 3); + offset += sizeof(this->echoes_length); + if(echoes_lengthT > echoes_length) + this->echoes = (float*)realloc(this->echoes, echoes_lengthT * sizeof(float)); + echoes_length = echoes_lengthT; + for( uint32_t i = 0; i < echoes_length; i++){ + union { + float real; + uint32_t base; + } u_st_echoes; + u_st_echoes.base = 0; + u_st_echoes.base |= ((uint32_t) (*(inbuffer + offset + 0))) << (8 * 0); + u_st_echoes.base |= ((uint32_t) (*(inbuffer + offset + 1))) << (8 * 1); + u_st_echoes.base |= ((uint32_t) (*(inbuffer + offset + 2))) << (8 * 2); + u_st_echoes.base |= ((uint32_t) (*(inbuffer + offset + 3))) << (8 * 3); + this->st_echoes = u_st_echoes.real; + offset += sizeof(this->st_echoes); + memcpy( &(this->echoes[i]), &(this->st_echoes), sizeof(float)); + } + return offset; + } + + const char * getType(){ return "sensor_msgs/LaserEcho"; }; + const char * getMD5(){ return "8bc5ae449b200fba4d552b4225586696"; }; + + }; + +} +#endif \ No newline at end of file diff --git a/turtlebot3_core/libraries/ros_lib/sensor_msgs/LaserScan.h b/turtlebot3_core/libraries/ros_lib/sensor_msgs/LaserScan.h new file mode 100644 index 00000000..eed7a289 --- /dev/null +++ b/turtlebot3_core/libraries/ros_lib/sensor_msgs/LaserScan.h @@ -0,0 +1,290 @@ +#ifndef _ROS_sensor_msgs_LaserScan_h +#define _ROS_sensor_msgs_LaserScan_h + +#include +#include +#include +#include "ros/msg.h" +#include "std_msgs/Header.h" + +namespace sensor_msgs +{ + + class LaserScan : public ros::Msg + { + public: + std_msgs::Header header; + float angle_min; + float angle_max; + float angle_increment; + float time_increment; + float scan_time; + float range_min; + float range_max; + uint32_t ranges_length; + float st_ranges; + float * ranges; + uint32_t intensities_length; + float st_intensities; + float * intensities; + + LaserScan(): + header(), + angle_min(0), + angle_max(0), + angle_increment(0), + time_increment(0), + scan_time(0), + range_min(0), + range_max(0), + ranges_length(0), ranges(NULL), + intensities_length(0), intensities(NULL) + { + } + + virtual int serialize(unsigned char *outbuffer) const + { + int offset = 0; + offset += this->header.serialize(outbuffer + offset); + union { + float real; + uint32_t base; + } u_angle_min; + u_angle_min.real = this->angle_min; + *(outbuffer + offset + 0) = (u_angle_min.base >> (8 * 0)) & 0xFF; + *(outbuffer + offset + 1) = (u_angle_min.base >> (8 * 1)) & 0xFF; + *(outbuffer + offset + 2) = (u_angle_min.base >> (8 * 2)) & 0xFF; + *(outbuffer + offset + 3) = (u_angle_min.base >> (8 * 3)) & 0xFF; + offset += sizeof(this->angle_min); + union { + float real; + uint32_t base; + } u_angle_max; + u_angle_max.real = this->angle_max; + *(outbuffer + offset + 0) = (u_angle_max.base >> (8 * 0)) & 0xFF; + *(outbuffer + offset + 1) = (u_angle_max.base >> (8 * 1)) & 0xFF; + *(outbuffer + offset + 2) = (u_angle_max.base >> (8 * 2)) & 0xFF; + *(outbuffer + offset + 3) = (u_angle_max.base >> (8 * 3)) & 0xFF; + offset += sizeof(this->angle_max); + union { + float real; + uint32_t base; + } u_angle_increment; + u_angle_increment.real = this->angle_increment; + *(outbuffer + offset + 0) = (u_angle_increment.base >> (8 * 0)) & 0xFF; + *(outbuffer + offset + 1) = (u_angle_increment.base >> (8 * 1)) & 0xFF; + *(outbuffer + offset + 2) = (u_angle_increment.base >> (8 * 2)) & 0xFF; + *(outbuffer + offset + 3) = (u_angle_increment.base >> (8 * 3)) & 0xFF; + offset += sizeof(this->angle_increment); + union { + float real; + uint32_t base; + } u_time_increment; + u_time_increment.real = this->time_increment; + *(outbuffer + offset + 0) = (u_time_increment.base >> (8 * 0)) & 0xFF; + *(outbuffer + offset + 1) = (u_time_increment.base >> (8 * 1)) & 0xFF; + *(outbuffer + offset + 2) = (u_time_increment.base >> (8 * 2)) & 0xFF; + *(outbuffer + offset + 3) = (u_time_increment.base >> (8 * 3)) & 0xFF; + offset += sizeof(this->time_increment); + union { + float real; + uint32_t base; + } u_scan_time; + u_scan_time.real = this->scan_time; + *(outbuffer + offset + 0) = (u_scan_time.base >> (8 * 0)) & 0xFF; + *(outbuffer + offset + 1) = (u_scan_time.base >> (8 * 1)) & 0xFF; + *(outbuffer + offset + 2) = (u_scan_time.base >> (8 * 2)) & 0xFF; + *(outbuffer + offset + 3) = (u_scan_time.base >> (8 * 3)) & 0xFF; + offset += sizeof(this->scan_time); + union { + float real; + uint32_t base; + } u_range_min; + u_range_min.real = this->range_min; + *(outbuffer + offset + 0) = (u_range_min.base >> (8 * 0)) & 0xFF; + *(outbuffer + offset + 1) = (u_range_min.base >> (8 * 1)) & 0xFF; + *(outbuffer + offset + 2) = (u_range_min.base >> (8 * 2)) & 0xFF; + *(outbuffer + offset + 3) = (u_range_min.base >> (8 * 3)) & 0xFF; + offset += sizeof(this->range_min); + union { + float real; + uint32_t base; + } u_range_max; + u_range_max.real = this->range_max; + *(outbuffer + offset + 0) = (u_range_max.base >> (8 * 0)) & 0xFF; + *(outbuffer + offset + 1) = (u_range_max.base >> (8 * 1)) & 0xFF; + *(outbuffer + offset + 2) = (u_range_max.base >> (8 * 2)) & 0xFF; + *(outbuffer + offset + 3) = (u_range_max.base >> (8 * 3)) & 0xFF; + offset += sizeof(this->range_max); + *(outbuffer + offset + 0) = (this->ranges_length >> (8 * 0)) & 0xFF; + *(outbuffer + offset + 1) = (this->ranges_length >> (8 * 1)) & 0xFF; + *(outbuffer + offset + 2) = (this->ranges_length >> (8 * 2)) & 0xFF; + *(outbuffer + offset + 3) = (this->ranges_length >> (8 * 3)) & 0xFF; + offset += sizeof(this->ranges_length); + for( uint32_t i = 0; i < ranges_length; i++){ + union { + float real; + uint32_t base; + } u_rangesi; + u_rangesi.real = this->ranges[i]; + *(outbuffer + offset + 0) = (u_rangesi.base >> (8 * 0)) & 0xFF; + *(outbuffer + offset + 1) = (u_rangesi.base >> (8 * 1)) & 0xFF; + *(outbuffer + offset + 2) = (u_rangesi.base >> (8 * 2)) & 0xFF; + *(outbuffer + offset + 3) = (u_rangesi.base >> (8 * 3)) & 0xFF; + offset += sizeof(this->ranges[i]); + } + *(outbuffer + offset + 0) = (this->intensities_length >> (8 * 0)) & 0xFF; + *(outbuffer + offset + 1) = (this->intensities_length >> (8 * 1)) & 0xFF; + *(outbuffer + offset + 2) = (this->intensities_length >> (8 * 2)) & 0xFF; + *(outbuffer + offset + 3) = (this->intensities_length >> (8 * 3)) & 0xFF; + offset += sizeof(this->intensities_length); + for( uint32_t i = 0; i < intensities_length; i++){ + union { + float real; + uint32_t base; + } u_intensitiesi; + u_intensitiesi.real = this->intensities[i]; + *(outbuffer + offset + 0) = (u_intensitiesi.base >> (8 * 0)) & 0xFF; + *(outbuffer + offset + 1) = (u_intensitiesi.base >> (8 * 1)) & 0xFF; + *(outbuffer + offset + 2) = (u_intensitiesi.base >> (8 * 2)) & 0xFF; + *(outbuffer + offset + 3) = (u_intensitiesi.base >> (8 * 3)) & 0xFF; + offset += sizeof(this->intensities[i]); + } + return offset; + } + + virtual int deserialize(unsigned char *inbuffer) + { + int offset = 0; + offset += this->header.deserialize(inbuffer + offset); + union { + float real; + uint32_t base; + } u_angle_min; + u_angle_min.base = 0; + u_angle_min.base |= ((uint32_t) (*(inbuffer + offset + 0))) << (8 * 0); + u_angle_min.base |= ((uint32_t) (*(inbuffer + offset + 1))) << (8 * 1); + u_angle_min.base |= ((uint32_t) (*(inbuffer + offset + 2))) << (8 * 2); + u_angle_min.base |= ((uint32_t) (*(inbuffer + offset + 3))) << (8 * 3); + this->angle_min = u_angle_min.real; + offset += sizeof(this->angle_min); + union { + float real; + uint32_t base; + } u_angle_max; + u_angle_max.base = 0; + u_angle_max.base |= ((uint32_t) (*(inbuffer + offset + 0))) << (8 * 0); + u_angle_max.base |= ((uint32_t) (*(inbuffer + offset + 1))) << (8 * 1); + u_angle_max.base |= ((uint32_t) (*(inbuffer + offset + 2))) << (8 * 2); + u_angle_max.base |= ((uint32_t) (*(inbuffer + offset + 3))) << (8 * 3); + this->angle_max = u_angle_max.real; + offset += sizeof(this->angle_max); + union { + float real; + uint32_t base; + } u_angle_increment; + u_angle_increment.base = 0; + u_angle_increment.base |= ((uint32_t) (*(inbuffer + offset + 0))) << (8 * 0); + u_angle_increment.base |= ((uint32_t) (*(inbuffer + offset + 1))) << (8 * 1); + u_angle_increment.base |= ((uint32_t) (*(inbuffer + offset + 2))) << (8 * 2); + u_angle_increment.base |= ((uint32_t) (*(inbuffer + offset + 3))) << (8 * 3); + this->angle_increment = u_angle_increment.real; + offset += sizeof(this->angle_increment); + union { + float real; + uint32_t base; + } u_time_increment; + u_time_increment.base = 0; + u_time_increment.base |= ((uint32_t) (*(inbuffer + offset + 0))) << (8 * 0); + u_time_increment.base |= ((uint32_t) (*(inbuffer + offset + 1))) << (8 * 1); + u_time_increment.base |= ((uint32_t) (*(inbuffer + offset + 2))) << (8 * 2); + u_time_increment.base |= ((uint32_t) (*(inbuffer + offset + 3))) << (8 * 3); + this->time_increment = u_time_increment.real; + offset += sizeof(this->time_increment); + union { + float real; + uint32_t base; + } u_scan_time; + u_scan_time.base = 0; + u_scan_time.base |= ((uint32_t) (*(inbuffer + offset + 0))) << (8 * 0); + u_scan_time.base |= ((uint32_t) (*(inbuffer + offset + 1))) << (8 * 1); + u_scan_time.base |= ((uint32_t) (*(inbuffer + offset + 2))) << (8 * 2); + u_scan_time.base |= ((uint32_t) (*(inbuffer + offset + 3))) << (8 * 3); + this->scan_time = u_scan_time.real; + offset += sizeof(this->scan_time); + union { + float real; + uint32_t base; + } u_range_min; + u_range_min.base = 0; + u_range_min.base |= ((uint32_t) (*(inbuffer + offset + 0))) << (8 * 0); + u_range_min.base |= ((uint32_t) (*(inbuffer + offset + 1))) << (8 * 1); + u_range_min.base |= ((uint32_t) (*(inbuffer + offset + 2))) << (8 * 2); + u_range_min.base |= ((uint32_t) (*(inbuffer + offset + 3))) << (8 * 3); + this->range_min = u_range_min.real; + offset += sizeof(this->range_min); + union { + float real; + uint32_t base; + } u_range_max; + u_range_max.base = 0; + u_range_max.base |= ((uint32_t) (*(inbuffer + offset + 0))) << (8 * 0); + u_range_max.base |= ((uint32_t) (*(inbuffer + offset + 1))) << (8 * 1); + u_range_max.base |= ((uint32_t) (*(inbuffer + offset + 2))) << (8 * 2); + u_range_max.base |= ((uint32_t) (*(inbuffer + offset + 3))) << (8 * 3); + this->range_max = u_range_max.real; + offset += sizeof(this->range_max); + uint32_t ranges_lengthT = ((uint32_t) (*(inbuffer + offset))); + ranges_lengthT |= ((uint32_t) (*(inbuffer + offset + 1))) << (8 * 1); + ranges_lengthT |= ((uint32_t) (*(inbuffer + offset + 2))) << (8 * 2); + ranges_lengthT |= ((uint32_t) (*(inbuffer + offset + 3))) << (8 * 3); + offset += sizeof(this->ranges_length); + if(ranges_lengthT > ranges_length) + this->ranges = (float*)realloc(this->ranges, ranges_lengthT * sizeof(float)); + ranges_length = ranges_lengthT; + for( uint32_t i = 0; i < ranges_length; i++){ + union { + float real; + uint32_t base; + } u_st_ranges; + u_st_ranges.base = 0; + u_st_ranges.base |= ((uint32_t) (*(inbuffer + offset + 0))) << (8 * 0); + u_st_ranges.base |= ((uint32_t) (*(inbuffer + offset + 1))) << (8 * 1); + u_st_ranges.base |= ((uint32_t) (*(inbuffer + offset + 2))) << (8 * 2); + u_st_ranges.base |= ((uint32_t) (*(inbuffer + offset + 3))) << (8 * 3); + this->st_ranges = u_st_ranges.real; + offset += sizeof(this->st_ranges); + memcpy( &(this->ranges[i]), &(this->st_ranges), sizeof(float)); + } + uint32_t intensities_lengthT = ((uint32_t) (*(inbuffer + offset))); + intensities_lengthT |= ((uint32_t) (*(inbuffer + offset + 1))) << (8 * 1); + intensities_lengthT |= ((uint32_t) (*(inbuffer + offset + 2))) << (8 * 2); + intensities_lengthT |= ((uint32_t) (*(inbuffer + offset + 3))) << (8 * 3); + offset += sizeof(this->intensities_length); + if(intensities_lengthT > intensities_length) + this->intensities = (float*)realloc(this->intensities, intensities_lengthT * sizeof(float)); + intensities_length = intensities_lengthT; + for( uint32_t i = 0; i < intensities_length; i++){ + union { + float real; + uint32_t base; + } u_st_intensities; + u_st_intensities.base = 0; + u_st_intensities.base |= ((uint32_t) (*(inbuffer + offset + 0))) << (8 * 0); + u_st_intensities.base |= ((uint32_t) (*(inbuffer + offset + 1))) << (8 * 1); + u_st_intensities.base |= ((uint32_t) (*(inbuffer + offset + 2))) << (8 * 2); + u_st_intensities.base |= ((uint32_t) (*(inbuffer + offset + 3))) << (8 * 3); + this->st_intensities = u_st_intensities.real; + offset += sizeof(this->st_intensities); + memcpy( &(this->intensities[i]), &(this->st_intensities), sizeof(float)); + } + return offset; + } + + const char * getType(){ return "sensor_msgs/LaserScan"; }; + const char * getMD5(){ return "90c7ef2dc6895d81024acba2ac42f369"; }; + + }; + +} +#endif \ No newline at end of file diff --git a/turtlebot3_core/libraries/ros_lib/sensor_msgs/MagneticField.h b/turtlebot3_core/libraries/ros_lib/sensor_msgs/MagneticField.h new file mode 100644 index 00000000..03e2ee71 --- /dev/null +++ b/turtlebot3_core/libraries/ros_lib/sensor_msgs/MagneticField.h @@ -0,0 +1,56 @@ +#ifndef _ROS_sensor_msgs_MagneticField_h +#define _ROS_sensor_msgs_MagneticField_h + +#include +#include +#include +#include "ros/msg.h" +#include "std_msgs/Header.h" +#include "geometry_msgs/Vector3.h" + +namespace sensor_msgs +{ + + class MagneticField : public ros::Msg + { + public: + std_msgs::Header header; + geometry_msgs::Vector3 magnetic_field; + float magnetic_field_covariance[9]; + + MagneticField(): + header(), + magnetic_field(), + magnetic_field_covariance() + { + } + + virtual int serialize(unsigned char *outbuffer) const + { + int offset = 0; + offset += this->header.serialize(outbuffer + offset); + offset += this->magnetic_field.serialize(outbuffer + offset); + for( uint32_t i = 0; i < 9; i++){ + offset += serializeAvrFloat64(outbuffer + offset, this->magnetic_field_covariance[i]); + } + return offset; + } + + virtual int deserialize(unsigned char *inbuffer) + { + int offset = 0; + offset += this->header.deserialize(inbuffer + offset); + offset += this->magnetic_field.deserialize(inbuffer + offset); + for( uint32_t i = 0; i < 9; i++){ + offset += deserializeAvrFloat64(inbuffer + offset, &(this->magnetic_field_covariance[i])); + } + return offset; + } + + const char * getType(){ return "sensor_msgs/MagneticField"; }; + const char * getMD5(){ return "2f3b0b43eed0c9501de0fa3ff89a45aa"; }; + + }; + +} +#endif \ No newline at end of file diff --git a/turtlebot3_core/libraries/ros_lib/sensor_msgs/MultiDOFJointState.h b/turtlebot3_core/libraries/ros_lib/sensor_msgs/MultiDOFJointState.h new file mode 100644 index 00000000..99f43fea --- /dev/null +++ b/turtlebot3_core/libraries/ros_lib/sensor_msgs/MultiDOFJointState.h @@ -0,0 +1,154 @@ +#ifndef _ROS_sensor_msgs_MultiDOFJointState_h +#define _ROS_sensor_msgs_MultiDOFJointState_h + +#include +#include +#include +#include "ros/msg.h" +#include "std_msgs/Header.h" +#include "geometry_msgs/Transform.h" +#include "geometry_msgs/Twist.h" +#include "geometry_msgs/Wrench.h" + +namespace sensor_msgs +{ + + class MultiDOFJointState : public ros::Msg + { + public: + std_msgs::Header header; + uint32_t joint_names_length; + char* st_joint_names; + char* * joint_names; + uint32_t transforms_length; + geometry_msgs::Transform st_transforms; + geometry_msgs::Transform * transforms; + uint32_t twist_length; + geometry_msgs::Twist st_twist; + geometry_msgs::Twist * twist; + uint32_t wrench_length; + geometry_msgs::Wrench st_wrench; + geometry_msgs::Wrench * wrench; + + MultiDOFJointState(): + header(), + joint_names_length(0), joint_names(NULL), + transforms_length(0), transforms(NULL), + twist_length(0), twist(NULL), + wrench_length(0), wrench(NULL) + { + } + + virtual int serialize(unsigned char *outbuffer) const + { + int offset = 0; + offset += this->header.serialize(outbuffer + offset); + *(outbuffer + offset + 0) = (this->joint_names_length >> (8 * 0)) & 0xFF; + *(outbuffer + offset + 1) = (this->joint_names_length >> (8 * 1)) & 0xFF; + *(outbuffer + offset + 2) = (this->joint_names_length >> (8 * 2)) & 0xFF; + *(outbuffer + offset + 3) = (this->joint_names_length >> (8 * 3)) & 0xFF; + offset += sizeof(this->joint_names_length); + for( uint32_t i = 0; i < joint_names_length; i++){ + uint32_t length_joint_namesi = strlen(this->joint_names[i]); + memcpy(outbuffer + offset, &length_joint_namesi, sizeof(uint32_t)); + offset += 4; + memcpy(outbuffer + offset, this->joint_names[i], length_joint_namesi); + offset += length_joint_namesi; + } + *(outbuffer + offset + 0) = (this->transforms_length >> (8 * 0)) & 0xFF; + *(outbuffer + offset + 1) = (this->transforms_length >> (8 * 1)) & 0xFF; + *(outbuffer + offset + 2) = (this->transforms_length >> (8 * 2)) & 0xFF; + *(outbuffer + offset + 3) = (this->transforms_length >> (8 * 3)) & 0xFF; + offset += sizeof(this->transforms_length); + for( uint32_t i = 0; i < transforms_length; i++){ + offset += this->transforms[i].serialize(outbuffer + offset); + } + *(outbuffer + offset + 0) = (this->twist_length >> (8 * 0)) & 0xFF; + *(outbuffer + offset + 1) = (this->twist_length >> (8 * 1)) & 0xFF; + *(outbuffer + offset + 2) = (this->twist_length >> (8 * 2)) & 0xFF; + *(outbuffer + offset + 3) = (this->twist_length >> (8 * 3)) & 0xFF; + offset += sizeof(this->twist_length); + for( uint32_t i = 0; i < twist_length; i++){ + offset += this->twist[i].serialize(outbuffer + offset); + } + *(outbuffer + offset + 0) = (this->wrench_length >> (8 * 0)) & 0xFF; + *(outbuffer + offset + 1) = (this->wrench_length >> (8 * 1)) & 0xFF; + *(outbuffer + offset + 2) = (this->wrench_length >> (8 * 2)) & 0xFF; + *(outbuffer + offset + 3) = (this->wrench_length >> (8 * 3)) & 0xFF; + offset += sizeof(this->wrench_length); + for( uint32_t i = 0; i < wrench_length; i++){ + offset += this->wrench[i].serialize(outbuffer + offset); + } + return offset; + } + + virtual int deserialize(unsigned char *inbuffer) + { + int offset = 0; + offset += this->header.deserialize(inbuffer + offset); + uint32_t joint_names_lengthT = ((uint32_t) (*(inbuffer + offset))); + joint_names_lengthT |= ((uint32_t) (*(inbuffer + offset + 1))) << (8 * 1); + joint_names_lengthT |= ((uint32_t) (*(inbuffer + offset + 2))) << (8 * 2); + joint_names_lengthT |= ((uint32_t) (*(inbuffer + offset + 3))) << (8 * 3); + offset += sizeof(this->joint_names_length); + if(joint_names_lengthT > joint_names_length) + this->joint_names = (char**)realloc(this->joint_names, joint_names_lengthT * sizeof(char*)); + joint_names_length = joint_names_lengthT; + for( uint32_t i = 0; i < joint_names_length; i++){ + uint32_t length_st_joint_names; + memcpy(&length_st_joint_names, (inbuffer + offset), sizeof(uint32_t)); + offset += 4; + for(unsigned int k= offset; k< offset+length_st_joint_names; ++k){ + inbuffer[k-1]=inbuffer[k]; + } + inbuffer[offset+length_st_joint_names-1]=0; + this->st_joint_names = (char *)(inbuffer + offset-1); + offset += length_st_joint_names; + memcpy( &(this->joint_names[i]), &(this->st_joint_names), sizeof(char*)); + } + uint32_t transforms_lengthT = ((uint32_t) (*(inbuffer + offset))); + transforms_lengthT |= ((uint32_t) (*(inbuffer + offset + 1))) << (8 * 1); + transforms_lengthT |= ((uint32_t) (*(inbuffer + offset + 2))) << (8 * 2); + transforms_lengthT |= ((uint32_t) (*(inbuffer + offset + 3))) << (8 * 3); + offset += sizeof(this->transforms_length); + if(transforms_lengthT > transforms_length) + this->transforms = (geometry_msgs::Transform*)realloc(this->transforms, transforms_lengthT * sizeof(geometry_msgs::Transform)); + transforms_length = transforms_lengthT; + for( uint32_t i = 0; i < transforms_length; i++){ + offset += this->st_transforms.deserialize(inbuffer + offset); + memcpy( &(this->transforms[i]), &(this->st_transforms), sizeof(geometry_msgs::Transform)); + } + uint32_t twist_lengthT = ((uint32_t) (*(inbuffer + offset))); + twist_lengthT |= ((uint32_t) (*(inbuffer + offset + 1))) << (8 * 1); + twist_lengthT |= ((uint32_t) (*(inbuffer + offset + 2))) << (8 * 2); + twist_lengthT |= ((uint32_t) (*(inbuffer + offset + 3))) << (8 * 3); + offset += sizeof(this->twist_length); + if(twist_lengthT > twist_length) + this->twist = (geometry_msgs::Twist*)realloc(this->twist, twist_lengthT * sizeof(geometry_msgs::Twist)); + twist_length = twist_lengthT; + for( uint32_t i = 0; i < twist_length; i++){ + offset += this->st_twist.deserialize(inbuffer + offset); + memcpy( &(this->twist[i]), &(this->st_twist), sizeof(geometry_msgs::Twist)); + } + uint32_t wrench_lengthT = ((uint32_t) (*(inbuffer + offset))); + wrench_lengthT |= ((uint32_t) (*(inbuffer + offset + 1))) << (8 * 1); + wrench_lengthT |= ((uint32_t) (*(inbuffer + offset + 2))) << (8 * 2); + wrench_lengthT |= ((uint32_t) (*(inbuffer + offset + 3))) << (8 * 3); + offset += sizeof(this->wrench_length); + if(wrench_lengthT > wrench_length) + this->wrench = (geometry_msgs::Wrench*)realloc(this->wrench, wrench_lengthT * sizeof(geometry_msgs::Wrench)); + wrench_length = wrench_lengthT; + for( uint32_t i = 0; i < wrench_length; i++){ + offset += this->st_wrench.deserialize(inbuffer + offset); + memcpy( &(this->wrench[i]), &(this->st_wrench), sizeof(geometry_msgs::Wrench)); + } + return offset; + } + + const char * getType(){ return "sensor_msgs/MultiDOFJointState"; }; + const char * getMD5(){ return "690f272f0640d2631c305eeb8301e59d"; }; + + }; + +} +#endif \ No newline at end of file diff --git a/turtlebot3_core/libraries/ros_lib/sensor_msgs/MultiEchoLaserScan.h b/turtlebot3_core/libraries/ros_lib/sensor_msgs/MultiEchoLaserScan.h new file mode 100644 index 00000000..04b6079d --- /dev/null +++ b/turtlebot3_core/libraries/ros_lib/sensor_msgs/MultiEchoLaserScan.h @@ -0,0 +1,253 @@ +#ifndef _ROS_sensor_msgs_MultiEchoLaserScan_h +#define _ROS_sensor_msgs_MultiEchoLaserScan_h + +#include +#include +#include +#include "ros/msg.h" +#include "std_msgs/Header.h" +#include "sensor_msgs/LaserEcho.h" + +namespace sensor_msgs +{ + + class MultiEchoLaserScan : public ros::Msg + { + public: + std_msgs::Header header; + float angle_min; + float angle_max; + float angle_increment; + float time_increment; + float scan_time; + float range_min; + float range_max; + uint32_t ranges_length; + sensor_msgs::LaserEcho st_ranges; + sensor_msgs::LaserEcho * ranges; + uint32_t intensities_length; + sensor_msgs::LaserEcho st_intensities; + sensor_msgs::LaserEcho * intensities; + + MultiEchoLaserScan(): + header(), + angle_min(0), + angle_max(0), + angle_increment(0), + time_increment(0), + scan_time(0), + range_min(0), + range_max(0), + ranges_length(0), ranges(NULL), + intensities_length(0), intensities(NULL) + { + } + + virtual int serialize(unsigned char *outbuffer) const + { + int offset = 0; + offset += this->header.serialize(outbuffer + offset); + union { + float real; + uint32_t base; + } u_angle_min; + u_angle_min.real = this->angle_min; + *(outbuffer + offset + 0) = (u_angle_min.base >> (8 * 0)) & 0xFF; + *(outbuffer + offset + 1) = (u_angle_min.base >> (8 * 1)) & 0xFF; + *(outbuffer + offset + 2) = (u_angle_min.base >> (8 * 2)) & 0xFF; + *(outbuffer + offset + 3) = (u_angle_min.base >> (8 * 3)) & 0xFF; + offset += sizeof(this->angle_min); + union { + float real; + uint32_t base; + } u_angle_max; + u_angle_max.real = this->angle_max; + *(outbuffer + offset + 0) = (u_angle_max.base >> (8 * 0)) & 0xFF; + *(outbuffer + offset + 1) = (u_angle_max.base >> (8 * 1)) & 0xFF; + *(outbuffer + offset + 2) = (u_angle_max.base >> (8 * 2)) & 0xFF; + *(outbuffer + offset + 3) = (u_angle_max.base >> (8 * 3)) & 0xFF; + offset += sizeof(this->angle_max); + union { + float real; + uint32_t base; + } u_angle_increment; + u_angle_increment.real = this->angle_increment; + *(outbuffer + offset + 0) = (u_angle_increment.base >> (8 * 0)) & 0xFF; + *(outbuffer + offset + 1) = (u_angle_increment.base >> (8 * 1)) & 0xFF; + *(outbuffer + offset + 2) = (u_angle_increment.base >> (8 * 2)) & 0xFF; + *(outbuffer + offset + 3) = (u_angle_increment.base >> (8 * 3)) & 0xFF; + offset += sizeof(this->angle_increment); + union { + float real; + uint32_t base; + } u_time_increment; + u_time_increment.real = this->time_increment; + *(outbuffer + offset + 0) = (u_time_increment.base >> (8 * 0)) & 0xFF; + *(outbuffer + offset + 1) = (u_time_increment.base >> (8 * 1)) & 0xFF; + *(outbuffer + offset + 2) = (u_time_increment.base >> (8 * 2)) & 0xFF; + *(outbuffer + offset + 3) = (u_time_increment.base >> (8 * 3)) & 0xFF; + offset += sizeof(this->time_increment); + union { + float real; + uint32_t base; + } u_scan_time; + u_scan_time.real = this->scan_time; + *(outbuffer + offset + 0) = (u_scan_time.base >> (8 * 0)) & 0xFF; + *(outbuffer + offset + 1) = (u_scan_time.base >> (8 * 1)) & 0xFF; + *(outbuffer + offset + 2) = (u_scan_time.base >> (8 * 2)) & 0xFF; + *(outbuffer + offset + 3) = (u_scan_time.base >> (8 * 3)) & 0xFF; + offset += sizeof(this->scan_time); + union { + float real; + uint32_t base; + } u_range_min; + u_range_min.real = this->range_min; + *(outbuffer + offset + 0) = (u_range_min.base >> (8 * 0)) & 0xFF; + *(outbuffer + offset + 1) = (u_range_min.base >> (8 * 1)) & 0xFF; + *(outbuffer + offset + 2) = (u_range_min.base >> (8 * 2)) & 0xFF; + *(outbuffer + offset + 3) = (u_range_min.base >> (8 * 3)) & 0xFF; + offset += sizeof(this->range_min); + union { + float real; + uint32_t base; + } u_range_max; + u_range_max.real = this->range_max; + *(outbuffer + offset + 0) = (u_range_max.base >> (8 * 0)) & 0xFF; + *(outbuffer + offset + 1) = (u_range_max.base >> (8 * 1)) & 0xFF; + *(outbuffer + offset + 2) = (u_range_max.base >> (8 * 2)) & 0xFF; + *(outbuffer + offset + 3) = (u_range_max.base >> (8 * 3)) & 0xFF; + offset += sizeof(this->range_max); + *(outbuffer + offset + 0) = (this->ranges_length >> (8 * 0)) & 0xFF; + *(outbuffer + offset + 1) = (this->ranges_length >> (8 * 1)) & 0xFF; + *(outbuffer + offset + 2) = (this->ranges_length >> (8 * 2)) & 0xFF; + *(outbuffer + offset + 3) = (this->ranges_length >> (8 * 3)) & 0xFF; + offset += sizeof(this->ranges_length); + for( uint32_t i = 0; i < ranges_length; i++){ + offset += this->ranges[i].serialize(outbuffer + offset); + } + *(outbuffer + offset + 0) = (this->intensities_length >> (8 * 0)) & 0xFF; + *(outbuffer + offset + 1) = (this->intensities_length >> (8 * 1)) & 0xFF; + *(outbuffer + offset + 2) = (this->intensities_length >> (8 * 2)) & 0xFF; + *(outbuffer + offset + 3) = (this->intensities_length >> (8 * 3)) & 0xFF; + offset += sizeof(this->intensities_length); + for( uint32_t i = 0; i < intensities_length; i++){ + offset += this->intensities[i].serialize(outbuffer + offset); + } + return offset; + } + + virtual int deserialize(unsigned char *inbuffer) + { + int offset = 0; + offset += this->header.deserialize(inbuffer + offset); + union { + float real; + uint32_t base; + } u_angle_min; + u_angle_min.base = 0; + u_angle_min.base |= ((uint32_t) (*(inbuffer + offset + 0))) << (8 * 0); + u_angle_min.base |= ((uint32_t) (*(inbuffer + offset + 1))) << (8 * 1); + u_angle_min.base |= ((uint32_t) (*(inbuffer + offset + 2))) << (8 * 2); + u_angle_min.base |= ((uint32_t) (*(inbuffer + offset + 3))) << (8 * 3); + this->angle_min = u_angle_min.real; + offset += sizeof(this->angle_min); + union { + float real; + uint32_t base; + } u_angle_max; + u_angle_max.base = 0; + u_angle_max.base |= ((uint32_t) (*(inbuffer + offset + 0))) << (8 * 0); + u_angle_max.base |= ((uint32_t) (*(inbuffer + offset + 1))) << (8 * 1); + u_angle_max.base |= ((uint32_t) (*(inbuffer + offset + 2))) << (8 * 2); + u_angle_max.base |= ((uint32_t) (*(inbuffer + offset + 3))) << (8 * 3); + this->angle_max = u_angle_max.real; + offset += sizeof(this->angle_max); + union { + float real; + uint32_t base; + } u_angle_increment; + u_angle_increment.base = 0; + u_angle_increment.base |= ((uint32_t) (*(inbuffer + offset + 0))) << (8 * 0); + u_angle_increment.base |= ((uint32_t) (*(inbuffer + offset + 1))) << (8 * 1); + u_angle_increment.base |= ((uint32_t) (*(inbuffer + offset + 2))) << (8 * 2); + u_angle_increment.base |= ((uint32_t) (*(inbuffer + offset + 3))) << (8 * 3); + this->angle_increment = u_angle_increment.real; + offset += sizeof(this->angle_increment); + union { + float real; + uint32_t base; + } u_time_increment; + u_time_increment.base = 0; + u_time_increment.base |= ((uint32_t) (*(inbuffer + offset + 0))) << (8 * 0); + u_time_increment.base |= ((uint32_t) (*(inbuffer + offset + 1))) << (8 * 1); + u_time_increment.base |= ((uint32_t) (*(inbuffer + offset + 2))) << (8 * 2); + u_time_increment.base |= ((uint32_t) (*(inbuffer + offset + 3))) << (8 * 3); + this->time_increment = u_time_increment.real; + offset += sizeof(this->time_increment); + union { + float real; + uint32_t base; + } u_scan_time; + u_scan_time.base = 0; + u_scan_time.base |= ((uint32_t) (*(inbuffer + offset + 0))) << (8 * 0); + u_scan_time.base |= ((uint32_t) (*(inbuffer + offset + 1))) << (8 * 1); + u_scan_time.base |= ((uint32_t) (*(inbuffer + offset + 2))) << (8 * 2); + u_scan_time.base |= ((uint32_t) (*(inbuffer + offset + 3))) << (8 * 3); + this->scan_time = u_scan_time.real; + offset += sizeof(this->scan_time); + union { + float real; + uint32_t base; + } u_range_min; + u_range_min.base = 0; + u_range_min.base |= ((uint32_t) (*(inbuffer + offset + 0))) << (8 * 0); + u_range_min.base |= ((uint32_t) (*(inbuffer + offset + 1))) << (8 * 1); + u_range_min.base |= ((uint32_t) (*(inbuffer + offset + 2))) << (8 * 2); + u_range_min.base |= ((uint32_t) (*(inbuffer + offset + 3))) << (8 * 3); + this->range_min = u_range_min.real; + offset += sizeof(this->range_min); + union { + float real; + uint32_t base; + } u_range_max; + u_range_max.base = 0; + u_range_max.base |= ((uint32_t) (*(inbuffer + offset + 0))) << (8 * 0); + u_range_max.base |= ((uint32_t) (*(inbuffer + offset + 1))) << (8 * 1); + u_range_max.base |= ((uint32_t) (*(inbuffer + offset + 2))) << (8 * 2); + u_range_max.base |= ((uint32_t) (*(inbuffer + offset + 3))) << (8 * 3); + this->range_max = u_range_max.real; + offset += sizeof(this->range_max); + uint32_t ranges_lengthT = ((uint32_t) (*(inbuffer + offset))); + ranges_lengthT |= ((uint32_t) (*(inbuffer + offset + 1))) << (8 * 1); + ranges_lengthT |= ((uint32_t) (*(inbuffer + offset + 2))) << (8 * 2); + ranges_lengthT |= ((uint32_t) (*(inbuffer + offset + 3))) << (8 * 3); + offset += sizeof(this->ranges_length); + if(ranges_lengthT > ranges_length) + this->ranges = (sensor_msgs::LaserEcho*)realloc(this->ranges, ranges_lengthT * sizeof(sensor_msgs::LaserEcho)); + ranges_length = ranges_lengthT; + for( uint32_t i = 0; i < ranges_length; i++){ + offset += this->st_ranges.deserialize(inbuffer + offset); + memcpy( &(this->ranges[i]), &(this->st_ranges), sizeof(sensor_msgs::LaserEcho)); + } + uint32_t intensities_lengthT = ((uint32_t) (*(inbuffer + offset))); + intensities_lengthT |= ((uint32_t) (*(inbuffer + offset + 1))) << (8 * 1); + intensities_lengthT |= ((uint32_t) (*(inbuffer + offset + 2))) << (8 * 2); + intensities_lengthT |= ((uint32_t) (*(inbuffer + offset + 3))) << (8 * 3); + offset += sizeof(this->intensities_length); + if(intensities_lengthT > intensities_length) + this->intensities = (sensor_msgs::LaserEcho*)realloc(this->intensities, intensities_lengthT * sizeof(sensor_msgs::LaserEcho)); + intensities_length = intensities_lengthT; + for( uint32_t i = 0; i < intensities_length; i++){ + offset += this->st_intensities.deserialize(inbuffer + offset); + memcpy( &(this->intensities[i]), &(this->st_intensities), sizeof(sensor_msgs::LaserEcho)); + } + return offset; + } + + const char * getType(){ return "sensor_msgs/MultiEchoLaserScan"; }; + const char * getMD5(){ return "6fefb0c6da89d7c8abe4b339f5c2f8fb"; }; + + }; + +} +#endif \ No newline at end of file diff --git a/turtlebot3_core/libraries/ros_lib/sensor_msgs/NavSatFix.h b/turtlebot3_core/libraries/ros_lib/sensor_msgs/NavSatFix.h new file mode 100644 index 00000000..6f2a80b1 --- /dev/null +++ b/turtlebot3_core/libraries/ros_lib/sensor_msgs/NavSatFix.h @@ -0,0 +1,78 @@ +#ifndef _ROS_sensor_msgs_NavSatFix_h +#define _ROS_sensor_msgs_NavSatFix_h + +#include +#include +#include +#include "ros/msg.h" +#include "std_msgs/Header.h" +#include "sensor_msgs/NavSatStatus.h" + +namespace sensor_msgs +{ + + class NavSatFix : public ros::Msg + { + public: + std_msgs::Header header; + sensor_msgs::NavSatStatus status; + float latitude; + float longitude; + float altitude; + float position_covariance[9]; + uint8_t position_covariance_type; + enum { COVARIANCE_TYPE_UNKNOWN = 0 }; + enum { COVARIANCE_TYPE_APPROXIMATED = 1 }; + enum { COVARIANCE_TYPE_DIAGONAL_KNOWN = 2 }; + enum { COVARIANCE_TYPE_KNOWN = 3 }; + + NavSatFix(): + header(), + status(), + latitude(0), + longitude(0), + altitude(0), + position_covariance(), + position_covariance_type(0) + { + } + + virtual int serialize(unsigned char *outbuffer) const + { + int offset = 0; + offset += this->header.serialize(outbuffer + offset); + offset += this->status.serialize(outbuffer + offset); + offset += serializeAvrFloat64(outbuffer + offset, this->latitude); + offset += serializeAvrFloat64(outbuffer + offset, this->longitude); + offset += serializeAvrFloat64(outbuffer + offset, this->altitude); + for( uint32_t i = 0; i < 9; i++){ + offset += serializeAvrFloat64(outbuffer + offset, this->position_covariance[i]); + } + *(outbuffer + offset + 0) = (this->position_covariance_type >> (8 * 0)) & 0xFF; + offset += sizeof(this->position_covariance_type); + return offset; + } + + virtual int deserialize(unsigned char *inbuffer) + { + int offset = 0; + offset += this->header.deserialize(inbuffer + offset); + offset += this->status.deserialize(inbuffer + offset); + offset += deserializeAvrFloat64(inbuffer + offset, &(this->latitude)); + offset += deserializeAvrFloat64(inbuffer + offset, &(this->longitude)); + offset += deserializeAvrFloat64(inbuffer + offset, &(this->altitude)); + for( uint32_t i = 0; i < 9; i++){ + offset += deserializeAvrFloat64(inbuffer + offset, &(this->position_covariance[i])); + } + this->position_covariance_type = ((uint8_t) (*(inbuffer + offset))); + offset += sizeof(this->position_covariance_type); + return offset; + } + + const char * getType(){ return "sensor_msgs/NavSatFix"; }; + const char * getMD5(){ return "2d3a8cd499b9b4a0249fb98fd05cfa48"; }; + + }; + +} +#endif \ No newline at end of file diff --git a/turtlebot3_core/libraries/ros_lib/sensor_msgs/NavSatStatus.h b/turtlebot3_core/libraries/ros_lib/sensor_msgs/NavSatStatus.h new file mode 100644 index 00000000..f696571a --- /dev/null +++ b/turtlebot3_core/libraries/ros_lib/sensor_msgs/NavSatStatus.h @@ -0,0 +1,71 @@ +#ifndef _ROS_sensor_msgs_NavSatStatus_h +#define _ROS_sensor_msgs_NavSatStatus_h + +#include +#include +#include +#include "ros/msg.h" + +namespace sensor_msgs +{ + + class NavSatStatus : public ros::Msg + { + public: + int8_t status; + uint16_t service; + enum { STATUS_NO_FIX = -1 }; + enum { STATUS_FIX = 0 }; + enum { STATUS_SBAS_FIX = 1 }; + enum { STATUS_GBAS_FIX = 2 }; + enum { SERVICE_GPS = 1 }; + enum { SERVICE_GLONASS = 2 }; + enum { SERVICE_COMPASS = 4 }; + enum { SERVICE_GALILEO = 8 }; + + NavSatStatus(): + status(0), + service(0) + { + } + + virtual int serialize(unsigned char *outbuffer) const + { + int offset = 0; + union { + int8_t real; + uint8_t base; + } u_status; + u_status.real = this->status; + *(outbuffer + offset + 0) = (u_status.base >> (8 * 0)) & 0xFF; + offset += sizeof(this->status); + *(outbuffer + offset + 0) = (this->service >> (8 * 0)) & 0xFF; + *(outbuffer + offset + 1) = (this->service >> (8 * 1)) & 0xFF; + offset += sizeof(this->service); + return offset; + } + + virtual int deserialize(unsigned char *inbuffer) + { + int offset = 0; + union { + int8_t real; + uint8_t base; + } u_status; + u_status.base = 0; + u_status.base |= ((uint8_t) (*(inbuffer + offset + 0))) << (8 * 0); + this->status = u_status.real; + offset += sizeof(this->status); + this->service = ((uint16_t) (*(inbuffer + offset))); + this->service |= ((uint16_t) (*(inbuffer + offset + 1))) << (8 * 1); + offset += sizeof(this->service); + return offset; + } + + const char * getType(){ return "sensor_msgs/NavSatStatus"; }; + const char * getMD5(){ return "331cdbddfa4bc96ffc3b9ad98900a54c"; }; + + }; + +} +#endif \ No newline at end of file diff --git a/turtlebot3_core/libraries/ros_lib/sensor_msgs/PointCloud.h b/turtlebot3_core/libraries/ros_lib/sensor_msgs/PointCloud.h new file mode 100644 index 00000000..40d120a2 --- /dev/null +++ b/turtlebot3_core/libraries/ros_lib/sensor_msgs/PointCloud.h @@ -0,0 +1,93 @@ +#ifndef _ROS_sensor_msgs_PointCloud_h +#define _ROS_sensor_msgs_PointCloud_h + +#include +#include +#include +#include "ros/msg.h" +#include "std_msgs/Header.h" +#include "geometry_msgs/Point32.h" +#include "sensor_msgs/ChannelFloat32.h" + +namespace sensor_msgs +{ + + class PointCloud : public ros::Msg + { + public: + std_msgs::Header header; + uint32_t points_length; + geometry_msgs::Point32 st_points; + geometry_msgs::Point32 * points; + uint32_t channels_length; + sensor_msgs::ChannelFloat32 st_channels; + sensor_msgs::ChannelFloat32 * channels; + + PointCloud(): + header(), + points_length(0), points(NULL), + channels_length(0), channels(NULL) + { + } + + virtual int serialize(unsigned char *outbuffer) const + { + int offset = 0; + offset += this->header.serialize(outbuffer + offset); + *(outbuffer + offset + 0) = (this->points_length >> (8 * 0)) & 0xFF; + *(outbuffer + offset + 1) = (this->points_length >> (8 * 1)) & 0xFF; + *(outbuffer + offset + 2) = (this->points_length >> (8 * 2)) & 0xFF; + *(outbuffer + offset + 3) = (this->points_length >> (8 * 3)) & 0xFF; + offset += sizeof(this->points_length); + for( uint32_t i = 0; i < points_length; i++){ + offset += this->points[i].serialize(outbuffer + offset); + } + *(outbuffer + offset + 0) = (this->channels_length >> (8 * 0)) & 0xFF; + *(outbuffer + offset + 1) = (this->channels_length >> (8 * 1)) & 0xFF; + *(outbuffer + offset + 2) = (this->channels_length >> (8 * 2)) & 0xFF; + *(outbuffer + offset + 3) = (this->channels_length >> (8 * 3)) & 0xFF; + offset += sizeof(this->channels_length); + for( uint32_t i = 0; i < channels_length; i++){ + offset += this->channels[i].serialize(outbuffer + offset); + } + return offset; + } + + virtual int deserialize(unsigned char *inbuffer) + { + int offset = 0; + offset += this->header.deserialize(inbuffer + offset); + uint32_t points_lengthT = ((uint32_t) (*(inbuffer + offset))); + points_lengthT |= ((uint32_t) (*(inbuffer + offset + 1))) << (8 * 1); + points_lengthT |= ((uint32_t) (*(inbuffer + offset + 2))) << (8 * 2); + points_lengthT |= ((uint32_t) (*(inbuffer + offset + 3))) << (8 * 3); + offset += sizeof(this->points_length); + if(points_lengthT > points_length) + this->points = (geometry_msgs::Point32*)realloc(this->points, points_lengthT * sizeof(geometry_msgs::Point32)); + points_length = points_lengthT; + for( uint32_t i = 0; i < points_length; i++){ + offset += this->st_points.deserialize(inbuffer + offset); + memcpy( &(this->points[i]), &(this->st_points), sizeof(geometry_msgs::Point32)); + } + uint32_t channels_lengthT = ((uint32_t) (*(inbuffer + offset))); + channels_lengthT |= ((uint32_t) (*(inbuffer + offset + 1))) << (8 * 1); + channels_lengthT |= ((uint32_t) (*(inbuffer + offset + 2))) << (8 * 2); + channels_lengthT |= ((uint32_t) (*(inbuffer + offset + 3))) << (8 * 3); + offset += sizeof(this->channels_length); + if(channels_lengthT > channels_length) + this->channels = (sensor_msgs::ChannelFloat32*)realloc(this->channels, channels_lengthT * sizeof(sensor_msgs::ChannelFloat32)); + channels_length = channels_lengthT; + for( uint32_t i = 0; i < channels_length; i++){ + offset += this->st_channels.deserialize(inbuffer + offset); + memcpy( &(this->channels[i]), &(this->st_channels), sizeof(sensor_msgs::ChannelFloat32)); + } + return offset; + } + + const char * getType(){ return "sensor_msgs/PointCloud"; }; + const char * getMD5(){ return "d8e9c3f5afbdd8a130fd1d2763945fca"; }; + + }; + +} +#endif \ No newline at end of file diff --git a/turtlebot3_core/libraries/ros_lib/sensor_msgs/PointCloud2.h b/turtlebot3_core/libraries/ros_lib/sensor_msgs/PointCloud2.h new file mode 100644 index 00000000..ed940eea --- /dev/null +++ b/turtlebot3_core/libraries/ros_lib/sensor_msgs/PointCloud2.h @@ -0,0 +1,176 @@ +#ifndef _ROS_sensor_msgs_PointCloud2_h +#define _ROS_sensor_msgs_PointCloud2_h + +#include +#include +#include +#include "ros/msg.h" +#include "std_msgs/Header.h" +#include "sensor_msgs/PointField.h" + +namespace sensor_msgs +{ + + class PointCloud2 : public ros::Msg + { + public: + std_msgs::Header header; + uint32_t height; + uint32_t width; + uint32_t fields_length; + sensor_msgs::PointField st_fields; + sensor_msgs::PointField * fields; + bool is_bigendian; + uint32_t point_step; + uint32_t row_step; + uint32_t data_length; + uint8_t st_data; + uint8_t * data; + bool is_dense; + + PointCloud2(): + header(), + height(0), + width(0), + fields_length(0), fields(NULL), + is_bigendian(0), + point_step(0), + row_step(0), + data_length(0), data(NULL), + is_dense(0) + { + } + + virtual int serialize(unsigned char *outbuffer) const + { + int offset = 0; + offset += this->header.serialize(outbuffer + offset); + *(outbuffer + offset + 0) = (this->height >> (8 * 0)) & 0xFF; + *(outbuffer + offset + 1) = (this->height >> (8 * 1)) & 0xFF; + *(outbuffer + offset + 2) = (this->height >> (8 * 2)) & 0xFF; + *(outbuffer + offset + 3) = (this->height >> (8 * 3)) & 0xFF; + offset += sizeof(this->height); + *(outbuffer + offset + 0) = (this->width >> (8 * 0)) & 0xFF; + *(outbuffer + offset + 1) = (this->width >> (8 * 1)) & 0xFF; + *(outbuffer + offset + 2) = (this->width >> (8 * 2)) & 0xFF; + *(outbuffer + offset + 3) = (this->width >> (8 * 3)) & 0xFF; + offset += sizeof(this->width); + *(outbuffer + offset + 0) = (this->fields_length >> (8 * 0)) & 0xFF; + *(outbuffer + offset + 1) = (this->fields_length >> (8 * 1)) & 0xFF; + *(outbuffer + offset + 2) = (this->fields_length >> (8 * 2)) & 0xFF; + *(outbuffer + offset + 3) = (this->fields_length >> (8 * 3)) & 0xFF; + offset += sizeof(this->fields_length); + for( uint32_t i = 0; i < fields_length; i++){ + offset += this->fields[i].serialize(outbuffer + offset); + } + union { + bool real; + uint8_t base; + } u_is_bigendian; + u_is_bigendian.real = this->is_bigendian; + *(outbuffer + offset + 0) = (u_is_bigendian.base >> (8 * 0)) & 0xFF; + offset += sizeof(this->is_bigendian); + *(outbuffer + offset + 0) = (this->point_step >> (8 * 0)) & 0xFF; + *(outbuffer + offset + 1) = (this->point_step >> (8 * 1)) & 0xFF; + *(outbuffer + offset + 2) = (this->point_step >> (8 * 2)) & 0xFF; + *(outbuffer + offset + 3) = (this->point_step >> (8 * 3)) & 0xFF; + offset += sizeof(this->point_step); + *(outbuffer + offset + 0) = (this->row_step >> (8 * 0)) & 0xFF; + *(outbuffer + offset + 1) = (this->row_step >> (8 * 1)) & 0xFF; + *(outbuffer + offset + 2) = (this->row_step >> (8 * 2)) & 0xFF; + *(outbuffer + offset + 3) = (this->row_step >> (8 * 3)) & 0xFF; + offset += sizeof(this->row_step); + *(outbuffer + offset + 0) = (this->data_length >> (8 * 0)) & 0xFF; + *(outbuffer + offset + 1) = (this->data_length >> (8 * 1)) & 0xFF; + *(outbuffer + offset + 2) = (this->data_length >> (8 * 2)) & 0xFF; + *(outbuffer + offset + 3) = (this->data_length >> (8 * 3)) & 0xFF; + offset += sizeof(this->data_length); + for( uint32_t i = 0; i < data_length; i++){ + *(outbuffer + offset + 0) = (this->data[i] >> (8 * 0)) & 0xFF; + offset += sizeof(this->data[i]); + } + union { + bool real; + uint8_t base; + } u_is_dense; + u_is_dense.real = this->is_dense; + *(outbuffer + offset + 0) = (u_is_dense.base >> (8 * 0)) & 0xFF; + offset += sizeof(this->is_dense); + return offset; + } + + virtual int deserialize(unsigned char *inbuffer) + { + int offset = 0; + offset += this->header.deserialize(inbuffer + offset); + this->height = ((uint32_t) (*(inbuffer + offset))); + this->height |= ((uint32_t) (*(inbuffer + offset + 1))) << (8 * 1); + this->height |= ((uint32_t) (*(inbuffer + offset + 2))) << (8 * 2); + this->height |= ((uint32_t) (*(inbuffer + offset + 3))) << (8 * 3); + offset += sizeof(this->height); + this->width = ((uint32_t) (*(inbuffer + offset))); + this->width |= ((uint32_t) (*(inbuffer + offset + 1))) << (8 * 1); + this->width |= ((uint32_t) (*(inbuffer + offset + 2))) << (8 * 2); + this->width |= ((uint32_t) (*(inbuffer + offset + 3))) << (8 * 3); + offset += sizeof(this->width); + uint32_t fields_lengthT = ((uint32_t) (*(inbuffer + offset))); + fields_lengthT |= ((uint32_t) (*(inbuffer + offset + 1))) << (8 * 1); + fields_lengthT |= ((uint32_t) (*(inbuffer + offset + 2))) << (8 * 2); + fields_lengthT |= ((uint32_t) (*(inbuffer + offset + 3))) << (8 * 3); + offset += sizeof(this->fields_length); + if(fields_lengthT > fields_length) + this->fields = (sensor_msgs::PointField*)realloc(this->fields, fields_lengthT * sizeof(sensor_msgs::PointField)); + fields_length = fields_lengthT; + for( uint32_t i = 0; i < fields_length; i++){ + offset += this->st_fields.deserialize(inbuffer + offset); + memcpy( &(this->fields[i]), &(this->st_fields), sizeof(sensor_msgs::PointField)); + } + union { + bool real; + uint8_t base; + } u_is_bigendian; + u_is_bigendian.base = 0; + u_is_bigendian.base |= ((uint8_t) (*(inbuffer + offset + 0))) << (8 * 0); + this->is_bigendian = u_is_bigendian.real; + offset += sizeof(this->is_bigendian); + this->point_step = ((uint32_t) (*(inbuffer + offset))); + this->point_step |= ((uint32_t) (*(inbuffer + offset + 1))) << (8 * 1); + this->point_step |= ((uint32_t) (*(inbuffer + offset + 2))) << (8 * 2); + this->point_step |= ((uint32_t) (*(inbuffer + offset + 3))) << (8 * 3); + offset += sizeof(this->point_step); + this->row_step = ((uint32_t) (*(inbuffer + offset))); + this->row_step |= ((uint32_t) (*(inbuffer + offset + 1))) << (8 * 1); + this->row_step |= ((uint32_t) (*(inbuffer + offset + 2))) << (8 * 2); + this->row_step |= ((uint32_t) (*(inbuffer + offset + 3))) << (8 * 3); + offset += sizeof(this->row_step); + uint32_t data_lengthT = ((uint32_t) (*(inbuffer + offset))); + data_lengthT |= ((uint32_t) (*(inbuffer + offset + 1))) << (8 * 1); + data_lengthT |= ((uint32_t) (*(inbuffer + offset + 2))) << (8 * 2); + data_lengthT |= ((uint32_t) (*(inbuffer + offset + 3))) << (8 * 3); + offset += sizeof(this->data_length); + if(data_lengthT > data_length) + this->data = (uint8_t*)realloc(this->data, data_lengthT * sizeof(uint8_t)); + data_length = data_lengthT; + for( uint32_t i = 0; i < data_length; i++){ + this->st_data = ((uint8_t) (*(inbuffer + offset))); + offset += sizeof(this->st_data); + memcpy( &(this->data[i]), &(this->st_data), sizeof(uint8_t)); + } + union { + bool real; + uint8_t base; + } u_is_dense; + u_is_dense.base = 0; + u_is_dense.base |= ((uint8_t) (*(inbuffer + offset + 0))) << (8 * 0); + this->is_dense = u_is_dense.real; + offset += sizeof(this->is_dense); + return offset; + } + + const char * getType(){ return "sensor_msgs/PointCloud2"; }; + const char * getMD5(){ return "1158d486dd51d683ce2f1be655c3c181"; }; + + }; + +} +#endif \ No newline at end of file diff --git a/turtlebot3_core/libraries/ros_lib/sensor_msgs/PointField.h b/turtlebot3_core/libraries/ros_lib/sensor_msgs/PointField.h new file mode 100644 index 00000000..5ef0d0e9 --- /dev/null +++ b/turtlebot3_core/libraries/ros_lib/sensor_msgs/PointField.h @@ -0,0 +1,92 @@ +#ifndef _ROS_sensor_msgs_PointField_h +#define _ROS_sensor_msgs_PointField_h + +#include +#include +#include +#include "ros/msg.h" + +namespace sensor_msgs +{ + + class PointField : public ros::Msg + { + public: + const char* name; + uint32_t offset; + uint8_t datatype; + uint32_t count; + enum { INT8 = 1 }; + enum { UINT8 = 2 }; + enum { INT16 = 3 }; + enum { UINT16 = 4 }; + enum { INT32 = 5 }; + enum { UINT32 = 6 }; + enum { FLOAT32 = 7 }; + enum { FLOAT64 = 8 }; + + PointField(): + name(""), + offset(0), + datatype(0), + count(0) + { + } + + virtual int serialize(unsigned char *outbuffer) const + { + int offset = 0; + uint32_t length_name = strlen(this->name); + memcpy(outbuffer + offset, &length_name, sizeof(uint32_t)); + offset += 4; + memcpy(outbuffer + offset, this->name, length_name); + offset += length_name; + *(outbuffer + offset + 0) = (this->offset >> (8 * 0)) & 0xFF; + *(outbuffer + offset + 1) = (this->offset >> (8 * 1)) & 0xFF; + *(outbuffer + offset + 2) = (this->offset >> (8 * 2)) & 0xFF; + *(outbuffer + offset + 3) = (this->offset >> (8 * 3)) & 0xFF; + offset += sizeof(this->offset); + *(outbuffer + offset + 0) = (this->datatype >> (8 * 0)) & 0xFF; + offset += sizeof(this->datatype); + *(outbuffer + offset + 0) = (this->count >> (8 * 0)) & 0xFF; + *(outbuffer + offset + 1) = (this->count >> (8 * 1)) & 0xFF; + *(outbuffer + offset + 2) = (this->count >> (8 * 2)) & 0xFF; + *(outbuffer + offset + 3) = (this->count >> (8 * 3)) & 0xFF; + offset += sizeof(this->count); + return offset; + } + + virtual int deserialize(unsigned char *inbuffer) + { + int offset = 0; + uint32_t length_name; + memcpy(&length_name, (inbuffer + offset), sizeof(uint32_t)); + offset += 4; + for(unsigned int k= offset; k< offset+length_name; ++k){ + inbuffer[k-1]=inbuffer[k]; + } + inbuffer[offset+length_name-1]=0; + this->name = (char *)(inbuffer + offset-1); + offset += length_name; + this->offset = ((uint32_t) (*(inbuffer + offset))); + this->offset |= ((uint32_t) (*(inbuffer + offset + 1))) << (8 * 1); + this->offset |= ((uint32_t) (*(inbuffer + offset + 2))) << (8 * 2); + this->offset |= ((uint32_t) (*(inbuffer + offset + 3))) << (8 * 3); + offset += sizeof(this->offset); + this->datatype = ((uint8_t) (*(inbuffer + offset))); + offset += sizeof(this->datatype); + this->count = ((uint32_t) (*(inbuffer + offset))); + this->count |= ((uint32_t) (*(inbuffer + offset + 1))) << (8 * 1); + this->count |= ((uint32_t) (*(inbuffer + offset + 2))) << (8 * 2); + this->count |= ((uint32_t) (*(inbuffer + offset + 3))) << (8 * 3); + offset += sizeof(this->count); + return offset; + } + + const char * getType(){ return "sensor_msgs/PointField"; }; + const char * getMD5(){ return "268eacb2962780ceac86cbd17e328150"; }; + + }; + +} +#endif \ No newline at end of file diff --git a/turtlebot3_core/libraries/ros_lib/sensor_msgs/Range.h b/turtlebot3_core/libraries/ros_lib/sensor_msgs/Range.h new file mode 100644 index 00000000..a8ddcaa9 --- /dev/null +++ b/turtlebot3_core/libraries/ros_lib/sensor_msgs/Range.h @@ -0,0 +1,143 @@ +#ifndef _ROS_sensor_msgs_Range_h +#define _ROS_sensor_msgs_Range_h + +#include +#include +#include +#include "ros/msg.h" +#include "std_msgs/Header.h" + +namespace sensor_msgs +{ + + class Range : public ros::Msg + { + public: + std_msgs::Header header; + uint8_t radiation_type; + float field_of_view; + float min_range; + float max_range; + float range; + enum { ULTRASOUND = 0 }; + enum { INFRARED = 1 }; + + Range(): + header(), + radiation_type(0), + field_of_view(0), + min_range(0), + max_range(0), + range(0) + { + } + + virtual int serialize(unsigned char *outbuffer) const + { + int offset = 0; + offset += this->header.serialize(outbuffer + offset); + *(outbuffer + offset + 0) = (this->radiation_type >> (8 * 0)) & 0xFF; + offset += sizeof(this->radiation_type); + union { + float real; + uint32_t base; + } u_field_of_view; + u_field_of_view.real = this->field_of_view; + *(outbuffer + offset + 0) = (u_field_of_view.base >> (8 * 0)) & 0xFF; + *(outbuffer + offset + 1) = (u_field_of_view.base >> (8 * 1)) & 0xFF; + *(outbuffer + offset + 2) = (u_field_of_view.base >> (8 * 2)) & 0xFF; + *(outbuffer + offset + 3) = (u_field_of_view.base >> (8 * 3)) & 0xFF; + offset += sizeof(this->field_of_view); + union { + float real; + uint32_t base; + } u_min_range; + u_min_range.real = this->min_range; + *(outbuffer + offset + 0) = (u_min_range.base >> (8 * 0)) & 0xFF; + *(outbuffer + offset + 1) = (u_min_range.base >> (8 * 1)) & 0xFF; + *(outbuffer + offset + 2) = (u_min_range.base >> (8 * 2)) & 0xFF; + *(outbuffer + offset + 3) = (u_min_range.base >> (8 * 3)) & 0xFF; + offset += sizeof(this->min_range); + union { + float real; + uint32_t base; + } u_max_range; + u_max_range.real = this->max_range; + *(outbuffer + offset + 0) = (u_max_range.base >> (8 * 0)) & 0xFF; + *(outbuffer + offset + 1) = (u_max_range.base >> (8 * 1)) & 0xFF; + *(outbuffer + offset + 2) = (u_max_range.base >> (8 * 2)) & 0xFF; + *(outbuffer + offset + 3) = (u_max_range.base >> (8 * 3)) & 0xFF; + offset += sizeof(this->max_range); + union { + float real; + uint32_t base; + } u_range; + u_range.real = this->range; + *(outbuffer + offset + 0) = (u_range.base >> (8 * 0)) & 0xFF; + *(outbuffer + offset + 1) = (u_range.base >> (8 * 1)) & 0xFF; + *(outbuffer + offset + 2) = (u_range.base >> (8 * 2)) & 0xFF; + *(outbuffer + offset + 3) = (u_range.base >> (8 * 3)) & 0xFF; + offset += sizeof(this->range); + return offset; + } + + virtual int deserialize(unsigned char *inbuffer) + { + int offset = 0; + offset += this->header.deserialize(inbuffer + offset); + this->radiation_type = ((uint8_t) (*(inbuffer + offset))); + offset += sizeof(this->radiation_type); + union { + float real; + uint32_t base; + } u_field_of_view; + u_field_of_view.base = 0; + u_field_of_view.base |= ((uint32_t) (*(inbuffer + offset + 0))) << (8 * 0); + u_field_of_view.base |= ((uint32_t) (*(inbuffer + offset + 1))) << (8 * 1); + u_field_of_view.base |= ((uint32_t) (*(inbuffer + offset + 2))) << (8 * 2); + u_field_of_view.base |= ((uint32_t) (*(inbuffer + offset + 3))) << (8 * 3); + this->field_of_view = u_field_of_view.real; + offset += sizeof(this->field_of_view); + union { + float real; + uint32_t base; + } u_min_range; + u_min_range.base = 0; + u_min_range.base |= ((uint32_t) (*(inbuffer + offset + 0))) << (8 * 0); + u_min_range.base |= ((uint32_t) (*(inbuffer + offset + 1))) << (8 * 1); + u_min_range.base |= ((uint32_t) (*(inbuffer + offset + 2))) << (8 * 2); + u_min_range.base |= ((uint32_t) (*(inbuffer + offset + 3))) << (8 * 3); + this->min_range = u_min_range.real; + offset += sizeof(this->min_range); + union { + float real; + uint32_t base; + } u_max_range; + u_max_range.base = 0; + u_max_range.base |= ((uint32_t) (*(inbuffer + offset + 0))) << (8 * 0); + u_max_range.base |= ((uint32_t) (*(inbuffer + offset + 1))) << (8 * 1); + u_max_range.base |= ((uint32_t) (*(inbuffer + offset + 2))) << (8 * 2); + u_max_range.base |= ((uint32_t) (*(inbuffer + offset + 3))) << (8 * 3); + this->max_range = u_max_range.real; + offset += sizeof(this->max_range); + union { + float real; + uint32_t base; + } u_range; + u_range.base = 0; + u_range.base |= ((uint32_t) (*(inbuffer + offset + 0))) << (8 * 0); + u_range.base |= ((uint32_t) (*(inbuffer + offset + 1))) << (8 * 1); + u_range.base |= ((uint32_t) (*(inbuffer + offset + 2))) << (8 * 2); + u_range.base |= ((uint32_t) (*(inbuffer + offset + 3))) << (8 * 3); + this->range = u_range.real; + offset += sizeof(this->range); + return offset; + } + + const char * getType(){ return "sensor_msgs/Range"; }; + const char * getMD5(){ return "c005c34273dc426c67a020a87bc24148"; }; + + }; + +} +#endif \ No newline at end of file diff --git a/turtlebot3_core/libraries/ros_lib/sensor_msgs/RegionOfInterest.h b/turtlebot3_core/libraries/ros_lib/sensor_msgs/RegionOfInterest.h new file mode 100644 index 00000000..ebdb9b34 --- /dev/null +++ b/turtlebot3_core/libraries/ros_lib/sensor_msgs/RegionOfInterest.h @@ -0,0 +1,103 @@ +#ifndef _ROS_sensor_msgs_RegionOfInterest_h +#define _ROS_sensor_msgs_RegionOfInterest_h + +#include +#include +#include +#include "ros/msg.h" + +namespace sensor_msgs +{ + + class RegionOfInterest : public ros::Msg + { + public: + uint32_t x_offset; + uint32_t y_offset; + uint32_t height; + uint32_t width; + bool do_rectify; + + RegionOfInterest(): + x_offset(0), + y_offset(0), + height(0), + width(0), + do_rectify(0) + { + } + + virtual int serialize(unsigned char *outbuffer) const + { + int offset = 0; + *(outbuffer + offset + 0) = (this->x_offset >> (8 * 0)) & 0xFF; + *(outbuffer + offset + 1) = (this->x_offset >> (8 * 1)) & 0xFF; + *(outbuffer + offset + 2) = (this->x_offset >> (8 * 2)) & 0xFF; + *(outbuffer + offset + 3) = (this->x_offset >> (8 * 3)) & 0xFF; + offset += sizeof(this->x_offset); + *(outbuffer + offset + 0) = (this->y_offset >> (8 * 0)) & 0xFF; + *(outbuffer + offset + 1) = (this->y_offset >> (8 * 1)) & 0xFF; + *(outbuffer + offset + 2) = (this->y_offset >> (8 * 2)) & 0xFF; + *(outbuffer + offset + 3) = (this->y_offset >> (8 * 3)) & 0xFF; + offset += sizeof(this->y_offset); + *(outbuffer + offset + 0) = (this->height >> (8 * 0)) & 0xFF; + *(outbuffer + offset + 1) = (this->height >> (8 * 1)) & 0xFF; + *(outbuffer + offset + 2) = (this->height >> (8 * 2)) & 0xFF; + *(outbuffer + offset + 3) = (this->height >> (8 * 3)) & 0xFF; + offset += sizeof(this->height); + *(outbuffer + offset + 0) = (this->width >> (8 * 0)) & 0xFF; + *(outbuffer + offset + 1) = (this->width >> (8 * 1)) & 0xFF; + *(outbuffer + offset + 2) = (this->width >> (8 * 2)) & 0xFF; + *(outbuffer + offset + 3) = (this->width >> (8 * 3)) & 0xFF; + offset += sizeof(this->width); + union { + bool real; + uint8_t base; + } u_do_rectify; + u_do_rectify.real = this->do_rectify; + *(outbuffer + offset + 0) = (u_do_rectify.base >> (8 * 0)) & 0xFF; + offset += sizeof(this->do_rectify); + return offset; + } + + virtual int deserialize(unsigned char *inbuffer) + { + int offset = 0; + this->x_offset = ((uint32_t) (*(inbuffer + offset))); + this->x_offset |= ((uint32_t) (*(inbuffer + offset + 1))) << (8 * 1); + this->x_offset |= ((uint32_t) (*(inbuffer + offset + 2))) << (8 * 2); + this->x_offset |= ((uint32_t) (*(inbuffer + offset + 3))) << (8 * 3); + offset += sizeof(this->x_offset); + this->y_offset = ((uint32_t) (*(inbuffer + offset))); + this->y_offset |= ((uint32_t) (*(inbuffer + offset + 1))) << (8 * 1); + this->y_offset |= ((uint32_t) (*(inbuffer + offset + 2))) << (8 * 2); + this->y_offset |= ((uint32_t) (*(inbuffer + offset + 3))) << (8 * 3); + offset += sizeof(this->y_offset); + this->height = ((uint32_t) (*(inbuffer + offset))); + this->height |= ((uint32_t) (*(inbuffer + offset + 1))) << (8 * 1); + this->height |= ((uint32_t) (*(inbuffer + offset + 2))) << (8 * 2); + this->height |= ((uint32_t) (*(inbuffer + offset + 3))) << (8 * 3); + offset += sizeof(this->height); + this->width = ((uint32_t) (*(inbuffer + offset))); + this->width |= ((uint32_t) (*(inbuffer + offset + 1))) << (8 * 1); + this->width |= ((uint32_t) (*(inbuffer + offset + 2))) << (8 * 2); + this->width |= ((uint32_t) (*(inbuffer + offset + 3))) << (8 * 3); + offset += sizeof(this->width); + union { + bool real; + uint8_t base; + } u_do_rectify; + u_do_rectify.base = 0; + u_do_rectify.base |= ((uint8_t) (*(inbuffer + offset + 0))) << (8 * 0); + this->do_rectify = u_do_rectify.real; + offset += sizeof(this->do_rectify); + return offset; + } + + const char * getType(){ return "sensor_msgs/RegionOfInterest"; }; + const char * getMD5(){ return "bdb633039d588fcccb441a4d43ccfe09"; }; + + }; + +} +#endif \ No newline at end of file diff --git a/turtlebot3_core/libraries/ros_lib/sensor_msgs/RelativeHumidity.h b/turtlebot3_core/libraries/ros_lib/sensor_msgs/RelativeHumidity.h new file mode 100644 index 00000000..2ec392a5 --- /dev/null +++ b/turtlebot3_core/libraries/ros_lib/sensor_msgs/RelativeHumidity.h @@ -0,0 +1,51 @@ +#ifndef _ROS_sensor_msgs_RelativeHumidity_h +#define _ROS_sensor_msgs_RelativeHumidity_h + +#include +#include +#include +#include "ros/msg.h" +#include "std_msgs/Header.h" + +namespace sensor_msgs +{ + + class RelativeHumidity : public ros::Msg + { + public: + std_msgs::Header header; + float relative_humidity; + float variance; + + RelativeHumidity(): + header(), + relative_humidity(0), + variance(0) + { + } + + virtual int serialize(unsigned char *outbuffer) const + { + int offset = 0; + offset += this->header.serialize(outbuffer + offset); + offset += serializeAvrFloat64(outbuffer + offset, this->relative_humidity); + offset += serializeAvrFloat64(outbuffer + offset, this->variance); + return offset; + } + + virtual int deserialize(unsigned char *inbuffer) + { + int offset = 0; + offset += this->header.deserialize(inbuffer + offset); + offset += deserializeAvrFloat64(inbuffer + offset, &(this->relative_humidity)); + offset += deserializeAvrFloat64(inbuffer + offset, &(this->variance)); + return offset; + } + + const char * getType(){ return "sensor_msgs/RelativeHumidity"; }; + const char * getMD5(){ return "8730015b05955b7e992ce29a2678d90f"; }; + + }; + +} +#endif \ No newline at end of file diff --git a/turtlebot3_core/libraries/ros_lib/sensor_msgs/SetCameraInfo.h b/turtlebot3_core/libraries/ros_lib/sensor_msgs/SetCameraInfo.h new file mode 100644 index 00000000..9f49b674 --- /dev/null +++ b/turtlebot3_core/libraries/ros_lib/sensor_msgs/SetCameraInfo.h @@ -0,0 +1,108 @@ +#ifndef _ROS_SERVICE_SetCameraInfo_h +#define _ROS_SERVICE_SetCameraInfo_h +#include +#include +#include +#include "ros/msg.h" +#include "sensor_msgs/CameraInfo.h" + +namespace sensor_msgs +{ + +static const char SETCAMERAINFO[] = "sensor_msgs/SetCameraInfo"; + + class SetCameraInfoRequest : public ros::Msg + { + public: + sensor_msgs::CameraInfo camera_info; + + SetCameraInfoRequest(): + camera_info() + { + } + + virtual int serialize(unsigned char *outbuffer) const + { + int offset = 0; + offset += this->camera_info.serialize(outbuffer + offset); + return offset; + } + + virtual int deserialize(unsigned char *inbuffer) + { + int offset = 0; + offset += this->camera_info.deserialize(inbuffer + offset); + return offset; + } + + const char * getType(){ return SETCAMERAINFO; }; + const char * getMD5(){ return "ee34be01fdeee563d0d99cd594d5581d"; }; + + }; + + class SetCameraInfoResponse : public ros::Msg + { + public: + bool success; + const char* status_message; + + SetCameraInfoResponse(): + success(0), + status_message("") + { + } + + virtual int serialize(unsigned char *outbuffer) const + { + int offset = 0; + union { + bool real; + uint8_t base; + } u_success; + u_success.real = this->success; + *(outbuffer + offset + 0) = (u_success.base >> (8 * 0)) & 0xFF; + offset += sizeof(this->success); + uint32_t length_status_message = strlen(this->status_message); + memcpy(outbuffer + offset, &length_status_message, sizeof(uint32_t)); + offset += 4; + memcpy(outbuffer + offset, this->status_message, length_status_message); + offset += length_status_message; + return offset; + } + + virtual int deserialize(unsigned char *inbuffer) + { + int offset = 0; + union { + bool real; + uint8_t base; + } u_success; + u_success.base = 0; + u_success.base |= ((uint8_t) (*(inbuffer + offset + 0))) << (8 * 0); + this->success = u_success.real; + offset += sizeof(this->success); + uint32_t length_status_message; + memcpy(&length_status_message, (inbuffer + offset), sizeof(uint32_t)); + offset += 4; + for(unsigned int k= offset; k< offset+length_status_message; ++k){ + inbuffer[k-1]=inbuffer[k]; + } + inbuffer[offset+length_status_message-1]=0; + this->status_message = (char *)(inbuffer + offset-1); + offset += length_status_message; + return offset; + } + + const char * getType(){ return SETCAMERAINFO; }; + const char * getMD5(){ return "2ec6f3eff0161f4257b808b12bc830c2"; }; + + }; + + class SetCameraInfo { + public: + typedef SetCameraInfoRequest Request; + typedef SetCameraInfoResponse Response; + }; + +} +#endif diff --git a/turtlebot3_core/libraries/ros_lib/sensor_msgs/Temperature.h b/turtlebot3_core/libraries/ros_lib/sensor_msgs/Temperature.h new file mode 100644 index 00000000..f630a39b --- /dev/null +++ b/turtlebot3_core/libraries/ros_lib/sensor_msgs/Temperature.h @@ -0,0 +1,51 @@ +#ifndef _ROS_sensor_msgs_Temperature_h +#define _ROS_sensor_msgs_Temperature_h + +#include +#include +#include +#include "ros/msg.h" +#include "std_msgs/Header.h" + +namespace sensor_msgs +{ + + class Temperature : public ros::Msg + { + public: + std_msgs::Header header; + float temperature; + float variance; + + Temperature(): + header(), + temperature(0), + variance(0) + { + } + + virtual int serialize(unsigned char *outbuffer) const + { + int offset = 0; + offset += this->header.serialize(outbuffer + offset); + offset += serializeAvrFloat64(outbuffer + offset, this->temperature); + offset += serializeAvrFloat64(outbuffer + offset, this->variance); + return offset; + } + + virtual int deserialize(unsigned char *inbuffer) + { + int offset = 0; + offset += this->header.deserialize(inbuffer + offset); + offset += deserializeAvrFloat64(inbuffer + offset, &(this->temperature)); + offset += deserializeAvrFloat64(inbuffer + offset, &(this->variance)); + return offset; + } + + const char * getType(){ return "sensor_msgs/Temperature"; }; + const char * getMD5(){ return "ff71b307acdbe7c871a5a6d7ed359100"; }; + + }; + +} +#endif \ No newline at end of file diff --git a/turtlebot3_core/libraries/ros_lib/sensor_msgs/TimeReference.h b/turtlebot3_core/libraries/ros_lib/sensor_msgs/TimeReference.h new file mode 100644 index 00000000..eab85e6f --- /dev/null +++ b/turtlebot3_core/libraries/ros_lib/sensor_msgs/TimeReference.h @@ -0,0 +1,82 @@ +#ifndef _ROS_sensor_msgs_TimeReference_h +#define _ROS_sensor_msgs_TimeReference_h + +#include +#include +#include +#include "ros/msg.h" +#include "std_msgs/Header.h" +#include "ros/time.h" + +namespace sensor_msgs +{ + + class TimeReference : public ros::Msg + { + public: + std_msgs::Header header; + ros::Time time_ref; + const char* source; + + TimeReference(): + header(), + time_ref(), + source("") + { + } + + virtual int serialize(unsigned char *outbuffer) const + { + int offset = 0; + offset += this->header.serialize(outbuffer + offset); + *(outbuffer + offset + 0) = (this->time_ref.sec >> (8 * 0)) & 0xFF; + *(outbuffer + offset + 1) = (this->time_ref.sec >> (8 * 1)) & 0xFF; + *(outbuffer + offset + 2) = (this->time_ref.sec >> (8 * 2)) & 0xFF; + *(outbuffer + offset + 3) = (this->time_ref.sec >> (8 * 3)) & 0xFF; + offset += sizeof(this->time_ref.sec); + *(outbuffer + offset + 0) = (this->time_ref.nsec >> (8 * 0)) & 0xFF; + *(outbuffer + offset + 1) = (this->time_ref.nsec >> (8 * 1)) & 0xFF; + *(outbuffer + offset + 2) = (this->time_ref.nsec >> (8 * 2)) & 0xFF; + *(outbuffer + offset + 3) = (this->time_ref.nsec >> (8 * 3)) & 0xFF; + offset += sizeof(this->time_ref.nsec); + uint32_t length_source = strlen(this->source); + memcpy(outbuffer + offset, &length_source, sizeof(uint32_t)); + offset += 4; + memcpy(outbuffer + offset, this->source, length_source); + offset += length_source; + return offset; + } + + virtual int deserialize(unsigned char *inbuffer) + { + int offset = 0; + offset += this->header.deserialize(inbuffer + offset); + this->time_ref.sec = ((uint32_t) (*(inbuffer + offset))); + this->time_ref.sec |= ((uint32_t) (*(inbuffer + offset + 1))) << (8 * 1); + this->time_ref.sec |= ((uint32_t) (*(inbuffer + offset + 2))) << (8 * 2); + this->time_ref.sec |= ((uint32_t) (*(inbuffer + offset + 3))) << (8 * 3); + offset += sizeof(this->time_ref.sec); + this->time_ref.nsec = ((uint32_t) (*(inbuffer + offset))); + this->time_ref.nsec |= ((uint32_t) (*(inbuffer + offset + 1))) << (8 * 1); + this->time_ref.nsec |= ((uint32_t) (*(inbuffer + offset + 2))) << (8 * 2); + this->time_ref.nsec |= ((uint32_t) (*(inbuffer + offset + 3))) << (8 * 3); + offset += sizeof(this->time_ref.nsec); + uint32_t length_source; + memcpy(&length_source, (inbuffer + offset), sizeof(uint32_t)); + offset += 4; + for(unsigned int k= offset; k< offset+length_source; ++k){ + inbuffer[k-1]=inbuffer[k]; + } + inbuffer[offset+length_source-1]=0; + this->source = (char *)(inbuffer + offset-1); + offset += length_source; + return offset; + } + + const char * getType(){ return "sensor_msgs/TimeReference"; }; + const char * getMD5(){ return "fded64a0265108ba86c3d38fb11c0c16"; }; + + }; + +} +#endif \ No newline at end of file diff --git a/turtlebot3_core/libraries/ros_lib/std_msgs/Bool.h b/turtlebot3_core/libraries/ros_lib/std_msgs/Bool.h new file mode 100644 index 00000000..1ec6d07f --- /dev/null +++ b/turtlebot3_core/libraries/ros_lib/std_msgs/Bool.h @@ -0,0 +1,55 @@ +#ifndef _ROS_std_msgs_Bool_h +#define _ROS_std_msgs_Bool_h + +#include +#include +#include +#include "ros/msg.h" + +namespace std_msgs +{ + + class Bool : public ros::Msg + { + public: + bool data; + + Bool(): + data(0) + { + } + + virtual int serialize(unsigned char *outbuffer) const + { + int offset = 0; + union { + bool real; + uint8_t base; + } u_data; + u_data.real = this->data; + *(outbuffer + offset + 0) = (u_data.base >> (8 * 0)) & 0xFF; + offset += sizeof(this->data); + return offset; + } + + virtual int deserialize(unsigned char *inbuffer) + { + int offset = 0; + union { + bool real; + uint8_t base; + } u_data; + u_data.base = 0; + u_data.base |= ((uint8_t) (*(inbuffer + offset + 0))) << (8 * 0); + this->data = u_data.real; + offset += sizeof(this->data); + return offset; + } + + const char * getType(){ return "std_msgs/Bool"; }; + const char * getMD5(){ return "8b94c1b53db61fb6aed406028ad6332a"; }; + + }; + +} +#endif \ No newline at end of file diff --git a/turtlebot3_core/libraries/ros_lib/std_msgs/Byte.h b/turtlebot3_core/libraries/ros_lib/std_msgs/Byte.h new file mode 100644 index 00000000..cbb1bc27 --- /dev/null +++ b/turtlebot3_core/libraries/ros_lib/std_msgs/Byte.h @@ -0,0 +1,55 @@ +#ifndef _ROS_std_msgs_Byte_h +#define _ROS_std_msgs_Byte_h + +#include +#include +#include +#include "ros/msg.h" + +namespace std_msgs +{ + + class Byte : public ros::Msg + { + public: + int8_t data; + + Byte(): + data(0) + { + } + + virtual int serialize(unsigned char *outbuffer) const + { + int offset = 0; + union { + int8_t real; + uint8_t base; + } u_data; + u_data.real = this->data; + *(outbuffer + offset + 0) = (u_data.base >> (8 * 0)) & 0xFF; + offset += sizeof(this->data); + return offset; + } + + virtual int deserialize(unsigned char *inbuffer) + { + int offset = 0; + union { + int8_t real; + uint8_t base; + } u_data; + u_data.base = 0; + u_data.base |= ((uint8_t) (*(inbuffer + offset + 0))) << (8 * 0); + this->data = u_data.real; + offset += sizeof(this->data); + return offset; + } + + const char * getType(){ return "std_msgs/Byte"; }; + const char * getMD5(){ return "ad736a2e8818154c487bb80fe42ce43b"; }; + + }; + +} +#endif \ No newline at end of file diff --git a/turtlebot3_core/libraries/ros_lib/std_msgs/ByteMultiArray.h b/turtlebot3_core/libraries/ros_lib/std_msgs/ByteMultiArray.h new file mode 100644 index 00000000..1ac1356d --- /dev/null +++ b/turtlebot3_core/libraries/ros_lib/std_msgs/ByteMultiArray.h @@ -0,0 +1,80 @@ +#ifndef _ROS_std_msgs_ByteMultiArray_h +#define _ROS_std_msgs_ByteMultiArray_h + +#include +#include +#include +#include "ros/msg.h" +#include "std_msgs/MultiArrayLayout.h" + +namespace std_msgs +{ + + class ByteMultiArray : public ros::Msg + { + public: + std_msgs::MultiArrayLayout layout; + uint32_t data_length; + int8_t st_data; + int8_t * data; + + ByteMultiArray(): + layout(), + data_length(0), data(NULL) + { + } + + virtual int serialize(unsigned char *outbuffer) const + { + int offset = 0; + offset += this->layout.serialize(outbuffer + offset); + *(outbuffer + offset + 0) = (this->data_length >> (8 * 0)) & 0xFF; + *(outbuffer + offset + 1) = (this->data_length >> (8 * 1)) & 0xFF; + *(outbuffer + offset + 2) = (this->data_length >> (8 * 2)) & 0xFF; + *(outbuffer + offset + 3) = (this->data_length >> (8 * 3)) & 0xFF; + offset += sizeof(this->data_length); + for( uint32_t i = 0; i < data_length; i++){ + union { + int8_t real; + uint8_t base; + } u_datai; + u_datai.real = this->data[i]; + *(outbuffer + offset + 0) = (u_datai.base >> (8 * 0)) & 0xFF; + offset += sizeof(this->data[i]); + } + return offset; + } + + virtual int deserialize(unsigned char *inbuffer) + { + int offset = 0; + offset += this->layout.deserialize(inbuffer + offset); + uint32_t data_lengthT = ((uint32_t) (*(inbuffer + offset))); + data_lengthT |= ((uint32_t) (*(inbuffer + offset + 1))) << (8 * 1); + data_lengthT |= ((uint32_t) (*(inbuffer + offset + 2))) << (8 * 2); + data_lengthT |= ((uint32_t) (*(inbuffer + offset + 3))) << (8 * 3); + offset += sizeof(this->data_length); + if(data_lengthT > data_length) + this->data = (int8_t*)realloc(this->data, data_lengthT * sizeof(int8_t)); + data_length = data_lengthT; + for( uint32_t i = 0; i < data_length; i++){ + union { + int8_t real; + uint8_t base; + } u_st_data; + u_st_data.base = 0; + u_st_data.base |= ((uint8_t) (*(inbuffer + offset + 0))) << (8 * 0); + this->st_data = u_st_data.real; + offset += sizeof(this->st_data); + memcpy( &(this->data[i]), &(this->st_data), sizeof(int8_t)); + } + return offset; + } + + const char * getType(){ return "std_msgs/ByteMultiArray"; }; + const char * getMD5(){ return "70ea476cbcfd65ac2f68f3cda1e891fe"; }; + + }; + +} +#endif \ No newline at end of file diff --git a/turtlebot3_core/libraries/ros_lib/std_msgs/Char.h b/turtlebot3_core/libraries/ros_lib/std_msgs/Char.h new file mode 100644 index 00000000..977cc8b8 --- /dev/null +++ b/turtlebot3_core/libraries/ros_lib/std_msgs/Char.h @@ -0,0 +1,44 @@ +#ifndef _ROS_std_msgs_Char_h +#define _ROS_std_msgs_Char_h + +#include +#include +#include +#include "ros/msg.h" + +namespace std_msgs +{ + + class Char : public ros::Msg + { + public: + uint8_t data; + + Char(): + data(0) + { + } + + virtual int serialize(unsigned char *outbuffer) const + { + int offset = 0; + *(outbuffer + offset + 0) = (this->data >> (8 * 0)) & 0xFF; + offset += sizeof(this->data); + return offset; + } + + virtual int deserialize(unsigned char *inbuffer) + { + int offset = 0; + this->data = ((uint8_t) (*(inbuffer + offset))); + offset += sizeof(this->data); + return offset; + } + + const char * getType(){ return "std_msgs/Char"; }; + const char * getMD5(){ return "1bf77f25acecdedba0e224b162199717"; }; + + }; + +} +#endif \ No newline at end of file diff --git a/turtlebot3_core/libraries/ros_lib/std_msgs/ColorRGBA.h b/turtlebot3_core/libraries/ros_lib/std_msgs/ColorRGBA.h new file mode 100644 index 00000000..d458c911 --- /dev/null +++ b/turtlebot3_core/libraries/ros_lib/std_msgs/ColorRGBA.h @@ -0,0 +1,130 @@ +#ifndef _ROS_std_msgs_ColorRGBA_h +#define _ROS_std_msgs_ColorRGBA_h + +#include +#include +#include +#include "ros/msg.h" + +namespace std_msgs +{ + + class ColorRGBA : public ros::Msg + { + public: + float r; + float g; + float b; + float a; + + ColorRGBA(): + r(0), + g(0), + b(0), + a(0) + { + } + + virtual int serialize(unsigned char *outbuffer) const + { + int offset = 0; + union { + float real; + uint32_t base; + } u_r; + u_r.real = this->r; + *(outbuffer + offset + 0) = (u_r.base >> (8 * 0)) & 0xFF; + *(outbuffer + offset + 1) = (u_r.base >> (8 * 1)) & 0xFF; + *(outbuffer + offset + 2) = (u_r.base >> (8 * 2)) & 0xFF; + *(outbuffer + offset + 3) = (u_r.base >> (8 * 3)) & 0xFF; + offset += sizeof(this->r); + union { + float real; + uint32_t base; + } u_g; + u_g.real = this->g; + *(outbuffer + offset + 0) = (u_g.base >> (8 * 0)) & 0xFF; + *(outbuffer + offset + 1) = (u_g.base >> (8 * 1)) & 0xFF; + *(outbuffer + offset + 2) = (u_g.base >> (8 * 2)) & 0xFF; + *(outbuffer + offset + 3) = (u_g.base >> (8 * 3)) & 0xFF; + offset += sizeof(this->g); + union { + float real; + uint32_t base; + } u_b; + u_b.real = this->b; + *(outbuffer + offset + 0) = (u_b.base >> (8 * 0)) & 0xFF; + *(outbuffer + offset + 1) = (u_b.base >> (8 * 1)) & 0xFF; + *(outbuffer + offset + 2) = (u_b.base >> (8 * 2)) & 0xFF; + *(outbuffer + offset + 3) = (u_b.base >> (8 * 3)) & 0xFF; + offset += sizeof(this->b); + union { + float real; + uint32_t base; + } u_a; + u_a.real = this->a; + *(outbuffer + offset + 0) = (u_a.base >> (8 * 0)) & 0xFF; + *(outbuffer + offset + 1) = (u_a.base >> (8 * 1)) & 0xFF; + *(outbuffer + offset + 2) = (u_a.base >> (8 * 2)) & 0xFF; + *(outbuffer + offset + 3) = (u_a.base >> (8 * 3)) & 0xFF; + offset += sizeof(this->a); + return offset; + } + + virtual int deserialize(unsigned char *inbuffer) + { + int offset = 0; + union { + float real; + uint32_t base; + } u_r; + u_r.base = 0; + u_r.base |= ((uint32_t) (*(inbuffer + offset + 0))) << (8 * 0); + u_r.base |= ((uint32_t) (*(inbuffer + offset + 1))) << (8 * 1); + u_r.base |= ((uint32_t) (*(inbuffer + offset + 2))) << (8 * 2); + u_r.base |= ((uint32_t) (*(inbuffer + offset + 3))) << (8 * 3); + this->r = u_r.real; + offset += sizeof(this->r); + union { + float real; + uint32_t base; + } u_g; + u_g.base = 0; + u_g.base |= ((uint32_t) (*(inbuffer + offset + 0))) << (8 * 0); + u_g.base |= ((uint32_t) (*(inbuffer + offset + 1))) << (8 * 1); + u_g.base |= ((uint32_t) (*(inbuffer + offset + 2))) << (8 * 2); + u_g.base |= ((uint32_t) (*(inbuffer + offset + 3))) << (8 * 3); + this->g = u_g.real; + offset += sizeof(this->g); + union { + float real; + uint32_t base; + } u_b; + u_b.base = 0; + u_b.base |= ((uint32_t) (*(inbuffer + offset + 0))) << (8 * 0); + u_b.base |= ((uint32_t) (*(inbuffer + offset + 1))) << (8 * 1); + u_b.base |= ((uint32_t) (*(inbuffer + offset + 2))) << (8 * 2); + u_b.base |= ((uint32_t) (*(inbuffer + offset + 3))) << (8 * 3); + this->b = u_b.real; + offset += sizeof(this->b); + union { + float real; + uint32_t base; + } u_a; + u_a.base = 0; + u_a.base |= ((uint32_t) (*(inbuffer + offset + 0))) << (8 * 0); + u_a.base |= ((uint32_t) (*(inbuffer + offset + 1))) << (8 * 1); + u_a.base |= ((uint32_t) (*(inbuffer + offset + 2))) << (8 * 2); + u_a.base |= ((uint32_t) (*(inbuffer + offset + 3))) << (8 * 3); + this->a = u_a.real; + offset += sizeof(this->a); + return offset; + } + + const char * getType(){ return "std_msgs/ColorRGBA"; }; + const char * getMD5(){ return "a29a96539573343b1310c73607334b00"; }; + + }; + +} +#endif \ No newline at end of file diff --git a/turtlebot3_core/libraries/ros_lib/std_msgs/Duration.h b/turtlebot3_core/libraries/ros_lib/std_msgs/Duration.h new file mode 100644 index 00000000..883a7524 --- /dev/null +++ b/turtlebot3_core/libraries/ros_lib/std_msgs/Duration.h @@ -0,0 +1,61 @@ +#ifndef _ROS_std_msgs_Duration_h +#define _ROS_std_msgs_Duration_h + +#include +#include +#include +#include "ros/msg.h" +#include "ros/duration.h" + +namespace std_msgs +{ + + class Duration : public ros::Msg + { + public: + ros::Duration data; + + Duration(): + data() + { + } + + virtual int serialize(unsigned char *outbuffer) const + { + int offset = 0; + *(outbuffer + offset + 0) = (this->data.sec >> (8 * 0)) & 0xFF; + *(outbuffer + offset + 1) = (this->data.sec >> (8 * 1)) & 0xFF; + *(outbuffer + offset + 2) = (this->data.sec >> (8 * 2)) & 0xFF; + *(outbuffer + offset + 3) = (this->data.sec >> (8 * 3)) & 0xFF; + offset += sizeof(this->data.sec); + *(outbuffer + offset + 0) = (this->data.nsec >> (8 * 0)) & 0xFF; + *(outbuffer + offset + 1) = (this->data.nsec >> (8 * 1)) & 0xFF; + *(outbuffer + offset + 2) = (this->data.nsec >> (8 * 2)) & 0xFF; + *(outbuffer + offset + 3) = (this->data.nsec >> (8 * 3)) & 0xFF; + offset += sizeof(this->data.nsec); + return offset; + } + + virtual int deserialize(unsigned char *inbuffer) + { + int offset = 0; + this->data.sec = ((uint32_t) (*(inbuffer + offset))); + this->data.sec |= ((uint32_t) (*(inbuffer + offset + 1))) << (8 * 1); + this->data.sec |= ((uint32_t) (*(inbuffer + offset + 2))) << (8 * 2); + this->data.sec |= ((uint32_t) (*(inbuffer + offset + 3))) << (8 * 3); + offset += sizeof(this->data.sec); + this->data.nsec = ((uint32_t) (*(inbuffer + offset))); + this->data.nsec |= ((uint32_t) (*(inbuffer + offset + 1))) << (8 * 1); + this->data.nsec |= ((uint32_t) (*(inbuffer + offset + 2))) << (8 * 2); + this->data.nsec |= ((uint32_t) (*(inbuffer + offset + 3))) << (8 * 3); + offset += sizeof(this->data.nsec); + return offset; + } + + const char * getType(){ return "std_msgs/Duration"; }; + const char * getMD5(){ return "3e286caf4241d664e55f3ad380e2ae46"; }; + + }; + +} +#endif \ No newline at end of file diff --git a/turtlebot3_core/libraries/ros_lib/std_msgs/Empty.h b/turtlebot3_core/libraries/ros_lib/std_msgs/Empty.h new file mode 100644 index 00000000..440e5edc --- /dev/null +++ b/turtlebot3_core/libraries/ros_lib/std_msgs/Empty.h @@ -0,0 +1,38 @@ +#ifndef _ROS_std_msgs_Empty_h +#define _ROS_std_msgs_Empty_h + +#include +#include +#include +#include "ros/msg.h" + +namespace std_msgs +{ + + class Empty : public ros::Msg + { + public: + + Empty() + { + } + + virtual int serialize(unsigned char *outbuffer) const + { + int offset = 0; + return offset; + } + + virtual int deserialize(unsigned char *inbuffer) + { + int offset = 0; + return offset; + } + + const char * getType(){ return "std_msgs/Empty"; }; + const char * getMD5(){ return "d41d8cd98f00b204e9800998ecf8427e"; }; + + }; + +} +#endif \ No newline at end of file diff --git a/turtlebot3_core/libraries/ros_lib/std_msgs/Float32.h b/turtlebot3_core/libraries/ros_lib/std_msgs/Float32.h new file mode 100644 index 00000000..b324c1bb --- /dev/null +++ b/turtlebot3_core/libraries/ros_lib/std_msgs/Float32.h @@ -0,0 +1,61 @@ +#ifndef _ROS_std_msgs_Float32_h +#define _ROS_std_msgs_Float32_h + +#include +#include +#include +#include "ros/msg.h" + +namespace std_msgs +{ + + class Float32 : public ros::Msg + { + public: + float data; + + Float32(): + data(0) + { + } + + virtual int serialize(unsigned char *outbuffer) const + { + int offset = 0; + union { + float real; + uint32_t base; + } u_data; + u_data.real = this->data; + *(outbuffer + offset + 0) = (u_data.base >> (8 * 0)) & 0xFF; + *(outbuffer + offset + 1) = (u_data.base >> (8 * 1)) & 0xFF; + *(outbuffer + offset + 2) = (u_data.base >> (8 * 2)) & 0xFF; + *(outbuffer + offset + 3) = (u_data.base >> (8 * 3)) & 0xFF; + offset += sizeof(this->data); + return offset; + } + + virtual int deserialize(unsigned char *inbuffer) + { + int offset = 0; + union { + float real; + uint32_t base; + } u_data; + u_data.base = 0; + u_data.base |= ((uint32_t) (*(inbuffer + offset + 0))) << (8 * 0); + u_data.base |= ((uint32_t) (*(inbuffer + offset + 1))) << (8 * 1); + u_data.base |= ((uint32_t) (*(inbuffer + offset + 2))) << (8 * 2); + u_data.base |= ((uint32_t) (*(inbuffer + offset + 3))) << (8 * 3); + this->data = u_data.real; + offset += sizeof(this->data); + return offset; + } + + const char * getType(){ return "std_msgs/Float32"; }; + const char * getMD5(){ return "73fcbf46b49191e672908e50842a83d4"; }; + + }; + +} +#endif \ No newline at end of file diff --git a/turtlebot3_core/libraries/ros_lib/std_msgs/Float32MultiArray.h b/turtlebot3_core/libraries/ros_lib/std_msgs/Float32MultiArray.h new file mode 100644 index 00000000..a2264285 --- /dev/null +++ b/turtlebot3_core/libraries/ros_lib/std_msgs/Float32MultiArray.h @@ -0,0 +1,86 @@ +#ifndef _ROS_std_msgs_Float32MultiArray_h +#define _ROS_std_msgs_Float32MultiArray_h + +#include +#include +#include +#include "ros/msg.h" +#include "std_msgs/MultiArrayLayout.h" + +namespace std_msgs +{ + + class Float32MultiArray : public ros::Msg + { + public: + std_msgs::MultiArrayLayout layout; + uint32_t data_length; + float st_data; + float * data; + + Float32MultiArray(): + layout(), + data_length(0), data(NULL) + { + } + + virtual int serialize(unsigned char *outbuffer) const + { + int offset = 0; + offset += this->layout.serialize(outbuffer + offset); + *(outbuffer + offset + 0) = (this->data_length >> (8 * 0)) & 0xFF; + *(outbuffer + offset + 1) = (this->data_length >> (8 * 1)) & 0xFF; + *(outbuffer + offset + 2) = (this->data_length >> (8 * 2)) & 0xFF; + *(outbuffer + offset + 3) = (this->data_length >> (8 * 3)) & 0xFF; + offset += sizeof(this->data_length); + for( uint32_t i = 0; i < data_length; i++){ + union { + float real; + uint32_t base; + } u_datai; + u_datai.real = this->data[i]; + *(outbuffer + offset + 0) = (u_datai.base >> (8 * 0)) & 0xFF; + *(outbuffer + offset + 1) = (u_datai.base >> (8 * 1)) & 0xFF; + *(outbuffer + offset + 2) = (u_datai.base >> (8 * 2)) & 0xFF; + *(outbuffer + offset + 3) = (u_datai.base >> (8 * 3)) & 0xFF; + offset += sizeof(this->data[i]); + } + return offset; + } + + virtual int deserialize(unsigned char *inbuffer) + { + int offset = 0; + offset += this->layout.deserialize(inbuffer + offset); + uint32_t data_lengthT = ((uint32_t) (*(inbuffer + offset))); + data_lengthT |= ((uint32_t) (*(inbuffer + offset + 1))) << (8 * 1); + data_lengthT |= ((uint32_t) (*(inbuffer + offset + 2))) << (8 * 2); + data_lengthT |= ((uint32_t) (*(inbuffer + offset + 3))) << (8 * 3); + offset += sizeof(this->data_length); + if(data_lengthT > data_length) + this->data = (float*)realloc(this->data, data_lengthT * sizeof(float)); + data_length = data_lengthT; + for( uint32_t i = 0; i < data_length; i++){ + union { + float real; + uint32_t base; + } u_st_data; + u_st_data.base = 0; + u_st_data.base |= ((uint32_t) (*(inbuffer + offset + 0))) << (8 * 0); + u_st_data.base |= ((uint32_t) (*(inbuffer + offset + 1))) << (8 * 1); + u_st_data.base |= ((uint32_t) (*(inbuffer + offset + 2))) << (8 * 2); + u_st_data.base |= ((uint32_t) (*(inbuffer + offset + 3))) << (8 * 3); + this->st_data = u_st_data.real; + offset += sizeof(this->st_data); + memcpy( &(this->data[i]), &(this->st_data), sizeof(float)); + } + return offset; + } + + const char * getType(){ return "std_msgs/Float32MultiArray"; }; + const char * getMD5(){ return "6a40e0ffa6a17a503ac3f8616991b1f6"; }; + + }; + +} +#endif \ No newline at end of file diff --git a/turtlebot3_core/libraries/ros_lib/std_msgs/Float64.h b/turtlebot3_core/libraries/ros_lib/std_msgs/Float64.h new file mode 100644 index 00000000..3cc9fd03 --- /dev/null +++ b/turtlebot3_core/libraries/ros_lib/std_msgs/Float64.h @@ -0,0 +1,42 @@ +#ifndef _ROS_std_msgs_Float64_h +#define _ROS_std_msgs_Float64_h + +#include +#include +#include +#include "ros/msg.h" + +namespace std_msgs +{ + + class Float64 : public ros::Msg + { + public: + float data; + + Float64(): + data(0) + { + } + + virtual int serialize(unsigned char *outbuffer) const + { + int offset = 0; + offset += serializeAvrFloat64(outbuffer + offset, this->data); + return offset; + } + + virtual int deserialize(unsigned char *inbuffer) + { + int offset = 0; + offset += deserializeAvrFloat64(inbuffer + offset, &(this->data)); + return offset; + } + + const char * getType(){ return "std_msgs/Float64"; }; + const char * getMD5(){ return "fdb28210bfa9d7c91146260178d9a584"; }; + + }; + +} +#endif \ No newline at end of file diff --git a/turtlebot3_core/libraries/ros_lib/std_msgs/Float64MultiArray.h b/turtlebot3_core/libraries/ros_lib/std_msgs/Float64MultiArray.h new file mode 100644 index 00000000..721f156a --- /dev/null +++ b/turtlebot3_core/libraries/ros_lib/std_msgs/Float64MultiArray.h @@ -0,0 +1,67 @@ +#ifndef _ROS_std_msgs_Float64MultiArray_h +#define _ROS_std_msgs_Float64MultiArray_h + +#include +#include +#include +#include "ros/msg.h" +#include "std_msgs/MultiArrayLayout.h" + +namespace std_msgs +{ + + class Float64MultiArray : public ros::Msg + { + public: + std_msgs::MultiArrayLayout layout; + uint32_t data_length; + float st_data; + float * data; + + Float64MultiArray(): + layout(), + data_length(0), data(NULL) + { + } + + virtual int serialize(unsigned char *outbuffer) const + { + int offset = 0; + offset += this->layout.serialize(outbuffer + offset); + *(outbuffer + offset + 0) = (this->data_length >> (8 * 0)) & 0xFF; + *(outbuffer + offset + 1) = (this->data_length >> (8 * 1)) & 0xFF; + *(outbuffer + offset + 2) = (this->data_length >> (8 * 2)) & 0xFF; + *(outbuffer + offset + 3) = (this->data_length >> (8 * 3)) & 0xFF; + offset += sizeof(this->data_length); + for( uint32_t i = 0; i < data_length; i++){ + offset += serializeAvrFloat64(outbuffer + offset, this->data[i]); + } + return offset; + } + + virtual int deserialize(unsigned char *inbuffer) + { + int offset = 0; + offset += this->layout.deserialize(inbuffer + offset); + uint32_t data_lengthT = ((uint32_t) (*(inbuffer + offset))); + data_lengthT |= ((uint32_t) (*(inbuffer + offset + 1))) << (8 * 1); + data_lengthT |= ((uint32_t) (*(inbuffer + offset + 2))) << (8 * 2); + data_lengthT |= ((uint32_t) (*(inbuffer + offset + 3))) << (8 * 3); + offset += sizeof(this->data_length); + if(data_lengthT > data_length) + this->data = (float*)realloc(this->data, data_lengthT * sizeof(float)); + data_length = data_lengthT; + for( uint32_t i = 0; i < data_length; i++){ + offset += deserializeAvrFloat64(inbuffer + offset, &(this->st_data)); + memcpy( &(this->data[i]), &(this->st_data), sizeof(float)); + } + return offset; + } + + const char * getType(){ return "std_msgs/Float64MultiArray"; }; + const char * getMD5(){ return "4b7d974086d4060e7db4613a7e6c3ba4"; }; + + }; + +} +#endif \ No newline at end of file diff --git a/turtlebot3_core/libraries/ros_lib/std_msgs/Header.h b/turtlebot3_core/libraries/ros_lib/std_msgs/Header.h new file mode 100644 index 00000000..2aa0f7f0 --- /dev/null +++ b/turtlebot3_core/libraries/ros_lib/std_msgs/Header.h @@ -0,0 +1,89 @@ +#ifndef _ROS_std_msgs_Header_h +#define _ROS_std_msgs_Header_h + +#include +#include +#include +#include "ros/msg.h" +#include "ros/time.h" + +namespace std_msgs +{ + + class Header : public ros::Msg + { + public: + uint32_t seq; + ros::Time stamp; + const char* frame_id; + + Header(): + seq(0), + stamp(), + frame_id("") + { + } + + virtual int serialize(unsigned char *outbuffer) const + { + int offset = 0; + *(outbuffer + offset + 0) = (this->seq >> (8 * 0)) & 0xFF; + *(outbuffer + offset + 1) = (this->seq >> (8 * 1)) & 0xFF; + *(outbuffer + offset + 2) = (this->seq >> (8 * 2)) & 0xFF; + *(outbuffer + offset + 3) = (this->seq >> (8 * 3)) & 0xFF; + offset += sizeof(this->seq); + *(outbuffer + offset + 0) = (this->stamp.sec >> (8 * 0)) & 0xFF; + *(outbuffer + offset + 1) = (this->stamp.sec >> (8 * 1)) & 0xFF; + *(outbuffer + offset + 2) = (this->stamp.sec >> (8 * 2)) & 0xFF; + *(outbuffer + offset + 3) = (this->stamp.sec >> (8 * 3)) & 0xFF; + offset += sizeof(this->stamp.sec); + *(outbuffer + offset + 0) = (this->stamp.nsec >> (8 * 0)) & 0xFF; + *(outbuffer + offset + 1) = (this->stamp.nsec >> (8 * 1)) & 0xFF; + *(outbuffer + offset + 2) = (this->stamp.nsec >> (8 * 2)) & 0xFF; + *(outbuffer + offset + 3) = (this->stamp.nsec >> (8 * 3)) & 0xFF; + offset += sizeof(this->stamp.nsec); + uint32_t length_frame_id = strlen(this->frame_id); + memcpy(outbuffer + offset, &length_frame_id, sizeof(uint32_t)); + offset += 4; + memcpy(outbuffer + offset, this->frame_id, length_frame_id); + offset += length_frame_id; + return offset; + } + + virtual int deserialize(unsigned char *inbuffer) + { + int offset = 0; + this->seq = ((uint32_t) (*(inbuffer + offset))); + this->seq |= ((uint32_t) (*(inbuffer + offset + 1))) << (8 * 1); + this->seq |= ((uint32_t) (*(inbuffer + offset + 2))) << (8 * 2); + this->seq |= ((uint32_t) (*(inbuffer + offset + 3))) << (8 * 3); + offset += sizeof(this->seq); + this->stamp.sec = ((uint32_t) (*(inbuffer + offset))); + this->stamp.sec |= ((uint32_t) (*(inbuffer + offset + 1))) << (8 * 1); + this->stamp.sec |= ((uint32_t) (*(inbuffer + offset + 2))) << (8 * 2); + this->stamp.sec |= ((uint32_t) (*(inbuffer + offset + 3))) << (8 * 3); + offset += sizeof(this->stamp.sec); + this->stamp.nsec = ((uint32_t) (*(inbuffer + offset))); + this->stamp.nsec |= ((uint32_t) (*(inbuffer + offset + 1))) << (8 * 1); + this->stamp.nsec |= ((uint32_t) (*(inbuffer + offset + 2))) << (8 * 2); + this->stamp.nsec |= ((uint32_t) (*(inbuffer + offset + 3))) << (8 * 3); + offset += sizeof(this->stamp.nsec); + uint32_t length_frame_id; + memcpy(&length_frame_id, (inbuffer + offset), sizeof(uint32_t)); + offset += 4; + for(unsigned int k= offset; k< offset+length_frame_id; ++k){ + inbuffer[k-1]=inbuffer[k]; + } + inbuffer[offset+length_frame_id-1]=0; + this->frame_id = (char *)(inbuffer + offset-1); + offset += length_frame_id; + return offset; + } + + const char * getType(){ return "std_msgs/Header"; }; + const char * getMD5(){ return "2176decaecbce78abc3b96ef049fabed"; }; + + }; + +} +#endif \ No newline at end of file diff --git a/turtlebot3_core/libraries/ros_lib/std_msgs/Int16.h b/turtlebot3_core/libraries/ros_lib/std_msgs/Int16.h new file mode 100644 index 00000000..a83f7b2a --- /dev/null +++ b/turtlebot3_core/libraries/ros_lib/std_msgs/Int16.h @@ -0,0 +1,57 @@ +#ifndef _ROS_std_msgs_Int16_h +#define _ROS_std_msgs_Int16_h + +#include +#include +#include +#include "ros/msg.h" + +namespace std_msgs +{ + + class Int16 : public ros::Msg + { + public: + int16_t data; + + Int16(): + data(0) + { + } + + virtual int serialize(unsigned char *outbuffer) const + { + int offset = 0; + union { + int16_t real; + uint16_t base; + } u_data; + u_data.real = this->data; + *(outbuffer + offset + 0) = (u_data.base >> (8 * 0)) & 0xFF; + *(outbuffer + offset + 1) = (u_data.base >> (8 * 1)) & 0xFF; + offset += sizeof(this->data); + return offset; + } + + virtual int deserialize(unsigned char *inbuffer) + { + int offset = 0; + union { + int16_t real; + uint16_t base; + } u_data; + u_data.base = 0; + u_data.base |= ((uint16_t) (*(inbuffer + offset + 0))) << (8 * 0); + u_data.base |= ((uint16_t) (*(inbuffer + offset + 1))) << (8 * 1); + this->data = u_data.real; + offset += sizeof(this->data); + return offset; + } + + const char * getType(){ return "std_msgs/Int16"; }; + const char * getMD5(){ return "8524586e34fbd7cb1c08c5f5f1ca0e57"; }; + + }; + +} +#endif \ No newline at end of file diff --git a/turtlebot3_core/libraries/ros_lib/std_msgs/Int16MultiArray.h b/turtlebot3_core/libraries/ros_lib/std_msgs/Int16MultiArray.h new file mode 100644 index 00000000..e6d85f4c --- /dev/null +++ b/turtlebot3_core/libraries/ros_lib/std_msgs/Int16MultiArray.h @@ -0,0 +1,82 @@ +#ifndef _ROS_std_msgs_Int16MultiArray_h +#define _ROS_std_msgs_Int16MultiArray_h + +#include +#include +#include +#include "ros/msg.h" +#include "std_msgs/MultiArrayLayout.h" + +namespace std_msgs +{ + + class Int16MultiArray : public ros::Msg + { + public: + std_msgs::MultiArrayLayout layout; + uint32_t data_length; + int16_t st_data; + int16_t * data; + + Int16MultiArray(): + layout(), + data_length(0), data(NULL) + { + } + + virtual int serialize(unsigned char *outbuffer) const + { + int offset = 0; + offset += this->layout.serialize(outbuffer + offset); + *(outbuffer + offset + 0) = (this->data_length >> (8 * 0)) & 0xFF; + *(outbuffer + offset + 1) = (this->data_length >> (8 * 1)) & 0xFF; + *(outbuffer + offset + 2) = (this->data_length >> (8 * 2)) & 0xFF; + *(outbuffer + offset + 3) = (this->data_length >> (8 * 3)) & 0xFF; + offset += sizeof(this->data_length); + for( uint32_t i = 0; i < data_length; i++){ + union { + int16_t real; + uint16_t base; + } u_datai; + u_datai.real = this->data[i]; + *(outbuffer + offset + 0) = (u_datai.base >> (8 * 0)) & 0xFF; + *(outbuffer + offset + 1) = (u_datai.base >> (8 * 1)) & 0xFF; + offset += sizeof(this->data[i]); + } + return offset; + } + + virtual int deserialize(unsigned char *inbuffer) + { + int offset = 0; + offset += this->layout.deserialize(inbuffer + offset); + uint32_t data_lengthT = ((uint32_t) (*(inbuffer + offset))); + data_lengthT |= ((uint32_t) (*(inbuffer + offset + 1))) << (8 * 1); + data_lengthT |= ((uint32_t) (*(inbuffer + offset + 2))) << (8 * 2); + data_lengthT |= ((uint32_t) (*(inbuffer + offset + 3))) << (8 * 3); + offset += sizeof(this->data_length); + if(data_lengthT > data_length) + this->data = (int16_t*)realloc(this->data, data_lengthT * sizeof(int16_t)); + data_length = data_lengthT; + for( uint32_t i = 0; i < data_length; i++){ + union { + int16_t real; + uint16_t base; + } u_st_data; + u_st_data.base = 0; + u_st_data.base |= ((uint16_t) (*(inbuffer + offset + 0))) << (8 * 0); + u_st_data.base |= ((uint16_t) (*(inbuffer + offset + 1))) << (8 * 1); + this->st_data = u_st_data.real; + offset += sizeof(this->st_data); + memcpy( &(this->data[i]), &(this->st_data), sizeof(int16_t)); + } + return offset; + } + + const char * getType(){ return "std_msgs/Int16MultiArray"; }; + const char * getMD5(){ return "d9338d7f523fcb692fae9d0a0e9f067c"; }; + + }; + +} +#endif \ No newline at end of file diff --git a/turtlebot3_core/libraries/ros_lib/std_msgs/Int32.h b/turtlebot3_core/libraries/ros_lib/std_msgs/Int32.h new file mode 100644 index 00000000..a2a364f0 --- /dev/null +++ b/turtlebot3_core/libraries/ros_lib/std_msgs/Int32.h @@ -0,0 +1,61 @@ +#ifndef _ROS_std_msgs_Int32_h +#define _ROS_std_msgs_Int32_h + +#include +#include +#include +#include "ros/msg.h" + +namespace std_msgs +{ + + class Int32 : public ros::Msg + { + public: + int32_t data; + + Int32(): + data(0) + { + } + + virtual int serialize(unsigned char *outbuffer) const + { + int offset = 0; + union { + int32_t real; + uint32_t base; + } u_data; + u_data.real = this->data; + *(outbuffer + offset + 0) = (u_data.base >> (8 * 0)) & 0xFF; + *(outbuffer + offset + 1) = (u_data.base >> (8 * 1)) & 0xFF; + *(outbuffer + offset + 2) = (u_data.base >> (8 * 2)) & 0xFF; + *(outbuffer + offset + 3) = (u_data.base >> (8 * 3)) & 0xFF; + offset += sizeof(this->data); + return offset; + } + + virtual int deserialize(unsigned char *inbuffer) + { + int offset = 0; + union { + int32_t real; + uint32_t base; + } u_data; + u_data.base = 0; + u_data.base |= ((uint32_t) (*(inbuffer + offset + 0))) << (8 * 0); + u_data.base |= ((uint32_t) (*(inbuffer + offset + 1))) << (8 * 1); + u_data.base |= ((uint32_t) (*(inbuffer + offset + 2))) << (8 * 2); + u_data.base |= ((uint32_t) (*(inbuffer + offset + 3))) << (8 * 3); + this->data = u_data.real; + offset += sizeof(this->data); + return offset; + } + + const char * getType(){ return "std_msgs/Int32"; }; + const char * getMD5(){ return "da5909fbe378aeaf85e547e830cc1bb7"; }; + + }; + +} +#endif \ No newline at end of file diff --git a/turtlebot3_core/libraries/ros_lib/std_msgs/Int32MultiArray.h b/turtlebot3_core/libraries/ros_lib/std_msgs/Int32MultiArray.h new file mode 100644 index 00000000..9491ec6d --- /dev/null +++ b/turtlebot3_core/libraries/ros_lib/std_msgs/Int32MultiArray.h @@ -0,0 +1,86 @@ +#ifndef _ROS_std_msgs_Int32MultiArray_h +#define _ROS_std_msgs_Int32MultiArray_h + +#include +#include +#include +#include "ros/msg.h" +#include "std_msgs/MultiArrayLayout.h" + +namespace std_msgs +{ + + class Int32MultiArray : public ros::Msg + { + public: + std_msgs::MultiArrayLayout layout; + uint32_t data_length; + int32_t st_data; + int32_t * data; + + Int32MultiArray(): + layout(), + data_length(0), data(NULL) + { + } + + virtual int serialize(unsigned char *outbuffer) const + { + int offset = 0; + offset += this->layout.serialize(outbuffer + offset); + *(outbuffer + offset + 0) = (this->data_length >> (8 * 0)) & 0xFF; + *(outbuffer + offset + 1) = (this->data_length >> (8 * 1)) & 0xFF; + *(outbuffer + offset + 2) = (this->data_length >> (8 * 2)) & 0xFF; + *(outbuffer + offset + 3) = (this->data_length >> (8 * 3)) & 0xFF; + offset += sizeof(this->data_length); + for( uint32_t i = 0; i < data_length; i++){ + union { + int32_t real; + uint32_t base; + } u_datai; + u_datai.real = this->data[i]; + *(outbuffer + offset + 0) = (u_datai.base >> (8 * 0)) & 0xFF; + *(outbuffer + offset + 1) = (u_datai.base >> (8 * 1)) & 0xFF; + *(outbuffer + offset + 2) = (u_datai.base >> (8 * 2)) & 0xFF; + *(outbuffer + offset + 3) = (u_datai.base >> (8 * 3)) & 0xFF; + offset += sizeof(this->data[i]); + } + return offset; + } + + virtual int deserialize(unsigned char *inbuffer) + { + int offset = 0; + offset += this->layout.deserialize(inbuffer + offset); + uint32_t data_lengthT = ((uint32_t) (*(inbuffer + offset))); + data_lengthT |= ((uint32_t) (*(inbuffer + offset + 1))) << (8 * 1); + data_lengthT |= ((uint32_t) (*(inbuffer + offset + 2))) << (8 * 2); + data_lengthT |= ((uint32_t) (*(inbuffer + offset + 3))) << (8 * 3); + offset += sizeof(this->data_length); + if(data_lengthT > data_length) + this->data = (int32_t*)realloc(this->data, data_lengthT * sizeof(int32_t)); + data_length = data_lengthT; + for( uint32_t i = 0; i < data_length; i++){ + union { + int32_t real; + uint32_t base; + } u_st_data; + u_st_data.base = 0; + u_st_data.base |= ((uint32_t) (*(inbuffer + offset + 0))) << (8 * 0); + u_st_data.base |= ((uint32_t) (*(inbuffer + offset + 1))) << (8 * 1); + u_st_data.base |= ((uint32_t) (*(inbuffer + offset + 2))) << (8 * 2); + u_st_data.base |= ((uint32_t) (*(inbuffer + offset + 3))) << (8 * 3); + this->st_data = u_st_data.real; + offset += sizeof(this->st_data); + memcpy( &(this->data[i]), &(this->st_data), sizeof(int32_t)); + } + return offset; + } + + const char * getType(){ return "std_msgs/Int32MultiArray"; }; + const char * getMD5(){ return "1d99f79f8b325b44fee908053e9c945b"; }; + + }; + +} +#endif \ No newline at end of file diff --git a/turtlebot3_core/libraries/ros_lib/std_msgs/Int64.h b/turtlebot3_core/libraries/ros_lib/std_msgs/Int64.h new file mode 100644 index 00000000..40160815 --- /dev/null +++ b/turtlebot3_core/libraries/ros_lib/std_msgs/Int64.h @@ -0,0 +1,69 @@ +#ifndef _ROS_std_msgs_Int64_h +#define _ROS_std_msgs_Int64_h + +#include +#include +#include +#include "ros/msg.h" + +namespace std_msgs +{ + + class Int64 : public ros::Msg + { + public: + int64_t data; + + Int64(): + data(0) + { + } + + virtual int serialize(unsigned char *outbuffer) const + { + int offset = 0; + union { + int64_t real; + uint64_t base; + } u_data; + u_data.real = this->data; + *(outbuffer + offset + 0) = (u_data.base >> (8 * 0)) & 0xFF; + *(outbuffer + offset + 1) = (u_data.base >> (8 * 1)) & 0xFF; + *(outbuffer + offset + 2) = (u_data.base >> (8 * 2)) & 0xFF; + *(outbuffer + offset + 3) = (u_data.base >> (8 * 3)) & 0xFF; + *(outbuffer + offset + 4) = (u_data.base >> (8 * 4)) & 0xFF; + *(outbuffer + offset + 5) = (u_data.base >> (8 * 5)) & 0xFF; + *(outbuffer + offset + 6) = (u_data.base >> (8 * 6)) & 0xFF; + *(outbuffer + offset + 7) = (u_data.base >> (8 * 7)) & 0xFF; + offset += sizeof(this->data); + return offset; + } + + virtual int deserialize(unsigned char *inbuffer) + { + int offset = 0; + union { + int64_t real; + uint64_t base; + } u_data; + u_data.base = 0; + u_data.base |= ((uint64_t) (*(inbuffer + offset + 0))) << (8 * 0); + u_data.base |= ((uint64_t) (*(inbuffer + offset + 1))) << (8 * 1); + u_data.base |= ((uint64_t) (*(inbuffer + offset + 2))) << (8 * 2); + u_data.base |= ((uint64_t) (*(inbuffer + offset + 3))) << (8 * 3); + u_data.base |= ((uint64_t) (*(inbuffer + offset + 4))) << (8 * 4); + u_data.base |= ((uint64_t) (*(inbuffer + offset + 5))) << (8 * 5); + u_data.base |= ((uint64_t) (*(inbuffer + offset + 6))) << (8 * 6); + u_data.base |= ((uint64_t) (*(inbuffer + offset + 7))) << (8 * 7); + this->data = u_data.real; + offset += sizeof(this->data); + return offset; + } + + const char * getType(){ return "std_msgs/Int64"; }; + const char * getMD5(){ return "34add168574510e6e17f5d23ecc077ef"; }; + + }; + +} +#endif \ No newline at end of file diff --git a/turtlebot3_core/libraries/ros_lib/std_msgs/Int64MultiArray.h b/turtlebot3_core/libraries/ros_lib/std_msgs/Int64MultiArray.h new file mode 100644 index 00000000..b54b6115 --- /dev/null +++ b/turtlebot3_core/libraries/ros_lib/std_msgs/Int64MultiArray.h @@ -0,0 +1,94 @@ +#ifndef _ROS_std_msgs_Int64MultiArray_h +#define _ROS_std_msgs_Int64MultiArray_h + +#include +#include +#include +#include "ros/msg.h" +#include "std_msgs/MultiArrayLayout.h" + +namespace std_msgs +{ + + class Int64MultiArray : public ros::Msg + { + public: + std_msgs::MultiArrayLayout layout; + uint32_t data_length; + int64_t st_data; + int64_t * data; + + Int64MultiArray(): + layout(), + data_length(0), data(NULL) + { + } + + virtual int serialize(unsigned char *outbuffer) const + { + int offset = 0; + offset += this->layout.serialize(outbuffer + offset); + *(outbuffer + offset + 0) = (this->data_length >> (8 * 0)) & 0xFF; + *(outbuffer + offset + 1) = (this->data_length >> (8 * 1)) & 0xFF; + *(outbuffer + offset + 2) = (this->data_length >> (8 * 2)) & 0xFF; + *(outbuffer + offset + 3) = (this->data_length >> (8 * 3)) & 0xFF; + offset += sizeof(this->data_length); + for( uint32_t i = 0; i < data_length; i++){ + union { + int64_t real; + uint64_t base; + } u_datai; + u_datai.real = this->data[i]; + *(outbuffer + offset + 0) = (u_datai.base >> (8 * 0)) & 0xFF; + *(outbuffer + offset + 1) = (u_datai.base >> (8 * 1)) & 0xFF; + *(outbuffer + offset + 2) = (u_datai.base >> (8 * 2)) & 0xFF; + *(outbuffer + offset + 3) = (u_datai.base >> (8 * 3)) & 0xFF; + *(outbuffer + offset + 4) = (u_datai.base >> (8 * 4)) & 0xFF; + *(outbuffer + offset + 5) = (u_datai.base >> (8 * 5)) & 0xFF; + *(outbuffer + offset + 6) = (u_datai.base >> (8 * 6)) & 0xFF; + *(outbuffer + offset + 7) = (u_datai.base >> (8 * 7)) & 0xFF; + offset += sizeof(this->data[i]); + } + return offset; + } + + virtual int deserialize(unsigned char *inbuffer) + { + int offset = 0; + offset += this->layout.deserialize(inbuffer + offset); + uint32_t data_lengthT = ((uint32_t) (*(inbuffer + offset))); + data_lengthT |= ((uint32_t) (*(inbuffer + offset + 1))) << (8 * 1); + data_lengthT |= ((uint32_t) (*(inbuffer + offset + 2))) << (8 * 2); + data_lengthT |= ((uint32_t) (*(inbuffer + offset + 3))) << (8 * 3); + offset += sizeof(this->data_length); + if(data_lengthT > data_length) + this->data = (int64_t*)realloc(this->data, data_lengthT * sizeof(int64_t)); + data_length = data_lengthT; + for( uint32_t i = 0; i < data_length; i++){ + union { + int64_t real; + uint64_t base; + } u_st_data; + u_st_data.base = 0; + u_st_data.base |= ((uint64_t) (*(inbuffer + offset + 0))) << (8 * 0); + u_st_data.base |= ((uint64_t) (*(inbuffer + offset + 1))) << (8 * 1); + u_st_data.base |= ((uint64_t) (*(inbuffer + offset + 2))) << (8 * 2); + u_st_data.base |= ((uint64_t) (*(inbuffer + offset + 3))) << (8 * 3); + u_st_data.base |= ((uint64_t) (*(inbuffer + offset + 4))) << (8 * 4); + u_st_data.base |= ((uint64_t) (*(inbuffer + offset + 5))) << (8 * 5); + u_st_data.base |= ((uint64_t) (*(inbuffer + offset + 6))) << (8 * 6); + u_st_data.base |= ((uint64_t) (*(inbuffer + offset + 7))) << (8 * 7); + this->st_data = u_st_data.real; + offset += sizeof(this->st_data); + memcpy( &(this->data[i]), &(this->st_data), sizeof(int64_t)); + } + return offset; + } + + const char * getType(){ return "std_msgs/Int64MultiArray"; }; + const char * getMD5(){ return "54865aa6c65be0448113a2afc6a49270"; }; + + }; + +} +#endif \ No newline at end of file diff --git a/turtlebot3_core/libraries/ros_lib/std_msgs/Int8.h b/turtlebot3_core/libraries/ros_lib/std_msgs/Int8.h new file mode 100644 index 00000000..b02ef77f --- /dev/null +++ b/turtlebot3_core/libraries/ros_lib/std_msgs/Int8.h @@ -0,0 +1,55 @@ +#ifndef _ROS_std_msgs_Int8_h +#define _ROS_std_msgs_Int8_h + +#include +#include +#include +#include "ros/msg.h" + +namespace std_msgs +{ + + class Int8 : public ros::Msg + { + public: + int8_t data; + + Int8(): + data(0) + { + } + + virtual int serialize(unsigned char *outbuffer) const + { + int offset = 0; + union { + int8_t real; + uint8_t base; + } u_data; + u_data.real = this->data; + *(outbuffer + offset + 0) = (u_data.base >> (8 * 0)) & 0xFF; + offset += sizeof(this->data); + return offset; + } + + virtual int deserialize(unsigned char *inbuffer) + { + int offset = 0; + union { + int8_t real; + uint8_t base; + } u_data; + u_data.base = 0; + u_data.base |= ((uint8_t) (*(inbuffer + offset + 0))) << (8 * 0); + this->data = u_data.real; + offset += sizeof(this->data); + return offset; + } + + const char * getType(){ return "std_msgs/Int8"; }; + const char * getMD5(){ return "27ffa0c9c4b8fb8492252bcad9e5c57b"; }; + + }; + +} +#endif \ No newline at end of file diff --git a/turtlebot3_core/libraries/ros_lib/std_msgs/Int8MultiArray.h b/turtlebot3_core/libraries/ros_lib/std_msgs/Int8MultiArray.h new file mode 100644 index 00000000..f84169a3 --- /dev/null +++ b/turtlebot3_core/libraries/ros_lib/std_msgs/Int8MultiArray.h @@ -0,0 +1,80 @@ +#ifndef _ROS_std_msgs_Int8MultiArray_h +#define _ROS_std_msgs_Int8MultiArray_h + +#include +#include +#include +#include "ros/msg.h" +#include "std_msgs/MultiArrayLayout.h" + +namespace std_msgs +{ + + class Int8MultiArray : public ros::Msg + { + public: + std_msgs::MultiArrayLayout layout; + uint32_t data_length; + int8_t st_data; + int8_t * data; + + Int8MultiArray(): + layout(), + data_length(0), data(NULL) + { + } + + virtual int serialize(unsigned char *outbuffer) const + { + int offset = 0; + offset += this->layout.serialize(outbuffer + offset); + *(outbuffer + offset + 0) = (this->data_length >> (8 * 0)) & 0xFF; + *(outbuffer + offset + 1) = (this->data_length >> (8 * 1)) & 0xFF; + *(outbuffer + offset + 2) = (this->data_length >> (8 * 2)) & 0xFF; + *(outbuffer + offset + 3) = (this->data_length >> (8 * 3)) & 0xFF; + offset += sizeof(this->data_length); + for( uint32_t i = 0; i < data_length; i++){ + union { + int8_t real; + uint8_t base; + } u_datai; + u_datai.real = this->data[i]; + *(outbuffer + offset + 0) = (u_datai.base >> (8 * 0)) & 0xFF; + offset += sizeof(this->data[i]); + } + return offset; + } + + virtual int deserialize(unsigned char *inbuffer) + { + int offset = 0; + offset += this->layout.deserialize(inbuffer + offset); + uint32_t data_lengthT = ((uint32_t) (*(inbuffer + offset))); + data_lengthT |= ((uint32_t) (*(inbuffer + offset + 1))) << (8 * 1); + data_lengthT |= ((uint32_t) (*(inbuffer + offset + 2))) << (8 * 2); + data_lengthT |= ((uint32_t) (*(inbuffer + offset + 3))) << (8 * 3); + offset += sizeof(this->data_length); + if(data_lengthT > data_length) + this->data = (int8_t*)realloc(this->data, data_lengthT * sizeof(int8_t)); + data_length = data_lengthT; + for( uint32_t i = 0; i < data_length; i++){ + union { + int8_t real; + uint8_t base; + } u_st_data; + u_st_data.base = 0; + u_st_data.base |= ((uint8_t) (*(inbuffer + offset + 0))) << (8 * 0); + this->st_data = u_st_data.real; + offset += sizeof(this->st_data); + memcpy( &(this->data[i]), &(this->st_data), sizeof(int8_t)); + } + return offset; + } + + const char * getType(){ return "std_msgs/Int8MultiArray"; }; + const char * getMD5(){ return "d7c1af35a1b4781bbe79e03dd94b7c13"; }; + + }; + +} +#endif \ No newline at end of file diff --git a/turtlebot3_core/libraries/ros_lib/std_msgs/MultiArrayDimension.h b/turtlebot3_core/libraries/ros_lib/std_msgs/MultiArrayDimension.h new file mode 100644 index 00000000..7cae4bab --- /dev/null +++ b/turtlebot3_core/libraries/ros_lib/std_msgs/MultiArrayDimension.h @@ -0,0 +1,78 @@ +#ifndef _ROS_std_msgs_MultiArrayDimension_h +#define _ROS_std_msgs_MultiArrayDimension_h + +#include +#include +#include +#include "ros/msg.h" + +namespace std_msgs +{ + + class MultiArrayDimension : public ros::Msg + { + public: + const char* label; + uint32_t size; + uint32_t stride; + + MultiArrayDimension(): + label(""), + size(0), + stride(0) + { + } + + virtual int serialize(unsigned char *outbuffer) const + { + int offset = 0; + uint32_t length_label = strlen(this->label); + memcpy(outbuffer + offset, &length_label, sizeof(uint32_t)); + offset += 4; + memcpy(outbuffer + offset, this->label, length_label); + offset += length_label; + *(outbuffer + offset + 0) = (this->size >> (8 * 0)) & 0xFF; + *(outbuffer + offset + 1) = (this->size >> (8 * 1)) & 0xFF; + *(outbuffer + offset + 2) = (this->size >> (8 * 2)) & 0xFF; + *(outbuffer + offset + 3) = (this->size >> (8 * 3)) & 0xFF; + offset += sizeof(this->size); + *(outbuffer + offset + 0) = (this->stride >> (8 * 0)) & 0xFF; + *(outbuffer + offset + 1) = (this->stride >> (8 * 1)) & 0xFF; + *(outbuffer + offset + 2) = (this->stride >> (8 * 2)) & 0xFF; + *(outbuffer + offset + 3) = (this->stride >> (8 * 3)) & 0xFF; + offset += sizeof(this->stride); + return offset; + } + + virtual int deserialize(unsigned char *inbuffer) + { + int offset = 0; + uint32_t length_label; + memcpy(&length_label, (inbuffer + offset), sizeof(uint32_t)); + offset += 4; + for(unsigned int k= offset; k< offset+length_label; ++k){ + inbuffer[k-1]=inbuffer[k]; + } + inbuffer[offset+length_label-1]=0; + this->label = (char *)(inbuffer + offset-1); + offset += length_label; + this->size = ((uint32_t) (*(inbuffer + offset))); + this->size |= ((uint32_t) (*(inbuffer + offset + 1))) << (8 * 1); + this->size |= ((uint32_t) (*(inbuffer + offset + 2))) << (8 * 2); + this->size |= ((uint32_t) (*(inbuffer + offset + 3))) << (8 * 3); + offset += sizeof(this->size); + this->stride = ((uint32_t) (*(inbuffer + offset))); + this->stride |= ((uint32_t) (*(inbuffer + offset + 1))) << (8 * 1); + this->stride |= ((uint32_t) (*(inbuffer + offset + 2))) << (8 * 2); + this->stride |= ((uint32_t) (*(inbuffer + offset + 3))) << (8 * 3); + offset += sizeof(this->stride); + return offset; + } + + const char * getType(){ return "std_msgs/MultiArrayDimension"; }; + const char * getMD5(){ return "4cd0c83a8683deae40ecdac60e53bfa8"; }; + + }; + +} +#endif \ No newline at end of file diff --git a/turtlebot3_core/libraries/ros_lib/std_msgs/MultiArrayLayout.h b/turtlebot3_core/libraries/ros_lib/std_msgs/MultiArrayLayout.h new file mode 100644 index 00000000..76b61594 --- /dev/null +++ b/turtlebot3_core/libraries/ros_lib/std_msgs/MultiArrayLayout.h @@ -0,0 +1,75 @@ +#ifndef _ROS_std_msgs_MultiArrayLayout_h +#define _ROS_std_msgs_MultiArrayLayout_h + +#include +#include +#include +#include "ros/msg.h" +#include "std_msgs/MultiArrayDimension.h" + +namespace std_msgs +{ + + class MultiArrayLayout : public ros::Msg + { + public: + uint32_t dim_length; + std_msgs::MultiArrayDimension st_dim; + std_msgs::MultiArrayDimension * dim; + uint32_t data_offset; + + MultiArrayLayout(): + dim_length(0), dim(NULL), + data_offset(0) + { + } + + virtual int serialize(unsigned char *outbuffer) const + { + int offset = 0; + *(outbuffer + offset + 0) = (this->dim_length >> (8 * 0)) & 0xFF; + *(outbuffer + offset + 1) = (this->dim_length >> (8 * 1)) & 0xFF; + *(outbuffer + offset + 2) = (this->dim_length >> (8 * 2)) & 0xFF; + *(outbuffer + offset + 3) = (this->dim_length >> (8 * 3)) & 0xFF; + offset += sizeof(this->dim_length); + for( uint32_t i = 0; i < dim_length; i++){ + offset += this->dim[i].serialize(outbuffer + offset); + } + *(outbuffer + offset + 0) = (this->data_offset >> (8 * 0)) & 0xFF; + *(outbuffer + offset + 1) = (this->data_offset >> (8 * 1)) & 0xFF; + *(outbuffer + offset + 2) = (this->data_offset >> (8 * 2)) & 0xFF; + *(outbuffer + offset + 3) = (this->data_offset >> (8 * 3)) & 0xFF; + offset += sizeof(this->data_offset); + return offset; + } + + virtual int deserialize(unsigned char *inbuffer) + { + int offset = 0; + uint32_t dim_lengthT = ((uint32_t) (*(inbuffer + offset))); + dim_lengthT |= ((uint32_t) (*(inbuffer + offset + 1))) << (8 * 1); + dim_lengthT |= ((uint32_t) (*(inbuffer + offset + 2))) << (8 * 2); + dim_lengthT |= ((uint32_t) (*(inbuffer + offset + 3))) << (8 * 3); + offset += sizeof(this->dim_length); + if(dim_lengthT > dim_length) + this->dim = (std_msgs::MultiArrayDimension*)realloc(this->dim, dim_lengthT * sizeof(std_msgs::MultiArrayDimension)); + dim_length = dim_lengthT; + for( uint32_t i = 0; i < dim_length; i++){ + offset += this->st_dim.deserialize(inbuffer + offset); + memcpy( &(this->dim[i]), &(this->st_dim), sizeof(std_msgs::MultiArrayDimension)); + } + this->data_offset = ((uint32_t) (*(inbuffer + offset))); + this->data_offset |= ((uint32_t) (*(inbuffer + offset + 1))) << (8 * 1); + this->data_offset |= ((uint32_t) (*(inbuffer + offset + 2))) << (8 * 2); + this->data_offset |= ((uint32_t) (*(inbuffer + offset + 3))) << (8 * 3); + offset += sizeof(this->data_offset); + return offset; + } + + const char * getType(){ return "std_msgs/MultiArrayLayout"; }; + const char * getMD5(){ return "0fed2a11c13e11c5571b4e2a995a91a3"; }; + + }; + +} +#endif \ No newline at end of file diff --git a/turtlebot3_core/libraries/ros_lib/std_msgs/String.h b/turtlebot3_core/libraries/ros_lib/std_msgs/String.h new file mode 100644 index 00000000..f5905003 --- /dev/null +++ b/turtlebot3_core/libraries/ros_lib/std_msgs/String.h @@ -0,0 +1,54 @@ +#ifndef _ROS_std_msgs_String_h +#define _ROS_std_msgs_String_h + +#include +#include +#include +#include "ros/msg.h" + +namespace std_msgs +{ + + class String : public ros::Msg + { + public: + const char* data; + + String(): + data("") + { + } + + virtual int serialize(unsigned char *outbuffer) const + { + int offset = 0; + uint32_t length_data = strlen(this->data); + memcpy(outbuffer + offset, &length_data, sizeof(uint32_t)); + offset += 4; + memcpy(outbuffer + offset, this->data, length_data); + offset += length_data; + return offset; + } + + virtual int deserialize(unsigned char *inbuffer) + { + int offset = 0; + uint32_t length_data; + memcpy(&length_data, (inbuffer + offset), sizeof(uint32_t)); + offset += 4; + for(unsigned int k= offset; k< offset+length_data; ++k){ + inbuffer[k-1]=inbuffer[k]; + } + inbuffer[offset+length_data-1]=0; + this->data = (char *)(inbuffer + offset-1); + offset += length_data; + return offset; + } + + const char * getType(){ return "std_msgs/String"; }; + const char * getMD5(){ return "992ce8a1687cec8c8bd883ec73ca41d1"; }; + + }; + +} +#endif \ No newline at end of file diff --git a/turtlebot3_core/libraries/ros_lib/std_msgs/Time.h b/turtlebot3_core/libraries/ros_lib/std_msgs/Time.h new file mode 100644 index 00000000..d87047b1 --- /dev/null +++ b/turtlebot3_core/libraries/ros_lib/std_msgs/Time.h @@ -0,0 +1,61 @@ +#ifndef _ROS_std_msgs_Time_h +#define _ROS_std_msgs_Time_h + +#include +#include +#include +#include "ros/msg.h" +#include "ros/time.h" + +namespace std_msgs +{ + + class Time : public ros::Msg + { + public: + ros::Time data; + + Time(): + data() + { + } + + virtual int serialize(unsigned char *outbuffer) const + { + int offset = 0; + *(outbuffer + offset + 0) = (this->data.sec >> (8 * 0)) & 0xFF; + *(outbuffer + offset + 1) = (this->data.sec >> (8 * 1)) & 0xFF; + *(outbuffer + offset + 2) = (this->data.sec >> (8 * 2)) & 0xFF; + *(outbuffer + offset + 3) = (this->data.sec >> (8 * 3)) & 0xFF; + offset += sizeof(this->data.sec); + *(outbuffer + offset + 0) = (this->data.nsec >> (8 * 0)) & 0xFF; + *(outbuffer + offset + 1) = (this->data.nsec >> (8 * 1)) & 0xFF; + *(outbuffer + offset + 2) = (this->data.nsec >> (8 * 2)) & 0xFF; + *(outbuffer + offset + 3) = (this->data.nsec >> (8 * 3)) & 0xFF; + offset += sizeof(this->data.nsec); + return offset; + } + + virtual int deserialize(unsigned char *inbuffer) + { + int offset = 0; + this->data.sec = ((uint32_t) (*(inbuffer + offset))); + this->data.sec |= ((uint32_t) (*(inbuffer + offset + 1))) << (8 * 1); + this->data.sec |= ((uint32_t) (*(inbuffer + offset + 2))) << (8 * 2); + this->data.sec |= ((uint32_t) (*(inbuffer + offset + 3))) << (8 * 3); + offset += sizeof(this->data.sec); + this->data.nsec = ((uint32_t) (*(inbuffer + offset))); + this->data.nsec |= ((uint32_t) (*(inbuffer + offset + 1))) << (8 * 1); + this->data.nsec |= ((uint32_t) (*(inbuffer + offset + 2))) << (8 * 2); + this->data.nsec |= ((uint32_t) (*(inbuffer + offset + 3))) << (8 * 3); + offset += sizeof(this->data.nsec); + return offset; + } + + const char * getType(){ return "std_msgs/Time"; }; + const char * getMD5(){ return "cd7166c74c552c311fbcc2fe5a7bc289"; }; + + }; + +} +#endif \ No newline at end of file diff --git a/turtlebot3_core/libraries/ros_lib/std_msgs/UInt16.h b/turtlebot3_core/libraries/ros_lib/std_msgs/UInt16.h new file mode 100644 index 00000000..dd3c0cb1 --- /dev/null +++ b/turtlebot3_core/libraries/ros_lib/std_msgs/UInt16.h @@ -0,0 +1,46 @@ +#ifndef _ROS_std_msgs_UInt16_h +#define _ROS_std_msgs_UInt16_h + +#include +#include +#include +#include "ros/msg.h" + +namespace std_msgs +{ + + class UInt16 : public ros::Msg + { + public: + uint16_t data; + + UInt16(): + data(0) + { + } + + virtual int serialize(unsigned char *outbuffer) const + { + int offset = 0; + *(outbuffer + offset + 0) = (this->data >> (8 * 0)) & 0xFF; + *(outbuffer + offset + 1) = (this->data >> (8 * 1)) & 0xFF; + offset += sizeof(this->data); + return offset; + } + + virtual int deserialize(unsigned char *inbuffer) + { + int offset = 0; + this->data = ((uint16_t) (*(inbuffer + offset))); + this->data |= ((uint16_t) (*(inbuffer + offset + 1))) << (8 * 1); + offset += sizeof(this->data); + return offset; + } + + const char * getType(){ return "std_msgs/UInt16"; }; + const char * getMD5(){ return "1df79edf208b629fe6b81923a544552d"; }; + + }; + +} +#endif \ No newline at end of file diff --git a/turtlebot3_core/libraries/ros_lib/std_msgs/UInt16MultiArray.h b/turtlebot3_core/libraries/ros_lib/std_msgs/UInt16MultiArray.h new file mode 100644 index 00000000..7048f0d7 --- /dev/null +++ b/turtlebot3_core/libraries/ros_lib/std_msgs/UInt16MultiArray.h @@ -0,0 +1,71 @@ +#ifndef _ROS_std_msgs_UInt16MultiArray_h +#define _ROS_std_msgs_UInt16MultiArray_h + +#include +#include +#include +#include "ros/msg.h" +#include "std_msgs/MultiArrayLayout.h" + +namespace std_msgs +{ + + class UInt16MultiArray : public ros::Msg + { + public: + std_msgs::MultiArrayLayout layout; + uint32_t data_length; + uint16_t st_data; + uint16_t * data; + + UInt16MultiArray(): + layout(), + data_length(0), data(NULL) + { + } + + virtual int serialize(unsigned char *outbuffer) const + { + int offset = 0; + offset += this->layout.serialize(outbuffer + offset); + *(outbuffer + offset + 0) = (this->data_length >> (8 * 0)) & 0xFF; + *(outbuffer + offset + 1) = (this->data_length >> (8 * 1)) & 0xFF; + *(outbuffer + offset + 2) = (this->data_length >> (8 * 2)) & 0xFF; + *(outbuffer + offset + 3) = (this->data_length >> (8 * 3)) & 0xFF; + offset += sizeof(this->data_length); + for( uint32_t i = 0; i < data_length; i++){ + *(outbuffer + offset + 0) = (this->data[i] >> (8 * 0)) & 0xFF; + *(outbuffer + offset + 1) = (this->data[i] >> (8 * 1)) & 0xFF; + offset += sizeof(this->data[i]); + } + return offset; + } + + virtual int deserialize(unsigned char *inbuffer) + { + int offset = 0; + offset += this->layout.deserialize(inbuffer + offset); + uint32_t data_lengthT = ((uint32_t) (*(inbuffer + offset))); + data_lengthT |= ((uint32_t) (*(inbuffer + offset + 1))) << (8 * 1); + data_lengthT |= ((uint32_t) (*(inbuffer + offset + 2))) << (8 * 2); + data_lengthT |= ((uint32_t) (*(inbuffer + offset + 3))) << (8 * 3); + offset += sizeof(this->data_length); + if(data_lengthT > data_length) + this->data = (uint16_t*)realloc(this->data, data_lengthT * sizeof(uint16_t)); + data_length = data_lengthT; + for( uint32_t i = 0; i < data_length; i++){ + this->st_data = ((uint16_t) (*(inbuffer + offset))); + this->st_data |= ((uint16_t) (*(inbuffer + offset + 1))) << (8 * 1); + offset += sizeof(this->st_data); + memcpy( &(this->data[i]), &(this->st_data), sizeof(uint16_t)); + } + return offset; + } + + const char * getType(){ return "std_msgs/UInt16MultiArray"; }; + const char * getMD5(){ return "52f264f1c973c4b73790d384c6cb4484"; }; + + }; + +} +#endif \ No newline at end of file diff --git a/turtlebot3_core/libraries/ros_lib/std_msgs/UInt32.h b/turtlebot3_core/libraries/ros_lib/std_msgs/UInt32.h new file mode 100644 index 00000000..170208ff --- /dev/null +++ b/turtlebot3_core/libraries/ros_lib/std_msgs/UInt32.h @@ -0,0 +1,50 @@ +#ifndef _ROS_std_msgs_UInt32_h +#define _ROS_std_msgs_UInt32_h + +#include +#include +#include +#include "ros/msg.h" + +namespace std_msgs +{ + + class UInt32 : public ros::Msg + { + public: + uint32_t data; + + UInt32(): + data(0) + { + } + + virtual int serialize(unsigned char *outbuffer) const + { + int offset = 0; + *(outbuffer + offset + 0) = (this->data >> (8 * 0)) & 0xFF; + *(outbuffer + offset + 1) = (this->data >> (8 * 1)) & 0xFF; + *(outbuffer + offset + 2) = (this->data >> (8 * 2)) & 0xFF; + *(outbuffer + offset + 3) = (this->data >> (8 * 3)) & 0xFF; + offset += sizeof(this->data); + return offset; + } + + virtual int deserialize(unsigned char *inbuffer) + { + int offset = 0; + this->data = ((uint32_t) (*(inbuffer + offset))); + this->data |= ((uint32_t) (*(inbuffer + offset + 1))) << (8 * 1); + this->data |= ((uint32_t) (*(inbuffer + offset + 2))) << (8 * 2); + this->data |= ((uint32_t) (*(inbuffer + offset + 3))) << (8 * 3); + offset += sizeof(this->data); + return offset; + } + + const char * getType(){ return "std_msgs/UInt32"; }; + const char * getMD5(){ return "304a39449588c7f8ce2df6e8001c5fce"; }; + + }; + +} +#endif \ No newline at end of file diff --git a/turtlebot3_core/libraries/ros_lib/std_msgs/UInt32MultiArray.h b/turtlebot3_core/libraries/ros_lib/std_msgs/UInt32MultiArray.h new file mode 100644 index 00000000..25f23878 --- /dev/null +++ b/turtlebot3_core/libraries/ros_lib/std_msgs/UInt32MultiArray.h @@ -0,0 +1,75 @@ +#ifndef _ROS_std_msgs_UInt32MultiArray_h +#define _ROS_std_msgs_UInt32MultiArray_h + +#include +#include +#include +#include "ros/msg.h" +#include "std_msgs/MultiArrayLayout.h" + +namespace std_msgs +{ + + class UInt32MultiArray : public ros::Msg + { + public: + std_msgs::MultiArrayLayout layout; + uint32_t data_length; + uint32_t st_data; + uint32_t * data; + + UInt32MultiArray(): + layout(), + data_length(0), data(NULL) + { + } + + virtual int serialize(unsigned char *outbuffer) const + { + int offset = 0; + offset += this->layout.serialize(outbuffer + offset); + *(outbuffer + offset + 0) = (this->data_length >> (8 * 0)) & 0xFF; + *(outbuffer + offset + 1) = (this->data_length >> (8 * 1)) & 0xFF; + *(outbuffer + offset + 2) = (this->data_length >> (8 * 2)) & 0xFF; + *(outbuffer + offset + 3) = (this->data_length >> (8 * 3)) & 0xFF; + offset += sizeof(this->data_length); + for( uint32_t i = 0; i < data_length; i++){ + *(outbuffer + offset + 0) = (this->data[i] >> (8 * 0)) & 0xFF; + *(outbuffer + offset + 1) = (this->data[i] >> (8 * 1)) & 0xFF; + *(outbuffer + offset + 2) = (this->data[i] >> (8 * 2)) & 0xFF; + *(outbuffer + offset + 3) = (this->data[i] >> (8 * 3)) & 0xFF; + offset += sizeof(this->data[i]); + } + return offset; + } + + virtual int deserialize(unsigned char *inbuffer) + { + int offset = 0; + offset += this->layout.deserialize(inbuffer + offset); + uint32_t data_lengthT = ((uint32_t) (*(inbuffer + offset))); + data_lengthT |= ((uint32_t) (*(inbuffer + offset + 1))) << (8 * 1); + data_lengthT |= ((uint32_t) (*(inbuffer + offset + 2))) << (8 * 2); + data_lengthT |= ((uint32_t) (*(inbuffer + offset + 3))) << (8 * 3); + offset += sizeof(this->data_length); + if(data_lengthT > data_length) + this->data = (uint32_t*)realloc(this->data, data_lengthT * sizeof(uint32_t)); + data_length = data_lengthT; + for( uint32_t i = 0; i < data_length; i++){ + this->st_data = ((uint32_t) (*(inbuffer + offset))); + this->st_data |= ((uint32_t) (*(inbuffer + offset + 1))) << (8 * 1); + this->st_data |= ((uint32_t) (*(inbuffer + offset + 2))) << (8 * 2); + this->st_data |= ((uint32_t) (*(inbuffer + offset + 3))) << (8 * 3); + offset += sizeof(this->st_data); + memcpy( &(this->data[i]), &(this->st_data), sizeof(uint32_t)); + } + return offset; + } + + const char * getType(){ return "std_msgs/UInt32MultiArray"; }; + const char * getMD5(){ return "4d6a180abc9be191b96a7eda6c8a233d"; }; + + }; + +} +#endif \ No newline at end of file diff --git a/turtlebot3_core/libraries/ros_lib/std_msgs/UInt64.h b/turtlebot3_core/libraries/ros_lib/std_msgs/UInt64.h new file mode 100644 index 00000000..d69b9fe6 --- /dev/null +++ b/turtlebot3_core/libraries/ros_lib/std_msgs/UInt64.h @@ -0,0 +1,61 @@ +#ifndef _ROS_std_msgs_UInt64_h +#define _ROS_std_msgs_UInt64_h + +#include +#include +#include +#include "ros/msg.h" + +namespace std_msgs +{ + + class UInt64 : public ros::Msg + { + public: + uint64_t data; + + UInt64(): + data(0) + { + } + + virtual int serialize(unsigned char *outbuffer) const + { + int offset = 0; + union { + uint64_t real; + uint32_t base; + } u_data; + u_data.real = this->data; + *(outbuffer + offset + 0) = (u_data.base >> (8 * 0)) & 0xFF; + *(outbuffer + offset + 1) = (u_data.base >> (8 * 1)) & 0xFF; + *(outbuffer + offset + 2) = (u_data.base >> (8 * 2)) & 0xFF; + *(outbuffer + offset + 3) = (u_data.base >> (8 * 3)) & 0xFF; + offset += sizeof(this->data); + return offset; + } + + virtual int deserialize(unsigned char *inbuffer) + { + int offset = 0; + union { + uint64_t real; + uint32_t base; + } u_data; + u_data.base = 0; + u_data.base |= ((uint32_t) (*(inbuffer + offset + 0))) << (8 * 0); + u_data.base |= ((uint32_t) (*(inbuffer + offset + 1))) << (8 * 1); + u_data.base |= ((uint32_t) (*(inbuffer + offset + 2))) << (8 * 2); + u_data.base |= ((uint32_t) (*(inbuffer + offset + 3))) << (8 * 3); + this->data = u_data.real; + offset += sizeof(this->data); + return offset; + } + + const char * getType(){ return "std_msgs/UInt64"; }; + const char * getMD5(){ return "1b2a79973e8bf53d7b53acb71299cb57"; }; + + }; + +} +#endif \ No newline at end of file diff --git a/turtlebot3_core/libraries/ros_lib/std_msgs/UInt64MultiArray.h b/turtlebot3_core/libraries/ros_lib/std_msgs/UInt64MultiArray.h new file mode 100644 index 00000000..1c2e3bc3 --- /dev/null +++ b/turtlebot3_core/libraries/ros_lib/std_msgs/UInt64MultiArray.h @@ -0,0 +1,86 @@ +#ifndef _ROS_std_msgs_UInt64MultiArray_h +#define _ROS_std_msgs_UInt64MultiArray_h + +#include +#include +#include +#include "ros/msg.h" +#include "std_msgs/MultiArrayLayout.h" + +namespace std_msgs +{ + + class UInt64MultiArray : public ros::Msg + { + public: + std_msgs::MultiArrayLayout layout; + uint32_t data_length; + uint64_t st_data; + uint64_t * data; + + UInt64MultiArray(): + layout(), + data_length(0), data(NULL) + { + } + + virtual int serialize(unsigned char *outbuffer) const + { + int offset = 0; + offset += this->layout.serialize(outbuffer + offset); + *(outbuffer + offset + 0) = (this->data_length >> (8 * 0)) & 0xFF; + *(outbuffer + offset + 1) = (this->data_length >> (8 * 1)) & 0xFF; + *(outbuffer + offset + 2) = (this->data_length >> (8 * 2)) & 0xFF; + *(outbuffer + offset + 3) = (this->data_length >> (8 * 3)) & 0xFF; + offset += sizeof(this->data_length); + for( uint32_t i = 0; i < data_length; i++){ + union { + uint64_t real; + uint32_t base; + } u_datai; + u_datai.real = this->data[i]; + *(outbuffer + offset + 0) = (u_datai.base >> (8 * 0)) & 0xFF; + *(outbuffer + offset + 1) = (u_datai.base >> (8 * 1)) & 0xFF; + *(outbuffer + offset + 2) = (u_datai.base >> (8 * 2)) & 0xFF; + *(outbuffer + offset + 3) = (u_datai.base >> (8 * 3)) & 0xFF; + offset += sizeof(this->data[i]); + } + return offset; + } + + virtual int deserialize(unsigned char *inbuffer) + { + int offset = 0; + offset += this->layout.deserialize(inbuffer + offset); + uint32_t data_lengthT = ((uint32_t) (*(inbuffer + offset))); + data_lengthT |= ((uint32_t) (*(inbuffer + offset + 1))) << (8 * 1); + data_lengthT |= ((uint32_t) (*(inbuffer + offset + 2))) << (8 * 2); + data_lengthT |= ((uint32_t) (*(inbuffer + offset + 3))) << (8 * 3); + offset += sizeof(this->data_length); + if(data_lengthT > data_length) + this->data = (uint64_t*)realloc(this->data, data_lengthT * sizeof(uint64_t)); + data_length = data_lengthT; + for( uint32_t i = 0; i < data_length; i++){ + union { + uint64_t real; + uint32_t base; + } u_st_data; + u_st_data.base = 0; + u_st_data.base |= ((uint32_t) (*(inbuffer + offset + 0))) << (8 * 0); + u_st_data.base |= ((uint32_t) (*(inbuffer + offset + 1))) << (8 * 1); + u_st_data.base |= ((uint32_t) (*(inbuffer + offset + 2))) << (8 * 2); + u_st_data.base |= ((uint32_t) (*(inbuffer + offset + 3))) << (8 * 3); + this->st_data = u_st_data.real; + offset += sizeof(this->st_data); + memcpy( &(this->data[i]), &(this->st_data), sizeof(uint64_t)); + } + return offset; + } + + const char * getType(){ return "std_msgs/UInt64MultiArray"; }; + const char * getMD5(){ return "6088f127afb1d6c72927aa1247e945af"; }; + + }; + +} +#endif \ No newline at end of file diff --git a/turtlebot3_core/libraries/ros_lib/std_msgs/UInt8.h b/turtlebot3_core/libraries/ros_lib/std_msgs/UInt8.h new file mode 100644 index 00000000..15dbb6f2 --- /dev/null +++ b/turtlebot3_core/libraries/ros_lib/std_msgs/UInt8.h @@ -0,0 +1,44 @@ +#ifndef _ROS_std_msgs_UInt8_h +#define _ROS_std_msgs_UInt8_h + +#include +#include +#include +#include "ros/msg.h" + +namespace std_msgs +{ + + class UInt8 : public ros::Msg + { + public: + uint8_t data; + + UInt8(): + data(0) + { + } + + virtual int serialize(unsigned char *outbuffer) const + { + int offset = 0; + *(outbuffer + offset + 0) = (this->data >> (8 * 0)) & 0xFF; + offset += sizeof(this->data); + return offset; + } + + virtual int deserialize(unsigned char *inbuffer) + { + int offset = 0; + this->data = ((uint8_t) (*(inbuffer + offset))); + offset += sizeof(this->data); + return offset; + } + + const char * getType(){ return "std_msgs/UInt8"; }; + const char * getMD5(){ return "7c8164229e7d2c17eb95e9231617fdee"; }; + + }; + +} +#endif \ No newline at end of file diff --git a/turtlebot3_core/libraries/ros_lib/std_msgs/UInt8MultiArray.h b/turtlebot3_core/libraries/ros_lib/std_msgs/UInt8MultiArray.h new file mode 100644 index 00000000..65ef540b --- /dev/null +++ b/turtlebot3_core/libraries/ros_lib/std_msgs/UInt8MultiArray.h @@ -0,0 +1,69 @@ +#ifndef _ROS_std_msgs_UInt8MultiArray_h +#define _ROS_std_msgs_UInt8MultiArray_h + +#include +#include +#include +#include "ros/msg.h" +#include "std_msgs/MultiArrayLayout.h" + +namespace std_msgs +{ + + class UInt8MultiArray : public ros::Msg + { + public: + std_msgs::MultiArrayLayout layout; + uint32_t data_length; + uint8_t st_data; + uint8_t * data; + + UInt8MultiArray(): + layout(), + data_length(0), data(NULL) + { + } + + virtual int serialize(unsigned char *outbuffer) const + { + int offset = 0; + offset += this->layout.serialize(outbuffer + offset); + *(outbuffer + offset + 0) = (this->data_length >> (8 * 0)) & 0xFF; + *(outbuffer + offset + 1) = (this->data_length >> (8 * 1)) & 0xFF; + *(outbuffer + offset + 2) = (this->data_length >> (8 * 2)) & 0xFF; + *(outbuffer + offset + 3) = (this->data_length >> (8 * 3)) & 0xFF; + offset += sizeof(this->data_length); + for( uint32_t i = 0; i < data_length; i++){ + *(outbuffer + offset + 0) = (this->data[i] >> (8 * 0)) & 0xFF; + offset += sizeof(this->data[i]); + } + return offset; + } + + virtual int deserialize(unsigned char *inbuffer) + { + int offset = 0; + offset += this->layout.deserialize(inbuffer + offset); + uint32_t data_lengthT = ((uint32_t) (*(inbuffer + offset))); + data_lengthT |= ((uint32_t) (*(inbuffer + offset + 1))) << (8 * 1); + data_lengthT |= ((uint32_t) (*(inbuffer + offset + 2))) << (8 * 2); + data_lengthT |= ((uint32_t) (*(inbuffer + offset + 3))) << (8 * 3); + offset += sizeof(this->data_length); + if(data_lengthT > data_length) + this->data = (uint8_t*)realloc(this->data, data_lengthT * sizeof(uint8_t)); + data_length = data_lengthT; + for( uint32_t i = 0; i < data_length; i++){ + this->st_data = ((uint8_t) (*(inbuffer + offset))); + offset += sizeof(this->st_data); + memcpy( &(this->data[i]), &(this->st_data), sizeof(uint8_t)); + } + return offset; + } + + const char * getType(){ return "std_msgs/UInt8MultiArray"; }; + const char * getMD5(){ return "82373f1612381bb6ee473b5cd6f5d89c"; }; + + }; + +} +#endif \ No newline at end of file diff --git a/turtlebot3_core/libraries/ros_lib/std_srvs/Empty.h b/turtlebot3_core/libraries/ros_lib/std_srvs/Empty.h new file mode 100644 index 00000000..b040dd2e --- /dev/null +++ b/turtlebot3_core/libraries/ros_lib/std_srvs/Empty.h @@ -0,0 +1,70 @@ +#ifndef _ROS_SERVICE_Empty_h +#define _ROS_SERVICE_Empty_h +#include +#include +#include +#include "ros/msg.h" + +namespace std_srvs +{ + +static const char EMPTY[] = "std_srvs/Empty"; + + class EmptyRequest : public ros::Msg + { + public: + + EmptyRequest() + { + } + + virtual int serialize(unsigned char *outbuffer) const + { + int offset = 0; + return offset; + } + + virtual int deserialize(unsigned char *inbuffer) + { + int offset = 0; + return offset; + } + + const char * getType(){ return EMPTY; }; + const char * getMD5(){ return "d41d8cd98f00b204e9800998ecf8427e"; }; + + }; + + class EmptyResponse : public ros::Msg + { + public: + + EmptyResponse() + { + } + + virtual int serialize(unsigned char *outbuffer) const + { + int offset = 0; + return offset; + } + + virtual int deserialize(unsigned char *inbuffer) + { + int offset = 0; + return offset; + } + + const char * getType(){ return EMPTY; }; + const char * getMD5(){ return "d41d8cd98f00b204e9800998ecf8427e"; }; + + }; + + class Empty { + public: + typedef EmptyRequest Request; + typedef EmptyResponse Response; + }; + +} +#endif diff --git a/turtlebot3_core/libraries/ros_lib/std_srvs/SetBool.h b/turtlebot3_core/libraries/ros_lib/std_srvs/SetBool.h new file mode 100644 index 00000000..796485b2 --- /dev/null +++ b/turtlebot3_core/libraries/ros_lib/std_srvs/SetBool.h @@ -0,0 +1,120 @@ +#ifndef _ROS_SERVICE_SetBool_h +#define _ROS_SERVICE_SetBool_h +#include +#include +#include +#include "ros/msg.h" + +namespace std_srvs +{ + +static const char SETBOOL[] = "std_srvs/SetBool"; + + class SetBoolRequest : public ros::Msg + { + public: + bool data; + + SetBoolRequest(): + data(0) + { + } + + virtual int serialize(unsigned char *outbuffer) const + { + int offset = 0; + union { + bool real; + uint8_t base; + } u_data; + u_data.real = this->data; + *(outbuffer + offset + 0) = (u_data.base >> (8 * 0)) & 0xFF; + offset += sizeof(this->data); + return offset; + } + + virtual int deserialize(unsigned char *inbuffer) + { + int offset = 0; + union { + bool real; + uint8_t base; + } u_data; + u_data.base = 0; + u_data.base |= ((uint8_t) (*(inbuffer + offset + 0))) << (8 * 0); + this->data = u_data.real; + offset += sizeof(this->data); + return offset; + } + + const char * getType(){ return SETBOOL; }; + const char * getMD5(){ return "8b94c1b53db61fb6aed406028ad6332a"; }; + + }; + + class SetBoolResponse : public ros::Msg + { + public: + bool success; + const char* message; + + SetBoolResponse(): + success(0), + message("") + { + } + + virtual int serialize(unsigned char *outbuffer) const + { + int offset = 0; + union { + bool real; + uint8_t base; + } u_success; + u_success.real = this->success; + *(outbuffer + offset + 0) = (u_success.base >> (8 * 0)) & 0xFF; + offset += sizeof(this->success); + uint32_t length_message = strlen(this->message); + memcpy(outbuffer + offset, &length_message, sizeof(uint32_t)); + offset += 4; + memcpy(outbuffer + offset, this->message, length_message); + offset += length_message; + return offset; + } + + virtual int deserialize(unsigned char *inbuffer) + { + int offset = 0; + union { + bool real; + uint8_t base; + } u_success; + u_success.base = 0; + u_success.base |= ((uint8_t) (*(inbuffer + offset + 0))) << (8 * 0); + this->success = u_success.real; + offset += sizeof(this->success); + uint32_t length_message; + memcpy(&length_message, (inbuffer + offset), sizeof(uint32_t)); + offset += 4; + for(unsigned int k= offset; k< offset+length_message; ++k){ + inbuffer[k-1]=inbuffer[k]; + } + inbuffer[offset+length_message-1]=0; + this->message = (char *)(inbuffer + offset-1); + offset += length_message; + return offset; + } + + const char * getType(){ return SETBOOL; }; + const char * getMD5(){ return "937c9679a518e3a18d831e57125ea522"; }; + + }; + + class SetBool { + public: + typedef SetBoolRequest Request; + typedef SetBoolResponse Response; + }; + +} +#endif diff --git a/turtlebot3_core/libraries/ros_lib/std_srvs/Trigger.h b/turtlebot3_core/libraries/ros_lib/std_srvs/Trigger.h new file mode 100644 index 00000000..958d2b82 --- /dev/null +++ b/turtlebot3_core/libraries/ros_lib/std_srvs/Trigger.h @@ -0,0 +1,103 @@ +#ifndef _ROS_SERVICE_Trigger_h +#define _ROS_SERVICE_Trigger_h +#include +#include +#include +#include "ros/msg.h" + +namespace std_srvs +{ + +static const char TRIGGER[] = "std_srvs/Trigger"; + + class TriggerRequest : public ros::Msg + { + public: + + TriggerRequest() + { + } + + virtual int serialize(unsigned char *outbuffer) const + { + int offset = 0; + return offset; + } + + virtual int deserialize(unsigned char *inbuffer) + { + int offset = 0; + return offset; + } + + const char * getType(){ return TRIGGER; }; + const char * getMD5(){ return "d41d8cd98f00b204e9800998ecf8427e"; }; + + }; + + class TriggerResponse : public ros::Msg + { + public: + bool success; + const char* message; + + TriggerResponse(): + success(0), + message("") + { + } + + virtual int serialize(unsigned char *outbuffer) const + { + int offset = 0; + union { + bool real; + uint8_t base; + } u_success; + u_success.real = this->success; + *(outbuffer + offset + 0) = (u_success.base >> (8 * 0)) & 0xFF; + offset += sizeof(this->success); + uint32_t length_message = strlen(this->message); + memcpy(outbuffer + offset, &length_message, sizeof(uint32_t)); + offset += 4; + memcpy(outbuffer + offset, this->message, length_message); + offset += length_message; + return offset; + } + + virtual int deserialize(unsigned char *inbuffer) + { + int offset = 0; + union { + bool real; + uint8_t base; + } u_success; + u_success.base = 0; + u_success.base |= ((uint8_t) (*(inbuffer + offset + 0))) << (8 * 0); + this->success = u_success.real; + offset += sizeof(this->success); + uint32_t length_message; + memcpy(&length_message, (inbuffer + offset), sizeof(uint32_t)); + offset += 4; + for(unsigned int k= offset; k< offset+length_message; ++k){ + inbuffer[k-1]=inbuffer[k]; + } + inbuffer[offset+length_message-1]=0; + this->message = (char *)(inbuffer + offset-1); + offset += length_message; + return offset; + } + + const char * getType(){ return TRIGGER; }; + const char * getMD5(){ return "937c9679a518e3a18d831e57125ea522"; }; + + }; + + class Trigger { + public: + typedef TriggerRequest Request; + typedef TriggerResponse Response; + }; + +} +#endif diff --git a/turtlebot3_core/libraries/ros_lib/tf/FrameGraph.h b/turtlebot3_core/libraries/ros_lib/tf/FrameGraph.h new file mode 100644 index 00000000..b7d2f31b --- /dev/null +++ b/turtlebot3_core/libraries/ros_lib/tf/FrameGraph.h @@ -0,0 +1,86 @@ +#ifndef _ROS_SERVICE_FrameGraph_h +#define _ROS_SERVICE_FrameGraph_h +#include +#include +#include +#include "ros/msg.h" + +namespace tf +{ + +static const char FRAMEGRAPH[] = "tf/FrameGraph"; + + class FrameGraphRequest : public ros::Msg + { + public: + + FrameGraphRequest() + { + } + + virtual int serialize(unsigned char *outbuffer) const + { + int offset = 0; + return offset; + } + + virtual int deserialize(unsigned char *inbuffer) + { + int offset = 0; + return offset; + } + + const char * getType(){ return FRAMEGRAPH; }; + const char * getMD5(){ return "d41d8cd98f00b204e9800998ecf8427e"; }; + + }; + + class FrameGraphResponse : public ros::Msg + { + public: + const char* dot_graph; + + FrameGraphResponse(): + dot_graph("") + { + } + + virtual int serialize(unsigned char *outbuffer) const + { + int offset = 0; + uint32_t length_dot_graph = strlen(this->dot_graph); + memcpy(outbuffer + offset, &length_dot_graph, sizeof(uint32_t)); + offset += 4; + memcpy(outbuffer + offset, this->dot_graph, length_dot_graph); + offset += length_dot_graph; + return offset; + } + + virtual int deserialize(unsigned char *inbuffer) + { + int offset = 0; + uint32_t length_dot_graph; + memcpy(&length_dot_graph, (inbuffer + offset), sizeof(uint32_t)); + offset += 4; + for(unsigned int k= offset; k< offset+length_dot_graph; ++k){ + inbuffer[k-1]=inbuffer[k]; + } + inbuffer[offset+length_dot_graph-1]=0; + this->dot_graph = (char *)(inbuffer + offset-1); + offset += length_dot_graph; + return offset; + } + + const char * getType(){ return FRAMEGRAPH; }; + const char * getMD5(){ return "c4af9ac907e58e906eb0b6e3c58478c0"; }; + + }; + + class FrameGraph { + public: + typedef FrameGraphRequest Request; + typedef FrameGraphResponse Response; + }; + +} +#endif diff --git a/turtlebot3_core/libraries/ros_lib/tf/tf.h b/turtlebot3_core/libraries/ros_lib/tf/tf.h new file mode 100644 index 00000000..a2888e3d --- /dev/null +++ b/turtlebot3_core/libraries/ros_lib/tf/tf.h @@ -0,0 +1,56 @@ +/* + * Software License Agreement (BSD License) + * + * Copyright (c) 2011, Willow Garage, Inc. + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above + * copyright notice, this list of conditions and the following + * disclaimer in the documentation and/or other materials provided + * with the distribution. + * * Neither the name of Willow Garage, Inc. nor the names of its + * contributors may be used to endorse or promote prducts derived + * from this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; + * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + */ + +#ifndef ROS_TF_H_ +#define ROS_TF_H_ + +#include "geometry_msgs/TransformStamped.h" + +namespace tf +{ + + static inline geometry_msgs::Quaternion createQuaternionFromYaw(double yaw) + { + geometry_msgs::Quaternion q; + q.x = 0; + q.y = 0; + q.z = sin(yaw * 0.5); + q.w = cos(yaw * 0.5); + return q; + } + +} + +#endif + diff --git a/turtlebot3_core/libraries/ros_lib/tf/tfMessage.h b/turtlebot3_core/libraries/ros_lib/tf/tfMessage.h new file mode 100644 index 00000000..6552abac --- /dev/null +++ b/turtlebot3_core/libraries/ros_lib/tf/tfMessage.h @@ -0,0 +1,63 @@ +#ifndef _ROS_tf_tfMessage_h +#define _ROS_tf_tfMessage_h + +#include +#include +#include +#include "ros/msg.h" +#include "geometry_msgs/TransformStamped.h" + +namespace tf +{ + + class tfMessage : public ros::Msg + { + public: + uint32_t transforms_length; + geometry_msgs::TransformStamped st_transforms; + geometry_msgs::TransformStamped * transforms; + + tfMessage(): + transforms_length(0), transforms(NULL) + { + } + + virtual int serialize(unsigned char *outbuffer) const + { + int offset = 0; + *(outbuffer + offset + 0) = (this->transforms_length >> (8 * 0)) & 0xFF; + *(outbuffer + offset + 1) = (this->transforms_length >> (8 * 1)) & 0xFF; + *(outbuffer + offset + 2) = (this->transforms_length >> (8 * 2)) & 0xFF; + *(outbuffer + offset + 3) = (this->transforms_length >> (8 * 3)) & 0xFF; + offset += sizeof(this->transforms_length); + for( uint32_t i = 0; i < transforms_length; i++){ + offset += this->transforms[i].serialize(outbuffer + offset); + } + return offset; + } + + virtual int deserialize(unsigned char *inbuffer) + { + int offset = 0; + uint32_t transforms_lengthT = ((uint32_t) (*(inbuffer + offset))); + transforms_lengthT |= ((uint32_t) (*(inbuffer + offset + 1))) << (8 * 1); + transforms_lengthT |= ((uint32_t) (*(inbuffer + offset + 2))) << (8 * 2); + transforms_lengthT |= ((uint32_t) (*(inbuffer + offset + 3))) << (8 * 3); + offset += sizeof(this->transforms_length); + if(transforms_lengthT > transforms_length) + this->transforms = (geometry_msgs::TransformStamped*)realloc(this->transforms, transforms_lengthT * sizeof(geometry_msgs::TransformStamped)); + transforms_length = transforms_lengthT; + for( uint32_t i = 0; i < transforms_length; i++){ + offset += this->st_transforms.deserialize(inbuffer + offset); + memcpy( &(this->transforms[i]), &(this->st_transforms), sizeof(geometry_msgs::TransformStamped)); + } + return offset; + } + + const char * getType(){ return "tf/tfMessage"; }; + const char * getMD5(){ return "94810edda583a504dfda3829e70d7eec"; }; + + }; + +} +#endif \ No newline at end of file diff --git a/turtlebot3_core/libraries/ros_lib/tf/transform_broadcaster.h b/turtlebot3_core/libraries/ros_lib/tf/transform_broadcaster.h new file mode 100644 index 00000000..caaba817 --- /dev/null +++ b/turtlebot3_core/libraries/ros_lib/tf/transform_broadcaster.h @@ -0,0 +1,69 @@ +/* + * Software License Agreement (BSD License) + * + * Copyright (c) 2011, Willow Garage, Inc. + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above + * copyright notice, this list of conditions and the following + * disclaimer in the documentation and/or other materials provided + * with the distribution. + * * Neither the name of Willow Garage, Inc. nor the names of its + * contributors may be used to endorse or promote prducts derived + * from this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; + * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + */ + +#ifndef ROS_TRANSFORM_BROADCASTER_H_ +#define ROS_TRANSFORM_BROADCASTER_H_ + +#include "ros.h" +#include "tfMessage.h" + +namespace tf +{ + + class TransformBroadcaster + { + public: + TransformBroadcaster() : publisher_("/tf", &internal_msg) {} + + void init(ros::NodeHandle &nh) + { + nh.advertise(publisher_); + } + + void sendTransform(geometry_msgs::TransformStamped &transform) + { + internal_msg.transforms_length = 1; + internal_msg.transforms = &transform; + publisher_.publish(&internal_msg); + } + + private: + tf::tfMessage internal_msg; + ros::Publisher publisher_; + }; + +} + +#endif + diff --git a/turtlebot3_core/libraries/ros_lib/tf2_msgs/FrameGraph.h b/turtlebot3_core/libraries/ros_lib/tf2_msgs/FrameGraph.h new file mode 100644 index 00000000..a703dd20 --- /dev/null +++ b/turtlebot3_core/libraries/ros_lib/tf2_msgs/FrameGraph.h @@ -0,0 +1,86 @@ +#ifndef _ROS_SERVICE_FrameGraph_h +#define _ROS_SERVICE_FrameGraph_h +#include +#include +#include +#include "ros/msg.h" + +namespace tf2_msgs +{ + +static const char FRAMEGRAPH[] = "tf2_msgs/FrameGraph"; + + class FrameGraphRequest : public ros::Msg + { + public: + + FrameGraphRequest() + { + } + + virtual int serialize(unsigned char *outbuffer) const + { + int offset = 0; + return offset; + } + + virtual int deserialize(unsigned char *inbuffer) + { + int offset = 0; + return offset; + } + + const char * getType(){ return FRAMEGRAPH; }; + const char * getMD5(){ return "d41d8cd98f00b204e9800998ecf8427e"; }; + + }; + + class FrameGraphResponse : public ros::Msg + { + public: + const char* frame_yaml; + + FrameGraphResponse(): + frame_yaml("") + { + } + + virtual int serialize(unsigned char *outbuffer) const + { + int offset = 0; + uint32_t length_frame_yaml = strlen(this->frame_yaml); + memcpy(outbuffer + offset, &length_frame_yaml, sizeof(uint32_t)); + offset += 4; + memcpy(outbuffer + offset, this->frame_yaml, length_frame_yaml); + offset += length_frame_yaml; + return offset; + } + + virtual int deserialize(unsigned char *inbuffer) + { + int offset = 0; + uint32_t length_frame_yaml; + memcpy(&length_frame_yaml, (inbuffer + offset), sizeof(uint32_t)); + offset += 4; + for(unsigned int k= offset; k< offset+length_frame_yaml; ++k){ + inbuffer[k-1]=inbuffer[k]; + } + inbuffer[offset+length_frame_yaml-1]=0; + this->frame_yaml = (char *)(inbuffer + offset-1); + offset += length_frame_yaml; + return offset; + } + + const char * getType(){ return FRAMEGRAPH; }; + const char * getMD5(){ return "437ea58e9463815a0d511c7326b686b0"; }; + + }; + + class FrameGraph { + public: + typedef FrameGraphRequest Request; + typedef FrameGraphResponse Response; + }; + +} +#endif diff --git a/turtlebot3_core/libraries/ros_lib/tf2_msgs/LookupTransformAction.h b/turtlebot3_core/libraries/ros_lib/tf2_msgs/LookupTransformAction.h new file mode 100644 index 00000000..2a0178f1 --- /dev/null +++ b/turtlebot3_core/libraries/ros_lib/tf2_msgs/LookupTransformAction.h @@ -0,0 +1,53 @@ +#ifndef _ROS_tf2_msgs_LookupTransformAction_h +#define _ROS_tf2_msgs_LookupTransformAction_h + +#include +#include +#include +#include "ros/msg.h" +#include "tf2_msgs/LookupTransformActionGoal.h" +#include "tf2_msgs/LookupTransformActionResult.h" +#include "tf2_msgs/LookupTransformActionFeedback.h" + +namespace tf2_msgs +{ + + class LookupTransformAction : public ros::Msg + { + public: + tf2_msgs::LookupTransformActionGoal action_goal; + tf2_msgs::LookupTransformActionResult action_result; + tf2_msgs::LookupTransformActionFeedback action_feedback; + + LookupTransformAction(): + action_goal(), + action_result(), + action_feedback() + { + } + + virtual int serialize(unsigned char *outbuffer) const + { + int offset = 0; + offset += this->action_goal.serialize(outbuffer + offset); + offset += this->action_result.serialize(outbuffer + offset); + offset += this->action_feedback.serialize(outbuffer + offset); + return offset; + } + + virtual int deserialize(unsigned char *inbuffer) + { + int offset = 0; + offset += this->action_goal.deserialize(inbuffer + offset); + offset += this->action_result.deserialize(inbuffer + offset); + offset += this->action_feedback.deserialize(inbuffer + offset); + return offset; + } + + const char * getType(){ return "tf2_msgs/LookupTransformAction"; }; + const char * getMD5(){ return "7ee01ba91a56c2245c610992dbaa3c37"; }; + + }; + +} +#endif \ No newline at end of file diff --git a/turtlebot3_core/libraries/ros_lib/tf2_msgs/LookupTransformActionFeedback.h b/turtlebot3_core/libraries/ros_lib/tf2_msgs/LookupTransformActionFeedback.h new file mode 100644 index 00000000..f0de7afc --- /dev/null +++ b/turtlebot3_core/libraries/ros_lib/tf2_msgs/LookupTransformActionFeedback.h @@ -0,0 +1,53 @@ +#ifndef _ROS_tf2_msgs_LookupTransformActionFeedback_h +#define _ROS_tf2_msgs_LookupTransformActionFeedback_h + +#include +#include +#include +#include "ros/msg.h" +#include "std_msgs/Header.h" +#include "actionlib_msgs/GoalStatus.h" +#include "tf2_msgs/LookupTransformFeedback.h" + +namespace tf2_msgs +{ + + class LookupTransformActionFeedback : public ros::Msg + { + public: + std_msgs::Header header; + actionlib_msgs::GoalStatus status; + tf2_msgs::LookupTransformFeedback feedback; + + LookupTransformActionFeedback(): + header(), + status(), + feedback() + { + } + + virtual int serialize(unsigned char *outbuffer) const + { + int offset = 0; + offset += this->header.serialize(outbuffer + offset); + offset += this->status.serialize(outbuffer + offset); + offset += this->feedback.serialize(outbuffer + offset); + return offset; + } + + virtual int deserialize(unsigned char *inbuffer) + { + int offset = 0; + offset += this->header.deserialize(inbuffer + offset); + offset += this->status.deserialize(inbuffer + offset); + offset += this->feedback.deserialize(inbuffer + offset); + return offset; + } + + const char * getType(){ return "tf2_msgs/LookupTransformActionFeedback"; }; + const char * getMD5(){ return "aae20e09065c3809e8a8e87c4c8953fd"; }; + + }; + +} +#endif \ No newline at end of file diff --git a/turtlebot3_core/libraries/ros_lib/tf2_msgs/LookupTransformActionGoal.h b/turtlebot3_core/libraries/ros_lib/tf2_msgs/LookupTransformActionGoal.h new file mode 100644 index 00000000..d668cd90 --- /dev/null +++ b/turtlebot3_core/libraries/ros_lib/tf2_msgs/LookupTransformActionGoal.h @@ -0,0 +1,53 @@ +#ifndef _ROS_tf2_msgs_LookupTransformActionGoal_h +#define _ROS_tf2_msgs_LookupTransformActionGoal_h + +#include +#include +#include +#include "ros/msg.h" +#include "std_msgs/Header.h" +#include "actionlib_msgs/GoalID.h" +#include "tf2_msgs/LookupTransformGoal.h" + +namespace tf2_msgs +{ + + class LookupTransformActionGoal : public ros::Msg + { + public: + std_msgs::Header header; + actionlib_msgs::GoalID goal_id; + tf2_msgs::LookupTransformGoal goal; + + LookupTransformActionGoal(): + header(), + goal_id(), + goal() + { + } + + virtual int serialize(unsigned char *outbuffer) const + { + int offset = 0; + offset += this->header.serialize(outbuffer + offset); + offset += this->goal_id.serialize(outbuffer + offset); + offset += this->goal.serialize(outbuffer + offset); + return offset; + } + + virtual int deserialize(unsigned char *inbuffer) + { + int offset = 0; + offset += this->header.deserialize(inbuffer + offset); + offset += this->goal_id.deserialize(inbuffer + offset); + offset += this->goal.deserialize(inbuffer + offset); + return offset; + } + + const char * getType(){ return "tf2_msgs/LookupTransformActionGoal"; }; + const char * getMD5(){ return "f2e7bcdb75c847978d0351a13e699da5"; }; + + }; + +} +#endif \ No newline at end of file diff --git a/turtlebot3_core/libraries/ros_lib/tf2_msgs/LookupTransformActionResult.h b/turtlebot3_core/libraries/ros_lib/tf2_msgs/LookupTransformActionResult.h new file mode 100644 index 00000000..99298ffa --- /dev/null +++ b/turtlebot3_core/libraries/ros_lib/tf2_msgs/LookupTransformActionResult.h @@ -0,0 +1,53 @@ +#ifndef _ROS_tf2_msgs_LookupTransformActionResult_h +#define _ROS_tf2_msgs_LookupTransformActionResult_h + +#include +#include +#include +#include "ros/msg.h" +#include "std_msgs/Header.h" +#include "actionlib_msgs/GoalStatus.h" +#include "tf2_msgs/LookupTransformResult.h" + +namespace tf2_msgs +{ + + class LookupTransformActionResult : public ros::Msg + { + public: + std_msgs::Header header; + actionlib_msgs::GoalStatus status; + tf2_msgs::LookupTransformResult result; + + LookupTransformActionResult(): + header(), + status(), + result() + { + } + + virtual int serialize(unsigned char *outbuffer) const + { + int offset = 0; + offset += this->header.serialize(outbuffer + offset); + offset += this->status.serialize(outbuffer + offset); + offset += this->result.serialize(outbuffer + offset); + return offset; + } + + virtual int deserialize(unsigned char *inbuffer) + { + int offset = 0; + offset += this->header.deserialize(inbuffer + offset); + offset += this->status.deserialize(inbuffer + offset); + offset += this->result.deserialize(inbuffer + offset); + return offset; + } + + const char * getType(){ return "tf2_msgs/LookupTransformActionResult"; }; + const char * getMD5(){ return "ac26ce75a41384fa8bb4dc10f491ab90"; }; + + }; + +} +#endif \ No newline at end of file diff --git a/turtlebot3_core/libraries/ros_lib/tf2_msgs/LookupTransformFeedback.h b/turtlebot3_core/libraries/ros_lib/tf2_msgs/LookupTransformFeedback.h new file mode 100644 index 00000000..6be07486 --- /dev/null +++ b/turtlebot3_core/libraries/ros_lib/tf2_msgs/LookupTransformFeedback.h @@ -0,0 +1,38 @@ +#ifndef _ROS_tf2_msgs_LookupTransformFeedback_h +#define _ROS_tf2_msgs_LookupTransformFeedback_h + +#include +#include +#include +#include "ros/msg.h" + +namespace tf2_msgs +{ + + class LookupTransformFeedback : public ros::Msg + { + public: + + LookupTransformFeedback() + { + } + + virtual int serialize(unsigned char *outbuffer) const + { + int offset = 0; + return offset; + } + + virtual int deserialize(unsigned char *inbuffer) + { + int offset = 0; + return offset; + } + + const char * getType(){ return "tf2_msgs/LookupTransformFeedback"; }; + const char * getMD5(){ return "d41d8cd98f00b204e9800998ecf8427e"; }; + + }; + +} +#endif \ No newline at end of file diff --git a/turtlebot3_core/libraries/ros_lib/tf2_msgs/LookupTransformGoal.h b/turtlebot3_core/libraries/ros_lib/tf2_msgs/LookupTransformGoal.h new file mode 100644 index 00000000..f498e451 --- /dev/null +++ b/turtlebot3_core/libraries/ros_lib/tf2_msgs/LookupTransformGoal.h @@ -0,0 +1,171 @@ +#ifndef _ROS_tf2_msgs_LookupTransformGoal_h +#define _ROS_tf2_msgs_LookupTransformGoal_h + +#include +#include +#include +#include "ros/msg.h" +#include "ros/time.h" +#include "ros/duration.h" + +namespace tf2_msgs +{ + + class LookupTransformGoal : public ros::Msg + { + public: + const char* target_frame; + const char* source_frame; + ros::Time source_time; + ros::Duration timeout; + ros::Time target_time; + const char* fixed_frame; + bool advanced; + + LookupTransformGoal(): + target_frame(""), + source_frame(""), + source_time(), + timeout(), + target_time(), + fixed_frame(""), + advanced(0) + { + } + + virtual int serialize(unsigned char *outbuffer) const + { + int offset = 0; + uint32_t length_target_frame = strlen(this->target_frame); + memcpy(outbuffer + offset, &length_target_frame, sizeof(uint32_t)); + offset += 4; + memcpy(outbuffer + offset, this->target_frame, length_target_frame); + offset += length_target_frame; + uint32_t length_source_frame = strlen(this->source_frame); + memcpy(outbuffer + offset, &length_source_frame, sizeof(uint32_t)); + offset += 4; + memcpy(outbuffer + offset, this->source_frame, length_source_frame); + offset += length_source_frame; + *(outbuffer + offset + 0) = (this->source_time.sec >> (8 * 0)) & 0xFF; + *(outbuffer + offset + 1) = (this->source_time.sec >> (8 * 1)) & 0xFF; + *(outbuffer + offset + 2) = (this->source_time.sec >> (8 * 2)) & 0xFF; + *(outbuffer + offset + 3) = (this->source_time.sec >> (8 * 3)) & 0xFF; + offset += sizeof(this->source_time.sec); + *(outbuffer + offset + 0) = (this->source_time.nsec >> (8 * 0)) & 0xFF; + *(outbuffer + offset + 1) = (this->source_time.nsec >> (8 * 1)) & 0xFF; + *(outbuffer + offset + 2) = (this->source_time.nsec >> (8 * 2)) & 0xFF; + *(outbuffer + offset + 3) = (this->source_time.nsec >> (8 * 3)) & 0xFF; + offset += sizeof(this->source_time.nsec); + *(outbuffer + offset + 0) = (this->timeout.sec >> (8 * 0)) & 0xFF; + *(outbuffer + offset + 1) = (this->timeout.sec >> (8 * 1)) & 0xFF; + *(outbuffer + offset + 2) = (this->timeout.sec >> (8 * 2)) & 0xFF; + *(outbuffer + offset + 3) = (this->timeout.sec >> (8 * 3)) & 0xFF; + offset += sizeof(this->timeout.sec); + *(outbuffer + offset + 0) = (this->timeout.nsec >> (8 * 0)) & 0xFF; + *(outbuffer + offset + 1) = (this->timeout.nsec >> (8 * 1)) & 0xFF; + *(outbuffer + offset + 2) = (this->timeout.nsec >> (8 * 2)) & 0xFF; + *(outbuffer + offset + 3) = (this->timeout.nsec >> (8 * 3)) & 0xFF; + offset += sizeof(this->timeout.nsec); + *(outbuffer + offset + 0) = (this->target_time.sec >> (8 * 0)) & 0xFF; + *(outbuffer + offset + 1) = (this->target_time.sec >> (8 * 1)) & 0xFF; + *(outbuffer + offset + 2) = (this->target_time.sec >> (8 * 2)) & 0xFF; + *(outbuffer + offset + 3) = (this->target_time.sec >> (8 * 3)) & 0xFF; + offset += sizeof(this->target_time.sec); + *(outbuffer + offset + 0) = (this->target_time.nsec >> (8 * 0)) & 0xFF; + *(outbuffer + offset + 1) = (this->target_time.nsec >> (8 * 1)) & 0xFF; + *(outbuffer + offset + 2) = (this->target_time.nsec >> (8 * 2)) & 0xFF; + *(outbuffer + offset + 3) = (this->target_time.nsec >> (8 * 3)) & 0xFF; + offset += sizeof(this->target_time.nsec); + uint32_t length_fixed_frame = strlen(this->fixed_frame); + memcpy(outbuffer + offset, &length_fixed_frame, sizeof(uint32_t)); + offset += 4; + memcpy(outbuffer + offset, this->fixed_frame, length_fixed_frame); + offset += length_fixed_frame; + union { + bool real; + uint8_t base; + } u_advanced; + u_advanced.real = this->advanced; + *(outbuffer + offset + 0) = (u_advanced.base >> (8 * 0)) & 0xFF; + offset += sizeof(this->advanced); + return offset; + } + + virtual int deserialize(unsigned char *inbuffer) + { + int offset = 0; + uint32_t length_target_frame; + memcpy(&length_target_frame, (inbuffer + offset), sizeof(uint32_t)); + offset += 4; + for(unsigned int k= offset; k< offset+length_target_frame; ++k){ + inbuffer[k-1]=inbuffer[k]; + } + inbuffer[offset+length_target_frame-1]=0; + this->target_frame = (char *)(inbuffer + offset-1); + offset += length_target_frame; + uint32_t length_source_frame; + memcpy(&length_source_frame, (inbuffer + offset), sizeof(uint32_t)); + offset += 4; + for(unsigned int k= offset; k< offset+length_source_frame; ++k){ + inbuffer[k-1]=inbuffer[k]; + } + inbuffer[offset+length_source_frame-1]=0; + this->source_frame = (char *)(inbuffer + offset-1); + offset += length_source_frame; + this->source_time.sec = ((uint32_t) (*(inbuffer + offset))); + this->source_time.sec |= ((uint32_t) (*(inbuffer + offset + 1))) << (8 * 1); + this->source_time.sec |= ((uint32_t) (*(inbuffer + offset + 2))) << (8 * 2); + this->source_time.sec |= ((uint32_t) (*(inbuffer + offset + 3))) << (8 * 3); + offset += sizeof(this->source_time.sec); + this->source_time.nsec = ((uint32_t) (*(inbuffer + offset))); + this->source_time.nsec |= ((uint32_t) (*(inbuffer + offset + 1))) << (8 * 1); + this->source_time.nsec |= ((uint32_t) (*(inbuffer + offset + 2))) << (8 * 2); + this->source_time.nsec |= ((uint32_t) (*(inbuffer + offset + 3))) << (8 * 3); + offset += sizeof(this->source_time.nsec); + this->timeout.sec = ((uint32_t) (*(inbuffer + offset))); + this->timeout.sec |= ((uint32_t) (*(inbuffer + offset + 1))) << (8 * 1); + this->timeout.sec |= ((uint32_t) (*(inbuffer + offset + 2))) << (8 * 2); + this->timeout.sec |= ((uint32_t) (*(inbuffer + offset + 3))) << (8 * 3); + offset += sizeof(this->timeout.sec); + this->timeout.nsec = ((uint32_t) (*(inbuffer + offset))); + this->timeout.nsec |= ((uint32_t) (*(inbuffer + offset + 1))) << (8 * 1); + this->timeout.nsec |= ((uint32_t) (*(inbuffer + offset + 2))) << (8 * 2); + this->timeout.nsec |= ((uint32_t) (*(inbuffer + offset + 3))) << (8 * 3); + offset += sizeof(this->timeout.nsec); + this->target_time.sec = ((uint32_t) (*(inbuffer + offset))); + this->target_time.sec |= ((uint32_t) (*(inbuffer + offset + 1))) << (8 * 1); + this->target_time.sec |= ((uint32_t) (*(inbuffer + offset + 2))) << (8 * 2); + this->target_time.sec |= ((uint32_t) (*(inbuffer + offset + 3))) << (8 * 3); + offset += sizeof(this->target_time.sec); + this->target_time.nsec = ((uint32_t) (*(inbuffer + offset))); + this->target_time.nsec |= ((uint32_t) (*(inbuffer + offset + 1))) << (8 * 1); + this->target_time.nsec |= ((uint32_t) (*(inbuffer + offset + 2))) << (8 * 2); + this->target_time.nsec |= ((uint32_t) (*(inbuffer + offset + 3))) << (8 * 3); + offset += sizeof(this->target_time.nsec); + uint32_t length_fixed_frame; + memcpy(&length_fixed_frame, (inbuffer + offset), sizeof(uint32_t)); + offset += 4; + for(unsigned int k= offset; k< offset+length_fixed_frame; ++k){ + inbuffer[k-1]=inbuffer[k]; + } + inbuffer[offset+length_fixed_frame-1]=0; + this->fixed_frame = (char *)(inbuffer + offset-1); + offset += length_fixed_frame; + union { + bool real; + uint8_t base; + } u_advanced; + u_advanced.base = 0; + u_advanced.base |= ((uint8_t) (*(inbuffer + offset + 0))) << (8 * 0); + this->advanced = u_advanced.real; + offset += sizeof(this->advanced); + return offset; + } + + const char * getType(){ return "tf2_msgs/LookupTransformGoal"; }; + const char * getMD5(){ return "35e3720468131d675a18bb6f3e5f22f8"; }; + + }; + +} +#endif \ No newline at end of file diff --git a/turtlebot3_core/libraries/ros_lib/tf2_msgs/LookupTransformResult.h b/turtlebot3_core/libraries/ros_lib/tf2_msgs/LookupTransformResult.h new file mode 100644 index 00000000..6dbf6f47 --- /dev/null +++ b/turtlebot3_core/libraries/ros_lib/tf2_msgs/LookupTransformResult.h @@ -0,0 +1,48 @@ +#ifndef _ROS_tf2_msgs_LookupTransformResult_h +#define _ROS_tf2_msgs_LookupTransformResult_h + +#include +#include +#include +#include "ros/msg.h" +#include "geometry_msgs/TransformStamped.h" +#include "tf2_msgs/TF2Error.h" + +namespace tf2_msgs +{ + + class LookupTransformResult : public ros::Msg + { + public: + geometry_msgs::TransformStamped transform; + tf2_msgs::TF2Error error; + + LookupTransformResult(): + transform(), + error() + { + } + + virtual int serialize(unsigned char *outbuffer) const + { + int offset = 0; + offset += this->transform.serialize(outbuffer + offset); + offset += this->error.serialize(outbuffer + offset); + return offset; + } + + virtual int deserialize(unsigned char *inbuffer) + { + int offset = 0; + offset += this->transform.deserialize(inbuffer + offset); + offset += this->error.deserialize(inbuffer + offset); + return offset; + } + + const char * getType(){ return "tf2_msgs/LookupTransformResult"; }; + const char * getMD5(){ return "3fe5db6a19ca9cfb675418c5ad875c36"; }; + + }; + +} +#endif \ No newline at end of file diff --git a/turtlebot3_core/libraries/ros_lib/tf2_msgs/TF2Error.h b/turtlebot3_core/libraries/ros_lib/tf2_msgs/TF2Error.h new file mode 100644 index 00000000..0fc1a7fb --- /dev/null +++ b/turtlebot3_core/libraries/ros_lib/tf2_msgs/TF2Error.h @@ -0,0 +1,67 @@ +#ifndef _ROS_tf2_msgs_TF2Error_h +#define _ROS_tf2_msgs_TF2Error_h + +#include +#include +#include +#include "ros/msg.h" + +namespace tf2_msgs +{ + + class TF2Error : public ros::Msg + { + public: + uint8_t error; + const char* error_string; + enum { NO_ERROR = 0 }; + enum { LOOKUP_ERROR = 1 }; + enum { CONNECTIVITY_ERROR = 2 }; + enum { EXTRAPOLATION_ERROR = 3 }; + enum { INVALID_ARGUMENT_ERROR = 4 }; + enum { TIMEOUT_ERROR = 5 }; + enum { TRANSFORM_ERROR = 6 }; + + TF2Error(): + error(0), + error_string("") + { + } + + virtual int serialize(unsigned char *outbuffer) const + { + int offset = 0; + *(outbuffer + offset + 0) = (this->error >> (8 * 0)) & 0xFF; + offset += sizeof(this->error); + uint32_t length_error_string = strlen(this->error_string); + memcpy(outbuffer + offset, &length_error_string, sizeof(uint32_t)); + offset += 4; + memcpy(outbuffer + offset, this->error_string, length_error_string); + offset += length_error_string; + return offset; + } + + virtual int deserialize(unsigned char *inbuffer) + { + int offset = 0; + this->error = ((uint8_t) (*(inbuffer + offset))); + offset += sizeof(this->error); + uint32_t length_error_string; + memcpy(&length_error_string, (inbuffer + offset), sizeof(uint32_t)); + offset += 4; + for(unsigned int k= offset; k< offset+length_error_string; ++k){ + inbuffer[k-1]=inbuffer[k]; + } + inbuffer[offset+length_error_string-1]=0; + this->error_string = (char *)(inbuffer + offset-1); + offset += length_error_string; + return offset; + } + + const char * getType(){ return "tf2_msgs/TF2Error"; }; + const char * getMD5(){ return "bc6848fd6fd750c92e38575618a4917d"; }; + + }; + +} +#endif \ No newline at end of file diff --git a/turtlebot3_core/libraries/ros_lib/tf2_msgs/TFMessage.h b/turtlebot3_core/libraries/ros_lib/tf2_msgs/TFMessage.h new file mode 100644 index 00000000..66c4eaae --- /dev/null +++ b/turtlebot3_core/libraries/ros_lib/tf2_msgs/TFMessage.h @@ -0,0 +1,63 @@ +#ifndef _ROS_tf2_msgs_TFMessage_h +#define _ROS_tf2_msgs_TFMessage_h + +#include +#include +#include +#include "ros/msg.h" +#include "geometry_msgs/TransformStamped.h" + +namespace tf2_msgs +{ + + class TFMessage : public ros::Msg + { + public: + uint32_t transforms_length; + geometry_msgs::TransformStamped st_transforms; + geometry_msgs::TransformStamped * transforms; + + TFMessage(): + transforms_length(0), transforms(NULL) + { + } + + virtual int serialize(unsigned char *outbuffer) const + { + int offset = 0; + *(outbuffer + offset + 0) = (this->transforms_length >> (8 * 0)) & 0xFF; + *(outbuffer + offset + 1) = (this->transforms_length >> (8 * 1)) & 0xFF; + *(outbuffer + offset + 2) = (this->transforms_length >> (8 * 2)) & 0xFF; + *(outbuffer + offset + 3) = (this->transforms_length >> (8 * 3)) & 0xFF; + offset += sizeof(this->transforms_length); + for( uint32_t i = 0; i < transforms_length; i++){ + offset += this->transforms[i].serialize(outbuffer + offset); + } + return offset; + } + + virtual int deserialize(unsigned char *inbuffer) + { + int offset = 0; + uint32_t transforms_lengthT = ((uint32_t) (*(inbuffer + offset))); + transforms_lengthT |= ((uint32_t) (*(inbuffer + offset + 1))) << (8 * 1); + transforms_lengthT |= ((uint32_t) (*(inbuffer + offset + 2))) << (8 * 2); + transforms_lengthT |= ((uint32_t) (*(inbuffer + offset + 3))) << (8 * 3); + offset += sizeof(this->transforms_length); + if(transforms_lengthT > transforms_length) + this->transforms = (geometry_msgs::TransformStamped*)realloc(this->transforms, transforms_lengthT * sizeof(geometry_msgs::TransformStamped)); + transforms_length = transforms_lengthT; + for( uint32_t i = 0; i < transforms_length; i++){ + offset += this->st_transforms.deserialize(inbuffer + offset); + memcpy( &(this->transforms[i]), &(this->st_transforms), sizeof(geometry_msgs::TransformStamped)); + } + return offset; + } + + const char * getType(){ return "tf2_msgs/TFMessage"; }; + const char * getMD5(){ return "94810edda583a504dfda3829e70d7eec"; }; + + }; + +} +#endif \ No newline at end of file diff --git a/turtlebot3_core/libraries/ros_lib/time.cpp b/turtlebot3_core/libraries/ros_lib/time.cpp new file mode 100644 index 00000000..93411964 --- /dev/null +++ b/turtlebot3_core/libraries/ros_lib/time.cpp @@ -0,0 +1,68 @@ +/* + * Software License Agreement (BSD License) + * + * Copyright (c) 2011, Willow Garage, Inc. + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above + * copyright notice, this list of conditions and the following + * disclaimer in the documentation and/or other materials provided + * with the distribution. + * * Neither the name of Willow Garage, Inc. nor the names of its + * contributors may be used to endorse or promote prducts derived + * from this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; + * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + */ + +#include "ros/time.h" + +namespace ros +{ + void normalizeSecNSec(uint32_t& sec, uint32_t& nsec){ + uint32_t nsec_part= nsec % 1000000000UL; + uint32_t sec_part = nsec / 1000000000UL; + sec += sec_part; + nsec = nsec_part; + } + + Time& Time::fromNSec(int32_t t) + { + sec = t / 1000000000; + nsec = t % 1000000000; + normalizeSecNSec(sec, nsec); + return *this; + } + + Time& Time::operator +=(const Duration &rhs) + { + sec += rhs.sec; + nsec += rhs.nsec; + normalizeSecNSec(sec, nsec); + return *this; + } + + Time& Time::operator -=(const Duration &rhs){ + sec += -rhs.sec; + nsec += -rhs.nsec; + normalizeSecNSec(sec, nsec); + return *this; + } +} diff --git a/turtlebot3_core/libraries/ros_lib/turtlebot3_msgs/DynamixelFeedback.h b/turtlebot3_core/libraries/ros_lib/turtlebot3_msgs/DynamixelFeedback.h new file mode 100644 index 00000000..fdc12c7a --- /dev/null +++ b/turtlebot3_core/libraries/ros_lib/turtlebot3_msgs/DynamixelFeedback.h @@ -0,0 +1,130 @@ +#ifndef _ROS_turtlebot3_msgs_DynamixelFeedback_h +#define _ROS_turtlebot3_msgs_DynamixelFeedback_h + +#include +#include +#include +#include "ros/msg.h" + +namespace turtlebot3_msgs +{ + + class DynamixelFeedback : public ros::Msg + { + public: + int32_t position1; + int32_t position2; + int32_t realtime_tick1; + int32_t realtime_tick2; + + DynamixelFeedback(): + position1(0), + position2(0), + realtime_tick1(0), + realtime_tick2(0) + { + } + + virtual int serialize(unsigned char *outbuffer) const + { + int offset = 0; + union { + int32_t real; + uint32_t base; + } u_position1; + u_position1.real = this->position1; + *(outbuffer + offset + 0) = (u_position1.base >> (8 * 0)) & 0xFF; + *(outbuffer + offset + 1) = (u_position1.base >> (8 * 1)) & 0xFF; + *(outbuffer + offset + 2) = (u_position1.base >> (8 * 2)) & 0xFF; + *(outbuffer + offset + 3) = (u_position1.base >> (8 * 3)) & 0xFF; + offset += sizeof(this->position1); + union { + int32_t real; + uint32_t base; + } u_position2; + u_position2.real = this->position2; + *(outbuffer + offset + 0) = (u_position2.base >> (8 * 0)) & 0xFF; + *(outbuffer + offset + 1) = (u_position2.base >> (8 * 1)) & 0xFF; + *(outbuffer + offset + 2) = (u_position2.base >> (8 * 2)) & 0xFF; + *(outbuffer + offset + 3) = (u_position2.base >> (8 * 3)) & 0xFF; + offset += sizeof(this->position2); + union { + int32_t real; + uint32_t base; + } u_realtime_tick1; + u_realtime_tick1.real = this->realtime_tick1; + *(outbuffer + offset + 0) = (u_realtime_tick1.base >> (8 * 0)) & 0xFF; + *(outbuffer + offset + 1) = (u_realtime_tick1.base >> (8 * 1)) & 0xFF; + *(outbuffer + offset + 2) = (u_realtime_tick1.base >> (8 * 2)) & 0xFF; + *(outbuffer + offset + 3) = (u_realtime_tick1.base >> (8 * 3)) & 0xFF; + offset += sizeof(this->realtime_tick1); + union { + int32_t real; + uint32_t base; + } u_realtime_tick2; + u_realtime_tick2.real = this->realtime_tick2; + *(outbuffer + offset + 0) = (u_realtime_tick2.base >> (8 * 0)) & 0xFF; + *(outbuffer + offset + 1) = (u_realtime_tick2.base >> (8 * 1)) & 0xFF; + *(outbuffer + offset + 2) = (u_realtime_tick2.base >> (8 * 2)) & 0xFF; + *(outbuffer + offset + 3) = (u_realtime_tick2.base >> (8 * 3)) & 0xFF; + offset += sizeof(this->realtime_tick2); + return offset; + } + + virtual int deserialize(unsigned char *inbuffer) + { + int offset = 0; + union { + int32_t real; + uint32_t base; + } u_position1; + u_position1.base = 0; + u_position1.base |= ((uint32_t) (*(inbuffer + offset + 0))) << (8 * 0); + u_position1.base |= ((uint32_t) (*(inbuffer + offset + 1))) << (8 * 1); + u_position1.base |= ((uint32_t) (*(inbuffer + offset + 2))) << (8 * 2); + u_position1.base |= ((uint32_t) (*(inbuffer + offset + 3))) << (8 * 3); + this->position1 = u_position1.real; + offset += sizeof(this->position1); + union { + int32_t real; + uint32_t base; + } u_position2; + u_position2.base = 0; + u_position2.base |= ((uint32_t) (*(inbuffer + offset + 0))) << (8 * 0); + u_position2.base |= ((uint32_t) (*(inbuffer + offset + 1))) << (8 * 1); + u_position2.base |= ((uint32_t) (*(inbuffer + offset + 2))) << (8 * 2); + u_position2.base |= ((uint32_t) (*(inbuffer + offset + 3))) << (8 * 3); + this->position2 = u_position2.real; + offset += sizeof(this->position2); + union { + int32_t real; + uint32_t base; + } u_realtime_tick1; + u_realtime_tick1.base = 0; + u_realtime_tick1.base |= ((uint32_t) (*(inbuffer + offset + 0))) << (8 * 0); + u_realtime_tick1.base |= ((uint32_t) (*(inbuffer + offset + 1))) << (8 * 1); + u_realtime_tick1.base |= ((uint32_t) (*(inbuffer + offset + 2))) << (8 * 2); + u_realtime_tick1.base |= ((uint32_t) (*(inbuffer + offset + 3))) << (8 * 3); + this->realtime_tick1 = u_realtime_tick1.real; + offset += sizeof(this->realtime_tick1); + union { + int32_t real; + uint32_t base; + } u_realtime_tick2; + u_realtime_tick2.base = 0; + u_realtime_tick2.base |= ((uint32_t) (*(inbuffer + offset + 0))) << (8 * 0); + u_realtime_tick2.base |= ((uint32_t) (*(inbuffer + offset + 1))) << (8 * 1); + u_realtime_tick2.base |= ((uint32_t) (*(inbuffer + offset + 2))) << (8 * 2); + u_realtime_tick2.base |= ((uint32_t) (*(inbuffer + offset + 3))) << (8 * 3); + this->realtime_tick2 = u_realtime_tick2.real; + offset += sizeof(this->realtime_tick2); + return offset; + } + + const char * getType(){ return "turtlebot3_msgs/DynamixelFeedback"; }; + const char * getMD5(){ return "70a080ba72122dcbd0aa4ba4de407e58"; }; + + }; + +} +#endif \ No newline at end of file diff --git a/turtlebot3_core/libraries/ros_lib/turtlebot3_msgs/MotorPower.h b/turtlebot3_core/libraries/ros_lib/turtlebot3_msgs/MotorPower.h new file mode 100644 index 00000000..10bb943c --- /dev/null +++ b/turtlebot3_core/libraries/ros_lib/turtlebot3_msgs/MotorPower.h @@ -0,0 +1,46 @@ +#ifndef _ROS_turtlebot3_msgs_MotorPower_h +#define _ROS_turtlebot3_msgs_MotorPower_h + +#include +#include +#include +#include "ros/msg.h" + +namespace turtlebot3_msgs +{ + + class MotorPower : public ros::Msg + { + public: + uint8_t state; + enum { OFF = 0 }; + enum { ON = 1 }; + + MotorPower(): + state(0) + { + } + + virtual int serialize(unsigned char *outbuffer) const + { + int offset = 0; + *(outbuffer + offset + 0) = (this->state >> (8 * 0)) & 0xFF; + offset += sizeof(this->state); + return offset; + } + + virtual int deserialize(unsigned char *inbuffer) + { + int offset = 0; + this->state = ((uint8_t) (*(inbuffer + offset))); + offset += sizeof(this->state); + return offset; + } + + const char * getType(){ return "turtlebot3_msgs/MotorPower"; }; + const char * getMD5(){ return "8f77c0161e0f2021493136bb5f3f0e51"; }; + + }; + +} +#endif \ No newline at end of file diff --git a/turtlebot3_core/libraries/ros_lib/turtlebot3_msgs/SensorState.h b/turtlebot3_core/libraries/ros_lib/turtlebot3_msgs/SensorState.h new file mode 100644 index 00000000..98e88937 --- /dev/null +++ b/turtlebot3_core/libraries/ros_lib/turtlebot3_msgs/SensorState.h @@ -0,0 +1,127 @@ +#ifndef _ROS_turtlebot3_msgs_SensorState_h +#define _ROS_turtlebot3_msgs_SensorState_h + +#include +#include +#include +#include "ros/msg.h" + +namespace turtlebot3_msgs +{ + + class SensorState : public ros::Msg + { + public: + uint16_t time_stamp; + uint8_t bumper; + uint8_t cliff; + uint8_t button; + int32_t left_encoder; + int32_t right_encoder; + uint8_t battery; + enum { BUMPER_RIGHT = 1 }; + enum { BUMPER_CENTER = 2 }; + enum { BUMPER_LEFT = 4 }; + enum { CLIFF_RIGHT = 1 }; + enum { CLIFF_CENTER = 2 }; + enum { CLIFF_LEFT = 4 }; + enum { BUTTON0 = 1 }; + enum { BUTTON1 = 2 }; + enum { BUTTON2 = 4 }; + enum { ERROR_LEFT_MOTOR = 1 }; + enum { ERROR_RIGHT_MOTOR = 2 }; + + SensorState(): + time_stamp(0), + bumper(0), + cliff(0), + button(0), + left_encoder(0), + right_encoder(0), + battery(0) + { + } + + virtual int serialize(unsigned char *outbuffer) const + { + int offset = 0; + *(outbuffer + offset + 0) = (this->time_stamp >> (8 * 0)) & 0xFF; + *(outbuffer + offset + 1) = (this->time_stamp >> (8 * 1)) & 0xFF; + offset += sizeof(this->time_stamp); + *(outbuffer + offset + 0) = (this->bumper >> (8 * 0)) & 0xFF; + offset += sizeof(this->bumper); + *(outbuffer + offset + 0) = (this->cliff >> (8 * 0)) & 0xFF; + offset += sizeof(this->cliff); + *(outbuffer + offset + 0) = (this->button >> (8 * 0)) & 0xFF; + offset += sizeof(this->button); + union { + int32_t real; + uint32_t base; + } u_left_encoder; + u_left_encoder.real = this->left_encoder; + *(outbuffer + offset + 0) = (u_left_encoder.base >> (8 * 0)) & 0xFF; + *(outbuffer + offset + 1) = (u_left_encoder.base >> (8 * 1)) & 0xFF; + *(outbuffer + offset + 2) = (u_left_encoder.base >> (8 * 2)) & 0xFF; + *(outbuffer + offset + 3) = (u_left_encoder.base >> (8 * 3)) & 0xFF; + offset += sizeof(this->left_encoder); + union { + int32_t real; + uint32_t base; + } u_right_encoder; + u_right_encoder.real = this->right_encoder; + *(outbuffer + offset + 0) = (u_right_encoder.base >> (8 * 0)) & 0xFF; + *(outbuffer + offset + 1) = (u_right_encoder.base >> (8 * 1)) & 0xFF; + *(outbuffer + offset + 2) = (u_right_encoder.base >> (8 * 2)) & 0xFF; + *(outbuffer + offset + 3) = (u_right_encoder.base >> (8 * 3)) & 0xFF; + offset += sizeof(this->right_encoder); + *(outbuffer + offset + 0) = (this->battery >> (8 * 0)) & 0xFF; + offset += sizeof(this->battery); + return offset; + } + + virtual int deserialize(unsigned char *inbuffer) + { + int offset = 0; + this->time_stamp = ((uint16_t) (*(inbuffer + offset))); + this->time_stamp |= ((uint16_t) (*(inbuffer + offset + 1))) << (8 * 1); + offset += sizeof(this->time_stamp); + this->bumper = ((uint8_t) (*(inbuffer + offset))); + offset += sizeof(this->bumper); + this->cliff = ((uint8_t) (*(inbuffer + offset))); + offset += sizeof(this->cliff); + this->button = ((uint8_t) (*(inbuffer + offset))); + offset += sizeof(this->button); + union { + int32_t real; + uint32_t base; + } u_left_encoder; + u_left_encoder.base = 0; + u_left_encoder.base |= ((uint32_t) (*(inbuffer + offset + 0))) << (8 * 0); + u_left_encoder.base |= ((uint32_t) (*(inbuffer + offset + 1))) << (8 * 1); + u_left_encoder.base |= ((uint32_t) (*(inbuffer + offset + 2))) << (8 * 2); + u_left_encoder.base |= ((uint32_t) (*(inbuffer + offset + 3))) << (8 * 3); + this->left_encoder = u_left_encoder.real; + offset += sizeof(this->left_encoder); + union { + int32_t real; + uint32_t base; + } u_right_encoder; + u_right_encoder.base = 0; + u_right_encoder.base |= ((uint32_t) (*(inbuffer + offset + 0))) << (8 * 0); + u_right_encoder.base |= ((uint32_t) (*(inbuffer + offset + 1))) << (8 * 1); + u_right_encoder.base |= ((uint32_t) (*(inbuffer + offset + 2))) << (8 * 2); + u_right_encoder.base |= ((uint32_t) (*(inbuffer + offset + 3))) << (8 * 3); + this->right_encoder = u_right_encoder.real; + offset += sizeof(this->right_encoder); + this->battery = ((uint8_t) (*(inbuffer + offset))); + offset += sizeof(this->battery); + return offset; + } + + const char * getType(){ return "turtlebot3_msgs/SensorState"; }; + const char * getMD5(){ return "ed3b0a4d8120279d32e68ec3399b1f9e"; }; + + }; + +} +#endif \ No newline at end of file diff --git a/turtlebot3_core/libraries/ros_lib/turtlebot3_msgs/VersionInfo.h b/turtlebot3_core/libraries/ros_lib/turtlebot3_msgs/VersionInfo.h new file mode 100644 index 00000000..534f6318 --- /dev/null +++ b/turtlebot3_core/libraries/ros_lib/turtlebot3_msgs/VersionInfo.h @@ -0,0 +1,86 @@ +#ifndef _ROS_turtlebot3_msgs_VersionInfo_h +#define _ROS_turtlebot3_msgs_VersionInfo_h + +#include +#include +#include +#include "ros/msg.h" + +namespace turtlebot3_msgs +{ + + class VersionInfo : public ros::Msg + { + public: + const char* hardware; + const char* firmware; + const char* software; + + VersionInfo(): + hardware(""), + firmware(""), + software("") + { + } + + virtual int serialize(unsigned char *outbuffer) const + { + int offset = 0; + uint32_t length_hardware = strlen(this->hardware); + memcpy(outbuffer + offset, &length_hardware, sizeof(uint32_t)); + offset += 4; + memcpy(outbuffer + offset, this->hardware, length_hardware); + offset += length_hardware; + uint32_t length_firmware = strlen(this->firmware); + memcpy(outbuffer + offset, &length_firmware, sizeof(uint32_t)); + offset += 4; + memcpy(outbuffer + offset, this->firmware, length_firmware); + offset += length_firmware; + uint32_t length_software = strlen(this->software); + memcpy(outbuffer + offset, &length_software, sizeof(uint32_t)); + offset += 4; + memcpy(outbuffer + offset, this->software, length_software); + offset += length_software; + return offset; + } + + virtual int deserialize(unsigned char *inbuffer) + { + int offset = 0; + uint32_t length_hardware; + memcpy(&length_hardware, (inbuffer + offset), sizeof(uint32_t)); + offset += 4; + for(unsigned int k= offset; k< offset+length_hardware; ++k){ + inbuffer[k-1]=inbuffer[k]; + } + inbuffer[offset+length_hardware-1]=0; + this->hardware = (char *)(inbuffer + offset-1); + offset += length_hardware; + uint32_t length_firmware; + memcpy(&length_firmware, (inbuffer + offset), sizeof(uint32_t)); + offset += 4; + for(unsigned int k= offset; k< offset+length_firmware; ++k){ + inbuffer[k-1]=inbuffer[k]; + } + inbuffer[offset+length_firmware-1]=0; + this->firmware = (char *)(inbuffer + offset-1); + offset += length_firmware; + uint32_t length_software; + memcpy(&length_software, (inbuffer + offset), sizeof(uint32_t)); + offset += 4; + for(unsigned int k= offset; k< offset+length_software; ++k){ + inbuffer[k-1]=inbuffer[k]; + } + inbuffer[offset+length_software-1]=0; + this->software = (char *)(inbuffer + offset-1); + offset += length_software; + return offset; + } + + const char * getType(){ return "turtlebot3_msgs/VersionInfo"; }; + const char * getMD5(){ return "43e0361461af2970a33107409403ef3c"; }; + + }; + +} +#endif \ No newline at end of file diff --git a/turtlebot3_core/turtlebot3_core.ino b/turtlebot3_core/turtlebot3_core.ino new file mode 100644 index 00000000..08b169ad --- /dev/null +++ b/turtlebot3_core/turtlebot3_core.ino @@ -0,0 +1,556 @@ +/******************************************************************************* +* Copyright 2016 ROBOTIS CO., LTD. +* +* Licensed under the Apache License, Version 2.0 (the "License"); +* you may not use this file except in compliance with the License. +* You may obtain a copy of the License at +* +* http://www.apache.org/licenses/LICENSE-2.0 +* +* Unless required by applicable law or agreed to in writing, software +* distributed under the License is distributed on an "AS IS" BASIS, +* WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +* See the License for the specific language governing permissions and +* limitations under the License. +*******************************************************************************/ + +/* Authors: Yoonseok Pyo, Leon Jung, Darby Lim */ + +#include "turtlebot3_core_config.h" + +/******************************************************************************* +* ROS NodeHandle +*******************************************************************************/ +ros::NodeHandle nh; + +/******************************************************************************* +* Subscriber +*******************************************************************************/ +void cmd_vel_callback(const geometry_msgs::Twist& cmd_vel_msg); +ros::Subscriber cmd_vel_sub("cmd_vel", cmd_vel_callback); + +/******************************************************************************* +* Publisher +*******************************************************************************/ +// Bumpers, cliffs, buttons, encoders, battery of Turtlebot3 +turtlebot3_msgs::SensorState sensor_state_msg; +ros::Publisher sensor_state_pub("sensor_state", &sensor_state_msg); + +// IMU of Turtlebot3 +sensor_msgs::Imu imu_msg; +ros::Publisher imu_pub("imu", &imu_msg); + +// Command velocity of Turtlebot3 using RC100 remocon +geometry_msgs::Twist cmd_vel_rc100_msg; +ros::Publisher cmd_vel_rc100_pub("cmd_vel_rc100", &cmd_vel_rc100_msg); + +nav_msgs::Odometry odom; +ros::Publisher odom_pub("odom", &odom); + +sensor_msgs::JointState joint_states; +ros::Publisher joint_states_pub("joint_states", &joint_states); + +/******************************************************************************* +* Transform Broadcaster +*******************************************************************************/ +// TF of Turtlebot3 +geometry_msgs::TransformStamped tfs_msg; +geometry_msgs::TransformStamped odom_tf; +tf::TransformBroadcaster tfbroadcaster; + +/******************************************************************************* +* HardwareTimer for imu, encoder, cmd_vel of Turtlebot3 +*******************************************************************************/ +HardwareTimer timer_sensors_state(TIMER_CH1); +HardwareTimer timer_imu(TIMER_CH2); +// HardwareTimer timer_cmd_velocity(TIMER_CH3); +// HardwareTimer timer_control_speed(TIMER_CH4); + +/******************************************************************************* +* SoftwareTimer for * of Turtlebot3 +*******************************************************************************/ +static uint32_t tTime[3]; + +/******************************************************************************* +* Declaration for motor +*******************************************************************************/ +Turtlebot3MotorDriver motor_driver; +double wheel_speed_cmd[2]; +bool init_encoder_[2] = {false, false}; +int32_t last_diff_tick_[2]; +int32_t last_tick_[2]; +double last_rad_[2]; +double last_velocity_[2]; +double goal_linear_velocity = 0.0; +double goal_angular_velocity = 0.0; + +/******************************************************************************* +* Declaration for IMU +*******************************************************************************/ +cIMU imu; + +/******************************************************************************* +* Declaration for Remocon (RC100) +*******************************************************************************/ +RC100 remote_controller; +int received_data = 0; +int vel[4] = {DXL_LEFT_ID, 0, DXL_RIGHT_ID, 0}; +int const_vel = 150; +double const_cmd_vel = 0.2; +bool rc100_remocon_mode = false; + +/******************************************************************************* +* Declaration for SLAM and navigation +*******************************************************************************/ +float odom_pose[3]; +float odom_vel[3]; +double pose_cov[36]; + +unsigned long prev_update_time; +double cmd_vel_timeout = 1.0; //sec + +char *joint_states_name[] = {"wheel_left_joint", "wheel_right_joint"}; +float joint_states_pos[2] = {0.0, 0.0}; +float joint_states_vel[2] = {0.0, 0.0}; +float joint_states_eff[2] = {0.0, 0.0}; + +/******************************************************************************* +* Setup function +*******************************************************************************/ +void setup() +{ + nh.initNode(); + nh.getHardware()->setBaud(1000000); + nh.subscribe(cmd_vel_sub); + nh.advertise(sensor_state_pub); + nh.advertise(imu_pub); + nh.advertise(cmd_vel_rc100_pub); + nh.advertise(odom_pub); + nh.advertise(joint_states_pub); + tfbroadcaster.init(nh); + + nh.loginfo("Connected to OpenCR..."); + + // for Dynamixel + motor_driver.init(); + + timer_sensors_state.pause(); + timer_sensors_state.setPeriod(1000000/SENSOR_STATE_PUBLISH_PERIOD); // 0.1 sec + timer_sensors_state.attachInterrupt(publish_sensor_state_msg); + timer_sensors_state.refresh(); + timer_sensors_state.resume(); + + // for IMU + imu.begin(); + + timer_imu.pause(); + timer_imu.setPeriod(1000000/IMU_PUBLISH_PERIOD); // 0.1 sec + timer_imu.attachInterrupt(publish_imu_msg); + timer_imu.refresh(); + timer_imu.resume(); + + // for cmd_vel + remote_controller.begin(1); //57600bps for RC100b + + // timer_cmd_velocity.pause(); + // timer_cmd_velocity.setPeriod(CMD_VELOCITY_PUBLISH_PERIOD * 1000000); // usec + // timer_cmd_velocity.attachInterrupt(interrupt_cmd_vel_rc100_pub); + // timer_cmd_velocity.refresh(); + // timer_cmd_velocity.resume(); + + cmd_vel_rc100_msg.linear.x = 0.0; + cmd_vel_rc100_msg.angular.z = 0.0; + + float pcov[36] = { 0.1, 0, 0, 0, 0, 0, + 0, 0.1, 0, 0, 0, 0, + 0, 0, 1e6, 0, 0, 0, + 0, 0, 0, 1e6, 0, 0, + 0, 0, 0, 0, 1e6, 0, + 0, 0, 0, 0, 0, 0.2}; + // memcpy(&(this->odom.pose.covariance),pcov,sizeof(double)*36); + // memcpy(&(this->odom.twist.covariance),pcov,sizeof(double)*36); + + odom_pose[0] = 0.0; + odom_pose[1] = 0.0; + odom_pose[2] = 0.0; + + joint_states.header.frame_id = "base_footprint"; + + joint_states.name_length = 2; + joint_states.position_length = 2; + joint_states.velocity_length = 2; + joint_states.effort_length = 2; + + joint_states.name = joint_states_name; + joint_states.position = joint_states_pos; + joint_states.velocity = joint_states_vel; + joint_states.effort = joint_states_eff; + + prev_update_time = millis(); + + // for speed controller + // timer_control_speed.pause(); + // timer_control_speed.setPeriod(SPEED_CONTROL_PERIOD * 1000000); // usec + // timer_control_speed.attachInterrupt(control_speed); + // timer_control_speed.refresh(); + // timer_control_speed.resume(); +} + +/******************************************************************************* +* loop function +*******************************************************************************/ +void loop() +{ + receive_remocon_data(); + + if( (millis()-tTime[1]) >= (1000 / SPEEDCONTROL_RATE) ) + { + control_speed(); + tTime[1] = millis(); + } + + if( (millis()-tTime[2]) >= (1000 / CMD_VEL_PUB_RATE) ) + { + cmd_vel_rc100_pub.publish(&cmd_vel_rc100_msg); + tTime[2] = millis(); + } + + if( (millis()-tTime[0]) >= (1000 / CMD_VEL_PUB_RATE) ) + { + publish_drive_information(); + tTime[0] = millis(); + } + + nh.spinOnce(); +} + +/******************************************************************************* +* Callback function for cmd_vel msg +*******************************************************************************/ +void cmd_vel_callback(const geometry_msgs::Twist& cmd_vel_msg) +{ + rc100_remocon_mode = false; + goal_linear_velocity = cmd_vel_msg.linear.x; + goal_angular_velocity = cmd_vel_msg.angular.z; +} + +/******************************************************************************* +* Publish msgs (IMU data: angular velocity, linear acceleration, orientation) +*******************************************************************************/ +void publish_imu_msg(void) +{ + /* + Range : Roll : +/- 180 deg/sec + Pitch : +/- 180 deg/sec + Yaw : +/- 180 deg/sec + Scale : Roll : 10 = 1 deg/sec + Pitch : 10 = 1 deg/sec + Yaw : 1 = 1 deg/sec + */ + + imu.update(); + + imu_msg.header.stamp = nh.now(); + imu_msg.header.frame_id = "imu_link"; + + imu_msg.angular_velocity.x = imu.SEN.gyroRAW[0]; + imu_msg.angular_velocity.y = imu.SEN.gyroRAW[1]; + imu_msg.angular_velocity.z = imu.SEN.gyroRAW[2]; + + imu_msg.linear_acceleration.x = imu.SEN.accRAW[0]; + imu_msg.linear_acceleration.y = imu.SEN.accRAW[1]; + imu_msg.linear_acceleration.z = imu.SEN.accRAW[2]; + + float cr2 = cos(imu.angle[0]/20); + float cp2 = cos(imu.angle[1]/20); + float cy2 = cos(imu.angle[2]/20); + float sr2 = sin(imu.angle[0]/20); + float sp2 = sin(imu.angle[1]/20); + float sy2 = sin(imu.angle[2]/20); + + float q1, q2, q3, q4; + + q1 = cr2*cp2*cy2 + sr2*sp2*sy2; + q2 = sr2*cp2*cy2 - cr2*sp2*sy2; + q3 = cr2*sp2*cy2 + sr2*cp2*sy2; + q4 = cr2*cp2*sy2 - sr2*sp2*cy2; + + imu_msg.orientation.x = q1; + imu_msg.orientation.y = q2; + imu_msg.orientation.z = q3; + imu_msg.orientation.w = q4; + + imu_pub.publish(&imu_msg); + + tfs_msg.header.stamp = nh.now(); + tfs_msg.header.frame_id = "base_footprint"; + tfs_msg.child_frame_id = "imu_link"; + tfs_msg.transform.rotation.x = q1; + tfs_msg.transform.rotation.y = q2; + tfs_msg.transform.rotation.z = q3; + tfs_msg.transform.rotation.w = q4; + tfbroadcaster.sendTransform(tfs_msg); +} + +/******************************************************************************* +* Publish msgs (sensor_state: bumpers, cliffs, buttons, encoders, battery) +*******************************************************************************/ +void publish_sensor_state_msg(void) +{ + bool dxl_comm_result = false; + + dxl_comm_result = motor_driver.readEncoder(ADDR_XM_PRESENT_POSITION, 4, sensor_state_msg.left_encoder, sensor_state_msg.right_encoder); + + if (dxl_comm_result == true) + { + sensor_state_pub.publish(&sensor_state_msg); + } + else + { + // ROS_ERROR("syncReadDynamixelRegister failed"); + } + + int32_t current_tick = sensor_state_msg.left_encoder; + + if (!init_encoder_[LEFT]) + { + last_tick_[LEFT] = current_tick; + init_encoder_[LEFT] = true; + } + + last_diff_tick_[LEFT] = current_tick - last_tick_[LEFT]; + last_tick_[LEFT] = current_tick; + last_rad_[LEFT] += TICK2RAD * (double)last_diff_tick_[LEFT]; + + current_tick = sensor_state_msg.right_encoder; + + if (!init_encoder_[RIGHT]) + { + last_tick_[RIGHT] = current_tick; + init_encoder_[RIGHT] = true; + } + + last_diff_tick_[RIGHT] = current_tick - last_tick_[RIGHT]; + last_tick_[RIGHT] = current_tick; + last_rad_[RIGHT] += TICK2RAD * (double)last_diff_tick_[RIGHT]; +} + +/******************************************************************************* +* Publish msgs (odometry, joint states, tf) +*******************************************************************************/ +void publish_drive_information(void) +{ + unsigned long time_now = millis(); + unsigned long step_time = time_now - prev_update_time; + prev_update_time = time_now; + ros::Time stamp_now = nh.now(); + + // odom + updateOdometry((double)(step_time * 1000)); + odom.header.stamp = stamp_now; + odom_pub.publish(&odom); + + // joint_states + updateJoint(); + joint_states.header.stamp = stamp_now; + joint_states_pub.publish(&joint_states); + + // tf + updateTF(odom_tf); + tfbroadcaster.sendTransform(odom_tf); +} + +/******************************************************************************* +* Calculate the odometry +*******************************************************************************/ +bool updateOdometry(double diff_time) +{ + double wheel_l, wheel_r; // rotation value of wheel [rad] + double v, w; // v = translational velocity [m/s], w = rotational velocity [rad/s] + double step_time; + wheel_l = wheel_r = 0.0; + v = w = 0.0; + step_time = 0.0; + + step_time = diff_time; + + if(step_time == 0) + return false; + + wheel_l = TICK2RAD * (double)last_diff_tick_[LEFT]; + wheel_r = TICK2RAD * (double)last_diff_tick_[RIGHT]; + + //ROS_INFO_STREAM("wheel_l = " << wheel_l); + //ROS_INFO_STREAM("wheel_r = " << wheel_r); + + if(isnan(wheel_l)) + { + wheel_l = 0.0; + } + + if(isnan(wheel_r)) + { + wheel_r = 0.0; + } + + v = WHEEL_RADIUS * (wheel_r + wheel_l) / 2 / step_time; + w = WHEEL_RADIUS * (wheel_r - wheel_l) / WHEEL_SEPARATION / step_time; + + //ROS_INFO_STREAM("step_time = " << ((double)last_diff_realtime_tick_left_ + (double)last_diff_realtime_tick_right_) / 2.0 / 1000.0); + //ROS_INFO_STREAM("step_time = " << step_time); + //ROS_INFO_STREAM("v = " << v); + //ROS_INFO_STREAM("w = " << w); + + last_velocity_[LEFT] = wheel_l / step_time; + last_velocity_[RIGHT] = wheel_r / step_time; + + //ROS_INFO_STREAM("last_velocity_left_ = " << last_velocity_left_); + //ROS_INFO_STREAM("last_velocity_right_ = " << last_velocity_right_); + + // compute odometric pose + odom_pose[0] += v * step_time * cos(odom_pose[2] + (w * step_time / 2)); + odom_pose[1] += v * step_time * sin(odom_pose[2] + (w * step_time / 2)); + odom_pose[2] += w * step_time; + + //ROS_INFO_STREAM(" x = " << odom_pose[0] << " y = " << odom_pose[1] << " r = " << odom_pose[2]); + + // compute odometric instantaneouse velocity + odom_vel[0] = v; + odom_vel[1] = 0.0; + odom_vel[2] = w; + + odom.pose.pose.position.x = odom_pose[0]; + odom.pose.pose.position.y = odom_pose[1]; + odom.pose.pose.position.z = 0; + odom.pose.pose.orientation = tf::createQuaternionFromYaw(odom_pose[2]); + + // We should update the twist of the odometry + odom.twist.twist.linear.x = odom_vel[0]; + odom.twist.twist.angular.z = odom_vel[2]; + + return true; +} + +/******************************************************************************* +* Calculate the joint states +*******************************************************************************/ +void updateJoint(void) +{ + joint_states_pos[LEFT] = last_rad_[LEFT]; + joint_states_pos[RIGHT] = last_rad_[RIGHT]; + + joint_states_vel[LEFT] = last_velocity_[LEFT]; + joint_states_vel[RIGHT] = last_velocity_[RIGHT]; + + joint_states.position = joint_states_pos; + joint_states.velocity = joint_states_vel; +} + +/******************************************************************************* +* Calculate the TF +*******************************************************************************/ +void updateTF(geometry_msgs::TransformStamped& odom_tf) +{ + odom.header.frame_id = "odom"; + odom_tf.header = odom.header; + odom_tf.child_frame_id = "base_footprint"; + odom_tf.transform.translation.x = odom.pose.pose.position.x; + odom_tf.transform.translation.y = odom.pose.pose.position.y; + odom_tf.transform.translation.z = odom.pose.pose.position.z; + odom_tf.transform.rotation = odom.pose.pose.orientation; +} + +/******************************************************************************* +* Receive remocon (RC100) data +*******************************************************************************/ +void receive_remocon_data(void) +{ + if (remote_controller.available()) + { + received_data = remote_controller.readData(); + + if(received_data & RC100_BTN_U) + { + rc100_remocon_mode = true; + cmd_vel_rc100_msg.linear.x += VELOCITY_LINEAR_X * SCALE_VELOCITY_LINEAR_X; + } + else if(received_data & RC100_BTN_D) + { + rc100_remocon_mode = true; + cmd_vel_rc100_msg.linear.x -= VELOCITY_LINEAR_X * SCALE_VELOCITY_LINEAR_X; + } + else if(received_data & RC100_BTN_L) + { + rc100_remocon_mode = true; + cmd_vel_rc100_msg.angular.z += VELOCITY_ANGULAR_Z * SCALE_VELOCITY_ANGULAR_Z; + } + else if(received_data & RC100_BTN_R) + { + rc100_remocon_mode = true; + cmd_vel_rc100_msg.angular.z -= VELOCITY_ANGULAR_Z * SCALE_VELOCITY_ANGULAR_Z; + } + else if(received_data & RC100_BTN_1) + { + rc100_remocon_mode = true; + const_cmd_vel += VELOCITY_STEP; + } + else if(received_data & RC100_BTN_3) + { + rc100_remocon_mode = true; + const_cmd_vel -= VELOCITY_STEP; + } + else if(received_data & RC100_BTN_6) + { + rc100_remocon_mode = true; + cmd_vel_rc100_msg.linear.x = const_cmd_vel; + cmd_vel_rc100_msg.angular.z = 0.0; + } + else if(received_data & RC100_BTN_5) + { + rc100_remocon_mode = true; + cmd_vel_rc100_msg.linear.x = 0.0; + cmd_vel_rc100_msg.angular.z = 0.0; + } + } +} + +/******************************************************************************* +* Control motors +*******************************************************************************/ +void control_speed(void) +{ + double lin_vel1; + double lin_vel2; + + if (rc100_remocon_mode) + { + wheel_speed_cmd[LEFT] = cmd_vel_rc100_msg.linear.x - (cmd_vel_rc100_msg.angular.z * WHEEL_SEPARATION / 2); + wheel_speed_cmd[RIGHT] = cmd_vel_rc100_msg.linear.x + (cmd_vel_rc100_msg.angular.z * WHEEL_SEPARATION / 2); + } + else + { + wheel_speed_cmd[LEFT] = goal_linear_velocity - (goal_angular_velocity * WHEEL_SEPARATION / 2); + wheel_speed_cmd[RIGHT] = goal_linear_velocity + (goal_angular_velocity * WHEEL_SEPARATION / 2); + } + + lin_vel1 = wheel_speed_cmd[LEFT] * VELOCITY_CONSTANT_VAULE; + + if (lin_vel1 > LIMIT_XM_MAX_VELOCITY) lin_vel1 = LIMIT_XM_MAX_VELOCITY; + else if (lin_vel1 < -LIMIT_XM_MAX_VELOCITY) lin_vel1 = -LIMIT_XM_MAX_VELOCITY; + + lin_vel2 = wheel_speed_cmd[RIGHT] * VELOCITY_CONSTANT_VAULE; + + if (lin_vel2 > LIMIT_XM_MAX_VELOCITY) lin_vel2 = LIMIT_XM_MAX_VELOCITY; + else if (lin_vel2 < -LIMIT_XM_MAX_VELOCITY) lin_vel2 = -LIMIT_XM_MAX_VELOCITY; + + bool dxl_comm_result = false; + + dxl_comm_result = motor_driver.speedControl(ADDR_XM_GOAL_VELOCITY, 4, (int64_t)lin_vel1, (int64_t)lin_vel2); + + if (dxl_comm_result == false) + { + // ROS_ERROR("syncWriteDynamixelRegister failed"); + } +} + +// EOF diff --git a/turtlebot3_core/turtlebot3_core_config.h b/turtlebot3_core/turtlebot3_core_config.h new file mode 100644 index 00000000..9de77ff5 --- /dev/null +++ b/turtlebot3_core/turtlebot3_core_config.h @@ -0,0 +1,91 @@ +/******************************************************************************* +* Copyright 2016 ROBOTIS CO., LTD. +* +* Licensed under the Apache License, Version 2.0 (the "License"); +* you may not use this file except in compliance with the License. +* You may obtain a copy of the License at +* +* http://www.apache.org/licenses/LICENSE-2.0 +* +* Unless required by applicable law or agreed to in writing, software +* distributed under the License is distributed on an "AS IS" BASIS, +* WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +* See the License for the specific language governing permissions and +* limitations under the License. +*******************************************************************************/ + +/* Authors: Yoonseok Pyo, Leon Jung, Darby Lim */ + +#ifndef TURTLEBOT3_CORE_CONFIG_H_ +#define TURTLEBOT3_CORE_CONFIG_H_ + +#include + +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + +#include +#include + +#include +#include + +#include "turtlebot3_motor_driver.h" + +#define IMU_PUBLISH_PERIOD 10 //hz +#define SENSOR_STATE_PUBLISH_PERIOD 10 //hz +// #define CMD_VELOCITY_PUBLISH_PERIOD 0.01 // sec +// #define SPEED_CONTROL_PERIOD 0.01 // sec + +#define FEEDBACK_PUB_RATE 10 //hz +#define SPEEDCONTROL_RATE 10 //hz +#define CMD_VEL_PUB_RATE 10 //hz + +#define WHEEL_RADIUS 0.033 // radian +#define WHEEL_SEPARATION 0.16 // meter +#define ENCODER_MIN -2147483648 // raw +#define ENCODER_MAX 2147483648 // raw + +#define LEFT 0 +#define RIGHT 1 + +#define VELOCITY_CONSTANT_VAULE 1263.632956882 // V = r * w = r * RPM * 0.10472 + // = 0.033 * 0.229 * Goal RPM * 0.10472 + // Goal RPM = V * 1263.632956882 + +#define VELOCITY 10.0 +#define VELOCITY_STEP 0.02 +#define VELOCITY_LINEAR_X 0.01 +#define VELOCITY_ANGULAR_Z 0.01 +#define SCALE_VELOCITY_LINEAR_X 1 +#define SCALE_VELOCITY_ANGULAR_Z 1 + +#define TICK2RAD 0.00153589f // 0.088[deg] * 3.14159265359 / 180 = 0.00153589 + +typedef struct +{ + uint8_t addr; + uint8_t length; + uint8_t attr; + uint8_t init_data; +} test; + +void cmd_vel_callback(const geometry_msgs::Twist& cmd_vel_msg); +void publish_imu_msg(void); +void publish_sensor_state_msg(void); +void publish_drive_information(void); +bool updateOdometry(double diff_time); +void updateJoint(void); +void updateTF(geometry_msgs::TransformStamped& odom_tf); +void receive_remocon_data(void); +void control_speed(void); + +#endif // TURTLEBOT3_CORE_CONFIG_H_ diff --git a/turtlebot3_core/turtlebot3_motor_driver.cpp b/turtlebot3_core/turtlebot3_motor_driver.cpp new file mode 100644 index 00000000..d66e4203 --- /dev/null +++ b/turtlebot3_core/turtlebot3_motor_driver.cpp @@ -0,0 +1,182 @@ +/******************************************************************************* +* Copyright 2016 ROBOTIS CO., LTD. +* +* Licensed under the Apache License, Version 2.0 (the "License"); +* you may not use this file except in compliance with the License. +* You may obtain a copy of the License at +* +* http://www.apache.org/licenses/LICENSE-2.0 +* +* Unless required by applicable law or agreed to in writing, software +* distributed under the License is distributed on an "AS IS" BASIS, +* WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +* See the License for the specific language governing permissions and +* limitations under the License. +*******************************************************************************/ + +/* Authors: Yoonseok Pyo, Leon Jung, Darby Lim */ + +#include "turtlebot3_motor_driver.h" + +Turtlebot3MotorDriver::Turtlebot3MotorDriver() +: baudrate_(BAUDRATE), + // device_name_(DEVICENAME), + protocol_version_(PROTOCOL_VERSION), + left_wheel_id_(DXL_LEFT_ID), + right_wheel_id_(DXL_RIGHT_ID) +{ + // int dxl_comm_result = false; // Communication result + // bool dxl_addparam_result = false; // addParam result + // bool dxl_getdata_result = false; // GetParam result + // uint8_t dxl_error = 0; // Dynamixel error + // int32_t dxl1_present_position = 0; // Present position + // int32_t dxl2_present_position = 0; // Present position + + + // dynamixel::GroupSyncWrite groupSyncWrite(portHandler, packetHandler, ADDR_XM_GOAL_VELOCITY, LEN_XM_GOAL_VELOCITY); + // dynamixel::GroupSyncRead groupSyncRead(portHandler, packetHandler, ADDR_XM_PRESENT_POSITION, LEN_XM_PRESENT_POSITION); +} + +Turtlebot3MotorDriver::~Turtlebot3MotorDriver() +{ + closeDynamixel(); +} + +bool Turtlebot3MotorDriver::init(void) +{ + portHandler_ = dynamixel::PortHandler::getPortHandler(DEVICENAME); + packetHandler_ = dynamixel::PacketHandler::getPacketHandler(PROTOCOL_VERSION); + + // Open port + if (portHandler_->openPort()) + { + // nh.loginfo("Succeeded to open the port!"); + } + else + { + // nh.loginfo("Failed to open the port!"); + // nh.loginfo("Press any key to terminate..."); + // return; + } + + // Set port baudrate + if (portHandler_->setBaudRate(baudrate_)) + { + // nh.loginfo("Succeeded to change the baudrate!"); + } + else + { + // nh.loginfo("Failed to change the baudrate!"); + // nh.loginfo("Press any key to terminate..."); + // return; + } + + // Enable Dynamixel Torque + setTorque(left_wheel_id_, true); + setTorque(right_wheel_id_, true); +} + +bool Turtlebot3MotorDriver::setTorque(uint8_t id, bool onoff) +{ + uint8_t dxl_error = 0; + int dxl_comm_result = COMM_TX_FAIL; + + dxl_comm_result = packetHandler_->write1ByteTxRx(portHandler_, id, ADDR_XM_TORQUE_ENABLE, onoff, &dxl_error); + + if(dxl_comm_result != COMM_SUCCESS) + packetHandler_->printTxRxResult(dxl_comm_result); + else if(dxl_error != 0) + packetHandler_->printRxPacketError(dxl_error); +} + +void Turtlebot3MotorDriver::closeDynamixel(void) +{ + // Disable Dynamixel Torque + setTorque(left_wheel_id_, false); + setTorque(right_wheel_id_, false); + + // Close port + portHandler_->closePort(); +} + +bool Turtlebot3MotorDriver::readEncoder(uint16_t addr, uint16_t length, int32_t &left_value, int32_t &right_value) +{ + int dxl_comm_result = COMM_TX_FAIL; // Communication result + bool dxl_addparam_result = false; // addParam result + bool dxl_getdata_result = false; // GetParam result + + dynamixel::GroupSyncRead groupSyncRead(portHandler_, packetHandler_, addr, length); + + // Set parameter + dxl_addparam_result = groupSyncRead.addParam(left_wheel_id_); + if (dxl_addparam_result != true) + { + // fprintf(stderr, "[ID:%03d] groupSyncRead addparam failed", left_wheel_id_); + return false; + } + + dxl_addparam_result = groupSyncRead.addParam(right_wheel_id_); + if (dxl_addparam_result != true) + { + // fprintf(stderr, "[ID:%03d] groupSyncRead addparam failed", right_wheel_id_); + return false; + } + + // Syncread present position + dxl_comm_result = groupSyncRead.txRxPacket(); + if (dxl_comm_result != COMM_SUCCESS) packetHandler_->printTxRxResult(dxl_comm_result); + + // Check if groupsyncread data of Dynamixels are available + dxl_getdata_result = groupSyncRead.isAvailable(left_wheel_id_, addr, length); + if (dxl_getdata_result != true) + { + // fprintf(stderr, "[ID:%03d] groupSyncRead getdata failed", left_wheel_id_); + return false; + } + + dxl_getdata_result = groupSyncRead.isAvailable(right_wheel_id_, addr, length); + if (dxl_getdata_result != true) + { + // fprintf(stderr, "[ID:%03d] groupSyncRead getdata failed", right_wheel_id_); + return false; + } + + // Get data + left_value = groupSyncRead.getData(left_wheel_id_, addr, length); + right_value = groupSyncRead.getData(right_wheel_id_, addr, length); + + return true; +} + +bool Turtlebot3MotorDriver::speedControl(uint16_t addr, uint16_t length, int64_t left_wheel_value, int64_t right_wheel_value) +{ + bool dxl_addparam_result_; + int8_t dxl_comm_result_; + + dynamixel::GroupSyncWrite groupSyncWrite(portHandler_, packetHandler_, addr, length); + + dxl_addparam_result_ = groupSyncWrite.addParam(left_wheel_id_, (uint8_t*)&left_wheel_value); + if (dxl_addparam_result_ != true) + { + // logerror("[ID:%03d] groupSyncWrite addparam failed", left_wheel_id_); + return false; + } + + dxl_addparam_result_ = groupSyncWrite.addParam(right_wheel_id_, (uint8_t*)&right_wheel_value); + if (dxl_addparam_result_ != true) + { + // logerror("[ID:%03d] groupSyncWrite addparam failed", right_wheel_id_); + return false; + } + + dxl_comm_result_ = groupSyncWrite.txPacket(); + + if (dxl_comm_result_ != COMM_SUCCESS) + { + packetHandler_->printTxRxResult(dxl_comm_result_); + return false; + } + + groupSyncWrite.clearParam(); + return true; +} diff --git a/turtlebot3_core/turtlebot3_motor_driver.h b/turtlebot3_core/turtlebot3_motor_driver.h new file mode 100644 index 00000000..b53aff3d --- /dev/null +++ b/turtlebot3_core/turtlebot3_motor_driver.h @@ -0,0 +1,74 @@ +/******************************************************************************* +* Copyright 2016 ROBOTIS CO., LTD. +* +* Licensed under the Apache License, Version 2.0 (the "License"); +* you may not use this file except in compliance with the License. +* You may obtain a copy of the License at +* +* http://www.apache.org/licenses/LICENSE-2.0 +* +* Unless required by applicable law or agreed to in writing, software +* distributed under the License is distributed on an "AS IS" BASIS, +* WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +* See the License for the specific language governing permissions and +* limitations under the License. +*******************************************************************************/ + +/* Authors: Yoonseok Pyo, Leon Jung, Darby Lim */ + +#ifndef TURTLEBOT3_MOTOR_DRIVER_H_ +#define TURTLEBOT3_MOTOR_DRIVER_H_ + +#include + +// Control table address (XM430-W350-T) +#define ADDR_XM_TORQUE_ENABLE 64 +#define ADDR_XM_GOAL_VELOCITY 104 +#define ADDR_XM_GOAL_POSITION 116 +#define ADDR_XM_REALTIME_TICK 120 +#define ADDR_XM_PRESENT_VELOCITY 128 +#define ADDR_XM_PRESENT_POSITION 132 + +// Limit values (XM430-W350-T) +#define LIMIT_XM_MAX_VELOCITY 350 //480(XM430-W210-T) + +// Data Byte Length +#define LEN_XM_TORQUE_ENABLE 1 +#define LEN_XM_GOAL_VELOCITY 4 +#define LEN_XM_GOAL_POSITION 4 +#define LEN_XM_REALTIME_TICK 2 +#define LEN_XM_PRESENT_VELOCITY 4 +#define LEN_XM_PRESENT_POSITION 4 + +#define PROTOCOL_VERSION 2.0 // Dynamixel protocol version 2.0 + +#define DXL_LEFT_ID 1 // ID of left motor +#define DXL_RIGHT_ID 2 // ID of right motor +#define BAUDRATE 1000000 // baurd rate of Dynamixel +#define DEVICENAME "" // no need setting on OpenCR + +#define TORQUE_ENABLE 1 // Value for enabling the torque +#define TORQUE_DISABLE 0 // Value for disabling the torque + +class Turtlebot3MotorDriver +{ + public: + Turtlebot3MotorDriver(); + ~Turtlebot3MotorDriver(); + bool init(void); + void closeDynamixel(void); + bool setTorque(uint8_t id, bool onoff); + bool readEncoder(uint16_t addr, uint16_t length, int32_t &left_value, int32_t &right_value); + bool speedControl(uint16_t addr, uint16_t length, int64_t left_wheel_value, int64_t right_wheel_value); + + private: + // char device_name_[]; + int8_t baudrate_; + float protocol_version_; + uint8_t left_wheel_id_; + uint8_t right_wheel_id_; + dynamixel::PortHandler *portHandler_; + dynamixel::PacketHandler *packetHandler_; +}; + +#endif // DYNAMIXEL_CONTROLLER_H_