diff --git a/include/pyrosmsg/converters.hpp b/include/pyrosmsg/converters.hpp index 7ba1b46..7a42546 100644 --- a/include/pyrosmsg/converters.hpp +++ b/include/pyrosmsg/converters.hpp @@ -5,100 +5,121 @@ #include +#include #include #include -#include -#include -#include -#include -#include -#include #include -#include +#include #include #include +#include +#include +#include +#include +#include - -static inline -bool is_ros_msg_type(pybind11::handle src, const std::string& msg_type_name) { +static inline bool is_ros_msg_type(pybind11::handle src, + const std::string &msg_type_name) { namespace py = pybind11; - if (!py::hasattr(src, "_type")) { return false; } + if (!py::hasattr(src, "_type")) { + return false; + } std::string msg_type(src.attr("_type").cast()); - if (msg_type != msg_type_name) { return false; } + if (msg_type != msg_type_name) { + return false; + } return true; } -namespace pybind11 { namespace detail { - -template <> struct type_caster { - public: - PYBIND11_TYPE_CASTER(ros::Time, _("ros::Time")); - - // python -> cpp - bool load(handle src, bool) { - PyObject *obj(src.ptr()); - if (!PyObject_HasAttrString(obj, "secs")) { return false; } - if (!PyObject_HasAttrString(obj, "nsecs")) { return false; } - - value.sec = (src.attr("secs")).cast(); - value.nsec = (src.attr("nsecs")).cast(); - return true; - } - - // cpp -> python - static handle cast(ros::Time src, return_value_policy policy, handle parent) { - object rospy = module::import("rospy"); - object TimeType = rospy.attr("Time"); - object pyts = TimeType(); - pyts.attr("secs") = pybind11::cast(src.sec); - pyts.attr("nsecs") = pybind11::cast(src.nsec); - pyts.inc_ref(); - return pyts; - } +namespace pybind11 { +namespace detail { + +template <> +struct type_caster { + public: + PYBIND11_TYPE_CASTER(ros::Time, _("ros::Time")); + + // python -> cpp + bool load(handle src, bool) { + PyObject *obj(src.ptr()); + if (!PyObject_HasAttrString(obj, "secs")) { + return false; + } + if (!PyObject_HasAttrString(obj, "nsecs")) { + return false; + } + + value.sec = (src.attr("secs")).cast(); + value.nsec = (src.attr("nsecs")).cast(); + return true; + } + + // cpp -> python + static handle cast(ros::Time src, return_value_policy policy, handle parent) { + object rospy = module::import("rospy"); + object TimeType = rospy.attr("Time"); + object pyts = TimeType(); + pyts.attr("secs") = pybind11::cast(src.sec); + pyts.attr("nsecs") = pybind11::cast(src.nsec); + pyts.inc_ref(); + return pyts; + } }; -template <> struct type_caster { - public: - PYBIND11_TYPE_CASTER(std_msgs::Header, _("std_msgs::Header")); - - // python -> cpp - bool load(handle src, bool) { - if (!is_ros_msg_type(src, "std_msgs/Header")) { return false; } - value.seq = src.attr("seq").cast(); - value.stamp = src.attr("stamp").cast(); - value.frame_id = src.attr("frame_id").cast(); - return true; - } - - // cpp -> python - static handle cast(std_msgs::Header header, return_value_policy policy, handle parent) { - object mod = module::import("std_msgs.msg._Header"); - object MsgType = mod.attr("Header"); - object msg = MsgType(); - msg.attr("seq") = pybind11::cast(header.seq); - msg.attr("stamp") = pybind11::cast(header.stamp); - // avoid !!python/unicode problem. - //msg.attr("frame_id") = pybind11::cast(header.frame_id); - msg.attr("frame_id") = pybind11::bytes(reinterpret_cast(&header.frame_id[0]), header.frame_id.size()); - msg.inc_ref(); - return msg; - } +template <> +struct type_caster { + public: + PYBIND11_TYPE_CASTER(std_msgs::Header, _("std_msgs::Header")); + + // python -> cpp + bool load(handle src, bool) { + if (!is_ros_msg_type(src, "std_msgs/Header")) { + return false; + } + value.seq = src.attr("seq").cast(); + value.stamp = src.attr("stamp").cast(); + value.frame_id = src.attr("frame_id").cast(); + return true; + } + + // cpp -> python + static handle cast(std_msgs::Header header, + return_value_policy policy, + handle parent) { + object mod = module::import("std_msgs.msg._Header"); + object MsgType = mod.attr("Header"); + object msg = MsgType(); + msg.attr("seq") = pybind11::cast(header.seq); + msg.attr("stamp") = pybind11::cast(header.stamp); + // avoid !!python/unicode problem. + // msg.attr("frame_id") = pybind11::cast(header.frame_id); + msg.attr("frame_id") = + pybind11::bytes(reinterpret_cast(&header.frame_id[0]), + header.frame_id.size()); + msg.inc_ref(); + return msg; + } }; -template <> struct type_caster { - public: - PYBIND11_TYPE_CASTER(geometry_msgs::Point, _("geometry_msgs::Point")); +template <> +struct type_caster { + public: + PYBIND11_TYPE_CASTER(geometry_msgs::Point, _("geometry_msgs::Point")); - bool load(handle src, bool) { - if (!is_ros_msg_type(src, "geometry_msgs/Point")) { return false; } + bool load(handle src, bool) { + if (!is_ros_msg_type(src, "geometry_msgs/Point")) { + return false; + } value.x = (src.attr("x")).cast(); value.y = (src.attr("y")).cast(); value.z = (src.attr("z")).cast(); return true; } - static handle cast(geometry_msgs::Point pt, return_value_policy policy, handle parent) { + static handle cast(geometry_msgs::Point pt, + return_value_policy policy, + handle parent) { object mod = module::import("geometry_msgs.msg._Point"); object MsgType = mod.attr("Point"); object msg = MsgType(); @@ -108,21 +129,25 @@ template <> struct type_caster { msg.inc_ref(); return msg; } - }; -template <> struct type_caster { - public: +template <> +struct type_caster { + public: PYBIND11_TYPE_CASTER(geometry_msgs::Vector3, _("geometry_msgs::Vector3")); bool load(handle src, bool) { - if (!is_ros_msg_type(src, "geometry_msgs/Vector3")) { return false; } + if (!is_ros_msg_type(src, "geometry_msgs/Vector3")) { + return false; + } value.x = (src.attr("x")).cast(); value.y = (src.attr("y")).cast(); value.z = (src.attr("z")).cast(); return true; } - static handle cast(geometry_msgs::Vector3 cpp_msg, return_value_policy policy, handle parent) { + static handle cast(geometry_msgs::Vector3 cpp_msg, + return_value_policy policy, + handle parent) { object mod = module::import("geometry_msgs.msg._Vector3"); object MsgType = mod.attr("Vector3"); object msg = MsgType(); @@ -134,11 +159,15 @@ template <> struct type_caster { } }; -template <> struct type_caster { - public: - PYBIND11_TYPE_CASTER(geometry_msgs::Quaternion, _("geometry_msgs::Quaternion")); +template <> +struct type_caster { + public: + PYBIND11_TYPE_CASTER(geometry_msgs::Quaternion, + _("geometry_msgs::Quaternion")); bool load(handle src, bool) { - if (!is_ros_msg_type(src, "geometry_msgs/Quaternion")) { return false; } + if (!is_ros_msg_type(src, "geometry_msgs/Quaternion")) { + return false; + } value.x = (src.attr("x")).cast(); value.y = (src.attr("y")).cast(); value.z = (src.attr("z")).cast(); @@ -146,7 +175,9 @@ template <> struct type_caster { return true; } - static handle cast(geometry_msgs::Quaternion cpp_msg, return_value_policy policy, handle parent) { + static handle cast(geometry_msgs::Quaternion cpp_msg, + return_value_policy policy, + handle parent) { object mod = module::import("geometry_msgs.msg._Quaternion"); object MsgType = mod.attr("Quaternion"); object msg = MsgType(); @@ -159,17 +190,23 @@ template <> struct type_caster { } }; -template <> struct type_caster { - public: +template <> +struct type_caster { + public: PYBIND11_TYPE_CASTER(geometry_msgs::Transform, _("geometry_msgs::Transform")); bool load(handle src, bool) { - if (!is_ros_msg_type(src, "geometry_msgs/Transform")) { return false; } - value.translation = (src.attr("translation")).cast(); + if (!is_ros_msg_type(src, "geometry_msgs/Transform")) { + return false; + } + value.translation = + (src.attr("translation")).cast(); value.rotation = (src.attr("rotation")).cast(); return true; } - static handle cast(geometry_msgs::Transform cpp_msg, return_value_policy policy, handle parent) { + static handle cast(geometry_msgs::Transform cpp_msg, + return_value_policy policy, + handle parent) { object mod = module::import("geometry_msgs.msg._Transform"); object MsgType = mod.attr("Transform"); object msg = MsgType(); @@ -180,35 +217,46 @@ template <> struct type_caster { } }; -template <> struct type_caster { - public: - PYBIND11_TYPE_CASTER(geometry_msgs::TransformStamped, _("geometry_msgs::TransformStamped")); +template <> +struct type_caster { + public: + PYBIND11_TYPE_CASTER(geometry_msgs::TransformStamped, + _("geometry_msgs::TransformStamped")); bool load(handle src, bool) { - if (!is_ros_msg_type(src, "geometry_msgs/TransformStamped")) { return false; } + if (!is_ros_msg_type(src, "geometry_msgs/TransformStamped")) { + return false; + } value.header = src.attr("header").cast(); value.child_frame_id = src.attr("child_frame_id").cast(); value.transform = src.attr("transform").cast(); return true; } - static handle cast(geometry_msgs::TransformStamped cpp_msg, return_value_policy policy, handle parent) { + static handle cast(geometry_msgs::TransformStamped cpp_msg, + return_value_policy policy, + handle parent) { object mod = module::import("geometry_msgs.msg._TransformStamped"); object MsgType = mod.attr("TransformStamped"); object msg = MsgType(); msg.attr("header") = pybind11::cast(cpp_msg.header); - //msg.attr("child_frame_id") = pybind11::cast(cpp_msg.child_frame_id); - msg.attr("child_frame_id") = pybind11::bytes(reinterpret_cast(&cpp_msg.child_frame_id[0]), cpp_msg.child_frame_id.size()); + // msg.attr("child_frame_id") = pybind11::cast(cpp_msg.child_frame_id); + msg.attr("child_frame_id") = pybind11::bytes( + reinterpret_cast(&cpp_msg.child_frame_id[0]), + cpp_msg.child_frame_id.size()); msg.attr("transform") = pybind11::cast(cpp_msg.transform); msg.inc_ref(); return msg; } }; -template <> struct type_caster { - public: +template <> +struct type_caster { + public: PYBIND11_TYPE_CASTER(sensor_msgs::PointField, _("sensor_msgs::PointField")); bool load(handle src, bool) { - if (!is_ros_msg_type(src, "sensor_msgs/PointField")) { return false; } + if (!is_ros_msg_type(src, "sensor_msgs/PointField")) { + return false; + } value.name = (src.attr("name")).cast(); value.offset = (src.attr("offset")).cast(); value.datatype = (src.attr("datatype")).cast(); @@ -216,14 +264,17 @@ template <> struct type_caster { return true; } - static handle cast(sensor_msgs::PointField cpp_msg, return_value_policy policy, handle parent) { + static handle cast(sensor_msgs::PointField cpp_msg, + return_value_policy policy, + handle parent) { object mod = module::import("sensor_msgs.msg._PointField"); object MsgType = mod.attr("PointField"); object msg = MsgType(); // avoid !!python/unicode problem. - //msg.attr("name") = pybind11::cast(cpp_msg.name); - //msg.attr("name") = PyString_FromString(cpp_msg.name.c_str()); - msg.attr("name") = pybind11::bytes(reinterpret_cast(&cpp_msg.name[0]), cpp_msg.name.size()); + // msg.attr("name") = pybind11::cast(cpp_msg.name); + // msg.attr("name") = PyString_FromString(cpp_msg.name.c_str()); + msg.attr("name") = pybind11::bytes( + reinterpret_cast(&cpp_msg.name[0]), cpp_msg.name.size()); msg.attr("offset") = pybind11::cast(cpp_msg.offset); msg.attr("datatype") = pybind11::cast(cpp_msg.datatype); msg.attr("count") = pybind11::cast(cpp_msg.count); @@ -232,59 +283,72 @@ template <> struct type_caster { } }; -template <> struct type_caster { - public: +template <> +struct type_caster { + public: PYBIND11_TYPE_CASTER(sensor_msgs::PointCloud2, _("sensor_msgs::PointCloud2")); bool load(handle src, bool) { - if (!is_ros_msg_type(src, "sensor_msgs/PointCloud2")) { return false; } + if (!is_ros_msg_type(src, "sensor_msgs/PointCloud2")) { + return false; + } value.header = (src.attr("header")).cast(); value.height = (src.attr("height")).cast(); value.width = (src.attr("width")).cast(); pybind11::list field_lst = (src.attr("fields")).cast(); - for (int i=0; i < pybind11::len(field_lst); ++i) { - sensor_msgs::PointField pf((field_lst[i]).cast()); + for (int i = 0; i < pybind11::len(field_lst); ++i) { + sensor_msgs::PointField pf( + (field_lst[i]).cast()); value.fields.push_back(pf); } value.is_bigendian = (src.attr("is_bigendian")).cast(); value.point_step = (src.attr("point_step")).cast(); value.row_step = (src.attr("row_step")).cast(); std::string data_str = (src.attr("data")).cast(); - value.data.insert(value.data.end(), data_str.c_str(), data_str.c_str()+data_str.length()); + value.data.insert(value.data.end(), + data_str.c_str(), + data_str.c_str() + data_str.length()); value.is_dense = (src.attr("is_dense")).cast(); return true; } - static handle cast(sensor_msgs::PointCloud2 cpp_msg, return_value_policy policy, handle parent) { + static handle cast(sensor_msgs::PointCloud2 cpp_msg, + return_value_policy policy, + handle parent) { object mod = module::import("sensor_msgs.msg._PointCloud2"); object MsgType = mod.attr("PointCloud2"); object msg = MsgType(); msg.attr("header") = pybind11::cast(cpp_msg.header); msg.attr("height") = pybind11::cast(cpp_msg.height); msg.attr("width") = pybind11::cast(cpp_msg.width); - //msg.attr("fields") = pybind11::cast(cpp_msg.fields); - //pybind11::list field_lst = (msg.attr("fields")).cast(); + // msg.attr("fields") = pybind11::cast(cpp_msg.fields); + // pybind11::list field_lst = (msg.attr("fields")).cast(); pybind11::list field_lst; - for (size_t i=0; i < cpp_msg.fields.size(); ++i) { - const sensor_msgs::PointField& pf(cpp_msg.fields[i]); + for (size_t i = 0; i < cpp_msg.fields.size(); ++i) { + const sensor_msgs::PointField &pf(cpp_msg.fields[i]); field_lst.append(pybind11::cast(pf)); } msg.attr("fields") = field_lst; msg.attr("is_bigendian") = pybind11::cast(cpp_msg.is_bigendian); msg.attr("point_step") = pybind11::cast(cpp_msg.point_step); msg.attr("row_step") = pybind11::cast(cpp_msg.row_step); - //msg.attr("data") = pybind11::bytes(std::string(cpp_msg.data.begin(), cpp_msg.data.end())); - msg.attr("data") = pybind11::bytes(reinterpret_cast(&cpp_msg.data[0]), cpp_msg.data.size()); + // msg.attr("data") = pybind11::bytes(std::string(cpp_msg.data.begin(), + // cpp_msg.data.end())); + msg.attr("data") = pybind11::bytes( + reinterpret_cast(&cpp_msg.data[0]), cpp_msg.data.size()); msg.attr("is_dense") = pybind11::cast(cpp_msg.is_dense); msg.inc_ref(); return msg; } }; -template <> struct type_caster { - public: +template <> +struct type_caster { + public: PYBIND11_TYPE_CASTER(sensor_msgs::Image, _("sensor_msgs::Image")); bool load(handle src, bool) { - if (!is_ros_msg_type(src, "sensor_msgs/Image")) { return false; } + if (!is_ros_msg_type(src, "sensor_msgs/Image")) { + return false; + } value.header = src.attr("header").cast(); value.height = src.attr("height").cast(); value.width = src.attr("width").cast(); @@ -292,11 +356,15 @@ template <> struct type_caster { value.is_bigendian = src.attr("is_bigendian").cast(); value.step = src.attr("step").cast(); std::string data_str = src.attr("data").cast(); - value.data.insert(value.data.end(), data_str.c_str(), data_str.c_str()+data_str.length()); + value.data.insert(value.data.end(), + data_str.c_str(), + data_str.c_str() + data_str.length()); return true; } - static handle cast(sensor_msgs::Image cpp_msg, return_value_policy policy, handle parent) { + static handle cast(sensor_msgs::Image cpp_msg, + return_value_policy policy, + handle parent) { object mod = module::import("sensor_msgs.msg._Image"); object MsgType = mod.attr("Image"); object msg = MsgType(); @@ -306,18 +374,23 @@ template <> struct type_caster { msg.attr("encoding") = pybind11::bytes(cpp_msg.encoding); msg.attr("is_bigendian") = pybind11::cast(cpp_msg.is_bigendian); msg.attr("step") = pybind11::cast(cpp_msg.step); - //msg.attr("data") = pybind11::bytes(std::string(cpp_msg.data.begin(), cpp_msg.data.end())); - msg.attr("data") = pybind11::bytes(reinterpret_cast(&cpp_msg.data[0]), cpp_msg.data.size()); + // msg.attr("data") = pybind11::bytes(std::string(cpp_msg.data.begin(), + // cpp_msg.data.end())); + msg.attr("data") = pybind11::bytes( + reinterpret_cast(&cpp_msg.data[0]), cpp_msg.data.size()); msg.inc_ref(); return msg; } }; -template <> struct type_caster { - public: +template <> +struct type_caster { + public: PYBIND11_TYPE_CASTER(sensor_msgs::CameraInfo, _("sensor_msgs::CameraInfo")); bool load(handle src, bool) { - if (!is_ros_msg_type(src, "sensor_msgs/CameraInfo")) { return false; } + if (!is_ros_msg_type(src, "sensor_msgs/CameraInfo")) { + return false; + } value.height = src.attr("height").cast(); value.width = src.attr("width").cast(); @@ -329,21 +402,21 @@ template <> struct type_caster { } } { - int i=0; + int i = 0; for (auto item : src.attr("K")) { value.K[i] = item.cast(); ++i; } } { - int i=0; + int i = 0; for (auto item : src.attr("R")) { value.R[i] = item.cast(); ++i; } } { - int i=0; + int i = 0; for (auto item : src.attr("P")) { value.P[i] = item.cast(); ++i; @@ -355,7 +428,9 @@ template <> struct type_caster { return true; } - static handle cast(sensor_msgs::CameraInfo cpp_msg, return_value_policy policy, handle parent) { + static handle cast(sensor_msgs::CameraInfo cpp_msg, + return_value_policy policy, + handle parent) { object mod = module::import("sensor_msgs.msg._CameraInfo"); object MsgType = mod.attr("CameraInfo"); object msg = MsgType(); @@ -364,7 +439,7 @@ template <> struct type_caster { return msg; } }; - -} } +} +} #endif /* end of include guard: CONVERTERS_H_L7OWNAZ8 */ diff --git a/include/pyrosmsg/serialization.hpp b/include/pyrosmsg/serialization.hpp index 82b3ad5..1064a27 100644 --- a/include/pyrosmsg/serialization.hpp +++ b/include/pyrosmsg/serialization.hpp @@ -8,7 +8,6 @@ * **@=*/ - #ifndef SERIALIZATION_H_QC30JWRG #define SERIALIZATION_H_QC30JWRG @@ -45,12 +44,11 @@ template void deserialize(const std::string s, MsgT& msg) { uint32_t serial_size = s.length(); uint8_t* ibuffer = new uint8_t[serial_size](); - std::copy(s.c_str(), s.c_str()+s.length(), &ibuffer[0]); + std::copy(s.c_str(), s.c_str() + s.length(), &ibuffer[0]); ros::serialization::IStream istream(&ibuffer[0], serial_size); ros::serialization::deserialize(istream, msg); delete[] ibuffer; } - } #endif /* end of include guard: SERIALIZATION_H_QC30JWRG */ diff --git a/src_wrap/module.cpp b/src_wrap/module.cpp index 2258613..9785090 100644 --- a/src_wrap/module.cpp +++ b/src_wrap/module.cpp @@ -8,8 +8,8 @@ * **@=*/ -#include #include +#include #include #include @@ -28,17 +28,23 @@ void print_cam_info(const sensor_msgs::CameraInfo& ci) { void print_centroid(const sensor_msgs::PointCloud2& cloud) { double cx = 0., cy = 0., cz = 0.; - for (size_t i=0; i < cloud.width; ++i) { - float x = *reinterpret_cast(&cloud.data[i*cloud.point_step]); - float y = *reinterpret_cast(&cloud.data[i*cloud.point_step + sizeof(float)]); - float z = *reinterpret_cast(&cloud.data[i*cloud.point_step + 2*sizeof(float)]); - cx += x; cy += y; cz += z; + for (size_t i = 0; i < cloud.width; ++i) { + float x = + *reinterpret_cast(&cloud.data[i * cloud.point_step]); + float y = *reinterpret_cast( + &cloud.data[i * cloud.point_step + sizeof(float)]); + float z = *reinterpret_cast( + &cloud.data[i * cloud.point_step + 2 * sizeof(float)]); + cx += x; + cy += y; + cz += z; } std::cerr << "cloud.width = " << cloud.width << std::endl; cx /= cloud.width; cy /= cloud.width; cz /= cloud.width; - std::cout << "centroid = [" << cx << " " << cy << " " << cz << "]" << std::endl; + std::cout << "centroid = [" << cx << " " << cy << " " << cz << "]" + << std::endl; } void print_centroid2(const std::string& smsg) { @@ -47,17 +53,23 @@ void print_centroid2(const std::string& smsg) { pyrosmsg::deserialize(smsg, cloud); double cx = 0., cy = 0., cz = 0.; - for (size_t i=0; i < cloud.width; ++i) { - float x = *reinterpret_cast(&cloud.data[i*cloud.point_step]); - float y = *reinterpret_cast(&cloud.data[i*cloud.point_step + sizeof(float)]); - float z = *reinterpret_cast(&cloud.data[i*cloud.point_step + 2*sizeof(float)]); - cx += x; cy += y; cz += z; + for (size_t i = 0; i < cloud.width; ++i) { + float x = + *reinterpret_cast(&cloud.data[i * cloud.point_step]); + float y = *reinterpret_cast( + &cloud.data[i * cloud.point_step + sizeof(float)]); + float z = *reinterpret_cast( + &cloud.data[i * cloud.point_step + 2 * sizeof(float)]); + cx += x; + cy += y; + cz += z; } std::cerr << "cloud.width = " << cloud.width << std::endl; cx /= cloud.width; cy /= cloud.width; cz /= cloud.width; - std::cout << "centroid = [" << cx << " " << cy << " " << cz << "]" << std::endl; + std::cout << "centroid = [" << cx << " " << cy << " " << cz << "]" + << std::endl; } sensor_msgs::PointCloud2 make_pc2(int rows) { @@ -65,17 +77,19 @@ sensor_msgs::PointCloud2 make_pc2(int rows) { pc.width = rows; pc.height = 1; sensor_msgs::PointField pfx; - pfx.name = "x"; + pfx.name = "x"; pfx.offset = 0; pfx.datatype = sensor_msgs::PointField::FLOAT32; pfx.count = 1; pc.fields.push_back(pfx); pc.point_step = sizeof(float); float data[rows]; - for (int i=0; i < rows; ++i) { data[i] = 28.0; } + for (int i = 0; i < rows; ++i) { + data[i] = 28.0; + } pc.data.insert(pc.data.end(), - reinterpret_cast(data), - reinterpret_cast(data+rows)); + reinterpret_cast(data), + reinterpret_cast(data + rows)); return pc; } @@ -83,33 +97,34 @@ sensor_msgs::PointCloud2 make_pc2_from_numpy(py::array_t xyz) { sensor_msgs::PointCloud2 pc; sensor_msgs::PointField pfx; - pfx.name = "x"; + pfx.name = "x"; pfx.offset = 0; pfx.datatype = sensor_msgs::PointField::FLOAT32; pfx.count = 1; pc.fields.push_back(pfx); sensor_msgs::PointField pfy; - pfx.name = "y"; + pfx.name = "y"; pfx.offset = 4; pfx.datatype = sensor_msgs::PointField::FLOAT32; pfx.count = 1; pc.fields.push_back(pfy); sensor_msgs::PointField pfz; - pfx.name = "z"; + pfx.name = "z"; pfx.offset = 4; pfx.datatype = sensor_msgs::PointField::FLOAT32; pfx.count = 1; pc.fields.push_back(pfz); - pc.point_step = sizeof(float)*3; + pc.point_step = sizeof(float) * 3; // making assumptions on c-style numpy! auto xyz_buf = xyz.unchecked(); - pc.data.insert(pc.data.end(), - reinterpret_cast(xyz_buf.data(0, 0)), - reinterpret_cast(xyz_buf.data(0, 0)+xyz_buf.nbytes())); + pc.data.insert( + pc.data.end(), + reinterpret_cast(xyz_buf.data(0, 0)), + reinterpret_cast(xyz_buf.data(0, 0) + xyz_buf.nbytes())); pc.width = xyz.shape(0); pc.height = 1; return pc; @@ -127,10 +142,11 @@ void print_time(const ros::Time& ts) { std::cerr << "ts.nsec = " << ts.nsec << std::endl; } -ros::Time increment_ts( const ros::Time& ts ) { +ros::Time increment_ts(const ros::Time& ts) { std::cerr << "ts.sec = " << ts.sec << ", ts.nsec = " << ts.nsec << std::endl; ros::Time newts(ts.sec + 1, ts.nsec + 1); - std::cerr << "newts.sec = " << newts.sec << ", newts.nsec = " << newts.nsec << std::endl; + std::cerr << "newts.sec = " << newts.sec << ", newts.nsec = " << newts.nsec + << std::endl; return newts; } @@ -150,7 +166,7 @@ void print_img(const sensor_msgs::Image& img) { std::cerr << "img.height = " << img.height << std::endl; std::cerr << "img.step = " << img.step << std::endl; std::cerr << "["; - for (int i=0; i < img.width*img.height; ++i) { + for (int i = 0; i < img.width * img.height; ++i) { std::cerr << (int)img.data[i] << ", "; } std::cerr << "]\n"; @@ -162,14 +178,13 @@ sensor_msgs::Image make_img(int width, int height) { msg.height = height; msg.encoding = "8UC1"; msg.step = width; - for (int i=0; i < height; ++i) { - for (int j=0; j < width; ++j) { - msg.data.push_back(width*i + j); + for (int i = 0; i < height; ++i) { + for (int j = 0; j < width; ++j) { + msg.data.push_back(width * i + j); } } return msg; } - } PYBIND11_MODULE(libpyrosmsg, m) {