├── .gitignore ├── maps ├── house.pgm └── house.yaml ├── arduino └── firmware │ ├── lib │ ├── Encoder │ │ ├── keywords.txt │ │ ├── Encoder.cpp │ │ ├── examples │ │ │ ├── Basic │ │ │ │ └── Basic.pde │ │ │ ├── TwoKnobs │ │ │ │ └── TwoKnobs.pde │ │ │ └── NoInterrupts │ │ │ │ └── NoInterrupts.pde │ │ └── utility │ │ │ ├── direct_pin_read.h │ │ │ └── interrupt_config.h │ ├── imu │ │ ├── magnetometer.h │ │ ├── accelerometer.h │ │ ├── gyroscope.h │ │ ├── imu_wire.h │ │ ├── magnetometer_HMC5883L.h │ │ ├── gyroscope_L3G4200D.h │ │ ├── gyroscope_ITG3205.h │ │ └── accelerometer_ADXL345.h │ ├── pid │ │ ├── PID.h │ │ ├── PID.cpp │ │ └── pid.ino │ ├── motor │ │ ├── Motor.h │ │ ├── motor.ino │ │ └── Motor.cpp │ ├── ros_lib │ │ ├── std_msgs │ │ │ ├── Empty.h │ │ │ ├── Char.h │ │ │ ├── UInt8.h │ │ │ ├── UInt16.h │ │ │ ├── Bool.h │ │ │ ├── Byte.h │ │ │ ├── Int8.h │ │ │ ├── String.h │ │ │ ├── UInt32.h │ │ │ ├── Int16.h │ │ │ ├── Int32.h │ │ │ ├── Float32.h │ │ │ ├── UInt8MultiArray.h │ │ │ ├── UInt64.h │ │ │ ├── UInt16MultiArray.h │ │ │ ├── Time.h │ │ │ ├── Duration.h │ │ │ ├── ByteMultiArray.h │ │ │ ├── Int8MultiArray.h │ │ │ ├── Int64.h │ │ │ ├── Float64.h │ │ │ ├── UInt32MultiArray.h │ │ │ ├── Int16MultiArray.h │ │ │ ├── MultiArrayLayout.h │ │ │ ├── Float32MultiArray.h │ │ │ ├── Int32MultiArray.h │ │ │ ├── MultiArrayDimension.h │ │ │ ├── UInt64MultiArray.h │ │ │ ├── Int64MultiArray.h │ │ │ └── Float64MultiArray.h │ │ ├── geometry_msgs │ │ │ ├── Accel.h │ │ │ ├── Twist.h │ │ │ ├── Wrench.h │ │ │ ├── PoseStamped.h │ │ │ ├── AccelStamped.h │ │ │ ├── PointStamped.h │ │ │ ├── Pose.h │ │ │ ├── TwistStamped.h │ │ │ ├── WrenchStamped.h │ │ │ ├── Vector3Stamped.h │ │ │ ├── InertiaStamped.h │ │ │ ├── PolygonStamped.h │ │ │ ├── Transform.h │ │ │ ├── QuaternionStamped.h │ │ │ ├── PoseWithCovarianceStamped.h │ │ │ ├── AccelWithCovarianceStamped.h │ │ │ ├── TwistWithCovarianceStamped.h │ │ │ ├── Polygon.h │ │ │ ├── PoseArray.h │ │ │ ├── TransformStamped.h │ │ │ ├── PoseWithCovariance.h │ │ │ ├── AccelWithCovariance.h │ │ │ └── TwistWithCovariance.h │ │ ├── rosserial_msgs │ │ │ └── Log.h │ │ ├── sensor_msgs │ │ │ ├── JoyFeedbackArray.h │ │ │ ├── NavSatStatus.h │ │ │ ├── JoyFeedback.h │ │ │ ├── LaserEcho.h │ │ │ ├── CompressedImage.h │ │ │ ├── PointCloud.h │ │ │ ├── TimeReference.h │ │ │ ├── ChannelFloat32.h │ │ │ └── SetCameraInfo.h │ │ ├── tf │ │ │ ├── tfMessage.h │ │ │ ├── tf.h │ │ │ ├── FrameGraph.h │ │ │ └── transform_broadcaster.h │ │ ├── time.cpp │ │ ├── ros │ │ │ ├── publisher.h │ │ │ ├── duration.h │ │ │ ├── time.h │ │ │ ├── service_server.h │ │ │ ├── subscriber.h │ │ │ └── service_client.h │ │ ├── ros_arduino_msgs │ │ │ ├── CmdDiffVel.h │ │ │ └── Encoders.h │ │ ├── duration.cpp │ │ ├── ros.h │ │ └── rosserial_arduino │ │ │ ├── Test.h │ │ │ └── Adc.h │ ├── config │ │ └── lino_base_config.h │ └── kinematics │ │ └── Kinematics.h │ └── platformio.ini ├── param ├── global_costmap_params.yaml ├── local_costmap_params.yaml ├── costmap_common_params.yaml ├── move_base_params.yaml └── base_local_planner_params.yaml ├── README.md ├── launch ├── laser.launch ├── bringup.launch ├── navigate.launch ├── slam.launch └── amcl.launch ├── CMakeLists.txt └── package.xml /.gitignore: -------------------------------------------------------------------------------- 1 | *.AppleDouble 2 | *.pionenvs 3 | -------------------------------------------------------------------------------- /maps/house.pgm: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/grassjelly/linorobot_ackermann/HEAD/maps/house.pgm -------------------------------------------------------------------------------- /maps/house.yaml: -------------------------------------------------------------------------------- 1 | image: house.pgm 2 | resolution: 0.050000 3 | origin: [-50.000000, -50.000000, 0.000000] 4 | negate: 0 5 | occupied_thresh: 0.65 6 | free_thresh: 0.196 7 | -------------------------------------------------------------------------------- /arduino/firmware/lib/Encoder/keywords.txt: -------------------------------------------------------------------------------- 1 | ENCODER_USE_INTERRUPTS LITERAL1 2 | ENCODER_OPTIMIZE_INTERRUPTS LITERAL1 3 | ENCODER_DO_NOT_USE_INTERRUPTS LITERAL1 4 | Encoder KEYWORD1 5 | -------------------------------------------------------------------------------- /param/global_costmap_params.yaml: -------------------------------------------------------------------------------- 1 | global_costmap: 2 | global_frame: /map 3 | robot_base_frame: /base_link 4 | update_frequency: 1.0 #before: 5.0 5 | publish_frequency: 0.5 #before 0.5 6 | static_map: true 7 | transform_tolerance: 0.5 8 | -------------------------------------------------------------------------------- /arduino/firmware/lib/Encoder/Encoder.cpp: -------------------------------------------------------------------------------- 1 | 2 | #include "Encoder.h" 3 | 4 | // Yes, all the code is in the header file, to provide the user 5 | // configure options with #define (before they include it), and 6 | // to facilitate some crafty optimizations! 7 | 8 | Encoder_internal_state_t * Encoder::interruptArgs[]; 9 | 10 | 11 | -------------------------------------------------------------------------------- /param/local_costmap_params.yaml: -------------------------------------------------------------------------------- 1 | local_costmap: 2 | global_frame: /odom 3 | robot_base_frame: /base_link 4 | update_frequency: 1.0 #before 5.0 5 | publish_frequency: 1.0 #before 2.0 6 | static_map: false 7 | rolling_window: true 8 | width: 2.5 9 | height: 2.5 10 | resolution: 0.05 #before 0.05 11 | transform_tolerance: 0.5 -------------------------------------------------------------------------------- /README.md: -------------------------------------------------------------------------------- 1 | # [ This is no longer maintained. Updated repo here: https://github.com/linorobot/linorobot] 2 | 3 | An Ackerman Steering base for Linorobot series. 4 | 5 | Linorobot is an Open Source robotic project that aims to provide students, developers, and researchers a low-cost platform in creating new exciting applications on top of ROS. 6 | 7 | http://linorobot.org 8 | -------------------------------------------------------------------------------- /param/costmap_common_params.yaml: -------------------------------------------------------------------------------- 1 | obstacle_range: 2.5 2 | raytrace_range: 3.0 3 | footprint: [[-0.180, -0.130], [-0.180, 0.130], [0.180, 0.130], [0.180, -0.130]] 4 | inflation_radius: 0.3 5 | transform_tolerance: 0.13 6 | 7 | observation_sources: scan 8 | scan: 9 | data_type: LaserScan 10 | topic: scan 11 | marking: true 12 | clearing: true 13 | 14 | map_type: costmap 15 | -------------------------------------------------------------------------------- /param/move_base_params.yaml: -------------------------------------------------------------------------------- 1 | shutdown_costmaps: false 2 | 3 | controller_frequency: 3.0 #before 5.0 4 | controller_patience: 3.0 5 | 6 | 7 | planner_frequency: 1.0 8 | planner_patience: 5.0 9 | 10 | oscillation_timeout: 10.0 11 | oscillation_distance: 0.2 12 | 13 | conservative_reset_dist: 0.10 #distance from an obstacle at which it will unstuck itself 14 | 15 | 16 | 17 | -------------------------------------------------------------------------------- /arduino/firmware/lib/imu/magnetometer.h: -------------------------------------------------------------------------------- 1 | #ifndef _MAGNETOMETER_H_ 2 | #define _MAGNETOMETER_H_ 3 | 4 | uint8_t mag_reads = 0; 5 | byte mag_buffer[6]; 6 | 7 | bool checkMagnetometer(); 8 | void measureMagnetometer(); 9 | geometry_msgs::Vector3 raw_magnetic_field; 10 | 11 | #if defined(HMC5883L) 12 | #include "magnetometer_HMC5883L.h" 13 | #endif 14 | 15 | #endif // _MAGNETOMETER_H_ 16 | -------------------------------------------------------------------------------- /arduino/firmware/lib/imu/accelerometer.h: -------------------------------------------------------------------------------- 1 | #ifndef _ACCELEROMETER_H_ 2 | #define _ACCELEROMETER_H_ 3 | 4 | uint8_t acc_reads = 0; 5 | byte acc_buffer[6]; 6 | 7 | bool checkAccelerometer(); 8 | 9 | void measureAcceleration(); 10 | geometry_msgs::Vector3 raw_acceleration; 11 | 12 | #if defined(ADXL345) 13 | #include "accelerometer_ADXL345.h" 14 | #endif 15 | 16 | #endif // _ACCELEROMETER_H_ 17 | -------------------------------------------------------------------------------- /arduino/firmware/lib/imu/gyroscope.h: -------------------------------------------------------------------------------- 1 | #ifndef _GYROSCOPE_H_ 2 | #define _GYROSCOPE_H_ 3 | 4 | uint8_t gyro_reads = 0; 5 | byte gyro_buffer[6]; 6 | 7 | bool check_gyroscope(); 8 | 9 | geometry_msgs::Vector3 raw_rotation; 10 | void measure_gyroscope(); 11 | 12 | #if defined(ITG3205) 13 | #include "gyroscope_ITG3205.h" 14 | #elif defined(L3G4200D) 15 | #include "gyroscope_L3G4200D.h" 16 | #endif 17 | 18 | #endif // _GYROSCOPE_H_ 19 | 20 | -------------------------------------------------------------------------------- /launch/laser.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | -------------------------------------------------------------------------------- /arduino/firmware/lib/pid/PID.h: -------------------------------------------------------------------------------- 1 | #ifndef PID_H 2 | #define PID_H 3 | 4 | #include "Arduino.h" 5 | 6 | class PID 7 | { 8 | public: 9 | PID(float min_val, float max_val, float kp, float ki, float kd); 10 | double compute(float setpoint, float measured_value); 11 | void updateConstants(float kp, float ki, float kd); 12 | 13 | private: 14 | float min_val_; 15 | float max_val_; 16 | float kp_; 17 | float ki_; 18 | float kd_; 19 | double integral_; 20 | double derivative_; 21 | double prev_error_; 22 | }; 23 | 24 | #endif 25 | -------------------------------------------------------------------------------- /arduino/firmware/lib/motor/Motor.h: -------------------------------------------------------------------------------- 1 | #ifndef Motor_h 2 | #define Motor_h 3 | 4 | #include "Arduino.h" 5 | 6 | class Motor 7 | { 8 | public: 9 | int rpm; 10 | static int counts_per_rev_; 11 | 12 | Motor(int pwm_pin, int motor_pinA, int motor_pinB); 13 | Motor(int motor_pinA, int motor_pinB); 14 | void updateSpeed(long encoder_ticks); 15 | void spin(int pwm); 16 | 17 | private: 18 | enum driver_ {L298, BTS7960}; 19 | driver_ motor_driver_; 20 | long prev_encoder_ticks_; 21 | unsigned long prev_update_time_; 22 | int pwm_pin_; 23 | int motor_pinA_; 24 | int motor_pinB_; 25 | }; 26 | 27 | #endif 28 | -------------------------------------------------------------------------------- /arduino/firmware/platformio.ini: -------------------------------------------------------------------------------- 1 | # 2 | # Project Configuration File 3 | # 4 | # A detailed documentation with the EXAMPLES is located here: 5 | # http://docs.platformio.org/en/latest/projectconf.html 6 | # 7 | 8 | # A sign `#` at the beginning of the line indicates a comment 9 | # Comment lines are ignored. 10 | 11 | # Simple and base environment 12 | # [env:mybaseenv] 13 | # platform = %INSTALLED_PLATFORM_NAME_HERE% 14 | # framework = 15 | # board = 16 | # 17 | # Automatic targets - enable auto-uploading 18 | # targets = upload 19 | 20 | [env:teensy31] 21 | platform = teensy 22 | framework = arduino 23 | board = teensy31 24 | upload_port = /dev/linobase 25 | # targets = upload 26 | # 27 | -------------------------------------------------------------------------------- /arduino/firmware/lib/ros_lib/std_msgs/Empty.h: -------------------------------------------------------------------------------- 1 | #ifndef _ROS_std_msgs_Empty_h 2 | #define _ROS_std_msgs_Empty_h 3 | 4 | #include 5 | #include 6 | #include 7 | #include "ros/msg.h" 8 | 9 | namespace std_msgs 10 | { 11 | 12 | class Empty : public ros::Msg 13 | { 14 | public: 15 | 16 | Empty() 17 | { 18 | } 19 | 20 | virtual int serialize(unsigned char *outbuffer) const 21 | { 22 | int offset = 0; 23 | return offset; 24 | } 25 | 26 | virtual int deserialize(unsigned char *inbuffer) 27 | { 28 | int offset = 0; 29 | return offset; 30 | } 31 | 32 | const char * getType(){ return "std_msgs/Empty"; }; 33 | const char * getMD5(){ return "d41d8cd98f00b204e9800998ecf8427e"; }; 34 | 35 | }; 36 | 37 | } 38 | #endif -------------------------------------------------------------------------------- /arduino/firmware/lib/imu/imu_wire.h: -------------------------------------------------------------------------------- 1 | #ifndef _IMU_WIRE_H_ 2 | #define _IMU_WIRE_H_ 3 | 4 | 5 | void send_value(int device_address, byte value) { 6 | 7 | Wire.beginTransmission(device_address); 8 | Wire.write(value); 9 | Wire.endTransmission(); 10 | } 11 | 12 | void write_to_register(int device_address, byte register_address, byte register_value) { 13 | 14 | Wire.beginTransmission(device_address); 15 | Wire.write(register_address); 16 | Wire.write(register_value); 17 | Wire.endTransmission(); 18 | 19 | } 20 | 21 | byte check_ID(int device_address, byte register_address) { 22 | 23 | Wire.beginTransmission(device_address); 24 | Wire.write(register_address); 25 | Wire.endTransmission(); 26 | delay(20); 27 | Wire.requestFrom(device_address, 1); 28 | return Wire.read(); 29 | } 30 | 31 | #endif // _IMU_WIRE_H_ 32 | -------------------------------------------------------------------------------- /arduino/firmware/lib/Encoder/examples/Basic/Basic.pde: -------------------------------------------------------------------------------- 1 | /* Encoder Library - Basic Example 2 | * http://www.pjrc.com/teensy/td_libs_Encoder.html 3 | * 4 | * This example code is in the public domain. 5 | */ 6 | 7 | #include 8 | 9 | // Change these two numbers to the pins connected to your encoder. 10 | // Best Performance: both pins have interrupt capability 11 | // Good Performance: only the first pin has interrupt capability 12 | // Low Performance: neither pin has interrupt capability 13 | Encoder myEnc(5, 6); 14 | // avoid using pins with LEDs attached 15 | 16 | void setup() { 17 | Serial.begin(9600); 18 | Serial.println("Basic Encoder Test:"); 19 | } 20 | 21 | long oldPosition = -999; 22 | 23 | void loop() { 24 | long newPosition = myEnc.read(); 25 | if (newPosition != oldPosition) { 26 | oldPosition = newPosition; 27 | Serial.println(newPosition); 28 | } 29 | } 30 | -------------------------------------------------------------------------------- /CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 2.8.3) 2 | project(linorobot_ackermann) 3 | 4 | ## Find catkin macros and libraries 5 | ## if COMPONENTS list like find_package(catkin REQUIRED COMPONENTS xyz) 6 | ## is used, also find other catkin packages 7 | find_package(catkin REQUIRED COMPONENTS 8 | geometry_msgs 9 | ros_arduino_msgs 10 | roscpp 11 | rospy 12 | rosserial_python 13 | sensor_msgs 14 | std_msgs 15 | tf 16 | tf 17 | ) 18 | 19 | 20 | catkin_package( 21 | # INCLUDE_DIRS include 22 | # LIBRARIES linorobot 23 | # CATKIN_DEPENDS geometry_msgs ros_arduino_msgs roscpp rospy rosserial_python sensor_msgs std_msgs tf tf 24 | # DEPENDS system_lib 25 | ) 26 | ########### 27 | ## Build ## 28 | ########### 29 | 30 | include_directories( 31 | ${catkin_INCLUDE_DIRS} 32 | ) 33 | 34 | add_executable(lino_base_node_ackermann src/lino_base_node_ackermann.cpp) 35 | target_link_libraries(lino_base_node_ackermann ${catkin_LIBRARIES}) 36 | -------------------------------------------------------------------------------- /arduino/firmware/lib/pid/PID.cpp: -------------------------------------------------------------------------------- 1 | #include "Arduino.h" 2 | #include "PID.h" 3 | 4 | PID::PID(float min_val, float max_val, float kp, float ki, float kd) 5 | { 6 | min_val_ = min_val; 7 | max_val_ = max_val; 8 | kp_ = kp; 9 | ki_ = ki; 10 | kd_ = kd; 11 | } 12 | 13 | double PID::compute(float setpoint, float measured_value) 14 | { 15 | double error; 16 | double pid; 17 | 18 | //setpoint is constrained between min and max to prevent pid from having too much error 19 | error = setpoint - measured_value; 20 | integral_ += error; 21 | derivative_ = error - prev_error_; 22 | 23 | if(setpoint == 0 && error == 0) 24 | { 25 | integral_ = 0; 26 | } 27 | 28 | pid = (kp_ * error) + (ki_ * integral_) + (kd_ * derivative_); 29 | prev_error_ = error; 30 | 31 | return constrain(pid, min_val_, max_val_); 32 | } 33 | 34 | void PID::updateConstants(float kp, float ki, float kd) 35 | { 36 | kp_ = kp; 37 | ki_ = ki; 38 | kd_ = kd; 39 | } 40 | -------------------------------------------------------------------------------- /arduino/firmware/lib/ros_lib/std_msgs/Char.h: -------------------------------------------------------------------------------- 1 | #ifndef _ROS_std_msgs_Char_h 2 | #define _ROS_std_msgs_Char_h 3 | 4 | #include 5 | #include 6 | #include 7 | #include "ros/msg.h" 8 | 9 | namespace std_msgs 10 | { 11 | 12 | class Char : public ros::Msg 13 | { 14 | public: 15 | uint8_t data; 16 | 17 | Char(): 18 | data(0) 19 | { 20 | } 21 | 22 | virtual int serialize(unsigned char *outbuffer) const 23 | { 24 | int offset = 0; 25 | *(outbuffer + offset + 0) = (this->data >> (8 * 0)) & 0xFF; 26 | offset += sizeof(this->data); 27 | return offset; 28 | } 29 | 30 | virtual int deserialize(unsigned char *inbuffer) 31 | { 32 | int offset = 0; 33 | this->data = ((uint8_t) (*(inbuffer + offset))); 34 | offset += sizeof(this->data); 35 | return offset; 36 | } 37 | 38 | const char * getType(){ return "std_msgs/Char"; }; 39 | const char * getMD5(){ return "1bf77f25acecdedba0e224b162199717"; }; 40 | 41 | }; 42 | 43 | } 44 | #endif -------------------------------------------------------------------------------- /arduino/firmware/lib/ros_lib/std_msgs/UInt8.h: -------------------------------------------------------------------------------- 1 | #ifndef _ROS_std_msgs_UInt8_h 2 | #define _ROS_std_msgs_UInt8_h 3 | 4 | #include 5 | #include 6 | #include 7 | #include "ros/msg.h" 8 | 9 | namespace std_msgs 10 | { 11 | 12 | class UInt8 : public ros::Msg 13 | { 14 | public: 15 | uint8_t data; 16 | 17 | UInt8(): 18 | data(0) 19 | { 20 | } 21 | 22 | virtual int serialize(unsigned char *outbuffer) const 23 | { 24 | int offset = 0; 25 | *(outbuffer + offset + 0) = (this->data >> (8 * 0)) & 0xFF; 26 | offset += sizeof(this->data); 27 | return offset; 28 | } 29 | 30 | virtual int deserialize(unsigned char *inbuffer) 31 | { 32 | int offset = 0; 33 | this->data = ((uint8_t) (*(inbuffer + offset))); 34 | offset += sizeof(this->data); 35 | return offset; 36 | } 37 | 38 | const char * getType(){ return "std_msgs/UInt8"; }; 39 | const char * getMD5(){ return "7c8164229e7d2c17eb95e9231617fdee"; }; 40 | 41 | }; 42 | 43 | } 44 | #endif -------------------------------------------------------------------------------- /arduino/firmware/lib/Encoder/utility/direct_pin_read.h: -------------------------------------------------------------------------------- 1 | #ifndef direct_pin_read_h_ 2 | #define direct_pin_read_h_ 3 | 4 | #if defined(__AVR__) || defined(__MK20DX128__) || defined(__MK20DX256__) 5 | 6 | #define IO_REG_TYPE uint8_t 7 | #define PIN_TO_BASEREG(pin) (portInputRegister(digitalPinToPort(pin))) 8 | #define PIN_TO_BITMASK(pin) (digitalPinToBitMask(pin)) 9 | #define DIRECT_PIN_READ(base, mask) (((*(base)) & (mask)) ? 1 : 0) 10 | 11 | #elif defined(__SAM3X8E__) 12 | 13 | #define IO_REG_TYPE uint32_t 14 | #define PIN_TO_BASEREG(pin) (portInputRegister(digitalPinToPort(pin))) 15 | #define PIN_TO_BITMASK(pin) (digitalPinToBitMask(pin)) 16 | #define DIRECT_PIN_READ(base, mask) (((*(base)) & (mask)) ? 1 : 0) 17 | 18 | #elif defined(__PIC32MX__) 19 | 20 | #define IO_REG_TYPE uint32_t 21 | #define PIN_TO_BASEREG(pin) (portModeRegister(digitalPinToPort(pin))) 22 | #define PIN_TO_BITMASK(pin) (digitalPinToBitMask(pin)) 23 | #define DIRECT_PIN_READ(base, mask) (((*(base+4)) & (mask)) ? 1 : 0) 24 | 25 | #endif 26 | 27 | #endif 28 | -------------------------------------------------------------------------------- /launch/bringup.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | imu/accelerometer_bias: {x: -0.131470156, y: -0.295752343, z: -0.5997614070000008} 11 | imu/gyroscope_bias: {x: -0.008431381556216777, y: 0.02036080765564346, z: 0.007239818370848954} 12 | imu/perform_calibration: false 13 | 14 | 15 | 16 | 17 | 18 | 19 | 20 | 21 | 22 | 23 | 24 | -------------------------------------------------------------------------------- /arduino/firmware/lib/ros_lib/std_msgs/UInt16.h: -------------------------------------------------------------------------------- 1 | #ifndef _ROS_std_msgs_UInt16_h 2 | #define _ROS_std_msgs_UInt16_h 3 | 4 | #include 5 | #include 6 | #include 7 | #include "ros/msg.h" 8 | 9 | namespace std_msgs 10 | { 11 | 12 | class UInt16 : public ros::Msg 13 | { 14 | public: 15 | uint16_t data; 16 | 17 | UInt16(): 18 | data(0) 19 | { 20 | } 21 | 22 | virtual int serialize(unsigned char *outbuffer) const 23 | { 24 | int offset = 0; 25 | *(outbuffer + offset + 0) = (this->data >> (8 * 0)) & 0xFF; 26 | *(outbuffer + offset + 1) = (this->data >> (8 * 1)) & 0xFF; 27 | offset += sizeof(this->data); 28 | return offset; 29 | } 30 | 31 | virtual int deserialize(unsigned char *inbuffer) 32 | { 33 | int offset = 0; 34 | this->data = ((uint16_t) (*(inbuffer + offset))); 35 | this->data |= ((uint16_t) (*(inbuffer + offset + 1))) << (8 * 1); 36 | offset += sizeof(this->data); 37 | return offset; 38 | } 39 | 40 | const char * getType(){ return "std_msgs/UInt16"; }; 41 | const char * getMD5(){ return "1df79edf208b629fe6b81923a544552d"; }; 42 | 43 | }; 44 | 45 | } 46 | #endif -------------------------------------------------------------------------------- /launch/navigate.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | 16 | 17 | 18 | 19 | 20 | -------------------------------------------------------------------------------- /arduino/firmware/lib/ros_lib/geometry_msgs/Accel.h: -------------------------------------------------------------------------------- 1 | #ifndef _ROS_geometry_msgs_Accel_h 2 | #define _ROS_geometry_msgs_Accel_h 3 | 4 | #include 5 | #include 6 | #include 7 | #include "ros/msg.h" 8 | #include "geometry_msgs/Vector3.h" 9 | 10 | namespace geometry_msgs 11 | { 12 | 13 | class Accel : public ros::Msg 14 | { 15 | public: 16 | geometry_msgs::Vector3 linear; 17 | geometry_msgs::Vector3 angular; 18 | 19 | Accel(): 20 | linear(), 21 | angular() 22 | { 23 | } 24 | 25 | virtual int serialize(unsigned char *outbuffer) const 26 | { 27 | int offset = 0; 28 | offset += this->linear.serialize(outbuffer + offset); 29 | offset += this->angular.serialize(outbuffer + offset); 30 | return offset; 31 | } 32 | 33 | virtual int deserialize(unsigned char *inbuffer) 34 | { 35 | int offset = 0; 36 | offset += this->linear.deserialize(inbuffer + offset); 37 | offset += this->angular.deserialize(inbuffer + offset); 38 | return offset; 39 | } 40 | 41 | const char * getType(){ return "geometry_msgs/Accel"; }; 42 | const char * getMD5(){ return "9f195f881246fdfa2798d1d3eebca84a"; }; 43 | 44 | }; 45 | 46 | } 47 | #endif -------------------------------------------------------------------------------- /arduino/firmware/lib/ros_lib/geometry_msgs/Twist.h: -------------------------------------------------------------------------------- 1 | #ifndef _ROS_geometry_msgs_Twist_h 2 | #define _ROS_geometry_msgs_Twist_h 3 | 4 | #include 5 | #include 6 | #include 7 | #include "ros/msg.h" 8 | #include "geometry_msgs/Vector3.h" 9 | 10 | namespace geometry_msgs 11 | { 12 | 13 | class Twist : public ros::Msg 14 | { 15 | public: 16 | geometry_msgs::Vector3 linear; 17 | geometry_msgs::Vector3 angular; 18 | 19 | Twist(): 20 | linear(), 21 | angular() 22 | { 23 | } 24 | 25 | virtual int serialize(unsigned char *outbuffer) const 26 | { 27 | int offset = 0; 28 | offset += this->linear.serialize(outbuffer + offset); 29 | offset += this->angular.serialize(outbuffer + offset); 30 | return offset; 31 | } 32 | 33 | virtual int deserialize(unsigned char *inbuffer) 34 | { 35 | int offset = 0; 36 | offset += this->linear.deserialize(inbuffer + offset); 37 | offset += this->angular.deserialize(inbuffer + offset); 38 | return offset; 39 | } 40 | 41 | const char * getType(){ return "geometry_msgs/Twist"; }; 42 | const char * getMD5(){ return "9f195f881246fdfa2798d1d3eebca84a"; }; 43 | 44 | }; 45 | 46 | } 47 | #endif -------------------------------------------------------------------------------- /arduino/firmware/lib/ros_lib/geometry_msgs/Wrench.h: -------------------------------------------------------------------------------- 1 | #ifndef _ROS_geometry_msgs_Wrench_h 2 | #define _ROS_geometry_msgs_Wrench_h 3 | 4 | #include 5 | #include 6 | #include 7 | #include "ros/msg.h" 8 | #include "geometry_msgs/Vector3.h" 9 | 10 | namespace geometry_msgs 11 | { 12 | 13 | class Wrench : public ros::Msg 14 | { 15 | public: 16 | geometry_msgs::Vector3 force; 17 | geometry_msgs::Vector3 torque; 18 | 19 | Wrench(): 20 | force(), 21 | torque() 22 | { 23 | } 24 | 25 | virtual int serialize(unsigned char *outbuffer) const 26 | { 27 | int offset = 0; 28 | offset += this->force.serialize(outbuffer + offset); 29 | offset += this->torque.serialize(outbuffer + offset); 30 | return offset; 31 | } 32 | 33 | virtual int deserialize(unsigned char *inbuffer) 34 | { 35 | int offset = 0; 36 | offset += this->force.deserialize(inbuffer + offset); 37 | offset += this->torque.deserialize(inbuffer + offset); 38 | return offset; 39 | } 40 | 41 | const char * getType(){ return "geometry_msgs/Wrench"; }; 42 | const char * getMD5(){ return "4f539cf138b23283b520fd271b567936"; }; 43 | 44 | }; 45 | 46 | } 47 | #endif -------------------------------------------------------------------------------- /arduino/firmware/lib/ros_lib/geometry_msgs/PoseStamped.h: -------------------------------------------------------------------------------- 1 | #ifndef _ROS_geometry_msgs_PoseStamped_h 2 | #define _ROS_geometry_msgs_PoseStamped_h 3 | 4 | #include 5 | #include 6 | #include 7 | #include "ros/msg.h" 8 | #include "std_msgs/Header.h" 9 | #include "geometry_msgs/Pose.h" 10 | 11 | namespace geometry_msgs 12 | { 13 | 14 | class PoseStamped : public ros::Msg 15 | { 16 | public: 17 | std_msgs::Header header; 18 | geometry_msgs::Pose pose; 19 | 20 | PoseStamped(): 21 | header(), 22 | pose() 23 | { 24 | } 25 | 26 | virtual int serialize(unsigned char *outbuffer) const 27 | { 28 | int offset = 0; 29 | offset += this->header.serialize(outbuffer + offset); 30 | offset += this->pose.serialize(outbuffer + offset); 31 | return offset; 32 | } 33 | 34 | virtual int deserialize(unsigned char *inbuffer) 35 | { 36 | int offset = 0; 37 | offset += this->header.deserialize(inbuffer + offset); 38 | offset += this->pose.deserialize(inbuffer + offset); 39 | return offset; 40 | } 41 | 42 | const char * getType(){ return "geometry_msgs/PoseStamped"; }; 43 | const char * getMD5(){ return "d3812c3cbc69362b77dc0b19b345f8f5"; }; 44 | 45 | }; 46 | 47 | } 48 | #endif -------------------------------------------------------------------------------- /arduino/firmware/lib/ros_lib/geometry_msgs/AccelStamped.h: -------------------------------------------------------------------------------- 1 | #ifndef _ROS_geometry_msgs_AccelStamped_h 2 | #define _ROS_geometry_msgs_AccelStamped_h 3 | 4 | #include 5 | #include 6 | #include 7 | #include "ros/msg.h" 8 | #include "std_msgs/Header.h" 9 | #include "geometry_msgs/Accel.h" 10 | 11 | namespace geometry_msgs 12 | { 13 | 14 | class AccelStamped : public ros::Msg 15 | { 16 | public: 17 | std_msgs::Header header; 18 | geometry_msgs::Accel accel; 19 | 20 | AccelStamped(): 21 | header(), 22 | accel() 23 | { 24 | } 25 | 26 | virtual int serialize(unsigned char *outbuffer) const 27 | { 28 | int offset = 0; 29 | offset += this->header.serialize(outbuffer + offset); 30 | offset += this->accel.serialize(outbuffer + offset); 31 | return offset; 32 | } 33 | 34 | virtual int deserialize(unsigned char *inbuffer) 35 | { 36 | int offset = 0; 37 | offset += this->header.deserialize(inbuffer + offset); 38 | offset += this->accel.deserialize(inbuffer + offset); 39 | return offset; 40 | } 41 | 42 | const char * getType(){ return "geometry_msgs/AccelStamped"; }; 43 | const char * getMD5(){ return "d8a98a5d81351b6eb0578c78557e7659"; }; 44 | 45 | }; 46 | 47 | } 48 | #endif -------------------------------------------------------------------------------- /arduino/firmware/lib/ros_lib/geometry_msgs/PointStamped.h: -------------------------------------------------------------------------------- 1 | #ifndef _ROS_geometry_msgs_PointStamped_h 2 | #define _ROS_geometry_msgs_PointStamped_h 3 | 4 | #include 5 | #include 6 | #include 7 | #include "ros/msg.h" 8 | #include "std_msgs/Header.h" 9 | #include "geometry_msgs/Point.h" 10 | 11 | namespace geometry_msgs 12 | { 13 | 14 | class PointStamped : public ros::Msg 15 | { 16 | public: 17 | std_msgs::Header header; 18 | geometry_msgs::Point point; 19 | 20 | PointStamped(): 21 | header(), 22 | point() 23 | { 24 | } 25 | 26 | virtual int serialize(unsigned char *outbuffer) const 27 | { 28 | int offset = 0; 29 | offset += this->header.serialize(outbuffer + offset); 30 | offset += this->point.serialize(outbuffer + offset); 31 | return offset; 32 | } 33 | 34 | virtual int deserialize(unsigned char *inbuffer) 35 | { 36 | int offset = 0; 37 | offset += this->header.deserialize(inbuffer + offset); 38 | offset += this->point.deserialize(inbuffer + offset); 39 | return offset; 40 | } 41 | 42 | const char * getType(){ return "geometry_msgs/PointStamped"; }; 43 | const char * getMD5(){ return "c63aecb41bfdfd6b7e1fac37c7cbe7bf"; }; 44 | 45 | }; 46 | 47 | } 48 | #endif -------------------------------------------------------------------------------- /arduino/firmware/lib/ros_lib/geometry_msgs/Pose.h: -------------------------------------------------------------------------------- 1 | #ifndef _ROS_geometry_msgs_Pose_h 2 | #define _ROS_geometry_msgs_Pose_h 3 | 4 | #include 5 | #include 6 | #include 7 | #include "ros/msg.h" 8 | #include "geometry_msgs/Point.h" 9 | #include "geometry_msgs/Quaternion.h" 10 | 11 | namespace geometry_msgs 12 | { 13 | 14 | class Pose : public ros::Msg 15 | { 16 | public: 17 | geometry_msgs::Point position; 18 | geometry_msgs::Quaternion orientation; 19 | 20 | Pose(): 21 | position(), 22 | orientation() 23 | { 24 | } 25 | 26 | virtual int serialize(unsigned char *outbuffer) const 27 | { 28 | int offset = 0; 29 | offset += this->position.serialize(outbuffer + offset); 30 | offset += this->orientation.serialize(outbuffer + offset); 31 | return offset; 32 | } 33 | 34 | virtual int deserialize(unsigned char *inbuffer) 35 | { 36 | int offset = 0; 37 | offset += this->position.deserialize(inbuffer + offset); 38 | offset += this->orientation.deserialize(inbuffer + offset); 39 | return offset; 40 | } 41 | 42 | const char * getType(){ return "geometry_msgs/Pose"; }; 43 | const char * getMD5(){ return "e45d45a5a1ce597b249e23fb30fc871f"; }; 44 | 45 | }; 46 | 47 | } 48 | #endif -------------------------------------------------------------------------------- /arduino/firmware/lib/ros_lib/geometry_msgs/TwistStamped.h: -------------------------------------------------------------------------------- 1 | #ifndef _ROS_geometry_msgs_TwistStamped_h 2 | #define _ROS_geometry_msgs_TwistStamped_h 3 | 4 | #include 5 | #include 6 | #include 7 | #include "ros/msg.h" 8 | #include "std_msgs/Header.h" 9 | #include "geometry_msgs/Twist.h" 10 | 11 | namespace geometry_msgs 12 | { 13 | 14 | class TwistStamped : public ros::Msg 15 | { 16 | public: 17 | std_msgs::Header header; 18 | geometry_msgs::Twist twist; 19 | 20 | TwistStamped(): 21 | header(), 22 | twist() 23 | { 24 | } 25 | 26 | virtual int serialize(unsigned char *outbuffer) const 27 | { 28 | int offset = 0; 29 | offset += this->header.serialize(outbuffer + offset); 30 | offset += this->twist.serialize(outbuffer + offset); 31 | return offset; 32 | } 33 | 34 | virtual int deserialize(unsigned char *inbuffer) 35 | { 36 | int offset = 0; 37 | offset += this->header.deserialize(inbuffer + offset); 38 | offset += this->twist.deserialize(inbuffer + offset); 39 | return offset; 40 | } 41 | 42 | const char * getType(){ return "geometry_msgs/TwistStamped"; }; 43 | const char * getMD5(){ return "98d34b0043a2093cf9d9345ab6eef12e"; }; 44 | 45 | }; 46 | 47 | } 48 | #endif -------------------------------------------------------------------------------- /arduino/firmware/lib/ros_lib/geometry_msgs/WrenchStamped.h: -------------------------------------------------------------------------------- 1 | #ifndef _ROS_geometry_msgs_WrenchStamped_h 2 | #define _ROS_geometry_msgs_WrenchStamped_h 3 | 4 | #include 5 | #include 6 | #include 7 | #include "ros/msg.h" 8 | #include "std_msgs/Header.h" 9 | #include "geometry_msgs/Wrench.h" 10 | 11 | namespace geometry_msgs 12 | { 13 | 14 | class WrenchStamped : public ros::Msg 15 | { 16 | public: 17 | std_msgs::Header header; 18 | geometry_msgs::Wrench wrench; 19 | 20 | WrenchStamped(): 21 | header(), 22 | wrench() 23 | { 24 | } 25 | 26 | virtual int serialize(unsigned char *outbuffer) const 27 | { 28 | int offset = 0; 29 | offset += this->header.serialize(outbuffer + offset); 30 | offset += this->wrench.serialize(outbuffer + offset); 31 | return offset; 32 | } 33 | 34 | virtual int deserialize(unsigned char *inbuffer) 35 | { 36 | int offset = 0; 37 | offset += this->header.deserialize(inbuffer + offset); 38 | offset += this->wrench.deserialize(inbuffer + offset); 39 | return offset; 40 | } 41 | 42 | const char * getType(){ return "geometry_msgs/WrenchStamped"; }; 43 | const char * getMD5(){ return "d78d3cb249ce23087ade7e7d0c40cfa7"; }; 44 | 45 | }; 46 | 47 | } 48 | #endif -------------------------------------------------------------------------------- /arduino/firmware/lib/ros_lib/geometry_msgs/Vector3Stamped.h: -------------------------------------------------------------------------------- 1 | #ifndef _ROS_geometry_msgs_Vector3Stamped_h 2 | #define _ROS_geometry_msgs_Vector3Stamped_h 3 | 4 | #include 5 | #include 6 | #include 7 | #include "ros/msg.h" 8 | #include "std_msgs/Header.h" 9 | #include "geometry_msgs/Vector3.h" 10 | 11 | namespace geometry_msgs 12 | { 13 | 14 | class Vector3Stamped : public ros::Msg 15 | { 16 | public: 17 | std_msgs::Header header; 18 | geometry_msgs::Vector3 vector; 19 | 20 | Vector3Stamped(): 21 | header(), 22 | vector() 23 | { 24 | } 25 | 26 | virtual int serialize(unsigned char *outbuffer) const 27 | { 28 | int offset = 0; 29 | offset += this->header.serialize(outbuffer + offset); 30 | offset += this->vector.serialize(outbuffer + offset); 31 | return offset; 32 | } 33 | 34 | virtual int deserialize(unsigned char *inbuffer) 35 | { 36 | int offset = 0; 37 | offset += this->header.deserialize(inbuffer + offset); 38 | offset += this->vector.deserialize(inbuffer + offset); 39 | return offset; 40 | } 41 | 42 | const char * getType(){ return "geometry_msgs/Vector3Stamped"; }; 43 | const char * getMD5(){ return "7b324c7325e683bf02a9b14b01090ec7"; }; 44 | 45 | }; 46 | 47 | } 48 | #endif -------------------------------------------------------------------------------- /arduino/firmware/lib/ros_lib/geometry_msgs/InertiaStamped.h: -------------------------------------------------------------------------------- 1 | #ifndef _ROS_geometry_msgs_InertiaStamped_h 2 | #define _ROS_geometry_msgs_InertiaStamped_h 3 | 4 | #include 5 | #include 6 | #include 7 | #include "ros/msg.h" 8 | #include "std_msgs/Header.h" 9 | #include "geometry_msgs/Inertia.h" 10 | 11 | namespace geometry_msgs 12 | { 13 | 14 | class InertiaStamped : public ros::Msg 15 | { 16 | public: 17 | std_msgs::Header header; 18 | geometry_msgs::Inertia inertia; 19 | 20 | InertiaStamped(): 21 | header(), 22 | inertia() 23 | { 24 | } 25 | 26 | virtual int serialize(unsigned char *outbuffer) const 27 | { 28 | int offset = 0; 29 | offset += this->header.serialize(outbuffer + offset); 30 | offset += this->inertia.serialize(outbuffer + offset); 31 | return offset; 32 | } 33 | 34 | virtual int deserialize(unsigned char *inbuffer) 35 | { 36 | int offset = 0; 37 | offset += this->header.deserialize(inbuffer + offset); 38 | offset += this->inertia.deserialize(inbuffer + offset); 39 | return offset; 40 | } 41 | 42 | const char * getType(){ return "geometry_msgs/InertiaStamped"; }; 43 | const char * getMD5(){ return "ddee48caeab5a966c5e8d166654a9ac7"; }; 44 | 45 | }; 46 | 47 | } 48 | #endif -------------------------------------------------------------------------------- /arduino/firmware/lib/ros_lib/geometry_msgs/PolygonStamped.h: -------------------------------------------------------------------------------- 1 | #ifndef _ROS_geometry_msgs_PolygonStamped_h 2 | #define _ROS_geometry_msgs_PolygonStamped_h 3 | 4 | #include 5 | #include 6 | #include 7 | #include "ros/msg.h" 8 | #include "std_msgs/Header.h" 9 | #include "geometry_msgs/Polygon.h" 10 | 11 | namespace geometry_msgs 12 | { 13 | 14 | class PolygonStamped : public ros::Msg 15 | { 16 | public: 17 | std_msgs::Header header; 18 | geometry_msgs::Polygon polygon; 19 | 20 | PolygonStamped(): 21 | header(), 22 | polygon() 23 | { 24 | } 25 | 26 | virtual int serialize(unsigned char *outbuffer) const 27 | { 28 | int offset = 0; 29 | offset += this->header.serialize(outbuffer + offset); 30 | offset += this->polygon.serialize(outbuffer + offset); 31 | return offset; 32 | } 33 | 34 | virtual int deserialize(unsigned char *inbuffer) 35 | { 36 | int offset = 0; 37 | offset += this->header.deserialize(inbuffer + offset); 38 | offset += this->polygon.deserialize(inbuffer + offset); 39 | return offset; 40 | } 41 | 42 | const char * getType(){ return "geometry_msgs/PolygonStamped"; }; 43 | const char * getMD5(){ return "c6be8f7dc3bee7fe9e8d296070f53340"; }; 44 | 45 | }; 46 | 47 | } 48 | #endif -------------------------------------------------------------------------------- /arduino/firmware/lib/ros_lib/geometry_msgs/Transform.h: -------------------------------------------------------------------------------- 1 | #ifndef _ROS_geometry_msgs_Transform_h 2 | #define _ROS_geometry_msgs_Transform_h 3 | 4 | #include 5 | #include 6 | #include 7 | #include "ros/msg.h" 8 | #include "geometry_msgs/Vector3.h" 9 | #include "geometry_msgs/Quaternion.h" 10 | 11 | namespace geometry_msgs 12 | { 13 | 14 | class Transform : public ros::Msg 15 | { 16 | public: 17 | geometry_msgs::Vector3 translation; 18 | geometry_msgs::Quaternion rotation; 19 | 20 | Transform(): 21 | translation(), 22 | rotation() 23 | { 24 | } 25 | 26 | virtual int serialize(unsigned char *outbuffer) const 27 | { 28 | int offset = 0; 29 | offset += this->translation.serialize(outbuffer + offset); 30 | offset += this->rotation.serialize(outbuffer + offset); 31 | return offset; 32 | } 33 | 34 | virtual int deserialize(unsigned char *inbuffer) 35 | { 36 | int offset = 0; 37 | offset += this->translation.deserialize(inbuffer + offset); 38 | offset += this->rotation.deserialize(inbuffer + offset); 39 | return offset; 40 | } 41 | 42 | const char * getType(){ return "geometry_msgs/Transform"; }; 43 | const char * getMD5(){ return "ac9eff44abf714214112b05d54a3cf9b"; }; 44 | 45 | }; 46 | 47 | } 48 | #endif -------------------------------------------------------------------------------- /arduino/firmware/lib/ros_lib/std_msgs/Bool.h: -------------------------------------------------------------------------------- 1 | #ifndef _ROS_std_msgs_Bool_h 2 | #define _ROS_std_msgs_Bool_h 3 | 4 | #include 5 | #include 6 | #include 7 | #include "ros/msg.h" 8 | 9 | namespace std_msgs 10 | { 11 | 12 | class Bool : public ros::Msg 13 | { 14 | public: 15 | bool data; 16 | 17 | Bool(): 18 | data(0) 19 | { 20 | } 21 | 22 | virtual int serialize(unsigned char *outbuffer) const 23 | { 24 | int offset = 0; 25 | union { 26 | bool real; 27 | uint8_t base; 28 | } u_data; 29 | u_data.real = this->data; 30 | *(outbuffer + offset + 0) = (u_data.base >> (8 * 0)) & 0xFF; 31 | offset += sizeof(this->data); 32 | return offset; 33 | } 34 | 35 | virtual int deserialize(unsigned char *inbuffer) 36 | { 37 | int offset = 0; 38 | union { 39 | bool real; 40 | uint8_t base; 41 | } u_data; 42 | u_data.base = 0; 43 | u_data.base |= ((uint8_t) (*(inbuffer + offset + 0))) << (8 * 0); 44 | this->data = u_data.real; 45 | offset += sizeof(this->data); 46 | return offset; 47 | } 48 | 49 | const char * getType(){ return "std_msgs/Bool"; }; 50 | const char * getMD5(){ return "8b94c1b53db61fb6aed406028ad6332a"; }; 51 | 52 | }; 53 | 54 | } 55 | #endif -------------------------------------------------------------------------------- /arduino/firmware/lib/ros_lib/std_msgs/Byte.h: -------------------------------------------------------------------------------- 1 | #ifndef _ROS_std_msgs_Byte_h 2 | #define _ROS_std_msgs_Byte_h 3 | 4 | #include 5 | #include 6 | #include 7 | #include "ros/msg.h" 8 | 9 | namespace std_msgs 10 | { 11 | 12 | class Byte : public ros::Msg 13 | { 14 | public: 15 | int8_t data; 16 | 17 | Byte(): 18 | data(0) 19 | { 20 | } 21 | 22 | virtual int serialize(unsigned char *outbuffer) const 23 | { 24 | int offset = 0; 25 | union { 26 | int8_t real; 27 | uint8_t base; 28 | } u_data; 29 | u_data.real = this->data; 30 | *(outbuffer + offset + 0) = (u_data.base >> (8 * 0)) & 0xFF; 31 | offset += sizeof(this->data); 32 | return offset; 33 | } 34 | 35 | virtual int deserialize(unsigned char *inbuffer) 36 | { 37 | int offset = 0; 38 | union { 39 | int8_t real; 40 | uint8_t base; 41 | } u_data; 42 | u_data.base = 0; 43 | u_data.base |= ((uint8_t) (*(inbuffer + offset + 0))) << (8 * 0); 44 | this->data = u_data.real; 45 | offset += sizeof(this->data); 46 | return offset; 47 | } 48 | 49 | const char * getType(){ return "std_msgs/Byte"; }; 50 | const char * getMD5(){ return "ad736a2e8818154c487bb80fe42ce43b"; }; 51 | 52 | }; 53 | 54 | } 55 | #endif -------------------------------------------------------------------------------- /arduino/firmware/lib/ros_lib/std_msgs/Int8.h: -------------------------------------------------------------------------------- 1 | #ifndef _ROS_std_msgs_Int8_h 2 | #define _ROS_std_msgs_Int8_h 3 | 4 | #include 5 | #include 6 | #include 7 | #include "ros/msg.h" 8 | 9 | namespace std_msgs 10 | { 11 | 12 | class Int8 : public ros::Msg 13 | { 14 | public: 15 | int8_t data; 16 | 17 | Int8(): 18 | data(0) 19 | { 20 | } 21 | 22 | virtual int serialize(unsigned char *outbuffer) const 23 | { 24 | int offset = 0; 25 | union { 26 | int8_t real; 27 | uint8_t base; 28 | } u_data; 29 | u_data.real = this->data; 30 | *(outbuffer + offset + 0) = (u_data.base >> (8 * 0)) & 0xFF; 31 | offset += sizeof(this->data); 32 | return offset; 33 | } 34 | 35 | virtual int deserialize(unsigned char *inbuffer) 36 | { 37 | int offset = 0; 38 | union { 39 | int8_t real; 40 | uint8_t base; 41 | } u_data; 42 | u_data.base = 0; 43 | u_data.base |= ((uint8_t) (*(inbuffer + offset + 0))) << (8 * 0); 44 | this->data = u_data.real; 45 | offset += sizeof(this->data); 46 | return offset; 47 | } 48 | 49 | const char * getType(){ return "std_msgs/Int8"; }; 50 | const char * getMD5(){ return "27ffa0c9c4b8fb8492252bcad9e5c57b"; }; 51 | 52 | }; 53 | 54 | } 55 | #endif -------------------------------------------------------------------------------- /arduino/firmware/lib/ros_lib/geometry_msgs/QuaternionStamped.h: -------------------------------------------------------------------------------- 1 | #ifndef _ROS_geometry_msgs_QuaternionStamped_h 2 | #define _ROS_geometry_msgs_QuaternionStamped_h 3 | 4 | #include 5 | #include 6 | #include 7 | #include "ros/msg.h" 8 | #include "std_msgs/Header.h" 9 | #include "geometry_msgs/Quaternion.h" 10 | 11 | namespace geometry_msgs 12 | { 13 | 14 | class QuaternionStamped : public ros::Msg 15 | { 16 | public: 17 | std_msgs::Header header; 18 | geometry_msgs::Quaternion quaternion; 19 | 20 | QuaternionStamped(): 21 | header(), 22 | quaternion() 23 | { 24 | } 25 | 26 | virtual int serialize(unsigned char *outbuffer) const 27 | { 28 | int offset = 0; 29 | offset += this->header.serialize(outbuffer + offset); 30 | offset += this->quaternion.serialize(outbuffer + offset); 31 | return offset; 32 | } 33 | 34 | virtual int deserialize(unsigned char *inbuffer) 35 | { 36 | int offset = 0; 37 | offset += this->header.deserialize(inbuffer + offset); 38 | offset += this->quaternion.deserialize(inbuffer + offset); 39 | return offset; 40 | } 41 | 42 | const char * getType(){ return "geometry_msgs/QuaternionStamped"; }; 43 | const char * getMD5(){ return "e57f1e547e0e1fd13504588ffc8334e2"; }; 44 | 45 | }; 46 | 47 | } 48 | #endif -------------------------------------------------------------------------------- /arduino/firmware/lib/ros_lib/geometry_msgs/PoseWithCovarianceStamped.h: -------------------------------------------------------------------------------- 1 | #ifndef _ROS_geometry_msgs_PoseWithCovarianceStamped_h 2 | #define _ROS_geometry_msgs_PoseWithCovarianceStamped_h 3 | 4 | #include 5 | #include 6 | #include 7 | #include "ros/msg.h" 8 | #include "std_msgs/Header.h" 9 | #include "geometry_msgs/PoseWithCovariance.h" 10 | 11 | namespace geometry_msgs 12 | { 13 | 14 | class PoseWithCovarianceStamped : public ros::Msg 15 | { 16 | public: 17 | std_msgs::Header header; 18 | geometry_msgs::PoseWithCovariance pose; 19 | 20 | PoseWithCovarianceStamped(): 21 | header(), 22 | pose() 23 | { 24 | } 25 | 26 | virtual int serialize(unsigned char *outbuffer) const 27 | { 28 | int offset = 0; 29 | offset += this->header.serialize(outbuffer + offset); 30 | offset += this->pose.serialize(outbuffer + offset); 31 | return offset; 32 | } 33 | 34 | virtual int deserialize(unsigned char *inbuffer) 35 | { 36 | int offset = 0; 37 | offset += this->header.deserialize(inbuffer + offset); 38 | offset += this->pose.deserialize(inbuffer + offset); 39 | return offset; 40 | } 41 | 42 | const char * getType(){ return "geometry_msgs/PoseWithCovarianceStamped"; }; 43 | const char * getMD5(){ return "953b798c0f514ff060a53a3498ce6246"; }; 44 | 45 | }; 46 | 47 | } 48 | #endif -------------------------------------------------------------------------------- /arduino/firmware/lib/ros_lib/geometry_msgs/AccelWithCovarianceStamped.h: -------------------------------------------------------------------------------- 1 | #ifndef _ROS_geometry_msgs_AccelWithCovarianceStamped_h 2 | #define _ROS_geometry_msgs_AccelWithCovarianceStamped_h 3 | 4 | #include 5 | #include 6 | #include 7 | #include "ros/msg.h" 8 | #include "std_msgs/Header.h" 9 | #include "geometry_msgs/AccelWithCovariance.h" 10 | 11 | namespace geometry_msgs 12 | { 13 | 14 | class AccelWithCovarianceStamped : public ros::Msg 15 | { 16 | public: 17 | std_msgs::Header header; 18 | geometry_msgs::AccelWithCovariance accel; 19 | 20 | AccelWithCovarianceStamped(): 21 | header(), 22 | accel() 23 | { 24 | } 25 | 26 | virtual int serialize(unsigned char *outbuffer) const 27 | { 28 | int offset = 0; 29 | offset += this->header.serialize(outbuffer + offset); 30 | offset += this->accel.serialize(outbuffer + offset); 31 | return offset; 32 | } 33 | 34 | virtual int deserialize(unsigned char *inbuffer) 35 | { 36 | int offset = 0; 37 | offset += this->header.deserialize(inbuffer + offset); 38 | offset += this->accel.deserialize(inbuffer + offset); 39 | return offset; 40 | } 41 | 42 | const char * getType(){ return "geometry_msgs/AccelWithCovarianceStamped"; }; 43 | const char * getMD5(){ return "96adb295225031ec8d57fb4251b0a886"; }; 44 | 45 | }; 46 | 47 | } 48 | #endif -------------------------------------------------------------------------------- /arduino/firmware/lib/ros_lib/geometry_msgs/TwistWithCovarianceStamped.h: -------------------------------------------------------------------------------- 1 | #ifndef _ROS_geometry_msgs_TwistWithCovarianceStamped_h 2 | #define _ROS_geometry_msgs_TwistWithCovarianceStamped_h 3 | 4 | #include 5 | #include 6 | #include 7 | #include "ros/msg.h" 8 | #include "std_msgs/Header.h" 9 | #include "geometry_msgs/TwistWithCovariance.h" 10 | 11 | namespace geometry_msgs 12 | { 13 | 14 | class TwistWithCovarianceStamped : public ros::Msg 15 | { 16 | public: 17 | std_msgs::Header header; 18 | geometry_msgs::TwistWithCovariance twist; 19 | 20 | TwistWithCovarianceStamped(): 21 | header(), 22 | twist() 23 | { 24 | } 25 | 26 | virtual int serialize(unsigned char *outbuffer) const 27 | { 28 | int offset = 0; 29 | offset += this->header.serialize(outbuffer + offset); 30 | offset += this->twist.serialize(outbuffer + offset); 31 | return offset; 32 | } 33 | 34 | virtual int deserialize(unsigned char *inbuffer) 35 | { 36 | int offset = 0; 37 | offset += this->header.deserialize(inbuffer + offset); 38 | offset += this->twist.deserialize(inbuffer + offset); 39 | return offset; 40 | } 41 | 42 | const char * getType(){ return "geometry_msgs/TwistWithCovarianceStamped"; }; 43 | const char * getMD5(){ return "8927a1a12fb2607ceea095b2dc440a96"; }; 44 | 45 | }; 46 | 47 | } 48 | #endif -------------------------------------------------------------------------------- /package.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | linorobot_ackermann 4 | 1.0.0 5 | Ackermann Steering ROS Platform 6 | t 7 | BSD 8 | 9 | http://www.linorobot.org 10 | 11 | Juan Miguel Jimeno 12 | 13 | catkin 14 | geometry_msgs 15 | ros_arduino_msgs 16 | roscpp 17 | rospy 18 | rosserial_python 19 | sensor_msgs 20 | std_msgs 21 | tf 22 | geometry_msgs 23 | ros_arduino_msgs 24 | roscpp 25 | rospy 26 | rosserial_python 27 | sensor_msgs 28 | std_msgs 29 | tf 30 | teb_local_planner 31 | costmap_converter 32 | interactive_markers 33 | tf_conversions 34 | 35 | 36 | 37 | 38 | 39 | -------------------------------------------------------------------------------- /arduino/firmware/lib/ros_lib/std_msgs/String.h: -------------------------------------------------------------------------------- 1 | #ifndef _ROS_std_msgs_String_h 2 | #define _ROS_std_msgs_String_h 3 | 4 | #include 5 | #include 6 | #include 7 | #include "ros/msg.h" 8 | 9 | namespace std_msgs 10 | { 11 | 12 | class String : public ros::Msg 13 | { 14 | public: 15 | const char* data; 16 | 17 | String(): 18 | data("") 19 | { 20 | } 21 | 22 | virtual int serialize(unsigned char *outbuffer) const 23 | { 24 | int offset = 0; 25 | uint32_t length_data = strlen(this->data); 26 | memcpy(outbuffer + offset, &length_data, sizeof(uint32_t)); 27 | offset += 4; 28 | memcpy(outbuffer + offset, this->data, length_data); 29 | offset += length_data; 30 | return offset; 31 | } 32 | 33 | virtual int deserialize(unsigned char *inbuffer) 34 | { 35 | int offset = 0; 36 | uint32_t length_data; 37 | memcpy(&length_data, (inbuffer + offset), sizeof(uint32_t)); 38 | offset += 4; 39 | for(unsigned int k= offset; k< offset+length_data; ++k){ 40 | inbuffer[k-1]=inbuffer[k]; 41 | } 42 | inbuffer[offset+length_data-1]=0; 43 | this->data = (char *)(inbuffer + offset-1); 44 | offset += length_data; 45 | return offset; 46 | } 47 | 48 | const char * getType(){ return "std_msgs/String"; }; 49 | const char * getMD5(){ return "992ce8a1687cec8c8bd883ec73ca41d1"; }; 50 | 51 | }; 52 | 53 | } 54 | #endif -------------------------------------------------------------------------------- /arduino/firmware/lib/Encoder/examples/TwoKnobs/TwoKnobs.pde: -------------------------------------------------------------------------------- 1 | /* Encoder Library - TwoKnobs Example 2 | * http://www.pjrc.com/teensy/td_libs_Encoder.html 3 | * 4 | * This example code is in the public domain. 5 | */ 6 | 7 | #include 8 | 9 | // Change these pin numbers to the pins connected to your encoder. 10 | // Best Performance: both pins have interrupt capability 11 | // Good Performance: only the first pin has interrupt capability 12 | // Low Performance: neither pin has interrupt capability 13 | Encoder knobLeft(5, 6); 14 | Encoder knobRight(7, 8); 15 | // avoid using pins with LEDs attached 16 | 17 | void setup() { 18 | Serial.begin(9600); 19 | Serial.println("TwoKnobs Encoder Test:"); 20 | } 21 | 22 | long positionLeft = -999; 23 | long positionRight = -999; 24 | 25 | void loop() { 26 | long newLeft, newRight; 27 | newLeft = knobLeft.read(); 28 | newRight = knobRight.read(); 29 | if (newLeft != positionLeft || newRight != positionRight) { 30 | Serial.print("Left = "); 31 | Serial.print(newLeft); 32 | Serial.print(", Right = "); 33 | Serial.print(newRight); 34 | Serial.println(); 35 | positionLeft = newLeft; 36 | positionRight = newRight; 37 | } 38 | // if a character is sent from the serial monitor, 39 | // reset both back to zero. 40 | if (Serial.available()) { 41 | Serial.read(); 42 | Serial.println("Reset both knobs to zero"); 43 | knobLeft.write(0); 44 | knobRight.write(0); 45 | } 46 | } 47 | -------------------------------------------------------------------------------- /arduino/firmware/lib/ros_lib/std_msgs/UInt32.h: -------------------------------------------------------------------------------- 1 | #ifndef _ROS_std_msgs_UInt32_h 2 | #define _ROS_std_msgs_UInt32_h 3 | 4 | #include 5 | #include 6 | #include 7 | #include "ros/msg.h" 8 | 9 | namespace std_msgs 10 | { 11 | 12 | class UInt32 : public ros::Msg 13 | { 14 | public: 15 | uint32_t data; 16 | 17 | UInt32(): 18 | data(0) 19 | { 20 | } 21 | 22 | virtual int serialize(unsigned char *outbuffer) const 23 | { 24 | int offset = 0; 25 | *(outbuffer + offset + 0) = (this->data >> (8 * 0)) & 0xFF; 26 | *(outbuffer + offset + 1) = (this->data >> (8 * 1)) & 0xFF; 27 | *(outbuffer + offset + 2) = (this->data >> (8 * 2)) & 0xFF; 28 | *(outbuffer + offset + 3) = (this->data >> (8 * 3)) & 0xFF; 29 | offset += sizeof(this->data); 30 | return offset; 31 | } 32 | 33 | virtual int deserialize(unsigned char *inbuffer) 34 | { 35 | int offset = 0; 36 | this->data = ((uint32_t) (*(inbuffer + offset))); 37 | this->data |= ((uint32_t) (*(inbuffer + offset + 1))) << (8 * 1); 38 | this->data |= ((uint32_t) (*(inbuffer + offset + 2))) << (8 * 2); 39 | this->data |= ((uint32_t) (*(inbuffer + offset + 3))) << (8 * 3); 40 | offset += sizeof(this->data); 41 | return offset; 42 | } 43 | 44 | const char * getType(){ return "std_msgs/UInt32"; }; 45 | const char * getMD5(){ return "304a39449588c7f8ce2df6e8001c5fce"; }; 46 | 47 | }; 48 | 49 | } 50 | #endif -------------------------------------------------------------------------------- /arduino/firmware/lib/ros_lib/std_msgs/Int16.h: -------------------------------------------------------------------------------- 1 | #ifndef _ROS_std_msgs_Int16_h 2 | #define _ROS_std_msgs_Int16_h 3 | 4 | #include 5 | #include 6 | #include 7 | #include "ros/msg.h" 8 | 9 | namespace std_msgs 10 | { 11 | 12 | class Int16 : public ros::Msg 13 | { 14 | public: 15 | int16_t data; 16 | 17 | Int16(): 18 | data(0) 19 | { 20 | } 21 | 22 | virtual int serialize(unsigned char *outbuffer) const 23 | { 24 | int offset = 0; 25 | union { 26 | int16_t real; 27 | uint16_t base; 28 | } u_data; 29 | u_data.real = this->data; 30 | *(outbuffer + offset + 0) = (u_data.base >> (8 * 0)) & 0xFF; 31 | *(outbuffer + offset + 1) = (u_data.base >> (8 * 1)) & 0xFF; 32 | offset += sizeof(this->data); 33 | return offset; 34 | } 35 | 36 | virtual int deserialize(unsigned char *inbuffer) 37 | { 38 | int offset = 0; 39 | union { 40 | int16_t real; 41 | uint16_t base; 42 | } u_data; 43 | u_data.base = 0; 44 | u_data.base |= ((uint16_t) (*(inbuffer + offset + 0))) << (8 * 0); 45 | u_data.base |= ((uint16_t) (*(inbuffer + offset + 1))) << (8 * 1); 46 | this->data = u_data.real; 47 | offset += sizeof(this->data); 48 | return offset; 49 | } 50 | 51 | const char * getType(){ return "std_msgs/Int16"; }; 52 | const char * getMD5(){ return "8524586e34fbd7cb1c08c5f5f1ca0e57"; }; 53 | 54 | }; 55 | 56 | } 57 | #endif -------------------------------------------------------------------------------- /arduino/firmware/lib/config/lino_base_config.h: -------------------------------------------------------------------------------- 1 | #ifndef LINO_BASE_CONFIG_H 2 | #define LINO_BASE_CONFIG_H 3 | 4 | #define L298_DRIVER 5 | // #define BTS7960_DRIVER 6 | 7 | #define DEBUG 0 8 | 9 | /*BTS7960 PID 10 | #define K_P 0.05 // P constant 11 | #define K_I 0.5 // I constant 12 | #define K_D 0.1 // D constant 13 | */ 14 | 15 | #define K_P 0.6 // P constant 16 | #define K_I 0.3 // I constant 17 | #define K_D 0.5 // D constant 18 | 19 | // define your robot' specs here 20 | #define MAX_RPM 45 // motor's maximum RPM 21 | #define COUNTS_PER_REV 4000 // wheel encoder's no of ticks per rev 22 | #define WHEEL_DIAMETER 0.15 // wheel's diameter in meters 23 | #define PWM_BITS 8 // PWM Resolution of the microcontroller 24 | #define BASE_WIDTH 0.43 // width of the plate you are using 25 | 26 | // ENCODER PINS 27 | // left side encoders pins 28 | #define MOTOR1_ENCODER_A 15 // front_A 29 | #define MOTOR1_ENCODER_B 14 // front_B 30 | 31 | // right side encoders pins 32 | #define MOTOR2_ENCODER_A 12 // front_A 33 | #define MOTOR2_ENCODER_B 11 // front_B 34 | 35 | #ifdef L298_DRIVER 36 | //left side motor pins 37 | #define MOTOR1_PWM 21 38 | #define MOTOR1_IN_A 20 39 | #define MOTOR1_IN_B 1 40 | 41 | //right side motor pins 42 | #define MOTOR2_PWM 5 43 | #define MOTOR2_IN_A 8 44 | #define MOTOR2_IN_B 6 45 | #endif 46 | 47 | #ifdef BTS7960_DRIVER 48 | // left side motor pins 49 | #define MOTOR1_IN_A 21 50 | #define MOTOR1_IN_B 20 51 | 52 | // right side motor pins 53 | #define MOTOR2_IN_A 5 54 | #define MOTOR2_IN_B 8 55 | #endif 56 | 57 | #define STEERING_PIN 22 58 | 59 | #endif 60 | -------------------------------------------------------------------------------- /arduino/firmware/lib/imu/magnetometer_HMC5883L.h: -------------------------------------------------------------------------------- 1 | #ifndef _MAGNETOMETER_HMC5883L_H_ 2 | #define _MAGNETOMETER_HMC5883L_H_ 3 | 4 | //HMC5883L REGISTERS 5 | #define HMC5883L_MAG_ADDRESS 0x1E 6 | #define HMC5883L_MAG_ID 0x10 7 | #define HMC5883L_MAG_REG_A 0x00 8 | #define HMC5883L_MAG_REG_B 0x01 9 | #define HMC5883L_MAG_MODE 0x02 10 | #define HMC5883L_MAG_DATAX0 0x03 11 | #define HMC5883L_MAG_GAIN 0x20 //Default gain 12 | #define HMC5883L_MAG_SCALE 0.92 // mG/LSb 13 | 14 | bool checkMagnetometer() 15 | { 16 | write_to_register(HMC5883L_MAG_ADDRESS,HMC5883L_MAG_REG_B,HMC5883L_MAG_GAIN); //Sets the gain 17 | delay(5); 18 | write_to_register(HMC5883L_MAG_ADDRESS,HMC5883L_MAG_REG_A,0x18); //75Hz output 19 | delay(5); 20 | write_to_register(HMC5883L_MAG_ADDRESS,HMC5883L_MAG_MODE,0x01); //Single-Measurement Mode 21 | delay(5); 22 | return true;; 23 | } 24 | 25 | void measureMagnetometer() 26 | { 27 | mag_reads = 0; 28 | send_value(HMC5883L_MAG_ADDRESS,HMC5883L_MAG_DATAX0); 29 | Wire.requestFrom(HMC5883L_MAG_ADDRESS,6); 30 | while(Wire.available()) 31 | { 32 | mag_buffer[mag_reads] = Wire.read(); 33 | mag_reads++; 34 | } 35 | raw_magnetic_field.x = (float)(MAG_X_INVERT * ((int16_t)((int)mag_buffer[2*MAG_X_AXIS] << 8) | (mag_buffer[2*MAG_X_AXIS+1]))) * HMC5883L_MAG_SCALE; 36 | raw_magnetic_field.y = (float)(MAG_Y_INVERT * ((int16_t)((int)mag_buffer[2*MAG_Y_AXIS] << 8) | (mag_buffer[2*MAG_Y_AXIS+1]))) * HMC5883L_MAG_SCALE; 37 | raw_magnetic_field.z = (float)(MAG_Z_INVERT * ((int16_t)((int)mag_buffer[2*MAG_Z_AXIS] << 8) | (mag_buffer[2*MAG_Z_AXIS+1]))) * HMC5883L_MAG_SCALE; 38 | write_to_register(HMC5883L_MAG_ADDRESS,HMC5883L_MAG_MODE,0x01); 39 | 40 | } 41 | 42 | #endif // _MAGNETOMETER_HMC5883L_H_ 43 | -------------------------------------------------------------------------------- /arduino/firmware/lib/imu/gyroscope_L3G4200D.h: -------------------------------------------------------------------------------- 1 | // FINISH THIS AND TEST 2 | #ifndef _GYROSCOPE_L3G4200D_H_ 3 | #define _GYROSCOPE_L3G4200D_H_ 4 | 5 | //L3G4200D REGISTERS 6 | #define L3G4200D_GYRO_ADDRESS 0x69 7 | #define L3G4200D_WHO_AM_I 0x0F 8 | #define L3G4200D_WHO_AM_I_VALUE 0xd3 9 | #define L3G4200D_CTRL_REG1 0x20 10 | #define L3G4200D_CTRL_REG4 0x23 11 | #define L3G4200D_CTRL_REG5 0x24 12 | #define L3G4200D_SCALE 939.275077264 //rad/s 13 | 14 | bool checkGyroscope() 15 | { 16 | if (check_ID(L3G4200D_GYRO_ADDRESS,L3G4200D_WHO_AM_I) == L3G4200D_WHO_AM_I_VALUE) 17 | { 18 | write_to_register(L3G4200D_GYRO_ADDRESS,L3G4200D_CTRL_REG1,0x0F); //xyz and no power down 19 | delay(10); 20 | write_to_register(L3G4200D_GYRO_ADDRESS,L3G4200D_CTRL_REG4,0x30); //full scale at 2000 dps 21 | delay(10); 22 | write_to_register(L3G4200D_GYRO_ADDRESS,L3G4200D_CTRL_REG5,0xf0); // hpf 23 | delay(20); 24 | return true; 25 | } 26 | else 27 | return false; 28 | } 29 | 30 | void measureGyroscope() 31 | { 32 | gyro_reads = 0; 33 | send_value(L3G4200D_GYRO_ADDRESS,0x80 | 0x28); 34 | Wire.requestFrom(L3G4200D_GYRO_ADDRESS,6); 35 | while(Wire.available()) 36 | { 37 | gyro_buffer[gyro_reads] = Wire.read(); 38 | gyro_reads++; 39 | } 40 | raw_rotation.x = (float)(GYRO_X_INVERT*(int16_t)(((int)gyro_buffer[2*GYRO_X_AXIS+1] <<8) | gyro_buffer[2*GYRO_X_AXIS])) / L3G4200D_SCALE; //rad/s 41 | raw_rotation.y = (float)(GYRO_Y_INVERT*(int16_t)(((int)gyro_buffer[2*GYRO_Y_AXIS+1] <<8) | gyro_buffer[2*GYRO_Y_AXIS])) / L3G4200D_SCALE; 42 | raw_rotation.z = (float)(GYRO_Z_INVERT*(int16_t)(((int)gyro_buffer[2*GYRO_Z_AXIS+1] <<8) | gyro_buffer[2*GYRO_Z_AXIS])) / L3G4200D_SCALE; 43 | 44 | 45 | } 46 | 47 | #endif // _GYROSCOPE_L3G4200D_H_ 48 | -------------------------------------------------------------------------------- /launch/slam.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | 16 | 17 | 18 | 19 | 20 | 21 | 22 | 23 | 24 | 25 | 26 | 27 | 28 | 29 | 30 | 31 | 32 | 33 | 34 | 35 | 36 | 37 | 38 | 39 | 40 | -------------------------------------------------------------------------------- /arduino/firmware/lib/imu/gyroscope_ITG3205.h: -------------------------------------------------------------------------------- 1 | #ifndef _GYROSCOPE_ITFG3205_H_ 2 | #define _GYROSCOPE_ITFG3205_H_ 3 | 4 | //ITG3205 REGISTERS 5 | #define ITG3205_GYRO_ADDRESS 0x68 6 | #define ITG3205_WHO_AM_I 0x00 7 | #define ITG3205_PWR_MGM 0x3E 8 | #define ITG3205_RESET 0x80 9 | #define ITG3205_DLPF_FS 0x16 10 | #define ITG3205_SCALE 0.00121414209 //rad/s 11 | 12 | bool checkGyroscope() 13 | { 14 | if ((check_ID(ITG3205_GYRO_ADDRESS,ITG3205_WHO_AM_I) & 0x7E) == ITG3205_GYRO_ADDRESS) 15 | { 16 | write_to_register(ITG3205_GYRO_ADDRESS,ITG3205_PWR_MGM,ITG3205_RESET); //reset the gyro 17 | delay(5); 18 | write_to_register(ITG3205_GYRO_ADDRESS,ITG3205_DLPF_FS,0x1B); // Set LP filter bandwidth to 42Hz 19 | delay(5); 20 | write_to_register(ITG3205_GYRO_ADDRESS,0x15,0x13); // sample rate to to 50Hz 21 | delay(5); 22 | write_to_register(ITG3205_GYRO_ADDRESS,ITG3205_PWR_MGM,0x03); // PLL with Z gyro ref 23 | delay(10); 24 | return true; 25 | } 26 | else 27 | return false; 28 | } 29 | 30 | void measureGyroscope() 31 | { 32 | gyro_reads = 0;; 33 | send_value(ITG3205_GYRO_ADDRESS,0x1D); 34 | Wire.requestFrom(ITG3205_GYRO_ADDRESS,6); 35 | while(Wire.available()) 36 | { 37 | gyro_buffer[gyro_reads] = Wire.read(); 38 | gyro_reads++; 39 | } 40 | raw_rotation.x = (float)(GYRO_X_INVERT*(int16_t)(((int)gyro_buffer[2*GYRO_X_AXIS] <<8) | gyro_buffer[2*GYRO_X_AXIS+1])) * ITG3205_SCALE; //rad/s 41 | raw_rotation.y = (float)(GYRO_Y_INVERT*(int16_t)(((int)gyro_buffer[2*GYRO_Y_AXIS] <<8) | gyro_buffer[2*GYRO_Y_AXIS+1])) * ITG3205_SCALE; 42 | raw_rotation.z = (float)(GYRO_Z_INVERT*(int16_t)(((int)gyro_buffer[2*GYRO_Z_AXIS] <<8) | gyro_buffer[2*GYRO_Z_AXIS+1])) * ITG3205_SCALE; 43 | 44 | 45 | } 46 | #endif // _GYROSCOPE_ITFG3205_H_ 47 | -------------------------------------------------------------------------------- /arduino/firmware/lib/ros_lib/std_msgs/Int32.h: -------------------------------------------------------------------------------- 1 | #ifndef _ROS_std_msgs_Int32_h 2 | #define _ROS_std_msgs_Int32_h 3 | 4 | #include 5 | #include 6 | #include 7 | #include "ros/msg.h" 8 | 9 | namespace std_msgs 10 | { 11 | 12 | class Int32 : public ros::Msg 13 | { 14 | public: 15 | int32_t data; 16 | 17 | Int32(): 18 | data(0) 19 | { 20 | } 21 | 22 | virtual int serialize(unsigned char *outbuffer) const 23 | { 24 | int offset = 0; 25 | union { 26 | int32_t real; 27 | uint32_t base; 28 | } u_data; 29 | u_data.real = this->data; 30 | *(outbuffer + offset + 0) = (u_data.base >> (8 * 0)) & 0xFF; 31 | *(outbuffer + offset + 1) = (u_data.base >> (8 * 1)) & 0xFF; 32 | *(outbuffer + offset + 2) = (u_data.base >> (8 * 2)) & 0xFF; 33 | *(outbuffer + offset + 3) = (u_data.base >> (8 * 3)) & 0xFF; 34 | offset += sizeof(this->data); 35 | return offset; 36 | } 37 | 38 | virtual int deserialize(unsigned char *inbuffer) 39 | { 40 | int offset = 0; 41 | union { 42 | int32_t real; 43 | uint32_t base; 44 | } u_data; 45 | u_data.base = 0; 46 | u_data.base |= ((uint32_t) (*(inbuffer + offset + 0))) << (8 * 0); 47 | u_data.base |= ((uint32_t) (*(inbuffer + offset + 1))) << (8 * 1); 48 | u_data.base |= ((uint32_t) (*(inbuffer + offset + 2))) << (8 * 2); 49 | u_data.base |= ((uint32_t) (*(inbuffer + offset + 3))) << (8 * 3); 50 | this->data = u_data.real; 51 | offset += sizeof(this->data); 52 | return offset; 53 | } 54 | 55 | const char * getType(){ return "std_msgs/Int32"; }; 56 | const char * getMD5(){ return "da5909fbe378aeaf85e547e830cc1bb7"; }; 57 | 58 | }; 59 | 60 | } 61 | #endif -------------------------------------------------------------------------------- /arduino/firmware/lib/ros_lib/std_msgs/Float32.h: -------------------------------------------------------------------------------- 1 | #ifndef _ROS_std_msgs_Float32_h 2 | #define _ROS_std_msgs_Float32_h 3 | 4 | #include 5 | #include 6 | #include 7 | #include "ros/msg.h" 8 | 9 | namespace std_msgs 10 | { 11 | 12 | class Float32 : public ros::Msg 13 | { 14 | public: 15 | float data; 16 | 17 | Float32(): 18 | data(0) 19 | { 20 | } 21 | 22 | virtual int serialize(unsigned char *outbuffer) const 23 | { 24 | int offset = 0; 25 | union { 26 | float real; 27 | uint32_t base; 28 | } u_data; 29 | u_data.real = this->data; 30 | *(outbuffer + offset + 0) = (u_data.base >> (8 * 0)) & 0xFF; 31 | *(outbuffer + offset + 1) = (u_data.base >> (8 * 1)) & 0xFF; 32 | *(outbuffer + offset + 2) = (u_data.base >> (8 * 2)) & 0xFF; 33 | *(outbuffer + offset + 3) = (u_data.base >> (8 * 3)) & 0xFF; 34 | offset += sizeof(this->data); 35 | return offset; 36 | } 37 | 38 | virtual int deserialize(unsigned char *inbuffer) 39 | { 40 | int offset = 0; 41 | union { 42 | float real; 43 | uint32_t base; 44 | } u_data; 45 | u_data.base = 0; 46 | u_data.base |= ((uint32_t) (*(inbuffer + offset + 0))) << (8 * 0); 47 | u_data.base |= ((uint32_t) (*(inbuffer + offset + 1))) << (8 * 1); 48 | u_data.base |= ((uint32_t) (*(inbuffer + offset + 2))) << (8 * 2); 49 | u_data.base |= ((uint32_t) (*(inbuffer + offset + 3))) << (8 * 3); 50 | this->data = u_data.real; 51 | offset += sizeof(this->data); 52 | return offset; 53 | } 54 | 55 | const char * getType(){ return "std_msgs/Float32"; }; 56 | const char * getMD5(){ return "73fcbf46b49191e672908e50842a83d4"; }; 57 | 58 | }; 59 | 60 | } 61 | #endif -------------------------------------------------------------------------------- /arduino/firmware/lib/Encoder/examples/NoInterrupts/NoInterrupts.pde: -------------------------------------------------------------------------------- 1 | /* Encoder Library - NoInterrupts Example 2 | * http://www.pjrc.com/teensy/td_libs_Encoder.html 3 | * 4 | * This example code is in the public domain. 5 | */ 6 | 7 | // If you define ENCODER_DO_NOT_USE_INTERRUPTS *before* including 8 | // Encoder, the library will never use interrupts. This is mainly 9 | // useful to reduce the size of the library when you are using it 10 | // with pins that do not support interrupts. Without interrupts, 11 | // your program must call the read() function rapidly, or risk 12 | // missing changes in position. 13 | #define ENCODER_DO_NOT_USE_INTERRUPTS 14 | #include 15 | 16 | // Beware of Serial.print() speed. Without interrupts, if you 17 | // transmit too much data with Serial.print() it can slow your 18 | // reading from Encoder. Arduino 1.0 has improved transmit code. 19 | // Using the fastest baud rate also helps. Teensy has USB packet 20 | // buffering. But all boards can experience problems if you print 21 | // too much and fill up buffers. 22 | 23 | // Change these two numbers to the pins connected to your encoder. 24 | // With ENCODER_DO_NOT_USE_INTERRUPTS, no interrupts are ever 25 | // used, even if the pin has interrupt capability 26 | Encoder myEnc(5, 6); 27 | // avoid using pins with LEDs attached 28 | 29 | void setup() { 30 | Serial.begin(9600); 31 | Serial.println("Basic NoInterrupts Test:"); 32 | } 33 | 34 | long position = -999; 35 | 36 | void loop() { 37 | long newPos = myEnc.read(); 38 | if (newPos != position) { 39 | position = newPos; 40 | Serial.println(position); 41 | } 42 | // With any substantial delay added, Encoder can only track 43 | // very slow motion. You may uncomment this line to see 44 | // how badly a delay affects your encoder. 45 | //delay(50); 46 | } 47 | -------------------------------------------------------------------------------- /arduino/firmware/lib/imu/accelerometer_ADXL345.h: -------------------------------------------------------------------------------- 1 | #ifndef _ACCELEROMETER_ADXL345_H_ 2 | #define _ACCELEROMETER_ADXL345_H_ 3 | 4 | #include "accelerometer.h" 5 | 6 | //ADXL345 REGISTERS 7 | #define ADXL345_DEVID 0x00 8 | #define ADXL345_BW_RATE 0x2C 9 | #define ADXL345_POWER_CTL 0x2D 10 | #define ADXL345_DATA_FORMAT 0x31 11 | #define ADXL345_DATAX0 0x32 12 | #define ADXL345_ACCELEROMETER_ADDRESS 0x53 13 | #define ADXL345_DEVICE_ID 0xE5 14 | #define ADXL345_SCALE 25.60000 15 | 16 | bool checkAccelerometer() 17 | { 18 | if (check_ID(ADXL345_ACCELEROMETER_ADDRESS,ADXL345_DEVID) == ADXL345_DEVICE_ID) 19 | { 20 | write_to_register(ADXL345_ACCELEROMETER_ADDRESS,ADXL345_POWER_CTL,0x08); //D3, enables measuring 21 | delay(5); 22 | write_to_register(ADXL345_ACCELEROMETER_ADDRESS,ADXL345_DATA_FORMAT,0x09); //D3 and D0, enables FULL_RES and +/-4g 23 | delay(5); 24 | write_to_register(ADXL345_ACCELEROMETER_ADDRESS,ADXL345_BW_RATE,0x09); //Set the bw to 0 Hz 25 | delay(5); 26 | return true; 27 | } 28 | else 29 | return false; 30 | } 31 | 32 | void measureAcceleration() 33 | { 34 | acc_reads = 0; 35 | send_value(ADXL345_ACCELEROMETER_ADDRESS, ADXL345_DATAX0); 36 | Wire.requestFrom(ADXL345_ACCELEROMETER_ADDRESS, 6); 37 | while(Wire.available()) 38 | { 39 | acc_buffer[acc_reads] = Wire.read(); 40 | acc_reads++; 41 | } 42 | 43 | raw_acceleration.x = ((float)ACC_X_INVERT*(int16_t)((int)acc_buffer[2*ACC_X_AXIS+1]<<8 | acc_buffer[2*ACC_X_AXIS]) / ADXL345_SCALE); 44 | raw_acceleration.y = ((float)ACC_Y_INVERT*(int16_t)((int)acc_buffer[2*ACC_Y_AXIS+1]<<8 | acc_buffer[2*ACC_Y_AXIS]) / ADXL345_SCALE); 45 | raw_acceleration.z = ((float)ACC_Z_INVERT*(int16_t)((int)acc_buffer[2*ACC_Z_AXIS+1]<<8 | acc_buffer[2*ACC_Z_AXIS]) / ADXL345_SCALE); 46 | 47 | } 48 | 49 | #endif // _ACCELEROMETER_ADXL345_H_ 50 | -------------------------------------------------------------------------------- /arduino/firmware/lib/ros_lib/geometry_msgs/Polygon.h: -------------------------------------------------------------------------------- 1 | #ifndef _ROS_geometry_msgs_Polygon_h 2 | #define _ROS_geometry_msgs_Polygon_h 3 | 4 | #include 5 | #include 6 | #include 7 | #include "ros/msg.h" 8 | #include "geometry_msgs/Point32.h" 9 | 10 | namespace geometry_msgs 11 | { 12 | 13 | class Polygon : public ros::Msg 14 | { 15 | public: 16 | uint8_t points_length; 17 | geometry_msgs::Point32 st_points; 18 | geometry_msgs::Point32 * points; 19 | 20 | Polygon(): 21 | points_length(0), points(NULL) 22 | { 23 | } 24 | 25 | virtual int serialize(unsigned char *outbuffer) const 26 | { 27 | int offset = 0; 28 | *(outbuffer + offset++) = points_length; 29 | *(outbuffer + offset++) = 0; 30 | *(outbuffer + offset++) = 0; 31 | *(outbuffer + offset++) = 0; 32 | for( uint8_t i = 0; i < points_length; i++){ 33 | offset += this->points[i].serialize(outbuffer + offset); 34 | } 35 | return offset; 36 | } 37 | 38 | virtual int deserialize(unsigned char *inbuffer) 39 | { 40 | int offset = 0; 41 | uint8_t points_lengthT = *(inbuffer + offset++); 42 | if(points_lengthT > points_length) 43 | this->points = (geometry_msgs::Point32*)realloc(this->points, points_lengthT * sizeof(geometry_msgs::Point32)); 44 | offset += 3; 45 | points_length = points_lengthT; 46 | for( uint8_t i = 0; i < points_length; i++){ 47 | offset += this->st_points.deserialize(inbuffer + offset); 48 | memcpy( &(this->points[i]), &(this->st_points), sizeof(geometry_msgs::Point32)); 49 | } 50 | return offset; 51 | } 52 | 53 | const char * getType(){ return "geometry_msgs/Polygon"; }; 54 | const char * getMD5(){ return "cd60a26494a087f577976f0329fa120e"; }; 55 | 56 | }; 57 | 58 | } 59 | #endif -------------------------------------------------------------------------------- /arduino/firmware/lib/ros_lib/rosserial_msgs/Log.h: -------------------------------------------------------------------------------- 1 | #ifndef _ROS_rosserial_msgs_Log_h 2 | #define _ROS_rosserial_msgs_Log_h 3 | 4 | #include 5 | #include 6 | #include 7 | #include "ros/msg.h" 8 | 9 | namespace rosserial_msgs 10 | { 11 | 12 | class Log : public ros::Msg 13 | { 14 | public: 15 | uint8_t level; 16 | const char* msg; 17 | enum { ROSDEBUG = 0 }; 18 | enum { INFO = 1 }; 19 | enum { WARN = 2 }; 20 | enum { ERROR = 3 }; 21 | enum { FATAL = 4 }; 22 | 23 | Log(): 24 | level(0), 25 | msg("") 26 | { 27 | } 28 | 29 | virtual int serialize(unsigned char *outbuffer) const 30 | { 31 | int offset = 0; 32 | *(outbuffer + offset + 0) = (this->level >> (8 * 0)) & 0xFF; 33 | offset += sizeof(this->level); 34 | uint32_t length_msg = strlen(this->msg); 35 | memcpy(outbuffer + offset, &length_msg, sizeof(uint32_t)); 36 | offset += 4; 37 | memcpy(outbuffer + offset, this->msg, length_msg); 38 | offset += length_msg; 39 | return offset; 40 | } 41 | 42 | virtual int deserialize(unsigned char *inbuffer) 43 | { 44 | int offset = 0; 45 | this->level = ((uint8_t) (*(inbuffer + offset))); 46 | offset += sizeof(this->level); 47 | uint32_t length_msg; 48 | memcpy(&length_msg, (inbuffer + offset), sizeof(uint32_t)); 49 | offset += 4; 50 | for(unsigned int k= offset; k< offset+length_msg; ++k){ 51 | inbuffer[k-1]=inbuffer[k]; 52 | } 53 | inbuffer[offset+length_msg-1]=0; 54 | this->msg = (char *)(inbuffer + offset-1); 55 | offset += length_msg; 56 | return offset; 57 | } 58 | 59 | const char * getType(){ return "rosserial_msgs/Log"; }; 60 | const char * getMD5(){ return "11abd731c25933261cd6183bd12d6295"; }; 61 | 62 | }; 63 | 64 | } 65 | #endif -------------------------------------------------------------------------------- /arduino/firmware/lib/ros_lib/sensor_msgs/JoyFeedbackArray.h: -------------------------------------------------------------------------------- 1 | #ifndef _ROS_sensor_msgs_JoyFeedbackArray_h 2 | #define _ROS_sensor_msgs_JoyFeedbackArray_h 3 | 4 | #include 5 | #include 6 | #include 7 | #include "ros/msg.h" 8 | #include "sensor_msgs/JoyFeedback.h" 9 | 10 | namespace sensor_msgs 11 | { 12 | 13 | class JoyFeedbackArray : public ros::Msg 14 | { 15 | public: 16 | uint8_t array_length; 17 | sensor_msgs::JoyFeedback st_array; 18 | sensor_msgs::JoyFeedback * array; 19 | 20 | JoyFeedbackArray(): 21 | array_length(0), array(NULL) 22 | { 23 | } 24 | 25 | virtual int serialize(unsigned char *outbuffer) const 26 | { 27 | int offset = 0; 28 | *(outbuffer + offset++) = array_length; 29 | *(outbuffer + offset++) = 0; 30 | *(outbuffer + offset++) = 0; 31 | *(outbuffer + offset++) = 0; 32 | for( uint8_t i = 0; i < array_length; i++){ 33 | offset += this->array[i].serialize(outbuffer + offset); 34 | } 35 | return offset; 36 | } 37 | 38 | virtual int deserialize(unsigned char *inbuffer) 39 | { 40 | int offset = 0; 41 | uint8_t array_lengthT = *(inbuffer + offset++); 42 | if(array_lengthT > array_length) 43 | this->array = (sensor_msgs::JoyFeedback*)realloc(this->array, array_lengthT * sizeof(sensor_msgs::JoyFeedback)); 44 | offset += 3; 45 | array_length = array_lengthT; 46 | for( uint8_t i = 0; i < array_length; i++){ 47 | offset += this->st_array.deserialize(inbuffer + offset); 48 | memcpy( &(this->array[i]), &(this->st_array), sizeof(sensor_msgs::JoyFeedback)); 49 | } 50 | return offset; 51 | } 52 | 53 | const char * getType(){ return "sensor_msgs/JoyFeedbackArray"; }; 54 | const char * getMD5(){ return "cde5730a895b1fc4dee6f91b754b213d"; }; 55 | 56 | }; 57 | 58 | } 59 | #endif -------------------------------------------------------------------------------- /arduino/firmware/lib/ros_lib/tf/tfMessage.h: -------------------------------------------------------------------------------- 1 | #ifndef _ROS_tf_tfMessage_h 2 | #define _ROS_tf_tfMessage_h 3 | 4 | #include 5 | #include 6 | #include 7 | #include "ros/msg.h" 8 | #include "geometry_msgs/TransformStamped.h" 9 | 10 | namespace tf 11 | { 12 | 13 | class tfMessage : public ros::Msg 14 | { 15 | public: 16 | uint8_t transforms_length; 17 | geometry_msgs::TransformStamped st_transforms; 18 | geometry_msgs::TransformStamped * transforms; 19 | 20 | tfMessage(): 21 | transforms_length(0), transforms(NULL) 22 | { 23 | } 24 | 25 | virtual int serialize(unsigned char *outbuffer) const 26 | { 27 | int offset = 0; 28 | *(outbuffer + offset++) = transforms_length; 29 | *(outbuffer + offset++) = 0; 30 | *(outbuffer + offset++) = 0; 31 | *(outbuffer + offset++) = 0; 32 | for( uint8_t i = 0; i < transforms_length; i++){ 33 | offset += this->transforms[i].serialize(outbuffer + offset); 34 | } 35 | return offset; 36 | } 37 | 38 | virtual int deserialize(unsigned char *inbuffer) 39 | { 40 | int offset = 0; 41 | uint8_t transforms_lengthT = *(inbuffer + offset++); 42 | if(transforms_lengthT > transforms_length) 43 | this->transforms = (geometry_msgs::TransformStamped*)realloc(this->transforms, transforms_lengthT * sizeof(geometry_msgs::TransformStamped)); 44 | offset += 3; 45 | transforms_length = transforms_lengthT; 46 | for( uint8_t i = 0; i < transforms_length; i++){ 47 | offset += this->st_transforms.deserialize(inbuffer + offset); 48 | memcpy( &(this->transforms[i]), &(this->st_transforms), sizeof(geometry_msgs::TransformStamped)); 49 | } 50 | return offset; 51 | } 52 | 53 | const char * getType(){ return "tf/tfMessage"; }; 54 | const char * getMD5(){ return "94810edda583a504dfda3829e70d7eec"; }; 55 | 56 | }; 57 | 58 | } 59 | #endif -------------------------------------------------------------------------------- /arduino/firmware/lib/motor/motor.ino: -------------------------------------------------------------------------------- 1 | /* 2 | Copyright (c) 2016, Juan Jimeno 3 | Source: http://research.ijcaonline.org/volume113/number3/pxc3901586.pdf 4 | All rights reserved. 5 | Redistribution and use in source and binary forms, with or without 6 | modification, are permitted provided that the following conditions are met: 7 | * Redistributions of source code must retain the above copyright notice, 8 | this list of conditions and the following disclaimer. 9 | * Redistributions in binary form must reproduce the above copyright 10 | notice, this list of conditions and the following disclaimer in the 11 | documentation and/or other materials provided with the distribution. 12 | * Neither the name of nor the names of its contributors may be used to 13 | endorse or promote products derived from this software without specific 14 | prior written permission. 15 | THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" 16 | AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE 17 | IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE 18 | ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE 19 | LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR 20 | CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF 21 | SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS 22 | INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN 23 | CONTRACT, STRICT LIABILITY, OR TORTPPIPI (INCLUDING NEGLIGENCE OR OTHERWISE) 24 | ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE 25 | POSSIBILITY OF SUCH DAMAGE. 26 | */ 27 | 28 | #include "Motor.h" 29 | Motor motor1(12,12); 30 | int Motor::counts_per_rev_ = 0; 31 | long ticks = 0; 32 | 33 | void setup() { 34 | Serial.begin(9600); 35 | // Motor::initEncoder(10); 36 | } 37 | 38 | void loop() { 39 | motor1.updateSpeed(ticks); 40 | Serial.println(motor1.current_rpm); 41 | ticks = ticks + 20; 42 | delay(1000); 43 | 44 | } 45 | -------------------------------------------------------------------------------- /arduino/firmware/lib/ros_lib/geometry_msgs/PoseArray.h: -------------------------------------------------------------------------------- 1 | #ifndef _ROS_geometry_msgs_PoseArray_h 2 | #define _ROS_geometry_msgs_PoseArray_h 3 | 4 | #include 5 | #include 6 | #include 7 | #include "ros/msg.h" 8 | #include "std_msgs/Header.h" 9 | #include "geometry_msgs/Pose.h" 10 | 11 | namespace geometry_msgs 12 | { 13 | 14 | class PoseArray : public ros::Msg 15 | { 16 | public: 17 | std_msgs::Header header; 18 | uint8_t poses_length; 19 | geometry_msgs::Pose st_poses; 20 | geometry_msgs::Pose * poses; 21 | 22 | PoseArray(): 23 | header(), 24 | poses_length(0), poses(NULL) 25 | { 26 | } 27 | 28 | virtual int serialize(unsigned char *outbuffer) const 29 | { 30 | int offset = 0; 31 | offset += this->header.serialize(outbuffer + offset); 32 | *(outbuffer + offset++) = poses_length; 33 | *(outbuffer + offset++) = 0; 34 | *(outbuffer + offset++) = 0; 35 | *(outbuffer + offset++) = 0; 36 | for( uint8_t i = 0; i < poses_length; i++){ 37 | offset += this->poses[i].serialize(outbuffer + offset); 38 | } 39 | return offset; 40 | } 41 | 42 | virtual int deserialize(unsigned char *inbuffer) 43 | { 44 | int offset = 0; 45 | offset += this->header.deserialize(inbuffer + offset); 46 | uint8_t poses_lengthT = *(inbuffer + offset++); 47 | if(poses_lengthT > poses_length) 48 | this->poses = (geometry_msgs::Pose*)realloc(this->poses, poses_lengthT * sizeof(geometry_msgs::Pose)); 49 | offset += 3; 50 | poses_length = poses_lengthT; 51 | for( uint8_t i = 0; i < poses_length; i++){ 52 | offset += this->st_poses.deserialize(inbuffer + offset); 53 | memcpy( &(this->poses[i]), &(this->st_poses), sizeof(geometry_msgs::Pose)); 54 | } 55 | return offset; 56 | } 57 | 58 | const char * getType(){ return "geometry_msgs/PoseArray"; }; 59 | const char * getMD5(){ return "916c28c5764443f268b296bb671b9d97"; }; 60 | 61 | }; 62 | 63 | } 64 | #endif -------------------------------------------------------------------------------- /arduino/firmware/lib/ros_lib/std_msgs/UInt8MultiArray.h: -------------------------------------------------------------------------------- 1 | #ifndef _ROS_std_msgs_UInt8MultiArray_h 2 | #define _ROS_std_msgs_UInt8MultiArray_h 3 | 4 | #include 5 | #include 6 | #include 7 | #include "ros/msg.h" 8 | #include "std_msgs/MultiArrayLayout.h" 9 | 10 | namespace std_msgs 11 | { 12 | 13 | class UInt8MultiArray : public ros::Msg 14 | { 15 | public: 16 | std_msgs::MultiArrayLayout layout; 17 | uint8_t data_length; 18 | uint8_t st_data; 19 | uint8_t * data; 20 | 21 | UInt8MultiArray(): 22 | layout(), 23 | data_length(0), data(NULL) 24 | { 25 | } 26 | 27 | virtual int serialize(unsigned char *outbuffer) const 28 | { 29 | int offset = 0; 30 | offset += this->layout.serialize(outbuffer + offset); 31 | *(outbuffer + offset++) = data_length; 32 | *(outbuffer + offset++) = 0; 33 | *(outbuffer + offset++) = 0; 34 | *(outbuffer + offset++) = 0; 35 | for( uint8_t i = 0; i < data_length; i++){ 36 | *(outbuffer + offset + 0) = (this->data[i] >> (8 * 0)) & 0xFF; 37 | offset += sizeof(this->data[i]); 38 | } 39 | return offset; 40 | } 41 | 42 | virtual int deserialize(unsigned char *inbuffer) 43 | { 44 | int offset = 0; 45 | offset += this->layout.deserialize(inbuffer + offset); 46 | uint8_t data_lengthT = *(inbuffer + offset++); 47 | if(data_lengthT > data_length) 48 | this->data = (uint8_t*)realloc(this->data, data_lengthT * sizeof(uint8_t)); 49 | offset += 3; 50 | data_length = data_lengthT; 51 | for( uint8_t i = 0; i < data_length; i++){ 52 | this->st_data = ((uint8_t) (*(inbuffer + offset))); 53 | offset += sizeof(this->st_data); 54 | memcpy( &(this->data[i]), &(this->st_data), sizeof(uint8_t)); 55 | } 56 | return offset; 57 | } 58 | 59 | const char * getType(){ return "std_msgs/UInt8MultiArray"; }; 60 | const char * getMD5(){ return "82373f1612381bb6ee473b5cd6f5d89c"; }; 61 | 62 | }; 63 | 64 | } 65 | #endif -------------------------------------------------------------------------------- /arduino/firmware/lib/motor/Motor.cpp: -------------------------------------------------------------------------------- 1 | #include "Arduino.h" 2 | #include "Motor.h" 3 | 4 | Motor::Motor(int pwm_pin, int motor_pinA, int motor_pinB) 5 | { 6 | motor_driver_ = L298; 7 | 8 | pinMode(pwm_pin, OUTPUT); 9 | pinMode(motor_pinA, OUTPUT); 10 | pinMode(motor_pinB, OUTPUT); 11 | 12 | pwm_pin_ = pwm_pin; 13 | motor_pinA_ = motor_pinA; 14 | motor_pinB_ = motor_pinB; 15 | } 16 | 17 | Motor::Motor(int motor_pinA, int motor_pinB) 18 | { 19 | motor_driver_ = BTS7960; 20 | 21 | pinMode(motor_pinA, OUTPUT); 22 | pinMode(motor_pinB, OUTPUT); 23 | 24 | motor_pinA_ = motor_pinA; 25 | motor_pinB_ = motor_pinB; 26 | } 27 | 28 | void Motor::updateSpeed(long encoder_ticks) 29 | { 30 | //this function calculates the motor's RPM based on encoder ticks and delta time 31 | unsigned long current_time = millis(); 32 | unsigned long dt = current_time - prev_update_time_; 33 | 34 | //convert the time from milliseconds to minutes 35 | double dtm = (double)dt / 60000; 36 | double delta_ticks = encoder_ticks - prev_encoder_ticks_; 37 | 38 | //calculate wheel's speed (RPM) 39 | rpm = (delta_ticks / counts_per_rev_) / dtm; 40 | 41 | prev_update_time_ = current_time; 42 | prev_encoder_ticks_ = encoder_ticks; 43 | } 44 | 45 | void Motor::spin(int pwm) 46 | { 47 | if(motor_driver_ == L298) 48 | { 49 | if(pwm > 0) 50 | { 51 | digitalWrite(motor_pinA_, HIGH); 52 | digitalWrite(motor_pinB_, LOW); 53 | } 54 | else if(pwm < 0) 55 | { 56 | digitalWrite(motor_pinA_, LOW); 57 | digitalWrite(motor_pinB_, HIGH); 58 | } 59 | analogWrite(pwm_pin_, abs(pwm)); 60 | } 61 | else if(motor_driver_ == BTS7960) 62 | { 63 | if (pwm > 0) 64 | { 65 | analogWrite(motor_pinA_, 0); 66 | analogWrite(motor_pinB_, abs(pwm)); 67 | } 68 | else if (pwm < 0) 69 | { 70 | analogWrite(motor_pinB_, 0); 71 | analogWrite(motor_pinA_, abs(pwm)); 72 | } 73 | else 74 | { 75 | analogWrite(motor_pinB_, 0); 76 | analogWrite(motor_pinA_, 0); 77 | } 78 | } 79 | } 80 | -------------------------------------------------------------------------------- /arduino/firmware/lib/ros_lib/std_msgs/UInt64.h: -------------------------------------------------------------------------------- 1 | #ifndef _ROS_std_msgs_UInt64_h 2 | #define _ROS_std_msgs_UInt64_h 3 | 4 | #include 5 | #include 6 | #include 7 | #include "ros/msg.h" 8 | 9 | namespace std_msgs 10 | { 11 | 12 | class UInt64 : public ros::Msg 13 | { 14 | public: 15 | uint64_t data; 16 | 17 | UInt64(): 18 | data(0) 19 | { 20 | } 21 | 22 | virtual int serialize(unsigned char *outbuffer) const 23 | { 24 | int offset = 0; 25 | *(outbuffer + offset + 0) = (this->data >> (8 * 0)) & 0xFF; 26 | *(outbuffer + offset + 1) = (this->data >> (8 * 1)) & 0xFF; 27 | *(outbuffer + offset + 2) = (this->data >> (8 * 2)) & 0xFF; 28 | *(outbuffer + offset + 3) = (this->data >> (8 * 3)) & 0xFF; 29 | *(outbuffer + offset + 4) = (this->data >> (8 * 4)) & 0xFF; 30 | *(outbuffer + offset + 5) = (this->data >> (8 * 5)) & 0xFF; 31 | *(outbuffer + offset + 6) = (this->data >> (8 * 6)) & 0xFF; 32 | *(outbuffer + offset + 7) = (this->data >> (8 * 7)) & 0xFF; 33 | offset += sizeof(this->data); 34 | return offset; 35 | } 36 | 37 | virtual int deserialize(unsigned char *inbuffer) 38 | { 39 | int offset = 0; 40 | this->data = ((uint64_t) (*(inbuffer + offset))); 41 | this->data |= ((uint64_t) (*(inbuffer + offset + 1))) << (8 * 1); 42 | this->data |= ((uint64_t) (*(inbuffer + offset + 2))) << (8 * 2); 43 | this->data |= ((uint64_t) (*(inbuffer + offset + 3))) << (8 * 3); 44 | this->data |= ((uint64_t) (*(inbuffer + offset + 4))) << (8 * 4); 45 | this->data |= ((uint64_t) (*(inbuffer + offset + 5))) << (8 * 5); 46 | this->data |= ((uint64_t) (*(inbuffer + offset + 6))) << (8 * 6); 47 | this->data |= ((uint64_t) (*(inbuffer + offset + 7))) << (8 * 7); 48 | offset += sizeof(this->data); 49 | return offset; 50 | } 51 | 52 | const char * getType(){ return "std_msgs/UInt64"; }; 53 | const char * getMD5(){ return "1b2a79973e8bf53d7b53acb71299cb57"; }; 54 | 55 | }; 56 | 57 | } 58 | #endif -------------------------------------------------------------------------------- /param/base_local_planner_params.yaml: -------------------------------------------------------------------------------- 1 | TebLocalPlannerROS: 2 | 3 | odom_topic: odom 4 | map_frame: /map 5 | 6 | # Trajectory 7 | 8 | teb_autosize: True 9 | dt_ref: 0.3 10 | dt_hysteresis: 0.1 11 | global_plan_overwrite_orientation: True 12 | max_global_plan_lookahead_dist: 3.0 13 | feasibility_check_no_poses: 2 14 | 15 | # Robot 16 | cmd_angle_instead_rotvel: True 17 | max_vel_x: 0.2 18 | max_vel_x_backwards: 0.2 19 | max_vel_theta: 0.3 20 | acc_lim_x: 1.5 21 | acc_lim_theta: 1.1 22 | min_turning_radius: 0.2 23 | wheelbase: 0.27 24 | footprint_model: # types: "point", "circular", "two_circles", "line", "polygon" 25 | type: "line" 26 | line_start: [-0.135, 0.0] # for type "line" 27 | line_end: [0.135, 0.0] # for type "line" 28 | 29 | # GoalTolerance 30 | 31 | xy_goal_tolerance: 0.3 32 | yaw_goal_tolerance: 0.1 33 | free_goal_vel: False 34 | 35 | # Obstacles 36 | 37 | min_obstacle_dist: 0.16 38 | include_costmap_obstacles: True 39 | costmap_obstacles_behind_robot_dist: 0.35 40 | obstacle_poses_affected: 30 41 | costmap_converter_plugin: "" 42 | costmap_converter_spin_thread: True 43 | costmap_converter_rate: 5 44 | 45 | # Optimization 46 | 47 | no_inner_iterations: 5 48 | no_outer_iterations: 4 49 | optimization_activate: True 50 | optimization_verbose: False 51 | penalty_epsilon: 0.1 52 | weight_max_vel_x: 2 53 | weight_max_vel_theta: 1 54 | weight_acc_lim_x: 1 55 | weight_acc_lim_theta: 1 56 | weight_kinematics_nh: 1000 57 | weight_kinematics_forward_drive: 1 58 | weight_kinematics_turning_radius: 1 59 | weight_optimaltime: 1 60 | weight_obstacle: 50 61 | weight_dynamic_obstacle: 10 # not in use yet 62 | alternative_time_cost: False # not in use yet 63 | 64 | # Homotopy Class Planner 65 | 66 | enable_homotopy_class_planning: False 67 | enable_multithreading: False 68 | simple_exploration: False 69 | max_number_classes: 4 70 | roadmap_graph_no_samples: 15 71 | roadmap_graph_area_width: 5 72 | h_signature_prescaler: 0.5 73 | h_signature_threshold: 0.1 74 | obstacle_keypoint_offset: 0.1 75 | obstacle_heading_threshold: 0.45 76 | visualize_hc_graph: False 77 | -------------------------------------------------------------------------------- /arduino/firmware/lib/ros_lib/geometry_msgs/TransformStamped.h: -------------------------------------------------------------------------------- 1 | #ifndef _ROS_geometry_msgs_TransformStamped_h 2 | #define _ROS_geometry_msgs_TransformStamped_h 3 | 4 | #include 5 | #include 6 | #include 7 | #include "ros/msg.h" 8 | #include "std_msgs/Header.h" 9 | #include "geometry_msgs/Transform.h" 10 | 11 | namespace geometry_msgs 12 | { 13 | 14 | class TransformStamped : public ros::Msg 15 | { 16 | public: 17 | std_msgs::Header header; 18 | const char* child_frame_id; 19 | geometry_msgs::Transform transform; 20 | 21 | TransformStamped(): 22 | header(), 23 | child_frame_id(""), 24 | transform() 25 | { 26 | } 27 | 28 | virtual int serialize(unsigned char *outbuffer) const 29 | { 30 | int offset = 0; 31 | offset += this->header.serialize(outbuffer + offset); 32 | uint32_t length_child_frame_id = strlen(this->child_frame_id); 33 | memcpy(outbuffer + offset, &length_child_frame_id, sizeof(uint32_t)); 34 | offset += 4; 35 | memcpy(outbuffer + offset, this->child_frame_id, length_child_frame_id); 36 | offset += length_child_frame_id; 37 | offset += this->transform.serialize(outbuffer + offset); 38 | return offset; 39 | } 40 | 41 | virtual int deserialize(unsigned char *inbuffer) 42 | { 43 | int offset = 0; 44 | offset += this->header.deserialize(inbuffer + offset); 45 | uint32_t length_child_frame_id; 46 | memcpy(&length_child_frame_id, (inbuffer + offset), sizeof(uint32_t)); 47 | offset += 4; 48 | for(unsigned int k= offset; k< offset+length_child_frame_id; ++k){ 49 | inbuffer[k-1]=inbuffer[k]; 50 | } 51 | inbuffer[offset+length_child_frame_id-1]=0; 52 | this->child_frame_id = (char *)(inbuffer + offset-1); 53 | offset += length_child_frame_id; 54 | offset += this->transform.deserialize(inbuffer + offset); 55 | return offset; 56 | } 57 | 58 | const char * getType(){ return "geometry_msgs/TransformStamped"; }; 59 | const char * getMD5(){ return "b5764a33bfeb3588febc2682852579b0"; }; 60 | 61 | }; 62 | 63 | } 64 | #endif -------------------------------------------------------------------------------- /arduino/firmware/lib/ros_lib/tf/tf.h: -------------------------------------------------------------------------------- 1 | /* 2 | * Software License Agreement (BSD License) 3 | * 4 | * Copyright (c) 2011, Willow Garage, Inc. 5 | * All rights reserved. 6 | * 7 | * Redistribution and use in source and binary forms, with or without 8 | * modification, are permitted provided that the following conditions 9 | * are met: 10 | * 11 | * * Redistributions of source code must retain the above copyright 12 | * notice, this list of conditions and the following disclaimer. 13 | * * Redistributions in binary form must reproduce the above 14 | * copyright notice, this list of conditions and the following 15 | * disclaimer in the documentation and/or other materials provided 16 | * with the distribution. 17 | * * Neither the name of Willow Garage, Inc. nor the names of its 18 | * contributors may be used to endorse or promote prducts derived 19 | * from this software without specific prior written permission. 20 | * 21 | * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS 22 | * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT 23 | * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS 24 | * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE 25 | * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, 26 | * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, 27 | * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; 28 | * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER 29 | * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT 30 | * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN 31 | * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE 32 | * POSSIBILITY OF SUCH DAMAGE. 33 | */ 34 | 35 | #ifndef ROS_TF_H_ 36 | #define ROS_TF_H_ 37 | 38 | #include "geometry_msgs/TransformStamped.h" 39 | 40 | namespace tf 41 | { 42 | 43 | static inline geometry_msgs::Quaternion createQuaternionFromYaw(double yaw) 44 | { 45 | geometry_msgs::Quaternion q; 46 | q.x = 0; 47 | q.y = 0; 48 | q.z = sin(yaw * 0.5); 49 | q.w = cos(yaw * 0.5); 50 | return q; 51 | } 52 | 53 | } 54 | 55 | #endif 56 | 57 | -------------------------------------------------------------------------------- /arduino/firmware/lib/ros_lib/std_msgs/UInt16MultiArray.h: -------------------------------------------------------------------------------- 1 | #ifndef _ROS_std_msgs_UInt16MultiArray_h 2 | #define _ROS_std_msgs_UInt16MultiArray_h 3 | 4 | #include 5 | #include 6 | #include 7 | #include "ros/msg.h" 8 | #include "std_msgs/MultiArrayLayout.h" 9 | 10 | namespace std_msgs 11 | { 12 | 13 | class UInt16MultiArray : public ros::Msg 14 | { 15 | public: 16 | std_msgs::MultiArrayLayout layout; 17 | uint8_t data_length; 18 | uint16_t st_data; 19 | uint16_t * data; 20 | 21 | UInt16MultiArray(): 22 | layout(), 23 | data_length(0), data(NULL) 24 | { 25 | } 26 | 27 | virtual int serialize(unsigned char *outbuffer) const 28 | { 29 | int offset = 0; 30 | offset += this->layout.serialize(outbuffer + offset); 31 | *(outbuffer + offset++) = data_length; 32 | *(outbuffer + offset++) = 0; 33 | *(outbuffer + offset++) = 0; 34 | *(outbuffer + offset++) = 0; 35 | for( uint8_t i = 0; i < data_length; i++){ 36 | *(outbuffer + offset + 0) = (this->data[i] >> (8 * 0)) & 0xFF; 37 | *(outbuffer + offset + 1) = (this->data[i] >> (8 * 1)) & 0xFF; 38 | offset += sizeof(this->data[i]); 39 | } 40 | return offset; 41 | } 42 | 43 | virtual int deserialize(unsigned char *inbuffer) 44 | { 45 | int offset = 0; 46 | offset += this->layout.deserialize(inbuffer + offset); 47 | uint8_t data_lengthT = *(inbuffer + offset++); 48 | if(data_lengthT > data_length) 49 | this->data = (uint16_t*)realloc(this->data, data_lengthT * sizeof(uint16_t)); 50 | offset += 3; 51 | data_length = data_lengthT; 52 | for( uint8_t i = 0; i < data_length; i++){ 53 | this->st_data = ((uint16_t) (*(inbuffer + offset))); 54 | this->st_data |= ((uint16_t) (*(inbuffer + offset + 1))) << (8 * 1); 55 | offset += sizeof(this->st_data); 56 | memcpy( &(this->data[i]), &(this->st_data), sizeof(uint16_t)); 57 | } 58 | return offset; 59 | } 60 | 61 | const char * getType(){ return "std_msgs/UInt16MultiArray"; }; 62 | const char * getMD5(){ return "52f264f1c973c4b73790d384c6cb4484"; }; 63 | 64 | }; 65 | 66 | } 67 | #endif -------------------------------------------------------------------------------- /arduino/firmware/lib/ros_lib/sensor_msgs/NavSatStatus.h: -------------------------------------------------------------------------------- 1 | #ifndef _ROS_sensor_msgs_NavSatStatus_h 2 | #define _ROS_sensor_msgs_NavSatStatus_h 3 | 4 | #include 5 | #include 6 | #include 7 | #include "ros/msg.h" 8 | 9 | namespace sensor_msgs 10 | { 11 | 12 | class NavSatStatus : public ros::Msg 13 | { 14 | public: 15 | int8_t status; 16 | uint16_t service; 17 | enum { STATUS_NO_FIX = -1 }; 18 | enum { STATUS_FIX = 0 }; 19 | enum { STATUS_SBAS_FIX = 1 }; 20 | enum { STATUS_GBAS_FIX = 2 }; 21 | enum { SERVICE_GPS = 1 }; 22 | enum { SERVICE_GLONASS = 2 }; 23 | enum { SERVICE_COMPASS = 4 }; 24 | enum { SERVICE_GALILEO = 8 }; 25 | 26 | NavSatStatus(): 27 | status(0), 28 | service(0) 29 | { 30 | } 31 | 32 | virtual int serialize(unsigned char *outbuffer) const 33 | { 34 | int offset = 0; 35 | union { 36 | int8_t real; 37 | uint8_t base; 38 | } u_status; 39 | u_status.real = this->status; 40 | *(outbuffer + offset + 0) = (u_status.base >> (8 * 0)) & 0xFF; 41 | offset += sizeof(this->status); 42 | *(outbuffer + offset + 0) = (this->service >> (8 * 0)) & 0xFF; 43 | *(outbuffer + offset + 1) = (this->service >> (8 * 1)) & 0xFF; 44 | offset += sizeof(this->service); 45 | return offset; 46 | } 47 | 48 | virtual int deserialize(unsigned char *inbuffer) 49 | { 50 | int offset = 0; 51 | union { 52 | int8_t real; 53 | uint8_t base; 54 | } u_status; 55 | u_status.base = 0; 56 | u_status.base |= ((uint8_t) (*(inbuffer + offset + 0))) << (8 * 0); 57 | this->status = u_status.real; 58 | offset += sizeof(this->status); 59 | this->service = ((uint16_t) (*(inbuffer + offset))); 60 | this->service |= ((uint16_t) (*(inbuffer + offset + 1))) << (8 * 1); 61 | offset += sizeof(this->service); 62 | return offset; 63 | } 64 | 65 | const char * getType(){ return "sensor_msgs/NavSatStatus"; }; 66 | const char * getMD5(){ return "331cdbddfa4bc96ffc3b9ad98900a54c"; }; 67 | 68 | }; 69 | 70 | } 71 | #endif -------------------------------------------------------------------------------- /arduino/firmware/lib/ros_lib/std_msgs/Time.h: -------------------------------------------------------------------------------- 1 | #ifndef _ROS_std_msgs_Time_h 2 | #define _ROS_std_msgs_Time_h 3 | 4 | #include 5 | #include 6 | #include 7 | #include "ros/msg.h" 8 | #include "ros/time.h" 9 | 10 | namespace std_msgs 11 | { 12 | 13 | class Time : public ros::Msg 14 | { 15 | public: 16 | ros::Time data; 17 | 18 | Time(): 19 | data() 20 | { 21 | } 22 | 23 | virtual int serialize(unsigned char *outbuffer) const 24 | { 25 | int offset = 0; 26 | *(outbuffer + offset + 0) = (this->data.sec >> (8 * 0)) & 0xFF; 27 | *(outbuffer + offset + 1) = (this->data.sec >> (8 * 1)) & 0xFF; 28 | *(outbuffer + offset + 2) = (this->data.sec >> (8 * 2)) & 0xFF; 29 | *(outbuffer + offset + 3) = (this->data.sec >> (8 * 3)) & 0xFF; 30 | offset += sizeof(this->data.sec); 31 | *(outbuffer + offset + 0) = (this->data.nsec >> (8 * 0)) & 0xFF; 32 | *(outbuffer + offset + 1) = (this->data.nsec >> (8 * 1)) & 0xFF; 33 | *(outbuffer + offset + 2) = (this->data.nsec >> (8 * 2)) & 0xFF; 34 | *(outbuffer + offset + 3) = (this->data.nsec >> (8 * 3)) & 0xFF; 35 | offset += sizeof(this->data.nsec); 36 | return offset; 37 | } 38 | 39 | virtual int deserialize(unsigned char *inbuffer) 40 | { 41 | int offset = 0; 42 | this->data.sec = ((uint32_t) (*(inbuffer + offset))); 43 | this->data.sec |= ((uint32_t) (*(inbuffer + offset + 1))) << (8 * 1); 44 | this->data.sec |= ((uint32_t) (*(inbuffer + offset + 2))) << (8 * 2); 45 | this->data.sec |= ((uint32_t) (*(inbuffer + offset + 3))) << (8 * 3); 46 | offset += sizeof(this->data.sec); 47 | this->data.nsec = ((uint32_t) (*(inbuffer + offset))); 48 | this->data.nsec |= ((uint32_t) (*(inbuffer + offset + 1))) << (8 * 1); 49 | this->data.nsec |= ((uint32_t) (*(inbuffer + offset + 2))) << (8 * 2); 50 | this->data.nsec |= ((uint32_t) (*(inbuffer + offset + 3))) << (8 * 3); 51 | offset += sizeof(this->data.nsec); 52 | return offset; 53 | } 54 | 55 | const char * getType(){ return "std_msgs/Time"; }; 56 | const char * getMD5(){ return "cd7166c74c552c311fbcc2fe5a7bc289"; }; 57 | 58 | }; 59 | 60 | } 61 | #endif -------------------------------------------------------------------------------- /arduino/firmware/lib/pid/pid.ino: -------------------------------------------------------------------------------- 1 | /* 2 | Copyright (c) 2016, Juan Jimeno 3 | Source: http://smithcsrobot.weebly.com/uploads/6/0/9/5/60954939/pid_control_document.pdf 4 | All rights reserved. 5 | Redistribution and use in source and binary forms, with or without 6 | modification, are permitted provided that the following conditions are met: 7 | * Redistributions of source code must retain the above copyright notice, 8 | this list of conditions and the following disclaimer. 9 | * Redistributions in binary form must reproduce the above copyright 10 | notice, this list of conditions and the following disclaimer in the 11 | documentation and/or other materials provided with the distribution. 12 | * Neither the name of nor the names of its contributors may be used to 13 | endorse or promote products derived from this software without specific 14 | prior written permission. 15 | THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" 16 | AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE 17 | IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE 18 | ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE 19 | LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR 20 | CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF 21 | SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS 22 | INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN 23 | CONTRACT, STRICT LIABILITY, OR TORTPPIPI (INCLUDING NEGLIGENCE OR OTHERWISE) 24 | ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE 25 | POSSIBILITY OF SUCH DAMAGE. 26 | */ 27 | 28 | #include "PID.h" 29 | PID pid( -255, 255, 0.05, 0.9, 0.1); 30 | /*PID(float min_val, float max_val, float kp, float ki, float kd) 31 | * min_val = min output PID value 32 | * max_val = max output PID value 33 | * kp = PID - P constant 34 | * ki = PID - I constant 35 | * di = PID - D constant 36 | */ 37 | 38 | float setpoint = 30; 39 | float measured_value = 0; 40 | void setup() 41 | { 42 | Serial.begin(9600); 43 | } 44 | 45 | void loop() 46 | { 47 | Serial.println(pid.compute(setpoint, measured_value)); 48 | delay(1000); 49 | measured_value++; 50 | } 51 | -------------------------------------------------------------------------------- /arduino/firmware/lib/ros_lib/std_msgs/Duration.h: -------------------------------------------------------------------------------- 1 | #ifndef _ROS_std_msgs_Duration_h 2 | #define _ROS_std_msgs_Duration_h 3 | 4 | #include 5 | #include 6 | #include 7 | #include "ros/msg.h" 8 | #include "ros/duration.h" 9 | 10 | namespace std_msgs 11 | { 12 | 13 | class Duration : public ros::Msg 14 | { 15 | public: 16 | ros::Duration data; 17 | 18 | Duration(): 19 | data() 20 | { 21 | } 22 | 23 | virtual int serialize(unsigned char *outbuffer) const 24 | { 25 | int offset = 0; 26 | *(outbuffer + offset + 0) = (this->data.sec >> (8 * 0)) & 0xFF; 27 | *(outbuffer + offset + 1) = (this->data.sec >> (8 * 1)) & 0xFF; 28 | *(outbuffer + offset + 2) = (this->data.sec >> (8 * 2)) & 0xFF; 29 | *(outbuffer + offset + 3) = (this->data.sec >> (8 * 3)) & 0xFF; 30 | offset += sizeof(this->data.sec); 31 | *(outbuffer + offset + 0) = (this->data.nsec >> (8 * 0)) & 0xFF; 32 | *(outbuffer + offset + 1) = (this->data.nsec >> (8 * 1)) & 0xFF; 33 | *(outbuffer + offset + 2) = (this->data.nsec >> (8 * 2)) & 0xFF; 34 | *(outbuffer + offset + 3) = (this->data.nsec >> (8 * 3)) & 0xFF; 35 | offset += sizeof(this->data.nsec); 36 | return offset; 37 | } 38 | 39 | virtual int deserialize(unsigned char *inbuffer) 40 | { 41 | int offset = 0; 42 | this->data.sec = ((uint32_t) (*(inbuffer + offset))); 43 | this->data.sec |= ((uint32_t) (*(inbuffer + offset + 1))) << (8 * 1); 44 | this->data.sec |= ((uint32_t) (*(inbuffer + offset + 2))) << (8 * 2); 45 | this->data.sec |= ((uint32_t) (*(inbuffer + offset + 3))) << (8 * 3); 46 | offset += sizeof(this->data.sec); 47 | this->data.nsec = ((uint32_t) (*(inbuffer + offset))); 48 | this->data.nsec |= ((uint32_t) (*(inbuffer + offset + 1))) << (8 * 1); 49 | this->data.nsec |= ((uint32_t) (*(inbuffer + offset + 2))) << (8 * 2); 50 | this->data.nsec |= ((uint32_t) (*(inbuffer + offset + 3))) << (8 * 3); 51 | offset += sizeof(this->data.nsec); 52 | return offset; 53 | } 54 | 55 | const char * getType(){ return "std_msgs/Duration"; }; 56 | const char * getMD5(){ return "3e286caf4241d664e55f3ad380e2ae46"; }; 57 | 58 | }; 59 | 60 | } 61 | #endif -------------------------------------------------------------------------------- /arduino/firmware/lib/ros_lib/tf/FrameGraph.h: -------------------------------------------------------------------------------- 1 | #ifndef _ROS_SERVICE_FrameGraph_h 2 | #define _ROS_SERVICE_FrameGraph_h 3 | #include 4 | #include 5 | #include 6 | #include "ros/msg.h" 7 | 8 | namespace tf 9 | { 10 | 11 | static const char FRAMEGRAPH[] = "tf/FrameGraph"; 12 | 13 | class FrameGraphRequest : public ros::Msg 14 | { 15 | public: 16 | 17 | FrameGraphRequest() 18 | { 19 | } 20 | 21 | virtual int serialize(unsigned char *outbuffer) const 22 | { 23 | int offset = 0; 24 | return offset; 25 | } 26 | 27 | virtual int deserialize(unsigned char *inbuffer) 28 | { 29 | int offset = 0; 30 | return offset; 31 | } 32 | 33 | const char * getType(){ return FRAMEGRAPH; }; 34 | const char * getMD5(){ return "d41d8cd98f00b204e9800998ecf8427e"; }; 35 | 36 | }; 37 | 38 | class FrameGraphResponse : public ros::Msg 39 | { 40 | public: 41 | const char* dot_graph; 42 | 43 | FrameGraphResponse(): 44 | dot_graph("") 45 | { 46 | } 47 | 48 | virtual int serialize(unsigned char *outbuffer) const 49 | { 50 | int offset = 0; 51 | uint32_t length_dot_graph = strlen(this->dot_graph); 52 | memcpy(outbuffer + offset, &length_dot_graph, sizeof(uint32_t)); 53 | offset += 4; 54 | memcpy(outbuffer + offset, this->dot_graph, length_dot_graph); 55 | offset += length_dot_graph; 56 | return offset; 57 | } 58 | 59 | virtual int deserialize(unsigned char *inbuffer) 60 | { 61 | int offset = 0; 62 | uint32_t length_dot_graph; 63 | memcpy(&length_dot_graph, (inbuffer + offset), sizeof(uint32_t)); 64 | offset += 4; 65 | for(unsigned int k= offset; k< offset+length_dot_graph; ++k){ 66 | inbuffer[k-1]=inbuffer[k]; 67 | } 68 | inbuffer[offset+length_dot_graph-1]=0; 69 | this->dot_graph = (char *)(inbuffer + offset-1); 70 | offset += length_dot_graph; 71 | return offset; 72 | } 73 | 74 | const char * getType(){ return FRAMEGRAPH; }; 75 | const char * getMD5(){ return "c4af9ac907e58e906eb0b6e3c58478c0"; }; 76 | 77 | }; 78 | 79 | class FrameGraph { 80 | public: 81 | typedef FrameGraphRequest Request; 82 | typedef FrameGraphResponse Response; 83 | }; 84 | 85 | } 86 | #endif 87 | -------------------------------------------------------------------------------- /arduino/firmware/lib/ros_lib/std_msgs/ByteMultiArray.h: -------------------------------------------------------------------------------- 1 | #ifndef _ROS_std_msgs_ByteMultiArray_h 2 | #define _ROS_std_msgs_ByteMultiArray_h 3 | 4 | #include 5 | #include 6 | #include 7 | #include "ros/msg.h" 8 | #include "std_msgs/MultiArrayLayout.h" 9 | 10 | namespace std_msgs 11 | { 12 | 13 | class ByteMultiArray : public ros::Msg 14 | { 15 | public: 16 | std_msgs::MultiArrayLayout layout; 17 | uint8_t data_length; 18 | int8_t st_data; 19 | int8_t * data; 20 | 21 | ByteMultiArray(): 22 | layout(), 23 | data_length(0), data(NULL) 24 | { 25 | } 26 | 27 | virtual int serialize(unsigned char *outbuffer) const 28 | { 29 | int offset = 0; 30 | offset += this->layout.serialize(outbuffer + offset); 31 | *(outbuffer + offset++) = data_length; 32 | *(outbuffer + offset++) = 0; 33 | *(outbuffer + offset++) = 0; 34 | *(outbuffer + offset++) = 0; 35 | for( uint8_t i = 0; i < data_length; i++){ 36 | union { 37 | int8_t real; 38 | uint8_t base; 39 | } u_datai; 40 | u_datai.real = this->data[i]; 41 | *(outbuffer + offset + 0) = (u_datai.base >> (8 * 0)) & 0xFF; 42 | offset += sizeof(this->data[i]); 43 | } 44 | return offset; 45 | } 46 | 47 | virtual int deserialize(unsigned char *inbuffer) 48 | { 49 | int offset = 0; 50 | offset += this->layout.deserialize(inbuffer + offset); 51 | uint8_t data_lengthT = *(inbuffer + offset++); 52 | if(data_lengthT > data_length) 53 | this->data = (int8_t*)realloc(this->data, data_lengthT * sizeof(int8_t)); 54 | offset += 3; 55 | data_length = data_lengthT; 56 | for( uint8_t i = 0; i < data_length; i++){ 57 | union { 58 | int8_t real; 59 | uint8_t base; 60 | } u_st_data; 61 | u_st_data.base = 0; 62 | u_st_data.base |= ((uint8_t) (*(inbuffer + offset + 0))) << (8 * 0); 63 | this->st_data = u_st_data.real; 64 | offset += sizeof(this->st_data); 65 | memcpy( &(this->data[i]), &(this->st_data), sizeof(int8_t)); 66 | } 67 | return offset; 68 | } 69 | 70 | const char * getType(){ return "std_msgs/ByteMultiArray"; }; 71 | const char * getMD5(){ return "70ea476cbcfd65ac2f68f3cda1e891fe"; }; 72 | 73 | }; 74 | 75 | } 76 | #endif -------------------------------------------------------------------------------- /arduino/firmware/lib/ros_lib/std_msgs/Int8MultiArray.h: -------------------------------------------------------------------------------- 1 | #ifndef _ROS_std_msgs_Int8MultiArray_h 2 | #define _ROS_std_msgs_Int8MultiArray_h 3 | 4 | #include 5 | #include 6 | #include 7 | #include "ros/msg.h" 8 | #include "std_msgs/MultiArrayLayout.h" 9 | 10 | namespace std_msgs 11 | { 12 | 13 | class Int8MultiArray : public ros::Msg 14 | { 15 | public: 16 | std_msgs::MultiArrayLayout layout; 17 | uint8_t data_length; 18 | int8_t st_data; 19 | int8_t * data; 20 | 21 | Int8MultiArray(): 22 | layout(), 23 | data_length(0), data(NULL) 24 | { 25 | } 26 | 27 | virtual int serialize(unsigned char *outbuffer) const 28 | { 29 | int offset = 0; 30 | offset += this->layout.serialize(outbuffer + offset); 31 | *(outbuffer + offset++) = data_length; 32 | *(outbuffer + offset++) = 0; 33 | *(outbuffer + offset++) = 0; 34 | *(outbuffer + offset++) = 0; 35 | for( uint8_t i = 0; i < data_length; i++){ 36 | union { 37 | int8_t real; 38 | uint8_t base; 39 | } u_datai; 40 | u_datai.real = this->data[i]; 41 | *(outbuffer + offset + 0) = (u_datai.base >> (8 * 0)) & 0xFF; 42 | offset += sizeof(this->data[i]); 43 | } 44 | return offset; 45 | } 46 | 47 | virtual int deserialize(unsigned char *inbuffer) 48 | { 49 | int offset = 0; 50 | offset += this->layout.deserialize(inbuffer + offset); 51 | uint8_t data_lengthT = *(inbuffer + offset++); 52 | if(data_lengthT > data_length) 53 | this->data = (int8_t*)realloc(this->data, data_lengthT * sizeof(int8_t)); 54 | offset += 3; 55 | data_length = data_lengthT; 56 | for( uint8_t i = 0; i < data_length; i++){ 57 | union { 58 | int8_t real; 59 | uint8_t base; 60 | } u_st_data; 61 | u_st_data.base = 0; 62 | u_st_data.base |= ((uint8_t) (*(inbuffer + offset + 0))) << (8 * 0); 63 | this->st_data = u_st_data.real; 64 | offset += sizeof(this->st_data); 65 | memcpy( &(this->data[i]), &(this->st_data), sizeof(int8_t)); 66 | } 67 | return offset; 68 | } 69 | 70 | const char * getType(){ return "std_msgs/Int8MultiArray"; }; 71 | const char * getMD5(){ return "d7c1af35a1b4781bbe79e03dd94b7c13"; }; 72 | 73 | }; 74 | 75 | } 76 | #endif -------------------------------------------------------------------------------- /arduino/firmware/lib/ros_lib/std_msgs/Int64.h: -------------------------------------------------------------------------------- 1 | #ifndef _ROS_std_msgs_Int64_h 2 | #define _ROS_std_msgs_Int64_h 3 | 4 | #include 5 | #include 6 | #include 7 | #include "ros/msg.h" 8 | 9 | namespace std_msgs 10 | { 11 | 12 | class Int64 : public ros::Msg 13 | { 14 | public: 15 | int64_t data; 16 | 17 | Int64(): 18 | data(0) 19 | { 20 | } 21 | 22 | virtual int serialize(unsigned char *outbuffer) const 23 | { 24 | int offset = 0; 25 | union { 26 | int64_t real; 27 | uint64_t base; 28 | } u_data; 29 | u_data.real = this->data; 30 | *(outbuffer + offset + 0) = (u_data.base >> (8 * 0)) & 0xFF; 31 | *(outbuffer + offset + 1) = (u_data.base >> (8 * 1)) & 0xFF; 32 | *(outbuffer + offset + 2) = (u_data.base >> (8 * 2)) & 0xFF; 33 | *(outbuffer + offset + 3) = (u_data.base >> (8 * 3)) & 0xFF; 34 | *(outbuffer + offset + 4) = (u_data.base >> (8 * 4)) & 0xFF; 35 | *(outbuffer + offset + 5) = (u_data.base >> (8 * 5)) & 0xFF; 36 | *(outbuffer + offset + 6) = (u_data.base >> (8 * 6)) & 0xFF; 37 | *(outbuffer + offset + 7) = (u_data.base >> (8 * 7)) & 0xFF; 38 | offset += sizeof(this->data); 39 | return offset; 40 | } 41 | 42 | virtual int deserialize(unsigned char *inbuffer) 43 | { 44 | int offset = 0; 45 | union { 46 | int64_t real; 47 | uint64_t base; 48 | } u_data; 49 | u_data.base = 0; 50 | u_data.base |= ((uint64_t) (*(inbuffer + offset + 0))) << (8 * 0); 51 | u_data.base |= ((uint64_t) (*(inbuffer + offset + 1))) << (8 * 1); 52 | u_data.base |= ((uint64_t) (*(inbuffer + offset + 2))) << (8 * 2); 53 | u_data.base |= ((uint64_t) (*(inbuffer + offset + 3))) << (8 * 3); 54 | u_data.base |= ((uint64_t) (*(inbuffer + offset + 4))) << (8 * 4); 55 | u_data.base |= ((uint64_t) (*(inbuffer + offset + 5))) << (8 * 5); 56 | u_data.base |= ((uint64_t) (*(inbuffer + offset + 6))) << (8 * 6); 57 | u_data.base |= ((uint64_t) (*(inbuffer + offset + 7))) << (8 * 7); 58 | this->data = u_data.real; 59 | offset += sizeof(this->data); 60 | return offset; 61 | } 62 | 63 | const char * getType(){ return "std_msgs/Int64"; }; 64 | const char * getMD5(){ return "34add168574510e6e17f5d23ecc077ef"; }; 65 | 66 | }; 67 | 68 | } 69 | #endif -------------------------------------------------------------------------------- /arduino/firmware/lib/Encoder/utility/interrupt_config.h: -------------------------------------------------------------------------------- 1 | #if defined(__AVR__) 2 | 3 | #include 4 | #include 5 | 6 | #define attachInterrupt(num, func, mode) enableInterrupt(num) 7 | #if defined(__AVR_ATmega1280__) || defined(__AVR_ATmega2560__) 8 | #define SCRAMBLE_INT_ORDER(num) ((num < 4) ? num + 2 : ((num < 6) ? num - 4 : num)) 9 | #define DESCRAMBLE_INT_ORDER(num) ((num < 2) ? num + 4 : ((num < 6) ? num - 2 : num)) 10 | #else 11 | #define SCRAMBLE_INT_ORDER(num) (num) 12 | #define DESCRAMBLE_INT_ORDER(num) (num) 13 | #endif 14 | 15 | static void enableInterrupt(uint8_t num) 16 | { 17 | switch (DESCRAMBLE_INT_ORDER(num)) { 18 | #if defined(EICRA) && defined(EIMSK) 19 | case 0: 20 | EICRA = (EICRA & 0xFC) | 0x01; 21 | EIMSK |= 0x01; 22 | return; 23 | case 1: 24 | EICRA = (EICRA & 0xF3) | 0x04; 25 | EIMSK |= 0x02; 26 | return; 27 | case 2: 28 | EICRA = (EICRA & 0xCF) | 0x10; 29 | EIMSK |= 0x04; 30 | return; 31 | case 3: 32 | EICRA = (EICRA & 0x3F) | 0x40; 33 | EIMSK |= 0x08; 34 | return; 35 | #elif defined(MCUCR) && defined(GICR) 36 | case 0: 37 | MCUCR = (MCUCR & ~((1 << ISC00) | (1 << ISC01))) | (mode << ISC00); 38 | GICR |= (1 << INT0); 39 | return; 40 | case 1: 41 | MCUCR = (MCUCR & ~((1 << ISC10) | (1 << ISC11))) | (mode << ISC10); 42 | GICR |= (1 << INT1); 43 | return; 44 | #elif defined(MCUCR) && defined(GIMSK) 45 | case 0: 46 | MCUCR = (MCUCR & ~((1 << ISC00) | (1 << ISC01))) | (mode << ISC00); 47 | GIMSK |= (1 << INT0); 48 | return; 49 | case 1: 50 | MCUCR = (MCUCR & ~((1 << ISC10) | (1 << ISC11))) | (mode << ISC10); 51 | GIMSK |= (1 << INT1); 52 | return; 53 | #endif 54 | #if defined(EICRB) && defined(EIMSK) 55 | case 4: 56 | EICRB = (EICRB & 0xFC) | 0x01; 57 | EIMSK |= 0x10; 58 | return; 59 | case 5: 60 | EICRB = (EICRB & 0xF3) | 0x04; 61 | EIMSK |= 0x20; 62 | return; 63 | case 6: 64 | EICRB = (EICRB & 0xCF) | 0x10; 65 | EIMSK |= 0x40; 66 | return; 67 | case 7: 68 | EICRB = (EICRB & 0x3F) | 0x40; 69 | EIMSK |= 0x80; 70 | return; 71 | #endif 72 | } 73 | } 74 | 75 | #elif defined(__PIC32MX__) 76 | 77 | #ifdef ENCODER_OPTIMIZE_INTERRUPTS 78 | #undef ENCODER_OPTIMIZE_INTERRUPTS 79 | #endif 80 | 81 | #else 82 | 83 | #ifdef ENCODER_OPTIMIZE_INTERRUPTS 84 | #undef ENCODER_OPTIMIZE_INTERRUPTS 85 | #endif 86 | 87 | #endif 88 | -------------------------------------------------------------------------------- /arduino/firmware/lib/ros_lib/std_msgs/Float64.h: -------------------------------------------------------------------------------- 1 | #ifndef _ROS_std_msgs_Float64_h 2 | #define _ROS_std_msgs_Float64_h 3 | 4 | #include 5 | #include 6 | #include 7 | #include "ros/msg.h" 8 | 9 | namespace std_msgs 10 | { 11 | 12 | class Float64 : public ros::Msg 13 | { 14 | public: 15 | double data; 16 | 17 | Float64(): 18 | data(0) 19 | { 20 | } 21 | 22 | virtual int serialize(unsigned char *outbuffer) const 23 | { 24 | int offset = 0; 25 | union { 26 | double real; 27 | uint64_t base; 28 | } u_data; 29 | u_data.real = this->data; 30 | *(outbuffer + offset + 0) = (u_data.base >> (8 * 0)) & 0xFF; 31 | *(outbuffer + offset + 1) = (u_data.base >> (8 * 1)) & 0xFF; 32 | *(outbuffer + offset + 2) = (u_data.base >> (8 * 2)) & 0xFF; 33 | *(outbuffer + offset + 3) = (u_data.base >> (8 * 3)) & 0xFF; 34 | *(outbuffer + offset + 4) = (u_data.base >> (8 * 4)) & 0xFF; 35 | *(outbuffer + offset + 5) = (u_data.base >> (8 * 5)) & 0xFF; 36 | *(outbuffer + offset + 6) = (u_data.base >> (8 * 6)) & 0xFF; 37 | *(outbuffer + offset + 7) = (u_data.base >> (8 * 7)) & 0xFF; 38 | offset += sizeof(this->data); 39 | return offset; 40 | } 41 | 42 | virtual int deserialize(unsigned char *inbuffer) 43 | { 44 | int offset = 0; 45 | union { 46 | double real; 47 | uint64_t base; 48 | } u_data; 49 | u_data.base = 0; 50 | u_data.base |= ((uint64_t) (*(inbuffer + offset + 0))) << (8 * 0); 51 | u_data.base |= ((uint64_t) (*(inbuffer + offset + 1))) << (8 * 1); 52 | u_data.base |= ((uint64_t) (*(inbuffer + offset + 2))) << (8 * 2); 53 | u_data.base |= ((uint64_t) (*(inbuffer + offset + 3))) << (8 * 3); 54 | u_data.base |= ((uint64_t) (*(inbuffer + offset + 4))) << (8 * 4); 55 | u_data.base |= ((uint64_t) (*(inbuffer + offset + 5))) << (8 * 5); 56 | u_data.base |= ((uint64_t) (*(inbuffer + offset + 6))) << (8 * 6); 57 | u_data.base |= ((uint64_t) (*(inbuffer + offset + 7))) << (8 * 7); 58 | this->data = u_data.real; 59 | offset += sizeof(this->data); 60 | return offset; 61 | } 62 | 63 | const char * getType(){ return "std_msgs/Float64"; }; 64 | const char * getMD5(){ return "fdb28210bfa9d7c91146260178d9a584"; }; 65 | 66 | }; 67 | 68 | } 69 | #endif -------------------------------------------------------------------------------- /arduino/firmware/lib/ros_lib/std_msgs/UInt32MultiArray.h: -------------------------------------------------------------------------------- 1 | #ifndef _ROS_std_msgs_UInt32MultiArray_h 2 | #define _ROS_std_msgs_UInt32MultiArray_h 3 | 4 | #include 5 | #include 6 | #include 7 | #include "ros/msg.h" 8 | #include "std_msgs/MultiArrayLayout.h" 9 | 10 | namespace std_msgs 11 | { 12 | 13 | class UInt32MultiArray : public ros::Msg 14 | { 15 | public: 16 | std_msgs::MultiArrayLayout layout; 17 | uint8_t data_length; 18 | uint32_t st_data; 19 | uint32_t * data; 20 | 21 | UInt32MultiArray(): 22 | layout(), 23 | data_length(0), data(NULL) 24 | { 25 | } 26 | 27 | virtual int serialize(unsigned char *outbuffer) const 28 | { 29 | int offset = 0; 30 | offset += this->layout.serialize(outbuffer + offset); 31 | *(outbuffer + offset++) = data_length; 32 | *(outbuffer + offset++) = 0; 33 | *(outbuffer + offset++) = 0; 34 | *(outbuffer + offset++) = 0; 35 | for( uint8_t i = 0; i < data_length; i++){ 36 | *(outbuffer + offset + 0) = (this->data[i] >> (8 * 0)) & 0xFF; 37 | *(outbuffer + offset + 1) = (this->data[i] >> (8 * 1)) & 0xFF; 38 | *(outbuffer + offset + 2) = (this->data[i] >> (8 * 2)) & 0xFF; 39 | *(outbuffer + offset + 3) = (this->data[i] >> (8 * 3)) & 0xFF; 40 | offset += sizeof(this->data[i]); 41 | } 42 | return offset; 43 | } 44 | 45 | virtual int deserialize(unsigned char *inbuffer) 46 | { 47 | int offset = 0; 48 | offset += this->layout.deserialize(inbuffer + offset); 49 | uint8_t data_lengthT = *(inbuffer + offset++); 50 | if(data_lengthT > data_length) 51 | this->data = (uint32_t*)realloc(this->data, data_lengthT * sizeof(uint32_t)); 52 | offset += 3; 53 | data_length = data_lengthT; 54 | for( uint8_t i = 0; i < data_length; i++){ 55 | this->st_data = ((uint32_t) (*(inbuffer + offset))); 56 | this->st_data |= ((uint32_t) (*(inbuffer + offset + 1))) << (8 * 1); 57 | this->st_data |= ((uint32_t) (*(inbuffer + offset + 2))) << (8 * 2); 58 | this->st_data |= ((uint32_t) (*(inbuffer + offset + 3))) << (8 * 3); 59 | offset += sizeof(this->st_data); 60 | memcpy( &(this->data[i]), &(this->st_data), sizeof(uint32_t)); 61 | } 62 | return offset; 63 | } 64 | 65 | const char * getType(){ return "std_msgs/UInt32MultiArray"; }; 66 | const char * getMD5(){ return "4d6a180abc9be191b96a7eda6c8a233d"; }; 67 | 68 | }; 69 | 70 | } 71 | #endif -------------------------------------------------------------------------------- /arduino/firmware/lib/ros_lib/std_msgs/Int16MultiArray.h: -------------------------------------------------------------------------------- 1 | #ifndef _ROS_std_msgs_Int16MultiArray_h 2 | #define _ROS_std_msgs_Int16MultiArray_h 3 | 4 | #include 5 | #include 6 | #include 7 | #include "ros/msg.h" 8 | #include "std_msgs/MultiArrayLayout.h" 9 | 10 | namespace std_msgs 11 | { 12 | 13 | class Int16MultiArray : public ros::Msg 14 | { 15 | public: 16 | std_msgs::MultiArrayLayout layout; 17 | uint8_t data_length; 18 | int16_t st_data; 19 | int16_t * data; 20 | 21 | Int16MultiArray(): 22 | layout(), 23 | data_length(0), data(NULL) 24 | { 25 | } 26 | 27 | virtual int serialize(unsigned char *outbuffer) const 28 | { 29 | int offset = 0; 30 | offset += this->layout.serialize(outbuffer + offset); 31 | *(outbuffer + offset++) = data_length; 32 | *(outbuffer + offset++) = 0; 33 | *(outbuffer + offset++) = 0; 34 | *(outbuffer + offset++) = 0; 35 | for( uint8_t i = 0; i < data_length; i++){ 36 | union { 37 | int16_t real; 38 | uint16_t base; 39 | } u_datai; 40 | u_datai.real = this->data[i]; 41 | *(outbuffer + offset + 0) = (u_datai.base >> (8 * 0)) & 0xFF; 42 | *(outbuffer + offset + 1) = (u_datai.base >> (8 * 1)) & 0xFF; 43 | offset += sizeof(this->data[i]); 44 | } 45 | return offset; 46 | } 47 | 48 | virtual int deserialize(unsigned char *inbuffer) 49 | { 50 | int offset = 0; 51 | offset += this->layout.deserialize(inbuffer + offset); 52 | uint8_t data_lengthT = *(inbuffer + offset++); 53 | if(data_lengthT > data_length) 54 | this->data = (int16_t*)realloc(this->data, data_lengthT * sizeof(int16_t)); 55 | offset += 3; 56 | data_length = data_lengthT; 57 | for( uint8_t i = 0; i < data_length; i++){ 58 | union { 59 | int16_t real; 60 | uint16_t base; 61 | } u_st_data; 62 | u_st_data.base = 0; 63 | u_st_data.base |= ((uint16_t) (*(inbuffer + offset + 0))) << (8 * 0); 64 | u_st_data.base |= ((uint16_t) (*(inbuffer + offset + 1))) << (8 * 1); 65 | this->st_data = u_st_data.real; 66 | offset += sizeof(this->st_data); 67 | memcpy( &(this->data[i]), &(this->st_data), sizeof(int16_t)); 68 | } 69 | return offset; 70 | } 71 | 72 | const char * getType(){ return "std_msgs/Int16MultiArray"; }; 73 | const char * getMD5(){ return "d9338d7f523fcb692fae9d0a0e9f067c"; }; 74 | 75 | }; 76 | 77 | } 78 | #endif -------------------------------------------------------------------------------- /arduino/firmware/lib/ros_lib/sensor_msgs/JoyFeedback.h: -------------------------------------------------------------------------------- 1 | #ifndef _ROS_sensor_msgs_JoyFeedback_h 2 | #define _ROS_sensor_msgs_JoyFeedback_h 3 | 4 | #include 5 | #include 6 | #include 7 | #include "ros/msg.h" 8 | 9 | namespace sensor_msgs 10 | { 11 | 12 | class JoyFeedback : public ros::Msg 13 | { 14 | public: 15 | uint8_t type; 16 | uint8_t id; 17 | float intensity; 18 | enum { TYPE_LED = 0 }; 19 | enum { TYPE_RUMBLE = 1 }; 20 | enum { TYPE_BUZZER = 2 }; 21 | 22 | JoyFeedback(): 23 | type(0), 24 | id(0), 25 | intensity(0) 26 | { 27 | } 28 | 29 | virtual int serialize(unsigned char *outbuffer) const 30 | { 31 | int offset = 0; 32 | *(outbuffer + offset + 0) = (this->type >> (8 * 0)) & 0xFF; 33 | offset += sizeof(this->type); 34 | *(outbuffer + offset + 0) = (this->id >> (8 * 0)) & 0xFF; 35 | offset += sizeof(this->id); 36 | union { 37 | float real; 38 | uint32_t base; 39 | } u_intensity; 40 | u_intensity.real = this->intensity; 41 | *(outbuffer + offset + 0) = (u_intensity.base >> (8 * 0)) & 0xFF; 42 | *(outbuffer + offset + 1) = (u_intensity.base >> (8 * 1)) & 0xFF; 43 | *(outbuffer + offset + 2) = (u_intensity.base >> (8 * 2)) & 0xFF; 44 | *(outbuffer + offset + 3) = (u_intensity.base >> (8 * 3)) & 0xFF; 45 | offset += sizeof(this->intensity); 46 | return offset; 47 | } 48 | 49 | virtual int deserialize(unsigned char *inbuffer) 50 | { 51 | int offset = 0; 52 | this->type = ((uint8_t) (*(inbuffer + offset))); 53 | offset += sizeof(this->type); 54 | this->id = ((uint8_t) (*(inbuffer + offset))); 55 | offset += sizeof(this->id); 56 | union { 57 | float real; 58 | uint32_t base; 59 | } u_intensity; 60 | u_intensity.base = 0; 61 | u_intensity.base |= ((uint32_t) (*(inbuffer + offset + 0))) << (8 * 0); 62 | u_intensity.base |= ((uint32_t) (*(inbuffer + offset + 1))) << (8 * 1); 63 | u_intensity.base |= ((uint32_t) (*(inbuffer + offset + 2))) << (8 * 2); 64 | u_intensity.base |= ((uint32_t) (*(inbuffer + offset + 3))) << (8 * 3); 65 | this->intensity = u_intensity.real; 66 | offset += sizeof(this->intensity); 67 | return offset; 68 | } 69 | 70 | const char * getType(){ return "sensor_msgs/JoyFeedback"; }; 71 | const char * getMD5(){ return "f4dcd73460360d98f36e55ee7f2e46f1"; }; 72 | 73 | }; 74 | 75 | } 76 | #endif -------------------------------------------------------------------------------- /arduino/firmware/lib/ros_lib/tf/transform_broadcaster.h: -------------------------------------------------------------------------------- 1 | /* 2 | * Software License Agreement (BSD License) 3 | * 4 | * Copyright (c) 2011, Willow Garage, Inc. 5 | * All rights reserved. 6 | * 7 | * Redistribution and use in source and binary forms, with or without 8 | * modification, are permitted provided that the following conditions 9 | * are met: 10 | * 11 | * * Redistributions of source code must retain the above copyright 12 | * notice, this list of conditions and the following disclaimer. 13 | * * Redistributions in binary form must reproduce the above 14 | * copyright notice, this list of conditions and the following 15 | * disclaimer in the documentation and/or other materials provided 16 | * with the distribution. 17 | * * Neither the name of Willow Garage, Inc. nor the names of its 18 | * contributors may be used to endorse or promote prducts derived 19 | * from this software without specific prior written permission. 20 | * 21 | * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS 22 | * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT 23 | * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS 24 | * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE 25 | * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, 26 | * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, 27 | * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; 28 | * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER 29 | * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT 30 | * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN 31 | * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE 32 | * POSSIBILITY OF SUCH DAMAGE. 33 | */ 34 | 35 | #ifndef ROS_TRANSFORM_BROADCASTER_H_ 36 | #define ROS_TRANSFORM_BROADCASTER_H_ 37 | 38 | #include "tfMessage.h" 39 | 40 | namespace tf 41 | { 42 | 43 | class TransformBroadcaster 44 | { 45 | public: 46 | TransformBroadcaster() : publisher_("/tf", &internal_msg) {} 47 | 48 | void init(ros::NodeHandle &nh) 49 | { 50 | nh.advertise(publisher_); 51 | } 52 | 53 | void sendTransform(geometry_msgs::TransformStamped &transform) 54 | { 55 | internal_msg.transforms_length = 1; 56 | internal_msg.transforms = &transform; 57 | publisher_.publish(&internal_msg); 58 | } 59 | 60 | private: 61 | tf::tfMessage internal_msg; 62 | ros::Publisher publisher_; 63 | }; 64 | 65 | } 66 | 67 | #endif 68 | 69 | -------------------------------------------------------------------------------- /arduino/firmware/lib/ros_lib/time.cpp: -------------------------------------------------------------------------------- 1 | /* 2 | * Software License Agreement (BSD License) 3 | * 4 | * Copyright (c) 2011, Willow Garage, Inc. 5 | * All rights reserved. 6 | * 7 | * Redistribution and use in source and binary forms, with or without 8 | * modification, are permitted provided that the following conditions 9 | * are met: 10 | * 11 | * * Redistributions of source code must retain the above copyright 12 | * notice, this list of conditions and the following disclaimer. 13 | * * Redistributions in binary form must reproduce the above 14 | * copyright notice, this list of conditions and the following 15 | * disclaimer in the documentation and/or other materials provided 16 | * with the distribution. 17 | * * Neither the name of Willow Garage, Inc. nor the names of its 18 | * contributors may be used to endorse or promote prducts derived 19 | * from this software without specific prior written permission. 20 | * 21 | * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS 22 | * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT 23 | * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS 24 | * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE 25 | * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, 26 | * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, 27 | * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; 28 | * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER 29 | * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT 30 | * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN 31 | * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE 32 | * POSSIBILITY OF SUCH DAMAGE. 33 | */ 34 | 35 | #include "ros/time.h" 36 | 37 | namespace ros 38 | { 39 | void normalizeSecNSec(uint32_t& sec, uint32_t& nsec){ 40 | uint32_t nsec_part= nsec % 1000000000UL; 41 | uint32_t sec_part = nsec / 1000000000UL; 42 | sec += sec_part; 43 | nsec = nsec_part; 44 | } 45 | 46 | Time& Time::fromNSec(int32_t t) 47 | { 48 | sec = t / 1000000000; 49 | nsec = t % 1000000000; 50 | normalizeSecNSec(sec, nsec); 51 | return *this; 52 | } 53 | 54 | Time& Time::operator +=(const Duration &rhs) 55 | { 56 | sec += rhs.sec; 57 | nsec += rhs.nsec; 58 | normalizeSecNSec(sec, nsec); 59 | return *this; 60 | } 61 | 62 | Time& Time::operator -=(const Duration &rhs){ 63 | sec += -rhs.sec; 64 | nsec += -rhs.nsec; 65 | normalizeSecNSec(sec, nsec); 66 | return *this; 67 | } 68 | } 69 | -------------------------------------------------------------------------------- /arduino/firmware/lib/ros_lib/sensor_msgs/LaserEcho.h: -------------------------------------------------------------------------------- 1 | #ifndef _ROS_sensor_msgs_LaserEcho_h 2 | #define _ROS_sensor_msgs_LaserEcho_h 3 | 4 | #include 5 | #include 6 | #include 7 | #include "ros/msg.h" 8 | 9 | namespace sensor_msgs 10 | { 11 | 12 | class LaserEcho : public ros::Msg 13 | { 14 | public: 15 | uint8_t echoes_length; 16 | float st_echoes; 17 | float * echoes; 18 | 19 | LaserEcho(): 20 | echoes_length(0), echoes(NULL) 21 | { 22 | } 23 | 24 | virtual int serialize(unsigned char *outbuffer) const 25 | { 26 | int offset = 0; 27 | *(outbuffer + offset++) = echoes_length; 28 | *(outbuffer + offset++) = 0; 29 | *(outbuffer + offset++) = 0; 30 | *(outbuffer + offset++) = 0; 31 | for( uint8_t i = 0; i < echoes_length; i++){ 32 | union { 33 | float real; 34 | uint32_t base; 35 | } u_echoesi; 36 | u_echoesi.real = this->echoes[i]; 37 | *(outbuffer + offset + 0) = (u_echoesi.base >> (8 * 0)) & 0xFF; 38 | *(outbuffer + offset + 1) = (u_echoesi.base >> (8 * 1)) & 0xFF; 39 | *(outbuffer + offset + 2) = (u_echoesi.base >> (8 * 2)) & 0xFF; 40 | *(outbuffer + offset + 3) = (u_echoesi.base >> (8 * 3)) & 0xFF; 41 | offset += sizeof(this->echoes[i]); 42 | } 43 | return offset; 44 | } 45 | 46 | virtual int deserialize(unsigned char *inbuffer) 47 | { 48 | int offset = 0; 49 | uint8_t echoes_lengthT = *(inbuffer + offset++); 50 | if(echoes_lengthT > echoes_length) 51 | this->echoes = (float*)realloc(this->echoes, echoes_lengthT * sizeof(float)); 52 | offset += 3; 53 | echoes_length = echoes_lengthT; 54 | for( uint8_t i = 0; i < echoes_length; i++){ 55 | union { 56 | float real; 57 | uint32_t base; 58 | } u_st_echoes; 59 | u_st_echoes.base = 0; 60 | u_st_echoes.base |= ((uint32_t) (*(inbuffer + offset + 0))) << (8 * 0); 61 | u_st_echoes.base |= ((uint32_t) (*(inbuffer + offset + 1))) << (8 * 1); 62 | u_st_echoes.base |= ((uint32_t) (*(inbuffer + offset + 2))) << (8 * 2); 63 | u_st_echoes.base |= ((uint32_t) (*(inbuffer + offset + 3))) << (8 * 3); 64 | this->st_echoes = u_st_echoes.real; 65 | offset += sizeof(this->st_echoes); 66 | memcpy( &(this->echoes[i]), &(this->st_echoes), sizeof(float)); 67 | } 68 | return offset; 69 | } 70 | 71 | const char * getType(){ return "sensor_msgs/LaserEcho"; }; 72 | const char * getMD5(){ return "8bc5ae449b200fba4d552b4225586696"; }; 73 | 74 | }; 75 | 76 | } 77 | #endif -------------------------------------------------------------------------------- /arduino/firmware/lib/ros_lib/std_msgs/MultiArrayLayout.h: -------------------------------------------------------------------------------- 1 | #ifndef _ROS_std_msgs_MultiArrayLayout_h 2 | #define _ROS_std_msgs_MultiArrayLayout_h 3 | 4 | #include 5 | #include 6 | #include 7 | #include "ros/msg.h" 8 | #include "std_msgs/MultiArrayDimension.h" 9 | 10 | namespace std_msgs 11 | { 12 | 13 | class MultiArrayLayout : public ros::Msg 14 | { 15 | public: 16 | uint8_t dim_length; 17 | std_msgs::MultiArrayDimension st_dim; 18 | std_msgs::MultiArrayDimension * dim; 19 | uint32_t data_offset; 20 | 21 | MultiArrayLayout(): 22 | dim_length(0), dim(NULL), 23 | data_offset(0) 24 | { 25 | } 26 | 27 | virtual int serialize(unsigned char *outbuffer) const 28 | { 29 | int offset = 0; 30 | *(outbuffer + offset++) = dim_length; 31 | *(outbuffer + offset++) = 0; 32 | *(outbuffer + offset++) = 0; 33 | *(outbuffer + offset++) = 0; 34 | for( uint8_t i = 0; i < dim_length; i++){ 35 | offset += this->dim[i].serialize(outbuffer + offset); 36 | } 37 | *(outbuffer + offset + 0) = (this->data_offset >> (8 * 0)) & 0xFF; 38 | *(outbuffer + offset + 1) = (this->data_offset >> (8 * 1)) & 0xFF; 39 | *(outbuffer + offset + 2) = (this->data_offset >> (8 * 2)) & 0xFF; 40 | *(outbuffer + offset + 3) = (this->data_offset >> (8 * 3)) & 0xFF; 41 | offset += sizeof(this->data_offset); 42 | return offset; 43 | } 44 | 45 | virtual int deserialize(unsigned char *inbuffer) 46 | { 47 | int offset = 0; 48 | uint8_t dim_lengthT = *(inbuffer + offset++); 49 | if(dim_lengthT > dim_length) 50 | this->dim = (std_msgs::MultiArrayDimension*)realloc(this->dim, dim_lengthT * sizeof(std_msgs::MultiArrayDimension)); 51 | offset += 3; 52 | dim_length = dim_lengthT; 53 | for( uint8_t i = 0; i < dim_length; i++){ 54 | offset += this->st_dim.deserialize(inbuffer + offset); 55 | memcpy( &(this->dim[i]), &(this->st_dim), sizeof(std_msgs::MultiArrayDimension)); 56 | } 57 | this->data_offset = ((uint32_t) (*(inbuffer + offset))); 58 | this->data_offset |= ((uint32_t) (*(inbuffer + offset + 1))) << (8 * 1); 59 | this->data_offset |= ((uint32_t) (*(inbuffer + offset + 2))) << (8 * 2); 60 | this->data_offset |= ((uint32_t) (*(inbuffer + offset + 3))) << (8 * 3); 61 | offset += sizeof(this->data_offset); 62 | return offset; 63 | } 64 | 65 | const char * getType(){ return "std_msgs/MultiArrayLayout"; }; 66 | const char * getMD5(){ return "0fed2a11c13e11c5571b4e2a995a91a3"; }; 67 | 68 | }; 69 | 70 | } 71 | #endif -------------------------------------------------------------------------------- /arduino/firmware/lib/ros_lib/ros/publisher.h: -------------------------------------------------------------------------------- 1 | /* 2 | * Software License Agreement (BSD License) 3 | * 4 | * Copyright (c) 2011, Willow Garage, Inc. 5 | * All rights reserved. 6 | * 7 | * Redistribution and use in source and binary forms, with or without 8 | * modification, are permitted provided that the following conditions 9 | * are met: 10 | * 11 | * * Redistributions of source code must retain the above copyright 12 | * notice, this list of conditions and the following disclaimer. 13 | * * Redistributions in binary form must reproduce the above 14 | * copyright notice, this list of conditions and the following 15 | * disclaimer in the documentation and/or other materials provided 16 | * with the distribution. 17 | * * Neither the name of Willow Garage, Inc. nor the names of its 18 | * contributors may be used to endorse or promote prducts derived 19 | * from this software without specific prior written permission. 20 | * 21 | * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS 22 | * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT 23 | * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS 24 | * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE 25 | * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, 26 | * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, 27 | * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; 28 | * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER 29 | * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT 30 | * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN 31 | * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE 32 | * POSSIBILITY OF SUCH DAMAGE. 33 | */ 34 | 35 | #ifndef _ROS_PUBLISHER_H_ 36 | #define _ROS_PUBLISHER_H_ 37 | 38 | #include "rosserial_msgs/TopicInfo.h" 39 | #include "node_handle.h" 40 | 41 | namespace ros { 42 | 43 | /* Generic Publisher */ 44 | class Publisher 45 | { 46 | public: 47 | Publisher( const char * topic_name, Msg * msg, int endpoint=rosserial_msgs::TopicInfo::ID_PUBLISHER) : 48 | topic_(topic_name), 49 | msg_(msg), 50 | endpoint_(endpoint) {}; 51 | 52 | int publish( const Msg * msg ) { return nh_->publish(id_, msg); }; 53 | int getEndpointType(){ return endpoint_; } 54 | 55 | const char * topic_; 56 | Msg *msg_; 57 | // id_ and no_ are set by NodeHandle when we advertise 58 | int id_; 59 | NodeHandleBase_* nh_; 60 | 61 | private: 62 | int endpoint_; 63 | }; 64 | 65 | } 66 | 67 | #endif 68 | -------------------------------------------------------------------------------- /arduino/firmware/lib/ros_lib/ros/duration.h: -------------------------------------------------------------------------------- 1 | /* 2 | * Software License Agreement (BSD License) 3 | * 4 | * Copyright (c) 2011, Willow Garage, Inc. 5 | * All rights reserved. 6 | * 7 | * Redistribution and use in source and binary forms, with or without 8 | * modification, are permitted provided that the following conditions 9 | * are met: 10 | * 11 | * * Redistributions of source code must retain the above copyright 12 | * notice, this list of conditions and the following disclaimer. 13 | * * Redistributions in binary form must reproduce the above 14 | * copyright notice, this list of conditions and the following 15 | * disclaimer in the documentation and/or other materials provided 16 | * with the distribution. 17 | * * Neither the name of Willow Garage, Inc. nor the names of its 18 | * contributors may be used to endorse or promote prducts derived 19 | * from this software without specific prior written permission. 20 | * 21 | * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS 22 | * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT 23 | * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS 24 | * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE 25 | * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, 26 | * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, 27 | * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; 28 | * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER 29 | * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT 30 | * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN 31 | * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE 32 | * POSSIBILITY OF SUCH DAMAGE. 33 | */ 34 | 35 | #ifndef _ROS_DURATION_H_ 36 | #define _ROS_DURATION_H_ 37 | 38 | #include 39 | #include 40 | 41 | namespace ros { 42 | 43 | void normalizeSecNSecSigned(int32_t& sec, int32_t& nsec); 44 | 45 | class Duration 46 | { 47 | public: 48 | int32_t sec, nsec; 49 | 50 | Duration() : sec(0), nsec(0) {} 51 | Duration(int32_t _sec, int32_t _nsec) : sec(_sec), nsec(_nsec) 52 | { 53 | normalizeSecNSecSigned(sec, nsec); 54 | } 55 | 56 | double toSec() const { return (double)sec + 1e-9*(double)nsec; }; 57 | void fromSec(double t) { sec = (uint32_t) floor(t); nsec = (uint32_t) round((t-sec) * 1e9); }; 58 | 59 | Duration& operator+=(const Duration &rhs); 60 | Duration& operator-=(const Duration &rhs); 61 | Duration& operator*=(double scale); 62 | }; 63 | 64 | } 65 | 66 | #endif 67 | 68 | -------------------------------------------------------------------------------- /arduino/firmware/lib/ros_lib/sensor_msgs/CompressedImage.h: -------------------------------------------------------------------------------- 1 | #ifndef _ROS_sensor_msgs_CompressedImage_h 2 | #define _ROS_sensor_msgs_CompressedImage_h 3 | 4 | #include 5 | #include 6 | #include 7 | #include "ros/msg.h" 8 | #include "std_msgs/Header.h" 9 | 10 | namespace sensor_msgs 11 | { 12 | 13 | class CompressedImage : public ros::Msg 14 | { 15 | public: 16 | std_msgs::Header header; 17 | const char* format; 18 | uint8_t data_length; 19 | uint8_t st_data; 20 | uint8_t * data; 21 | 22 | CompressedImage(): 23 | header(), 24 | format(""), 25 | data_length(0), data(NULL) 26 | { 27 | } 28 | 29 | virtual int serialize(unsigned char *outbuffer) const 30 | { 31 | int offset = 0; 32 | offset += this->header.serialize(outbuffer + offset); 33 | uint32_t length_format = strlen(this->format); 34 | memcpy(outbuffer + offset, &length_format, sizeof(uint32_t)); 35 | offset += 4; 36 | memcpy(outbuffer + offset, this->format, length_format); 37 | offset += length_format; 38 | *(outbuffer + offset++) = data_length; 39 | *(outbuffer + offset++) = 0; 40 | *(outbuffer + offset++) = 0; 41 | *(outbuffer + offset++) = 0; 42 | for( uint8_t i = 0; i < data_length; i++){ 43 | *(outbuffer + offset + 0) = (this->data[i] >> (8 * 0)) & 0xFF; 44 | offset += sizeof(this->data[i]); 45 | } 46 | return offset; 47 | } 48 | 49 | virtual int deserialize(unsigned char *inbuffer) 50 | { 51 | int offset = 0; 52 | offset += this->header.deserialize(inbuffer + offset); 53 | uint32_t length_format; 54 | memcpy(&length_format, (inbuffer + offset), sizeof(uint32_t)); 55 | offset += 4; 56 | for(unsigned int k= offset; k< offset+length_format; ++k){ 57 | inbuffer[k-1]=inbuffer[k]; 58 | } 59 | inbuffer[offset+length_format-1]=0; 60 | this->format = (char *)(inbuffer + offset-1); 61 | offset += length_format; 62 | uint8_t data_lengthT = *(inbuffer + offset++); 63 | if(data_lengthT > data_length) 64 | this->data = (uint8_t*)realloc(this->data, data_lengthT * sizeof(uint8_t)); 65 | offset += 3; 66 | data_length = data_lengthT; 67 | for( uint8_t i = 0; i < data_length; i++){ 68 | this->st_data = ((uint8_t) (*(inbuffer + offset))); 69 | offset += sizeof(this->st_data); 70 | memcpy( &(this->data[i]), &(this->st_data), sizeof(uint8_t)); 71 | } 72 | return offset; 73 | } 74 | 75 | const char * getType(){ return "sensor_msgs/CompressedImage"; }; 76 | const char * getMD5(){ return "8f7a12909da2c9d3332d540a0977563f"; }; 77 | 78 | }; 79 | 80 | } 81 | #endif -------------------------------------------------------------------------------- /arduino/firmware/lib/ros_lib/ros/time.h: -------------------------------------------------------------------------------- 1 | /* 2 | * Software License Agreement (BSD License) 3 | * 4 | * Copyright (c) 2011, Willow Garage, Inc. 5 | * All rights reserved. 6 | * 7 | * Redistribution and use in source and binary forms, with or without 8 | * modification, are permitted provided that the following conditions 9 | * are met: 10 | * 11 | * * Redistributions of source code must retain the above copyright 12 | * notice, this list of conditions and the following disclaimer. 13 | * * Redistributions in binary form must reproduce the above 14 | * copyright notice, this list of conditions and the following 15 | * disclaimer in the documentation and/or other materials provided 16 | * with the distribution. 17 | * * Neither the name of Willow Garage, Inc. nor the names of its 18 | * contributors may be used to endorse or promote prducts derived 19 | * from this software without specific prior written permission. 20 | * 21 | * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS 22 | * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT 23 | * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS 24 | * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE 25 | * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, 26 | * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, 27 | * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; 28 | * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER 29 | * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT 30 | * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN 31 | * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE 32 | * POSSIBILITY OF SUCH DAMAGE. 33 | */ 34 | 35 | #ifndef ROS_TIME_H_ 36 | #define ROS_TIME_H_ 37 | 38 | #include 39 | #include 40 | 41 | #include "ros/duration.h" 42 | 43 | namespace ros 44 | { 45 | void normalizeSecNSec(uint32_t &sec, uint32_t &nsec); 46 | 47 | class Time 48 | { 49 | public: 50 | uint32_t sec, nsec; 51 | 52 | Time() : sec(0), nsec(0) {} 53 | Time(uint32_t _sec, uint32_t _nsec) : sec(_sec), nsec(_nsec) 54 | { 55 | normalizeSecNSec(sec, nsec); 56 | } 57 | 58 | double toSec() const { return (double)sec + 1e-9*(double)nsec; }; 59 | void fromSec(double t) { sec = (uint32_t) floor(t); nsec = (uint32_t) round((t-sec) * 1e9); }; 60 | 61 | uint32_t toNsec() { return (uint32_t)sec*1000000000ull + (uint32_t)nsec; }; 62 | Time& fromNSec(int32_t t); 63 | 64 | Time& operator +=(const Duration &rhs); 65 | Time& operator -=(const Duration &rhs); 66 | 67 | static Time now(); 68 | static void setNow( Time & new_now); 69 | }; 70 | 71 | } 72 | 73 | #endif 74 | -------------------------------------------------------------------------------- /arduino/firmware/lib/ros_lib/std_msgs/Float32MultiArray.h: -------------------------------------------------------------------------------- 1 | #ifndef _ROS_std_msgs_Float32MultiArray_h 2 | #define _ROS_std_msgs_Float32MultiArray_h 3 | 4 | #include 5 | #include 6 | #include 7 | #include "ros/msg.h" 8 | #include "std_msgs/MultiArrayLayout.h" 9 | 10 | namespace std_msgs 11 | { 12 | 13 | class Float32MultiArray : public ros::Msg 14 | { 15 | public: 16 | std_msgs::MultiArrayLayout layout; 17 | uint8_t data_length; 18 | float st_data; 19 | float * data; 20 | 21 | Float32MultiArray(): 22 | layout(), 23 | data_length(0), data(NULL) 24 | { 25 | } 26 | 27 | virtual int serialize(unsigned char *outbuffer) const 28 | { 29 | int offset = 0; 30 | offset += this->layout.serialize(outbuffer + offset); 31 | *(outbuffer + offset++) = data_length; 32 | *(outbuffer + offset++) = 0; 33 | *(outbuffer + offset++) = 0; 34 | *(outbuffer + offset++) = 0; 35 | for( uint8_t i = 0; i < data_length; i++){ 36 | union { 37 | float real; 38 | uint32_t base; 39 | } u_datai; 40 | u_datai.real = this->data[i]; 41 | *(outbuffer + offset + 0) = (u_datai.base >> (8 * 0)) & 0xFF; 42 | *(outbuffer + offset + 1) = (u_datai.base >> (8 * 1)) & 0xFF; 43 | *(outbuffer + offset + 2) = (u_datai.base >> (8 * 2)) & 0xFF; 44 | *(outbuffer + offset + 3) = (u_datai.base >> (8 * 3)) & 0xFF; 45 | offset += sizeof(this->data[i]); 46 | } 47 | return offset; 48 | } 49 | 50 | virtual int deserialize(unsigned char *inbuffer) 51 | { 52 | int offset = 0; 53 | offset += this->layout.deserialize(inbuffer + offset); 54 | uint8_t data_lengthT = *(inbuffer + offset++); 55 | if(data_lengthT > data_length) 56 | this->data = (float*)realloc(this->data, data_lengthT * sizeof(float)); 57 | offset += 3; 58 | data_length = data_lengthT; 59 | for( uint8_t i = 0; i < data_length; i++){ 60 | union { 61 | float real; 62 | uint32_t base; 63 | } u_st_data; 64 | u_st_data.base = 0; 65 | u_st_data.base |= ((uint32_t) (*(inbuffer + offset + 0))) << (8 * 0); 66 | u_st_data.base |= ((uint32_t) (*(inbuffer + offset + 1))) << (8 * 1); 67 | u_st_data.base |= ((uint32_t) (*(inbuffer + offset + 2))) << (8 * 2); 68 | u_st_data.base |= ((uint32_t) (*(inbuffer + offset + 3))) << (8 * 3); 69 | this->st_data = u_st_data.real; 70 | offset += sizeof(this->st_data); 71 | memcpy( &(this->data[i]), &(this->st_data), sizeof(float)); 72 | } 73 | return offset; 74 | } 75 | 76 | const char * getType(){ return "std_msgs/Float32MultiArray"; }; 77 | const char * getMD5(){ return "6a40e0ffa6a17a503ac3f8616991b1f6"; }; 78 | 79 | }; 80 | 81 | } 82 | #endif -------------------------------------------------------------------------------- /arduino/firmware/lib/ros_lib/std_msgs/Int32MultiArray.h: -------------------------------------------------------------------------------- 1 | #ifndef _ROS_std_msgs_Int32MultiArray_h 2 | #define _ROS_std_msgs_Int32MultiArray_h 3 | 4 | #include 5 | #include 6 | #include 7 | #include "ros/msg.h" 8 | #include "std_msgs/MultiArrayLayout.h" 9 | 10 | namespace std_msgs 11 | { 12 | 13 | class Int32MultiArray : public ros::Msg 14 | { 15 | public: 16 | std_msgs::MultiArrayLayout layout; 17 | uint8_t data_length; 18 | int32_t st_data; 19 | int32_t * data; 20 | 21 | Int32MultiArray(): 22 | layout(), 23 | data_length(0), data(NULL) 24 | { 25 | } 26 | 27 | virtual int serialize(unsigned char *outbuffer) const 28 | { 29 | int offset = 0; 30 | offset += this->layout.serialize(outbuffer + offset); 31 | *(outbuffer + offset++) = data_length; 32 | *(outbuffer + offset++) = 0; 33 | *(outbuffer + offset++) = 0; 34 | *(outbuffer + offset++) = 0; 35 | for( uint8_t i = 0; i < data_length; i++){ 36 | union { 37 | int32_t real; 38 | uint32_t base; 39 | } u_datai; 40 | u_datai.real = this->data[i]; 41 | *(outbuffer + offset + 0) = (u_datai.base >> (8 * 0)) & 0xFF; 42 | *(outbuffer + offset + 1) = (u_datai.base >> (8 * 1)) & 0xFF; 43 | *(outbuffer + offset + 2) = (u_datai.base >> (8 * 2)) & 0xFF; 44 | *(outbuffer + offset + 3) = (u_datai.base >> (8 * 3)) & 0xFF; 45 | offset += sizeof(this->data[i]); 46 | } 47 | return offset; 48 | } 49 | 50 | virtual int deserialize(unsigned char *inbuffer) 51 | { 52 | int offset = 0; 53 | offset += this->layout.deserialize(inbuffer + offset); 54 | uint8_t data_lengthT = *(inbuffer + offset++); 55 | if(data_lengthT > data_length) 56 | this->data = (int32_t*)realloc(this->data, data_lengthT * sizeof(int32_t)); 57 | offset += 3; 58 | data_length = data_lengthT; 59 | for( uint8_t i = 0; i < data_length; i++){ 60 | union { 61 | int32_t real; 62 | uint32_t base; 63 | } u_st_data; 64 | u_st_data.base = 0; 65 | u_st_data.base |= ((uint32_t) (*(inbuffer + offset + 0))) << (8 * 0); 66 | u_st_data.base |= ((uint32_t) (*(inbuffer + offset + 1))) << (8 * 1); 67 | u_st_data.base |= ((uint32_t) (*(inbuffer + offset + 2))) << (8 * 2); 68 | u_st_data.base |= ((uint32_t) (*(inbuffer + offset + 3))) << (8 * 3); 69 | this->st_data = u_st_data.real; 70 | offset += sizeof(this->st_data); 71 | memcpy( &(this->data[i]), &(this->st_data), sizeof(int32_t)); 72 | } 73 | return offset; 74 | } 75 | 76 | const char * getType(){ return "std_msgs/Int32MultiArray"; }; 77 | const char * getMD5(){ return "1d99f79f8b325b44fee908053e9c945b"; }; 78 | 79 | }; 80 | 81 | } 82 | #endif -------------------------------------------------------------------------------- /arduino/firmware/lib/ros_lib/ros_arduino_msgs/CmdDiffVel.h: -------------------------------------------------------------------------------- 1 | #ifndef _ROS_ros_arduino_msgs_CmdDiffVel_h 2 | #define _ROS_ros_arduino_msgs_CmdDiffVel_h 3 | 4 | #include 5 | #include 6 | #include 7 | #include "ros/msg.h" 8 | 9 | namespace ros_arduino_msgs 10 | { 11 | 12 | class CmdDiffVel : public ros::Msg 13 | { 14 | public: 15 | float left; 16 | float right; 17 | 18 | CmdDiffVel(): 19 | left(0), 20 | right(0) 21 | { 22 | } 23 | 24 | virtual int serialize(unsigned char *outbuffer) const 25 | { 26 | int offset = 0; 27 | union { 28 | float real; 29 | uint32_t base; 30 | } u_left; 31 | u_left.real = this->left; 32 | *(outbuffer + offset + 0) = (u_left.base >> (8 * 0)) & 0xFF; 33 | *(outbuffer + offset + 1) = (u_left.base >> (8 * 1)) & 0xFF; 34 | *(outbuffer + offset + 2) = (u_left.base >> (8 * 2)) & 0xFF; 35 | *(outbuffer + offset + 3) = (u_left.base >> (8 * 3)) & 0xFF; 36 | offset += sizeof(this->left); 37 | union { 38 | float real; 39 | uint32_t base; 40 | } u_right; 41 | u_right.real = this->right; 42 | *(outbuffer + offset + 0) = (u_right.base >> (8 * 0)) & 0xFF; 43 | *(outbuffer + offset + 1) = (u_right.base >> (8 * 1)) & 0xFF; 44 | *(outbuffer + offset + 2) = (u_right.base >> (8 * 2)) & 0xFF; 45 | *(outbuffer + offset + 3) = (u_right.base >> (8 * 3)) & 0xFF; 46 | offset += sizeof(this->right); 47 | return offset; 48 | } 49 | 50 | virtual int deserialize(unsigned char *inbuffer) 51 | { 52 | int offset = 0; 53 | union { 54 | float real; 55 | uint32_t base; 56 | } u_left; 57 | u_left.base = 0; 58 | u_left.base |= ((uint32_t) (*(inbuffer + offset + 0))) << (8 * 0); 59 | u_left.base |= ((uint32_t) (*(inbuffer + offset + 1))) << (8 * 1); 60 | u_left.base |= ((uint32_t) (*(inbuffer + offset + 2))) << (8 * 2); 61 | u_left.base |= ((uint32_t) (*(inbuffer + offset + 3))) << (8 * 3); 62 | this->left = u_left.real; 63 | offset += sizeof(this->left); 64 | union { 65 | float real; 66 | uint32_t base; 67 | } u_right; 68 | u_right.base = 0; 69 | u_right.base |= ((uint32_t) (*(inbuffer + offset + 0))) << (8 * 0); 70 | u_right.base |= ((uint32_t) (*(inbuffer + offset + 1))) << (8 * 1); 71 | u_right.base |= ((uint32_t) (*(inbuffer + offset + 2))) << (8 * 2); 72 | u_right.base |= ((uint32_t) (*(inbuffer + offset + 3))) << (8 * 3); 73 | this->right = u_right.real; 74 | offset += sizeof(this->right); 75 | return offset; 76 | } 77 | 78 | const char * getType(){ return "ros_arduino_msgs/CmdDiffVel"; }; 79 | const char * getMD5(){ return "3a927990ab5d5c3d628e2d52b8533e52"; }; 80 | 81 | }; 82 | 83 | } 84 | #endif -------------------------------------------------------------------------------- /arduino/firmware/lib/kinematics/Kinematics.h: -------------------------------------------------------------------------------- 1 | /* 2 | Copyright (c) 2016, Juan Jimeno 3 | Source: http://research.ijcaonline.org/volume113/number3/pxc3901586.pdf 4 | All rights reserved. 5 | Redistribution and use in source and binary forms, with or without 6 | modification, are permitted provided that the following conditions are met: 7 | Redistributions of source code must retain the above copyright notice, 8 | this list of conditions and the following disclaimer. 9 | Redistributions in binary form must reproduce the above copyright 10 | notice, this list of conditions and the following disclaimer in the 11 | documentation and/or other materials provided with the distribution. 12 | Neither the name of nor the names of its contributors may be used to 13 | endorse or promote products derived from this software without specific 14 | prior written permission. 15 | THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" 16 | AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE 17 | IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE 18 | ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE 19 | LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR 20 | CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF 21 | SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS 22 | INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN 23 | CONTRACT, STRICT LIABILITY, OR TORTPPIPI (INCLUDING NEGLIGENCE OR OTHERWISE) 24 | ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE 25 | POSSIBILITY OF SUCH DAMAGE. 26 | */ 27 | 28 | #ifndef KINEMATICS_H 29 | #define KINEMATICS_H 30 | 31 | #include "Arduino.h" 32 | 33 | class Kinematics 34 | { 35 | public: 36 | struct output 37 | { 38 | int motor1; 39 | int motor2; 40 | int motor3; 41 | int motor4; 42 | }; 43 | struct velocities 44 | { 45 | float linear_x; 46 | float linear_y; 47 | float angular_z; 48 | }; 49 | Kinematics(int motor_max_rpm, float wheel_diameter, float base_width, int pwm_bits); 50 | velocities getVelocities(int motor1, int motor2); 51 | velocities getVelocities(int motor1, int motor2, int motor3, int motor4); 52 | output getRPM(float linear_x, float linear_y, float angular_z); 53 | output getPWM(float linear_x, float linear_y, float angular_z); 54 | int rpmToPWM(int rpm); 55 | 56 | private: 57 | float linear_vel_x_mins_; 58 | float linear_vel_y_mins_; 59 | float angular_vel_z_mins_; 60 | float circumference_; 61 | float tangential_vel_; 62 | float x_rpm_; 63 | float y_rpm_; 64 | float tan_rpm_; 65 | int max_rpm_; 66 | double wheel_diameter_; 67 | float base_width_; 68 | double pwm_res_; 69 | }; 70 | 71 | #endif 72 | -------------------------------------------------------------------------------- /arduino/firmware/lib/ros_lib/duration.cpp: -------------------------------------------------------------------------------- 1 | /* 2 | * Software License Agreement (BSD License) 3 | * 4 | * Copyright (c) 2011, Willow Garage, Inc. 5 | * All rights reserved. 6 | * 7 | * Redistribution and use in source and binary forms, with or without 8 | * modification, are permitted provided that the following conditions 9 | * are met: 10 | * 11 | * * Redistributions of source code must retain the above copyright 12 | * notice, this list of conditions and the following disclaimer. 13 | * * Redistributions in binary form must reproduce the above 14 | * copyright notice, this list of conditions and the following 15 | * disclaimer in the documentation and/or other materials provided 16 | * with the distribution. 17 | * * Neither the name of Willow Garage, Inc. nor the names of its 18 | * contributors may be used to endorse or promote prducts derived 19 | * from this software without specific prior written permission. 20 | * 21 | * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS 22 | * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT 23 | * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS 24 | * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE 25 | * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, 26 | * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, 27 | * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; 28 | * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER 29 | * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT 30 | * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN 31 | * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE 32 | * POSSIBILITY OF SUCH DAMAGE. 33 | */ 34 | 35 | #include 36 | #include "ros/duration.h" 37 | 38 | namespace ros 39 | { 40 | void normalizeSecNSecSigned(int32_t &sec, int32_t &nsec) 41 | { 42 | int32_t nsec_part = nsec; 43 | int32_t sec_part = sec; 44 | 45 | while (nsec_part > 1000000000L) 46 | { 47 | nsec_part -= 1000000000L; 48 | ++sec_part; 49 | } 50 | while (nsec_part < 0) 51 | { 52 | nsec_part += 1000000000L; 53 | --sec_part; 54 | } 55 | sec = sec_part; 56 | nsec = nsec_part; 57 | } 58 | 59 | Duration& Duration::operator+=(const Duration &rhs) 60 | { 61 | sec += rhs.sec; 62 | nsec += rhs.nsec; 63 | normalizeSecNSecSigned(sec, nsec); 64 | return *this; 65 | } 66 | 67 | Duration& Duration::operator-=(const Duration &rhs){ 68 | sec += -rhs.sec; 69 | nsec += -rhs.nsec; 70 | normalizeSecNSecSigned(sec, nsec); 71 | return *this; 72 | } 73 | 74 | Duration& Duration::operator*=(double scale){ 75 | sec *= scale; 76 | nsec *= scale; 77 | normalizeSecNSecSigned(sec, nsec); 78 | return *this; 79 | } 80 | 81 | } 82 | -------------------------------------------------------------------------------- /launch/amcl.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | 16 | 17 | 18 | 19 | 20 | 21 | 22 | 23 | 24 | 25 | 26 | 27 | 28 | 29 | 30 | 31 | 32 | 33 | 34 | 35 | 36 | 37 | -------------------------------------------------------------------------------- /arduino/firmware/lib/ros_lib/std_msgs/MultiArrayDimension.h: -------------------------------------------------------------------------------- 1 | #ifndef _ROS_std_msgs_MultiArrayDimension_h 2 | #define _ROS_std_msgs_MultiArrayDimension_h 3 | 4 | #include 5 | #include 6 | #include 7 | #include "ros/msg.h" 8 | 9 | namespace std_msgs 10 | { 11 | 12 | class MultiArrayDimension : public ros::Msg 13 | { 14 | public: 15 | const char* label; 16 | uint32_t size; 17 | uint32_t stride; 18 | 19 | MultiArrayDimension(): 20 | label(""), 21 | size(0), 22 | stride(0) 23 | { 24 | } 25 | 26 | virtual int serialize(unsigned char *outbuffer) const 27 | { 28 | int offset = 0; 29 | uint32_t length_label = strlen(this->label); 30 | memcpy(outbuffer + offset, &length_label, sizeof(uint32_t)); 31 | offset += 4; 32 | memcpy(outbuffer + offset, this->label, length_label); 33 | offset += length_label; 34 | *(outbuffer + offset + 0) = (this->size >> (8 * 0)) & 0xFF; 35 | *(outbuffer + offset + 1) = (this->size >> (8 * 1)) & 0xFF; 36 | *(outbuffer + offset + 2) = (this->size >> (8 * 2)) & 0xFF; 37 | *(outbuffer + offset + 3) = (this->size >> (8 * 3)) & 0xFF; 38 | offset += sizeof(this->size); 39 | *(outbuffer + offset + 0) = (this->stride >> (8 * 0)) & 0xFF; 40 | *(outbuffer + offset + 1) = (this->stride >> (8 * 1)) & 0xFF; 41 | *(outbuffer + offset + 2) = (this->stride >> (8 * 2)) & 0xFF; 42 | *(outbuffer + offset + 3) = (this->stride >> (8 * 3)) & 0xFF; 43 | offset += sizeof(this->stride); 44 | return offset; 45 | } 46 | 47 | virtual int deserialize(unsigned char *inbuffer) 48 | { 49 | int offset = 0; 50 | uint32_t length_label; 51 | memcpy(&length_label, (inbuffer + offset), sizeof(uint32_t)); 52 | offset += 4; 53 | for(unsigned int k= offset; k< offset+length_label; ++k){ 54 | inbuffer[k-1]=inbuffer[k]; 55 | } 56 | inbuffer[offset+length_label-1]=0; 57 | this->label = (char *)(inbuffer + offset-1); 58 | offset += length_label; 59 | this->size = ((uint32_t) (*(inbuffer + offset))); 60 | this->size |= ((uint32_t) (*(inbuffer + offset + 1))) << (8 * 1); 61 | this->size |= ((uint32_t) (*(inbuffer + offset + 2))) << (8 * 2); 62 | this->size |= ((uint32_t) (*(inbuffer + offset + 3))) << (8 * 3); 63 | offset += sizeof(this->size); 64 | this->stride = ((uint32_t) (*(inbuffer + offset))); 65 | this->stride |= ((uint32_t) (*(inbuffer + offset + 1))) << (8 * 1); 66 | this->stride |= ((uint32_t) (*(inbuffer + offset + 2))) << (8 * 2); 67 | this->stride |= ((uint32_t) (*(inbuffer + offset + 3))) << (8 * 3); 68 | offset += sizeof(this->stride); 69 | return offset; 70 | } 71 | 72 | const char * getType(){ return "std_msgs/MultiArrayDimension"; }; 73 | const char * getMD5(){ return "4cd0c83a8683deae40ecdac60e53bfa8"; }; 74 | 75 | }; 76 | 77 | } 78 | #endif -------------------------------------------------------------------------------- /arduino/firmware/lib/ros_lib/geometry_msgs/PoseWithCovariance.h: -------------------------------------------------------------------------------- 1 | #ifndef _ROS_geometry_msgs_PoseWithCovariance_h 2 | #define _ROS_geometry_msgs_PoseWithCovariance_h 3 | 4 | #include 5 | #include 6 | #include 7 | #include "ros/msg.h" 8 | #include "geometry_msgs/Pose.h" 9 | 10 | namespace geometry_msgs 11 | { 12 | 13 | class PoseWithCovariance : public ros::Msg 14 | { 15 | public: 16 | geometry_msgs::Pose pose; 17 | double covariance[36]; 18 | 19 | PoseWithCovariance(): 20 | pose(), 21 | covariance() 22 | { 23 | } 24 | 25 | virtual int serialize(unsigned char *outbuffer) const 26 | { 27 | int offset = 0; 28 | offset += this->pose.serialize(outbuffer + offset); 29 | for( uint8_t i = 0; i < 36; i++){ 30 | union { 31 | double real; 32 | uint64_t base; 33 | } u_covariancei; 34 | u_covariancei.real = this->covariance[i]; 35 | *(outbuffer + offset + 0) = (u_covariancei.base >> (8 * 0)) & 0xFF; 36 | *(outbuffer + offset + 1) = (u_covariancei.base >> (8 * 1)) & 0xFF; 37 | *(outbuffer + offset + 2) = (u_covariancei.base >> (8 * 2)) & 0xFF; 38 | *(outbuffer + offset + 3) = (u_covariancei.base >> (8 * 3)) & 0xFF; 39 | *(outbuffer + offset + 4) = (u_covariancei.base >> (8 * 4)) & 0xFF; 40 | *(outbuffer + offset + 5) = (u_covariancei.base >> (8 * 5)) & 0xFF; 41 | *(outbuffer + offset + 6) = (u_covariancei.base >> (8 * 6)) & 0xFF; 42 | *(outbuffer + offset + 7) = (u_covariancei.base >> (8 * 7)) & 0xFF; 43 | offset += sizeof(this->covariance[i]); 44 | } 45 | return offset; 46 | } 47 | 48 | virtual int deserialize(unsigned char *inbuffer) 49 | { 50 | int offset = 0; 51 | offset += this->pose.deserialize(inbuffer + offset); 52 | for( uint8_t i = 0; i < 36; i++){ 53 | union { 54 | double real; 55 | uint64_t base; 56 | } u_covariancei; 57 | u_covariancei.base = 0; 58 | u_covariancei.base |= ((uint64_t) (*(inbuffer + offset + 0))) << (8 * 0); 59 | u_covariancei.base |= ((uint64_t) (*(inbuffer + offset + 1))) << (8 * 1); 60 | u_covariancei.base |= ((uint64_t) (*(inbuffer + offset + 2))) << (8 * 2); 61 | u_covariancei.base |= ((uint64_t) (*(inbuffer + offset + 3))) << (8 * 3); 62 | u_covariancei.base |= ((uint64_t) (*(inbuffer + offset + 4))) << (8 * 4); 63 | u_covariancei.base |= ((uint64_t) (*(inbuffer + offset + 5))) << (8 * 5); 64 | u_covariancei.base |= ((uint64_t) (*(inbuffer + offset + 6))) << (8 * 6); 65 | u_covariancei.base |= ((uint64_t) (*(inbuffer + offset + 7))) << (8 * 7); 66 | this->covariance[i] = u_covariancei.real; 67 | offset += sizeof(this->covariance[i]); 68 | } 69 | return offset; 70 | } 71 | 72 | const char * getType(){ return "geometry_msgs/PoseWithCovariance"; }; 73 | const char * getMD5(){ return "c23e848cf1b7533a8d7c259073a97e6f"; }; 74 | 75 | }; 76 | 77 | } 78 | #endif -------------------------------------------------------------------------------- /arduino/firmware/lib/ros_lib/geometry_msgs/AccelWithCovariance.h: -------------------------------------------------------------------------------- 1 | #ifndef _ROS_geometry_msgs_AccelWithCovariance_h 2 | #define _ROS_geometry_msgs_AccelWithCovariance_h 3 | 4 | #include 5 | #include 6 | #include 7 | #include "ros/msg.h" 8 | #include "geometry_msgs/Accel.h" 9 | 10 | namespace geometry_msgs 11 | { 12 | 13 | class AccelWithCovariance : public ros::Msg 14 | { 15 | public: 16 | geometry_msgs::Accel accel; 17 | double covariance[36]; 18 | 19 | AccelWithCovariance(): 20 | accel(), 21 | covariance() 22 | { 23 | } 24 | 25 | virtual int serialize(unsigned char *outbuffer) const 26 | { 27 | int offset = 0; 28 | offset += this->accel.serialize(outbuffer + offset); 29 | for( uint8_t i = 0; i < 36; i++){ 30 | union { 31 | double real; 32 | uint64_t base; 33 | } u_covariancei; 34 | u_covariancei.real = this->covariance[i]; 35 | *(outbuffer + offset + 0) = (u_covariancei.base >> (8 * 0)) & 0xFF; 36 | *(outbuffer + offset + 1) = (u_covariancei.base >> (8 * 1)) & 0xFF; 37 | *(outbuffer + offset + 2) = (u_covariancei.base >> (8 * 2)) & 0xFF; 38 | *(outbuffer + offset + 3) = (u_covariancei.base >> (8 * 3)) & 0xFF; 39 | *(outbuffer + offset + 4) = (u_covariancei.base >> (8 * 4)) & 0xFF; 40 | *(outbuffer + offset + 5) = (u_covariancei.base >> (8 * 5)) & 0xFF; 41 | *(outbuffer + offset + 6) = (u_covariancei.base >> (8 * 6)) & 0xFF; 42 | *(outbuffer + offset + 7) = (u_covariancei.base >> (8 * 7)) & 0xFF; 43 | offset += sizeof(this->covariance[i]); 44 | } 45 | return offset; 46 | } 47 | 48 | virtual int deserialize(unsigned char *inbuffer) 49 | { 50 | int offset = 0; 51 | offset += this->accel.deserialize(inbuffer + offset); 52 | for( uint8_t i = 0; i < 36; i++){ 53 | union { 54 | double real; 55 | uint64_t base; 56 | } u_covariancei; 57 | u_covariancei.base = 0; 58 | u_covariancei.base |= ((uint64_t) (*(inbuffer + offset + 0))) << (8 * 0); 59 | u_covariancei.base |= ((uint64_t) (*(inbuffer + offset + 1))) << (8 * 1); 60 | u_covariancei.base |= ((uint64_t) (*(inbuffer + offset + 2))) << (8 * 2); 61 | u_covariancei.base |= ((uint64_t) (*(inbuffer + offset + 3))) << (8 * 3); 62 | u_covariancei.base |= ((uint64_t) (*(inbuffer + offset + 4))) << (8 * 4); 63 | u_covariancei.base |= ((uint64_t) (*(inbuffer + offset + 5))) << (8 * 5); 64 | u_covariancei.base |= ((uint64_t) (*(inbuffer + offset + 6))) << (8 * 6); 65 | u_covariancei.base |= ((uint64_t) (*(inbuffer + offset + 7))) << (8 * 7); 66 | this->covariance[i] = u_covariancei.real; 67 | offset += sizeof(this->covariance[i]); 68 | } 69 | return offset; 70 | } 71 | 72 | const char * getType(){ return "geometry_msgs/AccelWithCovariance"; }; 73 | const char * getMD5(){ return "ad5a718d699c6be72a02b8d6a139f334"; }; 74 | 75 | }; 76 | 77 | } 78 | #endif -------------------------------------------------------------------------------- /arduino/firmware/lib/ros_lib/geometry_msgs/TwistWithCovariance.h: -------------------------------------------------------------------------------- 1 | #ifndef _ROS_geometry_msgs_TwistWithCovariance_h 2 | #define _ROS_geometry_msgs_TwistWithCovariance_h 3 | 4 | #include 5 | #include 6 | #include 7 | #include "ros/msg.h" 8 | #include "geometry_msgs/Twist.h" 9 | 10 | namespace geometry_msgs 11 | { 12 | 13 | class TwistWithCovariance : public ros::Msg 14 | { 15 | public: 16 | geometry_msgs::Twist twist; 17 | double covariance[36]; 18 | 19 | TwistWithCovariance(): 20 | twist(), 21 | covariance() 22 | { 23 | } 24 | 25 | virtual int serialize(unsigned char *outbuffer) const 26 | { 27 | int offset = 0; 28 | offset += this->twist.serialize(outbuffer + offset); 29 | for( uint8_t i = 0; i < 36; i++){ 30 | union { 31 | double real; 32 | uint64_t base; 33 | } u_covariancei; 34 | u_covariancei.real = this->covariance[i]; 35 | *(outbuffer + offset + 0) = (u_covariancei.base >> (8 * 0)) & 0xFF; 36 | *(outbuffer + offset + 1) = (u_covariancei.base >> (8 * 1)) & 0xFF; 37 | *(outbuffer + offset + 2) = (u_covariancei.base >> (8 * 2)) & 0xFF; 38 | *(outbuffer + offset + 3) = (u_covariancei.base >> (8 * 3)) & 0xFF; 39 | *(outbuffer + offset + 4) = (u_covariancei.base >> (8 * 4)) & 0xFF; 40 | *(outbuffer + offset + 5) = (u_covariancei.base >> (8 * 5)) & 0xFF; 41 | *(outbuffer + offset + 6) = (u_covariancei.base >> (8 * 6)) & 0xFF; 42 | *(outbuffer + offset + 7) = (u_covariancei.base >> (8 * 7)) & 0xFF; 43 | offset += sizeof(this->covariance[i]); 44 | } 45 | return offset; 46 | } 47 | 48 | virtual int deserialize(unsigned char *inbuffer) 49 | { 50 | int offset = 0; 51 | offset += this->twist.deserialize(inbuffer + offset); 52 | for( uint8_t i = 0; i < 36; i++){ 53 | union { 54 | double real; 55 | uint64_t base; 56 | } u_covariancei; 57 | u_covariancei.base = 0; 58 | u_covariancei.base |= ((uint64_t) (*(inbuffer + offset + 0))) << (8 * 0); 59 | u_covariancei.base |= ((uint64_t) (*(inbuffer + offset + 1))) << (8 * 1); 60 | u_covariancei.base |= ((uint64_t) (*(inbuffer + offset + 2))) << (8 * 2); 61 | u_covariancei.base |= ((uint64_t) (*(inbuffer + offset + 3))) << (8 * 3); 62 | u_covariancei.base |= ((uint64_t) (*(inbuffer + offset + 4))) << (8 * 4); 63 | u_covariancei.base |= ((uint64_t) (*(inbuffer + offset + 5))) << (8 * 5); 64 | u_covariancei.base |= ((uint64_t) (*(inbuffer + offset + 6))) << (8 * 6); 65 | u_covariancei.base |= ((uint64_t) (*(inbuffer + offset + 7))) << (8 * 7); 66 | this->covariance[i] = u_covariancei.real; 67 | offset += sizeof(this->covariance[i]); 68 | } 69 | return offset; 70 | } 71 | 72 | const char * getType(){ return "geometry_msgs/TwistWithCovariance"; }; 73 | const char * getMD5(){ return "1fe8a28e6890a4cc3ae4c3ca5c7d82e6"; }; 74 | 75 | }; 76 | 77 | } 78 | #endif -------------------------------------------------------------------------------- /arduino/firmware/lib/ros_lib/ros.h: -------------------------------------------------------------------------------- 1 | /* 2 | * Software License Agreement (BSD License) 3 | * 4 | * Copyright (c) 2011, Willow Garage, Inc. 5 | * All rights reserved. 6 | * 7 | * Redistribution and use in source and binary forms, with or without 8 | * modification, are permitted provided that the following conditions 9 | * are met: 10 | * 11 | * * Redistributions of source code must retain the above copyright 12 | * notice, this list of conditions and the following disclaimer. 13 | * * Redistributions in binary form must reproduce the above 14 | * copyright notice, this list of conditions and the following 15 | * disclaimer in the documentation and/or other materials provided 16 | * with the distribution. 17 | * * Neither the name of Willow Garage, Inc. nor the names of its 18 | * contributors may be used to endorse or promote prducts derived 19 | * from this software without specific prior written permission. 20 | * 21 | * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS 22 | * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT 23 | * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS 24 | * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE 25 | * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, 26 | * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, 27 | * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; 28 | * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER 29 | * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT 30 | * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN 31 | * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE 32 | * POSSIBILITY OF SUCH DAMAGE. 33 | */ 34 | 35 | 36 | // 37 | // #ifndef _ROS_H_ 38 | // #define _ROS_H_ 39 | // 40 | // #include "ros/node_handle.h" 41 | // #include "ArduinoHardware.h" 42 | // 43 | // namespace ros 44 | // { 45 | // #if defined(__AVR_ATmega8__) || defined(__AVR_ATmega168__) 46 | // /* downsize our buffers */ 47 | // typedef NodeHandle_ NodeHandle; 48 | // 49 | // #elif defined(__AVR_ATmega328P__) 50 | // 51 | // typedef NodeHandle_ NodeHandle; 52 | // 53 | // #else 54 | // 55 | // typedef NodeHandle_ NodeHandle; 56 | // 57 | // #endif 58 | // } 59 | // 60 | // #endif 61 | 62 | #ifndef _ROS_H_ 63 | #define _ROS_H_ 64 | 65 | #include "ros/node_handle.h" 66 | #include "ArduinoHardware.h" 67 | 68 | namespace ros 69 | { 70 | #if defined(__AVR_ATmega8__) || defined(__AVR_ATmega168__) 71 | /* downsize our buffers */ 72 | typedef NodeHandle_ NodeHandle; 73 | 74 | #elif defined(__AVR_ATmega328P__) 75 | 76 | typedef NodeHandle_ NodeHandle; 77 | 78 | #else 79 | 80 | typedef NodeHandle_ NodeHandle; 81 | 82 | #endif 83 | } 84 | 85 | #endif 86 | -------------------------------------------------------------------------------- /arduino/firmware/lib/ros_lib/rosserial_arduino/Test.h: -------------------------------------------------------------------------------- 1 | #ifndef _ROS_SERVICE_Test_h 2 | #define _ROS_SERVICE_Test_h 3 | #include 4 | #include 5 | #include 6 | #include "ros/msg.h" 7 | 8 | namespace rosserial_arduino 9 | { 10 | 11 | static const char TEST[] = "rosserial_arduino/Test"; 12 | 13 | class TestRequest : public ros::Msg 14 | { 15 | public: 16 | const char* input; 17 | 18 | TestRequest(): 19 | input("") 20 | { 21 | } 22 | 23 | virtual int serialize(unsigned char *outbuffer) const 24 | { 25 | int offset = 0; 26 | uint32_t length_input = strlen(this->input); 27 | memcpy(outbuffer + offset, &length_input, sizeof(uint32_t)); 28 | offset += 4; 29 | memcpy(outbuffer + offset, this->input, length_input); 30 | offset += length_input; 31 | return offset; 32 | } 33 | 34 | virtual int deserialize(unsigned char *inbuffer) 35 | { 36 | int offset = 0; 37 | uint32_t length_input; 38 | memcpy(&length_input, (inbuffer + offset), sizeof(uint32_t)); 39 | offset += 4; 40 | for(unsigned int k= offset; k< offset+length_input; ++k){ 41 | inbuffer[k-1]=inbuffer[k]; 42 | } 43 | inbuffer[offset+length_input-1]=0; 44 | this->input = (char *)(inbuffer + offset-1); 45 | offset += length_input; 46 | return offset; 47 | } 48 | 49 | const char * getType(){ return TEST; }; 50 | const char * getMD5(){ return "39e92f1778057359c64c7b8a7d7b19de"; }; 51 | 52 | }; 53 | 54 | class TestResponse : public ros::Msg 55 | { 56 | public: 57 | const char* output; 58 | 59 | TestResponse(): 60 | output("") 61 | { 62 | } 63 | 64 | virtual int serialize(unsigned char *outbuffer) const 65 | { 66 | int offset = 0; 67 | uint32_t length_output = strlen(this->output); 68 | memcpy(outbuffer + offset, &length_output, sizeof(uint32_t)); 69 | offset += 4; 70 | memcpy(outbuffer + offset, this->output, length_output); 71 | offset += length_output; 72 | return offset; 73 | } 74 | 75 | virtual int deserialize(unsigned char *inbuffer) 76 | { 77 | int offset = 0; 78 | uint32_t length_output; 79 | memcpy(&length_output, (inbuffer + offset), sizeof(uint32_t)); 80 | offset += 4; 81 | for(unsigned int k= offset; k< offset+length_output; ++k){ 82 | inbuffer[k-1]=inbuffer[k]; 83 | } 84 | inbuffer[offset+length_output-1]=0; 85 | this->output = (char *)(inbuffer + offset-1); 86 | offset += length_output; 87 | return offset; 88 | } 89 | 90 | const char * getType(){ return TEST; }; 91 | const char * getMD5(){ return "0825d95fdfa2c8f4bbb4e9c74bccd3fd"; }; 92 | 93 | }; 94 | 95 | class Test { 96 | public: 97 | typedef TestRequest Request; 98 | typedef TestResponse Response; 99 | }; 100 | 101 | } 102 | #endif 103 | -------------------------------------------------------------------------------- /arduino/firmware/lib/ros_lib/ros_arduino_msgs/Encoders.h: -------------------------------------------------------------------------------- 1 | #ifndef _ROS_ros_arduino_msgs_Encoders_h 2 | #define _ROS_ros_arduino_msgs_Encoders_h 3 | 4 | #include 5 | #include 6 | #include 7 | #include "ros/msg.h" 8 | #include "std_msgs/Header.h" 9 | 10 | namespace ros_arduino_msgs 11 | { 12 | 13 | class Encoders : public ros::Msg 14 | { 15 | public: 16 | std_msgs::Header header; 17 | int32_t left; 18 | int32_t right; 19 | 20 | Encoders(): 21 | header(), 22 | left(0), 23 | right(0) 24 | { 25 | } 26 | 27 | virtual int serialize(unsigned char *outbuffer) const 28 | { 29 | int offset = 0; 30 | offset += this->header.serialize(outbuffer + offset); 31 | union { 32 | int32_t real; 33 | uint32_t base; 34 | } u_left; 35 | u_left.real = this->left; 36 | *(outbuffer + offset + 0) = (u_left.base >> (8 * 0)) & 0xFF; 37 | *(outbuffer + offset + 1) = (u_left.base >> (8 * 1)) & 0xFF; 38 | *(outbuffer + offset + 2) = (u_left.base >> (8 * 2)) & 0xFF; 39 | *(outbuffer + offset + 3) = (u_left.base >> (8 * 3)) & 0xFF; 40 | offset += sizeof(this->left); 41 | union { 42 | int32_t real; 43 | uint32_t base; 44 | } u_right; 45 | u_right.real = this->right; 46 | *(outbuffer + offset + 0) = (u_right.base >> (8 * 0)) & 0xFF; 47 | *(outbuffer + offset + 1) = (u_right.base >> (8 * 1)) & 0xFF; 48 | *(outbuffer + offset + 2) = (u_right.base >> (8 * 2)) & 0xFF; 49 | *(outbuffer + offset + 3) = (u_right.base >> (8 * 3)) & 0xFF; 50 | offset += sizeof(this->right); 51 | return offset; 52 | } 53 | 54 | virtual int deserialize(unsigned char *inbuffer) 55 | { 56 | int offset = 0; 57 | offset += this->header.deserialize(inbuffer + offset); 58 | union { 59 | int32_t real; 60 | uint32_t base; 61 | } u_left; 62 | u_left.base = 0; 63 | u_left.base |= ((uint32_t) (*(inbuffer + offset + 0))) << (8 * 0); 64 | u_left.base |= ((uint32_t) (*(inbuffer + offset + 1))) << (8 * 1); 65 | u_left.base |= ((uint32_t) (*(inbuffer + offset + 2))) << (8 * 2); 66 | u_left.base |= ((uint32_t) (*(inbuffer + offset + 3))) << (8 * 3); 67 | this->left = u_left.real; 68 | offset += sizeof(this->left); 69 | union { 70 | int32_t real; 71 | uint32_t base; 72 | } u_right; 73 | u_right.base = 0; 74 | u_right.base |= ((uint32_t) (*(inbuffer + offset + 0))) << (8 * 0); 75 | u_right.base |= ((uint32_t) (*(inbuffer + offset + 1))) << (8 * 1); 76 | u_right.base |= ((uint32_t) (*(inbuffer + offset + 2))) << (8 * 2); 77 | u_right.base |= ((uint32_t) (*(inbuffer + offset + 3))) << (8 * 3); 78 | this->right = u_right.real; 79 | offset += sizeof(this->right); 80 | return offset; 81 | } 82 | 83 | const char * getType(){ return "ros_arduino_msgs/Encoders"; }; 84 | const char * getMD5(){ return "1b78a4027c07bd9a8a89543455494fb2"; }; 85 | 86 | }; 87 | 88 | } 89 | #endif -------------------------------------------------------------------------------- /arduino/firmware/lib/ros_lib/ros/service_server.h: -------------------------------------------------------------------------------- 1 | /* 2 | * Software License Agreement (BSD License) 3 | * 4 | * Copyright (c) 2011, Willow Garage, Inc. 5 | * All rights reserved. 6 | * 7 | * Redistribution and use in source and binary forms, with or without 8 | * modification, are permitted provided that the following conditions 9 | * are met: 10 | * 11 | * * Redistributions of source code must retain the above copyright 12 | * notice, this list of conditions and the following disclaimer. 13 | * * Redistributions in binary form must reproduce the above 14 | * copyright notice, this list of conditions and the following 15 | * disclaimer in the documentation and/or other materials provided 16 | * with the distribution. 17 | * * Neither the name of Willow Garage, Inc. nor the names of its 18 | * contributors may be used to endorse or promote prducts derived 19 | * from this software without specific prior written permission. 20 | * 21 | * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS 22 | * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT 23 | * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS 24 | * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE 25 | * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, 26 | * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, 27 | * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; 28 | * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER 29 | * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT 30 | * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN 31 | * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE 32 | * POSSIBILITY OF SUCH DAMAGE. 33 | */ 34 | 35 | #ifndef _ROS_SERVICE_SERVER_H_ 36 | #define _ROS_SERVICE_SERVER_H_ 37 | 38 | #include "rosserial_msgs/TopicInfo.h" 39 | 40 | #include "publisher.h" 41 | #include "subscriber.h" 42 | 43 | namespace ros { 44 | 45 | template 46 | class ServiceServer : public Subscriber_ { 47 | public: 48 | typedef void(*CallbackT)(const MReq&, MRes&); 49 | 50 | ServiceServer(const char* topic_name, CallbackT cb) : 51 | pub(topic_name, &resp, rosserial_msgs::TopicInfo::ID_SERVICE_SERVER + rosserial_msgs::TopicInfo::ID_PUBLISHER) 52 | { 53 | this->topic_ = topic_name; 54 | this->cb_ = cb; 55 | } 56 | 57 | // these refer to the subscriber 58 | virtual void callback(unsigned char *data){ 59 | req.deserialize(data); 60 | cb_(req,resp); 61 | pub.publish(&resp); 62 | } 63 | virtual const char * getMsgType(){ return this->req.getType(); } 64 | virtual const char * getMsgMD5(){ return this->req.getMD5(); } 65 | virtual int getEndpointType(){ return rosserial_msgs::TopicInfo::ID_SERVICE_SERVER + rosserial_msgs::TopicInfo::ID_SUBSCRIBER; } 66 | 67 | MReq req; 68 | MRes resp; 69 | Publisher pub; 70 | private: 71 | CallbackT cb_; 72 | }; 73 | 74 | } 75 | 76 | #endif 77 | -------------------------------------------------------------------------------- /arduino/firmware/lib/ros_lib/std_msgs/UInt64MultiArray.h: -------------------------------------------------------------------------------- 1 | #ifndef _ROS_std_msgs_UInt64MultiArray_h 2 | #define _ROS_std_msgs_UInt64MultiArray_h 3 | 4 | #include 5 | #include 6 | #include 7 | #include "ros/msg.h" 8 | #include "std_msgs/MultiArrayLayout.h" 9 | 10 | namespace std_msgs 11 | { 12 | 13 | class UInt64MultiArray : public ros::Msg 14 | { 15 | public: 16 | std_msgs::MultiArrayLayout layout; 17 | uint8_t data_length; 18 | uint64_t st_data; 19 | uint64_t * data; 20 | 21 | UInt64MultiArray(): 22 | layout(), 23 | data_length(0), data(NULL) 24 | { 25 | } 26 | 27 | virtual int serialize(unsigned char *outbuffer) const 28 | { 29 | int offset = 0; 30 | offset += this->layout.serialize(outbuffer + offset); 31 | *(outbuffer + offset++) = data_length; 32 | *(outbuffer + offset++) = 0; 33 | *(outbuffer + offset++) = 0; 34 | *(outbuffer + offset++) = 0; 35 | for( uint8_t i = 0; i < data_length; i++){ 36 | *(outbuffer + offset + 0) = (this->data[i] >> (8 * 0)) & 0xFF; 37 | *(outbuffer + offset + 1) = (this->data[i] >> (8 * 1)) & 0xFF; 38 | *(outbuffer + offset + 2) = (this->data[i] >> (8 * 2)) & 0xFF; 39 | *(outbuffer + offset + 3) = (this->data[i] >> (8 * 3)) & 0xFF; 40 | *(outbuffer + offset + 4) = (this->data[i] >> (8 * 4)) & 0xFF; 41 | *(outbuffer + offset + 5) = (this->data[i] >> (8 * 5)) & 0xFF; 42 | *(outbuffer + offset + 6) = (this->data[i] >> (8 * 6)) & 0xFF; 43 | *(outbuffer + offset + 7) = (this->data[i] >> (8 * 7)) & 0xFF; 44 | offset += sizeof(this->data[i]); 45 | } 46 | return offset; 47 | } 48 | 49 | virtual int deserialize(unsigned char *inbuffer) 50 | { 51 | int offset = 0; 52 | offset += this->layout.deserialize(inbuffer + offset); 53 | uint8_t data_lengthT = *(inbuffer + offset++); 54 | if(data_lengthT > data_length) 55 | this->data = (uint64_t*)realloc(this->data, data_lengthT * sizeof(uint64_t)); 56 | offset += 3; 57 | data_length = data_lengthT; 58 | for( uint8_t i = 0; i < data_length; i++){ 59 | this->st_data = ((uint64_t) (*(inbuffer + offset))); 60 | this->st_data |= ((uint64_t) (*(inbuffer + offset + 1))) << (8 * 1); 61 | this->st_data |= ((uint64_t) (*(inbuffer + offset + 2))) << (8 * 2); 62 | this->st_data |= ((uint64_t) (*(inbuffer + offset + 3))) << (8 * 3); 63 | this->st_data |= ((uint64_t) (*(inbuffer + offset + 4))) << (8 * 4); 64 | this->st_data |= ((uint64_t) (*(inbuffer + offset + 5))) << (8 * 5); 65 | this->st_data |= ((uint64_t) (*(inbuffer + offset + 6))) << (8 * 6); 66 | this->st_data |= ((uint64_t) (*(inbuffer + offset + 7))) << (8 * 7); 67 | offset += sizeof(this->st_data); 68 | memcpy( &(this->data[i]), &(this->st_data), sizeof(uint64_t)); 69 | } 70 | return offset; 71 | } 72 | 73 | const char * getType(){ return "std_msgs/UInt64MultiArray"; }; 74 | const char * getMD5(){ return "6088f127afb1d6c72927aa1247e945af"; }; 75 | 76 | }; 77 | 78 | } 79 | #endif -------------------------------------------------------------------------------- /arduino/firmware/lib/ros_lib/sensor_msgs/PointCloud.h: -------------------------------------------------------------------------------- 1 | #ifndef _ROS_sensor_msgs_PointCloud_h 2 | #define _ROS_sensor_msgs_PointCloud_h 3 | 4 | #include 5 | #include 6 | #include 7 | #include "ros/msg.h" 8 | #include "std_msgs/Header.h" 9 | #include "geometry_msgs/Point32.h" 10 | #include "sensor_msgs/ChannelFloat32.h" 11 | 12 | namespace sensor_msgs 13 | { 14 | 15 | class PointCloud : public ros::Msg 16 | { 17 | public: 18 | std_msgs::Header header; 19 | uint8_t points_length; 20 | geometry_msgs::Point32 st_points; 21 | geometry_msgs::Point32 * points; 22 | uint8_t channels_length; 23 | sensor_msgs::ChannelFloat32 st_channels; 24 | sensor_msgs::ChannelFloat32 * channels; 25 | 26 | PointCloud(): 27 | header(), 28 | points_length(0), points(NULL), 29 | channels_length(0), channels(NULL) 30 | { 31 | } 32 | 33 | virtual int serialize(unsigned char *outbuffer) const 34 | { 35 | int offset = 0; 36 | offset += this->header.serialize(outbuffer + offset); 37 | *(outbuffer + offset++) = points_length; 38 | *(outbuffer + offset++) = 0; 39 | *(outbuffer + offset++) = 0; 40 | *(outbuffer + offset++) = 0; 41 | for( uint8_t i = 0; i < points_length; i++){ 42 | offset += this->points[i].serialize(outbuffer + offset); 43 | } 44 | *(outbuffer + offset++) = channels_length; 45 | *(outbuffer + offset++) = 0; 46 | *(outbuffer + offset++) = 0; 47 | *(outbuffer + offset++) = 0; 48 | for( uint8_t i = 0; i < channels_length; i++){ 49 | offset += this->channels[i].serialize(outbuffer + offset); 50 | } 51 | return offset; 52 | } 53 | 54 | virtual int deserialize(unsigned char *inbuffer) 55 | { 56 | int offset = 0; 57 | offset += this->header.deserialize(inbuffer + offset); 58 | uint8_t points_lengthT = *(inbuffer + offset++); 59 | if(points_lengthT > points_length) 60 | this->points = (geometry_msgs::Point32*)realloc(this->points, points_lengthT * sizeof(geometry_msgs::Point32)); 61 | offset += 3; 62 | points_length = points_lengthT; 63 | for( uint8_t i = 0; i < points_length; i++){ 64 | offset += this->st_points.deserialize(inbuffer + offset); 65 | memcpy( &(this->points[i]), &(this->st_points), sizeof(geometry_msgs::Point32)); 66 | } 67 | uint8_t channels_lengthT = *(inbuffer + offset++); 68 | if(channels_lengthT > channels_length) 69 | this->channels = (sensor_msgs::ChannelFloat32*)realloc(this->channels, channels_lengthT * sizeof(sensor_msgs::ChannelFloat32)); 70 | offset += 3; 71 | channels_length = channels_lengthT; 72 | for( uint8_t i = 0; i < channels_length; i++){ 73 | offset += this->st_channels.deserialize(inbuffer + offset); 74 | memcpy( &(this->channels[i]), &(this->st_channels), sizeof(sensor_msgs::ChannelFloat32)); 75 | } 76 | return offset; 77 | } 78 | 79 | const char * getType(){ return "sensor_msgs/PointCloud"; }; 80 | const char * getMD5(){ return "d8e9c3f5afbdd8a130fd1d2763945fca"; }; 81 | 82 | }; 83 | 84 | } 85 | #endif -------------------------------------------------------------------------------- /arduino/firmware/lib/ros_lib/rosserial_arduino/Adc.h: -------------------------------------------------------------------------------- 1 | #ifndef _ROS_rosserial_arduino_Adc_h 2 | #define _ROS_rosserial_arduino_Adc_h 3 | 4 | #include 5 | #include 6 | #include 7 | #include "ros/msg.h" 8 | 9 | namespace rosserial_arduino 10 | { 11 | 12 | class Adc : public ros::Msg 13 | { 14 | public: 15 | uint16_t adc0; 16 | uint16_t adc1; 17 | uint16_t adc2; 18 | uint16_t adc3; 19 | uint16_t adc4; 20 | uint16_t adc5; 21 | 22 | Adc(): 23 | adc0(0), 24 | adc1(0), 25 | adc2(0), 26 | adc3(0), 27 | adc4(0), 28 | adc5(0) 29 | { 30 | } 31 | 32 | virtual int serialize(unsigned char *outbuffer) const 33 | { 34 | int offset = 0; 35 | *(outbuffer + offset + 0) = (this->adc0 >> (8 * 0)) & 0xFF; 36 | *(outbuffer + offset + 1) = (this->adc0 >> (8 * 1)) & 0xFF; 37 | offset += sizeof(this->adc0); 38 | *(outbuffer + offset + 0) = (this->adc1 >> (8 * 0)) & 0xFF; 39 | *(outbuffer + offset + 1) = (this->adc1 >> (8 * 1)) & 0xFF; 40 | offset += sizeof(this->adc1); 41 | *(outbuffer + offset + 0) = (this->adc2 >> (8 * 0)) & 0xFF; 42 | *(outbuffer + offset + 1) = (this->adc2 >> (8 * 1)) & 0xFF; 43 | offset += sizeof(this->adc2); 44 | *(outbuffer + offset + 0) = (this->adc3 >> (8 * 0)) & 0xFF; 45 | *(outbuffer + offset + 1) = (this->adc3 >> (8 * 1)) & 0xFF; 46 | offset += sizeof(this->adc3); 47 | *(outbuffer + offset + 0) = (this->adc4 >> (8 * 0)) & 0xFF; 48 | *(outbuffer + offset + 1) = (this->adc4 >> (8 * 1)) & 0xFF; 49 | offset += sizeof(this->adc4); 50 | *(outbuffer + offset + 0) = (this->adc5 >> (8 * 0)) & 0xFF; 51 | *(outbuffer + offset + 1) = (this->adc5 >> (8 * 1)) & 0xFF; 52 | offset += sizeof(this->adc5); 53 | return offset; 54 | } 55 | 56 | virtual int deserialize(unsigned char *inbuffer) 57 | { 58 | int offset = 0; 59 | this->adc0 = ((uint16_t) (*(inbuffer + offset))); 60 | this->adc0 |= ((uint16_t) (*(inbuffer + offset + 1))) << (8 * 1); 61 | offset += sizeof(this->adc0); 62 | this->adc1 = ((uint16_t) (*(inbuffer + offset))); 63 | this->adc1 |= ((uint16_t) (*(inbuffer + offset + 1))) << (8 * 1); 64 | offset += sizeof(this->adc1); 65 | this->adc2 = ((uint16_t) (*(inbuffer + offset))); 66 | this->adc2 |= ((uint16_t) (*(inbuffer + offset + 1))) << (8 * 1); 67 | offset += sizeof(this->adc2); 68 | this->adc3 = ((uint16_t) (*(inbuffer + offset))); 69 | this->adc3 |= ((uint16_t) (*(inbuffer + offset + 1))) << (8 * 1); 70 | offset += sizeof(this->adc3); 71 | this->adc4 = ((uint16_t) (*(inbuffer + offset))); 72 | this->adc4 |= ((uint16_t) (*(inbuffer + offset + 1))) << (8 * 1); 73 | offset += sizeof(this->adc4); 74 | this->adc5 = ((uint16_t) (*(inbuffer + offset))); 75 | this->adc5 |= ((uint16_t) (*(inbuffer + offset + 1))) << (8 * 1); 76 | offset += sizeof(this->adc5); 77 | return offset; 78 | } 79 | 80 | const char * getType(){ return "rosserial_arduino/Adc"; }; 81 | const char * getMD5(){ return "6d7853a614e2e821319068311f2af25b"; }; 82 | 83 | }; 84 | 85 | } 86 | #endif -------------------------------------------------------------------------------- /arduino/firmware/lib/ros_lib/ros/subscriber.h: -------------------------------------------------------------------------------- 1 | /* 2 | * Software License Agreement (BSD License) 3 | * 4 | * Copyright (c) 2011, Willow Garage, Inc. 5 | * All rights reserved. 6 | * 7 | * Redistribution and use in source and binary forms, with or without 8 | * modification, are permitted provided that the following conditions 9 | * are met: 10 | * 11 | * * Redistributions of source code must retain the above copyright 12 | * notice, this list of conditions and the following disclaimer. 13 | * * Redistributions in binary form must reproduce the above 14 | * copyright notice, this list of conditions and the following 15 | * disclaimer in the documentation and/or other materials provided 16 | * with the distribution. 17 | * * Neither the name of Willow Garage, Inc. nor the names of its 18 | * contributors may be used to endorse or promote prducts derived 19 | * from this software without specific prior written permission. 20 | * 21 | * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS 22 | * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT 23 | * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS 24 | * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE 25 | * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, 26 | * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, 27 | * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; 28 | * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER 29 | * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT 30 | * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN 31 | * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE 32 | * POSSIBILITY OF SUCH DAMAGE. 33 | */ 34 | 35 | #ifndef ROS_SUBSCRIBER_H_ 36 | #define ROS_SUBSCRIBER_H_ 37 | 38 | #include "rosserial_msgs/TopicInfo.h" 39 | 40 | namespace ros { 41 | 42 | /* Base class for objects subscribers. */ 43 | class Subscriber_ 44 | { 45 | public: 46 | virtual void callback(unsigned char *data)=0; 47 | virtual int getEndpointType()=0; 48 | 49 | // id_ is set by NodeHandle when we advertise 50 | int id_; 51 | 52 | virtual const char * getMsgType()=0; 53 | virtual const char * getMsgMD5()=0; 54 | const char * topic_; 55 | }; 56 | 57 | 58 | /* Actual subscriber, templated on message type. */ 59 | template 60 | class Subscriber: public Subscriber_{ 61 | public: 62 | typedef void(*CallbackT)(const MsgT&); 63 | MsgT msg; 64 | 65 | Subscriber(const char * topic_name, CallbackT cb, int endpoint=rosserial_msgs::TopicInfo::ID_SUBSCRIBER) : 66 | cb_(cb), 67 | endpoint_(endpoint) 68 | { 69 | topic_ = topic_name; 70 | }; 71 | 72 | virtual void callback(unsigned char* data){ 73 | msg.deserialize(data); 74 | this->cb_(msg); 75 | } 76 | 77 | virtual const char * getMsgType(){ return this->msg.getType(); } 78 | virtual const char * getMsgMD5(){ return this->msg.getMD5(); } 79 | virtual int getEndpointType(){ return endpoint_; } 80 | 81 | private: 82 | CallbackT cb_; 83 | int endpoint_; 84 | }; 85 | 86 | } 87 | 88 | #endif 89 | -------------------------------------------------------------------------------- /arduino/firmware/lib/ros_lib/sensor_msgs/TimeReference.h: -------------------------------------------------------------------------------- 1 | #ifndef _ROS_sensor_msgs_TimeReference_h 2 | #define _ROS_sensor_msgs_TimeReference_h 3 | 4 | #include 5 | #include 6 | #include 7 | #include "ros/msg.h" 8 | #include "std_msgs/Header.h" 9 | #include "ros/time.h" 10 | 11 | namespace sensor_msgs 12 | { 13 | 14 | class TimeReference : public ros::Msg 15 | { 16 | public: 17 | std_msgs::Header header; 18 | ros::Time time_ref; 19 | const char* source; 20 | 21 | TimeReference(): 22 | header(), 23 | time_ref(), 24 | source("") 25 | { 26 | } 27 | 28 | virtual int serialize(unsigned char *outbuffer) const 29 | { 30 | int offset = 0; 31 | offset += this->header.serialize(outbuffer + offset); 32 | *(outbuffer + offset + 0) = (this->time_ref.sec >> (8 * 0)) & 0xFF; 33 | *(outbuffer + offset + 1) = (this->time_ref.sec >> (8 * 1)) & 0xFF; 34 | *(outbuffer + offset + 2) = (this->time_ref.sec >> (8 * 2)) & 0xFF; 35 | *(outbuffer + offset + 3) = (this->time_ref.sec >> (8 * 3)) & 0xFF; 36 | offset += sizeof(this->time_ref.sec); 37 | *(outbuffer + offset + 0) = (this->time_ref.nsec >> (8 * 0)) & 0xFF; 38 | *(outbuffer + offset + 1) = (this->time_ref.nsec >> (8 * 1)) & 0xFF; 39 | *(outbuffer + offset + 2) = (this->time_ref.nsec >> (8 * 2)) & 0xFF; 40 | *(outbuffer + offset + 3) = (this->time_ref.nsec >> (8 * 3)) & 0xFF; 41 | offset += sizeof(this->time_ref.nsec); 42 | uint32_t length_source = strlen(this->source); 43 | memcpy(outbuffer + offset, &length_source, sizeof(uint32_t)); 44 | offset += 4; 45 | memcpy(outbuffer + offset, this->source, length_source); 46 | offset += length_source; 47 | return offset; 48 | } 49 | 50 | virtual int deserialize(unsigned char *inbuffer) 51 | { 52 | int offset = 0; 53 | offset += this->header.deserialize(inbuffer + offset); 54 | this->time_ref.sec = ((uint32_t) (*(inbuffer + offset))); 55 | this->time_ref.sec |= ((uint32_t) (*(inbuffer + offset + 1))) << (8 * 1); 56 | this->time_ref.sec |= ((uint32_t) (*(inbuffer + offset + 2))) << (8 * 2); 57 | this->time_ref.sec |= ((uint32_t) (*(inbuffer + offset + 3))) << (8 * 3); 58 | offset += sizeof(this->time_ref.sec); 59 | this->time_ref.nsec = ((uint32_t) (*(inbuffer + offset))); 60 | this->time_ref.nsec |= ((uint32_t) (*(inbuffer + offset + 1))) << (8 * 1); 61 | this->time_ref.nsec |= ((uint32_t) (*(inbuffer + offset + 2))) << (8 * 2); 62 | this->time_ref.nsec |= ((uint32_t) (*(inbuffer + offset + 3))) << (8 * 3); 63 | offset += sizeof(this->time_ref.nsec); 64 | uint32_t length_source; 65 | memcpy(&length_source, (inbuffer + offset), sizeof(uint32_t)); 66 | offset += 4; 67 | for(unsigned int k= offset; k< offset+length_source; ++k){ 68 | inbuffer[k-1]=inbuffer[k]; 69 | } 70 | inbuffer[offset+length_source-1]=0; 71 | this->source = (char *)(inbuffer + offset-1); 72 | offset += length_source; 73 | return offset; 74 | } 75 | 76 | const char * getType(){ return "sensor_msgs/TimeReference"; }; 77 | const char * getMD5(){ return "fded64a0265108ba86c3d38fb11c0c16"; }; 78 | 79 | }; 80 | 81 | } 82 | #endif -------------------------------------------------------------------------------- /arduino/firmware/lib/ros_lib/sensor_msgs/ChannelFloat32.h: -------------------------------------------------------------------------------- 1 | #ifndef _ROS_sensor_msgs_ChannelFloat32_h 2 | #define _ROS_sensor_msgs_ChannelFloat32_h 3 | 4 | #include 5 | #include 6 | #include 7 | #include "ros/msg.h" 8 | 9 | namespace sensor_msgs 10 | { 11 | 12 | class ChannelFloat32 : public ros::Msg 13 | { 14 | public: 15 | const char* name; 16 | uint8_t values_length; 17 | float st_values; 18 | float * values; 19 | 20 | ChannelFloat32(): 21 | name(""), 22 | values_length(0), values(NULL) 23 | { 24 | } 25 | 26 | virtual int serialize(unsigned char *outbuffer) const 27 | { 28 | int offset = 0; 29 | uint32_t length_name = strlen(this->name); 30 | memcpy(outbuffer + offset, &length_name, sizeof(uint32_t)); 31 | offset += 4; 32 | memcpy(outbuffer + offset, this->name, length_name); 33 | offset += length_name; 34 | *(outbuffer + offset++) = values_length; 35 | *(outbuffer + offset++) = 0; 36 | *(outbuffer + offset++) = 0; 37 | *(outbuffer + offset++) = 0; 38 | for( uint8_t i = 0; i < values_length; i++){ 39 | union { 40 | float real; 41 | uint32_t base; 42 | } u_valuesi; 43 | u_valuesi.real = this->values[i]; 44 | *(outbuffer + offset + 0) = (u_valuesi.base >> (8 * 0)) & 0xFF; 45 | *(outbuffer + offset + 1) = (u_valuesi.base >> (8 * 1)) & 0xFF; 46 | *(outbuffer + offset + 2) = (u_valuesi.base >> (8 * 2)) & 0xFF; 47 | *(outbuffer + offset + 3) = (u_valuesi.base >> (8 * 3)) & 0xFF; 48 | offset += sizeof(this->values[i]); 49 | } 50 | return offset; 51 | } 52 | 53 | virtual int deserialize(unsigned char *inbuffer) 54 | { 55 | int offset = 0; 56 | uint32_t length_name; 57 | memcpy(&length_name, (inbuffer + offset), sizeof(uint32_t)); 58 | offset += 4; 59 | for(unsigned int k= offset; k< offset+length_name; ++k){ 60 | inbuffer[k-1]=inbuffer[k]; 61 | } 62 | inbuffer[offset+length_name-1]=0; 63 | this->name = (char *)(inbuffer + offset-1); 64 | offset += length_name; 65 | uint8_t values_lengthT = *(inbuffer + offset++); 66 | if(values_lengthT > values_length) 67 | this->values = (float*)realloc(this->values, values_lengthT * sizeof(float)); 68 | offset += 3; 69 | values_length = values_lengthT; 70 | for( uint8_t i = 0; i < values_length; i++){ 71 | union { 72 | float real; 73 | uint32_t base; 74 | } u_st_values; 75 | u_st_values.base = 0; 76 | u_st_values.base |= ((uint32_t) (*(inbuffer + offset + 0))) << (8 * 0); 77 | u_st_values.base |= ((uint32_t) (*(inbuffer + offset + 1))) << (8 * 1); 78 | u_st_values.base |= ((uint32_t) (*(inbuffer + offset + 2))) << (8 * 2); 79 | u_st_values.base |= ((uint32_t) (*(inbuffer + offset + 3))) << (8 * 3); 80 | this->st_values = u_st_values.real; 81 | offset += sizeof(this->st_values); 82 | memcpy( &(this->values[i]), &(this->st_values), sizeof(float)); 83 | } 84 | return offset; 85 | } 86 | 87 | const char * getType(){ return "sensor_msgs/ChannelFloat32"; }; 88 | const char * getMD5(){ return "3d40139cdd33dfedcb71ffeeeb42ae7f"; }; 89 | 90 | }; 91 | 92 | } 93 | #endif -------------------------------------------------------------------------------- /arduino/firmware/lib/ros_lib/ros/service_client.h: -------------------------------------------------------------------------------- 1 | /* 2 | * Software License Agreement (BSD License) 3 | * 4 | * Copyright (c) 2011, Willow Garage, Inc. 5 | * All rights reserved. 6 | * 7 | * Redistribution and use in source and binary forms, with or without 8 | * modification, are permitted provided that the following conditions 9 | * are met: 10 | * 11 | * * Redistributions of source code must retain the above copyright 12 | * notice, this list of conditions and the following disclaimer. 13 | * * Redistributions in binary form must reproduce the above 14 | * copyright notice, this list of conditions and the following 15 | * disclaimer in the documentation and/or other materials provided 16 | * with the distribution. 17 | * * Neither the name of Willow Garage, Inc. nor the names of its 18 | * contributors may be used to endorse or promote prducts derived 19 | * from this software without specific prior written permission. 20 | * 21 | * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS 22 | * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT 23 | * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS 24 | * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE 25 | * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, 26 | * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, 27 | * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; 28 | * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER 29 | * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT 30 | * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN 31 | * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE 32 | * POSSIBILITY OF SUCH DAMAGE. 33 | */ 34 | 35 | #ifndef _ROS_SERVICE_CLIENT_H_ 36 | #define _ROS_SERVICE_CLIENT_H_ 37 | 38 | #include "rosserial_msgs/TopicInfo.h" 39 | 40 | #include "publisher.h" 41 | #include "subscriber.h" 42 | 43 | namespace ros { 44 | 45 | template 46 | class ServiceClient : public Subscriber_ { 47 | public: 48 | ServiceClient(const char* topic_name) : 49 | pub(topic_name, &req, rosserial_msgs::TopicInfo::ID_SERVICE_CLIENT + rosserial_msgs::TopicInfo::ID_PUBLISHER) 50 | { 51 | this->topic_ = topic_name; 52 | this->waiting = true; 53 | } 54 | 55 | virtual void call(const MReq & request, MRes & response) 56 | { 57 | if(!pub.nh_->connected()) return; 58 | ret = &response; 59 | waiting = true; 60 | pub.publish(&request); 61 | while(waiting && pub.nh_->connected()) 62 | if(pub.nh_->spinOnce() < 0) break; 63 | } 64 | 65 | // these refer to the subscriber 66 | virtual void callback(unsigned char *data){ 67 | ret->deserialize(data); 68 | waiting = false; 69 | } 70 | virtual const char * getMsgType(){ return this->resp.getType(); } 71 | virtual const char * getMsgMD5(){ return this->resp.getMD5(); } 72 | virtual int getEndpointType(){ return rosserial_msgs::TopicInfo::ID_SERVICE_CLIENT + rosserial_msgs::TopicInfo::ID_SUBSCRIBER; } 73 | 74 | MReq req; 75 | MRes resp; 76 | MRes * ret; 77 | bool waiting; 78 | Publisher pub; 79 | }; 80 | 81 | } 82 | 83 | #endif 84 | -------------------------------------------------------------------------------- /arduino/firmware/lib/ros_lib/sensor_msgs/SetCameraInfo.h: -------------------------------------------------------------------------------- 1 | #ifndef _ROS_SERVICE_SetCameraInfo_h 2 | #define _ROS_SERVICE_SetCameraInfo_h 3 | #include 4 | #include 5 | #include 6 | #include "ros/msg.h" 7 | #include "sensor_msgs/CameraInfo.h" 8 | 9 | namespace sensor_msgs 10 | { 11 | 12 | static const char SETCAMERAINFO[] = "sensor_msgs/SetCameraInfo"; 13 | 14 | class SetCameraInfoRequest : public ros::Msg 15 | { 16 | public: 17 | sensor_msgs::CameraInfo camera_info; 18 | 19 | SetCameraInfoRequest(): 20 | camera_info() 21 | { 22 | } 23 | 24 | virtual int serialize(unsigned char *outbuffer) const 25 | { 26 | int offset = 0; 27 | offset += this->camera_info.serialize(outbuffer + offset); 28 | return offset; 29 | } 30 | 31 | virtual int deserialize(unsigned char *inbuffer) 32 | { 33 | int offset = 0; 34 | offset += this->camera_info.deserialize(inbuffer + offset); 35 | return offset; 36 | } 37 | 38 | const char * getType(){ return SETCAMERAINFO; }; 39 | const char * getMD5(){ return "ee34be01fdeee563d0d99cd594d5581d"; }; 40 | 41 | }; 42 | 43 | class SetCameraInfoResponse : public ros::Msg 44 | { 45 | public: 46 | bool success; 47 | const char* status_message; 48 | 49 | SetCameraInfoResponse(): 50 | success(0), 51 | status_message("") 52 | { 53 | } 54 | 55 | virtual int serialize(unsigned char *outbuffer) const 56 | { 57 | int offset = 0; 58 | union { 59 | bool real; 60 | uint8_t base; 61 | } u_success; 62 | u_success.real = this->success; 63 | *(outbuffer + offset + 0) = (u_success.base >> (8 * 0)) & 0xFF; 64 | offset += sizeof(this->success); 65 | uint32_t length_status_message = strlen(this->status_message); 66 | memcpy(outbuffer + offset, &length_status_message, sizeof(uint32_t)); 67 | offset += 4; 68 | memcpy(outbuffer + offset, this->status_message, length_status_message); 69 | offset += length_status_message; 70 | return offset; 71 | } 72 | 73 | virtual int deserialize(unsigned char *inbuffer) 74 | { 75 | int offset = 0; 76 | union { 77 | bool real; 78 | uint8_t base; 79 | } u_success; 80 | u_success.base = 0; 81 | u_success.base |= ((uint8_t) (*(inbuffer + offset + 0))) << (8 * 0); 82 | this->success = u_success.real; 83 | offset += sizeof(this->success); 84 | uint32_t length_status_message; 85 | memcpy(&length_status_message, (inbuffer + offset), sizeof(uint32_t)); 86 | offset += 4; 87 | for(unsigned int k= offset; k< offset+length_status_message; ++k){ 88 | inbuffer[k-1]=inbuffer[k]; 89 | } 90 | inbuffer[offset+length_status_message-1]=0; 91 | this->status_message = (char *)(inbuffer + offset-1); 92 | offset += length_status_message; 93 | return offset; 94 | } 95 | 96 | const char * getType(){ return SETCAMERAINFO; }; 97 | const char * getMD5(){ return "2ec6f3eff0161f4257b808b12bc830c2"; }; 98 | 99 | }; 100 | 101 | class SetCameraInfo { 102 | public: 103 | typedef SetCameraInfoRequest Request; 104 | typedef SetCameraInfoResponse Response; 105 | }; 106 | 107 | } 108 | #endif 109 | -------------------------------------------------------------------------------- /arduino/firmware/lib/ros_lib/std_msgs/Int64MultiArray.h: -------------------------------------------------------------------------------- 1 | #ifndef _ROS_std_msgs_Int64MultiArray_h 2 | #define _ROS_std_msgs_Int64MultiArray_h 3 | 4 | #include 5 | #include 6 | #include 7 | #include "ros/msg.h" 8 | #include "std_msgs/MultiArrayLayout.h" 9 | 10 | namespace std_msgs 11 | { 12 | 13 | class Int64MultiArray : public ros::Msg 14 | { 15 | public: 16 | std_msgs::MultiArrayLayout layout; 17 | uint8_t data_length; 18 | int64_t st_data; 19 | int64_t * data; 20 | 21 | Int64MultiArray(): 22 | layout(), 23 | data_length(0), data(NULL) 24 | { 25 | } 26 | 27 | virtual int serialize(unsigned char *outbuffer) const 28 | { 29 | int offset = 0; 30 | offset += this->layout.serialize(outbuffer + offset); 31 | *(outbuffer + offset++) = data_length; 32 | *(outbuffer + offset++) = 0; 33 | *(outbuffer + offset++) = 0; 34 | *(outbuffer + offset++) = 0; 35 | for( uint8_t i = 0; i < data_length; i++){ 36 | union { 37 | int64_t real; 38 | uint64_t base; 39 | } u_datai; 40 | u_datai.real = this->data[i]; 41 | *(outbuffer + offset + 0) = (u_datai.base >> (8 * 0)) & 0xFF; 42 | *(outbuffer + offset + 1) = (u_datai.base >> (8 * 1)) & 0xFF; 43 | *(outbuffer + offset + 2) = (u_datai.base >> (8 * 2)) & 0xFF; 44 | *(outbuffer + offset + 3) = (u_datai.base >> (8 * 3)) & 0xFF; 45 | *(outbuffer + offset + 4) = (u_datai.base >> (8 * 4)) & 0xFF; 46 | *(outbuffer + offset + 5) = (u_datai.base >> (8 * 5)) & 0xFF; 47 | *(outbuffer + offset + 6) = (u_datai.base >> (8 * 6)) & 0xFF; 48 | *(outbuffer + offset + 7) = (u_datai.base >> (8 * 7)) & 0xFF; 49 | offset += sizeof(this->data[i]); 50 | } 51 | return offset; 52 | } 53 | 54 | virtual int deserialize(unsigned char *inbuffer) 55 | { 56 | int offset = 0; 57 | offset += this->layout.deserialize(inbuffer + offset); 58 | uint8_t data_lengthT = *(inbuffer + offset++); 59 | if(data_lengthT > data_length) 60 | this->data = (int64_t*)realloc(this->data, data_lengthT * sizeof(int64_t)); 61 | offset += 3; 62 | data_length = data_lengthT; 63 | for( uint8_t i = 0; i < data_length; i++){ 64 | union { 65 | int64_t real; 66 | uint64_t base; 67 | } u_st_data; 68 | u_st_data.base = 0; 69 | u_st_data.base |= ((uint64_t) (*(inbuffer + offset + 0))) << (8 * 0); 70 | u_st_data.base |= ((uint64_t) (*(inbuffer + offset + 1))) << (8 * 1); 71 | u_st_data.base |= ((uint64_t) (*(inbuffer + offset + 2))) << (8 * 2); 72 | u_st_data.base |= ((uint64_t) (*(inbuffer + offset + 3))) << (8 * 3); 73 | u_st_data.base |= ((uint64_t) (*(inbuffer + offset + 4))) << (8 * 4); 74 | u_st_data.base |= ((uint64_t) (*(inbuffer + offset + 5))) << (8 * 5); 75 | u_st_data.base |= ((uint64_t) (*(inbuffer + offset + 6))) << (8 * 6); 76 | u_st_data.base |= ((uint64_t) (*(inbuffer + offset + 7))) << (8 * 7); 77 | this->st_data = u_st_data.real; 78 | offset += sizeof(this->st_data); 79 | memcpy( &(this->data[i]), &(this->st_data), sizeof(int64_t)); 80 | } 81 | return offset; 82 | } 83 | 84 | const char * getType(){ return "std_msgs/Int64MultiArray"; }; 85 | const char * getMD5(){ return "54865aa6c65be0448113a2afc6a49270"; }; 86 | 87 | }; 88 | 89 | } 90 | #endif -------------------------------------------------------------------------------- /arduino/firmware/lib/ros_lib/std_msgs/Float64MultiArray.h: -------------------------------------------------------------------------------- 1 | #ifndef _ROS_std_msgs_Float64MultiArray_h 2 | #define _ROS_std_msgs_Float64MultiArray_h 3 | 4 | #include 5 | #include 6 | #include 7 | #include "ros/msg.h" 8 | #include "std_msgs/MultiArrayLayout.h" 9 | 10 | namespace std_msgs 11 | { 12 | 13 | class Float64MultiArray : public ros::Msg 14 | { 15 | public: 16 | std_msgs::MultiArrayLayout layout; 17 | uint8_t data_length; 18 | double st_data; 19 | double * data; 20 | 21 | Float64MultiArray(): 22 | layout(), 23 | data_length(0), data(NULL) 24 | { 25 | } 26 | 27 | virtual int serialize(unsigned char *outbuffer) const 28 | { 29 | int offset = 0; 30 | offset += this->layout.serialize(outbuffer + offset); 31 | *(outbuffer + offset++) = data_length; 32 | *(outbuffer + offset++) = 0; 33 | *(outbuffer + offset++) = 0; 34 | *(outbuffer + offset++) = 0; 35 | for( uint8_t i = 0; i < data_length; i++){ 36 | union { 37 | double real; 38 | uint64_t base; 39 | } u_datai; 40 | u_datai.real = this->data[i]; 41 | *(outbuffer + offset + 0) = (u_datai.base >> (8 * 0)) & 0xFF; 42 | *(outbuffer + offset + 1) = (u_datai.base >> (8 * 1)) & 0xFF; 43 | *(outbuffer + offset + 2) = (u_datai.base >> (8 * 2)) & 0xFF; 44 | *(outbuffer + offset + 3) = (u_datai.base >> (8 * 3)) & 0xFF; 45 | *(outbuffer + offset + 4) = (u_datai.base >> (8 * 4)) & 0xFF; 46 | *(outbuffer + offset + 5) = (u_datai.base >> (8 * 5)) & 0xFF; 47 | *(outbuffer + offset + 6) = (u_datai.base >> (8 * 6)) & 0xFF; 48 | *(outbuffer + offset + 7) = (u_datai.base >> (8 * 7)) & 0xFF; 49 | offset += sizeof(this->data[i]); 50 | } 51 | return offset; 52 | } 53 | 54 | virtual int deserialize(unsigned char *inbuffer) 55 | { 56 | int offset = 0; 57 | offset += this->layout.deserialize(inbuffer + offset); 58 | uint8_t data_lengthT = *(inbuffer + offset++); 59 | if(data_lengthT > data_length) 60 | this->data = (double*)realloc(this->data, data_lengthT * sizeof(double)); 61 | offset += 3; 62 | data_length = data_lengthT; 63 | for( uint8_t i = 0; i < data_length; i++){ 64 | union { 65 | double real; 66 | uint64_t base; 67 | } u_st_data; 68 | u_st_data.base = 0; 69 | u_st_data.base |= ((uint64_t) (*(inbuffer + offset + 0))) << (8 * 0); 70 | u_st_data.base |= ((uint64_t) (*(inbuffer + offset + 1))) << (8 * 1); 71 | u_st_data.base |= ((uint64_t) (*(inbuffer + offset + 2))) << (8 * 2); 72 | u_st_data.base |= ((uint64_t) (*(inbuffer + offset + 3))) << (8 * 3); 73 | u_st_data.base |= ((uint64_t) (*(inbuffer + offset + 4))) << (8 * 4); 74 | u_st_data.base |= ((uint64_t) (*(inbuffer + offset + 5))) << (8 * 5); 75 | u_st_data.base |= ((uint64_t) (*(inbuffer + offset + 6))) << (8 * 6); 76 | u_st_data.base |= ((uint64_t) (*(inbuffer + offset + 7))) << (8 * 7); 77 | this->st_data = u_st_data.real; 78 | offset += sizeof(this->st_data); 79 | memcpy( &(this->data[i]), &(this->st_data), sizeof(double)); 80 | } 81 | return offset; 82 | } 83 | 84 | const char * getType(){ return "std_msgs/Float64MultiArray"; }; 85 | const char * getMD5(){ return "4b7d974086d4060e7db4613a7e6c3ba4"; }; 86 | 87 | }; 88 | 89 | } 90 | #endif --------------------------------------------------------------------------------