moveit2
The MoveIt Motion Planning Framework for ROS 2.
pybind_rosmsg_typecasters.h
Go to the documentation of this file.
1 /*********************************************************************
2  * Software License Agreement (BSD License)
3  *
4  * Copyright (c) 2021, Robert Haschke
5  * All rights reserved.
6  *
7  * Redistribution and use in source and binary forms, with or without
8  * modification, are permitted provided that the following conditions
9  * are met:
10  *
11  * * Redistributions of source code must retain the above copyright
12  * notice, this list of conditions and the following disclaimer.
13  * * Redistributions in binary form must reproduce the above
14  * copyright notice, this list of conditions and the following
15  * disclaimer in the documentation and/or other materials provided
16  * with the distribution.
17  * * The name of Robert Haschke may not be used to endorse or promote
18  * products derived from this software without specific prior
19  * written permission.
20  *
21  * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
22  * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
23  * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
24  * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
25  * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
26  * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
27  * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
28  * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
29  * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
30  * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
31  * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
32  * POSSIBILITY OF SUCH DAMAGE.
33  *********************************************************************/
34 
35 /* Author: Robert Haschke */
36 
37 #pragma once
38 
39 #include <pybind11/pybind11.h>
40 #include <ros/duration.h>
41 #include <ros/serialization.h>
42 
45 namespace moveit
46 {
47 namespace python
48 {
49 PYBIND11_EXPORT pybind11::object createMessage(const std::string& ros_msg_name);
50 PYBIND11_EXPORT bool convertible(const pybind11::handle& h, const char* ros_msg_name);
51 
52 } // namespace python
53 } // namespace moveit
54 
55 namespace pybind11
56 {
57 namespace detail
58 {
60 template <typename T>
62 {
63  // C++ -> Python
64  static handle cast(T&& src, return_value_policy /* policy */, handle /* parent */)
65  {
66  return PyFloat_FromDouble(src.toSec());
67  }
68 
69  // Python -> C++
70  bool load(handle src, bool convert)
71  {
72  if (hasattr(src, "to_sec"))
73  {
74  value = T(src.attr("to_sec")().cast<double>());
75  }
76  else if (convert)
77  {
78  value = T(src.cast<double>());
79  }
80  else
81  return false;
82  return true;
83  }
84 
85  PYBIND11_TYPE_CASTER(T, _("Duration"));
86 };
87 
88 template <>
89 struct type_caster<ros::Duration> : DurationCaster<ros::Duration>
90 {
91 };
92 
93 template <>
94 struct type_caster<ros::WallDuration> : DurationCaster<ros::WallDuration>
95 {
96 };
97 
99 template <typename T>
100 struct type_caster<T, enable_if_t<ros::message_traits::IsMessage<T>::value>>
101 {
102  // C++ -> Python
103  static handle cast(const T& src, return_value_policy /* policy */, handle /* parent */)
104  {
105  // serialize src into (python) buffer
106  std::size_t size = ros::serialization::serializationLength(src);
107  object pbuffer = reinterpret_steal<object>(PyBytes_FromStringAndSize(nullptr, size));
108  ros::serialization::OStream stream(reinterpret_cast<uint8_t*>(PyBytes_AsString(pbuffer.ptr())), size);
109  ros::serialization::serialize(stream, src);
110  // deserialize python type from buffer
111  object msg = moveit::python::createMessage(ros::message_traits::DataType<T>::value());
112  msg.attr("deserialize")(pbuffer);
113  return msg.release();
114  }
115 
116  // Python -> C++
117  bool load(handle src, bool /*convert*/)
118  {
119  if (!moveit::python::convertible(src, ros::message_traits::DataType<T>::value()))
120  return false;
121  // serialize src into (python) buffer
122  object pstream = module::import("io").attr("BytesIO")();
123  src.attr("serialize")(pstream);
124  object pbuffer = pstream.attr("getvalue")();
125  // deserialize C++ type from buffer
126  char* cbuffer = nullptr;
127  Py_ssize_t size;
128  PyBytes_AsStringAndSize(pbuffer.ptr(), &cbuffer, &size);
129  ros::serialization::IStream cstream(const_cast<uint8_t*>(reinterpret_cast<const uint8_t*>(cbuffer)), size);
130  ros::serialization::deserialize(cstream, value);
131  return true;
132  }
133 
135 };
136 } // namespace detail
137 } // namespace pybind11
PYBIND11_EXPORT pybind11::object createMessage(const std::string &ros_msg_name)
PYBIND11_EXPORT bool convertible(const pybind11::handle &h, const char *ros_msg_name)
Main namespace for MoveIt.
Definition: exceptions.h:43
Convert ros::Duration / ros::WallDuration into a float.
bool load(handle src, bool convert)
static handle cast(T &&src, return_value_policy, handle)
PYBIND11_TYPE_CASTER(T, _("Duration"))