From 03e81fbb17fe1dc71371d643f67624cef25c4219 Mon Sep 17 00:00:00 2001 From: Daniel Maturana Date: Mon, 5 Mar 2018 02:35:28 -0500 Subject: [PATCH] pyrosmsg --- CMakeLists.txt | 27 ++--- include/{pymsg => pyrosmsg}/converters.hpp | 29 +++-- include/{pymsg => pyrosmsg}/serialization.hpp | 8 +- package.xml | 7 +- python/{pymsg => pyrosmsg}/__init__.py | 2 +- scratch/sanity.py | 20 ++-- scratch/serialization.py | 7 +- scratch/unicheck.py | 15 +++ setup.py | 3 +- src_wrap/module.cpp | 100 ++++++++++++------ 10 files changed, 129 insertions(+), 89 deletions(-) rename include/{pymsg => pyrosmsg}/converters.hpp (91%) rename include/{pymsg => pyrosmsg}/serialization.hpp (86%) rename python/{pymsg => pyrosmsg}/__init__.py (86%) create mode 100644 scratch/unicheck.py diff --git a/CMakeLists.txt b/CMakeLists.txt index a94b1ad..86f7f33 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -1,5 +1,5 @@ cmake_minimum_required(VERSION 2.8.3) -project(pymsg) +project(pyrosmsg) set(CMAKE_CXX_FLAGS "-std=c++11 ${CMAKE_CXX_FLAGS}") @@ -7,7 +7,7 @@ find_package(catkin REQUIRED COMPONENTS cmake_modules geometry_msgs nav_msgs - pybind11_module + pybind11_catkin roscpp rospy sensor_msgs @@ -16,10 +16,7 @@ find_package(catkin REQUIRED COMPONENTS ) find_package(NUMPY REQUIRED) -find_package(Eigen3 3.3.1 REQUIRED) - -set(PY_PROJECT_NAME ${PROJECT_NAME}) -set(PY_PACKAGE_DIR python/${PROJECT_NAME}) +find_package(pybind11 REQUIRED) catkin_python_setup() @@ -30,30 +27,22 @@ catkin_package( cmake_modules geometry_msgs nav_msgs - pybind11_module + pybind11_catkin roscpp rospy sensor_msgs std_msgs visualization_msgs -# DEPENDS system_lib + #DEPENDS system_lib ) -########### -## Build ## -########### - - include_directories( include - ${Eigen3_INCLUDE_DIR} ${catkin_INCLUDE_DIRS} + #${PYBIND11_INCLUDE_DIRS} ) -add_python_export_library(${PY_PROJECT_NAME} ${PY_PACKAGE_DIR} +message("pybind11_include_dir " ${PYBIND11_INCLUDE_DIRS}) +pybind_add_module(libpyrosmsg src_wrap/module.cpp ) - -target_link_libraries(${PY_PROJECT_NAME} - ${catkin_LIBRARIES} -) diff --git a/include/pymsg/converters.hpp b/include/pyrosmsg/converters.hpp similarity index 91% rename from include/pymsg/converters.hpp rename to include/pyrosmsg/converters.hpp index cebb21f..1d4975d 100644 --- a/include/pymsg/converters.hpp +++ b/include/pyrosmsg/converters.hpp @@ -80,7 +80,9 @@ template <> struct type_caster { object msg = MsgType(); msg.attr("seq") = pybind11::cast(header.seq); msg.attr("stamp") = pybind11::cast(header.stamp); - msg.attr("frame_id") = pybind11::cast(header.frame_id); + // 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; } @@ -196,7 +198,8 @@ template <> struct type_caster { 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::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; @@ -219,7 +222,10 @@ template <> struct type_caster { object mod = module::import("sensor_msgs.msg._PointField"); object MsgType = mod.attr("PointField"); object msg = MsgType(); - msg.attr("name") = pybind11::cast(cpp_msg.name); + // 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("offset") = pybind11::cast(cpp_msg.offset); msg.attr("datatype") = pybind11::cast(cpp_msg.datatype); msg.attr("count") = pybind11::cast(cpp_msg.count); @@ -268,11 +274,9 @@ template <> struct type_caster { 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); - std::string data_str(cpp_msg.data.begin(), cpp_msg.data.end()); - pybind11::bytes data_str2(data_str); - msg.attr("data") = data_str2; + //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; } @@ -289,9 +293,7 @@ template <> struct type_caster { value.encoding = src.attr("encoding").cast(); value.is_bigendian = src.attr("is_bigendian").cast(); value.step = src.attr("step").cast(); - std::string data_str = src.attr("data").cast(); - //value.data = std::vector(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; } @@ -306,12 +308,8 @@ 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); - - // TODO not sure if this works - pybind11::bytes data_str(reinterpret_cast(&cpp_msg.data[0]), cpp_msg.data.size()); - //std::string data_str(img.data.begin(), img.data.end()); - msg.attr("data") = data_str; - + //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; } @@ -363,6 +361,7 @@ template <> struct type_caster { object mod = module::import("sensor_msgs.msg._CameraInfo"); object MsgType = mod.attr("CameraInfo"); object msg = MsgType(); + msg.inc_ref(); return msg; } diff --git a/include/pymsg/serialization.hpp b/include/pyrosmsg/serialization.hpp similarity index 86% rename from include/pymsg/serialization.hpp rename to include/pyrosmsg/serialization.hpp index abc2d05..82b3ad5 100644 --- a/include/pymsg/serialization.hpp +++ b/include/pyrosmsg/serialization.hpp @@ -16,7 +16,7 @@ #include -namespace ca { namespace pymsg { +namespace pyrosmsg { // in python // buf = StringIO.StringIO() @@ -30,8 +30,8 @@ namespace ca { namespace pymsg { template std::string serialize(const MsgT& msg) { uint32_t serial_size = ros::serialization::serializationLength(msg); - // in practice we could directly use a string.c_str() but that is living dangerously - //boost::shared_array obuffer(new uint8_t[serial_size]); + // we could directly use a string.c_str(), but not sure if + // that could have ownership issues uint8_t* obuffer = new uint8_t[serial_size]; ros::serialization::OStream os(obuffer, serial_size); ros::serialization::serialize(os, msg); @@ -51,6 +51,6 @@ void deserialize(const std::string s, MsgT& msg) { delete[] ibuffer; } -} } +} #endif /* end of include guard: SERIALIZATION_H_QC30JWRG */ diff --git a/package.xml b/package.xml index 0bc29c1..b318adf 100644 --- a/package.xml +++ b/package.xml @@ -1,8 +1,8 @@ - pymsg + pyrosmsg 0.0.0 - The pymsg package + The pyrosmsg package dmaturan @@ -12,7 +12,8 @@ geometry_msgs nav_msgs - pybind11_module + + pybind11_catkin roscpp rospy sensor_msgs diff --git a/python/pymsg/__init__.py b/python/pyrosmsg/__init__.py similarity index 86% rename from python/pymsg/__init__.py rename to python/pyrosmsg/__init__.py index 9a8238c..562b86d 100755 --- a/python/pymsg/__init__.py +++ b/python/pyrosmsg/__init__.py @@ -8,4 +8,4 @@ # # @= -from libpymsg import * +from libpyrosmsg import * diff --git a/scratch/sanity.py b/scratch/sanity.py index ebc4157..567e3f6 100644 --- a/scratch/sanity.py +++ b/scratch/sanity.py @@ -8,22 +8,22 @@ import cv_bridge -import pymsg +import pyrosmsg import pypcd -#ts = pymsg.make_time() +#ts = pyrosmsg.make_time() #print ts # #tsp = rospy.Time() -#pymsg.print_time(tsp) +#pyrosmsg.print_time(tsp) # -#hdr = pymsg.make_header(32) +#hdr = pyrosmsg.make_header(32) #print hdr # -#pymsg.print_header_seq(hdr) +#pyrosmsg.print_header_seq(hdr) # #print -#cloud = pymsg.make_pc2(32) +#cloud = pyrosmsg.make_pc2(32) #print cloud # #print @@ -36,7 +36,7 @@ print 'pc_msg.width:', pc_msg.width -pymsg.print_centroid(pc_msg) +pyrosmsg.print_centroid(pc_msg) print pc_data.mean(0) # @@ -44,7 +44,7 @@ # #ci = CameraInfo() #print ci -#pymsg.print_cam_info(ci) +#pyrosmsg.print_cam_info(ci) # #print 'img' #print @@ -55,9 +55,9 @@ #img_msg = bridge.cv2_to_imgmsg(cv_img, encoding='passthrough') #print img_msg #print '---' -#pymsg.print_img(img_msg) +#pyrosmsg.print_img(img_msg) # #print '***' -#img_msg2 = pymsg.make_img(8, 8) +#img_msg2 = pyrosmsg.make_img(8, 8) #print img_msg2 #cv_img2 = bridge.imgmsg_to_cv2(img_msg2, desired_encoding='passthrough') diff --git a/scratch/serialization.py b/scratch/serialization.py index dd8f82d..576889d 100644 --- a/scratch/serialization.py +++ b/scratch/serialization.py @@ -1,7 +1,6 @@ import cStringIO as StringIO import numpy as np -from pymsg import libpymsg from sensor_msgs.msg import PointCloud2 @@ -10,7 +9,7 @@ import pypcd -import pymsg.libpymsg +import pyrosmsg pc = pypcd.PointCloud.from_path('./tmp.pcd') msg = pc.to_msg() @@ -20,11 +19,11 @@ def with_caster(m): # 38.4 us per loop, - pymsg.libpymsg.print_centroid(m) + pyrosmsg.print_centroid(m) def with_serial(m): # 117 us buf = StringIO.StringIO() m.serialize(buf) smsg = buf.getvalue() - pymsg.libpymsg.print_centroid2(smsg) + pyrosmsg.print_centroid2(smsg) diff --git a/scratch/unicheck.py b/scratch/unicheck.py new file mode 100644 index 0000000..e0a5a97 --- /dev/null +++ b/scratch/unicheck.py @@ -0,0 +1,15 @@ + +import copy +import numpy as np +import rospy +from std_msgs.msg import Header +from sensor_msgs.msg import CameraInfo +from sensor_msgs.msg import Image + +import cv_bridge + +import pymsg + +hdr = pyrosmsg.make_header(32, "header") +print hdr + diff --git a/setup.py b/setup.py index 20a8cf9..cefd95c 100644 --- a/setup.py +++ b/setup.py @@ -1,4 +1,3 @@ - ## ! DO NOT MANUALLY INVOKE THIS setup.py, USE CATKIN INSTEAD from distutils.core import setup @@ -6,7 +5,7 @@ # fetch values from package.xml setup_args = generate_distutils_setup( - packages=['pymsg'], + packages=['pyrosmsg'], package_dir={'':'python'}) setup(**setup_args) diff --git a/src_wrap/module.cpp b/src_wrap/module.cpp index 1daf10b..2258613 100644 --- a/src_wrap/module.cpp +++ b/src_wrap/module.cpp @@ -8,9 +8,15 @@ * **@=*/ +#include +#include -#include -#include +#include +#include + +namespace pyrosmsg { + +namespace py = pybind11; void print_cam_info(const sensor_msgs::CameraInfo& ci) { std::cout << "distortion model\n"; @@ -20,7 +26,6 @@ void print_cam_info(const sensor_msgs::CameraInfo& ci) { std::cout << "\n"; } -// just a sanity check void print_centroid(const sensor_msgs::PointCloud2& cloud) { double cx = 0., cy = 0., cz = 0.; for (size_t i=0; i < cloud.width; ++i) { @@ -37,9 +42,9 @@ void print_centroid(const sensor_msgs::PointCloud2& cloud) { } void print_centroid2(const std::string& smsg) { + // using generic serialization method sensor_msgs::PointCloud2 cloud; - ca::pymsg::deserialize(smsg, cloud); - + pyrosmsg::deserialize(smsg, cloud); double cx = 0., cy = 0., cz = 0.; for (size_t i=0; i < cloud.width; ++i) { @@ -55,7 +60,6 @@ void print_centroid2(const std::string& smsg) { std::cout << "centroid = [" << cx << " " << cy << " " << cz << "]" << std::endl; } - sensor_msgs::PointCloud2 make_pc2(int rows) { sensor_msgs::PointCloud2 pc; pc.width = rows; @@ -70,11 +74,46 @@ sensor_msgs::PointCloud2 make_pc2(int rows) { float data[rows]; 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; } +sensor_msgs::PointCloud2 make_pc2_from_numpy(py::array_t xyz) { + sensor_msgs::PointCloud2 pc; + + sensor_msgs::PointField pfx; + 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.offset = 4; + pfx.datatype = sensor_msgs::PointField::FLOAT32; + pfx.count = 1; + pc.fields.push_back(pfy); + + sensor_msgs::PointField pfz; + 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; + + // 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.width = xyz.shape(0); + pc.height = 1; + return pc; +} ros::Time make_time() { ros::Time ts; @@ -95,8 +134,9 @@ ros::Time increment_ts( const ros::Time& ts ) { return newts; } -std_msgs::Header make_header(int seq) { +std_msgs::Header make_header(int seq, std::string frame_id) { std_msgs::Header out; + out.frame_id = frame_id; out.seq = seq; return out; } @@ -130,27 +170,25 @@ sensor_msgs::Image make_img(int width, int height) { return msg; } +} -PYBIND11_MODULE(libpymsg, m) { - namespace py = pybind11; - /** - * note - this stuff does nothing. just sanity - * checks. all the important stuff is in - * the header. - */ - - //py::module m("libpymsg", "libpymsg plugin"); - m.doc() = "libpymsg module"; - - m.def("print_cam_info", &print_cam_info); - m.def("print_centroid", &print_centroid); - m.def("print_centroid2", &print_centroid2); - m.def("make_pc2", &make_pc2); - m.def("print_time", &print_time, "print time"); - m.def("make_time", &make_time, "make time"); - m.def("make_header", &make_header); - m.def("increment_ts", &increment_ts); - m.def("print_header_seq", &print_header_seq); - m.def("print_img", &print_img); - m.def("make_img", &make_img); +PYBIND11_MODULE(libpyrosmsg, m) { + // note - these methods are just sanity + // checks. all the important stuff is in + // the header. + + m.doc() = "libpyrosmsg module"; + + m.def("print_cam_info", &pyrosmsg::print_cam_info); + m.def("print_centroid", &pyrosmsg::print_centroid); + m.def("print_centroid2", &pyrosmsg::print_centroid2); + m.def("make_pc2", &pyrosmsg::make_pc2); + m.def("make_pc2_from_numpy", &pyrosmsg::make_pc2_from_numpy); + m.def("print_time", &pyrosmsg::print_time, "print time"); + m.def("make_time", &pyrosmsg::make_time, "make time"); + m.def("make_header", &pyrosmsg::make_header); + m.def("increment_ts", &pyrosmsg::increment_ts); + m.def("print_header_seq", &pyrosmsg::print_header_seq); + m.def("print_img", &pyrosmsg::print_img); + m.def("make_img", &pyrosmsg::make_img); }