first commit

sensor_msgs/include/sensor_msgs/BatteryState.h

@@ -0,0 +1,63 @@
+#pragma once
+#include "std_msgs/Header.h"
+#include <cstdint>
+#include <string>
+#include <vector>
+#include <limits>
+
+namespace sensor_msgs
+{
+
+struct BatteryState
+{
+  // ===== Power supply status constants =====
+  static constexpr uint8_t POWER_SUPPLY_STATUS_UNKNOWN = 0;
+  static constexpr uint8_t POWER_SUPPLY_STATUS_CHARGING = 1;
+  static constexpr uint8_t POWER_SUPPLY_STATUS_DISCHARGING = 2;
+  static constexpr uint8_t POWER_SUPPLY_STATUS_NOT_CHARGING = 3;
+  static constexpr uint8_t POWER_SUPPLY_STATUS_FULL = 4;
+
+  // ===== Power supply health constants =====
+  static constexpr uint8_t POWER_SUPPLY_HEALTH_UNKNOWN = 0;
+  static constexpr uint8_t POWER_SUPPLY_HEALTH_GOOD = 1;
+  static constexpr uint8_t POWER_SUPPLY_HEALTH_OVERHEAT = 2;
+  static constexpr uint8_t POWER_SUPPLY_HEALTH_DEAD = 3;
+  static constexpr uint8_t POWER_SUPPLY_HEALTH_OVERVOLTAGE = 4;
+  static constexpr uint8_t POWER_SUPPLY_HEALTH_UNSPEC_FAILURE = 5;
+  static constexpr uint8_t POWER_SUPPLY_HEALTH_COLD = 6;
+  static constexpr uint8_t POWER_SUPPLY_HEALTH_WATCHDOG_TIMER_EXPIRE = 7;
+  static constexpr uint8_t POWER_SUPPLY_HEALTH_SAFETY_TIMER_EXPIRE = 8;
+
+  // ===== Power supply technology (chemistry) constants =====
+  static constexpr uint8_t POWER_SUPPLY_TECHNOLOGY_UNKNOWN = 0;
+  static constexpr uint8_t POWER_SUPPLY_TECHNOLOGY_NIMH = 1;
+  static constexpr uint8_t POWER_SUPPLY_TECHNOLOGY_LION = 2;
+  static constexpr uint8_t POWER_SUPPLY_TECHNOLOGY_LIPO = 3;
+  static constexpr uint8_t POWER_SUPPLY_TECHNOLOGY_LIFE = 4;
+  static constexpr uint8_t POWER_SUPPLY_TECHNOLOGY_NICD = 5;
+  static constexpr uint8_t POWER_SUPPLY_TECHNOLOGY_LIMN = 6;
+
+  // ===== Data fields =====
+  std_msgs::Header header;
+
+  float voltage = std::numeric_limits<float>::quiet_NaN();           // [V]
+  float temperature = std::numeric_limits<float>::quiet_NaN();       // [°C]
+  float current = std::numeric_limits<float>::quiet_NaN();           // [A]
+  float charge = std::numeric_limits<float>::quiet_NaN();            // [Ah]
+  float capacity = std::numeric_limits<float>::quiet_NaN();          // [Ah]
+  float design_capacity = std::numeric_limits<float>::quiet_NaN();   // [Ah]
+  float percentage = std::numeric_limits<float>::quiet_NaN();        // 0..1
+
+  uint8_t power_supply_status = POWER_SUPPLY_STATUS_UNKNOWN;
+  uint8_t power_supply_health = POWER_SUPPLY_HEALTH_UNKNOWN;
+  uint8_t power_supply_technology = POWER_SUPPLY_TECHNOLOGY_UNKNOWN;
+  bool present = false;
+
+  std::vector<float> cell_voltage;
+  std::vector<float> cell_temperature;
+
+  std::string location;
+  std::string serial_number;
+};
+
+}  // namespace sensor_msgs

sensor_msgs/include/sensor_msgs/CameraInfo.h

@@ -0,0 +1,54 @@
+
+
+# Time of image acquisition, camera coordinate frame ID
+Header header    # Header timestamp should be acquisition time of image
+
+
+uint32 height
+uint32 width
+
+string distortion_model
+
+float64[] D
+
+float64[9]  K # 3x3 row-major matrix
+
+
+float64[9]  R # 3x3 row-major matrix
+
+float64[12] P # 3x4 row-major matrix
+
+uint32 binning_x
+uint32 binning_y
+
+RegionOfInterest roi
+
+#pragma once
+#include <string>
+#include <vector>
+#include "msg/Header.h"
+#include "msg/RegionOfInterest.h"
+
+namespace sensor_msgs
+{
+struct CameraInfo
+{
+    std_msgs::Header header;   // thời gian và frame_id
+
+    // Calibration parameters
+    uint32_t height = 0;
+    uint32_t width = 0;
+    std::string distortion_model;
+
+    std::vector<double> D;     // distortion coefficients
+    double K[9] = {0.0};       // intrinsic matrix
+    double R[9] = {0.0};       // rectification matrix
+    double P[12] = {0.0};      // projection matrix
+
+    // Operational parameters
+    uint32_t binning_x = 0;
+    uint32_t binning_y = 0;
+    sensor_msgs::RegionOfInterest roi;
+};
+} // namespace sensor_msgs
+

sensor_msgs/include/sensor_msgs/ChannelFloat32.h

@@ -0,0 +1,12 @@
+#pragma once
+#include <string>
+#include <vector>
+
+namespace sensor_msgs
+{
+struct ChannelFloat32
+{
+    std::string name;         // Tên của channel (vd: "intensity", "rgb", "u", "v")
+    std::vector<float> values; // Dữ liệu float32 ứng với từng điểm trong PointCloud
+};
+} // namespace sensor_msgs

sensor_msgs/include/sensor_msgs/CompressedImage.h

@@ -0,0 +1,18 @@
+#pragma once
+#include <string>
+#include <vector>
+#include "msg/Header.h"
+
+namespace sensor_msgs
+{
+
+struct CompressedImage
+{
+    std_msgs::Header header;      // Thông tin thời gian & frame_id
+    std::string format;           // Định dạng nén (jpeg, png, ...)
+
+    // Dữ liệu ảnh nén (binary buffer)
+    std::vector<uint8_t> data;    
+};
+
+} // namespace sensor_msgs

sensor_msgs/include/sensor_msgs/FluidPressure.h

@@ -0,0 +1,17 @@
+#pragma once
+#include "msg/Header.h"
+
+namespace sensor_msgs
+{
+
+struct FluidPressure
+{
+    std_msgs::Header header;   // Thông tin thời gian và khung tọa độ
+    double fluid_pressure;     // Áp suất tuyệt đối (đơn vị: Pascal)
+    double variance;           // Phương sai (0 = không xác định)
+
+    FluidPressure()
+        : fluid_pressure(0.0), variance(0.0) {}
+};
+
+} // namespace sensor_msgs

sensor_msgs/include/sensor_msgs/Illuminance.h

@@ -0,0 +1,17 @@
+#pragma once
+#include "msg/Header.h"
+
+namespace sensor_msgs
+{
+
+struct Illuminance
+{
+    std_msgs::Header header;   // Thông tin thời gian và khung tọa độ
+    double illuminance;        // Độ rọi đo được (đơn vị: Lux)
+    double variance;           // Phương sai (0 = không xác định)
+
+    Illuminance()
+        : illuminance(0.0), variance(0.0) {}
+};
+
+} // namespace sensor_msgs

sensor_msgs/include/sensor_msgs/Image.h

@@ -0,0 +1,26 @@
+#pragma once
+#include <string>
+#include <vector>
+#include <cstdint>
+#include "msg/Header.h"
+
+namespace sensor_msgs
+{
+struct Image
+{
+    std_msgs::Header header;       // Thông tin thời gian và frame
+    uint32_t height = 0;           // Số hàng (pixels theo chiều dọc)
+    uint32_t width = 0;            // Số cột (pixels theo chiều ngang)
+    std::string encoding;          // Kiểu mã hóa (vd: "rgb8", "mono8")
+    uint8_t is_bigendian = 0;      // 0 = little endian
+    uint32_t step = 0;             // Số byte mỗi dòng ảnh
+    std::vector<uint8_t> data;     // Mảng dữ liệu ảnh (kích thước step * height)
+
+    Image() = default;
+
+    inline size_t size() const { return data.size(); }
+
+    inline bool empty() const { return data.empty(); }
+};
+
+} // namespace sensor_msgs

sensor_msgs/include/sensor_msgs/Imu.h

@@ -0,0 +1,31 @@
+#pragma once
+#include <array>
+#include "msg/Header.h"
+#include "geometry_msgs/Quaternion.h"
+#include "geometry_msgs/Vector3.h"
+
+namespace sensor_msgs
+{
+
+struct Imu
+{
+    std_msgs::Header header;  // Thời gian và frame gốc
+
+    geometry_msgs::Quaternion orientation;        // Góc quay (đơn vị: quaternion)
+    std::array<double, 9> orientation_covariance; // Ma trận hiệp phương sai (row-major)
+
+    geometry_msgs::Vector3 angular_velocity;        // Tốc độ góc (rad/s)
+    std::array<double, 9> angular_velocity_covariance; // Ma trận hiệp phương sai
+
+    geometry_msgs::Vector3 linear_acceleration;        // Gia tốc tuyến tính (m/s^2)
+    std::array<double, 9> linear_acceleration_covariance; // Ma trận hiệp phương sai
+
+    Imu()
+    {
+        orientation_covariance.fill(0.0);
+        angular_velocity_covariance.fill(0.0);
+        linear_acceleration_covariance.fill(0.0);
+    }
+};
+
+} // namespace sensor_msgs

sensor_msgs/include/sensor_msgs/JointState.h

@@ -0,0 +1,21 @@
+#pragma once
+#include <string>
+#include <vector>
+#include "msg/Header.h"
+
+namespace sensor_msgs
+{
+
+struct JointState
+{
+    std_msgs::Header header;            // Thời điểm và frame ghi nhận trạng thái khớp
+
+    std::vector<std::string> name;      // Tên từng khớp
+    std::vector<double> position;       // Vị trí (rad hoặc m)
+    std::vector<double> velocity;       // Vận tốc (rad/s hoặc m/s)
+    std::vector<double> effort;         // Mô-men hoặc lực (Nm hoặc N)
+
+    JointState() = default;
+};
+
+} // namespace sensor_msgs

sensor_msgs/include/sensor_msgs/Joy.h

@@ -0,0 +1,17 @@
+#pragma once
+#include <vector>
+#include "msg/Header.h"
+
+namespace sensor_msgs
+{
+
+struct Joy
+{
+    std_msgs::Header header;     // Thời điểm nhận dữ liệu từ joystick
+    std::vector<float> axes;     // Các giá trị trục analog (ví dụ: X, Y, Z, throttle, ...)
+    std::vector<int32_t> buttons; // Trạng thái nút bấm (0 = nhả, 1 = nhấn)
+
+    Joy() = default;
+};
+
+} // namespace sensor_msgs

sensor_msgs/include/sensor_msgs/JoyFeedback.h

@@ -0,0 +1,24 @@
+#ifndef JOY_FEEDBACK_H
+#define JOY_FEEDBACK_H
+
+#include <cstdint>
+
+namespace sensor_msgs
+{
+
+struct JoyFeedback
+{
+    // Declare of the type of feedback
+    static constexpr uint8_t TYPE_LED    = 0;
+    static constexpr uint8_t TYPE_RUMBLE = 1;
+    static constexpr uint8_t TYPE_BUZZER = 2;
+
+    uint8_t type;       // Loại feedback
+    uint8_t id;         // ID của từng loại feedback
+    float intensity;    // Cường độ (0.0 - 1.0)
+};
+
+}
+#endif // JOY_FEEDBACK_H
+
+

sensor_msgs/include/sensor_msgs/JoyFeedbackArray.h

@@ -0,0 +1,16 @@
+#ifndef JOY_FEEDBACK_ARRAY_H
+#define JOY_FEEDBACK_ARRAY_H
+
+#include <vector>
+#include "sensor_msgs/JoyFeedback.h"
+
+namespace sensor_msgs
+{
+
+struct JoyFeedbackArray
+{
+    std::vector<JoyFeedback> array;  // Danh sách các feedback
+};
+
+} // namespace sensor_msgs
+#endif // JOY_FEEDBACK_ARRAY_H

sensor_msgs/include/sensor_msgs/LaserEcho.h

@@ -0,0 +1,16 @@
+#ifndef LASER_ECHO_H
+#define LASER_ECHO_H
+
+#include <vector>
+namespace sensor_msgs
+{
+
+struct LaserEcho
+{
+    // Mảng chứa nhiều giá trị đo (range hoặc intensity)
+    // Tất cả đều thuộc cùng một góc quét
+    std::vector<float> echoes;
+};
+
+}
+#endif // LASER_ECHO_H

sensor_msgs/include/sensor_msgs/LaserScan.h

@@ -0,0 +1,34 @@
+#ifndef LASER_SCAN_H
+#define LASER_SCAN_H
+
+#include <vector>
+#include "std_msgs/Header.h"
+namespace sensor_msgs
+{
+
+struct LaserScan
+{
+    std_msgs::Header header;  // Thời gian và frame của phép đo
+
+    // Góc bắt đầu và kết thúc của tia quét [rad]
+    float angle_min = 0.0f;
+    float angle_max = 0.0f;
+    float angle_increment = 0.0f;  // Khoảng cách góc giữa hai tia quét liên tiếp [rad]
+
+    // Thông tin về thời gian quét
+    float time_increment = 0.0f;  // Thời gian giữa hai phép đo liên tiếp [s]
+    float scan_time = 0.0f;       // Thời gian hoàn tất một lần quét [s]
+
+    // Giới hạn khoảng đo
+    float range_min = 0.0f;  // Giá trị khoảng cách nhỏ nhất [m]
+    float range_max = 0.0f;  // Giá trị khoảng cách lớn nhất [m]
+
+    // Dữ liệu chính của laser
+    std::vector<float> ranges;       // Dữ liệu khoảng cách [m]
+    std::vector<float> intensities;  // Cường độ phản xạ (đơn vị phụ thuộc thiết bị)
+
+    LaserScan() = default;
+};
+
+}
+#endif // LASER_SCAN_H

sensor_msgs/include/sensor_msgs/MultiEchoLaserScan.h

@@ -0,0 +1,32 @@
+#ifndef MULTI_ECHO_LASER_SCAN_H
+#define MULTI_ECHO_LASER_SCAN_H
+
+#include <vector>
+#include "msg/Header.h"
+#include "msg/LaserEcho.h"   // Định nghĩa struct LaserEcho (float32[] echoes)
+
+namespace sensor_msgs
+{
+
+struct MultiEchoLaserScan
+{
+    std_msgs::Header header;           // Thông tin thời gian & frame của phép đo
+
+    float angle_min = 0.0f;            // Góc bắt đầu quét (radians)
+    float angle_max = 0.0f;            // Góc kết thúc quét (radians)
+    float angle_increment = 0.0f;      // Độ tăng góc giữa 2 tia liên tiếp (radians)
+
+    float time_increment = 0.0f;       // Thời gian giữa 2 phép đo (seconds)
+    float scan_time = 0.0f;            // Thời gian quét toàn bộ (seconds)
+
+    float range_min = 0.0f;            // Khoảng cách đo nhỏ nhất (m)
+    float range_max = 0.0f;            // Khoảng cách đo lớn nhất (m)
+
+    std::vector<LaserEcho> ranges;     // Dữ liệu khoảng cách (m)
+    std::vector<LaserEcho> intensities; // Dữ liệu cường độ (tùy chọn)
+
+    MultiEchoLaserScan() = default;
+};
+
+}
+#endif // MULTI_ECHO_LASER_SCAN_H

sensor_msgs/include/sensor_msgs/PointCloud.h

@@ -0,0 +1,24 @@
+#ifndef POINTCLOUD_H
+#define POINTCLOUD_H
+
+#include <vector>
+#include <string>
+#include "std_msgs/Header.h"
+#include "geometry_msgs/Point32.h"
+#include "sensor_msgs/ChannelFloat32.h"
+
+namespace sensor_msgs
+{
+
+struct PointCloud
+{
+    std_msgs::Header header;                     // Thông tin thời gian & frame của dữ liệu
+
+    std::vector<geometry_msgs::Point32> points;  // Danh sách các điểm 3D (x, y, z)
+    std::vector<ChannelFloat32> channels;        // Dữ liệu phụ đi kèm (vd: "intensity", "rgb")
+
+    PointCloud() = default;
+};
+
+}
+#endif // POINTCLOUD_H

sensor_msgs/include/sensor_msgs/PointCloud2.h

@@ -0,0 +1,34 @@
+#ifndef POINTCLOUD2_H
+#define POINTCLOUD2_H
+
+#include <cstdint>
+#include <string>
+#include <vector>
+#include "std_msgs/Header.h"
+#include "sensor_msgs/PointField.h"
+
+namespace sensor_msgs
+{
+
+struct PointCloud2
+{
+    std_msgs::Header header;                  // Thời gian và frame của dữ liệu
+
+    uint32_t height = 1;                      // Số hàng (1 nếu là point cloud 1D)
+    uint32_t width = 0;                       // Số lượng điểm trên mỗi hàng (tổng điểm = height * width)
+
+    std::vector<PointField> fields;           // Thông tin layout của từng trường trong dữ liệu (vd: x, y, z, intensity,...)
+
+    bool is_bigendian = false;                // true nếu dữ liệu lưu ở dạng big-endian
+    uint32_t point_step = 0;                  // Số byte cho mỗi điểm
+    uint32_t row_step = 0;                    // Số byte cho mỗi hàng
+
+    std::vector<uint8_t> data;                // Dữ liệu nhị phân (raw bytes), kích thước = row_step * height
+
+    bool is_dense = false;                    // true nếu không có điểm NaN hoặc vô hiệu
+
+    PointCloud2() = default;
+};
+
+}
+#endif // POINTCLOUD2_H

sensor_msgs/include/sensor_msgs/PointField.h

@@ -0,0 +1,31 @@
+#ifndef POINTFIELD_H
+#define POINTFIELD_H
+
+#include <cstdint>
+#include <string>
+
+namespace sensor_msgs
+{
+
+struct PointField
+{
+    // Các hằng số kiểu dữ liệu (theo chuẩn ROS)
+    static constexpr uint8_t INT8    = 1;
+    static constexpr uint8_t UINT8   = 2;
+    static constexpr uint8_t INT16   = 3;
+    static constexpr uint8_t UINT16  = 4;
+    static constexpr uint8_t INT32   = 5;
+    static constexpr uint8_t UINT32  = 6;
+    static constexpr uint8_t FLOAT32 = 7;
+    static constexpr uint8_t FLOAT64 = 8;
+
+    std::string name;      // Tên trường (vd: "x", "y", "z", "intensity")
+    uint32_t offset = 0;   // Vị trí byte bắt đầu trong cấu trúc mỗi điểm
+    uint8_t datatype = 0;  // Kiểu dữ liệu (sử dụng các hằng ở trên)
+    uint32_t count = 0;    // Số phần tử trong trường (vd: 3 cho vector3)
+
+    PointField() = default;
+};
+
+}
+#endif // POINTFIELD_H

sensor_msgs/include/sensor_msgs/Range.h

@@ -0,0 +1,28 @@
+#ifndef RANGE_H
+#define RANGE_H
+
+#include <cstdint>
+#include "Header.h"  // Header tương tự std_msgs/Header
+
+namespace sensor_msgs
+{
+
+struct Range
+{
+    // Các hằng số loại bức xạ (radiation type)
+    static constexpr uint8_t ULTRASOUND = 0;
+    static constexpr uint8_t INFRARED   = 1;
+
+    Header header;          // Thời gian đo và frame_id
+
+    uint8_t radiation_type = ULTRASOUND;  // Loại cảm biến (âm thanh, IR, v.v.)
+    float field_of_view = 0.0f;           // Góc mở của cảm biến [rad]
+    float min_range = 0.0f;               // Khoảng cách nhỏ nhất [m]
+    float max_range = 0.0f;               // Khoảng cách lớn nhất [m]
+    float range = 0.0f;                   // Khoảng cách đo được [m]
+
+    Range() = default;
+};
+
+}
+#endif // RANGE_H

sensor_msgs/include/sensor_msgs/RegionOfInterest.h

@@ -0,0 +1,21 @@
+#ifndef REGION_OF_INTEREST_H
+#define REGION_OF_INTEREST_H
+
+#include <cstdint>
+namespace sensor_msgs
+{
+
+struct RegionOfInterest
+{
+    uint32_t x_offset = 0;  // Pixel ngoài cùng bên trái của ROI
+    uint32_t y_offset = 0;  // Pixel ngoài cùng bên trên của ROI
+    uint32_t height   = 0;  // Chiều cao vùng ROI
+    uint32_t width    = 0;  // Chiều rộng vùng ROI
+
+    bool do_rectify   = false;  // Có cần tính toán lại ROI sau khi hiệu chỉnh ảnh không
+
+    RegionOfInterest() = default;
+};
+
+}
+#endif // REGION_OF_INTEREST_H

sensor_msgs/include/sensor_msgs/RelativeHumidity.h

@@ -0,0 +1,19 @@
+#ifndef RELATIVE_HUMIDITY_H
+#define RELATIVE_HUMIDITY_H
+
+#include <cstdint>
+#include "msg/Header.h"  // Giả định bạn đã có struct Header tương tự ROS std_msgs/Header
+namespace sensor_msgs
+{
+
+struct RelativeHumidity
+{
+    Header header;           // Thời điểm đo và khung tọa độ cảm biến
+    double relative_humidity = 0.0;  // Độ ẩm tương đối (0.0 - 1.0)
+    double variance = 0.0;            // Phương sai, 0 nghĩa là "không biết"
+
+    RelativeHumidity() = default;
+};
+
+}
+#endif // RELATIVE_HUMIDITY_H

sensor_msgs/include/sensor_msgs/Temperature.h

@@ -0,0 +1,19 @@
+#ifndef TEMPERATURE_H
+#define TEMPERATURE_H
+
+#include <cstdint>
+#include "msg/Header.h"  // Định nghĩa struct Header tương tự std_msgs/Header
+namespace sensor_msgs
+{
+
+struct Temperature
+{
+    Header header;        // Thông tin thời gian và vị trí cảm biến
+    double temperature = 0.0; // Nhiệt độ đo được (°C)
+    double variance = 0.0;    // Phương sai, 0 nghĩa là "không biết"
+
+    Temperature() = default;
+};
+
+}
+#endif // TEMPERATURE_H

sensor_msgs/include/sensor_msgs/TimeReference.h

@@ -0,0 +1,20 @@
+#ifndef TIMEREFERENCE_H
+#define TIMEREFERENCE_H
+
+#include <string>
+#include "msg/Header.h"   // Định nghĩa struct Header tương tự std_msgs/Header
+namespace sensor_msgs
+{
+
+struct TimeReference
+{
+    Header header;      // stamp: thời điểm đo (theo đồng hồ hệ thống)
+                        // frame_id: không sử dụng trong message này
+    double time_ref = 0.0; // Thời gian từ nguồn thời gian bên ngoài (giây)
+    std::string source;    // Tên của nguồn thời gian (tùy chọn)
+
+    TimeReference() = default;
+};
+
+}
+#endif // TIMEREFERENCE_H

sensor_msgs/include/sensor_msgs/distortion_models.h

@@ -0,0 +1,51 @@
+
+/*********************************************************************
+* Software License Agreement (BSD License)
+* 
+*  Copyright (c) 2010, Willow Garage, Inc.
+*  All rights reserved.
+* 
+*  Redistribution and use in source and binary forms, with or without
+*  modification, are permitted provided that the following conditions
+*  are met:
+* 
+*   * Redistributions of source code must retain the above copyright
+*     notice, this list of conditions and the following disclaimer.
+*   * Redistributions in binary form must reproduce the above
+*     copyright notice, this list of conditions and the following
+*     disclaimer in the documentation and/or other materials provided
+*     with the distribution.
+*   * Neither the name of the Willow Garage nor the names of its
+*     contributors may be used to endorse or promote products derived
+*     from this software without specific prior written permission.
+* 
+*  THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+*  "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+*  LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+*  FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+*  COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+*  INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+*  BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+*  LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
+*  CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+*  LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+*  ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+*  POSSIBILITY OF SUCH DAMAGE.
+*********************************************************************/
+
+#ifndef SENSOR_MSGS_DISTORTION_MODELS_H
+#define SENSOR_MSGS_DISTORTION_MODELS_H
+
+#include <string>
+
+namespace sensor_msgs
+{
+  namespace distortion_models
+  {
+    const std::string PLUMB_BOB = "plumb_bob";
+    const std::string RATIONAL_POLYNOMIAL = "rational_polynomial";
+    const std::string EQUIDISTANT = "equidistant";
+  }
+}
+
+#endif

sensor_msgs/include/sensor_msgs/fill_image.h

@@ -0,0 +1,70 @@
+/*********************************************************************
+* Software License Agreement (BSD License)
+* 
+*  Copyright (c) 2008, Willow Garage, Inc.
+*  All rights reserved.
+* 
+*  Redistribution and use in source and binary forms, with or without
+*  modification, are permitted provided that the following conditions
+*  are met:
+* 
+*   * Redistributions of source code must retain the above copyright
+*     notice, this list of conditions and the following disclaimer.
+*   * Redistributions in binary form must reproduce the above
+*     copyright notice, this list of conditions and the following
+*     disclaimer in the documentation and/or other materials provided
+*     with the distribution.
+*   * Neither the name of the Willow Garage nor the names of its
+*     contributors may be used to endorse or promote products derived
+*     from this software without specific prior written permission.
+* 
+*  THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+*  "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+*  LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+*  FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+*  COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+*  INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+*  BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+*  LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
+*  CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+*  LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+*  ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+*  POSSIBILITY OF SUCH DAMAGE.
+*********************************************************************/
+
+#ifndef FILLIMAGE_HH
+#define FILLIMAGE_HH
+
+#include "sensor_msgs/Image.h"
+#include "sensor_msgs/image_encodings.h"
+
+namespace sensor_msgs
+{
+
+  static inline bool fillImage(Image& image,
+                 const std::string& encoding_arg,
+                 uint32_t rows_arg,
+                 uint32_t cols_arg,
+                 uint32_t step_arg,
+                 const void* data_arg)
+  {
+    image.encoding = encoding_arg;
+    image.height   = rows_arg;
+    image.width    = cols_arg;
+    image.step     = step_arg;
+    size_t st0 = (step_arg * rows_arg);
+    image.data.resize(st0);
+    memcpy(&image.data[0], data_arg, st0);
+
+    image.is_bigendian = 0;
+    return true;
+  }
+
+  static inline void clearImage(Image& image)
+  {
+    image.data.resize(0);
+  }
+}
+
+
+#endif

sensor_msgs/include/sensor_msgs/image_encodings.h

@@ -0,0 +1,233 @@
+
+/*********************************************************************
+* Software License Agreement (BSD License)
+* 
+*  Copyright (c) 2009, Willow Garage, Inc.
+*  All rights reserved.
+* 
+*  Redistribution and use in source and binary forms, with or without
+*  modification, are permitted provided that the following conditions
+*  are met:
+* 
+*   * Redistributions of source code must retain the above copyright
+*     notice, this list of conditions and the following disclaimer.
+*   * Redistributions in binary form must reproduce the above
+*     copyright notice, this list of conditions and the following
+*     disclaimer in the documentation and/or other materials provided
+*     with the distribution.
+*   * Neither the name of the Willow Garage nor the names of its
+*     contributors may be used to endorse or promote products derived
+*     from this software without specific prior written permission.
+* 
+*  THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+*  "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+*  LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+*  FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+*  COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+*  INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+*  BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+*  LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
+*  CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+*  LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+*  ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+*  POSSIBILITY OF SUCH DAMAGE.
+*********************************************************************/
+
+#ifndef SENSOR_MSGS_IMAGE_ENCODINGS_H
+#define SENSOR_MSGS_IMAGE_ENCODINGS_H
+
+#include <cstdlib>
+#include <stdexcept>
+#include <string>
+
+namespace sensor_msgs
+{
+  namespace image_encodings
+  {
+    const std::string RGB8 = "rgb8";
+    const std::string RGBA8 = "rgba8";
+    const std::string RGB16 = "rgb16";
+    const std::string RGBA16 = "rgba16";
+    const std::string BGR8 = "bgr8";
+    const std::string BGRA8 = "bgra8";
+    const std::string BGR16 = "bgr16";
+    const std::string BGRA16 = "bgra16";
+    const std::string MONO8="mono8";
+    const std::string MONO16="mono16";
+
+    // OpenCV CvMat types
+    const std::string TYPE_8UC1="8UC1";
+    const std::string TYPE_8UC2="8UC2";
+    const std::string TYPE_8UC3="8UC3";
+    const std::string TYPE_8UC4="8UC4";
+    const std::string TYPE_8SC1="8SC1";
+    const std::string TYPE_8SC2="8SC2";
+    const std::string TYPE_8SC3="8SC3";
+    const std::string TYPE_8SC4="8SC4";
+    const std::string TYPE_16UC1="16UC1";
+    const std::string TYPE_16UC2="16UC2";
+    const std::string TYPE_16UC3="16UC3";
+    const std::string TYPE_16UC4="16UC4";
+    const std::string TYPE_16SC1="16SC1";
+    const std::string TYPE_16SC2="16SC2";
+    const std::string TYPE_16SC3="16SC3";
+    const std::string TYPE_16SC4="16SC4";
+    const std::string TYPE_32SC1="32SC1";
+    const std::string TYPE_32SC2="32SC2";
+    const std::string TYPE_32SC3="32SC3";
+    const std::string TYPE_32SC4="32SC4";
+    const std::string TYPE_32FC1="32FC1";
+    const std::string TYPE_32FC2="32FC2";
+    const std::string TYPE_32FC3="32FC3";
+    const std::string TYPE_32FC4="32FC4";
+    const std::string TYPE_64FC1="64FC1";
+    const std::string TYPE_64FC2="64FC2";
+    const std::string TYPE_64FC3="64FC3";
+    const std::string TYPE_64FC4="64FC4";
+
+    // Bayer encodings
+    const std::string BAYER_RGGB8="bayer_rggb8";
+    const std::string BAYER_BGGR8="bayer_bggr8";
+    const std::string BAYER_GBRG8="bayer_gbrg8";
+    const std::string BAYER_GRBG8="bayer_grbg8";
+    const std::string BAYER_RGGB16="bayer_rggb16";
+    const std::string BAYER_BGGR16="bayer_bggr16";
+    const std::string BAYER_GBRG16="bayer_gbrg16";
+    const std::string BAYER_GRBG16="bayer_grbg16";
+
+    // Miscellaneous
+    // This is the UYVY version of YUV422 codec http://www.fourcc.org/yuv.php#UYVY
+    // with an 8-bit depth
+    const std::string YUV422="yuv422";
+
+    // Prefixes for abstract image encodings
+    const std::string ABSTRACT_ENCODING_PREFIXES[] = {
+        "8UC", "8SC", "16UC", "16SC", "32SC", "32FC", "64FC"};
+
+    // Utility functions for inspecting an encoding string
+    static inline bool isColor(const std::string& encoding)
+    {
+      return encoding == RGB8  || encoding == BGR8 ||
+             encoding == RGBA8 || encoding == BGRA8 ||
+             encoding == RGB16 || encoding == BGR16 ||
+             encoding == RGBA16 || encoding == BGRA16;
+    }
+
+    static inline bool isMono(const std::string& encoding)
+    {
+      return encoding == MONO8 || encoding == MONO16;
+    }
+
+    static inline bool isBayer(const std::string& encoding)
+    {
+      return encoding == BAYER_RGGB8 || encoding == BAYER_BGGR8 ||
+             encoding == BAYER_GBRG8 || encoding == BAYER_GRBG8 ||
+             encoding == BAYER_RGGB16 || encoding == BAYER_BGGR16 ||
+             encoding == BAYER_GBRG16 || encoding == BAYER_GRBG16;
+    }
+
+    static inline bool hasAlpha(const std::string& encoding)
+    {
+      return encoding == RGBA8 || encoding == BGRA8 ||
+             encoding == RGBA16 || encoding == BGRA16;
+    }
+
+    static inline int numChannels(const std::string& encoding)
+    {
+      // First do the common-case encodings
+      if (encoding == MONO8 ||
+          encoding == MONO16)
+        return 1;
+      if (encoding == BGR8 ||
+          encoding == RGB8 ||
+          encoding == BGR16 ||
+          encoding == RGB16)
+        return 3;
+      if (encoding == BGRA8 ||
+          encoding == RGBA8 ||
+          encoding == BGRA16 ||
+          encoding == RGBA16)
+        return 4;
+      if (encoding == BAYER_RGGB8 ||
+          encoding == BAYER_BGGR8 ||
+          encoding == BAYER_GBRG8 ||
+          encoding == BAYER_GRBG8 ||
+          encoding == BAYER_RGGB16 ||
+          encoding == BAYER_BGGR16 ||
+          encoding == BAYER_GBRG16 ||
+          encoding == BAYER_GRBG16)
+        return 1;
+
+      // Now all the generic content encodings
+      // TODO: Rewrite with regex when ROS supports C++11
+      for (size_t i=0; i < sizeof(ABSTRACT_ENCODING_PREFIXES) / sizeof(*ABSTRACT_ENCODING_PREFIXES); i++)
+      {
+        std::string prefix = ABSTRACT_ENCODING_PREFIXES[i];
+        if (encoding.substr(0, prefix.size()) != prefix)
+          continue;
+        if (encoding.size() == prefix.size())
+          return 1;  // ex. 8UC -> 1
+        int n_channel = atoi(encoding.substr(prefix.size(),
+                                             encoding.size() - prefix.size()).c_str());  // ex. 8UC5 -> 5
+        if (n_channel != 0)
+          return n_channel;  // valid encoding string
+      }
+
+      if (encoding == YUV422)
+        return 2;
+
+      throw std::runtime_error("Unknown encoding " + encoding);
+      return -1;
+    }
+
+    static inline int bitDepth(const std::string& encoding)
+    {
+      if (encoding == MONO16)
+        return 16;
+      if (encoding == MONO8       ||
+          encoding == BGR8        ||
+          encoding == RGB8        ||
+          encoding == BGRA8       ||
+          encoding == RGBA8       ||
+          encoding == BAYER_RGGB8 ||
+          encoding == BAYER_BGGR8 ||
+          encoding == BAYER_GBRG8 ||
+          encoding == BAYER_GRBG8)
+        return 8;
+
+      if (encoding == MONO16       ||
+          encoding == BGR16        ||
+          encoding == RGB16        ||
+          encoding == BGRA16       ||
+          encoding == RGBA16       ||
+          encoding == BAYER_RGGB16 ||
+          encoding == BAYER_BGGR16 ||
+          encoding == BAYER_GBRG16 ||
+          encoding == BAYER_GRBG16)
+        return 16;
+
+      // Now all the generic content encodings
+      // TODO: Rewrite with regex when ROS supports C++11
+      for (size_t i=0; i < sizeof(ABSTRACT_ENCODING_PREFIXES) / sizeof(*ABSTRACT_ENCODING_PREFIXES); i++)
+      {
+        std::string prefix = ABSTRACT_ENCODING_PREFIXES[i];
+        if (encoding.substr(0, prefix.size()) != prefix)
+          continue;
+        if (encoding.size() == prefix.size())
+          return atoi(prefix.c_str());  // ex. 8UC -> 8
+        int n_channel = atoi(encoding.substr(prefix.size(),
+                                             encoding.size() - prefix.size()).c_str());  // ex. 8UC10 -> 10
+        if (n_channel != 0)
+          return atoi(prefix.c_str());  // valid encoding string
+      }
+
+      if (encoding == YUV422)
+        return 8;
+
+      throw std::runtime_error("Unknown encoding " + encoding);
+      return -1;
+    }
+  }
+}
+
+#endif

sensor_msgs/include/sensor_msgs/impl/point_cloud2_iterator.h

@@ -0,0 +1,417 @@
+/*
+ * Software License Agreement (BSD License)
+ *
+ *  Copyright (c) 2013, Open Source Robotics Foundation
+ *  All rights reserved.
+ *
+ *  Redistribution and use in source and binary forms, with or without
+ *  modification, are permitted provided that the following conditions
+ *  are met:
+ *
+ *   * Redistributions of source code must retain the above copyright
+ *     notice, this list of conditions and the following disclaimer.
+ *   * Redistributions in binary form must reproduce the above
+ *     copyright notice, this list of conditions and the following
+ *     disclaimer in the documentation and/or other materials provided
+ *     with the distribution.
+ *   * Neither the name of Open Source Robotics Foundation nor the names of its
+ *     contributors may be used to endorse or promote products derived
+ *     from this software without specific prior written permission.
+ *
+ *  THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ *  "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ *  LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+ *  FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+ *  COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+ *  INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+ *  BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+ *  LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
+ *  CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ *  LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ *  ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ *  POSSIBILITY OF SUCH DAMAGE.
+ */
+
+#ifndef SENSOR_MSGS_IMPL_POINT_CLOUD_ITERATOR_H
+#define SENSOR_MSGS_IMPL_POINT_CLOUD_ITERATOR_H
+
+#include <sensor_msgs/PointCloud2.h>
+#include <cstdarg>
+#include <sstream>
+#include <string>
+#include <vector>
+
+/**
+ * \brief Private implementation used by PointCloud2Iterator
+ * \author Vincent Rabaud
+ */
+
+namespace
+{
+/** Return the size of a datatype (which is an enum of sensor_msgs::PointField::) in bytes
+ * @param datatype one of the enums of sensor_msgs::PointField::
+ */
+inline int sizeOfPointField(int datatype)
+{
+  if ((datatype == sensor_msgs::PointField::INT8) || (datatype == sensor_msgs::PointField::UINT8))
+    return 1;
+  else if ((datatype == sensor_msgs::PointField::INT16) || (datatype == sensor_msgs::PointField::UINT16))
+    return 2;
+  else if ((datatype == sensor_msgs::PointField::INT32) || (datatype == sensor_msgs::PointField::UINT32) ||
+      (datatype == sensor_msgs::PointField::FLOAT32))
+    return 4;
+  else if (datatype == sensor_msgs::PointField::FLOAT64)
+    return 8;
+  else
+  {
+    std::stringstream err;
+    err << "PointField of type " << datatype << " does not exist";
+    throw std::runtime_error(err.str());
+  }
+  return -1;
+}
+
+/** Private function that adds a PointField to the "fields" member of a PointCloud2
+ * @param cloud_msg the PointCloud2 to add a field to
+ * @param name the name of the field
+ * @param count the number of elements in the PointField
+ * @param datatype the datatype of the elements
+ * @param offset the offset of that element
+ * @return the offset of the next PointField that will be added to the PointCLoud2
+ */
+inline int addPointField(sensor_msgs::PointCloud2 &cloud_msg, const std::string &name, int count, int datatype,
+    int offset)
+{
+  sensor_msgs::PointField point_field;
+  point_field.name = name;
+  point_field.count = count;
+  point_field.datatype = datatype;
+  point_field.offset = offset;
+  cloud_msg.fields.push_back(point_field);
+
+  // Update the offset
+  return offset + point_field.count * sizeOfPointField(datatype);
+}
+}
+
+////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
+
+namespace sensor_msgs
+{
+
+inline PointCloud2Modifier::PointCloud2Modifier(PointCloud2& cloud_msg) : cloud_msg_(cloud_msg)
+{
+}
+
+inline size_t PointCloud2Modifier::size() const
+{
+  return cloud_msg_.data.size() / cloud_msg_.point_step;
+}
+
+inline void PointCloud2Modifier::reserve(size_t size)
+{
+  cloud_msg_.data.reserve(size * cloud_msg_.point_step);
+}
+
+inline void PointCloud2Modifier::resize(size_t size)
+{
+  cloud_msg_.data.resize(size * cloud_msg_.point_step);
+
+  // Update height/width
+  if (cloud_msg_.height == 1) {
+    cloud_msg_.width = size;
+    cloud_msg_.row_step = size * cloud_msg_.point_step;
+  } else
+    if (cloud_msg_.width == 1)
+      cloud_msg_.height = size;
+    else {
+      cloud_msg_.height = 1;
+      cloud_msg_.width = size;
+      cloud_msg_.row_step = size * cloud_msg_.point_step;
+    }
+}
+
+inline void PointCloud2Modifier::clear()
+{
+  cloud_msg_.data.clear();
+
+  // Update height/width
+  if (cloud_msg_.height == 1)
+    cloud_msg_.row_step = cloud_msg_.width = 0;
+  else
+    if (cloud_msg_.width == 1)
+      cloud_msg_.height = 0;
+    else
+      cloud_msg_.row_step = cloud_msg_.width = cloud_msg_.height = 0;
+}
+
+
+/**
+ * @brief Function setting some fields in a PointCloud and adjusting the
+ *        internals of the PointCloud2
+ * @param n_fields the number of fields to add. The fields are given as
+ *        triplets: name of the field as char*, number of elements in the
+ *        field, the datatype of the elements in the field
+ *
+ * E.g, you create your PointCloud2 message with XYZ/RGB as follows:
+ * <PRE>
+ *   setPointCloud2FieldsByString(cloud_msg, 4, "x", 1, sensor_msgs::PointField::FLOAT32,
+ *                                              "y", 1, sensor_msgs::PointField::FLOAT32,
+ *                                              "z", 1, sensor_msgs::PointField::FLOAT32,
+ *                                              "rgb", 1, sensor_msgs::PointField::FLOAT32);
+ * </PRE>
+ * WARNING: THIS DOES NOT TAKE INTO ACCOUNT ANY PADDING AS DONE UNTIL HYDRO
+ * For simple usual cases, the overloaded setPointCloud2FieldsByString is what you want.
+ */
+inline void PointCloud2Modifier::setPointCloud2Fields(int n_fields, ...)
+{
+  cloud_msg_.fields.clear();
+  cloud_msg_.fields.reserve(n_fields);
+  va_list vl;
+  va_start(vl, n_fields);
+  int offset = 0;
+  for (int i = 0; i < n_fields; ++i) {
+    // Create the corresponding PointField
+    std::string name(va_arg(vl, char*));
+    int count(va_arg(vl, int));
+    int datatype(va_arg(vl, int));
+    offset = addPointField(cloud_msg_, name, count, datatype, offset);
+  }
+  va_end(vl);
+
+  // Resize the point cloud accordingly
+  cloud_msg_.point_step = offset;
+  cloud_msg_.row_step = cloud_msg_.width * cloud_msg_.point_step;
+  cloud_msg_.data.resize(cloud_msg_.height * cloud_msg_.row_step);
+}
+
+/**
+ * @brief Function setting some fields in a PointCloud and adjusting the
+ *        internals of the PointCloud2
+ * @param n_fields the number of fields to add. The fields are given as
+ *        strings: "xyz" (3 floats), "rgb" (3 uchar stacked in a float),
+ *        "rgba" (4 uchar stacked in a float)
+ * @return void
+ *
+ * WARNING: THIS FUNCTION DOES ADD ANY NECESSARY PADDING TRANSPARENTLY
+ */
+inline void PointCloud2Modifier::setPointCloud2FieldsByString(int n_fields, ...)
+{
+  cloud_msg_.fields.clear();
+  cloud_msg_.fields.reserve(n_fields);
+  va_list vl;
+  va_start(vl, n_fields);
+  int offset = 0;
+  for (int i = 0; i < n_fields; ++i) {
+    // Create the corresponding PointFields
+    std::string
+    field_name = std::string(va_arg(vl, char*));
+    if (field_name == "xyz") {
+      sensor_msgs::PointField point_field;
+      // Do x, y and z
+      offset = addPointField(cloud_msg_, "x", 1, sensor_msgs::PointField::FLOAT32, offset);
+      offset = addPointField(cloud_msg_, "y", 1, sensor_msgs::PointField::FLOAT32, offset);
+      offset = addPointField(cloud_msg_, "z", 1, sensor_msgs::PointField::FLOAT32, offset);
+      offset += sizeOfPointField(sensor_msgs::PointField::FLOAT32);
+    } else
+      if ((field_name == "rgb") || (field_name == "rgba")) {
+        offset = addPointField(cloud_msg_, field_name, 1, sensor_msgs::PointField::FLOAT32, offset);
+        offset += 3 * sizeOfPointField(sensor_msgs::PointField::FLOAT32);
+      } else
+        throw std::runtime_error("Field " + field_name + " does not exist");
+  }
+  va_end(vl);
+
+  // Resize the point cloud accordingly
+  cloud_msg_.point_step = offset;
+  cloud_msg_.row_step = cloud_msg_.width * cloud_msg_.point_step;
+  cloud_msg_.data.resize(cloud_msg_.height * cloud_msg_.row_step);
+}
+
+////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
+
+namespace impl
+{
+
+/**
+ */
+template<typename T, typename TT, typename U, typename C, template <typename> class V>
+PointCloud2IteratorBase<T, TT, U, C, V>::PointCloud2IteratorBase() : data_char_(0), data_(0), data_end_(0)
+{
+}
+
+/**
+ * @param cloud_msg_ The PointCloud2 to iterate upon
+ * @param field_name The field to iterate upon
+ */
+template<typename T, typename TT, typename U, typename C, template <typename> class V>
+PointCloud2IteratorBase<T, TT, U, C, V>::PointCloud2IteratorBase(C &cloud_msg, const std::string &field_name)
+{
+  int offset = set_field(cloud_msg, field_name);
+
+  data_char_ = cloud_msg.data.data() + offset;
+  data_ = reinterpret_cast<TT*>(data_char_);
+  data_end_ = reinterpret_cast<TT*>(data_char_ + cloud_msg.data.size());
+}
+
+/** Assignment operator
+ * @param iter the iterator to copy data from
+ * @return a reference to *this
+ */
+template<typename T, typename TT, typename U, typename C, template <typename> class V>
+V<T>& PointCloud2IteratorBase<T, TT, U, C, V>::operator =(const V<T> &iter)
+{
+  if (this != &iter)
+  {
+    point_step_ = iter.point_step_;
+    data_char_ = iter.data_char_;
+    data_ = iter.data_;
+    data_end_ = iter.data_end_;
+    is_bigendian_ = iter.is_bigendian_;
+  }
+
+  return *this;
+}
+
+/** Access the i th element starting at the current pointer (useful when a field has several elements of the same
+ * type)
+ * @param i
+ * @return a reference to the i^th value from the current position
+ */
+template<typename T, typename TT, typename U, typename C, template <typename> class V>
+TT& PointCloud2IteratorBase<T, TT, U, C, V>::operator [](size_t i) const
+{
+  return *(data_ + i);
+}
+
+/** Dereference the iterator. Equivalent to accessing it through [0]
+ * @return the value to which the iterator is pointing
+ */
+template<typename T, typename TT, typename U, typename C, template <typename> class V>
+TT& PointCloud2IteratorBase<T, TT, U, C, V>::operator *() const
+{
+  return *data_;
+}
+
+/** Increase the iterator to the next element
+ * @return a reference to the updated iterator
+ */
+template<typename T, typename TT, typename U, typename C, template <typename> class V>
+V<T>& PointCloud2IteratorBase<T, TT, U, C, V>::operator ++()
+{
+  data_char_ += point_step_;
+  data_ = reinterpret_cast<TT*>(data_char_);
+  return *static_cast<V<T>*>(this);
+}
+
+/** Basic pointer addition
+ * @param i the amount to increase the iterator by
+ * @return an iterator with an increased position
+ */
+template<typename T, typename TT, typename U, typename C, template <typename> class V>
+V<T> PointCloud2IteratorBase<T, TT, U, C, V>::operator +(int i)
+{
+  V<T> res = *static_cast<V<T>*>(this);
+
+  res.data_char_ += i*point_step_;
+  res.data_ = reinterpret_cast<TT*>(res.data_char_);
+
+  return res;
+}
+
+/** Increase the iterator by a certain amount
+ * @return a reference to the updated iterator
+ */
+template<typename T, typename TT, typename U, typename C, template <typename> class V>
+V<T>& PointCloud2IteratorBase<T, TT, U, C, V>::operator +=(int i)
+{
+  data_char_ += i*point_step_;
+  data_ = reinterpret_cast<TT*>(data_char_);
+  return *static_cast<V<T>*>(this);
+}
+
+/** Compare to another iterator
+ * @return whether the current iterator points to a different address than the other one
+ */
+template<typename T, typename TT, typename U, typename C, template <typename> class V>
+bool PointCloud2IteratorBase<T, TT, U, C, V>::operator !=(const V<T>& iter) const
+{
+  return iter.data_ != data_;
+}
+
+/** Return the end iterator
+ * @return the end iterator (useful when performing normal iterator processing with ++)
+ */
+template<typename T, typename TT, typename U, typename C, template <typename> class V>
+V<T> PointCloud2IteratorBase<T, TT, U, C, V>::end() const
+{
+  V<T> res = *static_cast<const V<T>*>(this);
+  res.data_ = data_end_;
+  return res;
+}
+
+/** Common code to set the field of the PointCloud2
+  * @param cloud_msg the PointCloud2 to modify
+  * @param field_name the name of the field to iterate upon
+  * @return the offset at which the field is found
+  */
+template<typename T, typename TT, typename U, typename C, template <typename> class V>
+int PointCloud2IteratorBase<T, TT, U, C, V>::set_field(const sensor_msgs::PointCloud2 &cloud_msg, const std::string &field_name)
+{
+  is_bigendian_ = cloud_msg.is_bigendian;
+  point_step_ = cloud_msg.point_step;
+  // make sure the channel is valid
+  std::vector<sensor_msgs::PointField>::const_iterator field_iter = cloud_msg.fields.begin(), field_end =
+      cloud_msg.fields.end();
+  while ((field_iter != field_end) && (field_iter->name != field_name))
+    ++field_iter;
+
+  if (field_iter == field_end) {
+    // Handle the special case of r,g,b,a (we assume they are understood as the channels of an rgb or rgba field)
+    if ((field_name == "r") || (field_name == "g") || (field_name == "b") || (field_name == "a"))
+    {
+      // Check that rgb or rgba is present
+      field_iter = cloud_msg.fields.begin();
+      while ((field_iter != field_end) && (field_iter->name != "rgb") && (field_iter->name != "rgba"))
+        ++field_iter;
+      if (field_iter == field_end)
+        throw std::runtime_error("Field " + field_name + " does not exist");
+      if (field_name == "r")
+      {
+        if (is_bigendian_)
+          return field_iter->offset + 1;
+        else
+          return field_iter->offset + 2;
+      }
+      if (field_name == "g")
+      {
+        if (is_bigendian_)
+          return field_iter->offset + 2;
+        else
+          return field_iter->offset + 1;
+      }
+      if (field_name == "b")
+      {
+        if (is_bigendian_)
+          return field_iter->offset + 3;
+        else
+          return field_iter->offset + 0;
+      }
+      if (field_name == "a")
+      {
+        if (is_bigendian_)
+          return field_iter->offset + 0;
+        else
+          return field_iter->offset + 3;
+      }
+    } else
+      throw std::runtime_error("Field " + field_name + " does not exist");
+  }
+
+  return field_iter->offset;
+}
+
+}
+}
+
+#endif// SENSOR_MSGS_IMPL_POINT_CLOUD_ITERATOR_H

sensor_msgs/include/sensor_msgs/point_cloud2_iterator.h

@@ -0,0 +1,302 @@
+/*
+ * Software License Agreement (BSD License)
+ *
+ *  Copyright (c) 2013, Open Source Robotics Foundation
+ *  All rights reserved.
+ *
+ *  Redistribution and use in source and binary forms, with or without
+ *  modification, are permitted provided that the following conditions
+ *  are met:
+ *
+ *   * Redistributions of source code must retain the above copyright
+ *     notice, this list of conditions and the following disclaimer.
+ *   * Redistributions in binary form must reproduce the above
+ *     copyright notice, this list of conditions and the following
+ *     disclaimer in the documentation and/or other materials provided
+ *     with the distribution.
+ *   * Neither the name of Open Source Robotics Foundation nor the names of its
+ *     contributors may be used to endorse or promote products derived
+ *     from this software without specific prior written permission.
+ *
+ *  THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ *  "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ *  LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+ *  FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+ *  COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+ *  INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+ *  BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+ *  LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
+ *  CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ *  LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ *  ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ *  POSSIBILITY OF SUCH DAMAGE.
+ */
+
+#ifndef SENSOR_MSGS_POINT_CLOUD2_ITERATOR_H
+#define SENSOR_MSGS_POINT_CLOUD2_ITERATOR_H
+
+#include <sensor_msgs/PointCloud2.h>
+#include <cstdarg>
+#include <string>
+#include <vector>
+
+/**
+ * \brief Tools for manipulating sensor_msgs
+ *
+ * This file provides two sets of utilities to modify and parse PointCloud2
+ * The first set allows you to conveniently set the fields by hand:
+ * <PRE>
+ *   #include <sensor_msgs/point_cloud_iterator.h>
+ *   // Create a PointCloud2
+ *   sensor_msgs::PointCloud2 cloud_msg;
+ *   // Fill some internals of the PoinCloud2 like the header/width/height ...
+ *   cloud_msgs.height = 1;  cloud_msgs.width = 4;
+ *   // Set the point fields to xyzrgb and resize the vector with the following command
+ *   // 4 is for the number of added fields. Each come in triplet: the name of the PointField,
+ *   // the number of occurrences of the type in the PointField, the type of the PointField
+ *   sensor_msgs::PointCloud2Modifier modifier(cloud_msg);
+ *   modifier.setPointCloud2Fields(4, "x", 1, sensor_msgs::PointField::FLOAT32,
+ *                                            "y", 1, sensor_msgs::PointField::FLOAT32,
+ *                                            "z", 1, sensor_msgs::PointField::FLOAT32,
+ *                                            "rgb", 1, sensor_msgs::PointField::FLOAT32);
+ *   // For convenience and the xyz, rgb, rgba fields, you can also use the following overloaded function.
+ *   // You have to be aware that the following function does add extra padding for backward compatibility though
+ *   // so it is definitely the solution of choice for PointXYZ and PointXYZRGB
+ *   // 2 is for the number of fields to add
+ *   modifier.setPointCloud2FieldsByString(2, "xyz", "rgb");
+ *   // You can then reserve / resize as usual
+ *   modifier.resize(100);
+ * </PRE>
+ *
+ * The second set allow you to traverse your PointCloud using an iterator:
+ * <PRE>
+ *   // Define some raw data we'll put in the PointCloud2
+ *   float point_data[] = {1.0, 2.0, 3.0, 4.0, 5.0, 6.0, 7.0, 8.0, 9.0, 10.0, 11.0, 12.0};
+ *   uint8_t color_data[] = {40, 80, 120, 160, 200, 240, 20, 40, 60, 80, 100, 120};
+ *   // Define the iterators. When doing so, you define the Field you would like to iterate upon and
+ *   // the type of you would like returned: it is not necessary the type of the PointField as sometimes
+ *   // you pack data in another type (e.g. 3 uchar + 1 uchar for RGB are packed in a float)
+ *   sensor_msgs::PointCloud2Iterator<float> iter_x(cloud_msg, "x");
+ *   sensor_msgs::PointCloud2Iterator<float> iter_y(cloud_msg, "y");
+ *   sensor_msgs::PointCloud2Iterator<float> iter_z(cloud_msg, "z");
+ *   // Even though the r,g,b,a fields do not exist (it's usually rgb, rgba), you can create iterators for
+ *   // those: they will handle data packing for you (in little endian RGB is packed as *,R,G,B in a float
+ *   // and RGBA as A,R,G,B)
+ *   sensor_msgs::PointCloud2Iterator<uint8_t> iter_r(cloud_msg, "r");
+ *   sensor_msgs::PointCloud2Iterator<uint8_t> iter_g(cloud_msg, "g");
+ *   sensor_msgs::PointCloud2Iterator<uint8_t> iter_b(cloud_msg, "b");
+ *   // Fill the PointCloud2
+ *   for(size_t i=0; i<n_points; ++i, ++iter_x, ++iter_y, ++iter_z, ++iter_r, ++iter_g, ++iter_b) {
+ *     *iter_x = point_data[3*i+0];
+ *     *iter_y = point_data[3*i+1];
+ *     *iter_z = point_data[3*i+2];
+ *     *iter_r = color_data[3*i+0];
+ *     *iter_g = color_data[3*i+1];
+ *     *iter_b = color_data[3*i+2];
+ *   }
+ * </PRE>
+ */
+
+namespace sensor_msgs
+{
+/**
+ * @brief Enables modifying a sensor_msgs::PointCloud2 like a container
+ */
+class PointCloud2Modifier
+{
+public:
+  /**
+   * @brief Default constructor
+   * @param cloud_msg The sensor_msgs::PointCloud2 to modify
+   */
+  PointCloud2Modifier(PointCloud2& cloud_msg);
+
+  /**
+   * @return the number of T's in the original sensor_msgs::PointCloud2
+   */
+  size_t size() const;
+
+  /**
+   * @param size The number of T's to reserve in the original sensor_msgs::PointCloud2 for
+   */
+  void reserve(size_t size);
+
+  /**
+   * @param size The number of T's to change the size of the original sensor_msgs::PointCloud2 by
+   */
+  void resize(size_t size);
+
+  /**
+   * @brief remove all T's from the original sensor_msgs::PointCloud2
+   */
+  void clear();
+
+  /**
+   * @brief Function setting some fields in a PointCloud and adjusting the
+   *        internals of the PointCloud2
+   * @param n_fields the number of fields to add. The fields are given as
+   *        triplets: name of the field as char*, number of elements in the
+   *        field, the datatype of the elements in the field
+   *
+   * E.g, you create your PointCloud2 message with XYZ/RGB as follows:
+   * <PRE>
+   *   setPointCloud2Fields(cloud_msg, 4, "x", 1, sensor_msgs::PointField::FLOAT32,
+   *                                              "y", 1, sensor_msgs::PointField::FLOAT32,
+   *                                              "z", 1, sensor_msgs::PointField::FLOAT32,
+   *                                              "rgb", 1, sensor_msgs::PointField::FLOAT32);
+   * </PRE>
+   * WARNING: THIS DOES NOT TAKE INTO ACCOUNT ANY PADDING AS DONE UNTIL HYDRO
+   * For simple usual cases, the overloaded setPointCloud2FieldsByString is what you want.
+   */
+  void setPointCloud2Fields(int n_fields, ...);
+
+  /**
+   * @brief Function setting some fields in a PointCloud and adjusting the
+   *        internals of the PointCloud2
+   * @param n_fields the number of fields to add. The fields are given as
+   *        strings: "xyz" (3 floats), "rgb" (3 uchar stacked in a float),
+   *        "rgba" (4 uchar stacked in a float)
+   * @return void
+   *
+   * WARNING: THIS FUNCTION DOES ADD ANY NECESSARY PADDING TRANSPARENTLY
+   */
+  void setPointCloud2FieldsByString(int n_fields, ...);
+protected:
+  /** A reference to the original sensor_msgs::PointCloud2 that we read */
+  PointCloud2& cloud_msg_;
+};
+
+namespace impl
+{
+/** Private base class for PointCloud2Iterator and PointCloud2ConstIterator
+ * T is the type of the value on which the child class will be templated
+ * TT is the type of the value to be retrieved (same as T except for constness)
+ * U is the type of the raw data in PointCloud2 (only uchar and const uchar are supported)
+ * C is the type of the pointcloud to intialize from (const or not)
+ * V is the derived class (yop, curiously recurring template pattern)
+ */
+template<typename T, typename TT, typename U, typename C, template <typename> class V>
+class PointCloud2IteratorBase
+{
+public:
+  /**
+   */
+  PointCloud2IteratorBase();
+
+  /**
+   * @param cloud_msg The PointCloud2 to iterate upon
+   * @param field_name The field to iterate upon
+   */
+  PointCloud2IteratorBase(C &cloud_msg, const std::string &field_name);
+
+  /** Assignment operator
+   * @param iter the iterator to copy data from
+   * @return a reference to *this
+   */
+  V<T>& operator =(const V<T>& iter);
+
+  /** Access the i th element starting at the current pointer (useful when a field has several elements of the same
+   * type)
+   * @param i
+   * @return a reference to the i^th value from the current position
+   */
+  TT& operator [](size_t i) const;
+
+  /** Dereference the iterator. Equivalent to accessing it through [0]
+   * @return the value to which the iterator is pointing
+   */
+  TT& operator *() const;
+
+  /** Increase the iterator to the next element
+   * @return a reference to the updated iterator
+   */
+  V<T>& operator ++();
+
+  /** Basic pointer addition
+   * @param i the amount to increase the iterator by
+   * @return an iterator with an increased position
+   */
+  V<T> operator +(int i);
+
+  /** Increase the iterator by a certain amount
+   * @return a reference to the updated iterator
+   */
+  V<T>& operator +=(int i);
+
+  /** Compare to another iterator
+   * @return whether the current iterator points to a different address than the other one
+   */
+  bool operator !=(const V<T>& iter) const;
+
+  /** Return the end iterator
+   * @return the end iterator (useful when performing normal iterator processing with ++)
+   */
+  V<T> end() const;
+
+private:
+  /** Common code to set the field of the PointCloud2
+   * @param cloud_msg the PointCloud2 to modify
+   * @param field_name the name of the field to iterate upon
+   * @return the offset at which the field is found
+   */
+  int set_field(const sensor_msgs::PointCloud2 &cloud_msg, const std::string &field_name);
+
+  /** The "point_step" of the original cloud */
+  int point_step_;
+  /** The raw data  in uchar* where the iterator is */
+  U* data_char_;
+  /** The cast data where the iterator is */
+  TT* data_;
+  /** The end() pointer of the iterator */
+  TT* data_end_;
+  /** Whether the fields are stored as bigendian */
+  bool is_bigendian_;
+};
+}
+
+/**
+ * \brief Class that can iterate over a PointCloud2
+ *
+ * T type of the element being iterated upon
+ * E.g, you create your PointClou2 message as follows:
+ * <PRE>
+ *   setPointCloud2FieldsByString(cloud_msg, 2, "xyz", "rgb");
+ * </PRE>
+ *
+ * For iterating over XYZ, you do :
+ * <PRE>
+ *   sensor_msgs::PointCloud2Iterator<float> iter_x(cloud_msg, "x");
+ * </PRE>
+ * and then access X through iter_x[0] or *iter_x
+ * You could create an iterator for Y and Z too but as they are consecutive,
+ * you can just use iter_x[1] and iter_x[2]
+ *
+ * For iterating over RGB, you do:
+ * <PRE>
+ * sensor_msgs::PointCloud2Iterator<uint8_t> iter_rgb(cloud_msg, "rgb");
+ * </PRE>
+ * and then access R,G,B through  iter_rgb[0], iter_rgb[1], iter_rgb[2]
+ */
+template<typename T>
+class PointCloud2Iterator : public impl::PointCloud2IteratorBase<T, T, unsigned char, sensor_msgs::PointCloud2, PointCloud2Iterator>
+{
+public:
+  PointCloud2Iterator(sensor_msgs::PointCloud2 &cloud_msg, const std::string &field_name) :
+    impl::PointCloud2IteratorBase<T, T, unsigned char, sensor_msgs::PointCloud2, sensor_msgs::PointCloud2Iterator>::PointCloud2IteratorBase(cloud_msg, field_name) {}
+};
+
+/**
+ * \brief Same as a PointCloud2Iterator but for const data
+ */
+template<typename T>
+class PointCloud2ConstIterator : public impl::PointCloud2IteratorBase<T, const T, const unsigned char, const sensor_msgs::PointCloud2, PointCloud2ConstIterator>
+{
+public:
+  PointCloud2ConstIterator(const sensor_msgs::PointCloud2 &cloud_msg, const std::string &field_name) :
+    impl::PointCloud2IteratorBase<T, const T, const unsigned char, const sensor_msgs::PointCloud2, sensor_msgs::PointCloud2ConstIterator>::PointCloud2IteratorBase(cloud_msg, field_name) {}
+};
+}
+
+#include <sensor_msgs/impl/point_cloud2_iterator.h>
+
+#endif// SENSOR_MSGS_POINT_CLOUD2_ITERATOR_H

sensor_msgs/include/sensor_msgs/point_cloud_conversion.h

@@ -0,0 +1,169 @@
+/*
+ * Software License Agreement (BSD License)
+ *
+ *  Copyright (c) 2010, Willow Garage, Inc.
+ *  All rights reserved.
+ *
+ *  Redistribution and use in source and binary forms, with or without
+ *  modification, are permitted provided that the following conditions
+ *  are met:
+ *
+ *   * Redistributions of source code must retain the above copyright
+ *     notice, this list of conditions and the following disclaimer.
+ *   * Redistributions in binary form must reproduce the above
+ *     copyright notice, this list of conditions and the following
+ *     disclaimer in the documentation and/or other materials provided
+ *     with the distribution.
+ *   * Neither the name of Willow Garage, Inc. nor the names of its
+ *     contributors may be used to endorse or promote products derived
+ *     from this software without specific prior written permission.
+ *
+ *  THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ *  "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ *  LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+ *  FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+ *  COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+ *  INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+ *  BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+ *  LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
+ *  CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ *  LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ *  ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ *  POSSIBILITY OF SUCH DAMAGE.
+ *
+ * $Id$
+ *
+ */
+
+#ifndef SENSOR_MSGS_POINT_CLOUD_CONVERSION_H
+#define SENSOR_MSGS_POINT_CLOUD_CONVERSION_H
+
+#include <sensor_msgs/PointCloud.h>
+#include <sensor_msgs/PointCloud2.h>
+#include <sensor_msgs/point_field_conversion.h>
+
+/** 
+  * \brief Convert between the old (sensor_msgs::PointCloud) and the new (sensor_msgs::PointCloud2) format.
+  * \author Radu Bogdan Rusu
+  */
+namespace sensor_msgs
+{
+  //////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
+  /** \brief Get the index of a specified field (i.e., dimension/channel)
+    * \param points the the point cloud message
+    * \param field_name the string defining the field name
+    */
+static inline int getPointCloud2FieldIndex (const sensor_msgs::PointCloud2 &cloud, const std::string &field_name)
+{
+  // Get the index we need
+  for (size_t d = 0; d < cloud.fields.size (); ++d)
+    if (cloud.fields[d].name == field_name)
+      return (d);
+  return (-1);
+}
+
+  ////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
+  /** \brief Convert a sensor_msgs::PointCloud message to a sensor_msgs::PointCloud2 message.
+    * \param input the message in the sensor_msgs::PointCloud format
+    * \param output the resultant message in the sensor_msgs::PointCloud2 format
+    */ 
+static inline bool convertPointCloudToPointCloud2 (const sensor_msgs::PointCloud &input, sensor_msgs::PointCloud2 &output)
+{
+  output.header = input.header;
+  output.width  = input.points.size ();
+  output.height = 1;
+  output.fields.resize (3 + input.channels.size ());
+  // Convert x/y/z to fields
+  output.fields[0].name = "x"; output.fields[1].name = "y"; output.fields[2].name = "z";
+  int offset = 0;
+  // All offsets are *4, as all field data types are float32
+  for (size_t d = 0; d < output.fields.size (); ++d, offset += 4)
+  {
+    output.fields[d].offset = offset;
+    output.fields[d].datatype = sensor_msgs::PointField::FLOAT32;
+    output.fields[d].count  = 1;
+  }
+  output.point_step = offset;
+  output.row_step   = output.point_step * output.width;
+  // Convert the remaining of the channels to fields
+  for (size_t d = 0; d < input.channels.size (); ++d)
+    output.fields[3 + d].name = input.channels[d].name;
+  output.data.resize (input.points.size () * output.point_step);
+  output.is_bigendian = false;  // @todo ?
+  output.is_dense     = false;
+
+  // Copy the data points
+  for (size_t cp = 0; cp < input.points.size (); ++cp)
+  {
+    memcpy (&output.data[cp * output.point_step + output.fields[0].offset], &input.points[cp].x, sizeof (float));
+    memcpy (&output.data[cp * output.point_step + output.fields[1].offset], &input.points[cp].y, sizeof (float));
+    memcpy (&output.data[cp * output.point_step + output.fields[2].offset], &input.points[cp].z, sizeof (float));
+    for (size_t d = 0; d < input.channels.size (); ++d)
+    {
+      if (input.channels[d].values.size() == input.points.size())
+      {
+        memcpy (&output.data[cp * output.point_step + output.fields[3 + d].offset], &input.channels[d].values[cp], sizeof (float));
+      }
+    }
+  }
+  return (true);
+}
+
+  ////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
+  /** \brief Convert a sensor_msgs::PointCloud2 message to a sensor_msgs::PointCloud message.
+    * \param input the message in the sensor_msgs::PointCloud2 format
+    * \param output the resultant message in the sensor_msgs::PointCloud format
+    */ 
+static inline bool convertPointCloud2ToPointCloud (const sensor_msgs::PointCloud2 &input, sensor_msgs::PointCloud &output)
+{
+
+  output.header = input.header;
+  output.points.resize (input.width * input.height);
+  output.channels.resize (input.fields.size () - 3);
+  // Get the x/y/z field offsets
+  int x_idx = getPointCloud2FieldIndex (input, "x");
+  int y_idx = getPointCloud2FieldIndex (input, "y");
+  int z_idx = getPointCloud2FieldIndex (input, "z");
+  if (x_idx == -1 || y_idx == -1 || z_idx == -1)
+  {
+    std::cerr << "x/y/z coordinates not found! Cannot convert to sensor_msgs::PointCloud!" << std::endl;
+    return (false);
+  }
+  int x_offset = input.fields[x_idx].offset;
+  int y_offset = input.fields[y_idx].offset;
+  int z_offset = input.fields[z_idx].offset;
+  uint8_t x_datatype = input.fields[x_idx].datatype;
+  uint8_t y_datatype = input.fields[y_idx].datatype;
+  uint8_t z_datatype = input.fields[z_idx].datatype;
+   
+  // Convert the fields to channels
+  int cur_c = 0;
+  for (size_t d = 0; d < input.fields.size (); ++d)
+  {
+    if ((int)input.fields[d].offset == x_offset || (int)input.fields[d].offset == y_offset || (int)input.fields[d].offset == z_offset)
+      continue;
+    output.channels[cur_c].name = input.fields[d].name;
+    output.channels[cur_c].values.resize (output.points.size ());
+    cur_c++;
+  }
+
+  // Copy the data points
+  for (size_t cp = 0; cp < output.points.size (); ++cp)
+  {
+    // Copy x/y/z
+    output.points[cp].x = sensor_msgs::readPointCloud2BufferValue<float>(&input.data[cp * input.point_step + x_offset], x_datatype);
+    output.points[cp].y = sensor_msgs::readPointCloud2BufferValue<float>(&input.data[cp * input.point_step + y_offset], y_datatype);
+    output.points[cp].z = sensor_msgs::readPointCloud2BufferValue<float>(&input.data[cp * input.point_step + z_offset], z_datatype);
+    // Copy the rest of the data
+    int cur_c = 0;
+    for (size_t d = 0; d < input.fields.size (); ++d)
+    {
+      if ((int)input.fields[d].offset == x_offset || (int)input.fields[d].offset == y_offset || (int)input.fields[d].offset == z_offset)
+        continue;
+      output.channels[cur_c++].values[cp] = sensor_msgs::readPointCloud2BufferValue<float>(&input.data[cp * input.point_step + input.fields[d].offset], input.fields[d].datatype);
+    }
+  }
+  return (true);
+}
+}
+#endif// SENSOR_MSGS_POINT_CLOUD_CONVERSION_H

sensor_msgs/include/sensor_msgs/point_field_conversion.h

@@ -0,0 +1,180 @@
+/*
+ * Software License Agreement (BSD License)
+ *
+ * Robot Operating System code by the University of Osnabrück
+ * Copyright (c) 2015, University of Osnabrück
+ * All rights reserved.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions
+ * are met:
+ *
+ *   1. Redistributions of source code must retain the above 
+ *      copyright notice, this list of conditions and the following
+ *      disclaimer.
+ *
+ *   2. Redistributions in binary form must reproduce the above 
+ *      copyright notice, this list of conditions and the following
+ *      disclaimer in the documentation and/or other materials provided
+ *      with the distribution.
+ *
+ *   3. Neither the name of the copyright holder nor the names of its
+ *      contributors may be used to endorse or promote products derived
+ *      from this software without specific prior written permission.
+ *
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED
+ * TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR
+ * PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR
+ * CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL,
+ * EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO,
+ * PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS;
+ * OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY,
+ * WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR
+ * OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF 
+ * ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
+ *
+ *
+ *
+ * point_field_conversion.h
+ *
+ * Created on: 16.07.2015
+ *  Authors: Sebastian Pütz <spuetz@uni-osnabrueck.de>
+ */
+
+#ifndef SENSOR_MSGS_POINT_FIELD_CONVERSION_H
+#define SENSOR_MSGS_POINT_FIELD_CONVERSION_H
+
+/** 
+  * \brief  This file provides a type to enum mapping for the different
+  *         PointField types and methods to read and write in 
+  *         a PointCloud2 buffer for the different PointField types.
+  * \author Sebastian Pütz
+  */
+namespace sensor_msgs{
+  /*!
+   * \Enum to type mapping.
+   */
+  template<int> struct pointFieldTypeAsType {};
+  template<> struct pointFieldTypeAsType<sensor_msgs::PointField::INT8>    { typedef int8_t   type; };
+  template<> struct pointFieldTypeAsType<sensor_msgs::PointField::UINT8>   { typedef uint8_t  type; };
+  template<> struct pointFieldTypeAsType<sensor_msgs::PointField::INT16>   { typedef int16_t  type; };
+  template<> struct pointFieldTypeAsType<sensor_msgs::PointField::UINT16>  { typedef uint16_t type; };
+  template<> struct pointFieldTypeAsType<sensor_msgs::PointField::INT32>   { typedef int32_t  type; };
+  template<> struct pointFieldTypeAsType<sensor_msgs::PointField::UINT32>  { typedef uint32_t type; };
+  template<> struct pointFieldTypeAsType<sensor_msgs::PointField::FLOAT32> { typedef float    type; };
+  template<> struct pointFieldTypeAsType<sensor_msgs::PointField::FLOAT64> { typedef double   type; };
+  
+  /*!
+   * \Type to enum mapping.
+   */
+  template<typename T> struct typeAsPointFieldType {};
+  template<> struct typeAsPointFieldType<int8_t>   { static const uint8_t value = sensor_msgs::PointField::INT8;    };
+  template<> struct typeAsPointFieldType<uint8_t>  { static const uint8_t value = sensor_msgs::PointField::UINT8;   };
+  template<> struct typeAsPointFieldType<int16_t>  { static const uint8_t value = sensor_msgs::PointField::INT16;   };
+  template<> struct typeAsPointFieldType<uint16_t> { static const uint8_t value = sensor_msgs::PointField::UINT16;  };
+  template<> struct typeAsPointFieldType<int32_t>  { static const uint8_t value = sensor_msgs::PointField::INT32;   };
+  template<> struct typeAsPointFieldType<uint32_t> { static const uint8_t value = sensor_msgs::PointField::UINT32;  };
+  template<> struct typeAsPointFieldType<float>    { static const uint8_t value = sensor_msgs::PointField::FLOAT32; };
+  template<> struct typeAsPointFieldType<double>   { static const uint8_t value = sensor_msgs::PointField::FLOAT64; };
+ 
+  /*!
+   * \Converts a value at the given pointer position, interpreted as the datatype
+   *  specified by the given template argument point_field_type, to the given
+   *  template type T and returns it.
+   * \param data_ptr            pointer into the point cloud 2 buffer
+   * \tparam point_field_type   sensor_msgs::PointField datatype value
+   * \tparam T                  return type
+   */
+  template<int point_field_type, typename T>
+    inline T readPointCloud2BufferValue(const unsigned char* data_ptr){
+      typedef typename pointFieldTypeAsType<point_field_type>::type type;
+      return static_cast<T>(*(reinterpret_cast<type const *>(data_ptr)));
+    }
+  
+  /*!
+   * \Converts a value at the given pointer position interpreted as the datatype
+   *  specified by the given datatype parameter to the given template type and returns it.
+   * \param data_ptr    pointer into the point cloud 2 buffer
+   * \param datatype    sensor_msgs::PointField datatype value
+   * \tparam T          return type
+   */
+  template<typename T>
+    inline T readPointCloud2BufferValue(const unsigned char* data_ptr, const unsigned char datatype){
+      switch(datatype){
+        case sensor_msgs::PointField::INT8:
+          return readPointCloud2BufferValue<sensor_msgs::PointField::INT8, T>(data_ptr);
+        case sensor_msgs::PointField::UINT8:
+          return readPointCloud2BufferValue<sensor_msgs::PointField::UINT8, T>(data_ptr);
+        case sensor_msgs::PointField::INT16:
+          return readPointCloud2BufferValue<sensor_msgs::PointField::INT16, T>(data_ptr);
+        case sensor_msgs::PointField::UINT16:
+          return readPointCloud2BufferValue<sensor_msgs::PointField::UINT16, T>(data_ptr);
+        case sensor_msgs::PointField::INT32:
+          return readPointCloud2BufferValue<sensor_msgs::PointField::INT32, T>(data_ptr);
+        case sensor_msgs::PointField::UINT32:
+          return readPointCloud2BufferValue<sensor_msgs::PointField::UINT32, T>(data_ptr);
+        case sensor_msgs::PointField::FLOAT32:
+          return readPointCloud2BufferValue<sensor_msgs::PointField::FLOAT32, T>(data_ptr);
+        case sensor_msgs::PointField::FLOAT64:
+          return readPointCloud2BufferValue<sensor_msgs::PointField::FLOAT64, T>(data_ptr);
+      }
+      // This should never be reached, but return statement added to avoid compiler warning. (#84)
+      return T();
+    }
+
+  /*!
+   * \Inserts a given value at the given point position interpreted as the datatype
+   *  specified by the template argument point_field_type.
+   * \param data_ptr            pointer into the point cloud 2 buffer
+   * \param value               the value to insert
+   * \tparam point_field_type   sensor_msgs::PointField datatype value
+   * \tparam T                  type of the value to insert
+   */
+  template<int point_field_type, typename T>
+    inline void writePointCloud2BufferValue(unsigned char* data_ptr, T value){
+      typedef typename pointFieldTypeAsType<point_field_type>::type type;
+      *(reinterpret_cast<type*>(data_ptr)) = static_cast<type>(value);
+    }
+
+  /*!
+   * \Inserts a given value at the given point position interpreted as the datatype
+   *  specified by the given datatype parameter.
+   * \param data_ptr    pointer into the point cloud 2 buffer
+   * \param datatype    sensor_msgs::PointField datatype value
+   * \param value       the value to insert
+   * \tparam T          type of the value to insert
+   */
+  template<typename T>
+    inline void writePointCloud2BufferValue(unsigned char* data_ptr, const unsigned char datatype, T value){
+      switch(datatype){
+        case sensor_msgs::PointField::INT8:
+          writePointCloud2BufferValue<sensor_msgs::PointField::INT8, T>(data_ptr, value);
+          break;
+        case sensor_msgs::PointField::UINT8:
+          writePointCloud2BufferValue<sensor_msgs::PointField::UINT8, T>(data_ptr, value);
+          break;
+        case sensor_msgs::PointField::INT16:
+          writePointCloud2BufferValue<sensor_msgs::PointField::INT16, T>(data_ptr, value);
+          break;
+        case sensor_msgs::PointField::UINT16:
+          writePointCloud2BufferValue<sensor_msgs::PointField::UINT16, T>(data_ptr, value);
+          break;
+        case sensor_msgs::PointField::INT32:
+          writePointCloud2BufferValue<sensor_msgs::PointField::INT32, T>(data_ptr, value);
+          break;
+        case sensor_msgs::PointField::UINT32:
+          writePointCloud2BufferValue<sensor_msgs::PointField::UINT32, T>(data_ptr, value);
+          break;
+        case sensor_msgs::PointField::FLOAT32:
+          writePointCloud2BufferValue<sensor_msgs::PointField::FLOAT32, T>(data_ptr, value);
+          break;
+        case sensor_msgs::PointField::FLOAT64:
+          writePointCloud2BufferValue<sensor_msgs::PointField::FLOAT64, T>(data_ptr, value);
+          break;
+      }
+    }
+}
+
+#endif /* point_field_conversion.h */