├── .gitignore ├── CMakeLists.txt ├── LICENSE ├── include └── pyrosmsg │ ├── converters.hpp │ ├── pyrosmsg.h │ └── serialization.hpp ├── package.xml ├── pyrosmsg └── __init__.py ├── readme.md ├── scratch ├── sanity.py ├── serialization.py └── unicheck.py ├── setup.py └── src_wrap └── module.cpp /.gitignore: -------------------------------------------------------------------------------- 1 | pyrosmsg/__pycache__/ 2 | -------------------------------------------------------------------------------- /CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 2.8.3) 2 | project(pyrosmsg) 3 | 4 | set(CMAKE_CXX_FLAGS "-std=c++11 ${CMAKE_CXX_FLAGS}") 5 | 6 | find_package(catkin REQUIRED COMPONENTS 7 | cmake_modules 8 | geometry_msgs 9 | nav_msgs 10 | pybind11 11 | pybind11_catkin 12 | roscpp 13 | rospy 14 | sensor_msgs 15 | std_msgs 16 | visualization_msgs 17 | ) 18 | 19 | find_package(NUMPY REQUIRED) 20 | #find_package(pybind11 REQUIRED) 21 | 22 | catkin_python_setup() 23 | 24 | catkin_package( 25 | INCLUDE_DIRS include 26 | LIBRARIES 27 | CATKIN_DEPENDS 28 | cmake_modules 29 | geometry_msgs 30 | nav_msgs 31 | pybind11_catkin 32 | roscpp 33 | rospy 34 | sensor_msgs 35 | std_msgs 36 | visualization_msgs 37 | #DEPENDS system_lib 38 | ) 39 | 40 | include_directories( 41 | include 42 | ${catkin_INCLUDE_DIRS} 43 | #${PYBIND11_INCLUDE_DIRS} 44 | ) 45 | 46 | #message("pybind11_include_dir " ${PYBIND11_INCLUDE_DIRS}) 47 | pybind_add_module(libpyrosmsg 48 | src_wrap/module.cpp 49 | ) 50 | -------------------------------------------------------------------------------- /LICENSE: -------------------------------------------------------------------------------- 1 | [This software is BSD licensed.](http://opensource.org/licenses/BSD-3-Clause) 2 | 3 | Copyright (c) 2015-2019, Carnegie Mellon University 4 | All rights reserved. 5 | 6 | Redistribution and use in source and binary forms, with or without 7 | modification, are permitted provided that the following conditions are met: 8 | 9 | 1. Redistributions of source code must retain the above copyright notice, this 10 | list of conditions and the following disclaimer. 11 | 12 | 2. Redistributions in binary form must reproduce the above copyright notice, 13 | this list of conditions and the following disclaimer in the documentation 14 | and/or other materials provided with the distribution. 15 | 16 | 3. Neither the name of the copyright holder nor the names of its contributors 17 | may be used to endorse or promote products derived from this software 18 | without specific prior written permission. 19 | 20 | THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND 21 | ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED 22 | WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE 23 | DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE 24 | FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL 25 | DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR 26 | SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER 27 | CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, 28 | OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE 29 | OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. 30 | -------------------------------------------------------------------------------- /include/pyrosmsg/converters.hpp: -------------------------------------------------------------------------------- 1 | #ifndef CONVERTERS_H_L7OWNAZ8 2 | #define CONVERTERS_H_L7OWNAZ8 3 | 4 | // TODO 5 | // should break this up by package, 6 | // or even msg type -- otherwise this file will grow 7 | // unwieldy, and also might affect compilation times 8 | 9 | #include 10 | 11 | #include 12 | 13 | #include 14 | #include 15 | #include 16 | 17 | #include 18 | #include 19 | #include 20 | #include 21 | #include 22 | #include 23 | #include 24 | #include 25 | #include 26 | #include 27 | #include 28 | #include 29 | 30 | static inline bool is_ros_msg_type(pybind11::handle src, 31 | const std::string &msg_type_name) { 32 | namespace py = pybind11; 33 | if (!py::hasattr(src, "_type")) { 34 | return false; 35 | } 36 | std::string msg_type(src.attr("_type").cast()); 37 | if (msg_type != msg_type_name) { 38 | return false; 39 | } 40 | return true; 41 | } 42 | 43 | namespace pybind11 { 44 | namespace detail { 45 | 46 | template <> 47 | struct type_caster { 48 | public: 49 | PYBIND11_TYPE_CASTER(ros::Time, _("ros::Time")); 50 | 51 | // python -> cpp 52 | bool load(handle src, bool) { 53 | PyObject *obj(src.ptr()); 54 | if (!PyObject_HasAttrString(obj, "secs")) { 55 | return false; 56 | } 57 | if (!PyObject_HasAttrString(obj, "nsecs")) { 58 | return false; 59 | } 60 | 61 | value.sec = (src.attr("secs")).cast(); 62 | value.nsec = (src.attr("nsecs")).cast(); 63 | return true; 64 | } 65 | 66 | // cpp -> python 67 | static handle cast(ros::Time src, return_value_policy policy, handle parent) { 68 | object rospy = module::import("rospy"); 69 | object TimeType = rospy.attr("Time"); 70 | object pyts = TimeType(); 71 | pyts.attr("secs") = pybind11::cast(src.sec); 72 | pyts.attr("nsecs") = pybind11::cast(src.nsec); 73 | pyts.inc_ref(); 74 | return pyts; 75 | } 76 | }; 77 | 78 | template <> 79 | struct type_caster { 80 | public: 81 | PYBIND11_TYPE_CASTER(std_msgs::Header, _("std_msgs::Header")); 82 | 83 | // python -> cpp 84 | bool load(handle src, bool) { 85 | if (!is_ros_msg_type(src, "std_msgs/Header")) { 86 | return false; 87 | } 88 | value.seq = src.attr("seq").cast(); 89 | value.stamp = src.attr("stamp").cast(); 90 | value.frame_id = src.attr("frame_id").cast(); 91 | return true; 92 | } 93 | 94 | // cpp -> python 95 | static handle cast(std_msgs::Header header, 96 | return_value_policy policy, 97 | handle parent) { 98 | object mod = module::import("std_msgs.msg._Header"); 99 | object MsgType = mod.attr("Header"); 100 | object msg = MsgType(); 101 | msg.attr("seq") = pybind11::cast(header.seq); 102 | msg.attr("stamp") = pybind11::cast(header.stamp); 103 | // avoid !!python/unicode problem. 104 | // msg.attr("frame_id") = pybind11::cast(header.frame_id); 105 | msg.attr("frame_id") = 106 | pybind11::bytes(reinterpret_cast(&header.frame_id[0]), 107 | header.frame_id.size()); 108 | msg.inc_ref(); 109 | return msg; 110 | } 111 | }; 112 | 113 | template <> 114 | struct type_caster { 115 | public: 116 | PYBIND11_TYPE_CASTER(geometry_msgs::Point, _("geometry_msgs::Point")); 117 | 118 | bool load(handle src, bool) { 119 | if (!is_ros_msg_type(src, "geometry_msgs/Point")) { 120 | return false; 121 | } 122 | value.x = (src.attr("x")).cast(); 123 | value.y = (src.attr("y")).cast(); 124 | value.z = (src.attr("z")).cast(); 125 | return true; 126 | } 127 | 128 | static handle cast(geometry_msgs::Point pt, 129 | return_value_policy policy, 130 | handle parent) { 131 | object mod = module::import("geometry_msgs.msg._Point"); 132 | object MsgType = mod.attr("Point"); 133 | object msg = MsgType(); 134 | msg.attr("x") = pybind11::cast(pt.x); 135 | msg.attr("y") = pybind11::cast(pt.y); 136 | msg.attr("z") = pybind11::cast(pt.z); 137 | msg.inc_ref(); 138 | return msg; 139 | } 140 | }; 141 | 142 | template <> 143 | struct type_caster { 144 | public: 145 | PYBIND11_TYPE_CASTER(geometry_msgs::Vector3, _("geometry_msgs::Vector3")); 146 | bool load(handle src, bool) { 147 | if (!is_ros_msg_type(src, "geometry_msgs/Vector3")) { 148 | return false; 149 | } 150 | value.x = (src.attr("x")).cast(); 151 | value.y = (src.attr("y")).cast(); 152 | value.z = (src.attr("z")).cast(); 153 | return true; 154 | } 155 | 156 | static handle cast(geometry_msgs::Vector3 cpp_msg, 157 | return_value_policy policy, 158 | handle parent) { 159 | object mod = module::import("geometry_msgs.msg._Vector3"); 160 | object MsgType = mod.attr("Vector3"); 161 | object msg = MsgType(); 162 | msg.attr("x") = pybind11::cast(cpp_msg.x); 163 | msg.attr("y") = pybind11::cast(cpp_msg.y); 164 | msg.attr("z") = pybind11::cast(cpp_msg.z); 165 | msg.inc_ref(); 166 | return msg; 167 | } 168 | }; 169 | 170 | template <> 171 | struct type_caster { 172 | public: 173 | PYBIND11_TYPE_CASTER(geometry_msgs::Quaternion, 174 | _("geometry_msgs::Quaternion")); 175 | bool load(handle src, bool) { 176 | if (!is_ros_msg_type(src, "geometry_msgs/Quaternion")) { 177 | return false; 178 | } 179 | value.x = (src.attr("x")).cast(); 180 | value.y = (src.attr("y")).cast(); 181 | value.z = (src.attr("z")).cast(); 182 | value.w = (src.attr("w")).cast(); 183 | return true; 184 | } 185 | 186 | static handle cast(geometry_msgs::Quaternion cpp_msg, 187 | return_value_policy policy, 188 | handle parent) { 189 | object mod = module::import("geometry_msgs.msg._Quaternion"); 190 | object MsgType = mod.attr("Quaternion"); 191 | object msg = MsgType(); 192 | msg.attr("x") = pybind11::cast(cpp_msg.x); 193 | msg.attr("y") = pybind11::cast(cpp_msg.y); 194 | msg.attr("z") = pybind11::cast(cpp_msg.z); 195 | msg.attr("w") = pybind11::cast(cpp_msg.w); 196 | msg.inc_ref(); 197 | return msg; 198 | } 199 | }; 200 | 201 | template <> 202 | struct type_caster { 203 | public: 204 | PYBIND11_TYPE_CASTER(geometry_msgs::Transform, _("geometry_msgs::Transform")); 205 | bool load(handle src, bool) { 206 | if (!is_ros_msg_type(src, "geometry_msgs/Transform")) { 207 | return false; 208 | } 209 | value.translation = 210 | (src.attr("translation")).cast(); 211 | value.rotation = (src.attr("rotation")).cast(); 212 | return true; 213 | } 214 | 215 | static handle cast(geometry_msgs::Transform cpp_msg, 216 | return_value_policy policy, 217 | handle parent) { 218 | object mod = module::import("geometry_msgs.msg._Transform"); 219 | object MsgType = mod.attr("Transform"); 220 | object msg = MsgType(); 221 | msg.attr("translation") = pybind11::cast(cpp_msg.translation); 222 | msg.attr("rotation") = pybind11::cast(cpp_msg.rotation); 223 | msg.inc_ref(); 224 | return msg; 225 | } 226 | }; 227 | 228 | template <> 229 | struct type_caster { 230 | public: 231 | PYBIND11_TYPE_CASTER(geometry_msgs::TransformStamped, 232 | _("geometry_msgs::TransformStamped")); 233 | bool load(handle src, bool) { 234 | if (!is_ros_msg_type(src, "geometry_msgs/TransformStamped")) { 235 | return false; 236 | } 237 | value.header = src.attr("header").cast(); 238 | value.child_frame_id = src.attr("child_frame_id").cast(); 239 | value.transform = src.attr("transform").cast(); 240 | return true; 241 | } 242 | 243 | static handle cast(geometry_msgs::TransformStamped cpp_msg, 244 | return_value_policy policy, 245 | handle parent) { 246 | object mod = module::import("geometry_msgs.msg._TransformStamped"); 247 | object MsgType = mod.attr("TransformStamped"); 248 | object msg = MsgType(); 249 | msg.attr("header") = pybind11::cast(cpp_msg.header); 250 | // msg.attr("child_frame_id") = pybind11::cast(cpp_msg.child_frame_id); 251 | msg.attr("child_frame_id") = pybind11::bytes( 252 | reinterpret_cast(&cpp_msg.child_frame_id[0]), 253 | cpp_msg.child_frame_id.size()); 254 | msg.attr("transform") = pybind11::cast(cpp_msg.transform); 255 | msg.inc_ref(); 256 | return msg; 257 | } 258 | }; 259 | 260 | template <> 261 | struct type_caster { 262 | public: 263 | PYBIND11_TYPE_CASTER(sensor_msgs::PointField, _("sensor_msgs::PointField")); 264 | bool load(handle src, bool) { 265 | if (!is_ros_msg_type(src, "sensor_msgs/PointField")) { 266 | return false; 267 | } 268 | value.name = (src.attr("name")).cast(); 269 | value.offset = (src.attr("offset")).cast(); 270 | value.datatype = (src.attr("datatype")).cast(); 271 | value.count = (src.attr("count")).cast(); 272 | return true; 273 | } 274 | 275 | static handle cast(sensor_msgs::PointField cpp_msg, 276 | return_value_policy policy, 277 | handle parent) { 278 | object mod = module::import("sensor_msgs.msg._PointField"); 279 | object MsgType = mod.attr("PointField"); 280 | object msg = MsgType(); 281 | // avoid !!python/unicode problem. 282 | // msg.attr("name") = pybind11::cast(cpp_msg.name); 283 | // msg.attr("name") = PyString_FromString(cpp_msg.name.c_str()); 284 | msg.attr("name") = pybind11::bytes( 285 | reinterpret_cast(&cpp_msg.name[0]), cpp_msg.name.size()); 286 | msg.attr("offset") = pybind11::cast(cpp_msg.offset); 287 | msg.attr("datatype") = pybind11::cast(cpp_msg.datatype); 288 | msg.attr("count") = pybind11::cast(cpp_msg.count); 289 | msg.inc_ref(); 290 | return msg; 291 | } 292 | }; 293 | 294 | template <> 295 | struct type_caster { 296 | public: 297 | PYBIND11_TYPE_CASTER(sensor_msgs::PointCloud2, _("sensor_msgs::PointCloud2")); 298 | bool load(handle src, bool) { 299 | if (!is_ros_msg_type(src, "sensor_msgs/PointCloud2")) { 300 | return false; 301 | } 302 | value.header = (src.attr("header")).cast(); 303 | value.height = (src.attr("height")).cast(); 304 | value.width = (src.attr("width")).cast(); 305 | pybind11::list field_lst = (src.attr("fields")).cast(); 306 | for (int i = 0; i < pybind11::len(field_lst); ++i) { 307 | sensor_msgs::PointField pf( 308 | (field_lst[i]).cast()); 309 | value.fields.push_back(pf); 310 | } 311 | value.is_bigendian = (src.attr("is_bigendian")).cast(); 312 | value.point_step = (src.attr("point_step")).cast(); 313 | value.row_step = (src.attr("row_step")).cast(); 314 | std::string data_str = (src.attr("data")).cast(); 315 | value.data.insert(value.data.end(), 316 | data_str.c_str(), 317 | data_str.c_str() + data_str.length()); 318 | value.is_dense = (src.attr("is_dense")).cast(); 319 | return true; 320 | } 321 | 322 | static handle cast(sensor_msgs::PointCloud2 cpp_msg, 323 | return_value_policy policy, 324 | handle parent) { 325 | object mod = module::import("sensor_msgs.msg._PointCloud2"); 326 | object MsgType = mod.attr("PointCloud2"); 327 | object msg = MsgType(); 328 | msg.attr("header") = pybind11::cast(cpp_msg.header); 329 | msg.attr("height") = pybind11::cast(cpp_msg.height); 330 | msg.attr("width") = pybind11::cast(cpp_msg.width); 331 | // msg.attr("fields") = pybind11::cast(cpp_msg.fields); 332 | // pybind11::list field_lst = (msg.attr("fields")).cast(); 333 | pybind11::list field_lst; 334 | for (size_t i = 0; i < cpp_msg.fields.size(); ++i) { 335 | const sensor_msgs::PointField &pf(cpp_msg.fields[i]); 336 | field_lst.append(pybind11::cast(pf)); 337 | } 338 | msg.attr("fields") = field_lst; 339 | msg.attr("is_bigendian") = pybind11::cast(cpp_msg.is_bigendian); 340 | msg.attr("point_step") = pybind11::cast(cpp_msg.point_step); 341 | msg.attr("row_step") = pybind11::cast(cpp_msg.row_step); 342 | // msg.attr("data") = pybind11::bytes(std::string(cpp_msg.data.begin(), 343 | // cpp_msg.data.end())); 344 | msg.attr("data") = pybind11::bytes( 345 | reinterpret_cast(&cpp_msg.data[0]), cpp_msg.data.size()); 346 | msg.attr("is_dense") = pybind11::cast(cpp_msg.is_dense); 347 | msg.inc_ref(); 348 | return msg; 349 | } 350 | }; 351 | 352 | template <> 353 | struct type_caster { 354 | public: 355 | PYBIND11_TYPE_CASTER(sensor_msgs::Image, _("sensor_msgs::Image")); 356 | bool load(handle src, bool) { 357 | if (!is_ros_msg_type(src, "sensor_msgs/Image")) { 358 | return false; 359 | } 360 | value.header = src.attr("header").cast(); 361 | value.height = src.attr("height").cast(); 362 | value.width = src.attr("width").cast(); 363 | value.encoding = src.attr("encoding").cast(); 364 | value.is_bigendian = src.attr("is_bigendian").cast(); 365 | value.step = src.attr("step").cast(); 366 | std::string data_str = src.attr("data").cast(); 367 | value.data.insert(value.data.end(), 368 | data_str.c_str(), 369 | data_str.c_str() + data_str.length()); 370 | return true; 371 | } 372 | 373 | static handle cast(sensor_msgs::Image cpp_msg, 374 | return_value_policy policy, 375 | handle parent) { 376 | object mod = module::import("sensor_msgs.msg._Image"); 377 | object MsgType = mod.attr("Image"); 378 | object msg = MsgType(); 379 | msg.attr("header") = pybind11::cast(cpp_msg.header); 380 | msg.attr("height") = pybind11::cast(cpp_msg.height); 381 | msg.attr("width") = pybind11::cast(cpp_msg.width); 382 | msg.attr("encoding") = pybind11::bytes(cpp_msg.encoding); 383 | msg.attr("is_bigendian") = pybind11::cast(cpp_msg.is_bigendian); 384 | msg.attr("step") = pybind11::cast(cpp_msg.step); 385 | // msg.attr("data") = pybind11::bytes(std::string(cpp_msg.data.begin(), 386 | // cpp_msg.data.end())); 387 | msg.attr("data") = pybind11::bytes( 388 | reinterpret_cast(&cpp_msg.data[0]), cpp_msg.data.size()); 389 | msg.inc_ref(); 390 | return msg; 391 | } 392 | }; 393 | 394 | template <> 395 | struct type_caster { 396 | public: 397 | PYBIND11_TYPE_CASTER(sensor_msgs::CameraInfo, _("sensor_msgs::CameraInfo")); 398 | bool load(handle src, bool) { 399 | if (!is_ros_msg_type(src, "sensor_msgs/CameraInfo")) { 400 | return false; 401 | } 402 | 403 | value.height = src.attr("height").cast(); 404 | value.width = src.attr("width").cast(); 405 | value.distortion_model = src.attr("distortion_model").cast(); 406 | 407 | { 408 | for (auto item : src.attr("D")) { 409 | value.D.push_back(item.cast()); 410 | } 411 | } 412 | { 413 | int i = 0; 414 | for (auto item : src.attr("K")) { 415 | value.K[i] = item.cast(); 416 | ++i; 417 | } 418 | } 419 | { 420 | int i = 0; 421 | for (auto item : src.attr("R")) { 422 | value.R[i] = item.cast(); 423 | ++i; 424 | } 425 | } 426 | { 427 | int i = 0; 428 | for (auto item : src.attr("P")) { 429 | value.P[i] = item.cast(); 430 | ++i; 431 | } 432 | } 433 | 434 | value.header = src.attr("header").cast(); 435 | 436 | return true; 437 | } 438 | 439 | static handle cast(sensor_msgs::CameraInfo cpp_msg, 440 | return_value_policy policy, 441 | handle parent) { 442 | object mod = module::import("sensor_msgs.msg._CameraInfo"); 443 | object MsgType = mod.attr("CameraInfo"); 444 | object msg = MsgType(); 445 | 446 | // TODO untested 447 | 448 | msg.attr("height") = cpp_msg.height; 449 | msg.attr("width") = cpp_msg.width; 450 | 451 | msg.attr("distortion_model") = cpp_msg.distortion_model; 452 | 453 | for (size_t i = 0; i < cpp_msg.D.size(); ++i) { 454 | pybind11::list D = msg.attr("D"); 455 | D[i] = cpp_msg.K[i]; 456 | } 457 | 458 | for (size_t i = 0; i < cpp_msg.K.size(); ++i) { 459 | pybind11::list K = msg.attr("K"); 460 | K[i] = cpp_msg.K[i]; 461 | } 462 | 463 | for (size_t i = 0; i < cpp_msg.R.size(); ++i) { 464 | pybind11::list R = msg.attr("R"); 465 | R[i] = cpp_msg.K[i]; 466 | } 467 | 468 | for (size_t i = 0; i < cpp_msg.P.size(); ++i) { 469 | pybind11::list P = msg.attr("P"); 470 | P[i] = cpp_msg.P[i]; 471 | } 472 | 473 | msg.attr("header") = pybind11::cast(cpp_msg.header); 474 | 475 | msg.inc_ref(); 476 | return msg; 477 | } 478 | }; 479 | 480 | template <> 481 | struct type_caster { 482 | public: 483 | PYBIND11_TYPE_CASTER(geometry_msgs::Pose, _("geometry_msgs::Pose")); 484 | bool load(handle src, bool) { 485 | if (!is_ros_msg_type(src, "geometry_msgs/Pose")) { 486 | return false; 487 | } 488 | value.position = (src.attr("position")).cast(); 489 | value.orientation = 490 | (src.attr("orientation")).cast(); 491 | return true; 492 | } 493 | 494 | static handle cast(geometry_msgs::Pose cpp_msg, 495 | return_value_policy policy, 496 | handle parent) { 497 | object mod = module::import("geometry_msgs.msg._Pose"); 498 | object MsgType = mod.attr("Pose"); 499 | object msg = MsgType(); 500 | msg.attr("position") = pybind11::cast(cpp_msg.position); 501 | msg.attr("orientation") = pybind11::cast(cpp_msg.orientation); 502 | msg.inc_ref(); 503 | return msg; 504 | } 505 | }; 506 | } 507 | } 508 | 509 | #endif /* end of include guard: CONVERTERS_H_L7OWNAZ8 */ 510 | -------------------------------------------------------------------------------- /include/pyrosmsg/pyrosmsg.h: -------------------------------------------------------------------------------- 1 | #ifndef PYROSMSG_H_B5DJTH81 2 | #define PYROSMSG_H_B5DJTH81 3 | 4 | #include 5 | #include 6 | 7 | #endif /* end of include guard: PYROSMSG_H_B5DJTH81 */ 8 | -------------------------------------------------------------------------------- /include/pyrosmsg/serialization.hpp: -------------------------------------------------------------------------------- 1 | /** 2 | * @author Daniel Maturana 3 | * @year 2015 4 | * 5 | * @attention Copyright (c) 2015 6 | * @attention Carnegie Mellon University 7 | * @attention All rights reserved. 8 | * 9 | **@=*/ 10 | 11 | #ifndef SERIALIZATION_H_QC30JWRG 12 | #define SERIALIZATION_H_QC30JWRG 13 | 14 | #include 15 | 16 | #include 17 | 18 | namespace pyrosmsg { 19 | 20 | // in python 21 | // buf = StringIO.StringIO() 22 | // msg.serialize(buf) 23 | // s = msg.getvalue() 24 | // 25 | // from string 26 | // msg = sensor_msgs.msg.PointCloud2() 27 | // msg.deserialize(msg_str) 28 | 29 | template 30 | std::string serialize(const MsgT& msg) { 31 | uint32_t serial_size = ros::serialization::serializationLength(msg); 32 | // we could directly use a string.c_str(), but not sure if 33 | // that could have ownership issues 34 | uint8_t* obuffer = new uint8_t[serial_size]; 35 | ros::serialization::OStream os(obuffer, serial_size); 36 | ros::serialization::serialize(os, msg); 37 | // TODO do we need null-termination? 38 | std::string out(obuffer, obuffer + serial_size); 39 | delete[] obuffer; 40 | return out; 41 | } 42 | 43 | template 44 | void deserialize(const std::string s, MsgT& msg) { 45 | uint32_t serial_size = s.length(); 46 | uint8_t* ibuffer = new uint8_t[serial_size](); 47 | std::copy(s.c_str(), s.c_str() + s.length(), &ibuffer[0]); 48 | ros::serialization::IStream istream(&ibuffer[0], serial_size); 49 | ros::serialization::deserialize(istream, msg); 50 | delete[] ibuffer; 51 | } 52 | } 53 | 54 | #endif /* end of include guard: SERIALIZATION_H_QC30JWRG */ 55 | -------------------------------------------------------------------------------- /package.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | pyrosmsg 4 | 0.0.0 5 | The pyrosmsg package 6 | 7 | dmaturan 8 | 9 | BSD license 10 | 11 | catkin 12 | 13 | geometry_msgs 14 | nav_msgs 15 | 16 | pybind11_catkin 17 | roscpp 18 | rospy 19 | sensor_msgs 20 | std_msgs 21 | visualization_msgs 22 | cmake_modules 23 | 24 | 25 | -------------------------------------------------------------------------------- /pyrosmsg/__init__.py: -------------------------------------------------------------------------------- 1 | # 2 | # @author Daniel Maturana 3 | # @year 2015 4 | # 5 | # @attention Copyright (c) 2015 6 | # @attention Carnegie Mellon University 7 | # @attention All rights reserved. 8 | # 9 | # @= 10 | 11 | from libpyrosmsg import * 12 | -------------------------------------------------------------------------------- /readme.md: -------------------------------------------------------------------------------- 1 | # pyrosmsg 2 | 3 | Daniel Maturana 2018, dimatura@gmail.com 4 | 5 | # What 6 | 7 | Bidirectional pybind11 converters for a few ROS messages. 8 | 9 | The converters will accept Python messages and convert them to C++ messages or viceversa. 10 | 11 | This library only really makes sense for C++ code wrapped with pybind11 for use with Python. 12 | 13 | # Why 14 | 15 | The two main languages ROS supports are C++ and Python. For any given message type, ROS 16 | will generate Python and C++ versions of that message that are interoperable when sending them 17 | over the network -- i.e., it's fine to send messages from python nodes to C++ nodes or viceversa. 18 | However, if we have a Python node that uses wrapped C++ code (something I do a lot), an issue is that 19 | (from the computer's point of view) the Python and C++ messages are actually completely unrelated objects. 20 | So e.g., to use a Python PointCloud2 message in C++ code or viceversa, conversion is needed. 21 | 22 | One option is to use the built-in serialization capabilities of ROS - e.g., if we have a Python message, 23 | we can serialize it to a string, pass it as a string to the C++ code (trivial with `pybind11`), and deserialize 24 | it there. This would be the equivalent of sending the a message over the network, except we're just copying 25 | strings within the same process. This is actually implemented in this package (`serialization.h`). However, 26 | I found that even without the network overhead, this method is fairly slow. 27 | 28 | Another option is to use the converters in this package. 29 | It uses `pybind11` to make this conversion as transparent as possible from both the C++ and Python 30 | ends -- all supported message types are converted automatically in each direction. 31 | I should note that whenever a message crosses the Python/C++ barrier with this method, the data is copied -- 32 | there is no memory shared between the Python and C++ messages. While this will incur in some overhead, 33 | it is far less than the (de)serialization method described above. 34 | 35 | One drawback is that the converters are (currently) written manually. It's a fairly mechanical process, 36 | and in theory it could be automated, but I have not done so yet. I would welcome converters for other message types. 37 | 38 | # How 39 | 40 | This is a catkin ROS package, and as such should be installed in a catkin workspace like any other. 41 | 42 | To use from the C++ side, simply `#include `. This will register `pybind11` converters that 43 | can be used to transparently accept and return the support ROS messages in your code. 44 | 45 | To use from the Python side, simply `import pyrosmsg` before using wrapped functions. 46 | 47 | So in your C++ code, 48 | 49 | ```cpp 50 | // this code is in a pybind11-wrapped module called, say, 'mycloudlib' 51 | 52 | #include 53 | #include 54 | 55 | // etc 56 | 57 | void process_pointcloud(const sensor_msgs::PointCloud2& cloud) { 58 | // do fast C++ stuff with your point cloud here 59 | } 60 | 61 | ``` 62 | 63 | And in your python code, presumably a ROS node, 64 | 65 | ```python 66 | from sensor_msgs.msg import PointCloud2 67 | import pyrosmsg 68 | import mycloudlib 69 | 70 | def my_pc2_callback(pc2msg): 71 | # pc2msg is a *python* sensor_msgs.msg.PointCloud2 72 | # transparently pass the PointCloud2 to your C++ code. 73 | # under the hood, pyrosmsg will create a C++ copy of the message 74 | # and pass it to the wrapped mycloudlib function. 75 | mycloudlib.process_pointcloud(pc2msg) 76 | ``` 77 | 78 | 79 | # Currently supported messages 80 | 81 | - `ros::Time` 82 | - `std_msgs::Header` 83 | - `geometry_msgs::Point` 84 | - `geometry_msgs::Vector3` 85 | - `geometry_msgs::Quaternion` 86 | - `geometry_msgs::Transform` 87 | - `geometry_msgs::TransformStamped` 88 | - `sensor_msgs::PointField` 89 | - `sensor_msgs::PointCloud2` 90 | - `sensor_msgs::Image` 91 | 92 | 93 | # Platforms 94 | 95 | Only tested with Ubuntu 16.04, ROS Kinetic, and Python 2.7. Would probably work with other Linux and (Indigo and newer) ROS distros. 96 | Python 3, probably not without minor modifications. 97 | 98 | # TODO 99 | 100 | It would be nice to avoid copying data, if possible. In my own experience copying image and point cloud data within a process has 101 | never been a bottleneck compared to whatever data processing I perform with the data -- after all, the copy is just 102 | a `memcpy` of a contiguous chunk a memory. But for massive amounts of data unnecessary copying is a drag. 103 | In theory, this should be possible -- the only difficulty is correct memory management across the Python and C++ 104 | border. 105 | 106 | 107 | # LICENSE 108 | 109 | BSD, see LICENSE. 110 | -------------------------------------------------------------------------------- /scratch/sanity.py: -------------------------------------------------------------------------------- 1 | 2 | import copy 3 | import numpy as np 4 | import rospy 5 | from std_msgs.msg import Header 6 | from sensor_msgs.msg import CameraInfo 7 | from sensor_msgs.msg import Image 8 | 9 | import cv_bridge 10 | 11 | import pyrosmsg 12 | import pypcd 13 | 14 | #ts = pyrosmsg.make_time() 15 | #print ts 16 | # 17 | #tsp = rospy.Time() 18 | #pyrosmsg.print_time(tsp) 19 | # 20 | #hdr = pyrosmsg.make_header(32) 21 | #print hdr 22 | # 23 | #pyrosmsg.print_header_seq(hdr) 24 | # 25 | #print 26 | #cloud = pyrosmsg.make_pc2(32) 27 | #print cloud 28 | # 29 | #print 30 | 31 | pc_data = np.random.random((4, 3)).astype('float32') 32 | pc = pypcd.make_xyz_point_cloud(pc_data) 33 | print 'pc.width:', pc.width 34 | pc_msg = pc.to_msg() 35 | print type(pc_msg) 36 | 37 | print 'pc_msg.width:', pc_msg.width 38 | 39 | pyrosmsg.print_centroid(pc_msg) 40 | print pc_data.mean(0) 41 | 42 | # 43 | #print 'ci' 44 | # 45 | #ci = CameraInfo() 46 | #print ci 47 | #pyrosmsg.print_cam_info(ci) 48 | # 49 | #print 'img' 50 | #print 51 | 52 | # 53 | #bridge = cv_bridge.CvBridge() 54 | #cv_img = (np.random.random((8, 8))*255).astype('u1') 55 | #img_msg = bridge.cv2_to_imgmsg(cv_img, encoding='passthrough') 56 | #print img_msg 57 | #print '---' 58 | #pyrosmsg.print_img(img_msg) 59 | # 60 | #print '***' 61 | #img_msg2 = pyrosmsg.make_img(8, 8) 62 | #print img_msg2 63 | #cv_img2 = bridge.imgmsg_to_cv2(img_msg2, desired_encoding='passthrough') 64 | -------------------------------------------------------------------------------- /scratch/serialization.py: -------------------------------------------------------------------------------- 1 | 2 | import cStringIO as StringIO 3 | import numpy as np 4 | 5 | from sensor_msgs.msg import PointCloud2 6 | 7 | #from rospy.numpy_msg import numpy_msg 8 | #PointCloud2Np = numpy_msg(PointCloud2) 9 | 10 | import pypcd 11 | 12 | import pyrosmsg 13 | 14 | pc = pypcd.PointCloud.from_path('./tmp.pcd') 15 | msg = pc.to_msg() 16 | 17 | #msg2 = PointCloud2() 18 | #msg2.deserialize(smsg) 19 | 20 | def with_caster(m): 21 | # 38.4 us per loop, 22 | pyrosmsg.print_centroid(m) 23 | 24 | def with_serial(m): 25 | # 117 us 26 | buf = StringIO.StringIO() 27 | m.serialize(buf) 28 | smsg = buf.getvalue() 29 | pyrosmsg.print_centroid2(smsg) 30 | -------------------------------------------------------------------------------- /scratch/unicheck.py: -------------------------------------------------------------------------------- 1 | 2 | import copy 3 | import numpy as np 4 | import rospy 5 | from std_msgs.msg import Header 6 | from sensor_msgs.msg import CameraInfo 7 | from sensor_msgs.msg import Image 8 | 9 | import cv_bridge 10 | 11 | import pymsg 12 | 13 | hdr = pyrosmsg.make_header(32, "header") 14 | print hdr 15 | 16 | -------------------------------------------------------------------------------- /setup.py: -------------------------------------------------------------------------------- 1 | ## ! DO NOT MANUALLY INVOKE THIS setup.py, USE CATKIN INSTEAD 2 | 3 | from distutils.core import setup 4 | from catkin_pkg.python_setup import generate_distutils_setup 5 | 6 | # fetch values from package.xml 7 | setup_args = generate_distutils_setup( 8 | packages=['pyrosmsg'], 9 | package_dir={'':''}) 10 | 11 | setup(**setup_args) 12 | -------------------------------------------------------------------------------- /src_wrap/module.cpp: -------------------------------------------------------------------------------- 1 | /** 2 | * @author Daniel Maturana 3 | * @year 2015 4 | * 5 | * @attention Copyright (c) 2015 6 | * @attention Carnegie Mellon University 7 | * @attention All rights reserved. 8 | * 9 | **@=*/ 10 | 11 | #include 12 | #include 13 | 14 | #include 15 | #include 16 | 17 | namespace pyrosmsg { 18 | 19 | namespace py = pybind11; 20 | 21 | void print_cam_info(const sensor_msgs::CameraInfo& ci) { 22 | std::cout << "distortion model\n"; 23 | std::cout << ci.distortion_model << "\n"; 24 | std::cout << "K R P [8]\n"; 25 | std::cout << ci.K[8] << " " << ci.R[8] << " " << ci.P[8] << "\n"; 26 | std::cout << "\n"; 27 | } 28 | 29 | void print_centroid(const sensor_msgs::PointCloud2& cloud) { 30 | double cx = 0., cy = 0., cz = 0.; 31 | for (size_t i = 0; i < cloud.width; ++i) { 32 | float x = 33 | *reinterpret_cast(&cloud.data[i * cloud.point_step]); 34 | float y = *reinterpret_cast( 35 | &cloud.data[i * cloud.point_step + sizeof(float)]); 36 | float z = *reinterpret_cast( 37 | &cloud.data[i * cloud.point_step + 2 * sizeof(float)]); 38 | cx += x; 39 | cy += y; 40 | cz += z; 41 | } 42 | std::cerr << "cloud.width = " << cloud.width << std::endl; 43 | cx /= cloud.width; 44 | cy /= cloud.width; 45 | cz /= cloud.width; 46 | std::cout << "centroid = [" << cx << " " << cy << " " << cz << "]" 47 | << std::endl; 48 | } 49 | 50 | void print_centroid2(const std::string& smsg) { 51 | // using generic serialization method 52 | sensor_msgs::PointCloud2 cloud; 53 | pyrosmsg::deserialize(smsg, cloud); 54 | 55 | double cx = 0., cy = 0., cz = 0.; 56 | for (size_t i = 0; i < cloud.width; ++i) { 57 | float x = 58 | *reinterpret_cast(&cloud.data[i * cloud.point_step]); 59 | float y = *reinterpret_cast( 60 | &cloud.data[i * cloud.point_step + sizeof(float)]); 61 | float z = *reinterpret_cast( 62 | &cloud.data[i * cloud.point_step + 2 * sizeof(float)]); 63 | cx += x; 64 | cy += y; 65 | cz += z; 66 | } 67 | std::cerr << "cloud.width = " << cloud.width << std::endl; 68 | cx /= cloud.width; 69 | cy /= cloud.width; 70 | cz /= cloud.width; 71 | std::cout << "centroid = [" << cx << " " << cy << " " << cz << "]" 72 | << std::endl; 73 | } 74 | 75 | sensor_msgs::PointCloud2 make_pc2(int rows) { 76 | sensor_msgs::PointCloud2 pc; 77 | pc.width = rows; 78 | pc.height = 1; 79 | sensor_msgs::PointField pfx; 80 | pfx.name = "x"; 81 | pfx.offset = 0; 82 | pfx.datatype = sensor_msgs::PointField::FLOAT32; 83 | pfx.count = 1; 84 | pc.fields.push_back(pfx); 85 | pc.point_step = sizeof(float); 86 | float data[rows]; 87 | for (int i = 0; i < rows; ++i) { 88 | data[i] = 28.0; 89 | } 90 | pc.data.insert(pc.data.end(), 91 | reinterpret_cast(data), 92 | reinterpret_cast(data + rows)); 93 | return pc; 94 | } 95 | 96 | sensor_msgs::PointCloud2 make_pc2_from_numpy(py::array_t xyz) { 97 | sensor_msgs::PointCloud2 pc; 98 | 99 | sensor_msgs::PointField pfx; 100 | pfx.name = "x"; 101 | pfx.offset = 0; 102 | pfx.datatype = sensor_msgs::PointField::FLOAT32; 103 | pfx.count = 1; 104 | pc.fields.push_back(pfx); 105 | 106 | sensor_msgs::PointField pfy; 107 | pfx.name = "y"; 108 | pfx.offset = 4; 109 | pfx.datatype = sensor_msgs::PointField::FLOAT32; 110 | pfx.count = 1; 111 | pc.fields.push_back(pfy); 112 | 113 | sensor_msgs::PointField pfz; 114 | pfx.name = "z"; 115 | pfx.offset = 4; 116 | pfx.datatype = sensor_msgs::PointField::FLOAT32; 117 | pfx.count = 1; 118 | pc.fields.push_back(pfz); 119 | 120 | pc.point_step = sizeof(float) * 3; 121 | 122 | // making assumptions on c-style numpy! 123 | auto xyz_buf = xyz.unchecked(); 124 | pc.data.insert( 125 | pc.data.end(), 126 | reinterpret_cast(xyz_buf.data(0, 0)), 127 | reinterpret_cast(xyz_buf.data(0, 0) + xyz_buf.nbytes())); 128 | pc.width = xyz.shape(0); 129 | pc.height = 1; 130 | return pc; 131 | } 132 | 133 | ros::Time make_time() { 134 | ros::Time ts; 135 | ts.sec = 28; 136 | ts.nsec = 999; 137 | return ts; 138 | } 139 | 140 | void print_time(const ros::Time& ts) { 141 | std::cerr << "ts.sec = " << ts.sec << std::endl; 142 | std::cerr << "ts.nsec = " << ts.nsec << std::endl; 143 | } 144 | 145 | ros::Time increment_ts(const ros::Time& ts) { 146 | std::cerr << "ts.sec = " << ts.sec << ", ts.nsec = " << ts.nsec << std::endl; 147 | ros::Time newts(ts.sec + 1, ts.nsec + 1); 148 | std::cerr << "newts.sec = " << newts.sec << ", newts.nsec = " << newts.nsec 149 | << std::endl; 150 | return newts; 151 | } 152 | 153 | std_msgs::Header make_header(int seq, std::string frame_id) { 154 | std_msgs::Header out; 155 | out.frame_id = frame_id; 156 | out.seq = seq; 157 | return out; 158 | } 159 | 160 | void print_header_seq(std_msgs::Header& header) { 161 | std::cerr << "header.seq = " << header.seq << std::endl; 162 | } 163 | 164 | void print_img(const sensor_msgs::Image& img) { 165 | std::cerr << "img.width = " << img.width << std::endl; 166 | std::cerr << "img.height = " << img.height << std::endl; 167 | std::cerr << "img.step = " << img.step << std::endl; 168 | std::cerr << "["; 169 | for (int i = 0; i < img.width * img.height; ++i) { 170 | std::cerr << (int)img.data[i] << ", "; 171 | } 172 | std::cerr << "]\n"; 173 | } 174 | 175 | sensor_msgs::Image make_img(int width, int height) { 176 | sensor_msgs::Image msg; 177 | msg.width = width; 178 | msg.height = height; 179 | msg.encoding = "8UC1"; 180 | msg.step = width; 181 | for (int i = 0; i < height; ++i) { 182 | for (int j = 0; j < width; ++j) { 183 | msg.data.push_back(width * i + j); 184 | } 185 | } 186 | return msg; 187 | } 188 | } 189 | 190 | PYBIND11_MODULE(libpyrosmsg, m) { 191 | // note - these methods are just sanity 192 | // checks. all the important stuff is in 193 | // the header. 194 | 195 | m.doc() = "libpyrosmsg module"; 196 | 197 | m.def("print_cam_info", &pyrosmsg::print_cam_info); 198 | m.def("print_centroid", &pyrosmsg::print_centroid); 199 | m.def("print_centroid2", &pyrosmsg::print_centroid2); 200 | m.def("make_pc2", &pyrosmsg::make_pc2); 201 | m.def("make_pc2_from_numpy", &pyrosmsg::make_pc2_from_numpy); 202 | m.def("print_time", &pyrosmsg::print_time, "print time"); 203 | m.def("make_time", &pyrosmsg::make_time, "make time"); 204 | m.def("make_header", &pyrosmsg::make_header); 205 | m.def("increment_ts", &pyrosmsg::increment_ts); 206 | m.def("print_header_seq", &pyrosmsg::print_header_seq); 207 | m.def("print_img", &pyrosmsg::print_img); 208 | m.def("make_img", &pyrosmsg::make_img); 209 | } 210 | --------------------------------------------------------------------------------