diff --git a/.gitignore b/.gitignore new file mode 100644 index 00000000..d874231d --- /dev/null +++ b/.gitignore @@ -0,0 +1,4 @@ +build +__pycache__ +*cpython* +.vscode \ No newline at end of file diff --git a/CMakeLists.txt b/CMakeLists.txt index 280584af..baa1f85d 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -3,7 +3,9 @@ cmake_minimum_required(VERSION 3.5) SET(CMAKE_CXX_STANDARD 17) -include_directories(/usr/local/include/ddscxx /usr/local/include/iceoryx/v2.0.2) +include_directories(include thirdparty/include thirdparty/include/ddscxx thirdparty/include/iceoryx/v2.0.2) +link_directories(lib/${CMAKE_HOST_SYSTEM_PROCESSOR} thirdparty/lib/${CMAKE_HOST_SYSTEM_PROCESSOR}) + link_libraries(unitree_sdk2 ddsc ddscxx rt pthread) add_executable(test_publisher example/helloworld/publisher.cpp example/helloworld/HelloWorldData.cpp) @@ -12,6 +14,7 @@ add_executable(high_follow_sin example/high_level/follow_sin.cpp) add_executable(sportmode_test example/high_level/sportmode_test.cpp) add_executable(low_level example/low_level/low_level.cpp) add_executable(stand_example_go2 example/low_level/stand_example_go2.cpp) +add_executable(disable_sports_mode_go2 example/low_level/disable_sports_mode_go2.cpp) add_executable(stand_example_b2 example/low_level/stand_example_b2.cpp) add_executable(wireless example/wireless/wireless.cpp) add_executable(advanced_gamepad example/advanced_gamepad/main.cpp) diff --git a/README.md b/README.md index 0ffd31c0..cd68eb56 100644 --- a/README.md +++ b/README.md @@ -1,5 +1,12 @@ # unitree_sdk2 -Unitree robot sdk version 2. +Unitree robot sdk version 2 modified by @yihuai-gao + +## Additional features +- python/crc_module.cpython-310-aarch64-linux-gnu.so to calculate crc for each message pack when running python ros2 +- build/disable_sports_mode_go2 to disable sports mode before running self-built controller + +usage: [README](python/README.md) + ### Prebuild environment * OS (Ubuntu 20.04 LTS) diff --git a/example/low_level/disable_sports_mode_go2.cpp b/example/low_level/disable_sports_mode_go2.cpp new file mode 100644 index 00000000..c52fe048 --- /dev/null +++ b/example/low_level/disable_sports_mode_go2.cpp @@ -0,0 +1,308 @@ +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + +using namespace unitree::common; +using namespace unitree::robot; +using namespace unitree::robot::go2; + +#define TOPIC_LOWCMD "rt/lowcmd" +#define TOPIC_LOWSTATE "rt/lowstate" + +constexpr double PosStopF = (2.146E+9f); +constexpr double VelStopF = (16000.0f); + +class Custom +{ +public: + explicit Custom(){} + ~Custom(){} + + void Init(); + void InitRobotStateClient(); + int queryServiceStatus(const std::string& serviceName); + void activateService(const std::string& serviceName,int activate); +private: + void InitLowCmd(); + void LowStateMessageHandler(const void* messages); + void LowCmdWrite(); + +private: + float Kp = 60.0; + float Kd = 5.0; + double time_consume = 0; + int rate_count = 0; + int sin_count = 0; + int motiontime = 0; + float dt = 0.002; // 0.001~0.01 + + RobotStateClient rsc; + + unitree_go::msg::dds_::LowCmd_ low_cmd{}; // default init + unitree_go::msg::dds_::LowState_ low_state{}; // default init + + /*publisher*/ + ChannelPublisherPtr lowcmd_publisher; + /*subscriber*/ + ChannelSubscriberPtr lowstate_subscriber; + + /*LowCmd write thread*/ + ThreadPtr lowCmdWriteThreadPtr; + + float _targetPos_1[12] = {0.0, 1.36, -2.65, 0.0, 1.36, -2.65, + -0.2, 1.36, -2.65, 0.2, 1.36, -2.65}; + + float _targetPos_2[12] = {0.0, 0.67, -1.3, 0.0, 0.67, -1.3, + 0.0, 0.67, -1.3, 0.0, 0.67, -1.3}; + + float _targetPos_3[12] = {-0.35, 1.36, -2.65, 0.35, 1.36, -2.65, + -0.5, 1.36, -2.65, 0.5, 1.36, -2.65}; + + float _startPos[12]; + float _duration_1 = 500; + float _duration_2 = 500; + float _duration_3 = 1000; + float _duration_4 = 900; + float _percent_1 = 0; + float _percent_2 = 0; + float _percent_3 = 0; + float _percent_4 = 0; + + bool firstRun = true; + bool done = false; +}; + +uint32_t crc32_core(uint32_t* ptr, uint32_t len) +{ + unsigned int xbit = 0; + unsigned int data = 0; + unsigned int CRC32 = 0xFFFFFFFF; + const unsigned int dwPolynomial = 0x04c11db7; + + for (unsigned int i = 0; i < len; i++) + { + xbit = 1 << 31; + data = ptr[i]; + for (unsigned int bits = 0; bits < 32; bits++) + { + if (CRC32 & 0x80000000) + { + CRC32 <<= 1; + CRC32 ^= dwPolynomial; + } + else + { + CRC32 <<= 1; + } + + if (data & xbit) + CRC32 ^= dwPolynomial; + xbit >>= 1; + } + } + + return CRC32; +} + +void Custom::Init() +{ + InitLowCmd(); + + /*create publisher*/ + lowcmd_publisher.reset(new ChannelPublisher(TOPIC_LOWCMD)); + lowcmd_publisher->InitChannel(); + + /*create subscriber*/ + lowstate_subscriber.reset(new ChannelSubscriber(TOPIC_LOWSTATE)); + lowstate_subscriber->InitChannel(std::bind(&Custom::LowStateMessageHandler, this, std::placeholders::_1), 1); + + /*loop publishing thread*/ + lowCmdWriteThreadPtr = CreateRecurrentThreadEx("writebasiccmd", UT_CPU_ID_NONE, 2000, &Custom::LowCmdWrite, this); +} + +void Custom::InitLowCmd() +{ + low_cmd.head()[0] = 0xFE; + low_cmd.head()[1] = 0xEF; + low_cmd.level_flag() = 0xFF; + low_cmd.gpio() = 0; + + for(int i=0; i<20; i++) + { + low_cmd.motor_cmd()[i].mode() = (0x01); // motor switch to servo (PMSM) mode + low_cmd.motor_cmd()[i].q() = (PosStopF); + low_cmd.motor_cmd()[i].kp() = (0); + low_cmd.motor_cmd()[i].dq() = (VelStopF); + low_cmd.motor_cmd()[i].kd() = (0); + low_cmd.motor_cmd()[i].tau() = (0); + } +} + +void Custom::InitRobotStateClient() +{ + rsc.SetTimeout(10.0f); + rsc.Init(); +} + +int Custom::queryServiceStatus(const std::string& serviceName) +{ + std::vector serviceStateList; + int ret,serviceStatus; + ret = rsc.ServiceList(serviceStateList); + size_t i, count=serviceStateList.size(); + for (i=0; i=500) + { + if(firstRun) + { + for(int i = 0; i < 12; i++) + { + _startPos[i] = low_state.motor_state()[i].q(); + } + firstRun = false; + } + + _percent_1 += (float)1 / _duration_1; + _percent_1 = _percent_1 > 1 ? 1 : _percent_1; + if (_percent_1 < 1) + { + for (int j = 0; j < 12; j++) + { + low_cmd.motor_cmd()[j].q() = (1 - _percent_1) * _startPos[j] + _percent_1 * _targetPos_1[j]; + low_cmd.motor_cmd()[j].dq() = 0; + low_cmd.motor_cmd()[j].kp() = Kp; + low_cmd.motor_cmd()[j].kd() = Kd; + low_cmd.motor_cmd()[j].tau() = 0; + } + + } + if ((_percent_1 == 1)&&(_percent_2 < 1)) + { + _percent_2 += (float)1 / _duration_2; + _percent_2 = _percent_2 > 1 ? 1 : _percent_2; + + for (int j = 0; j < 12; j++) + { + low_cmd.motor_cmd()[j].q() = (1 - _percent_2) * _targetPos_1[j] + _percent_2 * _targetPos_2[j]; + low_cmd.motor_cmd()[j].dq() = 0; + low_cmd.motor_cmd()[j].kp() = Kp; + low_cmd.motor_cmd()[j].kd() = Kd; + low_cmd.motor_cmd()[j].tau() = 0; + } + } + + if ((_percent_1 == 1)&&(_percent_2 == 1)&&(_percent_3<1)) + { + _percent_3 += (float)1 / _duration_3; + _percent_3 = _percent_3 > 1 ? 1 : _percent_3; + + for (int j = 0; j < 12; j++) + { + low_cmd.motor_cmd()[j].q() = _targetPos_2[j]; + low_cmd.motor_cmd()[j].dq() = 0; + low_cmd.motor_cmd()[j].kp() = Kp; + low_cmd.motor_cmd()[j].kd() = Kd; + low_cmd.motor_cmd()[j].tau() = 0; + } + } + if ((_percent_1 == 1)&&(_percent_2 == 1)&&(_percent_3==1)&&((_percent_4<=1))) + { + _percent_4 += (float)1 / _duration_4; + _percent_4 = _percent_4 > 1 ? 1 : _percent_4; + for (int j = 0; j < 12; j++) + { + low_cmd.motor_cmd()[j].q() = (1 - _percent_4) * _targetPos_2[j] + _percent_4 * _targetPos_3[j]; + low_cmd.motor_cmd()[j].dq() = 0; + low_cmd.motor_cmd()[j].kp() = Kp; + low_cmd.motor_cmd()[j].kd() = Kd; + low_cmd.motor_cmd()[j].tau() = 0; + } + } + low_cmd.crc() = crc32_core((uint32_t *)&low_cmd, (sizeof(unitree_go::msg::dds_::LowCmd_)>>2)-1); + + lowcmd_publisher->Write(low_cmd); + } + +} + +int main(int argc, const char** argv) +{ + if (argc < 2) + { + std::cout << "Usage: " << argv[0] << " networkInterface" << std::endl; + exit(-1); + } + + std::cout << "WARNING: Make sure the robot is hung up or lying on the ground." << std::endl + << "Press Enter to continue..." << std::endl; + std::cin.ignore(); + + ChannelFactory::Instance()->Init(0, argv[1]); + + Custom custom; + custom.InitRobotStateClient(); + while(custom.queryServiceStatus("sport_mode")) + { + std::cout<<"Try to deactivate the service: "<<"sport_mode"< int: + """Calculates the CRC of a LowCmd message. Same as the C++ function.""" + ... \ No newline at end of file diff --git a/python/crc_module.so b/python/crc_module.so new file mode 100755 index 00000000..5ad145a1 Binary files /dev/null and b/python/crc_module.so differ diff --git a/python/crc_pybind.cpp b/python/crc_pybind.cpp new file mode 100644 index 00000000..3a72d582 --- /dev/null +++ b/python/crc_pybind.cpp @@ -0,0 +1,142 @@ +#include +#include +#include + +namespace py = pybind11; + +typedef struct +{ + uint8_t off; // off 0xA5 + std::array reserve; +} BmsCmd; + +typedef struct +{ + uint8_t mode; // desired working mode + float q; // desired angle (unit: radian) + float dq; // desired velocity (unit: radian/second) + float tau; // desired output torque (unit: N.m) + float Kp; // desired position stiffness (unit: N.m/rad ) + float Kd; // desired velocity stiffness (unit: N.m/(rad/s) ) + std::array reserve; +} MotorCmd; // motor control + +typedef struct +{ + std::array head; + uint8_t levelFlag; + uint8_t frameReserve; + + std::array SN; + std::array version; + uint16_t bandWidth; + std::array motorCmd; + BmsCmd bms; + std::array wirelessRemote; + std::array led; + std::array fan; + uint8_t gpio; + uint32_t reserve; + + uint32_t crc; +} LowCmd; + + +uint32_t crc32_core(uint32_t* ptr, uint32_t len) +{ + unsigned int xbit = 0; + unsigned int data = 0; + unsigned int CRC32 = 0xFFFFFFFF; + const unsigned int dwPolynomial = 0x04c11db7; + + for (unsigned int i = 0; i < len; i++) + { + xbit = 1 << 31; + data = ptr[i]; + for (unsigned int bits = 0; bits < 32; bits++) + { + if (CRC32 & 0x80000000) + { + CRC32 <<= 1; + CRC32 ^= dwPolynomial; + } + else + { + CRC32 <<= 1; + } + + if (data & xbit) + CRC32 ^= dwPolynomial; + xbit >>= 1; + } + } + + return CRC32; +} + +uint32_t get_crc(py::object msg) +{ + LowCmd raw{}; + + std::array head = py::cast>(msg.attr("head")); + + memcpy(&raw.head, &head, 2); + + raw.levelFlag = py::cast(msg.attr("level_flag")); + raw.frameReserve = py::cast(msg.attr("frame_reserve")); + + std::array SN = py::cast>(msg.attr("sn")); + memcpy(&raw.SN, &SN, 8); + + std::array version = py::cast>(msg.attr("version")); + memcpy(&raw.version, &version, 8); + + raw.bandWidth = py::cast(msg.attr("bandwidth")); + std::list motor_cmds = py::cast>(msg.attr("motor_cmd")); + + for (int i = 0; i < 20; i++) + { + py::object motor_cmd = motor_cmds.front(); + uint8_t mode = py::cast(motor_cmd.attr("mode")); + float q = py::cast(motor_cmd.attr("q")); + float dq = py::cast(motor_cmd.attr("dq")); + float tau = py::cast(motor_cmd.attr("tau")); + float Kp = py::cast(motor_cmd.attr("kp")); + float Kd = py::cast(motor_cmd.attr("kd")); + raw.motorCmd[i].mode = mode; + raw.motorCmd[i].q = q; + raw.motorCmd[i].dq = dq; + raw.motorCmd[i].tau = tau; + raw.motorCmd[i].Kp = Kp; + raw.motorCmd[i].Kd = Kd; + motor_cmds.pop_front(); + } + + BmsCmd bms; + py::object bms_msg = msg.attr("bms_cmd"); + bms.off = py::cast(bms_msg.attr("off")); + bms.reserve = py::cast>(bms_msg.attr("reserve")); + raw.bms.off = bms.off; + memcpy(&raw.bms.reserve, &bms.reserve, bms.reserve.size()*sizeof(uint8_t)); + + std::array wirelessRemote = py::cast>(msg.attr("wireless_remote")); + memcpy(&raw.wirelessRemote, &wirelessRemote, wirelessRemote.size()*sizeof(uint8_t)); + std::array led = py::cast>(msg.attr("led")); + memcpy(&raw.led, &led, led.size()*sizeof(uint8_t)); + std::array fan = py::cast>(msg.attr("fan")); + memcpy(&raw.fan, &fan, fan.size()*sizeof(uint8_t)); + + raw.gpio = py::cast(msg.attr("gpio")); + raw.reserve = py::cast(msg.attr("reserve")); + + + uint32_t crc = crc32_core((uint32_t *)&raw, (sizeof(LowCmd)>>2)-1); + return crc; +} + +PYBIND11_MODULE(crc_module, m) { + m.doc() = "pybind11 example plugin"; // optional module docstring + + m.def("crc32_core", &crc32_core, "A function which calculates the crc32 of a given array of uint32_t"); + m.def("get_crc", &get_crc, "A function which calculates the crc32 of a given LowCmd_ message"); +} \ No newline at end of file diff --git a/python/test_crc.py b/python/test_crc.py new file mode 100644 index 00000000..fbebefa3 --- /dev/null +++ b/python/test_crc.py @@ -0,0 +1,18 @@ +from crc_module import get_crc + +from unitree_go.msg import LowCmd + +lowcmd = LowCmd() +lowcmd.head = [20, 30] +for k in range(20): + lowcmd.motor_cmd[k].mode = 1 + lowcmd.motor_cmd[k].q = float(k) + lowcmd.motor_cmd[k].dq = float(k) + lowcmd.motor_cmd[k].tau = float(k) + lowcmd.motor_cmd[k].kp = float(k) + lowcmd.motor_cmd[k].kd = float(k) + + +lowcmd.led = [1, 2, 3, 4, 5, 6, 7, 8, 9, 10, 11, 12] +print(get_crc(lowcmd)) +lowcmd.crc = get_crc(lowcmd) \ No newline at end of file diff --git a/python/unitree_go/__init__.py b/python/unitree_go/__init__.py new file mode 100644 index 00000000..e69de29b diff --git a/python/unitree_go/msg/__init__.py b/python/unitree_go/msg/__init__.py new file mode 100644 index 00000000..5c000805 --- /dev/null +++ b/python/unitree_go/msg/__init__.py @@ -0,0 +1,22 @@ +from unitree_go.msg._audio_data import AudioData # noqa: F401 +from unitree_go.msg._bms_cmd import BmsCmd # noqa: F401 +from unitree_go.msg._bms_state import BmsState # noqa: F401 +from unitree_go.msg._error import Error # noqa: F401 +from unitree_go.msg._go2_front_video_data import Go2FrontVideoData # noqa: F401 +from unitree_go.msg._height_map import HeightMap # noqa: F401 +from unitree_go.msg._imu_state import IMUState # noqa: F401 +from unitree_go.msg._interface_config import InterfaceConfig # noqa: F401 +from unitree_go.msg._lidar_state import LidarState # noqa: F401 +from unitree_go.msg._low_cmd import LowCmd # noqa: F401 +from unitree_go.msg._low_state import LowState # noqa: F401 +from unitree_go.msg._motor_cmd import MotorCmd # noqa: F401 +from unitree_go.msg._motor_state import MotorState # noqa: F401 +from unitree_go.msg._path_point import PathPoint # noqa: F401 +from unitree_go.msg._req import Req # noqa: F401 +from unitree_go.msg._res import Res # noqa: F401 +from unitree_go.msg._sport_mode_cmd import SportModeCmd # noqa: F401 +from unitree_go.msg._sport_mode_state import SportModeState # noqa: F401 +from unitree_go.msg._time_spec import TimeSpec # noqa: F401 +from unitree_go.msg._uwb_state import UwbState # noqa: F401 +from unitree_go.msg._uwb_switch import UwbSwitch # noqa: F401 +from unitree_go.msg._wireless_controller import WirelessController # noqa: F401 diff --git a/python/unitree_go/msg/_audio_data.py b/python/unitree_go/msg/_audio_data.py new file mode 100644 index 00000000..9c9cf863 --- /dev/null +++ b/python/unitree_go/msg/_audio_data.py @@ -0,0 +1,161 @@ +# generated from rosidl_generator_py/resource/_idl.py.em +# with input from unitree_go:msg/AudioData.idl +# generated code does not contain a copyright notice + + +# Import statements for member types + +# Member 'data' +import array # noqa: E402, I100 + +import rosidl_parser.definition # noqa: E402, I100 + + +class Metaclass_AudioData(type): + """Metaclass of message 'AudioData'.""" + + _CREATE_ROS_MESSAGE = None + _CONVERT_FROM_PY = None + _CONVERT_TO_PY = None + _DESTROY_ROS_MESSAGE = None + _TYPE_SUPPORT = None + + __constants = { + } + + @classmethod + def __import_type_support__(cls): + try: + from rosidl_generator_py import import_type_support + module = import_type_support('unitree_go') + except ImportError: + import logging + import traceback + logger = logging.getLogger( + 'unitree_go.msg.AudioData') + logger.debug( + 'Failed to import needed modules for type support:\n' + + traceback.format_exc()) + else: + cls._CREATE_ROS_MESSAGE = module.create_ros_message_msg__msg__audio_data + cls._CONVERT_FROM_PY = module.convert_from_py_msg__msg__audio_data + cls._CONVERT_TO_PY = module.convert_to_py_msg__msg__audio_data + cls._TYPE_SUPPORT = module.type_support_msg__msg__audio_data + cls._DESTROY_ROS_MESSAGE = module.destroy_ros_message_msg__msg__audio_data + + @classmethod + def __prepare__(cls, name, bases, **kwargs): + # list constant names here so that they appear in the help text of + # the message class under "Data and other attributes defined here:" + # as well as populate each message instance + return { + } + + +class AudioData(metaclass=Metaclass_AudioData): + """Message class 'AudioData'.""" + + __slots__ = [ + '_time_frame', + '_data', + ] + + _fields_and_field_types = { + 'time_frame': 'uint64', + 'data': 'sequence', + } + + SLOT_TYPES = ( + rosidl_parser.definition.BasicType('uint64'), # noqa: E501 + rosidl_parser.definition.UnboundedSequence(rosidl_parser.definition.BasicType('uint8')), # noqa: E501 + ) + + def __init__(self, **kwargs): + assert all('_' + key in self.__slots__ for key in kwargs.keys()), \ + 'Invalid arguments passed to constructor: %s' % \ + ', '.join(sorted(k for k in kwargs.keys() if '_' + k not in self.__slots__)) + self.time_frame = kwargs.get('time_frame', int()) + self.data = array.array('B', kwargs.get('data', [])) + + def __repr__(self): + typename = self.__class__.__module__.split('.') + typename.pop() + typename.append(self.__class__.__name__) + args = [] + for s, t in zip(self.__slots__, self.SLOT_TYPES): + field = getattr(self, s) + fieldstr = repr(field) + # We use Python array type for fields that can be directly stored + # in them, and "normal" sequences for everything else. If it is + # a type that we store in an array, strip off the 'array' portion. + if ( + isinstance(t, rosidl_parser.definition.AbstractSequence) and + isinstance(t.value_type, rosidl_parser.definition.BasicType) and + t.value_type.typename in ['float', 'double', 'int8', 'uint8', 'int16', 'uint16', 'int32', 'uint32', 'int64', 'uint64'] + ): + if len(field) == 0: + fieldstr = '[]' + else: + assert fieldstr.startswith('array(') + prefix = "array('X', " + suffix = ')' + fieldstr = fieldstr[len(prefix):-len(suffix)] + args.append(s[1:] + '=' + fieldstr) + return '%s(%s)' % ('.'.join(typename), ', '.join(args)) + + def __eq__(self, other): + if not isinstance(other, self.__class__): + return False + if self.time_frame != other.time_frame: + return False + if self.data != other.data: + return False + return True + + @classmethod + def get_fields_and_field_types(cls): + from copy import copy + return copy(cls._fields_and_field_types) + + @property + def time_frame(self): + """Message field 'time_frame'.""" + return self._time_frame + + @time_frame.setter + def time_frame(self, value): + if __debug__: + assert \ + isinstance(value, int), \ + "The 'time_frame' field must be of type 'int'" + assert value >= 0 and value < 18446744073709551616, \ + "The 'time_frame' field must be an unsigned integer in [0, 18446744073709551615]" + self._time_frame = value + + @property + def data(self): + """Message field 'data'.""" + return self._data + + @data.setter + def data(self, value): + if isinstance(value, array.array): + assert value.typecode == 'B', \ + "The 'data' array.array() must have the type code of 'B'" + self._data = value + return + if __debug__: + from collections.abc import Sequence + from collections.abc import Set + from collections import UserList + from collections import UserString + assert \ + ((isinstance(value, Sequence) or + isinstance(value, Set) or + isinstance(value, UserList)) and + not isinstance(value, str) and + not isinstance(value, UserString) and + all(isinstance(v, int) for v in value) and + all(val >= 0 and val < 256 for val in value)), \ + "The 'data' field must be a set or sequence and each value of type 'int' and each unsigned integer in [0, 255]" + self._data = array.array('B', value) diff --git a/python/unitree_go/msg/_audio_data_s.c b/python/unitree_go/msg/_audio_data_s.c new file mode 100644 index 00000000..e85e1b15 --- /dev/null +++ b/python/unitree_go/msg/_audio_data_s.c @@ -0,0 +1,221 @@ +// generated from rosidl_generator_py/resource/_idl_support.c.em +// with input from unitree_go:msg/AudioData.idl +// generated code does not contain a copyright notice +#define NPY_NO_DEPRECATED_API NPY_1_7_API_VERSION +#include +#include +#ifndef _WIN32 +# pragma GCC diagnostic push +# pragma GCC diagnostic ignored "-Wunused-function" +#endif +#include "numpy/ndarrayobject.h" +#ifndef _WIN32 +# pragma GCC diagnostic pop +#endif +#include "rosidl_runtime_c/visibility_control.h" +#include "unitree_go/msg/detail/audio_data__struct.h" +#include "unitree_go/msg/detail/audio_data__functions.h" + +#include "rosidl_runtime_c/primitives_sequence.h" +#include "rosidl_runtime_c/primitives_sequence_functions.h" + + +ROSIDL_GENERATOR_C_EXPORT +bool unitree_go__msg__audio_data__convert_from_py(PyObject * _pymsg, void * _ros_message) +{ + // check that the passed message is of the expected Python class + { + char full_classname_dest[37]; + { + char * class_name = NULL; + char * module_name = NULL; + { + PyObject * class_attr = PyObject_GetAttrString(_pymsg, "__class__"); + if (class_attr) { + PyObject * name_attr = PyObject_GetAttrString(class_attr, "__name__"); + if (name_attr) { + class_name = (char *)PyUnicode_1BYTE_DATA(name_attr); + Py_DECREF(name_attr); + } + PyObject * module_attr = PyObject_GetAttrString(class_attr, "__module__"); + if (module_attr) { + module_name = (char *)PyUnicode_1BYTE_DATA(module_attr); + Py_DECREF(module_attr); + } + Py_DECREF(class_attr); + } + } + if (!class_name || !module_name) { + return false; + } + snprintf(full_classname_dest, sizeof(full_classname_dest), "%s.%s", module_name, class_name); + } + assert(strncmp("unitree_go.msg._audio_data.AudioData", full_classname_dest, 36) == 0); + } + unitree_go__msg__AudioData * ros_message = _ros_message; + { // time_frame + PyObject * field = PyObject_GetAttrString(_pymsg, "time_frame"); + if (!field) { + return false; + } + assert(PyLong_Check(field)); + ros_message->time_frame = PyLong_AsUnsignedLongLong(field); + Py_DECREF(field); + } + { // data + PyObject * field = PyObject_GetAttrString(_pymsg, "data"); + if (!field) { + return false; + } + if (PyObject_CheckBuffer(field)) { + // Optimization for converting arrays of primitives + Py_buffer view; + int rc = PyObject_GetBuffer(field, &view, PyBUF_SIMPLE); + if (rc < 0) { + Py_DECREF(field); + return false; + } + Py_ssize_t size = view.len / sizeof(uint8_t); + if (!rosidl_runtime_c__uint8__Sequence__init(&(ros_message->data), size)) { + PyErr_SetString(PyExc_RuntimeError, "unable to create uint8__Sequence ros_message"); + PyBuffer_Release(&view); + Py_DECREF(field); + return false; + } + uint8_t * dest = ros_message->data.data; + rc = PyBuffer_ToContiguous(dest, &view, view.len, 'C'); + if (rc < 0) { + PyBuffer_Release(&view); + Py_DECREF(field); + return false; + } + PyBuffer_Release(&view); + } else { + PyObject * seq_field = PySequence_Fast(field, "expected a sequence in 'data'"); + if (!seq_field) { + Py_DECREF(field); + return false; + } + Py_ssize_t size = PySequence_Size(field); + if (-1 == size) { + Py_DECREF(seq_field); + Py_DECREF(field); + return false; + } + if (!rosidl_runtime_c__uint8__Sequence__init(&(ros_message->data), size)) { + PyErr_SetString(PyExc_RuntimeError, "unable to create uint8__Sequence ros_message"); + Py_DECREF(seq_field); + Py_DECREF(field); + return false; + } + uint8_t * dest = ros_message->data.data; + for (Py_ssize_t i = 0; i < size; ++i) { + PyObject * item = PySequence_Fast_GET_ITEM(seq_field, i); + if (!item) { + Py_DECREF(seq_field); + Py_DECREF(field); + return false; + } + assert(PyLong_Check(item)); + uint8_t tmp = (uint8_t)PyLong_AsUnsignedLong(item); + + memcpy(&dest[i], &tmp, sizeof(uint8_t)); + } + Py_DECREF(seq_field); + } + Py_DECREF(field); + } + + return true; +} + +ROSIDL_GENERATOR_C_EXPORT +PyObject * unitree_go__msg__audio_data__convert_to_py(void * raw_ros_message) +{ + /* NOTE(esteve): Call constructor of AudioData */ + PyObject * _pymessage = NULL; + { + PyObject * pymessage_module = PyImport_ImportModule("unitree_go.msg._audio_data"); + assert(pymessage_module); + PyObject * pymessage_class = PyObject_GetAttrString(pymessage_module, "AudioData"); + assert(pymessage_class); + Py_DECREF(pymessage_module); + _pymessage = PyObject_CallObject(pymessage_class, NULL); + Py_DECREF(pymessage_class); + if (!_pymessage) { + return NULL; + } + } + unitree_go__msg__AudioData * ros_message = (unitree_go__msg__AudioData *)raw_ros_message; + { // time_frame + PyObject * field = NULL; + field = PyLong_FromUnsignedLongLong(ros_message->time_frame); + { + int rc = PyObject_SetAttrString(_pymessage, "time_frame", field); + Py_DECREF(field); + if (rc) { + return NULL; + } + } + } + { // data + PyObject * field = NULL; + field = PyObject_GetAttrString(_pymessage, "data"); + if (!field) { + return NULL; + } + assert(field->ob_type != NULL); + assert(field->ob_type->tp_name != NULL); + assert(strcmp(field->ob_type->tp_name, "array.array") == 0); + // ensure that itemsize matches the sizeof of the ROS message field + PyObject * itemsize_attr = PyObject_GetAttrString(field, "itemsize"); + assert(itemsize_attr != NULL); + size_t itemsize = PyLong_AsSize_t(itemsize_attr); + Py_DECREF(itemsize_attr); + if (itemsize != sizeof(uint8_t)) { + PyErr_SetString(PyExc_RuntimeError, "itemsize doesn't match expectation"); + Py_DECREF(field); + return NULL; + } + // clear the array, poor approach to remove potential default values + Py_ssize_t length = PyObject_Length(field); + if (-1 == length) { + Py_DECREF(field); + return NULL; + } + if (length > 0) { + PyObject * pop = PyObject_GetAttrString(field, "pop"); + assert(pop != NULL); + for (Py_ssize_t i = 0; i < length; ++i) { + PyObject * ret = PyObject_CallFunctionObjArgs(pop, NULL); + if (!ret) { + Py_DECREF(pop); + Py_DECREF(field); + return NULL; + } + Py_DECREF(ret); + } + Py_DECREF(pop); + } + if (ros_message->data.size > 0) { + // populating the array.array using the frombytes method + PyObject * frombytes = PyObject_GetAttrString(field, "frombytes"); + assert(frombytes != NULL); + uint8_t * src = &(ros_message->data.data[0]); + PyObject * data = PyBytes_FromStringAndSize((const char *)src, ros_message->data.size * sizeof(uint8_t)); + assert(data != NULL); + PyObject * ret = PyObject_CallFunctionObjArgs(frombytes, data, NULL); + Py_DECREF(data); + Py_DECREF(frombytes); + if (!ret) { + Py_DECREF(field); + return NULL; + } + Py_DECREF(ret); + } + Py_DECREF(field); + } + + // ownership of _pymessage is transferred to the caller + return _pymessage; +} diff --git a/python/unitree_go/msg/_bms_cmd.py b/python/unitree_go/msg/_bms_cmd.py new file mode 100644 index 00000000..6034fdea --- /dev/null +++ b/python/unitree_go/msg/_bms_cmd.py @@ -0,0 +1,168 @@ +# generated from rosidl_generator_py/resource/_idl.py.em +# with input from unitree_go:msg/BmsCmd.idl +# generated code does not contain a copyright notice + + +# Import statements for member types + +# Member 'reserve' +import numpy # noqa: E402, I100 + +import rosidl_parser.definition # noqa: E402, I100 + + +class Metaclass_BmsCmd(type): + """Metaclass of message 'BmsCmd'.""" + + _CREATE_ROS_MESSAGE = None + _CONVERT_FROM_PY = None + _CONVERT_TO_PY = None + _DESTROY_ROS_MESSAGE = None + _TYPE_SUPPORT = None + + __constants = { + } + + @classmethod + def __import_type_support__(cls): + try: + from rosidl_generator_py import import_type_support + module = import_type_support('unitree_go') + except ImportError: + import logging + import traceback + logger = logging.getLogger( + 'unitree_go.msg.BmsCmd') + logger.debug( + 'Failed to import needed modules for type support:\n' + + traceback.format_exc()) + else: + cls._CREATE_ROS_MESSAGE = module.create_ros_message_msg__msg__bms_cmd + cls._CONVERT_FROM_PY = module.convert_from_py_msg__msg__bms_cmd + cls._CONVERT_TO_PY = module.convert_to_py_msg__msg__bms_cmd + cls._TYPE_SUPPORT = module.type_support_msg__msg__bms_cmd + cls._DESTROY_ROS_MESSAGE = module.destroy_ros_message_msg__msg__bms_cmd + + @classmethod + def __prepare__(cls, name, bases, **kwargs): + # list constant names here so that they appear in the help text of + # the message class under "Data and other attributes defined here:" + # as well as populate each message instance + return { + } + + +class BmsCmd(metaclass=Metaclass_BmsCmd): + """Message class 'BmsCmd'.""" + + __slots__ = [ + '_off', + '_reserve', + ] + + _fields_and_field_types = { + 'off': 'uint8', + 'reserve': 'uint8[3]', + } + + SLOT_TYPES = ( + rosidl_parser.definition.BasicType('uint8'), # noqa: E501 + rosidl_parser.definition.Array(rosidl_parser.definition.BasicType('uint8'), 3), # noqa: E501 + ) + + def __init__(self, **kwargs): + assert all('_' + key in self.__slots__ for key in kwargs.keys()), \ + 'Invalid arguments passed to constructor: %s' % \ + ', '.join(sorted(k for k in kwargs.keys() if '_' + k not in self.__slots__)) + self.off = kwargs.get('off', int()) + if 'reserve' not in kwargs: + self.reserve = numpy.zeros(3, dtype=numpy.uint8) + else: + self.reserve = numpy.array(kwargs.get('reserve'), dtype=numpy.uint8) + assert self.reserve.shape == (3, ) + + def __repr__(self): + typename = self.__class__.__module__.split('.') + typename.pop() + typename.append(self.__class__.__name__) + args = [] + for s, t in zip(self.__slots__, self.SLOT_TYPES): + field = getattr(self, s) + fieldstr = repr(field) + # We use Python array type for fields that can be directly stored + # in them, and "normal" sequences for everything else. If it is + # a type that we store in an array, strip off the 'array' portion. + if ( + isinstance(t, rosidl_parser.definition.AbstractSequence) and + isinstance(t.value_type, rosidl_parser.definition.BasicType) and + t.value_type.typename in ['float', 'double', 'int8', 'uint8', 'int16', 'uint16', 'int32', 'uint32', 'int64', 'uint64'] + ): + if len(field) == 0: + fieldstr = '[]' + else: + assert fieldstr.startswith('array(') + prefix = "array('X', " + suffix = ')' + fieldstr = fieldstr[len(prefix):-len(suffix)] + args.append(s[1:] + '=' + fieldstr) + return '%s(%s)' % ('.'.join(typename), ', '.join(args)) + + def __eq__(self, other): + if not isinstance(other, self.__class__): + return False + if self.off != other.off: + return False + if all(self.reserve != other.reserve): + return False + return True + + @classmethod + def get_fields_and_field_types(cls): + from copy import copy + return copy(cls._fields_and_field_types) + + @property + def off(self): + """Message field 'off'.""" + return self._off + + @off.setter + def off(self, value): + if __debug__: + assert \ + isinstance(value, int), \ + "The 'off' field must be of type 'int'" + assert value >= 0 and value < 256, \ + "The 'off' field must be an unsigned integer in [0, 255]" + self._off = value + + @property + def reserve(self): + """Message field 'reserve'.""" + return self._reserve + + @reserve.setter + def reserve(self, value): + if isinstance(value, numpy.ndarray): + assert value.dtype == numpy.uint8, \ + "The 'reserve' numpy.ndarray() must have the dtype of 'numpy.uint8'" + assert value.size == 3, \ + "The 'reserve' numpy.ndarray() must have a size of 3" + self._reserve = value + return + if __debug__: + from collections.abc import Sequence + from collections.abc import Set + from collections import UserList + from collections import UserString + assert \ + ((isinstance(value, Sequence) or + isinstance(value, Set) or + isinstance(value, UserList)) and + not isinstance(value, str) and + not isinstance(value, UserString) and + len(value) == 3 and + all(isinstance(v, int) for v in value) and + all(val >= 0 and val < 256 for val in value)), \ + "The 'reserve' field must be a set or sequence with length 3 and each value of type 'int' and each unsigned integer in [0, 255]" + self._reserve = numpy.array(value, dtype=numpy.uint8) diff --git a/python/unitree_go/msg/_bms_cmd_s.c b/python/unitree_go/msg/_bms_cmd_s.c new file mode 100644 index 00000000..6c1efbd8 --- /dev/null +++ b/python/unitree_go/msg/_bms_cmd_s.c @@ -0,0 +1,143 @@ +// generated from rosidl_generator_py/resource/_idl_support.c.em +// with input from unitree_go:msg/BmsCmd.idl +// generated code does not contain a copyright notice +#define NPY_NO_DEPRECATED_API NPY_1_7_API_VERSION +#include +#include +#ifndef _WIN32 +# pragma GCC diagnostic push +# pragma GCC diagnostic ignored "-Wunused-function" +#endif +#include "numpy/ndarrayobject.h" +#ifndef _WIN32 +# pragma GCC diagnostic pop +#endif +#include "rosidl_runtime_c/visibility_control.h" +#include "unitree_go/msg/detail/bms_cmd__struct.h" +#include "unitree_go/msg/detail/bms_cmd__functions.h" + +#include "rosidl_runtime_c/primitives_sequence.h" +#include "rosidl_runtime_c/primitives_sequence_functions.h" + + +ROSIDL_GENERATOR_C_EXPORT +bool unitree_go__msg__bms_cmd__convert_from_py(PyObject * _pymsg, void * _ros_message) +{ + // check that the passed message is of the expected Python class + { + char full_classname_dest[31]; + { + char * class_name = NULL; + char * module_name = NULL; + { + PyObject * class_attr = PyObject_GetAttrString(_pymsg, "__class__"); + if (class_attr) { + PyObject * name_attr = PyObject_GetAttrString(class_attr, "__name__"); + if (name_attr) { + class_name = (char *)PyUnicode_1BYTE_DATA(name_attr); + Py_DECREF(name_attr); + } + PyObject * module_attr = PyObject_GetAttrString(class_attr, "__module__"); + if (module_attr) { + module_name = (char *)PyUnicode_1BYTE_DATA(module_attr); + Py_DECREF(module_attr); + } + Py_DECREF(class_attr); + } + } + if (!class_name || !module_name) { + return false; + } + snprintf(full_classname_dest, sizeof(full_classname_dest), "%s.%s", module_name, class_name); + } + assert(strncmp("unitree_go.msg._bms_cmd.BmsCmd", full_classname_dest, 30) == 0); + } + unitree_go__msg__BmsCmd * ros_message = _ros_message; + { // off + PyObject * field = PyObject_GetAttrString(_pymsg, "off"); + if (!field) { + return false; + } + assert(PyLong_Check(field)); + ros_message->off = (uint8_t)PyLong_AsUnsignedLong(field); + Py_DECREF(field); + } + { // reserve + PyObject * field = PyObject_GetAttrString(_pymsg, "reserve"); + if (!field) { + return false; + } + { + // TODO(dirk-thomas) use a better way to check the type before casting + assert(field->ob_type != NULL); + assert(field->ob_type->tp_name != NULL); + assert(strcmp(field->ob_type->tp_name, "numpy.ndarray") == 0); + PyArrayObject * seq_field = (PyArrayObject *)field; + Py_INCREF(seq_field); + assert(PyArray_NDIM(seq_field) == 1); + assert(PyArray_TYPE(seq_field) == NPY_UINT8); + Py_ssize_t size = 3; + uint8_t * dest = ros_message->reserve; + for (Py_ssize_t i = 0; i < size; ++i) { + uint8_t tmp = *(npy_uint8 *)PyArray_GETPTR1(seq_field, i); + memcpy(&dest[i], &tmp, sizeof(uint8_t)); + } + Py_DECREF(seq_field); + } + Py_DECREF(field); + } + + return true; +} + +ROSIDL_GENERATOR_C_EXPORT +PyObject * unitree_go__msg__bms_cmd__convert_to_py(void * raw_ros_message) +{ + /* NOTE(esteve): Call constructor of BmsCmd */ + PyObject * _pymessage = NULL; + { + PyObject * pymessage_module = PyImport_ImportModule("unitree_go.msg._bms_cmd"); + assert(pymessage_module); + PyObject * pymessage_class = PyObject_GetAttrString(pymessage_module, "BmsCmd"); + assert(pymessage_class); + Py_DECREF(pymessage_module); + _pymessage = PyObject_CallObject(pymessage_class, NULL); + Py_DECREF(pymessage_class); + if (!_pymessage) { + return NULL; + } + } + unitree_go__msg__BmsCmd * ros_message = (unitree_go__msg__BmsCmd *)raw_ros_message; + { // off + PyObject * field = NULL; + field = PyLong_FromUnsignedLong(ros_message->off); + { + int rc = PyObject_SetAttrString(_pymessage, "off", field); + Py_DECREF(field); + if (rc) { + return NULL; + } + } + } + { // reserve + PyObject * field = NULL; + field = PyObject_GetAttrString(_pymessage, "reserve"); + if (!field) { + return NULL; + } + assert(field->ob_type != NULL); + assert(field->ob_type->tp_name != NULL); + assert(strcmp(field->ob_type->tp_name, "numpy.ndarray") == 0); + PyArrayObject * seq_field = (PyArrayObject *)field; + assert(PyArray_NDIM(seq_field) == 1); + assert(PyArray_TYPE(seq_field) == NPY_UINT8); + assert(sizeof(npy_uint8) == sizeof(uint8_t)); + npy_uint8 * dst = (npy_uint8 *)PyArray_GETPTR1(seq_field, 0); + uint8_t * src = &(ros_message->reserve[0]); + memcpy(dst, src, 3 * sizeof(uint8_t)); + Py_DECREF(field); + } + + // ownership of _pymessage is transferred to the caller + return _pymessage; +} diff --git a/python/unitree_go/msg/_bms_state.py b/python/unitree_go/msg/_bms_state.py new file mode 100644 index 00000000..e727d628 --- /dev/null +++ b/python/unitree_go/msg/_bms_state.py @@ -0,0 +1,357 @@ +# generated from rosidl_generator_py/resource/_idl.py.em +# with input from unitree_go:msg/BmsState.idl +# generated code does not contain a copyright notice + + +# Import statements for member types + +# Member 'bq_ntc' +# Member 'mcu_ntc' +# Member 'cell_vol' +import numpy # noqa: E402, I100 + +import rosidl_parser.definition # noqa: E402, I100 + + +class Metaclass_BmsState(type): + """Metaclass of message 'BmsState'.""" + + _CREATE_ROS_MESSAGE = None + _CONVERT_FROM_PY = None + _CONVERT_TO_PY = None + _DESTROY_ROS_MESSAGE = None + _TYPE_SUPPORT = None + + __constants = { + } + + @classmethod + def __import_type_support__(cls): + try: + from rosidl_generator_py import import_type_support + module = import_type_support('unitree_go') + except ImportError: + import logging + import traceback + logger = logging.getLogger( + 'unitree_go.msg.BmsState') + logger.debug( + 'Failed to import needed modules for type support:\n' + + traceback.format_exc()) + else: + cls._CREATE_ROS_MESSAGE = module.create_ros_message_msg__msg__bms_state + cls._CONVERT_FROM_PY = module.convert_from_py_msg__msg__bms_state + cls._CONVERT_TO_PY = module.convert_to_py_msg__msg__bms_state + cls._TYPE_SUPPORT = module.type_support_msg__msg__bms_state + cls._DESTROY_ROS_MESSAGE = module.destroy_ros_message_msg__msg__bms_state + + @classmethod + def __prepare__(cls, name, bases, **kwargs): + # list constant names here so that they appear in the help text of + # the message class under "Data and other attributes defined here:" + # as well as populate each message instance + return { + } + + +class BmsState(metaclass=Metaclass_BmsState): + """Message class 'BmsState'.""" + + __slots__ = [ + '_version_high', + '_version_low', + '_status', + '_soc', + '_current', + '_cycle', + '_bq_ntc', + '_mcu_ntc', + '_cell_vol', + ] + + _fields_and_field_types = { + 'version_high': 'uint8', + 'version_low': 'uint8', + 'status': 'uint8', + 'soc': 'uint8', + 'current': 'int32', + 'cycle': 'uint16', + 'bq_ntc': 'int8[2]', + 'mcu_ntc': 'int8[2]', + 'cell_vol': 'uint16[15]', + } + + SLOT_TYPES = ( + rosidl_parser.definition.BasicType('uint8'), # noqa: E501 + rosidl_parser.definition.BasicType('uint8'), # noqa: E501 + rosidl_parser.definition.BasicType('uint8'), # noqa: E501 + rosidl_parser.definition.BasicType('uint8'), # noqa: E501 + rosidl_parser.definition.BasicType('int32'), # noqa: E501 + rosidl_parser.definition.BasicType('uint16'), # noqa: E501 + rosidl_parser.definition.Array(rosidl_parser.definition.BasicType('int8'), 2), # noqa: E501 + rosidl_parser.definition.Array(rosidl_parser.definition.BasicType('int8'), 2), # noqa: E501 + rosidl_parser.definition.Array(rosidl_parser.definition.BasicType('uint16'), 15), # noqa: E501 + ) + + def __init__(self, **kwargs): + assert all('_' + key in self.__slots__ for key in kwargs.keys()), \ + 'Invalid arguments passed to constructor: %s' % \ + ', '.join(sorted(k for k in kwargs.keys() if '_' + k not in self.__slots__)) + self.version_high = kwargs.get('version_high', int()) + self.version_low = kwargs.get('version_low', int()) + self.status = kwargs.get('status', int()) + self.soc = kwargs.get('soc', int()) + self.current = kwargs.get('current', int()) + self.cycle = kwargs.get('cycle', int()) + if 'bq_ntc' not in kwargs: + self.bq_ntc = numpy.zeros(2, dtype=numpy.int8) + else: + self.bq_ntc = numpy.array(kwargs.get('bq_ntc'), dtype=numpy.int8) + assert self.bq_ntc.shape == (2, ) + if 'mcu_ntc' not in kwargs: + self.mcu_ntc = numpy.zeros(2, dtype=numpy.int8) + else: + self.mcu_ntc = numpy.array(kwargs.get('mcu_ntc'), dtype=numpy.int8) + assert self.mcu_ntc.shape == (2, ) + if 'cell_vol' not in kwargs: + self.cell_vol = numpy.zeros(15, dtype=numpy.uint16) + else: + self.cell_vol = numpy.array(kwargs.get('cell_vol'), dtype=numpy.uint16) + assert self.cell_vol.shape == (15, ) + + def __repr__(self): + typename = self.__class__.__module__.split('.') + typename.pop() + typename.append(self.__class__.__name__) + args = [] + for s, t in zip(self.__slots__, self.SLOT_TYPES): + field = getattr(self, s) + fieldstr = repr(field) + # We use Python array type for fields that can be directly stored + # in them, and "normal" sequences for everything else. If it is + # a type that we store in an array, strip off the 'array' portion. + if ( + isinstance(t, rosidl_parser.definition.AbstractSequence) and + isinstance(t.value_type, rosidl_parser.definition.BasicType) and + t.value_type.typename in ['float', 'double', 'int8', 'uint8', 'int16', 'uint16', 'int32', 'uint32', 'int64', 'uint64'] + ): + if len(field) == 0: + fieldstr = '[]' + else: + assert fieldstr.startswith('array(') + prefix = "array('X', " + suffix = ')' + fieldstr = fieldstr[len(prefix):-len(suffix)] + args.append(s[1:] + '=' + fieldstr) + return '%s(%s)' % ('.'.join(typename), ', '.join(args)) + + def __eq__(self, other): + if not isinstance(other, self.__class__): + return False + if self.version_high != other.version_high: + return False + if self.version_low != other.version_low: + return False + if self.status != other.status: + return False + if self.soc != other.soc: + return False + if self.current != other.current: + return False + if self.cycle != other.cycle: + return False + if all(self.bq_ntc != other.bq_ntc): + return False + if all(self.mcu_ntc != other.mcu_ntc): + return False + if all(self.cell_vol != other.cell_vol): + return False + return True + + @classmethod + def get_fields_and_field_types(cls): + from copy import copy + return copy(cls._fields_and_field_types) + + @property + def version_high(self): + """Message field 'version_high'.""" + return self._version_high + + @version_high.setter + def version_high(self, value): + if __debug__: + assert \ + isinstance(value, int), \ + "The 'version_high' field must be of type 'int'" + assert value >= 0 and value < 256, \ + "The 'version_high' field must be an unsigned integer in [0, 255]" + self._version_high = value + + @property + def version_low(self): + """Message field 'version_low'.""" + return self._version_low + + @version_low.setter + def version_low(self, value): + if __debug__: + assert \ + isinstance(value, int), \ + "The 'version_low' field must be of type 'int'" + assert value >= 0 and value < 256, \ + "The 'version_low' field must be an unsigned integer in [0, 255]" + self._version_low = value + + @property + def status(self): + """Message field 'status'.""" + return self._status + + @status.setter + def status(self, value): + if __debug__: + assert \ + isinstance(value, int), \ + "The 'status' field must be of type 'int'" + assert value >= 0 and value < 256, \ + "The 'status' field must be an unsigned integer in [0, 255]" + self._status = value + + @property + def soc(self): + """Message field 'soc'.""" + return self._soc + + @soc.setter + def soc(self, value): + if __debug__: + assert \ + isinstance(value, int), \ + "The 'soc' field must be of type 'int'" + assert value >= 0 and value < 256, \ + "The 'soc' field must be an unsigned integer in [0, 255]" + self._soc = value + + @property + def current(self): + """Message field 'current'.""" + return self._current + + @current.setter + def current(self, value): + if __debug__: + assert \ + isinstance(value, int), \ + "The 'current' field must be of type 'int'" + assert value >= -2147483648 and value < 2147483648, \ + "The 'current' field must be an integer in [-2147483648, 2147483647]" + self._current = value + + @property + def cycle(self): + """Message field 'cycle'.""" + return self._cycle + + @cycle.setter + def cycle(self, value): + if __debug__: + assert \ + isinstance(value, int), \ + "The 'cycle' field must be of type 'int'" + assert value >= 0 and value < 65536, \ + "The 'cycle' field must be an unsigned integer in [0, 65535]" + self._cycle = value + + @property + def bq_ntc(self): + """Message field 'bq_ntc'.""" + return self._bq_ntc + + @bq_ntc.setter + def bq_ntc(self, value): + if isinstance(value, numpy.ndarray): + assert value.dtype == numpy.int8, \ + "The 'bq_ntc' numpy.ndarray() must have the dtype of 'numpy.int8'" + assert value.size == 2, \ + "The 'bq_ntc' numpy.ndarray() must have a size of 2" + self._bq_ntc = value + return + if __debug__: + from collections.abc import Sequence + from collections.abc import Set + from collections import UserList + from collections import UserString + assert \ + ((isinstance(value, Sequence) or + isinstance(value, Set) or + isinstance(value, UserList)) and + not isinstance(value, str) and + not isinstance(value, UserString) and + len(value) == 2 and + all(isinstance(v, int) for v in value) and + all(val >= -128 and val < 128 for val in value)), \ + "The 'bq_ntc' field must be a set or sequence with length 2 and each value of type 'int' and each integer in [-128, 127]" + self._bq_ntc = numpy.array(value, dtype=numpy.int8) + + @property + def mcu_ntc(self): + """Message field 'mcu_ntc'.""" + return self._mcu_ntc + + @mcu_ntc.setter + def mcu_ntc(self, value): + if isinstance(value, numpy.ndarray): + assert value.dtype == numpy.int8, \ + "The 'mcu_ntc' numpy.ndarray() must have the dtype of 'numpy.int8'" + assert value.size == 2, \ + "The 'mcu_ntc' numpy.ndarray() must have a size of 2" + self._mcu_ntc = value + return + if __debug__: + from collections.abc import Sequence + from collections.abc import Set + from collections import UserList + from collections import UserString + assert \ + ((isinstance(value, Sequence) or + isinstance(value, Set) or + isinstance(value, UserList)) and + not isinstance(value, str) and + not isinstance(value, UserString) and + len(value) == 2 and + all(isinstance(v, int) for v in value) and + all(val >= -128 and val < 128 for val in value)), \ + "The 'mcu_ntc' field must be a set or sequence with length 2 and each value of type 'int' and each integer in [-128, 127]" + self._mcu_ntc = numpy.array(value, dtype=numpy.int8) + + @property + def cell_vol(self): + """Message field 'cell_vol'.""" + return self._cell_vol + + @cell_vol.setter + def cell_vol(self, value): + if isinstance(value, numpy.ndarray): + assert value.dtype == numpy.uint16, \ + "The 'cell_vol' numpy.ndarray() must have the dtype of 'numpy.uint16'" + assert value.size == 15, \ + "The 'cell_vol' numpy.ndarray() must have a size of 15" + self._cell_vol = value + return + if __debug__: + from collections.abc import Sequence + from collections.abc import Set + from collections import UserList + from collections import UserString + assert \ + ((isinstance(value, Sequence) or + isinstance(value, Set) or + isinstance(value, UserList)) and + not isinstance(value, str) and + not isinstance(value, UserString) and + len(value) == 15 and + all(isinstance(v, int) for v in value) and + all(val >= 0 and val < 65536 for val in value)), \ + "The 'cell_vol' field must be a set or sequence with length 15 and each value of type 'int' and each unsigned integer in [0, 65535]" + self._cell_vol = numpy.array(value, dtype=numpy.uint16) diff --git a/python/unitree_go/msg/_bms_state_s.c b/python/unitree_go/msg/_bms_state_s.c new file mode 100644 index 00000000..6e269733 --- /dev/null +++ b/python/unitree_go/msg/_bms_state_s.c @@ -0,0 +1,327 @@ +// generated from rosidl_generator_py/resource/_idl_support.c.em +// with input from unitree_go:msg/BmsState.idl +// generated code does not contain a copyright notice +#define NPY_NO_DEPRECATED_API NPY_1_7_API_VERSION +#include +#include +#ifndef _WIN32 +# pragma GCC diagnostic push +# pragma GCC diagnostic ignored "-Wunused-function" +#endif +#include "numpy/ndarrayobject.h" +#ifndef _WIN32 +# pragma GCC diagnostic pop +#endif +#include "rosidl_runtime_c/visibility_control.h" +#include "unitree_go/msg/detail/bms_state__struct.h" +#include "unitree_go/msg/detail/bms_state__functions.h" + +#include "rosidl_runtime_c/primitives_sequence.h" +#include "rosidl_runtime_c/primitives_sequence_functions.h" + + +ROSIDL_GENERATOR_C_EXPORT +bool unitree_go__msg__bms_state__convert_from_py(PyObject * _pymsg, void * _ros_message) +{ + // check that the passed message is of the expected Python class + { + char full_classname_dest[35]; + { + char * class_name = NULL; + char * module_name = NULL; + { + PyObject * class_attr = PyObject_GetAttrString(_pymsg, "__class__"); + if (class_attr) { + PyObject * name_attr = PyObject_GetAttrString(class_attr, "__name__"); + if (name_attr) { + class_name = (char *)PyUnicode_1BYTE_DATA(name_attr); + Py_DECREF(name_attr); + } + PyObject * module_attr = PyObject_GetAttrString(class_attr, "__module__"); + if (module_attr) { + module_name = (char *)PyUnicode_1BYTE_DATA(module_attr); + Py_DECREF(module_attr); + } + Py_DECREF(class_attr); + } + } + if (!class_name || !module_name) { + return false; + } + snprintf(full_classname_dest, sizeof(full_classname_dest), "%s.%s", module_name, class_name); + } + assert(strncmp("unitree_go.msg._bms_state.BmsState", full_classname_dest, 34) == 0); + } + unitree_go__msg__BmsState * ros_message = _ros_message; + { // version_high + PyObject * field = PyObject_GetAttrString(_pymsg, "version_high"); + if (!field) { + return false; + } + assert(PyLong_Check(field)); + ros_message->version_high = (uint8_t)PyLong_AsUnsignedLong(field); + Py_DECREF(field); + } + { // version_low + PyObject * field = PyObject_GetAttrString(_pymsg, "version_low"); + if (!field) { + return false; + } + assert(PyLong_Check(field)); + ros_message->version_low = (uint8_t)PyLong_AsUnsignedLong(field); + Py_DECREF(field); + } + { // status + PyObject * field = PyObject_GetAttrString(_pymsg, "status"); + if (!field) { + return false; + } + assert(PyLong_Check(field)); + ros_message->status = (uint8_t)PyLong_AsUnsignedLong(field); + Py_DECREF(field); + } + { // soc + PyObject * field = PyObject_GetAttrString(_pymsg, "soc"); + if (!field) { + return false; + } + assert(PyLong_Check(field)); + ros_message->soc = (uint8_t)PyLong_AsUnsignedLong(field); + Py_DECREF(field); + } + { // current + PyObject * field = PyObject_GetAttrString(_pymsg, "current"); + if (!field) { + return false; + } + assert(PyLong_Check(field)); + ros_message->current = (int32_t)PyLong_AsLong(field); + Py_DECREF(field); + } + { // cycle + PyObject * field = PyObject_GetAttrString(_pymsg, "cycle"); + if (!field) { + return false; + } + assert(PyLong_Check(field)); + ros_message->cycle = (uint16_t)PyLong_AsUnsignedLong(field); + Py_DECREF(field); + } + { // bq_ntc + PyObject * field = PyObject_GetAttrString(_pymsg, "bq_ntc"); + if (!field) { + return false; + } + { + // TODO(dirk-thomas) use a better way to check the type before casting + assert(field->ob_type != NULL); + assert(field->ob_type->tp_name != NULL); + assert(strcmp(field->ob_type->tp_name, "numpy.ndarray") == 0); + PyArrayObject * seq_field = (PyArrayObject *)field; + Py_INCREF(seq_field); + assert(PyArray_NDIM(seq_field) == 1); + assert(PyArray_TYPE(seq_field) == NPY_INT8); + Py_ssize_t size = 2; + int8_t * dest = ros_message->bq_ntc; + for (Py_ssize_t i = 0; i < size; ++i) { + int8_t tmp = *(npy_int8 *)PyArray_GETPTR1(seq_field, i); + memcpy(&dest[i], &tmp, sizeof(int8_t)); + } + Py_DECREF(seq_field); + } + Py_DECREF(field); + } + { // mcu_ntc + PyObject * field = PyObject_GetAttrString(_pymsg, "mcu_ntc"); + if (!field) { + return false; + } + { + // TODO(dirk-thomas) use a better way to check the type before casting + assert(field->ob_type != NULL); + assert(field->ob_type->tp_name != NULL); + assert(strcmp(field->ob_type->tp_name, "numpy.ndarray") == 0); + PyArrayObject * seq_field = (PyArrayObject *)field; + Py_INCREF(seq_field); + assert(PyArray_NDIM(seq_field) == 1); + assert(PyArray_TYPE(seq_field) == NPY_INT8); + Py_ssize_t size = 2; + int8_t * dest = ros_message->mcu_ntc; + for (Py_ssize_t i = 0; i < size; ++i) { + int8_t tmp = *(npy_int8 *)PyArray_GETPTR1(seq_field, i); + memcpy(&dest[i], &tmp, sizeof(int8_t)); + } + Py_DECREF(seq_field); + } + Py_DECREF(field); + } + { // cell_vol + PyObject * field = PyObject_GetAttrString(_pymsg, "cell_vol"); + if (!field) { + return false; + } + { + // TODO(dirk-thomas) use a better way to check the type before casting + assert(field->ob_type != NULL); + assert(field->ob_type->tp_name != NULL); + assert(strcmp(field->ob_type->tp_name, "numpy.ndarray") == 0); + PyArrayObject * seq_field = (PyArrayObject *)field; + Py_INCREF(seq_field); + assert(PyArray_NDIM(seq_field) == 1); + assert(PyArray_TYPE(seq_field) == NPY_UINT16); + Py_ssize_t size = 15; + uint16_t * dest = ros_message->cell_vol; + for (Py_ssize_t i = 0; i < size; ++i) { + uint16_t tmp = *(npy_uint16 *)PyArray_GETPTR1(seq_field, i); + memcpy(&dest[i], &tmp, sizeof(uint16_t)); + } + Py_DECREF(seq_field); + } + Py_DECREF(field); + } + + return true; +} + +ROSIDL_GENERATOR_C_EXPORT +PyObject * unitree_go__msg__bms_state__convert_to_py(void * raw_ros_message) +{ + /* NOTE(esteve): Call constructor of BmsState */ + PyObject * _pymessage = NULL; + { + PyObject * pymessage_module = PyImport_ImportModule("unitree_go.msg._bms_state"); + assert(pymessage_module); + PyObject * pymessage_class = PyObject_GetAttrString(pymessage_module, "BmsState"); + assert(pymessage_class); + Py_DECREF(pymessage_module); + _pymessage = PyObject_CallObject(pymessage_class, NULL); + Py_DECREF(pymessage_class); + if (!_pymessage) { + return NULL; + } + } + unitree_go__msg__BmsState * ros_message = (unitree_go__msg__BmsState *)raw_ros_message; + { // version_high + PyObject * field = NULL; + field = PyLong_FromUnsignedLong(ros_message->version_high); + { + int rc = PyObject_SetAttrString(_pymessage, "version_high", field); + Py_DECREF(field); + if (rc) { + return NULL; + } + } + } + { // version_low + PyObject * field = NULL; + field = PyLong_FromUnsignedLong(ros_message->version_low); + { + int rc = PyObject_SetAttrString(_pymessage, "version_low", field); + Py_DECREF(field); + if (rc) { + return NULL; + } + } + } + { // status + PyObject * field = NULL; + field = PyLong_FromUnsignedLong(ros_message->status); + { + int rc = PyObject_SetAttrString(_pymessage, "status", field); + Py_DECREF(field); + if (rc) { + return NULL; + } + } + } + { // soc + PyObject * field = NULL; + field = PyLong_FromUnsignedLong(ros_message->soc); + { + int rc = PyObject_SetAttrString(_pymessage, "soc", field); + Py_DECREF(field); + if (rc) { + return NULL; + } + } + } + { // current + PyObject * field = NULL; + field = PyLong_FromLong(ros_message->current); + { + int rc = PyObject_SetAttrString(_pymessage, "current", field); + Py_DECREF(field); + if (rc) { + return NULL; + } + } + } + { // cycle + PyObject * field = NULL; + field = PyLong_FromUnsignedLong(ros_message->cycle); + { + int rc = PyObject_SetAttrString(_pymessage, "cycle", field); + Py_DECREF(field); + if (rc) { + return NULL; + } + } + } + { // bq_ntc + PyObject * field = NULL; + field = PyObject_GetAttrString(_pymessage, "bq_ntc"); + if (!field) { + return NULL; + } + assert(field->ob_type != NULL); + assert(field->ob_type->tp_name != NULL); + assert(strcmp(field->ob_type->tp_name, "numpy.ndarray") == 0); + PyArrayObject * seq_field = (PyArrayObject *)field; + assert(PyArray_NDIM(seq_field) == 1); + assert(PyArray_TYPE(seq_field) == NPY_INT8); + assert(sizeof(npy_int8) == sizeof(int8_t)); + npy_int8 * dst = (npy_int8 *)PyArray_GETPTR1(seq_field, 0); + int8_t * src = &(ros_message->bq_ntc[0]); + memcpy(dst, src, 2 * sizeof(int8_t)); + Py_DECREF(field); + } + { // mcu_ntc + PyObject * field = NULL; + field = PyObject_GetAttrString(_pymessage, "mcu_ntc"); + if (!field) { + return NULL; + } + assert(field->ob_type != NULL); + assert(field->ob_type->tp_name != NULL); + assert(strcmp(field->ob_type->tp_name, "numpy.ndarray") == 0); + PyArrayObject * seq_field = (PyArrayObject *)field; + assert(PyArray_NDIM(seq_field) == 1); + assert(PyArray_TYPE(seq_field) == NPY_INT8); + assert(sizeof(npy_int8) == sizeof(int8_t)); + npy_int8 * dst = (npy_int8 *)PyArray_GETPTR1(seq_field, 0); + int8_t * src = &(ros_message->mcu_ntc[0]); + memcpy(dst, src, 2 * sizeof(int8_t)); + Py_DECREF(field); + } + { // cell_vol + PyObject * field = NULL; + field = PyObject_GetAttrString(_pymessage, "cell_vol"); + if (!field) { + return NULL; + } + assert(field->ob_type != NULL); + assert(field->ob_type->tp_name != NULL); + assert(strcmp(field->ob_type->tp_name, "numpy.ndarray") == 0); + PyArrayObject * seq_field = (PyArrayObject *)field; + assert(PyArray_NDIM(seq_field) == 1); + assert(PyArray_TYPE(seq_field) == NPY_UINT16); + assert(sizeof(npy_uint16) == sizeof(uint16_t)); + npy_uint16 * dst = (npy_uint16 *)PyArray_GETPTR1(seq_field, 0); + uint16_t * src = &(ros_message->cell_vol[0]); + memcpy(dst, src, 15 * sizeof(uint16_t)); + Py_DECREF(field); + } + + // ownership of _pymessage is transferred to the caller + return _pymessage; +} diff --git a/python/unitree_go/msg/_error.py b/python/unitree_go/msg/_error.py new file mode 100644 index 00000000..142341a6 --- /dev/null +++ b/python/unitree_go/msg/_error.py @@ -0,0 +1,145 @@ +# generated from rosidl_generator_py/resource/_idl.py.em +# with input from unitree_go:msg/Error.idl +# generated code does not contain a copyright notice + + +# Import statements for member types + +import rosidl_parser.definition # noqa: E402, I100 + + +class Metaclass_Error(type): + """Metaclass of message 'Error'.""" + + _CREATE_ROS_MESSAGE = None + _CONVERT_FROM_PY = None + _CONVERT_TO_PY = None + _DESTROY_ROS_MESSAGE = None + _TYPE_SUPPORT = None + + __constants = { + } + + @classmethod + def __import_type_support__(cls): + try: + from rosidl_generator_py import import_type_support + module = import_type_support('unitree_go') + except ImportError: + import logging + import traceback + logger = logging.getLogger( + 'unitree_go.msg.Error') + logger.debug( + 'Failed to import needed modules for type support:\n' + + traceback.format_exc()) + else: + cls._CREATE_ROS_MESSAGE = module.create_ros_message_msg__msg__error + cls._CONVERT_FROM_PY = module.convert_from_py_msg__msg__error + cls._CONVERT_TO_PY = module.convert_to_py_msg__msg__error + cls._TYPE_SUPPORT = module.type_support_msg__msg__error + cls._DESTROY_ROS_MESSAGE = module.destroy_ros_message_msg__msg__error + + @classmethod + def __prepare__(cls, name, bases, **kwargs): + # list constant names here so that they appear in the help text of + # the message class under "Data and other attributes defined here:" + # as well as populate each message instance + return { + } + + +class Error(metaclass=Metaclass_Error): + """Message class 'Error'.""" + + __slots__ = [ + '_source', + '_state', + ] + + _fields_and_field_types = { + 'source': 'uint32', + 'state': 'uint32', + } + + SLOT_TYPES = ( + rosidl_parser.definition.BasicType('uint32'), # noqa: E501 + rosidl_parser.definition.BasicType('uint32'), # noqa: E501 + ) + + def __init__(self, **kwargs): + assert all('_' + key in self.__slots__ for key in kwargs.keys()), \ + 'Invalid arguments passed to constructor: %s' % \ + ', '.join(sorted(k for k in kwargs.keys() if '_' + k not in self.__slots__)) + self.source = kwargs.get('source', int()) + self.state = kwargs.get('state', int()) + + def __repr__(self): + typename = self.__class__.__module__.split('.') + typename.pop() + typename.append(self.__class__.__name__) + args = [] + for s, t in zip(self.__slots__, self.SLOT_TYPES): + field = getattr(self, s) + fieldstr = repr(field) + # We use Python array type for fields that can be directly stored + # in them, and "normal" sequences for everything else. If it is + # a type that we store in an array, strip off the 'array' portion. + if ( + isinstance(t, rosidl_parser.definition.AbstractSequence) and + isinstance(t.value_type, rosidl_parser.definition.BasicType) and + t.value_type.typename in ['float', 'double', 'int8', 'uint8', 'int16', 'uint16', 'int32', 'uint32', 'int64', 'uint64'] + ): + if len(field) == 0: + fieldstr = '[]' + else: + assert fieldstr.startswith('array(') + prefix = "array('X', " + suffix = ')' + fieldstr = fieldstr[len(prefix):-len(suffix)] + args.append(s[1:] + '=' + fieldstr) + return '%s(%s)' % ('.'.join(typename), ', '.join(args)) + + def __eq__(self, other): + if not isinstance(other, self.__class__): + return False + if self.source != other.source: + return False + if self.state != other.state: + return False + return True + + @classmethod + def get_fields_and_field_types(cls): + from copy import copy + return copy(cls._fields_and_field_types) + + @property + def source(self): + """Message field 'source'.""" + return self._source + + @source.setter + def source(self, value): + if __debug__: + assert \ + isinstance(value, int), \ + "The 'source' field must be of type 'int'" + assert value >= 0 and value < 4294967296, \ + "The 'source' field must be an unsigned integer in [0, 4294967295]" + self._source = value + + @property + def state(self): + """Message field 'state'.""" + return self._state + + @state.setter + def state(self, value): + if __debug__: + assert \ + isinstance(value, int), \ + "The 'state' field must be of type 'int'" + assert value >= 0 and value < 4294967296, \ + "The 'state' field must be an unsigned integer in [0, 4294967295]" + self._state = value diff --git a/python/unitree_go/msg/_error_s.c b/python/unitree_go/msg/_error_s.c new file mode 100644 index 00000000..4cafeba2 --- /dev/null +++ b/python/unitree_go/msg/_error_s.c @@ -0,0 +1,118 @@ +// generated from rosidl_generator_py/resource/_idl_support.c.em +// with input from unitree_go:msg/Error.idl +// generated code does not contain a copyright notice +#define NPY_NO_DEPRECATED_API NPY_1_7_API_VERSION +#include +#include +#ifndef _WIN32 +# pragma GCC diagnostic push +# pragma GCC diagnostic ignored "-Wunused-function" +#endif +#include "numpy/ndarrayobject.h" +#ifndef _WIN32 +# pragma GCC diagnostic pop +#endif +#include "rosidl_runtime_c/visibility_control.h" +#include "unitree_go/msg/detail/error__struct.h" +#include "unitree_go/msg/detail/error__functions.h" + + +ROSIDL_GENERATOR_C_EXPORT +bool unitree_go__msg__error__convert_from_py(PyObject * _pymsg, void * _ros_message) +{ + // check that the passed message is of the expected Python class + { + char full_classname_dest[28]; + { + char * class_name = NULL; + char * module_name = NULL; + { + PyObject * class_attr = PyObject_GetAttrString(_pymsg, "__class__"); + if (class_attr) { + PyObject * name_attr = PyObject_GetAttrString(class_attr, "__name__"); + if (name_attr) { + class_name = (char *)PyUnicode_1BYTE_DATA(name_attr); + Py_DECREF(name_attr); + } + PyObject * module_attr = PyObject_GetAttrString(class_attr, "__module__"); + if (module_attr) { + module_name = (char *)PyUnicode_1BYTE_DATA(module_attr); + Py_DECREF(module_attr); + } + Py_DECREF(class_attr); + } + } + if (!class_name || !module_name) { + return false; + } + snprintf(full_classname_dest, sizeof(full_classname_dest), "%s.%s", module_name, class_name); + } + assert(strncmp("unitree_go.msg._error.Error", full_classname_dest, 27) == 0); + } + unitree_go__msg__Error * ros_message = _ros_message; + { // source + PyObject * field = PyObject_GetAttrString(_pymsg, "source"); + if (!field) { + return false; + } + assert(PyLong_Check(field)); + ros_message->source = PyLong_AsUnsignedLong(field); + Py_DECREF(field); + } + { // state + PyObject * field = PyObject_GetAttrString(_pymsg, "state"); + if (!field) { + return false; + } + assert(PyLong_Check(field)); + ros_message->state = PyLong_AsUnsignedLong(field); + Py_DECREF(field); + } + + return true; +} + +ROSIDL_GENERATOR_C_EXPORT +PyObject * unitree_go__msg__error__convert_to_py(void * raw_ros_message) +{ + /* NOTE(esteve): Call constructor of Error */ + PyObject * _pymessage = NULL; + { + PyObject * pymessage_module = PyImport_ImportModule("unitree_go.msg._error"); + assert(pymessage_module); + PyObject * pymessage_class = PyObject_GetAttrString(pymessage_module, "Error"); + assert(pymessage_class); + Py_DECREF(pymessage_module); + _pymessage = PyObject_CallObject(pymessage_class, NULL); + Py_DECREF(pymessage_class); + if (!_pymessage) { + return NULL; + } + } + unitree_go__msg__Error * ros_message = (unitree_go__msg__Error *)raw_ros_message; + { // source + PyObject * field = NULL; + field = PyLong_FromUnsignedLong(ros_message->source); + { + int rc = PyObject_SetAttrString(_pymessage, "source", field); + Py_DECREF(field); + if (rc) { + return NULL; + } + } + } + { // state + PyObject * field = NULL; + field = PyLong_FromUnsignedLong(ros_message->state); + { + int rc = PyObject_SetAttrString(_pymessage, "state", field); + Py_DECREF(field); + if (rc) { + return NULL; + } + } + } + + // ownership of _pymessage is transferred to the caller + return _pymessage; +} diff --git a/python/unitree_go/msg/_go2_front_video_data.py b/python/unitree_go/msg/_go2_front_video_data.py new file mode 100644 index 00000000..81c366ce --- /dev/null +++ b/python/unitree_go/msg/_go2_front_video_data.py @@ -0,0 +1,231 @@ +# generated from rosidl_generator_py/resource/_idl.py.em +# with input from unitree_go:msg/Go2FrontVideoData.idl +# generated code does not contain a copyright notice + + +# Import statements for member types + +# Member 'video720p' +# Member 'video360p' +# Member 'video180p' +import array # noqa: E402, I100 + +import rosidl_parser.definition # noqa: E402, I100 + + +class Metaclass_Go2FrontVideoData(type): + """Metaclass of message 'Go2FrontVideoData'.""" + + _CREATE_ROS_MESSAGE = None + _CONVERT_FROM_PY = None + _CONVERT_TO_PY = None + _DESTROY_ROS_MESSAGE = None + _TYPE_SUPPORT = None + + __constants = { + } + + @classmethod + def __import_type_support__(cls): + try: + from rosidl_generator_py import import_type_support + module = import_type_support('unitree_go') + except ImportError: + import logging + import traceback + logger = logging.getLogger( + 'unitree_go.msg.Go2FrontVideoData') + logger.debug( + 'Failed to import needed modules for type support:\n' + + traceback.format_exc()) + else: + cls._CREATE_ROS_MESSAGE = module.create_ros_message_msg__msg__go2_front_video_data + cls._CONVERT_FROM_PY = module.convert_from_py_msg__msg__go2_front_video_data + cls._CONVERT_TO_PY = module.convert_to_py_msg__msg__go2_front_video_data + cls._TYPE_SUPPORT = module.type_support_msg__msg__go2_front_video_data + cls._DESTROY_ROS_MESSAGE = module.destroy_ros_message_msg__msg__go2_front_video_data + + @classmethod + def __prepare__(cls, name, bases, **kwargs): + # list constant names here so that they appear in the help text of + # the message class under "Data and other attributes defined here:" + # as well as populate each message instance + return { + } + + +class Go2FrontVideoData(metaclass=Metaclass_Go2FrontVideoData): + """Message class 'Go2FrontVideoData'.""" + + __slots__ = [ + '_time_frame', + '_video720p', + '_video360p', + '_video180p', + ] + + _fields_and_field_types = { + 'time_frame': 'uint64', + 'video720p': 'sequence', + 'video360p': 'sequence', + 'video180p': 'sequence', + } + + SLOT_TYPES = ( + rosidl_parser.definition.BasicType('uint64'), # noqa: E501 + rosidl_parser.definition.UnboundedSequence(rosidl_parser.definition.BasicType('uint8')), # noqa: E501 + rosidl_parser.definition.UnboundedSequence(rosidl_parser.definition.BasicType('uint8')), # noqa: E501 + rosidl_parser.definition.UnboundedSequence(rosidl_parser.definition.BasicType('uint8')), # noqa: E501 + ) + + def __init__(self, **kwargs): + assert all('_' + key in self.__slots__ for key in kwargs.keys()), \ + 'Invalid arguments passed to constructor: %s' % \ + ', '.join(sorted(k for k in kwargs.keys() if '_' + k not in self.__slots__)) + self.time_frame = kwargs.get('time_frame', int()) + self.video720p = array.array('B', kwargs.get('video720p', [])) + self.video360p = array.array('B', kwargs.get('video360p', [])) + self.video180p = array.array('B', kwargs.get('video180p', [])) + + def __repr__(self): + typename = self.__class__.__module__.split('.') + typename.pop() + typename.append(self.__class__.__name__) + args = [] + for s, t in zip(self.__slots__, self.SLOT_TYPES): + field = getattr(self, s) + fieldstr = repr(field) + # We use Python array type for fields that can be directly stored + # in them, and "normal" sequences for everything else. If it is + # a type that we store in an array, strip off the 'array' portion. + if ( + isinstance(t, rosidl_parser.definition.AbstractSequence) and + isinstance(t.value_type, rosidl_parser.definition.BasicType) and + t.value_type.typename in ['float', 'double', 'int8', 'uint8', 'int16', 'uint16', 'int32', 'uint32', 'int64', 'uint64'] + ): + if len(field) == 0: + fieldstr = '[]' + else: + assert fieldstr.startswith('array(') + prefix = "array('X', " + suffix = ')' + fieldstr = fieldstr[len(prefix):-len(suffix)] + args.append(s[1:] + '=' + fieldstr) + return '%s(%s)' % ('.'.join(typename), ', '.join(args)) + + def __eq__(self, other): + if not isinstance(other, self.__class__): + return False + if self.time_frame != other.time_frame: + return False + if self.video720p != other.video720p: + return False + if self.video360p != other.video360p: + return False + if self.video180p != other.video180p: + return False + return True + + @classmethod + def get_fields_and_field_types(cls): + from copy import copy + return copy(cls._fields_and_field_types) + + @property + def time_frame(self): + """Message field 'time_frame'.""" + return self._time_frame + + @time_frame.setter + def time_frame(self, value): + if __debug__: + assert \ + isinstance(value, int), \ + "The 'time_frame' field must be of type 'int'" + assert value >= 0 and value < 18446744073709551616, \ + "The 'time_frame' field must be an unsigned integer in [0, 18446744073709551615]" + self._time_frame = value + + @property + def video720p(self): + """Message field 'video720p'.""" + return self._video720p + + @video720p.setter + def video720p(self, value): + if isinstance(value, array.array): + assert value.typecode == 'B', \ + "The 'video720p' array.array() must have the type code of 'B'" + self._video720p = value + return + if __debug__: + from collections.abc import Sequence + from collections.abc import Set + from collections import UserList + from collections import UserString + assert \ + ((isinstance(value, Sequence) or + isinstance(value, Set) or + isinstance(value, UserList)) and + not isinstance(value, str) and + not isinstance(value, UserString) and + all(isinstance(v, int) for v in value) and + all(val >= 0 and val < 256 for val in value)), \ + "The 'video720p' field must be a set or sequence and each value of type 'int' and each unsigned integer in [0, 255]" + self._video720p = array.array('B', value) + + @property + def video360p(self): + """Message field 'video360p'.""" + return self._video360p + + @video360p.setter + def video360p(self, value): + if isinstance(value, array.array): + assert value.typecode == 'B', \ + "The 'video360p' array.array() must have the type code of 'B'" + self._video360p = value + return + if __debug__: + from collections.abc import Sequence + from collections.abc import Set + from collections import UserList + from collections import UserString + assert \ + ((isinstance(value, Sequence) or + isinstance(value, Set) or + isinstance(value, UserList)) and + not isinstance(value, str) and + not isinstance(value, UserString) and + all(isinstance(v, int) for v in value) and + all(val >= 0 and val < 256 for val in value)), \ + "The 'video360p' field must be a set or sequence and each value of type 'int' and each unsigned integer in [0, 255]" + self._video360p = array.array('B', value) + + @property + def video180p(self): + """Message field 'video180p'.""" + return self._video180p + + @video180p.setter + def video180p(self, value): + if isinstance(value, array.array): + assert value.typecode == 'B', \ + "The 'video180p' array.array() must have the type code of 'B'" + self._video180p = value + return + if __debug__: + from collections.abc import Sequence + from collections.abc import Set + from collections import UserList + from collections import UserString + assert \ + ((isinstance(value, Sequence) or + isinstance(value, Set) or + isinstance(value, UserList)) and + not isinstance(value, str) and + not isinstance(value, UserString) and + all(isinstance(v, int) for v in value) and + all(val >= 0 and val < 256 for val in value)), \ + "The 'video180p' field must be a set or sequence and each value of type 'int' and each unsigned integer in [0, 255]" + self._video180p = array.array('B', value) diff --git a/python/unitree_go/msg/_go2_front_video_data_s.c b/python/unitree_go/msg/_go2_front_video_data_s.c new file mode 100644 index 00000000..2492f400 --- /dev/null +++ b/python/unitree_go/msg/_go2_front_video_data_s.c @@ -0,0 +1,461 @@ +// generated from rosidl_generator_py/resource/_idl_support.c.em +// with input from unitree_go:msg/Go2FrontVideoData.idl +// generated code does not contain a copyright notice +#define NPY_NO_DEPRECATED_API NPY_1_7_API_VERSION +#include +#include +#ifndef _WIN32 +# pragma GCC diagnostic push +# pragma GCC diagnostic ignored "-Wunused-function" +#endif +#include "numpy/ndarrayobject.h" +#ifndef _WIN32 +# pragma GCC diagnostic pop +#endif +#include "rosidl_runtime_c/visibility_control.h" +#include "unitree_go/msg/detail/go2_front_video_data__struct.h" +#include "unitree_go/msg/detail/go2_front_video_data__functions.h" + +#include "rosidl_runtime_c/primitives_sequence.h" +#include "rosidl_runtime_c/primitives_sequence_functions.h" + + +ROSIDL_GENERATOR_C_EXPORT +bool unitree_go__msg__go2_front_video_data__convert_from_py(PyObject * _pymsg, void * _ros_message) +{ + // check that the passed message is of the expected Python class + { + char full_classname_dest[55]; + { + char * class_name = NULL; + char * module_name = NULL; + { + PyObject * class_attr = PyObject_GetAttrString(_pymsg, "__class__"); + if (class_attr) { + PyObject * name_attr = PyObject_GetAttrString(class_attr, "__name__"); + if (name_attr) { + class_name = (char *)PyUnicode_1BYTE_DATA(name_attr); + Py_DECREF(name_attr); + } + PyObject * module_attr = PyObject_GetAttrString(class_attr, "__module__"); + if (module_attr) { + module_name = (char *)PyUnicode_1BYTE_DATA(module_attr); + Py_DECREF(module_attr); + } + Py_DECREF(class_attr); + } + } + if (!class_name || !module_name) { + return false; + } + snprintf(full_classname_dest, sizeof(full_classname_dest), "%s.%s", module_name, class_name); + } + assert(strncmp("unitree_go.msg._go2_front_video_data.Go2FrontVideoData", full_classname_dest, 54) == 0); + } + unitree_go__msg__Go2FrontVideoData * ros_message = _ros_message; + { // time_frame + PyObject * field = PyObject_GetAttrString(_pymsg, "time_frame"); + if (!field) { + return false; + } + assert(PyLong_Check(field)); + ros_message->time_frame = PyLong_AsUnsignedLongLong(field); + Py_DECREF(field); + } + { // video720p + PyObject * field = PyObject_GetAttrString(_pymsg, "video720p"); + if (!field) { + return false; + } + if (PyObject_CheckBuffer(field)) { + // Optimization for converting arrays of primitives + Py_buffer view; + int rc = PyObject_GetBuffer(field, &view, PyBUF_SIMPLE); + if (rc < 0) { + Py_DECREF(field); + return false; + } + Py_ssize_t size = view.len / sizeof(uint8_t); + if (!rosidl_runtime_c__uint8__Sequence__init(&(ros_message->video720p), size)) { + PyErr_SetString(PyExc_RuntimeError, "unable to create uint8__Sequence ros_message"); + PyBuffer_Release(&view); + Py_DECREF(field); + return false; + } + uint8_t * dest = ros_message->video720p.data; + rc = PyBuffer_ToContiguous(dest, &view, view.len, 'C'); + if (rc < 0) { + PyBuffer_Release(&view); + Py_DECREF(field); + return false; + } + PyBuffer_Release(&view); + } else { + PyObject * seq_field = PySequence_Fast(field, "expected a sequence in 'video720p'"); + if (!seq_field) { + Py_DECREF(field); + return false; + } + Py_ssize_t size = PySequence_Size(field); + if (-1 == size) { + Py_DECREF(seq_field); + Py_DECREF(field); + return false; + } + if (!rosidl_runtime_c__uint8__Sequence__init(&(ros_message->video720p), size)) { + PyErr_SetString(PyExc_RuntimeError, "unable to create uint8__Sequence ros_message"); + Py_DECREF(seq_field); + Py_DECREF(field); + return false; + } + uint8_t * dest = ros_message->video720p.data; + for (Py_ssize_t i = 0; i < size; ++i) { + PyObject * item = PySequence_Fast_GET_ITEM(seq_field, i); + if (!item) { + Py_DECREF(seq_field); + Py_DECREF(field); + return false; + } + assert(PyLong_Check(item)); + uint8_t tmp = (uint8_t)PyLong_AsUnsignedLong(item); + + memcpy(&dest[i], &tmp, sizeof(uint8_t)); + } + Py_DECREF(seq_field); + } + Py_DECREF(field); + } + { // video360p + PyObject * field = PyObject_GetAttrString(_pymsg, "video360p"); + if (!field) { + return false; + } + if (PyObject_CheckBuffer(field)) { + // Optimization for converting arrays of primitives + Py_buffer view; + int rc = PyObject_GetBuffer(field, &view, PyBUF_SIMPLE); + if (rc < 0) { + Py_DECREF(field); + return false; + } + Py_ssize_t size = view.len / sizeof(uint8_t); + if (!rosidl_runtime_c__uint8__Sequence__init(&(ros_message->video360p), size)) { + PyErr_SetString(PyExc_RuntimeError, "unable to create uint8__Sequence ros_message"); + PyBuffer_Release(&view); + Py_DECREF(field); + return false; + } + uint8_t * dest = ros_message->video360p.data; + rc = PyBuffer_ToContiguous(dest, &view, view.len, 'C'); + if (rc < 0) { + PyBuffer_Release(&view); + Py_DECREF(field); + return false; + } + PyBuffer_Release(&view); + } else { + PyObject * seq_field = PySequence_Fast(field, "expected a sequence in 'video360p'"); + if (!seq_field) { + Py_DECREF(field); + return false; + } + Py_ssize_t size = PySequence_Size(field); + if (-1 == size) { + Py_DECREF(seq_field); + Py_DECREF(field); + return false; + } + if (!rosidl_runtime_c__uint8__Sequence__init(&(ros_message->video360p), size)) { + PyErr_SetString(PyExc_RuntimeError, "unable to create uint8__Sequence ros_message"); + Py_DECREF(seq_field); + Py_DECREF(field); + return false; + } + uint8_t * dest = ros_message->video360p.data; + for (Py_ssize_t i = 0; i < size; ++i) { + PyObject * item = PySequence_Fast_GET_ITEM(seq_field, i); + if (!item) { + Py_DECREF(seq_field); + Py_DECREF(field); + return false; + } + assert(PyLong_Check(item)); + uint8_t tmp = (uint8_t)PyLong_AsUnsignedLong(item); + + memcpy(&dest[i], &tmp, sizeof(uint8_t)); + } + Py_DECREF(seq_field); + } + Py_DECREF(field); + } + { // video180p + PyObject * field = PyObject_GetAttrString(_pymsg, "video180p"); + if (!field) { + return false; + } + if (PyObject_CheckBuffer(field)) { + // Optimization for converting arrays of primitives + Py_buffer view; + int rc = PyObject_GetBuffer(field, &view, PyBUF_SIMPLE); + if (rc < 0) { + Py_DECREF(field); + return false; + } + Py_ssize_t size = view.len / sizeof(uint8_t); + if (!rosidl_runtime_c__uint8__Sequence__init(&(ros_message->video180p), size)) { + PyErr_SetString(PyExc_RuntimeError, "unable to create uint8__Sequence ros_message"); + PyBuffer_Release(&view); + Py_DECREF(field); + return false; + } + uint8_t * dest = ros_message->video180p.data; + rc = PyBuffer_ToContiguous(dest, &view, view.len, 'C'); + if (rc < 0) { + PyBuffer_Release(&view); + Py_DECREF(field); + return false; + } + PyBuffer_Release(&view); + } else { + PyObject * seq_field = PySequence_Fast(field, "expected a sequence in 'video180p'"); + if (!seq_field) { + Py_DECREF(field); + return false; + } + Py_ssize_t size = PySequence_Size(field); + if (-1 == size) { + Py_DECREF(seq_field); + Py_DECREF(field); + return false; + } + if (!rosidl_runtime_c__uint8__Sequence__init(&(ros_message->video180p), size)) { + PyErr_SetString(PyExc_RuntimeError, "unable to create uint8__Sequence ros_message"); + Py_DECREF(seq_field); + Py_DECREF(field); + return false; + } + uint8_t * dest = ros_message->video180p.data; + for (Py_ssize_t i = 0; i < size; ++i) { + PyObject * item = PySequence_Fast_GET_ITEM(seq_field, i); + if (!item) { + Py_DECREF(seq_field); + Py_DECREF(field); + return false; + } + assert(PyLong_Check(item)); + uint8_t tmp = (uint8_t)PyLong_AsUnsignedLong(item); + + memcpy(&dest[i], &tmp, sizeof(uint8_t)); + } + Py_DECREF(seq_field); + } + Py_DECREF(field); + } + + return true; +} + +ROSIDL_GENERATOR_C_EXPORT +PyObject * unitree_go__msg__go2_front_video_data__convert_to_py(void * raw_ros_message) +{ + /* NOTE(esteve): Call constructor of Go2FrontVideoData */ + PyObject * _pymessage = NULL; + { + PyObject * pymessage_module = PyImport_ImportModule("unitree_go.msg._go2_front_video_data"); + assert(pymessage_module); + PyObject * pymessage_class = PyObject_GetAttrString(pymessage_module, "Go2FrontVideoData"); + assert(pymessage_class); + Py_DECREF(pymessage_module); + _pymessage = PyObject_CallObject(pymessage_class, NULL); + Py_DECREF(pymessage_class); + if (!_pymessage) { + return NULL; + } + } + unitree_go__msg__Go2FrontVideoData * ros_message = (unitree_go__msg__Go2FrontVideoData *)raw_ros_message; + { // time_frame + PyObject * field = NULL; + field = PyLong_FromUnsignedLongLong(ros_message->time_frame); + { + int rc = PyObject_SetAttrString(_pymessage, "time_frame", field); + Py_DECREF(field); + if (rc) { + return NULL; + } + } + } + { // video720p + PyObject * field = NULL; + field = PyObject_GetAttrString(_pymessage, "video720p"); + if (!field) { + return NULL; + } + assert(field->ob_type != NULL); + assert(field->ob_type->tp_name != NULL); + assert(strcmp(field->ob_type->tp_name, "array.array") == 0); + // ensure that itemsize matches the sizeof of the ROS message field + PyObject * itemsize_attr = PyObject_GetAttrString(field, "itemsize"); + assert(itemsize_attr != NULL); + size_t itemsize = PyLong_AsSize_t(itemsize_attr); + Py_DECREF(itemsize_attr); + if (itemsize != sizeof(uint8_t)) { + PyErr_SetString(PyExc_RuntimeError, "itemsize doesn't match expectation"); + Py_DECREF(field); + return NULL; + } + // clear the array, poor approach to remove potential default values + Py_ssize_t length = PyObject_Length(field); + if (-1 == length) { + Py_DECREF(field); + return NULL; + } + if (length > 0) { + PyObject * pop = PyObject_GetAttrString(field, "pop"); + assert(pop != NULL); + for (Py_ssize_t i = 0; i < length; ++i) { + PyObject * ret = PyObject_CallFunctionObjArgs(pop, NULL); + if (!ret) { + Py_DECREF(pop); + Py_DECREF(field); + return NULL; + } + Py_DECREF(ret); + } + Py_DECREF(pop); + } + if (ros_message->video720p.size > 0) { + // populating the array.array using the frombytes method + PyObject * frombytes = PyObject_GetAttrString(field, "frombytes"); + assert(frombytes != NULL); + uint8_t * src = &(ros_message->video720p.data[0]); + PyObject * data = PyBytes_FromStringAndSize((const char *)src, ros_message->video720p.size * sizeof(uint8_t)); + assert(data != NULL); + PyObject * ret = PyObject_CallFunctionObjArgs(frombytes, data, NULL); + Py_DECREF(data); + Py_DECREF(frombytes); + if (!ret) { + Py_DECREF(field); + return NULL; + } + Py_DECREF(ret); + } + Py_DECREF(field); + } + { // video360p + PyObject * field = NULL; + field = PyObject_GetAttrString(_pymessage, "video360p"); + if (!field) { + return NULL; + } + assert(field->ob_type != NULL); + assert(field->ob_type->tp_name != NULL); + assert(strcmp(field->ob_type->tp_name, "array.array") == 0); + // ensure that itemsize matches the sizeof of the ROS message field + PyObject * itemsize_attr = PyObject_GetAttrString(field, "itemsize"); + assert(itemsize_attr != NULL); + size_t itemsize = PyLong_AsSize_t(itemsize_attr); + Py_DECREF(itemsize_attr); + if (itemsize != sizeof(uint8_t)) { + PyErr_SetString(PyExc_RuntimeError, "itemsize doesn't match expectation"); + Py_DECREF(field); + return NULL; + } + // clear the array, poor approach to remove potential default values + Py_ssize_t length = PyObject_Length(field); + if (-1 == length) { + Py_DECREF(field); + return NULL; + } + if (length > 0) { + PyObject * pop = PyObject_GetAttrString(field, "pop"); + assert(pop != NULL); + for (Py_ssize_t i = 0; i < length; ++i) { + PyObject * ret = PyObject_CallFunctionObjArgs(pop, NULL); + if (!ret) { + Py_DECREF(pop); + Py_DECREF(field); + return NULL; + } + Py_DECREF(ret); + } + Py_DECREF(pop); + } + if (ros_message->video360p.size > 0) { + // populating the array.array using the frombytes method + PyObject * frombytes = PyObject_GetAttrString(field, "frombytes"); + assert(frombytes != NULL); + uint8_t * src = &(ros_message->video360p.data[0]); + PyObject * data = PyBytes_FromStringAndSize((const char *)src, ros_message->video360p.size * sizeof(uint8_t)); + assert(data != NULL); + PyObject * ret = PyObject_CallFunctionObjArgs(frombytes, data, NULL); + Py_DECREF(data); + Py_DECREF(frombytes); + if (!ret) { + Py_DECREF(field); + return NULL; + } + Py_DECREF(ret); + } + Py_DECREF(field); + } + { // video180p + PyObject * field = NULL; + field = PyObject_GetAttrString(_pymessage, "video180p"); + if (!field) { + return NULL; + } + assert(field->ob_type != NULL); + assert(field->ob_type->tp_name != NULL); + assert(strcmp(field->ob_type->tp_name, "array.array") == 0); + // ensure that itemsize matches the sizeof of the ROS message field + PyObject * itemsize_attr = PyObject_GetAttrString(field, "itemsize"); + assert(itemsize_attr != NULL); + size_t itemsize = PyLong_AsSize_t(itemsize_attr); + Py_DECREF(itemsize_attr); + if (itemsize != sizeof(uint8_t)) { + PyErr_SetString(PyExc_RuntimeError, "itemsize doesn't match expectation"); + Py_DECREF(field); + return NULL; + } + // clear the array, poor approach to remove potential default values + Py_ssize_t length = PyObject_Length(field); + if (-1 == length) { + Py_DECREF(field); + return NULL; + } + if (length > 0) { + PyObject * pop = PyObject_GetAttrString(field, "pop"); + assert(pop != NULL); + for (Py_ssize_t i = 0; i < length; ++i) { + PyObject * ret = PyObject_CallFunctionObjArgs(pop, NULL); + if (!ret) { + Py_DECREF(pop); + Py_DECREF(field); + return NULL; + } + Py_DECREF(ret); + } + Py_DECREF(pop); + } + if (ros_message->video180p.size > 0) { + // populating the array.array using the frombytes method + PyObject * frombytes = PyObject_GetAttrString(field, "frombytes"); + assert(frombytes != NULL); + uint8_t * src = &(ros_message->video180p.data[0]); + PyObject * data = PyBytes_FromStringAndSize((const char *)src, ros_message->video180p.size * sizeof(uint8_t)); + assert(data != NULL); + PyObject * ret = PyObject_CallFunctionObjArgs(frombytes, data, NULL); + Py_DECREF(data); + Py_DECREF(frombytes); + if (!ret) { + Py_DECREF(field); + return NULL; + } + Py_DECREF(ret); + } + Py_DECREF(field); + } + + // ownership of _pymessage is transferred to the caller + return _pymessage; +} diff --git a/python/unitree_go/msg/_height_map.py b/python/unitree_go/msg/_height_map.py new file mode 100644 index 00000000..935fedaf --- /dev/null +++ b/python/unitree_go/msg/_height_map.py @@ -0,0 +1,283 @@ +# generated from rosidl_generator_py/resource/_idl.py.em +# with input from unitree_go:msg/HeightMap.idl +# generated code does not contain a copyright notice + + +# Import statements for member types + +# Member 'data' +import array # noqa: E402, I100 + +# Member 'origin' +import numpy # noqa: E402, I100 + +import rosidl_parser.definition # noqa: E402, I100 + + +class Metaclass_HeightMap(type): + """Metaclass of message 'HeightMap'.""" + + _CREATE_ROS_MESSAGE = None + _CONVERT_FROM_PY = None + _CONVERT_TO_PY = None + _DESTROY_ROS_MESSAGE = None + _TYPE_SUPPORT = None + + __constants = { + } + + @classmethod + def __import_type_support__(cls): + try: + from rosidl_generator_py import import_type_support + module = import_type_support('unitree_go') + except ImportError: + import logging + import traceback + logger = logging.getLogger( + 'unitree_go.msg.HeightMap') + logger.debug( + 'Failed to import needed modules for type support:\n' + + traceback.format_exc()) + else: + cls._CREATE_ROS_MESSAGE = module.create_ros_message_msg__msg__height_map + cls._CONVERT_FROM_PY = module.convert_from_py_msg__msg__height_map + cls._CONVERT_TO_PY = module.convert_to_py_msg__msg__height_map + cls._TYPE_SUPPORT = module.type_support_msg__msg__height_map + cls._DESTROY_ROS_MESSAGE = module.destroy_ros_message_msg__msg__height_map + + @classmethod + def __prepare__(cls, name, bases, **kwargs): + # list constant names here so that they appear in the help text of + # the message class under "Data and other attributes defined here:" + # as well as populate each message instance + return { + } + + +class HeightMap(metaclass=Metaclass_HeightMap): + """Message class 'HeightMap'.""" + + __slots__ = [ + '_stamp', + '_frame_id', + '_resolution', + '_width', + '_height', + '_origin', + '_data', + ] + + _fields_and_field_types = { + 'stamp': 'double', + 'frame_id': 'string', + 'resolution': 'float', + 'width': 'uint32', + 'height': 'uint32', + 'origin': 'float[2]', + 'data': 'sequence', + } + + SLOT_TYPES = ( + rosidl_parser.definition.BasicType('double'), # noqa: E501 + rosidl_parser.definition.UnboundedString(), # noqa: E501 + rosidl_parser.definition.BasicType('float'), # noqa: E501 + rosidl_parser.definition.BasicType('uint32'), # noqa: E501 + rosidl_parser.definition.BasicType('uint32'), # noqa: E501 + rosidl_parser.definition.Array(rosidl_parser.definition.BasicType('float'), 2), # noqa: E501 + rosidl_parser.definition.UnboundedSequence(rosidl_parser.definition.BasicType('float')), # noqa: E501 + ) + + def __init__(self, **kwargs): + assert all('_' + key in self.__slots__ for key in kwargs.keys()), \ + 'Invalid arguments passed to constructor: %s' % \ + ', '.join(sorted(k for k in kwargs.keys() if '_' + k not in self.__slots__)) + self.stamp = kwargs.get('stamp', float()) + self.frame_id = kwargs.get('frame_id', str()) + self.resolution = kwargs.get('resolution', float()) + self.width = kwargs.get('width', int()) + self.height = kwargs.get('height', int()) + if 'origin' not in kwargs: + self.origin = numpy.zeros(2, dtype=numpy.float32) + else: + self.origin = numpy.array(kwargs.get('origin'), dtype=numpy.float32) + assert self.origin.shape == (2, ) + self.data = array.array('f', kwargs.get('data', [])) + + def __repr__(self): + typename = self.__class__.__module__.split('.') + typename.pop() + typename.append(self.__class__.__name__) + args = [] + for s, t in zip(self.__slots__, self.SLOT_TYPES): + field = getattr(self, s) + fieldstr = repr(field) + # We use Python array type for fields that can be directly stored + # in them, and "normal" sequences for everything else. If it is + # a type that we store in an array, strip off the 'array' portion. + if ( + isinstance(t, rosidl_parser.definition.AbstractSequence) and + isinstance(t.value_type, rosidl_parser.definition.BasicType) and + t.value_type.typename in ['float', 'double', 'int8', 'uint8', 'int16', 'uint16', 'int32', 'uint32', 'int64', 'uint64'] + ): + if len(field) == 0: + fieldstr = '[]' + else: + assert fieldstr.startswith('array(') + prefix = "array('X', " + suffix = ')' + fieldstr = fieldstr[len(prefix):-len(suffix)] + args.append(s[1:] + '=' + fieldstr) + return '%s(%s)' % ('.'.join(typename), ', '.join(args)) + + def __eq__(self, other): + if not isinstance(other, self.__class__): + return False + if self.stamp != other.stamp: + return False + if self.frame_id != other.frame_id: + return False + if self.resolution != other.resolution: + return False + if self.width != other.width: + return False + if self.height != other.height: + return False + if all(self.origin != other.origin): + return False + if self.data != other.data: + return False + return True + + @classmethod + def get_fields_and_field_types(cls): + from copy import copy + return copy(cls._fields_and_field_types) + + @property + def stamp(self): + """Message field 'stamp'.""" + return self._stamp + + @stamp.setter + def stamp(self, value): + if __debug__: + assert \ + isinstance(value, float), \ + "The 'stamp' field must be of type 'float'" + self._stamp = value + + @property + def frame_id(self): + """Message field 'frame_id'.""" + return self._frame_id + + @frame_id.setter + def frame_id(self, value): + if __debug__: + assert \ + isinstance(value, str), \ + "The 'frame_id' field must be of type 'str'" + self._frame_id = value + + @property + def resolution(self): + """Message field 'resolution'.""" + return self._resolution + + @resolution.setter + def resolution(self, value): + if __debug__: + assert \ + isinstance(value, float), \ + "The 'resolution' field must be of type 'float'" + self._resolution = value + + @property + def width(self): + """Message field 'width'.""" + return self._width + + @width.setter + def width(self, value): + if __debug__: + assert \ + isinstance(value, int), \ + "The 'width' field must be of type 'int'" + assert value >= 0 and value < 4294967296, \ + "The 'width' field must be an unsigned integer in [0, 4294967295]" + self._width = value + + @property + def height(self): + """Message field 'height'.""" + return self._height + + @height.setter + def height(self, value): + if __debug__: + assert \ + isinstance(value, int), \ + "The 'height' field must be of type 'int'" + assert value >= 0 and value < 4294967296, \ + "The 'height' field must be an unsigned integer in [0, 4294967295]" + self._height = value + + @property + def origin(self): + """Message field 'origin'.""" + return self._origin + + @origin.setter + def origin(self, value): + if isinstance(value, numpy.ndarray): + assert value.dtype == numpy.float32, \ + "The 'origin' numpy.ndarray() must have the dtype of 'numpy.float32'" + assert value.size == 2, \ + "The 'origin' numpy.ndarray() must have a size of 2" + self._origin = value + return + if __debug__: + from collections.abc import Sequence + from collections.abc import Set + from collections import UserList + from collections import UserString + assert \ + ((isinstance(value, Sequence) or + isinstance(value, Set) or + isinstance(value, UserList)) and + not isinstance(value, str) and + not isinstance(value, UserString) and + len(value) == 2 and + all(isinstance(v, float) for v in value) and + True), \ + "The 'origin' field must be a set or sequence with length 2 and each value of type 'float'" + self._origin = numpy.array(value, dtype=numpy.float32) + + @property + def data(self): + """Message field 'data'.""" + return self._data + + @data.setter + def data(self, value): + if isinstance(value, array.array): + assert value.typecode == 'f', \ + "The 'data' array.array() must have the type code of 'f'" + self._data = value + return + if __debug__: + from collections.abc import Sequence + from collections.abc import Set + from collections import UserList + from collections import UserString + assert \ + ((isinstance(value, Sequence) or + isinstance(value, Set) or + isinstance(value, UserList)) and + not isinstance(value, str) and + not isinstance(value, UserString) and + all(isinstance(v, float) for v in value) and + True), \ + "The 'data' field must be a set or sequence and each value of type 'float'" + self._data = array.array('f', value) diff --git a/python/unitree_go/msg/_height_map_s.c b/python/unitree_go/msg/_height_map_s.c new file mode 100644 index 00000000..c8b312c2 --- /dev/null +++ b/python/unitree_go/msg/_height_map_s.c @@ -0,0 +1,357 @@ +// generated from rosidl_generator_py/resource/_idl_support.c.em +// with input from unitree_go:msg/HeightMap.idl +// generated code does not contain a copyright notice +#define NPY_NO_DEPRECATED_API NPY_1_7_API_VERSION +#include +#include +#ifndef _WIN32 +# pragma GCC diagnostic push +# pragma GCC diagnostic ignored "-Wunused-function" +#endif +#include "numpy/ndarrayobject.h" +#ifndef _WIN32 +# pragma GCC diagnostic pop +#endif +#include "rosidl_runtime_c/visibility_control.h" +#include "unitree_go/msg/detail/height_map__struct.h" +#include "unitree_go/msg/detail/height_map__functions.h" + +#include "rosidl_runtime_c/string.h" +#include "rosidl_runtime_c/string_functions.h" + +#include "rosidl_runtime_c/primitives_sequence.h" +#include "rosidl_runtime_c/primitives_sequence_functions.h" + + +ROSIDL_GENERATOR_C_EXPORT +bool unitree_go__msg__height_map__convert_from_py(PyObject * _pymsg, void * _ros_message) +{ + // check that the passed message is of the expected Python class + { + char full_classname_dest[37]; + { + char * class_name = NULL; + char * module_name = NULL; + { + PyObject * class_attr = PyObject_GetAttrString(_pymsg, "__class__"); + if (class_attr) { + PyObject * name_attr = PyObject_GetAttrString(class_attr, "__name__"); + if (name_attr) { + class_name = (char *)PyUnicode_1BYTE_DATA(name_attr); + Py_DECREF(name_attr); + } + PyObject * module_attr = PyObject_GetAttrString(class_attr, "__module__"); + if (module_attr) { + module_name = (char *)PyUnicode_1BYTE_DATA(module_attr); + Py_DECREF(module_attr); + } + Py_DECREF(class_attr); + } + } + if (!class_name || !module_name) { + return false; + } + snprintf(full_classname_dest, sizeof(full_classname_dest), "%s.%s", module_name, class_name); + } + assert(strncmp("unitree_go.msg._height_map.HeightMap", full_classname_dest, 36) == 0); + } + unitree_go__msg__HeightMap * ros_message = _ros_message; + { // stamp + PyObject * field = PyObject_GetAttrString(_pymsg, "stamp"); + if (!field) { + return false; + } + assert(PyFloat_Check(field)); + ros_message->stamp = PyFloat_AS_DOUBLE(field); + Py_DECREF(field); + } + { // frame_id + PyObject * field = PyObject_GetAttrString(_pymsg, "frame_id"); + if (!field) { + return false; + } + assert(PyUnicode_Check(field)); + PyObject * encoded_field = PyUnicode_AsUTF8String(field); + if (!encoded_field) { + Py_DECREF(field); + return false; + } + rosidl_runtime_c__String__assign(&ros_message->frame_id, PyBytes_AS_STRING(encoded_field)); + Py_DECREF(encoded_field); + Py_DECREF(field); + } + { // resolution + PyObject * field = PyObject_GetAttrString(_pymsg, "resolution"); + if (!field) { + return false; + } + assert(PyFloat_Check(field)); + ros_message->resolution = (float)PyFloat_AS_DOUBLE(field); + Py_DECREF(field); + } + { // width + PyObject * field = PyObject_GetAttrString(_pymsg, "width"); + if (!field) { + return false; + } + assert(PyLong_Check(field)); + ros_message->width = PyLong_AsUnsignedLong(field); + Py_DECREF(field); + } + { // height + PyObject * field = PyObject_GetAttrString(_pymsg, "height"); + if (!field) { + return false; + } + assert(PyLong_Check(field)); + ros_message->height = PyLong_AsUnsignedLong(field); + Py_DECREF(field); + } + { // origin + PyObject * field = PyObject_GetAttrString(_pymsg, "origin"); + if (!field) { + return false; + } + { + // TODO(dirk-thomas) use a better way to check the type before casting + assert(field->ob_type != NULL); + assert(field->ob_type->tp_name != NULL); + assert(strcmp(field->ob_type->tp_name, "numpy.ndarray") == 0); + PyArrayObject * seq_field = (PyArrayObject *)field; + Py_INCREF(seq_field); + assert(PyArray_NDIM(seq_field) == 1); + assert(PyArray_TYPE(seq_field) == NPY_FLOAT32); + Py_ssize_t size = 2; + float * dest = ros_message->origin; + for (Py_ssize_t i = 0; i < size; ++i) { + float tmp = *(npy_float32 *)PyArray_GETPTR1(seq_field, i); + memcpy(&dest[i], &tmp, sizeof(float)); + } + Py_DECREF(seq_field); + } + Py_DECREF(field); + } + { // data + PyObject * field = PyObject_GetAttrString(_pymsg, "data"); + if (!field) { + return false; + } + if (PyObject_CheckBuffer(field)) { + // Optimization for converting arrays of primitives + Py_buffer view; + int rc = PyObject_GetBuffer(field, &view, PyBUF_SIMPLE); + if (rc < 0) { + Py_DECREF(field); + return false; + } + Py_ssize_t size = view.len / sizeof(float); + if (!rosidl_runtime_c__float__Sequence__init(&(ros_message->data), size)) { + PyErr_SetString(PyExc_RuntimeError, "unable to create float__Sequence ros_message"); + PyBuffer_Release(&view); + Py_DECREF(field); + return false; + } + float * dest = ros_message->data.data; + rc = PyBuffer_ToContiguous(dest, &view, view.len, 'C'); + if (rc < 0) { + PyBuffer_Release(&view); + Py_DECREF(field); + return false; + } + PyBuffer_Release(&view); + } else { + PyObject * seq_field = PySequence_Fast(field, "expected a sequence in 'data'"); + if (!seq_field) { + Py_DECREF(field); + return false; + } + Py_ssize_t size = PySequence_Size(field); + if (-1 == size) { + Py_DECREF(seq_field); + Py_DECREF(field); + return false; + } + if (!rosidl_runtime_c__float__Sequence__init(&(ros_message->data), size)) { + PyErr_SetString(PyExc_RuntimeError, "unable to create float__Sequence ros_message"); + Py_DECREF(seq_field); + Py_DECREF(field); + return false; + } + float * dest = ros_message->data.data; + for (Py_ssize_t i = 0; i < size; ++i) { + PyObject * item = PySequence_Fast_GET_ITEM(seq_field, i); + if (!item) { + Py_DECREF(seq_field); + Py_DECREF(field); + return false; + } + assert(PyFloat_Check(item)); + float tmp = (float)PyFloat_AS_DOUBLE(item); + memcpy(&dest[i], &tmp, sizeof(float)); + } + Py_DECREF(seq_field); + } + Py_DECREF(field); + } + + return true; +} + +ROSIDL_GENERATOR_C_EXPORT +PyObject * unitree_go__msg__height_map__convert_to_py(void * raw_ros_message) +{ + /* NOTE(esteve): Call constructor of HeightMap */ + PyObject * _pymessage = NULL; + { + PyObject * pymessage_module = PyImport_ImportModule("unitree_go.msg._height_map"); + assert(pymessage_module); + PyObject * pymessage_class = PyObject_GetAttrString(pymessage_module, "HeightMap"); + assert(pymessage_class); + Py_DECREF(pymessage_module); + _pymessage = PyObject_CallObject(pymessage_class, NULL); + Py_DECREF(pymessage_class); + if (!_pymessage) { + return NULL; + } + } + unitree_go__msg__HeightMap * ros_message = (unitree_go__msg__HeightMap *)raw_ros_message; + { // stamp + PyObject * field = NULL; + field = PyFloat_FromDouble(ros_message->stamp); + { + int rc = PyObject_SetAttrString(_pymessage, "stamp", field); + Py_DECREF(field); + if (rc) { + return NULL; + } + } + } + { // frame_id + PyObject * field = NULL; + field = PyUnicode_DecodeUTF8( + ros_message->frame_id.data, + strlen(ros_message->frame_id.data), + "replace"); + if (!field) { + return NULL; + } + { + int rc = PyObject_SetAttrString(_pymessage, "frame_id", field); + Py_DECREF(field); + if (rc) { + return NULL; + } + } + } + { // resolution + PyObject * field = NULL; + field = PyFloat_FromDouble(ros_message->resolution); + { + int rc = PyObject_SetAttrString(_pymessage, "resolution", field); + Py_DECREF(field); + if (rc) { + return NULL; + } + } + } + { // width + PyObject * field = NULL; + field = PyLong_FromUnsignedLong(ros_message->width); + { + int rc = PyObject_SetAttrString(_pymessage, "width", field); + Py_DECREF(field); + if (rc) { + return NULL; + } + } + } + { // height + PyObject * field = NULL; + field = PyLong_FromUnsignedLong(ros_message->height); + { + int rc = PyObject_SetAttrString(_pymessage, "height", field); + Py_DECREF(field); + if (rc) { + return NULL; + } + } + } + { // origin + PyObject * field = NULL; + field = PyObject_GetAttrString(_pymessage, "origin"); + if (!field) { + return NULL; + } + assert(field->ob_type != NULL); + assert(field->ob_type->tp_name != NULL); + assert(strcmp(field->ob_type->tp_name, "numpy.ndarray") == 0); + PyArrayObject * seq_field = (PyArrayObject *)field; + assert(PyArray_NDIM(seq_field) == 1); + assert(PyArray_TYPE(seq_field) == NPY_FLOAT32); + assert(sizeof(npy_float32) == sizeof(float)); + npy_float32 * dst = (npy_float32 *)PyArray_GETPTR1(seq_field, 0); + float * src = &(ros_message->origin[0]); + memcpy(dst, src, 2 * sizeof(float)); + Py_DECREF(field); + } + { // data + PyObject * field = NULL; + field = PyObject_GetAttrString(_pymessage, "data"); + if (!field) { + return NULL; + } + assert(field->ob_type != NULL); + assert(field->ob_type->tp_name != NULL); + assert(strcmp(field->ob_type->tp_name, "array.array") == 0); + // ensure that itemsize matches the sizeof of the ROS message field + PyObject * itemsize_attr = PyObject_GetAttrString(field, "itemsize"); + assert(itemsize_attr != NULL); + size_t itemsize = PyLong_AsSize_t(itemsize_attr); + Py_DECREF(itemsize_attr); + if (itemsize != sizeof(float)) { + PyErr_SetString(PyExc_RuntimeError, "itemsize doesn't match expectation"); + Py_DECREF(field); + return NULL; + } + // clear the array, poor approach to remove potential default values + Py_ssize_t length = PyObject_Length(field); + if (-1 == length) { + Py_DECREF(field); + return NULL; + } + if (length > 0) { + PyObject * pop = PyObject_GetAttrString(field, "pop"); + assert(pop != NULL); + for (Py_ssize_t i = 0; i < length; ++i) { + PyObject * ret = PyObject_CallFunctionObjArgs(pop, NULL); + if (!ret) { + Py_DECREF(pop); + Py_DECREF(field); + return NULL; + } + Py_DECREF(ret); + } + Py_DECREF(pop); + } + if (ros_message->data.size > 0) { + // populating the array.array using the frombytes method + PyObject * frombytes = PyObject_GetAttrString(field, "frombytes"); + assert(frombytes != NULL); + float * src = &(ros_message->data.data[0]); + PyObject * data = PyBytes_FromStringAndSize((const char *)src, ros_message->data.size * sizeof(float)); + assert(data != NULL); + PyObject * ret = PyObject_CallFunctionObjArgs(frombytes, data, NULL); + Py_DECREF(data); + Py_DECREF(frombytes); + if (!ret) { + Py_DECREF(field); + return NULL; + } + Py_DECREF(ret); + } + Py_DECREF(field); + } + + // ownership of _pymessage is transferred to the caller + return _pymessage; +} diff --git a/python/unitree_go/msg/_imu_state.py b/python/unitree_go/msg/_imu_state.py new file mode 100644 index 00000000..78f6d31a --- /dev/null +++ b/python/unitree_go/msg/_imu_state.py @@ -0,0 +1,294 @@ +# generated from rosidl_generator_py/resource/_idl.py.em +# with input from unitree_go:msg/IMUState.idl +# generated code does not contain a copyright notice + + +# Import statements for member types + +# Member 'quaternion' +# Member 'gyroscope' +# Member 'accelerometer' +# Member 'rpy' +import numpy # noqa: E402, I100 + +import rosidl_parser.definition # noqa: E402, I100 + + +class Metaclass_IMUState(type): + """Metaclass of message 'IMUState'.""" + + _CREATE_ROS_MESSAGE = None + _CONVERT_FROM_PY = None + _CONVERT_TO_PY = None + _DESTROY_ROS_MESSAGE = None + _TYPE_SUPPORT = None + + __constants = { + } + + @classmethod + def __import_type_support__(cls): + try: + from rosidl_generator_py import import_type_support + module = import_type_support('unitree_go') + except ImportError: + import logging + import traceback + logger = logging.getLogger( + 'unitree_go.msg.IMUState') + logger.debug( + 'Failed to import needed modules for type support:\n' + + traceback.format_exc()) + else: + cls._CREATE_ROS_MESSAGE = module.create_ros_message_msg__msg__imu_state + cls._CONVERT_FROM_PY = module.convert_from_py_msg__msg__imu_state + cls._CONVERT_TO_PY = module.convert_to_py_msg__msg__imu_state + cls._TYPE_SUPPORT = module.type_support_msg__msg__imu_state + cls._DESTROY_ROS_MESSAGE = module.destroy_ros_message_msg__msg__imu_state + + @classmethod + def __prepare__(cls, name, bases, **kwargs): + # list constant names here so that they appear in the help text of + # the message class under "Data and other attributes defined here:" + # as well as populate each message instance + return { + } + + +class IMUState(metaclass=Metaclass_IMUState): + """Message class 'IMUState'.""" + + __slots__ = [ + '_quaternion', + '_gyroscope', + '_accelerometer', + '_rpy', + '_temperature', + ] + + _fields_and_field_types = { + 'quaternion': 'float[4]', + 'gyroscope': 'float[3]', + 'accelerometer': 'float[3]', + 'rpy': 'float[3]', + 'temperature': 'int8', + } + + SLOT_TYPES = ( + rosidl_parser.definition.Array(rosidl_parser.definition.BasicType('float'), 4), # noqa: E501 + rosidl_parser.definition.Array(rosidl_parser.definition.BasicType('float'), 3), # noqa: E501 + rosidl_parser.definition.Array(rosidl_parser.definition.BasicType('float'), 3), # noqa: E501 + rosidl_parser.definition.Array(rosidl_parser.definition.BasicType('float'), 3), # noqa: E501 + rosidl_parser.definition.BasicType('int8'), # noqa: E501 + ) + + def __init__(self, **kwargs): + assert all('_' + key in self.__slots__ for key in kwargs.keys()), \ + 'Invalid arguments passed to constructor: %s' % \ + ', '.join(sorted(k for k in kwargs.keys() if '_' + k not in self.__slots__)) + if 'quaternion' not in kwargs: + self.quaternion = numpy.zeros(4, dtype=numpy.float32) + else: + self.quaternion = numpy.array(kwargs.get('quaternion'), dtype=numpy.float32) + assert self.quaternion.shape == (4, ) + if 'gyroscope' not in kwargs: + self.gyroscope = numpy.zeros(3, dtype=numpy.float32) + else: + self.gyroscope = numpy.array(kwargs.get('gyroscope'), dtype=numpy.float32) + assert self.gyroscope.shape == (3, ) + if 'accelerometer' not in kwargs: + self.accelerometer = numpy.zeros(3, dtype=numpy.float32) + else: + self.accelerometer = numpy.array(kwargs.get('accelerometer'), dtype=numpy.float32) + assert self.accelerometer.shape == (3, ) + if 'rpy' not in kwargs: + self.rpy = numpy.zeros(3, dtype=numpy.float32) + else: + self.rpy = numpy.array(kwargs.get('rpy'), dtype=numpy.float32) + assert self.rpy.shape == (3, ) + self.temperature = kwargs.get('temperature', int()) + + def __repr__(self): + typename = self.__class__.__module__.split('.') + typename.pop() + typename.append(self.__class__.__name__) + args = [] + for s, t in zip(self.__slots__, self.SLOT_TYPES): + field = getattr(self, s) + fieldstr = repr(field) + # We use Python array type for fields that can be directly stored + # in them, and "normal" sequences for everything else. If it is + # a type that we store in an array, strip off the 'array' portion. + if ( + isinstance(t, rosidl_parser.definition.AbstractSequence) and + isinstance(t.value_type, rosidl_parser.definition.BasicType) and + t.value_type.typename in ['float', 'double', 'int8', 'uint8', 'int16', 'uint16', 'int32', 'uint32', 'int64', 'uint64'] + ): + if len(field) == 0: + fieldstr = '[]' + else: + assert fieldstr.startswith('array(') + prefix = "array('X', " + suffix = ')' + fieldstr = fieldstr[len(prefix):-len(suffix)] + args.append(s[1:] + '=' + fieldstr) + return '%s(%s)' % ('.'.join(typename), ', '.join(args)) + + def __eq__(self, other): + if not isinstance(other, self.__class__): + return False + if all(self.quaternion != other.quaternion): + return False + if all(self.gyroscope != other.gyroscope): + return False + if all(self.accelerometer != other.accelerometer): + return False + if all(self.rpy != other.rpy): + return False + if self.temperature != other.temperature: + return False + return True + + @classmethod + def get_fields_and_field_types(cls): + from copy import copy + return copy(cls._fields_and_field_types) + + @property + def quaternion(self): + """Message field 'quaternion'.""" + return self._quaternion + + @quaternion.setter + def quaternion(self, value): + if isinstance(value, numpy.ndarray): + assert value.dtype == numpy.float32, \ + "The 'quaternion' numpy.ndarray() must have the dtype of 'numpy.float32'" + assert value.size == 4, \ + "The 'quaternion' numpy.ndarray() must have a size of 4" + self._quaternion = value + return + if __debug__: + from collections.abc import Sequence + from collections.abc import Set + from collections import UserList + from collections import UserString + assert \ + ((isinstance(value, Sequence) or + isinstance(value, Set) or + isinstance(value, UserList)) and + not isinstance(value, str) and + not isinstance(value, UserString) and + len(value) == 4 and + all(isinstance(v, float) for v in value) and + True), \ + "The 'quaternion' field must be a set or sequence with length 4 and each value of type 'float'" + self._quaternion = numpy.array(value, dtype=numpy.float32) + + @property + def gyroscope(self): + """Message field 'gyroscope'.""" + return self._gyroscope + + @gyroscope.setter + def gyroscope(self, value): + if isinstance(value, numpy.ndarray): + assert value.dtype == numpy.float32, \ + "The 'gyroscope' numpy.ndarray() must have the dtype of 'numpy.float32'" + assert value.size == 3, \ + "The 'gyroscope' numpy.ndarray() must have a size of 3" + self._gyroscope = value + return + if __debug__: + from collections.abc import Sequence + from collections.abc import Set + from collections import UserList + from collections import UserString + assert \ + ((isinstance(value, Sequence) or + isinstance(value, Set) or + isinstance(value, UserList)) and + not isinstance(value, str) and + not isinstance(value, UserString) and + len(value) == 3 and + all(isinstance(v, float) for v in value) and + True), \ + "The 'gyroscope' field must be a set or sequence with length 3 and each value of type 'float'" + self._gyroscope = numpy.array(value, dtype=numpy.float32) + + @property + def accelerometer(self): + """Message field 'accelerometer'.""" + return self._accelerometer + + @accelerometer.setter + def accelerometer(self, value): + if isinstance(value, numpy.ndarray): + assert value.dtype == numpy.float32, \ + "The 'accelerometer' numpy.ndarray() must have the dtype of 'numpy.float32'" + assert value.size == 3, \ + "The 'accelerometer' numpy.ndarray() must have a size of 3" + self._accelerometer = value + return + if __debug__: + from collections.abc import Sequence + from collections.abc import Set + from collections import UserList + from collections import UserString + assert \ + ((isinstance(value, Sequence) or + isinstance(value, Set) or + isinstance(value, UserList)) and + not isinstance(value, str) and + not isinstance(value, UserString) and + len(value) == 3 and + all(isinstance(v, float) for v in value) and + True), \ + "The 'accelerometer' field must be a set or sequence with length 3 and each value of type 'float'" + self._accelerometer = numpy.array(value, dtype=numpy.float32) + + @property + def rpy(self): + """Message field 'rpy'.""" + return self._rpy + + @rpy.setter + def rpy(self, value): + if isinstance(value, numpy.ndarray): + assert value.dtype == numpy.float32, \ + "The 'rpy' numpy.ndarray() must have the dtype of 'numpy.float32'" + assert value.size == 3, \ + "The 'rpy' numpy.ndarray() must have a size of 3" + self._rpy = value + return + if __debug__: + from collections.abc import Sequence + from collections.abc import Set + from collections import UserList + from collections import UserString + assert \ + ((isinstance(value, Sequence) or + isinstance(value, Set) or + isinstance(value, UserList)) and + not isinstance(value, str) and + not isinstance(value, UserString) and + len(value) == 3 and + all(isinstance(v, float) for v in value) and + True), \ + "The 'rpy' field must be a set or sequence with length 3 and each value of type 'float'" + self._rpy = numpy.array(value, dtype=numpy.float32) + + @property + def temperature(self): + """Message field 'temperature'.""" + return self._temperature + + @temperature.setter + def temperature(self, value): + if __debug__: + assert \ + isinstance(value, int), \ + "The 'temperature' field must be of type 'int'" + assert value >= -128 and value < 128, \ + "The 'temperature' field must be an integer in [-128, 127]" + self._temperature = value diff --git a/python/unitree_go/msg/_imu_state_s.c b/python/unitree_go/msg/_imu_state_s.c new file mode 100644 index 00000000..55cdeb77 --- /dev/null +++ b/python/unitree_go/msg/_imu_state_s.c @@ -0,0 +1,269 @@ +// generated from rosidl_generator_py/resource/_idl_support.c.em +// with input from unitree_go:msg/IMUState.idl +// generated code does not contain a copyright notice +#define NPY_NO_DEPRECATED_API NPY_1_7_API_VERSION +#include +#include +#ifndef _WIN32 +# pragma GCC diagnostic push +# pragma GCC diagnostic ignored "-Wunused-function" +#endif +#include "numpy/ndarrayobject.h" +#ifndef _WIN32 +# pragma GCC diagnostic pop +#endif +#include "rosidl_runtime_c/visibility_control.h" +#include "unitree_go/msg/detail/imu_state__struct.h" +#include "unitree_go/msg/detail/imu_state__functions.h" + +#include "rosidl_runtime_c/primitives_sequence.h" +#include "rosidl_runtime_c/primitives_sequence_functions.h" + + +ROSIDL_GENERATOR_C_EXPORT +bool unitree_go__msg__imu_state__convert_from_py(PyObject * _pymsg, void * _ros_message) +{ + // check that the passed message is of the expected Python class + { + char full_classname_dest[35]; + { + char * class_name = NULL; + char * module_name = NULL; + { + PyObject * class_attr = PyObject_GetAttrString(_pymsg, "__class__"); + if (class_attr) { + PyObject * name_attr = PyObject_GetAttrString(class_attr, "__name__"); + if (name_attr) { + class_name = (char *)PyUnicode_1BYTE_DATA(name_attr); + Py_DECREF(name_attr); + } + PyObject * module_attr = PyObject_GetAttrString(class_attr, "__module__"); + if (module_attr) { + module_name = (char *)PyUnicode_1BYTE_DATA(module_attr); + Py_DECREF(module_attr); + } + Py_DECREF(class_attr); + } + } + if (!class_name || !module_name) { + return false; + } + snprintf(full_classname_dest, sizeof(full_classname_dest), "%s.%s", module_name, class_name); + } + assert(strncmp("unitree_go.msg._imu_state.IMUState", full_classname_dest, 34) == 0); + } + unitree_go__msg__IMUState * ros_message = _ros_message; + { // quaternion + PyObject * field = PyObject_GetAttrString(_pymsg, "quaternion"); + if (!field) { + return false; + } + { + // TODO(dirk-thomas) use a better way to check the type before casting + assert(field->ob_type != NULL); + assert(field->ob_type->tp_name != NULL); + assert(strcmp(field->ob_type->tp_name, "numpy.ndarray") == 0); + PyArrayObject * seq_field = (PyArrayObject *)field; + Py_INCREF(seq_field); + assert(PyArray_NDIM(seq_field) == 1); + assert(PyArray_TYPE(seq_field) == NPY_FLOAT32); + Py_ssize_t size = 4; + float * dest = ros_message->quaternion; + for (Py_ssize_t i = 0; i < size; ++i) { + float tmp = *(npy_float32 *)PyArray_GETPTR1(seq_field, i); + memcpy(&dest[i], &tmp, sizeof(float)); + } + Py_DECREF(seq_field); + } + Py_DECREF(field); + } + { // gyroscope + PyObject * field = PyObject_GetAttrString(_pymsg, "gyroscope"); + if (!field) { + return false; + } + { + // TODO(dirk-thomas) use a better way to check the type before casting + assert(field->ob_type != NULL); + assert(field->ob_type->tp_name != NULL); + assert(strcmp(field->ob_type->tp_name, "numpy.ndarray") == 0); + PyArrayObject * seq_field = (PyArrayObject *)field; + Py_INCREF(seq_field); + assert(PyArray_NDIM(seq_field) == 1); + assert(PyArray_TYPE(seq_field) == NPY_FLOAT32); + Py_ssize_t size = 3; + float * dest = ros_message->gyroscope; + for (Py_ssize_t i = 0; i < size; ++i) { + float tmp = *(npy_float32 *)PyArray_GETPTR1(seq_field, i); + memcpy(&dest[i], &tmp, sizeof(float)); + } + Py_DECREF(seq_field); + } + Py_DECREF(field); + } + { // accelerometer + PyObject * field = PyObject_GetAttrString(_pymsg, "accelerometer"); + if (!field) { + return false; + } + { + // TODO(dirk-thomas) use a better way to check the type before casting + assert(field->ob_type != NULL); + assert(field->ob_type->tp_name != NULL); + assert(strcmp(field->ob_type->tp_name, "numpy.ndarray") == 0); + PyArrayObject * seq_field = (PyArrayObject *)field; + Py_INCREF(seq_field); + assert(PyArray_NDIM(seq_field) == 1); + assert(PyArray_TYPE(seq_field) == NPY_FLOAT32); + Py_ssize_t size = 3; + float * dest = ros_message->accelerometer; + for (Py_ssize_t i = 0; i < size; ++i) { + float tmp = *(npy_float32 *)PyArray_GETPTR1(seq_field, i); + memcpy(&dest[i], &tmp, sizeof(float)); + } + Py_DECREF(seq_field); + } + Py_DECREF(field); + } + { // rpy + PyObject * field = PyObject_GetAttrString(_pymsg, "rpy"); + if (!field) { + return false; + } + { + // TODO(dirk-thomas) use a better way to check the type before casting + assert(field->ob_type != NULL); + assert(field->ob_type->tp_name != NULL); + assert(strcmp(field->ob_type->tp_name, "numpy.ndarray") == 0); + PyArrayObject * seq_field = (PyArrayObject *)field; + Py_INCREF(seq_field); + assert(PyArray_NDIM(seq_field) == 1); + assert(PyArray_TYPE(seq_field) == NPY_FLOAT32); + Py_ssize_t size = 3; + float * dest = ros_message->rpy; + for (Py_ssize_t i = 0; i < size; ++i) { + float tmp = *(npy_float32 *)PyArray_GETPTR1(seq_field, i); + memcpy(&dest[i], &tmp, sizeof(float)); + } + Py_DECREF(seq_field); + } + Py_DECREF(field); + } + { // temperature + PyObject * field = PyObject_GetAttrString(_pymsg, "temperature"); + if (!field) { + return false; + } + assert(PyLong_Check(field)); + ros_message->temperature = (int8_t)PyLong_AsLong(field); + Py_DECREF(field); + } + + return true; +} + +ROSIDL_GENERATOR_C_EXPORT +PyObject * unitree_go__msg__imu_state__convert_to_py(void * raw_ros_message) +{ + /* NOTE(esteve): Call constructor of IMUState */ + PyObject * _pymessage = NULL; + { + PyObject * pymessage_module = PyImport_ImportModule("unitree_go.msg._imu_state"); + assert(pymessage_module); + PyObject * pymessage_class = PyObject_GetAttrString(pymessage_module, "IMUState"); + assert(pymessage_class); + Py_DECREF(pymessage_module); + _pymessage = PyObject_CallObject(pymessage_class, NULL); + Py_DECREF(pymessage_class); + if (!_pymessage) { + return NULL; + } + } + unitree_go__msg__IMUState * ros_message = (unitree_go__msg__IMUState *)raw_ros_message; + { // quaternion + PyObject * field = NULL; + field = PyObject_GetAttrString(_pymessage, "quaternion"); + if (!field) { + return NULL; + } + assert(field->ob_type != NULL); + assert(field->ob_type->tp_name != NULL); + assert(strcmp(field->ob_type->tp_name, "numpy.ndarray") == 0); + PyArrayObject * seq_field = (PyArrayObject *)field; + assert(PyArray_NDIM(seq_field) == 1); + assert(PyArray_TYPE(seq_field) == NPY_FLOAT32); + assert(sizeof(npy_float32) == sizeof(float)); + npy_float32 * dst = (npy_float32 *)PyArray_GETPTR1(seq_field, 0); + float * src = &(ros_message->quaternion[0]); + memcpy(dst, src, 4 * sizeof(float)); + Py_DECREF(field); + } + { // gyroscope + PyObject * field = NULL; + field = PyObject_GetAttrString(_pymessage, "gyroscope"); + if (!field) { + return NULL; + } + assert(field->ob_type != NULL); + assert(field->ob_type->tp_name != NULL); + assert(strcmp(field->ob_type->tp_name, "numpy.ndarray") == 0); + PyArrayObject * seq_field = (PyArrayObject *)field; + assert(PyArray_NDIM(seq_field) == 1); + assert(PyArray_TYPE(seq_field) == NPY_FLOAT32); + assert(sizeof(npy_float32) == sizeof(float)); + npy_float32 * dst = (npy_float32 *)PyArray_GETPTR1(seq_field, 0); + float * src = &(ros_message->gyroscope[0]); + memcpy(dst, src, 3 * sizeof(float)); + Py_DECREF(field); + } + { // accelerometer + PyObject * field = NULL; + field = PyObject_GetAttrString(_pymessage, "accelerometer"); + if (!field) { + return NULL; + } + assert(field->ob_type != NULL); + assert(field->ob_type->tp_name != NULL); + assert(strcmp(field->ob_type->tp_name, "numpy.ndarray") == 0); + PyArrayObject * seq_field = (PyArrayObject *)field; + assert(PyArray_NDIM(seq_field) == 1); + assert(PyArray_TYPE(seq_field) == NPY_FLOAT32); + assert(sizeof(npy_float32) == sizeof(float)); + npy_float32 * dst = (npy_float32 *)PyArray_GETPTR1(seq_field, 0); + float * src = &(ros_message->accelerometer[0]); + memcpy(dst, src, 3 * sizeof(float)); + Py_DECREF(field); + } + { // rpy + PyObject * field = NULL; + field = PyObject_GetAttrString(_pymessage, "rpy"); + if (!field) { + return NULL; + } + assert(field->ob_type != NULL); + assert(field->ob_type->tp_name != NULL); + assert(strcmp(field->ob_type->tp_name, "numpy.ndarray") == 0); + PyArrayObject * seq_field = (PyArrayObject *)field; + assert(PyArray_NDIM(seq_field) == 1); + assert(PyArray_TYPE(seq_field) == NPY_FLOAT32); + assert(sizeof(npy_float32) == sizeof(float)); + npy_float32 * dst = (npy_float32 *)PyArray_GETPTR1(seq_field, 0); + float * src = &(ros_message->rpy[0]); + memcpy(dst, src, 3 * sizeof(float)); + Py_DECREF(field); + } + { // temperature + PyObject * field = NULL; + field = PyLong_FromLong(ros_message->temperature); + { + int rc = PyObject_SetAttrString(_pymessage, "temperature", field); + Py_DECREF(field); + if (rc) { + return NULL; + } + } + } + + // ownership of _pymessage is transferred to the caller + return _pymessage; +} diff --git a/python/unitree_go/msg/_interface_config.py b/python/unitree_go/msg/_interface_config.py new file mode 100644 index 00000000..cee6940e --- /dev/null +++ b/python/unitree_go/msg/_interface_config.py @@ -0,0 +1,189 @@ +# generated from rosidl_generator_py/resource/_idl.py.em +# with input from unitree_go:msg/InterfaceConfig.idl +# generated code does not contain a copyright notice + + +# Import statements for member types + +# Member 'reserve' +import numpy # noqa: E402, I100 + +import rosidl_parser.definition # noqa: E402, I100 + + +class Metaclass_InterfaceConfig(type): + """Metaclass of message 'InterfaceConfig'.""" + + _CREATE_ROS_MESSAGE = None + _CONVERT_FROM_PY = None + _CONVERT_TO_PY = None + _DESTROY_ROS_MESSAGE = None + _TYPE_SUPPORT = None + + __constants = { + } + + @classmethod + def __import_type_support__(cls): + try: + from rosidl_generator_py import import_type_support + module = import_type_support('unitree_go') + except ImportError: + import logging + import traceback + logger = logging.getLogger( + 'unitree_go.msg.InterfaceConfig') + logger.debug( + 'Failed to import needed modules for type support:\n' + + traceback.format_exc()) + else: + cls._CREATE_ROS_MESSAGE = module.create_ros_message_msg__msg__interface_config + cls._CONVERT_FROM_PY = module.convert_from_py_msg__msg__interface_config + cls._CONVERT_TO_PY = module.convert_to_py_msg__msg__interface_config + cls._TYPE_SUPPORT = module.type_support_msg__msg__interface_config + cls._DESTROY_ROS_MESSAGE = module.destroy_ros_message_msg__msg__interface_config + + @classmethod + def __prepare__(cls, name, bases, **kwargs): + # list constant names here so that they appear in the help text of + # the message class under "Data and other attributes defined here:" + # as well as populate each message instance + return { + } + + +class InterfaceConfig(metaclass=Metaclass_InterfaceConfig): + """Message class 'InterfaceConfig'.""" + + __slots__ = [ + '_mode', + '_value', + '_reserve', + ] + + _fields_and_field_types = { + 'mode': 'uint8', + 'value': 'uint8', + 'reserve': 'uint8[2]', + } + + SLOT_TYPES = ( + rosidl_parser.definition.BasicType('uint8'), # noqa: E501 + rosidl_parser.definition.BasicType('uint8'), # noqa: E501 + rosidl_parser.definition.Array(rosidl_parser.definition.BasicType('uint8'), 2), # noqa: E501 + ) + + def __init__(self, **kwargs): + assert all('_' + key in self.__slots__ for key in kwargs.keys()), \ + 'Invalid arguments passed to constructor: %s' % \ + ', '.join(sorted(k for k in kwargs.keys() if '_' + k not in self.__slots__)) + self.mode = kwargs.get('mode', int()) + self.value = kwargs.get('value', int()) + if 'reserve' not in kwargs: + self.reserve = numpy.zeros(2, dtype=numpy.uint8) + else: + self.reserve = numpy.array(kwargs.get('reserve'), dtype=numpy.uint8) + assert self.reserve.shape == (2, ) + + def __repr__(self): + typename = self.__class__.__module__.split('.') + typename.pop() + typename.append(self.__class__.__name__) + args = [] + for s, t in zip(self.__slots__, self.SLOT_TYPES): + field = getattr(self, s) + fieldstr = repr(field) + # We use Python array type for fields that can be directly stored + # in them, and "normal" sequences for everything else. If it is + # a type that we store in an array, strip off the 'array' portion. + if ( + isinstance(t, rosidl_parser.definition.AbstractSequence) and + isinstance(t.value_type, rosidl_parser.definition.BasicType) and + t.value_type.typename in ['float', 'double', 'int8', 'uint8', 'int16', 'uint16', 'int32', 'uint32', 'int64', 'uint64'] + ): + if len(field) == 0: + fieldstr = '[]' + else: + assert fieldstr.startswith('array(') + prefix = "array('X', " + suffix = ')' + fieldstr = fieldstr[len(prefix):-len(suffix)] + args.append(s[1:] + '=' + fieldstr) + return '%s(%s)' % ('.'.join(typename), ', '.join(args)) + + def __eq__(self, other): + if not isinstance(other, self.__class__): + return False + if self.mode != other.mode: + return False + if self.value != other.value: + return False + if all(self.reserve != other.reserve): + return False + return True + + @classmethod + def get_fields_and_field_types(cls): + from copy import copy + return copy(cls._fields_and_field_types) + + @property + def mode(self): + """Message field 'mode'.""" + return self._mode + + @mode.setter + def mode(self, value): + if __debug__: + assert \ + isinstance(value, int), \ + "The 'mode' field must be of type 'int'" + assert value >= 0 and value < 256, \ + "The 'mode' field must be an unsigned integer in [0, 255]" + self._mode = value + + @property + def value(self): + """Message field 'value'.""" + return self._value + + @value.setter + def value(self, value): + if __debug__: + assert \ + isinstance(value, int), \ + "The 'value' field must be of type 'int'" + assert value >= 0 and value < 256, \ + "The 'value' field must be an unsigned integer in [0, 255]" + self._value = value + + @property + def reserve(self): + """Message field 'reserve'.""" + return self._reserve + + @reserve.setter + def reserve(self, value): + if isinstance(value, numpy.ndarray): + assert value.dtype == numpy.uint8, \ + "The 'reserve' numpy.ndarray() must have the dtype of 'numpy.uint8'" + assert value.size == 2, \ + "The 'reserve' numpy.ndarray() must have a size of 2" + self._reserve = value + return + if __debug__: + from collections.abc import Sequence + from collections.abc import Set + from collections import UserList + from collections import UserString + assert \ + ((isinstance(value, Sequence) or + isinstance(value, Set) or + isinstance(value, UserList)) and + not isinstance(value, str) and + not isinstance(value, UserString) and + len(value) == 2 and + all(isinstance(v, int) for v in value) and + all(val >= 0 and val < 256 for val in value)), \ + "The 'reserve' field must be a set or sequence with length 2 and each value of type 'int' and each unsigned integer in [0, 255]" + self._reserve = numpy.array(value, dtype=numpy.uint8) diff --git a/python/unitree_go/msg/_interface_config_s.c b/python/unitree_go/msg/_interface_config_s.c new file mode 100644 index 00000000..024a5c56 --- /dev/null +++ b/python/unitree_go/msg/_interface_config_s.c @@ -0,0 +1,163 @@ +// generated from rosidl_generator_py/resource/_idl_support.c.em +// with input from unitree_go:msg/InterfaceConfig.idl +// generated code does not contain a copyright notice +#define NPY_NO_DEPRECATED_API NPY_1_7_API_VERSION +#include +#include +#ifndef _WIN32 +# pragma GCC diagnostic push +# pragma GCC diagnostic ignored "-Wunused-function" +#endif +#include "numpy/ndarrayobject.h" +#ifndef _WIN32 +# pragma GCC diagnostic pop +#endif +#include "rosidl_runtime_c/visibility_control.h" +#include "unitree_go/msg/detail/interface_config__struct.h" +#include "unitree_go/msg/detail/interface_config__functions.h" + +#include "rosidl_runtime_c/primitives_sequence.h" +#include "rosidl_runtime_c/primitives_sequence_functions.h" + + +ROSIDL_GENERATOR_C_EXPORT +bool unitree_go__msg__interface_config__convert_from_py(PyObject * _pymsg, void * _ros_message) +{ + // check that the passed message is of the expected Python class + { + char full_classname_dest[49]; + { + char * class_name = NULL; + char * module_name = NULL; + { + PyObject * class_attr = PyObject_GetAttrString(_pymsg, "__class__"); + if (class_attr) { + PyObject * name_attr = PyObject_GetAttrString(class_attr, "__name__"); + if (name_attr) { + class_name = (char *)PyUnicode_1BYTE_DATA(name_attr); + Py_DECREF(name_attr); + } + PyObject * module_attr = PyObject_GetAttrString(class_attr, "__module__"); + if (module_attr) { + module_name = (char *)PyUnicode_1BYTE_DATA(module_attr); + Py_DECREF(module_attr); + } + Py_DECREF(class_attr); + } + } + if (!class_name || !module_name) { + return false; + } + snprintf(full_classname_dest, sizeof(full_classname_dest), "%s.%s", module_name, class_name); + } + assert(strncmp("unitree_go.msg._interface_config.InterfaceConfig", full_classname_dest, 48) == 0); + } + unitree_go__msg__InterfaceConfig * ros_message = _ros_message; + { // mode + PyObject * field = PyObject_GetAttrString(_pymsg, "mode"); + if (!field) { + return false; + } + assert(PyLong_Check(field)); + ros_message->mode = (uint8_t)PyLong_AsUnsignedLong(field); + Py_DECREF(field); + } + { // value + PyObject * field = PyObject_GetAttrString(_pymsg, "value"); + if (!field) { + return false; + } + assert(PyLong_Check(field)); + ros_message->value = (uint8_t)PyLong_AsUnsignedLong(field); + Py_DECREF(field); + } + { // reserve + PyObject * field = PyObject_GetAttrString(_pymsg, "reserve"); + if (!field) { + return false; + } + { + // TODO(dirk-thomas) use a better way to check the type before casting + assert(field->ob_type != NULL); + assert(field->ob_type->tp_name != NULL); + assert(strcmp(field->ob_type->tp_name, "numpy.ndarray") == 0); + PyArrayObject * seq_field = (PyArrayObject *)field; + Py_INCREF(seq_field); + assert(PyArray_NDIM(seq_field) == 1); + assert(PyArray_TYPE(seq_field) == NPY_UINT8); + Py_ssize_t size = 2; + uint8_t * dest = ros_message->reserve; + for (Py_ssize_t i = 0; i < size; ++i) { + uint8_t tmp = *(npy_uint8 *)PyArray_GETPTR1(seq_field, i); + memcpy(&dest[i], &tmp, sizeof(uint8_t)); + } + Py_DECREF(seq_field); + } + Py_DECREF(field); + } + + return true; +} + +ROSIDL_GENERATOR_C_EXPORT +PyObject * unitree_go__msg__interface_config__convert_to_py(void * raw_ros_message) +{ + /* NOTE(esteve): Call constructor of InterfaceConfig */ + PyObject * _pymessage = NULL; + { + PyObject * pymessage_module = PyImport_ImportModule("unitree_go.msg._interface_config"); + assert(pymessage_module); + PyObject * pymessage_class = PyObject_GetAttrString(pymessage_module, "InterfaceConfig"); + assert(pymessage_class); + Py_DECREF(pymessage_module); + _pymessage = PyObject_CallObject(pymessage_class, NULL); + Py_DECREF(pymessage_class); + if (!_pymessage) { + return NULL; + } + } + unitree_go__msg__InterfaceConfig * ros_message = (unitree_go__msg__InterfaceConfig *)raw_ros_message; + { // mode + PyObject * field = NULL; + field = PyLong_FromUnsignedLong(ros_message->mode); + { + int rc = PyObject_SetAttrString(_pymessage, "mode", field); + Py_DECREF(field); + if (rc) { + return NULL; + } + } + } + { // value + PyObject * field = NULL; + field = PyLong_FromUnsignedLong(ros_message->value); + { + int rc = PyObject_SetAttrString(_pymessage, "value", field); + Py_DECREF(field); + if (rc) { + return NULL; + } + } + } + { // reserve + PyObject * field = NULL; + field = PyObject_GetAttrString(_pymessage, "reserve"); + if (!field) { + return NULL; + } + assert(field->ob_type != NULL); + assert(field->ob_type->tp_name != NULL); + assert(strcmp(field->ob_type->tp_name, "numpy.ndarray") == 0); + PyArrayObject * seq_field = (PyArrayObject *)field; + assert(PyArray_NDIM(seq_field) == 1); + assert(PyArray_TYPE(seq_field) == NPY_UINT8); + assert(sizeof(npy_uint8) == sizeof(uint8_t)); + npy_uint8 * dst = (npy_uint8 *)PyArray_GETPTR1(seq_field, 0); + uint8_t * src = &(ros_message->reserve[0]); + memcpy(dst, src, 2 * sizeof(uint8_t)); + Py_DECREF(field); + } + + // ownership of _pymessage is transferred to the caller + return _pymessage; +} diff --git a/python/unitree_go/msg/_lidar_state.py b/python/unitree_go/msg/_lidar_state.py new file mode 100644 index 00000000..6b6da0b1 --- /dev/null +++ b/python/unitree_go/msg/_lidar_state.py @@ -0,0 +1,461 @@ +# generated from rosidl_generator_py/resource/_idl.py.em +# with input from unitree_go:msg/LidarState.idl +# generated code does not contain a copyright notice + + +# Import statements for member types + +# Member 'imu_rpy' +import numpy # noqa: E402, I100 + +import rosidl_parser.definition # noqa: E402, I100 + + +class Metaclass_LidarState(type): + """Metaclass of message 'LidarState'.""" + + _CREATE_ROS_MESSAGE = None + _CONVERT_FROM_PY = None + _CONVERT_TO_PY = None + _DESTROY_ROS_MESSAGE = None + _TYPE_SUPPORT = None + + __constants = { + } + + @classmethod + def __import_type_support__(cls): + try: + from rosidl_generator_py import import_type_support + module = import_type_support('unitree_go') + except ImportError: + import logging + import traceback + logger = logging.getLogger( + 'unitree_go.msg.LidarState') + logger.debug( + 'Failed to import needed modules for type support:\n' + + traceback.format_exc()) + else: + cls._CREATE_ROS_MESSAGE = module.create_ros_message_msg__msg__lidar_state + cls._CONVERT_FROM_PY = module.convert_from_py_msg__msg__lidar_state + cls._CONVERT_TO_PY = module.convert_to_py_msg__msg__lidar_state + cls._TYPE_SUPPORT = module.type_support_msg__msg__lidar_state + cls._DESTROY_ROS_MESSAGE = module.destroy_ros_message_msg__msg__lidar_state + + @classmethod + def __prepare__(cls, name, bases, **kwargs): + # list constant names here so that they appear in the help text of + # the message class under "Data and other attributes defined here:" + # as well as populate each message instance + return { + } + + +class LidarState(metaclass=Metaclass_LidarState): + """Message class 'LidarState'.""" + + __slots__ = [ + '_stamp', + '_firmware_version', + '_software_version', + '_sdk_version', + '_sys_rotation_speed', + '_com_rotation_speed', + '_error_state', + '_cloud_frequency', + '_cloud_packet_loss_rate', + '_cloud_size', + '_cloud_scan_num', + '_imu_frequency', + '_imu_packet_loss_rate', + '_imu_rpy', + '_serial_recv_stamp', + '_serial_buffer_size', + '_serial_buffer_read', + ] + + _fields_and_field_types = { + 'stamp': 'double', + 'firmware_version': 'string', + 'software_version': 'string', + 'sdk_version': 'string', + 'sys_rotation_speed': 'float', + 'com_rotation_speed': 'float', + 'error_state': 'uint8', + 'cloud_frequency': 'float', + 'cloud_packet_loss_rate': 'float', + 'cloud_size': 'uint32', + 'cloud_scan_num': 'uint32', + 'imu_frequency': 'float', + 'imu_packet_loss_rate': 'float', + 'imu_rpy': 'float[3]', + 'serial_recv_stamp': 'double', + 'serial_buffer_size': 'uint32', + 'serial_buffer_read': 'uint32', + } + + SLOT_TYPES = ( + rosidl_parser.definition.BasicType('double'), # noqa: E501 + rosidl_parser.definition.UnboundedString(), # noqa: E501 + rosidl_parser.definition.UnboundedString(), # noqa: E501 + rosidl_parser.definition.UnboundedString(), # noqa: E501 + rosidl_parser.definition.BasicType('float'), # noqa: E501 + rosidl_parser.definition.BasicType('float'), # noqa: E501 + rosidl_parser.definition.BasicType('uint8'), # noqa: E501 + rosidl_parser.definition.BasicType('float'), # noqa: E501 + rosidl_parser.definition.BasicType('float'), # noqa: E501 + rosidl_parser.definition.BasicType('uint32'), # noqa: E501 + rosidl_parser.definition.BasicType('uint32'), # noqa: E501 + rosidl_parser.definition.BasicType('float'), # noqa: E501 + rosidl_parser.definition.BasicType('float'), # noqa: E501 + rosidl_parser.definition.Array(rosidl_parser.definition.BasicType('float'), 3), # noqa: E501 + rosidl_parser.definition.BasicType('double'), # noqa: E501 + rosidl_parser.definition.BasicType('uint32'), # noqa: E501 + rosidl_parser.definition.BasicType('uint32'), # noqa: E501 + ) + + def __init__(self, **kwargs): + assert all('_' + key in self.__slots__ for key in kwargs.keys()), \ + 'Invalid arguments passed to constructor: %s' % \ + ', '.join(sorted(k for k in kwargs.keys() if '_' + k not in self.__slots__)) + self.stamp = kwargs.get('stamp', float()) + self.firmware_version = kwargs.get('firmware_version', str()) + self.software_version = kwargs.get('software_version', str()) + self.sdk_version = kwargs.get('sdk_version', str()) + self.sys_rotation_speed = kwargs.get('sys_rotation_speed', float()) + self.com_rotation_speed = kwargs.get('com_rotation_speed', float()) + self.error_state = kwargs.get('error_state', int()) + self.cloud_frequency = kwargs.get('cloud_frequency', float()) + self.cloud_packet_loss_rate = kwargs.get('cloud_packet_loss_rate', float()) + self.cloud_size = kwargs.get('cloud_size', int()) + self.cloud_scan_num = kwargs.get('cloud_scan_num', int()) + self.imu_frequency = kwargs.get('imu_frequency', float()) + self.imu_packet_loss_rate = kwargs.get('imu_packet_loss_rate', float()) + if 'imu_rpy' not in kwargs: + self.imu_rpy = numpy.zeros(3, dtype=numpy.float32) + else: + self.imu_rpy = numpy.array(kwargs.get('imu_rpy'), dtype=numpy.float32) + assert self.imu_rpy.shape == (3, ) + self.serial_recv_stamp = kwargs.get('serial_recv_stamp', float()) + self.serial_buffer_size = kwargs.get('serial_buffer_size', int()) + self.serial_buffer_read = kwargs.get('serial_buffer_read', int()) + + def __repr__(self): + typename = self.__class__.__module__.split('.') + typename.pop() + typename.append(self.__class__.__name__) + args = [] + for s, t in zip(self.__slots__, self.SLOT_TYPES): + field = getattr(self, s) + fieldstr = repr(field) + # We use Python array type for fields that can be directly stored + # in them, and "normal" sequences for everything else. If it is + # a type that we store in an array, strip off the 'array' portion. + if ( + isinstance(t, rosidl_parser.definition.AbstractSequence) and + isinstance(t.value_type, rosidl_parser.definition.BasicType) and + t.value_type.typename in ['float', 'double', 'int8', 'uint8', 'int16', 'uint16', 'int32', 'uint32', 'int64', 'uint64'] + ): + if len(field) == 0: + fieldstr = '[]' + else: + assert fieldstr.startswith('array(') + prefix = "array('X', " + suffix = ')' + fieldstr = fieldstr[len(prefix):-len(suffix)] + args.append(s[1:] + '=' + fieldstr) + return '%s(%s)' % ('.'.join(typename), ', '.join(args)) + + def __eq__(self, other): + if not isinstance(other, self.__class__): + return False + if self.stamp != other.stamp: + return False + if self.firmware_version != other.firmware_version: + return False + if self.software_version != other.software_version: + return False + if self.sdk_version != other.sdk_version: + return False + if self.sys_rotation_speed != other.sys_rotation_speed: + return False + if self.com_rotation_speed != other.com_rotation_speed: + return False + if self.error_state != other.error_state: + return False + if self.cloud_frequency != other.cloud_frequency: + return False + if self.cloud_packet_loss_rate != other.cloud_packet_loss_rate: + return False + if self.cloud_size != other.cloud_size: + return False + if self.cloud_scan_num != other.cloud_scan_num: + return False + if self.imu_frequency != other.imu_frequency: + return False + if self.imu_packet_loss_rate != other.imu_packet_loss_rate: + return False + if all(self.imu_rpy != other.imu_rpy): + return False + if self.serial_recv_stamp != other.serial_recv_stamp: + return False + if self.serial_buffer_size != other.serial_buffer_size: + return False + if self.serial_buffer_read != other.serial_buffer_read: + return False + return True + + @classmethod + def get_fields_and_field_types(cls): + from copy import copy + return copy(cls._fields_and_field_types) + + @property + def stamp(self): + """Message field 'stamp'.""" + return self._stamp + + @stamp.setter + def stamp(self, value): + if __debug__: + assert \ + isinstance(value, float), \ + "The 'stamp' field must be of type 'float'" + self._stamp = value + + @property + def firmware_version(self): + """Message field 'firmware_version'.""" + return self._firmware_version + + @firmware_version.setter + def firmware_version(self, value): + if __debug__: + assert \ + isinstance(value, str), \ + "The 'firmware_version' field must be of type 'str'" + self._firmware_version = value + + @property + def software_version(self): + """Message field 'software_version'.""" + return self._software_version + + @software_version.setter + def software_version(self, value): + if __debug__: + assert \ + isinstance(value, str), \ + "The 'software_version' field must be of type 'str'" + self._software_version = value + + @property + def sdk_version(self): + """Message field 'sdk_version'.""" + return self._sdk_version + + @sdk_version.setter + def sdk_version(self, value): + if __debug__: + assert \ + isinstance(value, str), \ + "The 'sdk_version' field must be of type 'str'" + self._sdk_version = value + + @property + def sys_rotation_speed(self): + """Message field 'sys_rotation_speed'.""" + return self._sys_rotation_speed + + @sys_rotation_speed.setter + def sys_rotation_speed(self, value): + if __debug__: + assert \ + isinstance(value, float), \ + "The 'sys_rotation_speed' field must be of type 'float'" + self._sys_rotation_speed = value + + @property + def com_rotation_speed(self): + """Message field 'com_rotation_speed'.""" + return self._com_rotation_speed + + @com_rotation_speed.setter + def com_rotation_speed(self, value): + if __debug__: + assert \ + isinstance(value, float), \ + "The 'com_rotation_speed' field must be of type 'float'" + self._com_rotation_speed = value + + @property + def error_state(self): + """Message field 'error_state'.""" + return self._error_state + + @error_state.setter + def error_state(self, value): + if __debug__: + assert \ + isinstance(value, int), \ + "The 'error_state' field must be of type 'int'" + assert value >= 0 and value < 256, \ + "The 'error_state' field must be an unsigned integer in [0, 255]" + self._error_state = value + + @property + def cloud_frequency(self): + """Message field 'cloud_frequency'.""" + return self._cloud_frequency + + @cloud_frequency.setter + def cloud_frequency(self, value): + if __debug__: + assert \ + isinstance(value, float), \ + "The 'cloud_frequency' field must be of type 'float'" + self._cloud_frequency = value + + @property + def cloud_packet_loss_rate(self): + """Message field 'cloud_packet_loss_rate'.""" + return self._cloud_packet_loss_rate + + @cloud_packet_loss_rate.setter + def cloud_packet_loss_rate(self, value): + if __debug__: + assert \ + isinstance(value, float), \ + "The 'cloud_packet_loss_rate' field must be of type 'float'" + self._cloud_packet_loss_rate = value + + @property + def cloud_size(self): + """Message field 'cloud_size'.""" + return self._cloud_size + + @cloud_size.setter + def cloud_size(self, value): + if __debug__: + assert \ + isinstance(value, int), \ + "The 'cloud_size' field must be of type 'int'" + assert value >= 0 and value < 4294967296, \ + "The 'cloud_size' field must be an unsigned integer in [0, 4294967295]" + self._cloud_size = value + + @property + def cloud_scan_num(self): + """Message field 'cloud_scan_num'.""" + return self._cloud_scan_num + + @cloud_scan_num.setter + def cloud_scan_num(self, value): + if __debug__: + assert \ + isinstance(value, int), \ + "The 'cloud_scan_num' field must be of type 'int'" + assert value >= 0 and value < 4294967296, \ + "The 'cloud_scan_num' field must be an unsigned integer in [0, 4294967295]" + self._cloud_scan_num = value + + @property + def imu_frequency(self): + """Message field 'imu_frequency'.""" + return self._imu_frequency + + @imu_frequency.setter + def imu_frequency(self, value): + if __debug__: + assert \ + isinstance(value, float), \ + "The 'imu_frequency' field must be of type 'float'" + self._imu_frequency = value + + @property + def imu_packet_loss_rate(self): + """Message field 'imu_packet_loss_rate'.""" + return self._imu_packet_loss_rate + + @imu_packet_loss_rate.setter + def imu_packet_loss_rate(self, value): + if __debug__: + assert \ + isinstance(value, float), \ + "The 'imu_packet_loss_rate' field must be of type 'float'" + self._imu_packet_loss_rate = value + + @property + def imu_rpy(self): + """Message field 'imu_rpy'.""" + return self._imu_rpy + + @imu_rpy.setter + def imu_rpy(self, value): + if isinstance(value, numpy.ndarray): + assert value.dtype == numpy.float32, \ + "The 'imu_rpy' numpy.ndarray() must have the dtype of 'numpy.float32'" + assert value.size == 3, \ + "The 'imu_rpy' numpy.ndarray() must have a size of 3" + self._imu_rpy = value + return + if __debug__: + from collections.abc import Sequence + from collections.abc import Set + from collections import UserList + from collections import UserString + assert \ + ((isinstance(value, Sequence) or + isinstance(value, Set) or + isinstance(value, UserList)) and + not isinstance(value, str) and + not isinstance(value, UserString) and + len(value) == 3 and + all(isinstance(v, float) for v in value) and + True), \ + "The 'imu_rpy' field must be a set or sequence with length 3 and each value of type 'float'" + self._imu_rpy = numpy.array(value, dtype=numpy.float32) + + @property + def serial_recv_stamp(self): + """Message field 'serial_recv_stamp'.""" + return self._serial_recv_stamp + + @serial_recv_stamp.setter + def serial_recv_stamp(self, value): + if __debug__: + assert \ + isinstance(value, float), \ + "The 'serial_recv_stamp' field must be of type 'float'" + self._serial_recv_stamp = value + + @property + def serial_buffer_size(self): + """Message field 'serial_buffer_size'.""" + return self._serial_buffer_size + + @serial_buffer_size.setter + def serial_buffer_size(self, value): + if __debug__: + assert \ + isinstance(value, int), \ + "The 'serial_buffer_size' field must be of type 'int'" + assert value >= 0 and value < 4294967296, \ + "The 'serial_buffer_size' field must be an unsigned integer in [0, 4294967295]" + self._serial_buffer_size = value + + @property + def serial_buffer_read(self): + """Message field 'serial_buffer_read'.""" + return self._serial_buffer_read + + @serial_buffer_read.setter + def serial_buffer_read(self, value): + if __debug__: + assert \ + isinstance(value, int), \ + "The 'serial_buffer_read' field must be of type 'int'" + assert value >= 0 and value < 4294967296, \ + "The 'serial_buffer_read' field must be an unsigned integer in [0, 4294967295]" + self._serial_buffer_read = value diff --git a/python/unitree_go/msg/_lidar_state_s.c b/python/unitree_go/msg/_lidar_state_s.c new file mode 100644 index 00000000..fe949673 --- /dev/null +++ b/python/unitree_go/msg/_lidar_state_s.c @@ -0,0 +1,482 @@ +// generated from rosidl_generator_py/resource/_idl_support.c.em +// with input from unitree_go:msg/LidarState.idl +// generated code does not contain a copyright notice +#define NPY_NO_DEPRECATED_API NPY_1_7_API_VERSION +#include +#include +#ifndef _WIN32 +# pragma GCC diagnostic push +# pragma GCC diagnostic ignored "-Wunused-function" +#endif +#include "numpy/ndarrayobject.h" +#ifndef _WIN32 +# pragma GCC diagnostic pop +#endif +#include "rosidl_runtime_c/visibility_control.h" +#include "unitree_go/msg/detail/lidar_state__struct.h" +#include "unitree_go/msg/detail/lidar_state__functions.h" + +#include "rosidl_runtime_c/string.h" +#include "rosidl_runtime_c/string_functions.h" + +#include "rosidl_runtime_c/primitives_sequence.h" +#include "rosidl_runtime_c/primitives_sequence_functions.h" + + +ROSIDL_GENERATOR_C_EXPORT +bool unitree_go__msg__lidar_state__convert_from_py(PyObject * _pymsg, void * _ros_message) +{ + // check that the passed message is of the expected Python class + { + char full_classname_dest[39]; + { + char * class_name = NULL; + char * module_name = NULL; + { + PyObject * class_attr = PyObject_GetAttrString(_pymsg, "__class__"); + if (class_attr) { + PyObject * name_attr = PyObject_GetAttrString(class_attr, "__name__"); + if (name_attr) { + class_name = (char *)PyUnicode_1BYTE_DATA(name_attr); + Py_DECREF(name_attr); + } + PyObject * module_attr = PyObject_GetAttrString(class_attr, "__module__"); + if (module_attr) { + module_name = (char *)PyUnicode_1BYTE_DATA(module_attr); + Py_DECREF(module_attr); + } + Py_DECREF(class_attr); + } + } + if (!class_name || !module_name) { + return false; + } + snprintf(full_classname_dest, sizeof(full_classname_dest), "%s.%s", module_name, class_name); + } + assert(strncmp("unitree_go.msg._lidar_state.LidarState", full_classname_dest, 38) == 0); + } + unitree_go__msg__LidarState * ros_message = _ros_message; + { // stamp + PyObject * field = PyObject_GetAttrString(_pymsg, "stamp"); + if (!field) { + return false; + } + assert(PyFloat_Check(field)); + ros_message->stamp = PyFloat_AS_DOUBLE(field); + Py_DECREF(field); + } + { // firmware_version + PyObject * field = PyObject_GetAttrString(_pymsg, "firmware_version"); + if (!field) { + return false; + } + assert(PyUnicode_Check(field)); + PyObject * encoded_field = PyUnicode_AsUTF8String(field); + if (!encoded_field) { + Py_DECREF(field); + return false; + } + rosidl_runtime_c__String__assign(&ros_message->firmware_version, PyBytes_AS_STRING(encoded_field)); + Py_DECREF(encoded_field); + Py_DECREF(field); + } + { // software_version + PyObject * field = PyObject_GetAttrString(_pymsg, "software_version"); + if (!field) { + return false; + } + assert(PyUnicode_Check(field)); + PyObject * encoded_field = PyUnicode_AsUTF8String(field); + if (!encoded_field) { + Py_DECREF(field); + return false; + } + rosidl_runtime_c__String__assign(&ros_message->software_version, PyBytes_AS_STRING(encoded_field)); + Py_DECREF(encoded_field); + Py_DECREF(field); + } + { // sdk_version + PyObject * field = PyObject_GetAttrString(_pymsg, "sdk_version"); + if (!field) { + return false; + } + assert(PyUnicode_Check(field)); + PyObject * encoded_field = PyUnicode_AsUTF8String(field); + if (!encoded_field) { + Py_DECREF(field); + return false; + } + rosidl_runtime_c__String__assign(&ros_message->sdk_version, PyBytes_AS_STRING(encoded_field)); + Py_DECREF(encoded_field); + Py_DECREF(field); + } + { // sys_rotation_speed + PyObject * field = PyObject_GetAttrString(_pymsg, "sys_rotation_speed"); + if (!field) { + return false; + } + assert(PyFloat_Check(field)); + ros_message->sys_rotation_speed = (float)PyFloat_AS_DOUBLE(field); + Py_DECREF(field); + } + { // com_rotation_speed + PyObject * field = PyObject_GetAttrString(_pymsg, "com_rotation_speed"); + if (!field) { + return false; + } + assert(PyFloat_Check(field)); + ros_message->com_rotation_speed = (float)PyFloat_AS_DOUBLE(field); + Py_DECREF(field); + } + { // error_state + PyObject * field = PyObject_GetAttrString(_pymsg, "error_state"); + if (!field) { + return false; + } + assert(PyLong_Check(field)); + ros_message->error_state = (uint8_t)PyLong_AsUnsignedLong(field); + Py_DECREF(field); + } + { // cloud_frequency + PyObject * field = PyObject_GetAttrString(_pymsg, "cloud_frequency"); + if (!field) { + return false; + } + assert(PyFloat_Check(field)); + ros_message->cloud_frequency = (float)PyFloat_AS_DOUBLE(field); + Py_DECREF(field); + } + { // cloud_packet_loss_rate + PyObject * field = PyObject_GetAttrString(_pymsg, "cloud_packet_loss_rate"); + if (!field) { + return false; + } + assert(PyFloat_Check(field)); + ros_message->cloud_packet_loss_rate = (float)PyFloat_AS_DOUBLE(field); + Py_DECREF(field); + } + { // cloud_size + PyObject * field = PyObject_GetAttrString(_pymsg, "cloud_size"); + if (!field) { + return false; + } + assert(PyLong_Check(field)); + ros_message->cloud_size = PyLong_AsUnsignedLong(field); + Py_DECREF(field); + } + { // cloud_scan_num + PyObject * field = PyObject_GetAttrString(_pymsg, "cloud_scan_num"); + if (!field) { + return false; + } + assert(PyLong_Check(field)); + ros_message->cloud_scan_num = PyLong_AsUnsignedLong(field); + Py_DECREF(field); + } + { // imu_frequency + PyObject * field = PyObject_GetAttrString(_pymsg, "imu_frequency"); + if (!field) { + return false; + } + assert(PyFloat_Check(field)); + ros_message->imu_frequency = (float)PyFloat_AS_DOUBLE(field); + Py_DECREF(field); + } + { // imu_packet_loss_rate + PyObject * field = PyObject_GetAttrString(_pymsg, "imu_packet_loss_rate"); + if (!field) { + return false; + } + assert(PyFloat_Check(field)); + ros_message->imu_packet_loss_rate = (float)PyFloat_AS_DOUBLE(field); + Py_DECREF(field); + } + { // imu_rpy + PyObject * field = PyObject_GetAttrString(_pymsg, "imu_rpy"); + if (!field) { + return false; + } + { + // TODO(dirk-thomas) use a better way to check the type before casting + assert(field->ob_type != NULL); + assert(field->ob_type->tp_name != NULL); + assert(strcmp(field->ob_type->tp_name, "numpy.ndarray") == 0); + PyArrayObject * seq_field = (PyArrayObject *)field; + Py_INCREF(seq_field); + assert(PyArray_NDIM(seq_field) == 1); + assert(PyArray_TYPE(seq_field) == NPY_FLOAT32); + Py_ssize_t size = 3; + float * dest = ros_message->imu_rpy; + for (Py_ssize_t i = 0; i < size; ++i) { + float tmp = *(npy_float32 *)PyArray_GETPTR1(seq_field, i); + memcpy(&dest[i], &tmp, sizeof(float)); + } + Py_DECREF(seq_field); + } + Py_DECREF(field); + } + { // serial_recv_stamp + PyObject * field = PyObject_GetAttrString(_pymsg, "serial_recv_stamp"); + if (!field) { + return false; + } + assert(PyFloat_Check(field)); + ros_message->serial_recv_stamp = PyFloat_AS_DOUBLE(field); + Py_DECREF(field); + } + { // serial_buffer_size + PyObject * field = PyObject_GetAttrString(_pymsg, "serial_buffer_size"); + if (!field) { + return false; + } + assert(PyLong_Check(field)); + ros_message->serial_buffer_size = PyLong_AsUnsignedLong(field); + Py_DECREF(field); + } + { // serial_buffer_read + PyObject * field = PyObject_GetAttrString(_pymsg, "serial_buffer_read"); + if (!field) { + return false; + } + assert(PyLong_Check(field)); + ros_message->serial_buffer_read = PyLong_AsUnsignedLong(field); + Py_DECREF(field); + } + + return true; +} + +ROSIDL_GENERATOR_C_EXPORT +PyObject * unitree_go__msg__lidar_state__convert_to_py(void * raw_ros_message) +{ + /* NOTE(esteve): Call constructor of LidarState */ + PyObject * _pymessage = NULL; + { + PyObject * pymessage_module = PyImport_ImportModule("unitree_go.msg._lidar_state"); + assert(pymessage_module); + PyObject * pymessage_class = PyObject_GetAttrString(pymessage_module, "LidarState"); + assert(pymessage_class); + Py_DECREF(pymessage_module); + _pymessage = PyObject_CallObject(pymessage_class, NULL); + Py_DECREF(pymessage_class); + if (!_pymessage) { + return NULL; + } + } + unitree_go__msg__LidarState * ros_message = (unitree_go__msg__LidarState *)raw_ros_message; + { // stamp + PyObject * field = NULL; + field = PyFloat_FromDouble(ros_message->stamp); + { + int rc = PyObject_SetAttrString(_pymessage, "stamp", field); + Py_DECREF(field); + if (rc) { + return NULL; + } + } + } + { // firmware_version + PyObject * field = NULL; + field = PyUnicode_DecodeUTF8( + ros_message->firmware_version.data, + strlen(ros_message->firmware_version.data), + "replace"); + if (!field) { + return NULL; + } + { + int rc = PyObject_SetAttrString(_pymessage, "firmware_version", field); + Py_DECREF(field); + if (rc) { + return NULL; + } + } + } + { // software_version + PyObject * field = NULL; + field = PyUnicode_DecodeUTF8( + ros_message->software_version.data, + strlen(ros_message->software_version.data), + "replace"); + if (!field) { + return NULL; + } + { + int rc = PyObject_SetAttrString(_pymessage, "software_version", field); + Py_DECREF(field); + if (rc) { + return NULL; + } + } + } + { // sdk_version + PyObject * field = NULL; + field = PyUnicode_DecodeUTF8( + ros_message->sdk_version.data, + strlen(ros_message->sdk_version.data), + "replace"); + if (!field) { + return NULL; + } + { + int rc = PyObject_SetAttrString(_pymessage, "sdk_version", field); + Py_DECREF(field); + if (rc) { + return NULL; + } + } + } + { // sys_rotation_speed + PyObject * field = NULL; + field = PyFloat_FromDouble(ros_message->sys_rotation_speed); + { + int rc = PyObject_SetAttrString(_pymessage, "sys_rotation_speed", field); + Py_DECREF(field); + if (rc) { + return NULL; + } + } + } + { // com_rotation_speed + PyObject * field = NULL; + field = PyFloat_FromDouble(ros_message->com_rotation_speed); + { + int rc = PyObject_SetAttrString(_pymessage, "com_rotation_speed", field); + Py_DECREF(field); + if (rc) { + return NULL; + } + } + } + { // error_state + PyObject * field = NULL; + field = PyLong_FromUnsignedLong(ros_message->error_state); + { + int rc = PyObject_SetAttrString(_pymessage, "error_state", field); + Py_DECREF(field); + if (rc) { + return NULL; + } + } + } + { // cloud_frequency + PyObject * field = NULL; + field = PyFloat_FromDouble(ros_message->cloud_frequency); + { + int rc = PyObject_SetAttrString(_pymessage, "cloud_frequency", field); + Py_DECREF(field); + if (rc) { + return NULL; + } + } + } + { // cloud_packet_loss_rate + PyObject * field = NULL; + field = PyFloat_FromDouble(ros_message->cloud_packet_loss_rate); + { + int rc = PyObject_SetAttrString(_pymessage, "cloud_packet_loss_rate", field); + Py_DECREF(field); + if (rc) { + return NULL; + } + } + } + { // cloud_size + PyObject * field = NULL; + field = PyLong_FromUnsignedLong(ros_message->cloud_size); + { + int rc = PyObject_SetAttrString(_pymessage, "cloud_size", field); + Py_DECREF(field); + if (rc) { + return NULL; + } + } + } + { // cloud_scan_num + PyObject * field = NULL; + field = PyLong_FromUnsignedLong(ros_message->cloud_scan_num); + { + int rc = PyObject_SetAttrString(_pymessage, "cloud_scan_num", field); + Py_DECREF(field); + if (rc) { + return NULL; + } + } + } + { // imu_frequency + PyObject * field = NULL; + field = PyFloat_FromDouble(ros_message->imu_frequency); + { + int rc = PyObject_SetAttrString(_pymessage, "imu_frequency", field); + Py_DECREF(field); + if (rc) { + return NULL; + } + } + } + { // imu_packet_loss_rate + PyObject * field = NULL; + field = PyFloat_FromDouble(ros_message->imu_packet_loss_rate); + { + int rc = PyObject_SetAttrString(_pymessage, "imu_packet_loss_rate", field); + Py_DECREF(field); + if (rc) { + return NULL; + } + } + } + { // imu_rpy + PyObject * field = NULL; + field = PyObject_GetAttrString(_pymessage, "imu_rpy"); + if (!field) { + return NULL; + } + assert(field->ob_type != NULL); + assert(field->ob_type->tp_name != NULL); + assert(strcmp(field->ob_type->tp_name, "numpy.ndarray") == 0); + PyArrayObject * seq_field = (PyArrayObject *)field; + assert(PyArray_NDIM(seq_field) == 1); + assert(PyArray_TYPE(seq_field) == NPY_FLOAT32); + assert(sizeof(npy_float32) == sizeof(float)); + npy_float32 * dst = (npy_float32 *)PyArray_GETPTR1(seq_field, 0); + float * src = &(ros_message->imu_rpy[0]); + memcpy(dst, src, 3 * sizeof(float)); + Py_DECREF(field); + } + { // serial_recv_stamp + PyObject * field = NULL; + field = PyFloat_FromDouble(ros_message->serial_recv_stamp); + { + int rc = PyObject_SetAttrString(_pymessage, "serial_recv_stamp", field); + Py_DECREF(field); + if (rc) { + return NULL; + } + } + } + { // serial_buffer_size + PyObject * field = NULL; + field = PyLong_FromUnsignedLong(ros_message->serial_buffer_size); + { + int rc = PyObject_SetAttrString(_pymessage, "serial_buffer_size", field); + Py_DECREF(field); + if (rc) { + return NULL; + } + } + } + { // serial_buffer_read + PyObject * field = NULL; + field = PyLong_FromUnsignedLong(ros_message->serial_buffer_read); + { + int rc = PyObject_SetAttrString(_pymessage, "serial_buffer_read", field); + Py_DECREF(field); + if (rc) { + return NULL; + } + } + } + + // ownership of _pymessage is transferred to the caller + return _pymessage; +} diff --git a/python/unitree_go/msg/_low_cmd.py b/python/unitree_go/msg/_low_cmd.py new file mode 100644 index 00000000..566e4dec --- /dev/null +++ b/python/unitree_go/msg/_low_cmd.py @@ -0,0 +1,547 @@ +# generated from rosidl_generator_py/resource/_idl.py.em +# with input from unitree_go:msg/LowCmd.idl +# generated code does not contain a copyright notice + + +# Import statements for member types + +# Member 'head' +# Member 'sn' +# Member 'version' +# Member 'wireless_remote' +# Member 'led' +# Member 'fan' +import numpy # noqa: E402, I100 + +import rosidl_parser.definition # noqa: E402, I100 + + +class Metaclass_LowCmd(type): + """Metaclass of message 'LowCmd'.""" + + _CREATE_ROS_MESSAGE = None + _CONVERT_FROM_PY = None + _CONVERT_TO_PY = None + _DESTROY_ROS_MESSAGE = None + _TYPE_SUPPORT = None + + __constants = { + } + + @classmethod + def __import_type_support__(cls): + try: + from rosidl_generator_py import import_type_support + module = import_type_support('unitree_go') + except ImportError: + import logging + import traceback + logger = logging.getLogger( + 'unitree_go.msg.LowCmd') + logger.debug( + 'Failed to import needed modules for type support:\n' + + traceback.format_exc()) + else: + cls._CREATE_ROS_MESSAGE = module.create_ros_message_msg__msg__low_cmd + cls._CONVERT_FROM_PY = module.convert_from_py_msg__msg__low_cmd + cls._CONVERT_TO_PY = module.convert_to_py_msg__msg__low_cmd + cls._TYPE_SUPPORT = module.type_support_msg__msg__low_cmd + cls._DESTROY_ROS_MESSAGE = module.destroy_ros_message_msg__msg__low_cmd + + from unitree_go.msg import BmsCmd + if BmsCmd.__class__._TYPE_SUPPORT is None: + BmsCmd.__class__.__import_type_support__() + + from unitree_go.msg import MotorCmd + if MotorCmd.__class__._TYPE_SUPPORT is None: + MotorCmd.__class__.__import_type_support__() + + @classmethod + def __prepare__(cls, name, bases, **kwargs): + # list constant names here so that they appear in the help text of + # the message class under "Data and other attributes defined here:" + # as well as populate each message instance + return { + } + + +class LowCmd(metaclass=Metaclass_LowCmd): + """Message class 'LowCmd'.""" + + __slots__ = [ + '_head', + '_level_flag', + '_frame_reserve', + '_sn', + '_version', + '_bandwidth', + '_motor_cmd', + '_bms_cmd', + '_wireless_remote', + '_led', + '_fan', + '_gpio', + '_reserve', + '_crc', + ] + + _fields_and_field_types = { + 'head': 'uint8[2]', + 'level_flag': 'uint8', + 'frame_reserve': 'uint8', + 'sn': 'uint32[2]', + 'version': 'uint32[2]', + 'bandwidth': 'uint16', + 'motor_cmd': 'unitree_go/MotorCmd[20]', + 'bms_cmd': 'unitree_go/BmsCmd', + 'wireless_remote': 'uint8[40]', + 'led': 'uint8[12]', + 'fan': 'uint8[2]', + 'gpio': 'uint8', + 'reserve': 'uint32', + 'crc': 'uint32', + } + + SLOT_TYPES = ( + rosidl_parser.definition.Array(rosidl_parser.definition.BasicType('uint8'), 2), # noqa: E501 + rosidl_parser.definition.BasicType('uint8'), # noqa: E501 + rosidl_parser.definition.BasicType('uint8'), # noqa: E501 + rosidl_parser.definition.Array(rosidl_parser.definition.BasicType('uint32'), 2), # noqa: E501 + rosidl_parser.definition.Array(rosidl_parser.definition.BasicType('uint32'), 2), # noqa: E501 + rosidl_parser.definition.BasicType('uint16'), # noqa: E501 + rosidl_parser.definition.Array(rosidl_parser.definition.NamespacedType(['unitree_go', 'msg'], 'MotorCmd'), 20), # noqa: E501 + rosidl_parser.definition.NamespacedType(['unitree_go', 'msg'], 'BmsCmd'), # noqa: E501 + rosidl_parser.definition.Array(rosidl_parser.definition.BasicType('uint8'), 40), # noqa: E501 + rosidl_parser.definition.Array(rosidl_parser.definition.BasicType('uint8'), 12), # noqa: E501 + rosidl_parser.definition.Array(rosidl_parser.definition.BasicType('uint8'), 2), # noqa: E501 + rosidl_parser.definition.BasicType('uint8'), # noqa: E501 + rosidl_parser.definition.BasicType('uint32'), # noqa: E501 + rosidl_parser.definition.BasicType('uint32'), # noqa: E501 + ) + + def __init__(self, **kwargs): + assert all('_' + key in self.__slots__ for key in kwargs.keys()), \ + 'Invalid arguments passed to constructor: %s' % \ + ', '.join(sorted(k for k in kwargs.keys() if '_' + k not in self.__slots__)) + if 'head' not in kwargs: + self.head = numpy.zeros(2, dtype=numpy.uint8) + else: + self.head = numpy.array(kwargs.get('head'), dtype=numpy.uint8) + assert self.head.shape == (2, ) + self.level_flag = kwargs.get('level_flag', int()) + self.frame_reserve = kwargs.get('frame_reserve', int()) + if 'sn' not in kwargs: + self.sn = numpy.zeros(2, dtype=numpy.uint32) + else: + self.sn = numpy.array(kwargs.get('sn'), dtype=numpy.uint32) + assert self.sn.shape == (2, ) + if 'version' not in kwargs: + self.version = numpy.zeros(2, dtype=numpy.uint32) + else: + self.version = numpy.array(kwargs.get('version'), dtype=numpy.uint32) + assert self.version.shape == (2, ) + self.bandwidth = kwargs.get('bandwidth', int()) + from unitree_go.msg import MotorCmd + self.motor_cmd = kwargs.get( + 'motor_cmd', + [MotorCmd() for x in range(20)] + ) + from unitree_go.msg import BmsCmd + self.bms_cmd = kwargs.get('bms_cmd', BmsCmd()) + if 'wireless_remote' not in kwargs: + self.wireless_remote = numpy.zeros(40, dtype=numpy.uint8) + else: + self.wireless_remote = numpy.array(kwargs.get('wireless_remote'), dtype=numpy.uint8) + assert self.wireless_remote.shape == (40, ) + if 'led' not in kwargs: + self.led = numpy.zeros(12, dtype=numpy.uint8) + else: + self.led = numpy.array(kwargs.get('led'), dtype=numpy.uint8) + assert self.led.shape == (12, ) + if 'fan' not in kwargs: + self.fan = numpy.zeros(2, dtype=numpy.uint8) + else: + self.fan = numpy.array(kwargs.get('fan'), dtype=numpy.uint8) + assert self.fan.shape == (2, ) + self.gpio = kwargs.get('gpio', int()) + self.reserve = kwargs.get('reserve', int()) + self.crc = kwargs.get('crc', int()) + + def __repr__(self): + typename = self.__class__.__module__.split('.') + typename.pop() + typename.append(self.__class__.__name__) + args = [] + for s, t in zip(self.__slots__, self.SLOT_TYPES): + field = getattr(self, s) + fieldstr = repr(field) + # We use Python array type for fields that can be directly stored + # in them, and "normal" sequences for everything else. If it is + # a type that we store in an array, strip off the 'array' portion. + if ( + isinstance(t, rosidl_parser.definition.AbstractSequence) and + isinstance(t.value_type, rosidl_parser.definition.BasicType) and + t.value_type.typename in ['float', 'double', 'int8', 'uint8', 'int16', 'uint16', 'int32', 'uint32', 'int64', 'uint64'] + ): + if len(field) == 0: + fieldstr = '[]' + else: + assert fieldstr.startswith('array(') + prefix = "array('X', " + suffix = ')' + fieldstr = fieldstr[len(prefix):-len(suffix)] + args.append(s[1:] + '=' + fieldstr) + return '%s(%s)' % ('.'.join(typename), ', '.join(args)) + + def __eq__(self, other): + if not isinstance(other, self.__class__): + return False + if all(self.head != other.head): + return False + if self.level_flag != other.level_flag: + return False + if self.frame_reserve != other.frame_reserve: + return False + if all(self.sn != other.sn): + return False + if all(self.version != other.version): + return False + if self.bandwidth != other.bandwidth: + return False + if self.motor_cmd != other.motor_cmd: + return False + if self.bms_cmd != other.bms_cmd: + return False + if all(self.wireless_remote != other.wireless_remote): + return False + if all(self.led != other.led): + return False + if all(self.fan != other.fan): + return False + if self.gpio != other.gpio: + return False + if self.reserve != other.reserve: + return False + if self.crc != other.crc: + return False + return True + + @classmethod + def get_fields_and_field_types(cls): + from copy import copy + return copy(cls._fields_and_field_types) + + @property + def head(self): + """Message field 'head'.""" + return self._head + + @head.setter + def head(self, value): + if isinstance(value, numpy.ndarray): + assert value.dtype == numpy.uint8, \ + "The 'head' numpy.ndarray() must have the dtype of 'numpy.uint8'" + assert value.size == 2, \ + "The 'head' numpy.ndarray() must have a size of 2" + self._head = value + return + if __debug__: + from collections.abc import Sequence + from collections.abc import Set + from collections import UserList + from collections import UserString + assert \ + ((isinstance(value, Sequence) or + isinstance(value, Set) or + isinstance(value, UserList)) and + not isinstance(value, str) and + not isinstance(value, UserString) and + len(value) == 2 and + all(isinstance(v, int) for v in value) and + all(val >= 0 and val < 256 for val in value)), \ + "The 'head' field must be a set or sequence with length 2 and each value of type 'int' and each unsigned integer in [0, 255]" + self._head = numpy.array(value, dtype=numpy.uint8) + + @property + def level_flag(self): + """Message field 'level_flag'.""" + return self._level_flag + + @level_flag.setter + def level_flag(self, value): + if __debug__: + assert \ + isinstance(value, int), \ + "The 'level_flag' field must be of type 'int'" + assert value >= 0 and value < 256, \ + "The 'level_flag' field must be an unsigned integer in [0, 255]" + self._level_flag = value + + @property + def frame_reserve(self): + """Message field 'frame_reserve'.""" + return self._frame_reserve + + @frame_reserve.setter + def frame_reserve(self, value): + if __debug__: + assert \ + isinstance(value, int), \ + "The 'frame_reserve' field must be of type 'int'" + assert value >= 0 and value < 256, \ + "The 'frame_reserve' field must be an unsigned integer in [0, 255]" + self._frame_reserve = value + + @property + def sn(self): + """Message field 'sn'.""" + return self._sn + + @sn.setter + def sn(self, value): + if isinstance(value, numpy.ndarray): + assert value.dtype == numpy.uint32, \ + "The 'sn' numpy.ndarray() must have the dtype of 'numpy.uint32'" + assert value.size == 2, \ + "The 'sn' numpy.ndarray() must have a size of 2" + self._sn = value + return + if __debug__: + from collections.abc import Sequence + from collections.abc import Set + from collections import UserList + from collections import UserString + assert \ + ((isinstance(value, Sequence) or + isinstance(value, Set) or + isinstance(value, UserList)) and + not isinstance(value, str) and + not isinstance(value, UserString) and + len(value) == 2 and + all(isinstance(v, int) for v in value) and + all(val >= 0 and val < 4294967296 for val in value)), \ + "The 'sn' field must be a set or sequence with length 2 and each value of type 'int' and each unsigned integer in [0, 4294967295]" + self._sn = numpy.array(value, dtype=numpy.uint32) + + @property + def version(self): + """Message field 'version'.""" + return self._version + + @version.setter + def version(self, value): + if isinstance(value, numpy.ndarray): + assert value.dtype == numpy.uint32, \ + "The 'version' numpy.ndarray() must have the dtype of 'numpy.uint32'" + assert value.size == 2, \ + "The 'version' numpy.ndarray() must have a size of 2" + self._version = value + return + if __debug__: + from collections.abc import Sequence + from collections.abc import Set + from collections import UserList + from collections import UserString + assert \ + ((isinstance(value, Sequence) or + isinstance(value, Set) or + isinstance(value, UserList)) and + not isinstance(value, str) and + not isinstance(value, UserString) and + len(value) == 2 and + all(isinstance(v, int) for v in value) and + all(val >= 0 and val < 4294967296 for val in value)), \ + "The 'version' field must be a set or sequence with length 2 and each value of type 'int' and each unsigned integer in [0, 4294967295]" + self._version = numpy.array(value, dtype=numpy.uint32) + + @property + def bandwidth(self): + """Message field 'bandwidth'.""" + return self._bandwidth + + @bandwidth.setter + def bandwidth(self, value): + if __debug__: + assert \ + isinstance(value, int), \ + "The 'bandwidth' field must be of type 'int'" + assert value >= 0 and value < 65536, \ + "The 'bandwidth' field must be an unsigned integer in [0, 65535]" + self._bandwidth = value + + @property + def motor_cmd(self): + """Message field 'motor_cmd'.""" + return self._motor_cmd + + @motor_cmd.setter + def motor_cmd(self, value): + if __debug__: + from unitree_go.msg import MotorCmd + from collections.abc import Sequence + from collections.abc import Set + from collections import UserList + from collections import UserString + assert \ + ((isinstance(value, Sequence) or + isinstance(value, Set) or + isinstance(value, UserList)) and + not isinstance(value, str) and + not isinstance(value, UserString) and + len(value) == 20 and + all(isinstance(v, MotorCmd) for v in value) and + True), \ + "The 'motor_cmd' field must be a set or sequence with length 20 and each value of type 'MotorCmd'" + self._motor_cmd = value + + @property + def bms_cmd(self): + """Message field 'bms_cmd'.""" + return self._bms_cmd + + @bms_cmd.setter + def bms_cmd(self, value): + if __debug__: + from unitree_go.msg import BmsCmd + assert \ + isinstance(value, BmsCmd), \ + "The 'bms_cmd' field must be a sub message of type 'BmsCmd'" + self._bms_cmd = value + + @property + def wireless_remote(self): + """Message field 'wireless_remote'.""" + return self._wireless_remote + + @wireless_remote.setter + def wireless_remote(self, value): + if isinstance(value, numpy.ndarray): + assert value.dtype == numpy.uint8, \ + "The 'wireless_remote' numpy.ndarray() must have the dtype of 'numpy.uint8'" + assert value.size == 40, \ + "The 'wireless_remote' numpy.ndarray() must have a size of 40" + self._wireless_remote = value + return + if __debug__: + from collections.abc import Sequence + from collections.abc import Set + from collections import UserList + from collections import UserString + assert \ + ((isinstance(value, Sequence) or + isinstance(value, Set) or + isinstance(value, UserList)) and + not isinstance(value, str) and + not isinstance(value, UserString) and + len(value) == 40 and + all(isinstance(v, int) for v in value) and + all(val >= 0 and val < 256 for val in value)), \ + "The 'wireless_remote' field must be a set or sequence with length 40 and each value of type 'int' and each unsigned integer in [0, 255]" + self._wireless_remote = numpy.array(value, dtype=numpy.uint8) + + @property + def led(self): + """Message field 'led'.""" + return self._led + + @led.setter + def led(self, value): + if isinstance(value, numpy.ndarray): + assert value.dtype == numpy.uint8, \ + "The 'led' numpy.ndarray() must have the dtype of 'numpy.uint8'" + assert value.size == 12, \ + "The 'led' numpy.ndarray() must have a size of 12" + self._led = value + return + if __debug__: + from collections.abc import Sequence + from collections.abc import Set + from collections import UserList + from collections import UserString + assert \ + ((isinstance(value, Sequence) or + isinstance(value, Set) or + isinstance(value, UserList)) and + not isinstance(value, str) and + not isinstance(value, UserString) and + len(value) == 12 and + all(isinstance(v, int) for v in value) and + all(val >= 0 and val < 256 for val in value)), \ + "The 'led' field must be a set or sequence with length 12 and each value of type 'int' and each unsigned integer in [0, 255]" + self._led = numpy.array(value, dtype=numpy.uint8) + + @property + def fan(self): + """Message field 'fan'.""" + return self._fan + + @fan.setter + def fan(self, value): + if isinstance(value, numpy.ndarray): + assert value.dtype == numpy.uint8, \ + "The 'fan' numpy.ndarray() must have the dtype of 'numpy.uint8'" + assert value.size == 2, \ + "The 'fan' numpy.ndarray() must have a size of 2" + self._fan = value + return + if __debug__: + from collections.abc import Sequence + from collections.abc import Set + from collections import UserList + from collections import UserString + assert \ + ((isinstance(value, Sequence) or + isinstance(value, Set) or + isinstance(value, UserList)) and + not isinstance(value, str) and + not isinstance(value, UserString) and + len(value) == 2 and + all(isinstance(v, int) for v in value) and + all(val >= 0 and val < 256 for val in value)), \ + "The 'fan' field must be a set or sequence with length 2 and each value of type 'int' and each unsigned integer in [0, 255]" + self._fan = numpy.array(value, dtype=numpy.uint8) + + @property + def gpio(self): + """Message field 'gpio'.""" + return self._gpio + + @gpio.setter + def gpio(self, value): + if __debug__: + assert \ + isinstance(value, int), \ + "The 'gpio' field must be of type 'int'" + assert value >= 0 and value < 256, \ + "The 'gpio' field must be an unsigned integer in [0, 255]" + self._gpio = value + + @property + def reserve(self): + """Message field 'reserve'.""" + return self._reserve + + @reserve.setter + def reserve(self, value): + if __debug__: + assert \ + isinstance(value, int), \ + "The 'reserve' field must be of type 'int'" + assert value >= 0 and value < 4294967296, \ + "The 'reserve' field must be an unsigned integer in [0, 4294967295]" + self._reserve = value + + @property + def crc(self): + """Message field 'crc'.""" + return self._crc + + @crc.setter + def crc(self, value): + if __debug__: + assert \ + isinstance(value, int), \ + "The 'crc' field must be of type 'int'" + assert value >= 0 and value < 4294967296, \ + "The 'crc' field must be an unsigned integer in [0, 4294967295]" + self._crc = value diff --git a/python/unitree_go/msg/_low_cmd_s.c b/python/unitree_go/msg/_low_cmd_s.c new file mode 100644 index 00000000..d8683653 --- /dev/null +++ b/python/unitree_go/msg/_low_cmd_s.c @@ -0,0 +1,535 @@ +// generated from rosidl_generator_py/resource/_idl_support.c.em +// with input from unitree_go:msg/LowCmd.idl +// generated code does not contain a copyright notice +#define NPY_NO_DEPRECATED_API NPY_1_7_API_VERSION +#include +#include +#ifndef _WIN32 +# pragma GCC diagnostic push +# pragma GCC diagnostic ignored "-Wunused-function" +#endif +#include "numpy/ndarrayobject.h" +#ifndef _WIN32 +# pragma GCC diagnostic pop +#endif +#include "rosidl_runtime_c/visibility_control.h" +#include "unitree_go/msg/detail/low_cmd__struct.h" +#include "unitree_go/msg/detail/low_cmd__functions.h" + +#include "rosidl_runtime_c/primitives_sequence.h" +#include "rosidl_runtime_c/primitives_sequence_functions.h" + +// Nested array functions includes +#include "unitree_go/msg/detail/motor_cmd__functions.h" +// end nested array functions include +bool unitree_go__msg__motor_cmd__convert_from_py(PyObject * _pymsg, void * _ros_message); +PyObject * unitree_go__msg__motor_cmd__convert_to_py(void * raw_ros_message); +bool unitree_go__msg__bms_cmd__convert_from_py(PyObject * _pymsg, void * _ros_message); +PyObject * unitree_go__msg__bms_cmd__convert_to_py(void * raw_ros_message); + +ROSIDL_GENERATOR_C_EXPORT +bool unitree_go__msg__low_cmd__convert_from_py(PyObject * _pymsg, void * _ros_message) +{ + // check that the passed message is of the expected Python class + { + char full_classname_dest[31]; + { + char * class_name = NULL; + char * module_name = NULL; + { + PyObject * class_attr = PyObject_GetAttrString(_pymsg, "__class__"); + if (class_attr) { + PyObject * name_attr = PyObject_GetAttrString(class_attr, "__name__"); + if (name_attr) { + class_name = (char *)PyUnicode_1BYTE_DATA(name_attr); + Py_DECREF(name_attr); + } + PyObject * module_attr = PyObject_GetAttrString(class_attr, "__module__"); + if (module_attr) { + module_name = (char *)PyUnicode_1BYTE_DATA(module_attr); + Py_DECREF(module_attr); + } + Py_DECREF(class_attr); + } + } + if (!class_name || !module_name) { + return false; + } + snprintf(full_classname_dest, sizeof(full_classname_dest), "%s.%s", module_name, class_name); + } + assert(strncmp("unitree_go.msg._low_cmd.LowCmd", full_classname_dest, 30) == 0); + } + unitree_go__msg__LowCmd * ros_message = _ros_message; + { // head + PyObject * field = PyObject_GetAttrString(_pymsg, "head"); + if (!field) { + return false; + } + { + // TODO(dirk-thomas) use a better way to check the type before casting + assert(field->ob_type != NULL); + assert(field->ob_type->tp_name != NULL); + assert(strcmp(field->ob_type->tp_name, "numpy.ndarray") == 0); + PyArrayObject * seq_field = (PyArrayObject *)field; + Py_INCREF(seq_field); + assert(PyArray_NDIM(seq_field) == 1); + assert(PyArray_TYPE(seq_field) == NPY_UINT8); + Py_ssize_t size = 2; + uint8_t * dest = ros_message->head; + for (Py_ssize_t i = 0; i < size; ++i) { + uint8_t tmp = *(npy_uint8 *)PyArray_GETPTR1(seq_field, i); + memcpy(&dest[i], &tmp, sizeof(uint8_t)); + } + Py_DECREF(seq_field); + } + Py_DECREF(field); + } + { // level_flag + PyObject * field = PyObject_GetAttrString(_pymsg, "level_flag"); + if (!field) { + return false; + } + assert(PyLong_Check(field)); + ros_message->level_flag = (uint8_t)PyLong_AsUnsignedLong(field); + Py_DECREF(field); + } + { // frame_reserve + PyObject * field = PyObject_GetAttrString(_pymsg, "frame_reserve"); + if (!field) { + return false; + } + assert(PyLong_Check(field)); + ros_message->frame_reserve = (uint8_t)PyLong_AsUnsignedLong(field); + Py_DECREF(field); + } + { // sn + PyObject * field = PyObject_GetAttrString(_pymsg, "sn"); + if (!field) { + return false; + } + { + // TODO(dirk-thomas) use a better way to check the type before casting + assert(field->ob_type != NULL); + assert(field->ob_type->tp_name != NULL); + assert(strcmp(field->ob_type->tp_name, "numpy.ndarray") == 0); + PyArrayObject * seq_field = (PyArrayObject *)field; + Py_INCREF(seq_field); + assert(PyArray_NDIM(seq_field) == 1); + assert(PyArray_TYPE(seq_field) == NPY_UINT32); + Py_ssize_t size = 2; + uint32_t * dest = ros_message->sn; + for (Py_ssize_t i = 0; i < size; ++i) { + uint32_t tmp = *(npy_uint32 *)PyArray_GETPTR1(seq_field, i); + memcpy(&dest[i], &tmp, sizeof(uint32_t)); + } + Py_DECREF(seq_field); + } + Py_DECREF(field); + } + { // version + PyObject * field = PyObject_GetAttrString(_pymsg, "version"); + if (!field) { + return false; + } + { + // TODO(dirk-thomas) use a better way to check the type before casting + assert(field->ob_type != NULL); + assert(field->ob_type->tp_name != NULL); + assert(strcmp(field->ob_type->tp_name, "numpy.ndarray") == 0); + PyArrayObject * seq_field = (PyArrayObject *)field; + Py_INCREF(seq_field); + assert(PyArray_NDIM(seq_field) == 1); + assert(PyArray_TYPE(seq_field) == NPY_UINT32); + Py_ssize_t size = 2; + uint32_t * dest = ros_message->version; + for (Py_ssize_t i = 0; i < size; ++i) { + uint32_t tmp = *(npy_uint32 *)PyArray_GETPTR1(seq_field, i); + memcpy(&dest[i], &tmp, sizeof(uint32_t)); + } + Py_DECREF(seq_field); + } + Py_DECREF(field); + } + { // bandwidth + PyObject * field = PyObject_GetAttrString(_pymsg, "bandwidth"); + if (!field) { + return false; + } + assert(PyLong_Check(field)); + ros_message->bandwidth = (uint16_t)PyLong_AsUnsignedLong(field); + Py_DECREF(field); + } + { // motor_cmd + PyObject * field = PyObject_GetAttrString(_pymsg, "motor_cmd"); + if (!field) { + return false; + } + PyObject * seq_field = PySequence_Fast(field, "expected a sequence in 'motor_cmd'"); + if (!seq_field) { + Py_DECREF(field); + return false; + } + Py_ssize_t size = 20; + unitree_go__msg__MotorCmd * dest = ros_message->motor_cmd; + for (Py_ssize_t i = 0; i < size; ++i) { + if (!unitree_go__msg__motor_cmd__convert_from_py(PySequence_Fast_GET_ITEM(seq_field, i), &dest[i])) { + Py_DECREF(seq_field); + Py_DECREF(field); + return false; + } + } + Py_DECREF(seq_field); + Py_DECREF(field); + } + { // bms_cmd + PyObject * field = PyObject_GetAttrString(_pymsg, "bms_cmd"); + if (!field) { + return false; + } + if (!unitree_go__msg__bms_cmd__convert_from_py(field, &ros_message->bms_cmd)) { + Py_DECREF(field); + return false; + } + Py_DECREF(field); + } + { // wireless_remote + PyObject * field = PyObject_GetAttrString(_pymsg, "wireless_remote"); + if (!field) { + return false; + } + { + // TODO(dirk-thomas) use a better way to check the type before casting + assert(field->ob_type != NULL); + assert(field->ob_type->tp_name != NULL); + assert(strcmp(field->ob_type->tp_name, "numpy.ndarray") == 0); + PyArrayObject * seq_field = (PyArrayObject *)field; + Py_INCREF(seq_field); + assert(PyArray_NDIM(seq_field) == 1); + assert(PyArray_TYPE(seq_field) == NPY_UINT8); + Py_ssize_t size = 40; + uint8_t * dest = ros_message->wireless_remote; + for (Py_ssize_t i = 0; i < size; ++i) { + uint8_t tmp = *(npy_uint8 *)PyArray_GETPTR1(seq_field, i); + memcpy(&dest[i], &tmp, sizeof(uint8_t)); + } + Py_DECREF(seq_field); + } + Py_DECREF(field); + } + { // led + PyObject * field = PyObject_GetAttrString(_pymsg, "led"); + if (!field) { + return false; + } + { + // TODO(dirk-thomas) use a better way to check the type before casting + assert(field->ob_type != NULL); + assert(field->ob_type->tp_name != NULL); + assert(strcmp(field->ob_type->tp_name, "numpy.ndarray") == 0); + PyArrayObject * seq_field = (PyArrayObject *)field; + Py_INCREF(seq_field); + assert(PyArray_NDIM(seq_field) == 1); + assert(PyArray_TYPE(seq_field) == NPY_UINT8); + Py_ssize_t size = 12; + uint8_t * dest = ros_message->led; + for (Py_ssize_t i = 0; i < size; ++i) { + uint8_t tmp = *(npy_uint8 *)PyArray_GETPTR1(seq_field, i); + memcpy(&dest[i], &tmp, sizeof(uint8_t)); + } + Py_DECREF(seq_field); + } + Py_DECREF(field); + } + { // fan + PyObject * field = PyObject_GetAttrString(_pymsg, "fan"); + if (!field) { + return false; + } + { + // TODO(dirk-thomas) use a better way to check the type before casting + assert(field->ob_type != NULL); + assert(field->ob_type->tp_name != NULL); + assert(strcmp(field->ob_type->tp_name, "numpy.ndarray") == 0); + PyArrayObject * seq_field = (PyArrayObject *)field; + Py_INCREF(seq_field); + assert(PyArray_NDIM(seq_field) == 1); + assert(PyArray_TYPE(seq_field) == NPY_UINT8); + Py_ssize_t size = 2; + uint8_t * dest = ros_message->fan; + for (Py_ssize_t i = 0; i < size; ++i) { + uint8_t tmp = *(npy_uint8 *)PyArray_GETPTR1(seq_field, i); + memcpy(&dest[i], &tmp, sizeof(uint8_t)); + } + Py_DECREF(seq_field); + } + Py_DECREF(field); + } + { // gpio + PyObject * field = PyObject_GetAttrString(_pymsg, "gpio"); + if (!field) { + return false; + } + assert(PyLong_Check(field)); + ros_message->gpio = (uint8_t)PyLong_AsUnsignedLong(field); + Py_DECREF(field); + } + { // reserve + PyObject * field = PyObject_GetAttrString(_pymsg, "reserve"); + if (!field) { + return false; + } + assert(PyLong_Check(field)); + ros_message->reserve = PyLong_AsUnsignedLong(field); + Py_DECREF(field); + } + { // crc + PyObject * field = PyObject_GetAttrString(_pymsg, "crc"); + if (!field) { + return false; + } + assert(PyLong_Check(field)); + ros_message->crc = PyLong_AsUnsignedLong(field); + Py_DECREF(field); + } + + return true; +} + +ROSIDL_GENERATOR_C_EXPORT +PyObject * unitree_go__msg__low_cmd__convert_to_py(void * raw_ros_message) +{ + /* NOTE(esteve): Call constructor of LowCmd */ + PyObject * _pymessage = NULL; + { + PyObject * pymessage_module = PyImport_ImportModule("unitree_go.msg._low_cmd"); + assert(pymessage_module); + PyObject * pymessage_class = PyObject_GetAttrString(pymessage_module, "LowCmd"); + assert(pymessage_class); + Py_DECREF(pymessage_module); + _pymessage = PyObject_CallObject(pymessage_class, NULL); + Py_DECREF(pymessage_class); + if (!_pymessage) { + return NULL; + } + } + unitree_go__msg__LowCmd * ros_message = (unitree_go__msg__LowCmd *)raw_ros_message; + { // head + PyObject * field = NULL; + field = PyObject_GetAttrString(_pymessage, "head"); + if (!field) { + return NULL; + } + assert(field->ob_type != NULL); + assert(field->ob_type->tp_name != NULL); + assert(strcmp(field->ob_type->tp_name, "numpy.ndarray") == 0); + PyArrayObject * seq_field = (PyArrayObject *)field; + assert(PyArray_NDIM(seq_field) == 1); + assert(PyArray_TYPE(seq_field) == NPY_UINT8); + assert(sizeof(npy_uint8) == sizeof(uint8_t)); + npy_uint8 * dst = (npy_uint8 *)PyArray_GETPTR1(seq_field, 0); + uint8_t * src = &(ros_message->head[0]); + memcpy(dst, src, 2 * sizeof(uint8_t)); + Py_DECREF(field); + } + { // level_flag + PyObject * field = NULL; + field = PyLong_FromUnsignedLong(ros_message->level_flag); + { + int rc = PyObject_SetAttrString(_pymessage, "level_flag", field); + Py_DECREF(field); + if (rc) { + return NULL; + } + } + } + { // frame_reserve + PyObject * field = NULL; + field = PyLong_FromUnsignedLong(ros_message->frame_reserve); + { + int rc = PyObject_SetAttrString(_pymessage, "frame_reserve", field); + Py_DECREF(field); + if (rc) { + return NULL; + } + } + } + { // sn + PyObject * field = NULL; + field = PyObject_GetAttrString(_pymessage, "sn"); + if (!field) { + return NULL; + } + assert(field->ob_type != NULL); + assert(field->ob_type->tp_name != NULL); + assert(strcmp(field->ob_type->tp_name, "numpy.ndarray") == 0); + PyArrayObject * seq_field = (PyArrayObject *)field; + assert(PyArray_NDIM(seq_field) == 1); + assert(PyArray_TYPE(seq_field) == NPY_UINT32); + assert(sizeof(npy_uint32) == sizeof(uint32_t)); + npy_uint32 * dst = (npy_uint32 *)PyArray_GETPTR1(seq_field, 0); + uint32_t * src = &(ros_message->sn[0]); + memcpy(dst, src, 2 * sizeof(uint32_t)); + Py_DECREF(field); + } + { // version + PyObject * field = NULL; + field = PyObject_GetAttrString(_pymessage, "version"); + if (!field) { + return NULL; + } + assert(field->ob_type != NULL); + assert(field->ob_type->tp_name != NULL); + assert(strcmp(field->ob_type->tp_name, "numpy.ndarray") == 0); + PyArrayObject * seq_field = (PyArrayObject *)field; + assert(PyArray_NDIM(seq_field) == 1); + assert(PyArray_TYPE(seq_field) == NPY_UINT32); + assert(sizeof(npy_uint32) == sizeof(uint32_t)); + npy_uint32 * dst = (npy_uint32 *)PyArray_GETPTR1(seq_field, 0); + uint32_t * src = &(ros_message->version[0]); + memcpy(dst, src, 2 * sizeof(uint32_t)); + Py_DECREF(field); + } + { // bandwidth + PyObject * field = NULL; + field = PyLong_FromUnsignedLong(ros_message->bandwidth); + { + int rc = PyObject_SetAttrString(_pymessage, "bandwidth", field); + Py_DECREF(field); + if (rc) { + return NULL; + } + } + } + { // motor_cmd + PyObject * field = NULL; + size_t size = 20; + field = PyList_New(size); + if (!field) { + return NULL; + } + unitree_go__msg__MotorCmd * item; + for (size_t i = 0; i < size; ++i) { + item = &(ros_message->motor_cmd[i]); + PyObject * pyitem = unitree_go__msg__motor_cmd__convert_to_py(item); + if (!pyitem) { + Py_DECREF(field); + return NULL; + } + int rc = PyList_SetItem(field, i, pyitem); + (void)rc; + assert(rc == 0); + } + assert(PySequence_Check(field)); + { + int rc = PyObject_SetAttrString(_pymessage, "motor_cmd", field); + Py_DECREF(field); + if (rc) { + return NULL; + } + } + } + { // bms_cmd + PyObject * field = NULL; + field = unitree_go__msg__bms_cmd__convert_to_py(&ros_message->bms_cmd); + if (!field) { + return NULL; + } + { + int rc = PyObject_SetAttrString(_pymessage, "bms_cmd", field); + Py_DECREF(field); + if (rc) { + return NULL; + } + } + } + { // wireless_remote + PyObject * field = NULL; + field = PyObject_GetAttrString(_pymessage, "wireless_remote"); + if (!field) { + return NULL; + } + assert(field->ob_type != NULL); + assert(field->ob_type->tp_name != NULL); + assert(strcmp(field->ob_type->tp_name, "numpy.ndarray") == 0); + PyArrayObject * seq_field = (PyArrayObject *)field; + assert(PyArray_NDIM(seq_field) == 1); + assert(PyArray_TYPE(seq_field) == NPY_UINT8); + assert(sizeof(npy_uint8) == sizeof(uint8_t)); + npy_uint8 * dst = (npy_uint8 *)PyArray_GETPTR1(seq_field, 0); + uint8_t * src = &(ros_message->wireless_remote[0]); + memcpy(dst, src, 40 * sizeof(uint8_t)); + Py_DECREF(field); + } + { // led + PyObject * field = NULL; + field = PyObject_GetAttrString(_pymessage, "led"); + if (!field) { + return NULL; + } + assert(field->ob_type != NULL); + assert(field->ob_type->tp_name != NULL); + assert(strcmp(field->ob_type->tp_name, "numpy.ndarray") == 0); + PyArrayObject * seq_field = (PyArrayObject *)field; + assert(PyArray_NDIM(seq_field) == 1); + assert(PyArray_TYPE(seq_field) == NPY_UINT8); + assert(sizeof(npy_uint8) == sizeof(uint8_t)); + npy_uint8 * dst = (npy_uint8 *)PyArray_GETPTR1(seq_field, 0); + uint8_t * src = &(ros_message->led[0]); + memcpy(dst, src, 12 * sizeof(uint8_t)); + Py_DECREF(field); + } + { // fan + PyObject * field = NULL; + field = PyObject_GetAttrString(_pymessage, "fan"); + if (!field) { + return NULL; + } + assert(field->ob_type != NULL); + assert(field->ob_type->tp_name != NULL); + assert(strcmp(field->ob_type->tp_name, "numpy.ndarray") == 0); + PyArrayObject * seq_field = (PyArrayObject *)field; + assert(PyArray_NDIM(seq_field) == 1); + assert(PyArray_TYPE(seq_field) == NPY_UINT8); + assert(sizeof(npy_uint8) == sizeof(uint8_t)); + npy_uint8 * dst = (npy_uint8 *)PyArray_GETPTR1(seq_field, 0); + uint8_t * src = &(ros_message->fan[0]); + memcpy(dst, src, 2 * sizeof(uint8_t)); + Py_DECREF(field); + } + { // gpio + PyObject * field = NULL; + field = PyLong_FromUnsignedLong(ros_message->gpio); + { + int rc = PyObject_SetAttrString(_pymessage, "gpio", field); + Py_DECREF(field); + if (rc) { + return NULL; + } + } + } + { // reserve + PyObject * field = NULL; + field = PyLong_FromUnsignedLong(ros_message->reserve); + { + int rc = PyObject_SetAttrString(_pymessage, "reserve", field); + Py_DECREF(field); + if (rc) { + return NULL; + } + } + } + { // crc + PyObject * field = NULL; + field = PyLong_FromUnsignedLong(ros_message->crc); + { + int rc = PyObject_SetAttrString(_pymessage, "crc", field); + Py_DECREF(field); + if (rc) { + return NULL; + } + } + } + + // ownership of _pymessage is transferred to the caller + return _pymessage; +} diff --git a/python/unitree_go/msg/_low_state.py b/python/unitree_go/msg/_low_state.py new file mode 100644 index 00000000..6c02bc41 --- /dev/null +++ b/python/unitree_go/msg/_low_state.py @@ -0,0 +1,734 @@ +# generated from rosidl_generator_py/resource/_idl.py.em +# with input from unitree_go:msg/LowState.idl +# generated code does not contain a copyright notice + + +# Import statements for member types + +# Member 'head' +# Member 'sn' +# Member 'version' +# Member 'foot_force' +# Member 'foot_force_est' +# Member 'wireless_remote' +# Member 'fan_frequency' +import numpy # noqa: E402, I100 + +import rosidl_parser.definition # noqa: E402, I100 + + +class Metaclass_LowState(type): + """Metaclass of message 'LowState'.""" + + _CREATE_ROS_MESSAGE = None + _CONVERT_FROM_PY = None + _CONVERT_TO_PY = None + _DESTROY_ROS_MESSAGE = None + _TYPE_SUPPORT = None + + __constants = { + } + + @classmethod + def __import_type_support__(cls): + try: + from rosidl_generator_py import import_type_support + module = import_type_support('unitree_go') + except ImportError: + import logging + import traceback + logger = logging.getLogger( + 'unitree_go.msg.LowState') + logger.debug( + 'Failed to import needed modules for type support:\n' + + traceback.format_exc()) + else: + cls._CREATE_ROS_MESSAGE = module.create_ros_message_msg__msg__low_state + cls._CONVERT_FROM_PY = module.convert_from_py_msg__msg__low_state + cls._CONVERT_TO_PY = module.convert_to_py_msg__msg__low_state + cls._TYPE_SUPPORT = module.type_support_msg__msg__low_state + cls._DESTROY_ROS_MESSAGE = module.destroy_ros_message_msg__msg__low_state + + from unitree_go.msg import BmsState + if BmsState.__class__._TYPE_SUPPORT is None: + BmsState.__class__.__import_type_support__() + + from unitree_go.msg import IMUState + if IMUState.__class__._TYPE_SUPPORT is None: + IMUState.__class__.__import_type_support__() + + from unitree_go.msg import MotorState + if MotorState.__class__._TYPE_SUPPORT is None: + MotorState.__class__.__import_type_support__() + + @classmethod + def __prepare__(cls, name, bases, **kwargs): + # list constant names here so that they appear in the help text of + # the message class under "Data and other attributes defined here:" + # as well as populate each message instance + return { + } + + +class LowState(metaclass=Metaclass_LowState): + """Message class 'LowState'.""" + + __slots__ = [ + '_head', + '_level_flag', + '_frame_reserve', + '_sn', + '_version', + '_bandwidth', + '_imu_state', + '_motor_state', + '_bms_state', + '_foot_force', + '_foot_force_est', + '_tick', + '_wireless_remote', + '_bit_flag', + '_adc_reel', + '_temperature_ntc1', + '_temperature_ntc2', + '_power_v', + '_power_a', + '_fan_frequency', + '_reserve', + '_crc', + ] + + _fields_and_field_types = { + 'head': 'uint8[2]', + 'level_flag': 'uint8', + 'frame_reserve': 'uint8', + 'sn': 'uint32[2]', + 'version': 'uint32[2]', + 'bandwidth': 'uint16', + 'imu_state': 'unitree_go/IMUState', + 'motor_state': 'unitree_go/MotorState[20]', + 'bms_state': 'unitree_go/BmsState', + 'foot_force': 'int16[4]', + 'foot_force_est': 'int16[4]', + 'tick': 'uint32', + 'wireless_remote': 'uint8[40]', + 'bit_flag': 'uint8', + 'adc_reel': 'float', + 'temperature_ntc1': 'int8', + 'temperature_ntc2': 'int8', + 'power_v': 'float', + 'power_a': 'float', + 'fan_frequency': 'uint16[4]', + 'reserve': 'uint32', + 'crc': 'uint32', + } + + SLOT_TYPES = ( + rosidl_parser.definition.Array(rosidl_parser.definition.BasicType('uint8'), 2), # noqa: E501 + rosidl_parser.definition.BasicType('uint8'), # noqa: E501 + rosidl_parser.definition.BasicType('uint8'), # noqa: E501 + rosidl_parser.definition.Array(rosidl_parser.definition.BasicType('uint32'), 2), # noqa: E501 + rosidl_parser.definition.Array(rosidl_parser.definition.BasicType('uint32'), 2), # noqa: E501 + rosidl_parser.definition.BasicType('uint16'), # noqa: E501 + rosidl_parser.definition.NamespacedType(['unitree_go', 'msg'], 'IMUState'), # noqa: E501 + rosidl_parser.definition.Array(rosidl_parser.definition.NamespacedType(['unitree_go', 'msg'], 'MotorState'), 20), # noqa: E501 + rosidl_parser.definition.NamespacedType(['unitree_go', 'msg'], 'BmsState'), # noqa: E501 + rosidl_parser.definition.Array(rosidl_parser.definition.BasicType('int16'), 4), # noqa: E501 + rosidl_parser.definition.Array(rosidl_parser.definition.BasicType('int16'), 4), # noqa: E501 + rosidl_parser.definition.BasicType('uint32'), # noqa: E501 + rosidl_parser.definition.Array(rosidl_parser.definition.BasicType('uint8'), 40), # noqa: E501 + rosidl_parser.definition.BasicType('uint8'), # noqa: E501 + rosidl_parser.definition.BasicType('float'), # noqa: E501 + rosidl_parser.definition.BasicType('int8'), # noqa: E501 + rosidl_parser.definition.BasicType('int8'), # noqa: E501 + rosidl_parser.definition.BasicType('float'), # noqa: E501 + rosidl_parser.definition.BasicType('float'), # noqa: E501 + rosidl_parser.definition.Array(rosidl_parser.definition.BasicType('uint16'), 4), # noqa: E501 + rosidl_parser.definition.BasicType('uint32'), # noqa: E501 + rosidl_parser.definition.BasicType('uint32'), # noqa: E501 + ) + + def __init__(self, **kwargs): + assert all('_' + key in self.__slots__ for key in kwargs.keys()), \ + 'Invalid arguments passed to constructor: %s' % \ + ', '.join(sorted(k for k in kwargs.keys() if '_' + k not in self.__slots__)) + if 'head' not in kwargs: + self.head = numpy.zeros(2, dtype=numpy.uint8) + else: + self.head = numpy.array(kwargs.get('head'), dtype=numpy.uint8) + assert self.head.shape == (2, ) + self.level_flag = kwargs.get('level_flag', int()) + self.frame_reserve = kwargs.get('frame_reserve', int()) + if 'sn' not in kwargs: + self.sn = numpy.zeros(2, dtype=numpy.uint32) + else: + self.sn = numpy.array(kwargs.get('sn'), dtype=numpy.uint32) + assert self.sn.shape == (2, ) + if 'version' not in kwargs: + self.version = numpy.zeros(2, dtype=numpy.uint32) + else: + self.version = numpy.array(kwargs.get('version'), dtype=numpy.uint32) + assert self.version.shape == (2, ) + self.bandwidth = kwargs.get('bandwidth', int()) + from unitree_go.msg import IMUState + self.imu_state = kwargs.get('imu_state', IMUState()) + from unitree_go.msg import MotorState + self.motor_state = kwargs.get( + 'motor_state', + [MotorState() for x in range(20)] + ) + from unitree_go.msg import BmsState + self.bms_state = kwargs.get('bms_state', BmsState()) + if 'foot_force' not in kwargs: + self.foot_force = numpy.zeros(4, dtype=numpy.int16) + else: + self.foot_force = numpy.array(kwargs.get('foot_force'), dtype=numpy.int16) + assert self.foot_force.shape == (4, ) + if 'foot_force_est' not in kwargs: + self.foot_force_est = numpy.zeros(4, dtype=numpy.int16) + else: + self.foot_force_est = numpy.array(kwargs.get('foot_force_est'), dtype=numpy.int16) + assert self.foot_force_est.shape == (4, ) + self.tick = kwargs.get('tick', int()) + if 'wireless_remote' not in kwargs: + self.wireless_remote = numpy.zeros(40, dtype=numpy.uint8) + else: + self.wireless_remote = numpy.array(kwargs.get('wireless_remote'), dtype=numpy.uint8) + assert self.wireless_remote.shape == (40, ) + self.bit_flag = kwargs.get('bit_flag', int()) + self.adc_reel = kwargs.get('adc_reel', float()) + self.temperature_ntc1 = kwargs.get('temperature_ntc1', int()) + self.temperature_ntc2 = kwargs.get('temperature_ntc2', int()) + self.power_v = kwargs.get('power_v', float()) + self.power_a = kwargs.get('power_a', float()) + if 'fan_frequency' not in kwargs: + self.fan_frequency = numpy.zeros(4, dtype=numpy.uint16) + else: + self.fan_frequency = numpy.array(kwargs.get('fan_frequency'), dtype=numpy.uint16) + assert self.fan_frequency.shape == (4, ) + self.reserve = kwargs.get('reserve', int()) + self.crc = kwargs.get('crc', int()) + + def __repr__(self): + typename = self.__class__.__module__.split('.') + typename.pop() + typename.append(self.__class__.__name__) + args = [] + for s, t in zip(self.__slots__, self.SLOT_TYPES): + field = getattr(self, s) + fieldstr = repr(field) + # We use Python array type for fields that can be directly stored + # in them, and "normal" sequences for everything else. If it is + # a type that we store in an array, strip off the 'array' portion. + if ( + isinstance(t, rosidl_parser.definition.AbstractSequence) and + isinstance(t.value_type, rosidl_parser.definition.BasicType) and + t.value_type.typename in ['float', 'double', 'int8', 'uint8', 'int16', 'uint16', 'int32', 'uint32', 'int64', 'uint64'] + ): + if len(field) == 0: + fieldstr = '[]' + else: + assert fieldstr.startswith('array(') + prefix = "array('X', " + suffix = ')' + fieldstr = fieldstr[len(prefix):-len(suffix)] + args.append(s[1:] + '=' + fieldstr) + return '%s(%s)' % ('.'.join(typename), ', '.join(args)) + + def __eq__(self, other): + if not isinstance(other, self.__class__): + return False + if all(self.head != other.head): + return False + if self.level_flag != other.level_flag: + return False + if self.frame_reserve != other.frame_reserve: + return False + if all(self.sn != other.sn): + return False + if all(self.version != other.version): + return False + if self.bandwidth != other.bandwidth: + return False + if self.imu_state != other.imu_state: + return False + if self.motor_state != other.motor_state: + return False + if self.bms_state != other.bms_state: + return False + if all(self.foot_force != other.foot_force): + return False + if all(self.foot_force_est != other.foot_force_est): + return False + if self.tick != other.tick: + return False + if all(self.wireless_remote != other.wireless_remote): + return False + if self.bit_flag != other.bit_flag: + return False + if self.adc_reel != other.adc_reel: + return False + if self.temperature_ntc1 != other.temperature_ntc1: + return False + if self.temperature_ntc2 != other.temperature_ntc2: + return False + if self.power_v != other.power_v: + return False + if self.power_a != other.power_a: + return False + if all(self.fan_frequency != other.fan_frequency): + return False + if self.reserve != other.reserve: + return False + if self.crc != other.crc: + return False + return True + + @classmethod + def get_fields_and_field_types(cls): + from copy import copy + return copy(cls._fields_and_field_types) + + @property + def head(self): + """Message field 'head'.""" + return self._head + + @head.setter + def head(self, value): + if isinstance(value, numpy.ndarray): + assert value.dtype == numpy.uint8, \ + "The 'head' numpy.ndarray() must have the dtype of 'numpy.uint8'" + assert value.size == 2, \ + "The 'head' numpy.ndarray() must have a size of 2" + self._head = value + return + if __debug__: + from collections.abc import Sequence + from collections.abc import Set + from collections import UserList + from collections import UserString + assert \ + ((isinstance(value, Sequence) or + isinstance(value, Set) or + isinstance(value, UserList)) and + not isinstance(value, str) and + not isinstance(value, UserString) and + len(value) == 2 and + all(isinstance(v, int) for v in value) and + all(val >= 0 and val < 256 for val in value)), \ + "The 'head' field must be a set or sequence with length 2 and each value of type 'int' and each unsigned integer in [0, 255]" + self._head = numpy.array(value, dtype=numpy.uint8) + + @property + def level_flag(self): + """Message field 'level_flag'.""" + return self._level_flag + + @level_flag.setter + def level_flag(self, value): + if __debug__: + assert \ + isinstance(value, int), \ + "The 'level_flag' field must be of type 'int'" + assert value >= 0 and value < 256, \ + "The 'level_flag' field must be an unsigned integer in [0, 255]" + self._level_flag = value + + @property + def frame_reserve(self): + """Message field 'frame_reserve'.""" + return self._frame_reserve + + @frame_reserve.setter + def frame_reserve(self, value): + if __debug__: + assert \ + isinstance(value, int), \ + "The 'frame_reserve' field must be of type 'int'" + assert value >= 0 and value < 256, \ + "The 'frame_reserve' field must be an unsigned integer in [0, 255]" + self._frame_reserve = value + + @property + def sn(self): + """Message field 'sn'.""" + return self._sn + + @sn.setter + def sn(self, value): + if isinstance(value, numpy.ndarray): + assert value.dtype == numpy.uint32, \ + "The 'sn' numpy.ndarray() must have the dtype of 'numpy.uint32'" + assert value.size == 2, \ + "The 'sn' numpy.ndarray() must have a size of 2" + self._sn = value + return + if __debug__: + from collections.abc import Sequence + from collections.abc import Set + from collections import UserList + from collections import UserString + assert \ + ((isinstance(value, Sequence) or + isinstance(value, Set) or + isinstance(value, UserList)) and + not isinstance(value, str) and + not isinstance(value, UserString) and + len(value) == 2 and + all(isinstance(v, int) for v in value) and + all(val >= 0 and val < 4294967296 for val in value)), \ + "The 'sn' field must be a set or sequence with length 2 and each value of type 'int' and each unsigned integer in [0, 4294967295]" + self._sn = numpy.array(value, dtype=numpy.uint32) + + @property + def version(self): + """Message field 'version'.""" + return self._version + + @version.setter + def version(self, value): + if isinstance(value, numpy.ndarray): + assert value.dtype == numpy.uint32, \ + "The 'version' numpy.ndarray() must have the dtype of 'numpy.uint32'" + assert value.size == 2, \ + "The 'version' numpy.ndarray() must have a size of 2" + self._version = value + return + if __debug__: + from collections.abc import Sequence + from collections.abc import Set + from collections import UserList + from collections import UserString + assert \ + ((isinstance(value, Sequence) or + isinstance(value, Set) or + isinstance(value, UserList)) and + not isinstance(value, str) and + not isinstance(value, UserString) and + len(value) == 2 and + all(isinstance(v, int) for v in value) and + all(val >= 0 and val < 4294967296 for val in value)), \ + "The 'version' field must be a set or sequence with length 2 and each value of type 'int' and each unsigned integer in [0, 4294967295]" + self._version = numpy.array(value, dtype=numpy.uint32) + + @property + def bandwidth(self): + """Message field 'bandwidth'.""" + return self._bandwidth + + @bandwidth.setter + def bandwidth(self, value): + if __debug__: + assert \ + isinstance(value, int), \ + "The 'bandwidth' field must be of type 'int'" + assert value >= 0 and value < 65536, \ + "The 'bandwidth' field must be an unsigned integer in [0, 65535]" + self._bandwidth = value + + @property + def imu_state(self): + """Message field 'imu_state'.""" + return self._imu_state + + @imu_state.setter + def imu_state(self, value): + if __debug__: + from unitree_go.msg import IMUState + assert \ + isinstance(value, IMUState), \ + "The 'imu_state' field must be a sub message of type 'IMUState'" + self._imu_state = value + + @property + def motor_state(self): + """Message field 'motor_state'.""" + return self._motor_state + + @motor_state.setter + def motor_state(self, value): + if __debug__: + from unitree_go.msg import MotorState + from collections.abc import Sequence + from collections.abc import Set + from collections import UserList + from collections import UserString + assert \ + ((isinstance(value, Sequence) or + isinstance(value, Set) or + isinstance(value, UserList)) and + not isinstance(value, str) and + not isinstance(value, UserString) and + len(value) == 20 and + all(isinstance(v, MotorState) for v in value) and + True), \ + "The 'motor_state' field must be a set or sequence with length 20 and each value of type 'MotorState'" + self._motor_state = value + + @property + def bms_state(self): + """Message field 'bms_state'.""" + return self._bms_state + + @bms_state.setter + def bms_state(self, value): + if __debug__: + from unitree_go.msg import BmsState + assert \ + isinstance(value, BmsState), \ + "The 'bms_state' field must be a sub message of type 'BmsState'" + self._bms_state = value + + @property + def foot_force(self): + """Message field 'foot_force'.""" + return self._foot_force + + @foot_force.setter + def foot_force(self, value): + if isinstance(value, numpy.ndarray): + assert value.dtype == numpy.int16, \ + "The 'foot_force' numpy.ndarray() must have the dtype of 'numpy.int16'" + assert value.size == 4, \ + "The 'foot_force' numpy.ndarray() must have a size of 4" + self._foot_force = value + return + if __debug__: + from collections.abc import Sequence + from collections.abc import Set + from collections import UserList + from collections import UserString + assert \ + ((isinstance(value, Sequence) or + isinstance(value, Set) or + isinstance(value, UserList)) and + not isinstance(value, str) and + not isinstance(value, UserString) and + len(value) == 4 and + all(isinstance(v, int) for v in value) and + all(val >= -32768 and val < 32768 for val in value)), \ + "The 'foot_force' field must be a set or sequence with length 4 and each value of type 'int' and each integer in [-32768, 32767]" + self._foot_force = numpy.array(value, dtype=numpy.int16) + + @property + def foot_force_est(self): + """Message field 'foot_force_est'.""" + return self._foot_force_est + + @foot_force_est.setter + def foot_force_est(self, value): + if isinstance(value, numpy.ndarray): + assert value.dtype == numpy.int16, \ + "The 'foot_force_est' numpy.ndarray() must have the dtype of 'numpy.int16'" + assert value.size == 4, \ + "The 'foot_force_est' numpy.ndarray() must have a size of 4" + self._foot_force_est = value + return + if __debug__: + from collections.abc import Sequence + from collections.abc import Set + from collections import UserList + from collections import UserString + assert \ + ((isinstance(value, Sequence) or + isinstance(value, Set) or + isinstance(value, UserList)) and + not isinstance(value, str) and + not isinstance(value, UserString) and + len(value) == 4 and + all(isinstance(v, int) for v in value) and + all(val >= -32768 and val < 32768 for val in value)), \ + "The 'foot_force_est' field must be a set or sequence with length 4 and each value of type 'int' and each integer in [-32768, 32767]" + self._foot_force_est = numpy.array(value, dtype=numpy.int16) + + @property + def tick(self): + """Message field 'tick'.""" + return self._tick + + @tick.setter + def tick(self, value): + if __debug__: + assert \ + isinstance(value, int), \ + "The 'tick' field must be of type 'int'" + assert value >= 0 and value < 4294967296, \ + "The 'tick' field must be an unsigned integer in [0, 4294967295]" + self._tick = value + + @property + def wireless_remote(self): + """Message field 'wireless_remote'.""" + return self._wireless_remote + + @wireless_remote.setter + def wireless_remote(self, value): + if isinstance(value, numpy.ndarray): + assert value.dtype == numpy.uint8, \ + "The 'wireless_remote' numpy.ndarray() must have the dtype of 'numpy.uint8'" + assert value.size == 40, \ + "The 'wireless_remote' numpy.ndarray() must have a size of 40" + self._wireless_remote = value + return + if __debug__: + from collections.abc import Sequence + from collections.abc import Set + from collections import UserList + from collections import UserString + assert \ + ((isinstance(value, Sequence) or + isinstance(value, Set) or + isinstance(value, UserList)) and + not isinstance(value, str) and + not isinstance(value, UserString) and + len(value) == 40 and + all(isinstance(v, int) for v in value) and + all(val >= 0 and val < 256 for val in value)), \ + "The 'wireless_remote' field must be a set or sequence with length 40 and each value of type 'int' and each unsigned integer in [0, 255]" + self._wireless_remote = numpy.array(value, dtype=numpy.uint8) + + @property + def bit_flag(self): + """Message field 'bit_flag'.""" + return self._bit_flag + + @bit_flag.setter + def bit_flag(self, value): + if __debug__: + assert \ + isinstance(value, int), \ + "The 'bit_flag' field must be of type 'int'" + assert value >= 0 and value < 256, \ + "The 'bit_flag' field must be an unsigned integer in [0, 255]" + self._bit_flag = value + + @property + def adc_reel(self): + """Message field 'adc_reel'.""" + return self._adc_reel + + @adc_reel.setter + def adc_reel(self, value): + if __debug__: + assert \ + isinstance(value, float), \ + "The 'adc_reel' field must be of type 'float'" + self._adc_reel = value + + @property + def temperature_ntc1(self): + """Message field 'temperature_ntc1'.""" + return self._temperature_ntc1 + + @temperature_ntc1.setter + def temperature_ntc1(self, value): + if __debug__: + assert \ + isinstance(value, int), \ + "The 'temperature_ntc1' field must be of type 'int'" + assert value >= -128 and value < 128, \ + "The 'temperature_ntc1' field must be an integer in [-128, 127]" + self._temperature_ntc1 = value + + @property + def temperature_ntc2(self): + """Message field 'temperature_ntc2'.""" + return self._temperature_ntc2 + + @temperature_ntc2.setter + def temperature_ntc2(self, value): + if __debug__: + assert \ + isinstance(value, int), \ + "The 'temperature_ntc2' field must be of type 'int'" + assert value >= -128 and value < 128, \ + "The 'temperature_ntc2' field must be an integer in [-128, 127]" + self._temperature_ntc2 = value + + @property + def power_v(self): + """Message field 'power_v'.""" + return self._power_v + + @power_v.setter + def power_v(self, value): + if __debug__: + assert \ + isinstance(value, float), \ + "The 'power_v' field must be of type 'float'" + self._power_v = value + + @property + def power_a(self): + """Message field 'power_a'.""" + return self._power_a + + @power_a.setter + def power_a(self, value): + if __debug__: + assert \ + isinstance(value, float), \ + "The 'power_a' field must be of type 'float'" + self._power_a = value + + @property + def fan_frequency(self): + """Message field 'fan_frequency'.""" + return self._fan_frequency + + @fan_frequency.setter + def fan_frequency(self, value): + if isinstance(value, numpy.ndarray): + assert value.dtype == numpy.uint16, \ + "The 'fan_frequency' numpy.ndarray() must have the dtype of 'numpy.uint16'" + assert value.size == 4, \ + "The 'fan_frequency' numpy.ndarray() must have a size of 4" + self._fan_frequency = value + return + if __debug__: + from collections.abc import Sequence + from collections.abc import Set + from collections import UserList + from collections import UserString + assert \ + ((isinstance(value, Sequence) or + isinstance(value, Set) or + isinstance(value, UserList)) and + not isinstance(value, str) and + not isinstance(value, UserString) and + len(value) == 4 and + all(isinstance(v, int) for v in value) and + all(val >= 0 and val < 65536 for val in value)), \ + "The 'fan_frequency' field must be a set or sequence with length 4 and each value of type 'int' and each unsigned integer in [0, 65535]" + self._fan_frequency = numpy.array(value, dtype=numpy.uint16) + + @property + def reserve(self): + """Message field 'reserve'.""" + return self._reserve + + @reserve.setter + def reserve(self, value): + if __debug__: + assert \ + isinstance(value, int), \ + "The 'reserve' field must be of type 'int'" + assert value >= 0 and value < 4294967296, \ + "The 'reserve' field must be an unsigned integer in [0, 4294967295]" + self._reserve = value + + @property + def crc(self): + """Message field 'crc'.""" + return self._crc + + @crc.setter + def crc(self, value): + if __debug__: + assert \ + isinstance(value, int), \ + "The 'crc' field must be of type 'int'" + assert value >= 0 and value < 4294967296, \ + "The 'crc' field must be an unsigned integer in [0, 4294967295]" + self._crc = value diff --git a/python/unitree_go/msg/_low_state_s.c b/python/unitree_go/msg/_low_state_s.c new file mode 100644 index 00000000..ad3c234c --- /dev/null +++ b/python/unitree_go/msg/_low_state_s.c @@ -0,0 +1,724 @@ +// generated from rosidl_generator_py/resource/_idl_support.c.em +// with input from unitree_go:msg/LowState.idl +// generated code does not contain a copyright notice +#define NPY_NO_DEPRECATED_API NPY_1_7_API_VERSION +#include +#include +#ifndef _WIN32 +# pragma GCC diagnostic push +# pragma GCC diagnostic ignored "-Wunused-function" +#endif +#include "numpy/ndarrayobject.h" +#ifndef _WIN32 +# pragma GCC diagnostic pop +#endif +#include "rosidl_runtime_c/visibility_control.h" +#include "unitree_go/msg/detail/low_state__struct.h" +#include "unitree_go/msg/detail/low_state__functions.h" + +#include "rosidl_runtime_c/primitives_sequence.h" +#include "rosidl_runtime_c/primitives_sequence_functions.h" + +// Nested array functions includes +#include "unitree_go/msg/detail/motor_state__functions.h" +// end nested array functions include +bool unitree_go__msg__imu_state__convert_from_py(PyObject * _pymsg, void * _ros_message); +PyObject * unitree_go__msg__imu_state__convert_to_py(void * raw_ros_message); +bool unitree_go__msg__motor_state__convert_from_py(PyObject * _pymsg, void * _ros_message); +PyObject * unitree_go__msg__motor_state__convert_to_py(void * raw_ros_message); +bool unitree_go__msg__bms_state__convert_from_py(PyObject * _pymsg, void * _ros_message); +PyObject * unitree_go__msg__bms_state__convert_to_py(void * raw_ros_message); + +ROSIDL_GENERATOR_C_EXPORT +bool unitree_go__msg__low_state__convert_from_py(PyObject * _pymsg, void * _ros_message) +{ + // check that the passed message is of the expected Python class + { + char full_classname_dest[35]; + { + char * class_name = NULL; + char * module_name = NULL; + { + PyObject * class_attr = PyObject_GetAttrString(_pymsg, "__class__"); + if (class_attr) { + PyObject * name_attr = PyObject_GetAttrString(class_attr, "__name__"); + if (name_attr) { + class_name = (char *)PyUnicode_1BYTE_DATA(name_attr); + Py_DECREF(name_attr); + } + PyObject * module_attr = PyObject_GetAttrString(class_attr, "__module__"); + if (module_attr) { + module_name = (char *)PyUnicode_1BYTE_DATA(module_attr); + Py_DECREF(module_attr); + } + Py_DECREF(class_attr); + } + } + if (!class_name || !module_name) { + return false; + } + snprintf(full_classname_dest, sizeof(full_classname_dest), "%s.%s", module_name, class_name); + } + assert(strncmp("unitree_go.msg._low_state.LowState", full_classname_dest, 34) == 0); + } + unitree_go__msg__LowState * ros_message = _ros_message; + { // head + PyObject * field = PyObject_GetAttrString(_pymsg, "head"); + if (!field) { + return false; + } + { + // TODO(dirk-thomas) use a better way to check the type before casting + assert(field->ob_type != NULL); + assert(field->ob_type->tp_name != NULL); + assert(strcmp(field->ob_type->tp_name, "numpy.ndarray") == 0); + PyArrayObject * seq_field = (PyArrayObject *)field; + Py_INCREF(seq_field); + assert(PyArray_NDIM(seq_field) == 1); + assert(PyArray_TYPE(seq_field) == NPY_UINT8); + Py_ssize_t size = 2; + uint8_t * dest = ros_message->head; + for (Py_ssize_t i = 0; i < size; ++i) { + uint8_t tmp = *(npy_uint8 *)PyArray_GETPTR1(seq_field, i); + memcpy(&dest[i], &tmp, sizeof(uint8_t)); + } + Py_DECREF(seq_field); + } + Py_DECREF(field); + } + { // level_flag + PyObject * field = PyObject_GetAttrString(_pymsg, "level_flag"); + if (!field) { + return false; + } + assert(PyLong_Check(field)); + ros_message->level_flag = (uint8_t)PyLong_AsUnsignedLong(field); + Py_DECREF(field); + } + { // frame_reserve + PyObject * field = PyObject_GetAttrString(_pymsg, "frame_reserve"); + if (!field) { + return false; + } + assert(PyLong_Check(field)); + ros_message->frame_reserve = (uint8_t)PyLong_AsUnsignedLong(field); + Py_DECREF(field); + } + { // sn + PyObject * field = PyObject_GetAttrString(_pymsg, "sn"); + if (!field) { + return false; + } + { + // TODO(dirk-thomas) use a better way to check the type before casting + assert(field->ob_type != NULL); + assert(field->ob_type->tp_name != NULL); + assert(strcmp(field->ob_type->tp_name, "numpy.ndarray") == 0); + PyArrayObject * seq_field = (PyArrayObject *)field; + Py_INCREF(seq_field); + assert(PyArray_NDIM(seq_field) == 1); + assert(PyArray_TYPE(seq_field) == NPY_UINT32); + Py_ssize_t size = 2; + uint32_t * dest = ros_message->sn; + for (Py_ssize_t i = 0; i < size; ++i) { + uint32_t tmp = *(npy_uint32 *)PyArray_GETPTR1(seq_field, i); + memcpy(&dest[i], &tmp, sizeof(uint32_t)); + } + Py_DECREF(seq_field); + } + Py_DECREF(field); + } + { // version + PyObject * field = PyObject_GetAttrString(_pymsg, "version"); + if (!field) { + return false; + } + { + // TODO(dirk-thomas) use a better way to check the type before casting + assert(field->ob_type != NULL); + assert(field->ob_type->tp_name != NULL); + assert(strcmp(field->ob_type->tp_name, "numpy.ndarray") == 0); + PyArrayObject * seq_field = (PyArrayObject *)field; + Py_INCREF(seq_field); + assert(PyArray_NDIM(seq_field) == 1); + assert(PyArray_TYPE(seq_field) == NPY_UINT32); + Py_ssize_t size = 2; + uint32_t * dest = ros_message->version; + for (Py_ssize_t i = 0; i < size; ++i) { + uint32_t tmp = *(npy_uint32 *)PyArray_GETPTR1(seq_field, i); + memcpy(&dest[i], &tmp, sizeof(uint32_t)); + } + Py_DECREF(seq_field); + } + Py_DECREF(field); + } + { // bandwidth + PyObject * field = PyObject_GetAttrString(_pymsg, "bandwidth"); + if (!field) { + return false; + } + assert(PyLong_Check(field)); + ros_message->bandwidth = (uint16_t)PyLong_AsUnsignedLong(field); + Py_DECREF(field); + } + { // imu_state + PyObject * field = PyObject_GetAttrString(_pymsg, "imu_state"); + if (!field) { + return false; + } + if (!unitree_go__msg__imu_state__convert_from_py(field, &ros_message->imu_state)) { + Py_DECREF(field); + return false; + } + Py_DECREF(field); + } + { // motor_state + PyObject * field = PyObject_GetAttrString(_pymsg, "motor_state"); + if (!field) { + return false; + } + PyObject * seq_field = PySequence_Fast(field, "expected a sequence in 'motor_state'"); + if (!seq_field) { + Py_DECREF(field); + return false; + } + Py_ssize_t size = 20; + unitree_go__msg__MotorState * dest = ros_message->motor_state; + for (Py_ssize_t i = 0; i < size; ++i) { + if (!unitree_go__msg__motor_state__convert_from_py(PySequence_Fast_GET_ITEM(seq_field, i), &dest[i])) { + Py_DECREF(seq_field); + Py_DECREF(field); + return false; + } + } + Py_DECREF(seq_field); + Py_DECREF(field); + } + { // bms_state + PyObject * field = PyObject_GetAttrString(_pymsg, "bms_state"); + if (!field) { + return false; + } + if (!unitree_go__msg__bms_state__convert_from_py(field, &ros_message->bms_state)) { + Py_DECREF(field); + return false; + } + Py_DECREF(field); + } + { // foot_force + PyObject * field = PyObject_GetAttrString(_pymsg, "foot_force"); + if (!field) { + return false; + } + { + // TODO(dirk-thomas) use a better way to check the type before casting + assert(field->ob_type != NULL); + assert(field->ob_type->tp_name != NULL); + assert(strcmp(field->ob_type->tp_name, "numpy.ndarray") == 0); + PyArrayObject * seq_field = (PyArrayObject *)field; + Py_INCREF(seq_field); + assert(PyArray_NDIM(seq_field) == 1); + assert(PyArray_TYPE(seq_field) == NPY_INT16); + Py_ssize_t size = 4; + int16_t * dest = ros_message->foot_force; + for (Py_ssize_t i = 0; i < size; ++i) { + int16_t tmp = *(npy_int16 *)PyArray_GETPTR1(seq_field, i); + memcpy(&dest[i], &tmp, sizeof(int16_t)); + } + Py_DECREF(seq_field); + } + Py_DECREF(field); + } + { // foot_force_est + PyObject * field = PyObject_GetAttrString(_pymsg, "foot_force_est"); + if (!field) { + return false; + } + { + // TODO(dirk-thomas) use a better way to check the type before casting + assert(field->ob_type != NULL); + assert(field->ob_type->tp_name != NULL); + assert(strcmp(field->ob_type->tp_name, "numpy.ndarray") == 0); + PyArrayObject * seq_field = (PyArrayObject *)field; + Py_INCREF(seq_field); + assert(PyArray_NDIM(seq_field) == 1); + assert(PyArray_TYPE(seq_field) == NPY_INT16); + Py_ssize_t size = 4; + int16_t * dest = ros_message->foot_force_est; + for (Py_ssize_t i = 0; i < size; ++i) { + int16_t tmp = *(npy_int16 *)PyArray_GETPTR1(seq_field, i); + memcpy(&dest[i], &tmp, sizeof(int16_t)); + } + Py_DECREF(seq_field); + } + Py_DECREF(field); + } + { // tick + PyObject * field = PyObject_GetAttrString(_pymsg, "tick"); + if (!field) { + return false; + } + assert(PyLong_Check(field)); + ros_message->tick = PyLong_AsUnsignedLong(field); + Py_DECREF(field); + } + { // wireless_remote + PyObject * field = PyObject_GetAttrString(_pymsg, "wireless_remote"); + if (!field) { + return false; + } + { + // TODO(dirk-thomas) use a better way to check the type before casting + assert(field->ob_type != NULL); + assert(field->ob_type->tp_name != NULL); + assert(strcmp(field->ob_type->tp_name, "numpy.ndarray") == 0); + PyArrayObject * seq_field = (PyArrayObject *)field; + Py_INCREF(seq_field); + assert(PyArray_NDIM(seq_field) == 1); + assert(PyArray_TYPE(seq_field) == NPY_UINT8); + Py_ssize_t size = 40; + uint8_t * dest = ros_message->wireless_remote; + for (Py_ssize_t i = 0; i < size; ++i) { + uint8_t tmp = *(npy_uint8 *)PyArray_GETPTR1(seq_field, i); + memcpy(&dest[i], &tmp, sizeof(uint8_t)); + } + Py_DECREF(seq_field); + } + Py_DECREF(field); + } + { // bit_flag + PyObject * field = PyObject_GetAttrString(_pymsg, "bit_flag"); + if (!field) { + return false; + } + assert(PyLong_Check(field)); + ros_message->bit_flag = (uint8_t)PyLong_AsUnsignedLong(field); + Py_DECREF(field); + } + { // adc_reel + PyObject * field = PyObject_GetAttrString(_pymsg, "adc_reel"); + if (!field) { + return false; + } + assert(PyFloat_Check(field)); + ros_message->adc_reel = (float)PyFloat_AS_DOUBLE(field); + Py_DECREF(field); + } + { // temperature_ntc1 + PyObject * field = PyObject_GetAttrString(_pymsg, "temperature_ntc1"); + if (!field) { + return false; + } + assert(PyLong_Check(field)); + ros_message->temperature_ntc1 = (int8_t)PyLong_AsLong(field); + Py_DECREF(field); + } + { // temperature_ntc2 + PyObject * field = PyObject_GetAttrString(_pymsg, "temperature_ntc2"); + if (!field) { + return false; + } + assert(PyLong_Check(field)); + ros_message->temperature_ntc2 = (int8_t)PyLong_AsLong(field); + Py_DECREF(field); + } + { // power_v + PyObject * field = PyObject_GetAttrString(_pymsg, "power_v"); + if (!field) { + return false; + } + assert(PyFloat_Check(field)); + ros_message->power_v = (float)PyFloat_AS_DOUBLE(field); + Py_DECREF(field); + } + { // power_a + PyObject * field = PyObject_GetAttrString(_pymsg, "power_a"); + if (!field) { + return false; + } + assert(PyFloat_Check(field)); + ros_message->power_a = (float)PyFloat_AS_DOUBLE(field); + Py_DECREF(field); + } + { // fan_frequency + PyObject * field = PyObject_GetAttrString(_pymsg, "fan_frequency"); + if (!field) { + return false; + } + { + // TODO(dirk-thomas) use a better way to check the type before casting + assert(field->ob_type != NULL); + assert(field->ob_type->tp_name != NULL); + assert(strcmp(field->ob_type->tp_name, "numpy.ndarray") == 0); + PyArrayObject * seq_field = (PyArrayObject *)field; + Py_INCREF(seq_field); + assert(PyArray_NDIM(seq_field) == 1); + assert(PyArray_TYPE(seq_field) == NPY_UINT16); + Py_ssize_t size = 4; + uint16_t * dest = ros_message->fan_frequency; + for (Py_ssize_t i = 0; i < size; ++i) { + uint16_t tmp = *(npy_uint16 *)PyArray_GETPTR1(seq_field, i); + memcpy(&dest[i], &tmp, sizeof(uint16_t)); + } + Py_DECREF(seq_field); + } + Py_DECREF(field); + } + { // reserve + PyObject * field = PyObject_GetAttrString(_pymsg, "reserve"); + if (!field) { + return false; + } + assert(PyLong_Check(field)); + ros_message->reserve = PyLong_AsUnsignedLong(field); + Py_DECREF(field); + } + { // crc + PyObject * field = PyObject_GetAttrString(_pymsg, "crc"); + if (!field) { + return false; + } + assert(PyLong_Check(field)); + ros_message->crc = PyLong_AsUnsignedLong(field); + Py_DECREF(field); + } + + return true; +} + +ROSIDL_GENERATOR_C_EXPORT +PyObject * unitree_go__msg__low_state__convert_to_py(void * raw_ros_message) +{ + /* NOTE(esteve): Call constructor of LowState */ + PyObject * _pymessage = NULL; + { + PyObject * pymessage_module = PyImport_ImportModule("unitree_go.msg._low_state"); + assert(pymessage_module); + PyObject * pymessage_class = PyObject_GetAttrString(pymessage_module, "LowState"); + assert(pymessage_class); + Py_DECREF(pymessage_module); + _pymessage = PyObject_CallObject(pymessage_class, NULL); + Py_DECREF(pymessage_class); + if (!_pymessage) { + return NULL; + } + } + unitree_go__msg__LowState * ros_message = (unitree_go__msg__LowState *)raw_ros_message; + { // head + PyObject * field = NULL; + field = PyObject_GetAttrString(_pymessage, "head"); + if (!field) { + return NULL; + } + assert(field->ob_type != NULL); + assert(field->ob_type->tp_name != NULL); + assert(strcmp(field->ob_type->tp_name, "numpy.ndarray") == 0); + PyArrayObject * seq_field = (PyArrayObject *)field; + assert(PyArray_NDIM(seq_field) == 1); + assert(PyArray_TYPE(seq_field) == NPY_UINT8); + assert(sizeof(npy_uint8) == sizeof(uint8_t)); + npy_uint8 * dst = (npy_uint8 *)PyArray_GETPTR1(seq_field, 0); + uint8_t * src = &(ros_message->head[0]); + memcpy(dst, src, 2 * sizeof(uint8_t)); + Py_DECREF(field); + } + { // level_flag + PyObject * field = NULL; + field = PyLong_FromUnsignedLong(ros_message->level_flag); + { + int rc = PyObject_SetAttrString(_pymessage, "level_flag", field); + Py_DECREF(field); + if (rc) { + return NULL; + } + } + } + { // frame_reserve + PyObject * field = NULL; + field = PyLong_FromUnsignedLong(ros_message->frame_reserve); + { + int rc = PyObject_SetAttrString(_pymessage, "frame_reserve", field); + Py_DECREF(field); + if (rc) { + return NULL; + } + } + } + { // sn + PyObject * field = NULL; + field = PyObject_GetAttrString(_pymessage, "sn"); + if (!field) { + return NULL; + } + assert(field->ob_type != NULL); + assert(field->ob_type->tp_name != NULL); + assert(strcmp(field->ob_type->tp_name, "numpy.ndarray") == 0); + PyArrayObject * seq_field = (PyArrayObject *)field; + assert(PyArray_NDIM(seq_field) == 1); + assert(PyArray_TYPE(seq_field) == NPY_UINT32); + assert(sizeof(npy_uint32) == sizeof(uint32_t)); + npy_uint32 * dst = (npy_uint32 *)PyArray_GETPTR1(seq_field, 0); + uint32_t * src = &(ros_message->sn[0]); + memcpy(dst, src, 2 * sizeof(uint32_t)); + Py_DECREF(field); + } + { // version + PyObject * field = NULL; + field = PyObject_GetAttrString(_pymessage, "version"); + if (!field) { + return NULL; + } + assert(field->ob_type != NULL); + assert(field->ob_type->tp_name != NULL); + assert(strcmp(field->ob_type->tp_name, "numpy.ndarray") == 0); + PyArrayObject * seq_field = (PyArrayObject *)field; + assert(PyArray_NDIM(seq_field) == 1); + assert(PyArray_TYPE(seq_field) == NPY_UINT32); + assert(sizeof(npy_uint32) == sizeof(uint32_t)); + npy_uint32 * dst = (npy_uint32 *)PyArray_GETPTR1(seq_field, 0); + uint32_t * src = &(ros_message->version[0]); + memcpy(dst, src, 2 * sizeof(uint32_t)); + Py_DECREF(field); + } + { // bandwidth + PyObject * field = NULL; + field = PyLong_FromUnsignedLong(ros_message->bandwidth); + { + int rc = PyObject_SetAttrString(_pymessage, "bandwidth", field); + Py_DECREF(field); + if (rc) { + return NULL; + } + } + } + { // imu_state + PyObject * field = NULL; + field = unitree_go__msg__imu_state__convert_to_py(&ros_message->imu_state); + if (!field) { + return NULL; + } + { + int rc = PyObject_SetAttrString(_pymessage, "imu_state", field); + Py_DECREF(field); + if (rc) { + return NULL; + } + } + } + { // motor_state + PyObject * field = NULL; + size_t size = 20; + field = PyList_New(size); + if (!field) { + return NULL; + } + unitree_go__msg__MotorState * item; + for (size_t i = 0; i < size; ++i) { + item = &(ros_message->motor_state[i]); + PyObject * pyitem = unitree_go__msg__motor_state__convert_to_py(item); + if (!pyitem) { + Py_DECREF(field); + return NULL; + } + int rc = PyList_SetItem(field, i, pyitem); + (void)rc; + assert(rc == 0); + } + assert(PySequence_Check(field)); + { + int rc = PyObject_SetAttrString(_pymessage, "motor_state", field); + Py_DECREF(field); + if (rc) { + return NULL; + } + } + } + { // bms_state + PyObject * field = NULL; + field = unitree_go__msg__bms_state__convert_to_py(&ros_message->bms_state); + if (!field) { + return NULL; + } + { + int rc = PyObject_SetAttrString(_pymessage, "bms_state", field); + Py_DECREF(field); + if (rc) { + return NULL; + } + } + } + { // foot_force + PyObject * field = NULL; + field = PyObject_GetAttrString(_pymessage, "foot_force"); + if (!field) { + return NULL; + } + assert(field->ob_type != NULL); + assert(field->ob_type->tp_name != NULL); + assert(strcmp(field->ob_type->tp_name, "numpy.ndarray") == 0); + PyArrayObject * seq_field = (PyArrayObject *)field; + assert(PyArray_NDIM(seq_field) == 1); + assert(PyArray_TYPE(seq_field) == NPY_INT16); + assert(sizeof(npy_int16) == sizeof(int16_t)); + npy_int16 * dst = (npy_int16 *)PyArray_GETPTR1(seq_field, 0); + int16_t * src = &(ros_message->foot_force[0]); + memcpy(dst, src, 4 * sizeof(int16_t)); + Py_DECREF(field); + } + { // foot_force_est + PyObject * field = NULL; + field = PyObject_GetAttrString(_pymessage, "foot_force_est"); + if (!field) { + return NULL; + } + assert(field->ob_type != NULL); + assert(field->ob_type->tp_name != NULL); + assert(strcmp(field->ob_type->tp_name, "numpy.ndarray") == 0); + PyArrayObject * seq_field = (PyArrayObject *)field; + assert(PyArray_NDIM(seq_field) == 1); + assert(PyArray_TYPE(seq_field) == NPY_INT16); + assert(sizeof(npy_int16) == sizeof(int16_t)); + npy_int16 * dst = (npy_int16 *)PyArray_GETPTR1(seq_field, 0); + int16_t * src = &(ros_message->foot_force_est[0]); + memcpy(dst, src, 4 * sizeof(int16_t)); + Py_DECREF(field); + } + { // tick + PyObject * field = NULL; + field = PyLong_FromUnsignedLong(ros_message->tick); + { + int rc = PyObject_SetAttrString(_pymessage, "tick", field); + Py_DECREF(field); + if (rc) { + return NULL; + } + } + } + { // wireless_remote + PyObject * field = NULL; + field = PyObject_GetAttrString(_pymessage, "wireless_remote"); + if (!field) { + return NULL; + } + assert(field->ob_type != NULL); + assert(field->ob_type->tp_name != NULL); + assert(strcmp(field->ob_type->tp_name, "numpy.ndarray") == 0); + PyArrayObject * seq_field = (PyArrayObject *)field; + assert(PyArray_NDIM(seq_field) == 1); + assert(PyArray_TYPE(seq_field) == NPY_UINT8); + assert(sizeof(npy_uint8) == sizeof(uint8_t)); + npy_uint8 * dst = (npy_uint8 *)PyArray_GETPTR1(seq_field, 0); + uint8_t * src = &(ros_message->wireless_remote[0]); + memcpy(dst, src, 40 * sizeof(uint8_t)); + Py_DECREF(field); + } + { // bit_flag + PyObject * field = NULL; + field = PyLong_FromUnsignedLong(ros_message->bit_flag); + { + int rc = PyObject_SetAttrString(_pymessage, "bit_flag", field); + Py_DECREF(field); + if (rc) { + return NULL; + } + } + } + { // adc_reel + PyObject * field = NULL; + field = PyFloat_FromDouble(ros_message->adc_reel); + { + int rc = PyObject_SetAttrString(_pymessage, "adc_reel", field); + Py_DECREF(field); + if (rc) { + return NULL; + } + } + } + { // temperature_ntc1 + PyObject * field = NULL; + field = PyLong_FromLong(ros_message->temperature_ntc1); + { + int rc = PyObject_SetAttrString(_pymessage, "temperature_ntc1", field); + Py_DECREF(field); + if (rc) { + return NULL; + } + } + } + { // temperature_ntc2 + PyObject * field = NULL; + field = PyLong_FromLong(ros_message->temperature_ntc2); + { + int rc = PyObject_SetAttrString(_pymessage, "temperature_ntc2", field); + Py_DECREF(field); + if (rc) { + return NULL; + } + } + } + { // power_v + PyObject * field = NULL; + field = PyFloat_FromDouble(ros_message->power_v); + { + int rc = PyObject_SetAttrString(_pymessage, "power_v", field); + Py_DECREF(field); + if (rc) { + return NULL; + } + } + } + { // power_a + PyObject * field = NULL; + field = PyFloat_FromDouble(ros_message->power_a); + { + int rc = PyObject_SetAttrString(_pymessage, "power_a", field); + Py_DECREF(field); + if (rc) { + return NULL; + } + } + } + { // fan_frequency + PyObject * field = NULL; + field = PyObject_GetAttrString(_pymessage, "fan_frequency"); + if (!field) { + return NULL; + } + assert(field->ob_type != NULL); + assert(field->ob_type->tp_name != NULL); + assert(strcmp(field->ob_type->tp_name, "numpy.ndarray") == 0); + PyArrayObject * seq_field = (PyArrayObject *)field; + assert(PyArray_NDIM(seq_field) == 1); + assert(PyArray_TYPE(seq_field) == NPY_UINT16); + assert(sizeof(npy_uint16) == sizeof(uint16_t)); + npy_uint16 * dst = (npy_uint16 *)PyArray_GETPTR1(seq_field, 0); + uint16_t * src = &(ros_message->fan_frequency[0]); + memcpy(dst, src, 4 * sizeof(uint16_t)); + Py_DECREF(field); + } + { // reserve + PyObject * field = NULL; + field = PyLong_FromUnsignedLong(ros_message->reserve); + { + int rc = PyObject_SetAttrString(_pymessage, "reserve", field); + Py_DECREF(field); + if (rc) { + return NULL; + } + } + } + { // crc + PyObject * field = NULL; + field = PyLong_FromUnsignedLong(ros_message->crc); + { + int rc = PyObject_SetAttrString(_pymessage, "crc", field); + Py_DECREF(field); + if (rc) { + return NULL; + } + } + } + + // ownership of _pymessage is transferred to the caller + return _pymessage; +} diff --git a/python/unitree_go/msg/_motor_cmd.py b/python/unitree_go/msg/_motor_cmd.py new file mode 100644 index 00000000..f580bd6a --- /dev/null +++ b/python/unitree_go/msg/_motor_cmd.py @@ -0,0 +1,263 @@ +# generated from rosidl_generator_py/resource/_idl.py.em +# with input from unitree_go:msg/MotorCmd.idl +# generated code does not contain a copyright notice + + +# Import statements for member types + +# Member 'reserve' +import numpy # noqa: E402, I100 + +import rosidl_parser.definition # noqa: E402, I100 + + +class Metaclass_MotorCmd(type): + """Metaclass of message 'MotorCmd'.""" + + _CREATE_ROS_MESSAGE = None + _CONVERT_FROM_PY = None + _CONVERT_TO_PY = None + _DESTROY_ROS_MESSAGE = None + _TYPE_SUPPORT = None + + __constants = { + } + + @classmethod + def __import_type_support__(cls): + try: + from rosidl_generator_py import import_type_support + module = import_type_support('unitree_go') + except ImportError: + import logging + import traceback + logger = logging.getLogger( + 'unitree_go.msg.MotorCmd') + logger.debug( + 'Failed to import needed modules for type support:\n' + + traceback.format_exc()) + else: + cls._CREATE_ROS_MESSAGE = module.create_ros_message_msg__msg__motor_cmd + cls._CONVERT_FROM_PY = module.convert_from_py_msg__msg__motor_cmd + cls._CONVERT_TO_PY = module.convert_to_py_msg__msg__motor_cmd + cls._TYPE_SUPPORT = module.type_support_msg__msg__motor_cmd + cls._DESTROY_ROS_MESSAGE = module.destroy_ros_message_msg__msg__motor_cmd + + @classmethod + def __prepare__(cls, name, bases, **kwargs): + # list constant names here so that they appear in the help text of + # the message class under "Data and other attributes defined here:" + # as well as populate each message instance + return { + } + + +class MotorCmd(metaclass=Metaclass_MotorCmd): + """Message class 'MotorCmd'.""" + + __slots__ = [ + '_mode', + '_q', + '_dq', + '_tau', + '_kp', + '_kd', + '_reserve', + ] + + _fields_and_field_types = { + 'mode': 'uint8', + 'q': 'float', + 'dq': 'float', + 'tau': 'float', + 'kp': 'float', + 'kd': 'float', + 'reserve': 'uint32[3]', + } + + SLOT_TYPES = ( + rosidl_parser.definition.BasicType('uint8'), # noqa: E501 + rosidl_parser.definition.BasicType('float'), # noqa: E501 + rosidl_parser.definition.BasicType('float'), # noqa: E501 + rosidl_parser.definition.BasicType('float'), # noqa: E501 + rosidl_parser.definition.BasicType('float'), # noqa: E501 + rosidl_parser.definition.BasicType('float'), # noqa: E501 + rosidl_parser.definition.Array(rosidl_parser.definition.BasicType('uint32'), 3), # noqa: E501 + ) + + def __init__(self, **kwargs): + assert all('_' + key in self.__slots__ for key in kwargs.keys()), \ + 'Invalid arguments passed to constructor: %s' % \ + ', '.join(sorted(k for k in kwargs.keys() if '_' + k not in self.__slots__)) + self.mode = kwargs.get('mode', int()) + self.q = kwargs.get('q', float()) + self.dq = kwargs.get('dq', float()) + self.tau = kwargs.get('tau', float()) + self.kp = kwargs.get('kp', float()) + self.kd = kwargs.get('kd', float()) + if 'reserve' not in kwargs: + self.reserve = numpy.zeros(3, dtype=numpy.uint32) + else: + self.reserve = numpy.array(kwargs.get('reserve'), dtype=numpy.uint32) + assert self.reserve.shape == (3, ) + + def __repr__(self): + typename = self.__class__.__module__.split('.') + typename.pop() + typename.append(self.__class__.__name__) + args = [] + for s, t in zip(self.__slots__, self.SLOT_TYPES): + field = getattr(self, s) + fieldstr = repr(field) + # We use Python array type for fields that can be directly stored + # in them, and "normal" sequences for everything else. If it is + # a type that we store in an array, strip off the 'array' portion. + if ( + isinstance(t, rosidl_parser.definition.AbstractSequence) and + isinstance(t.value_type, rosidl_parser.definition.BasicType) and + t.value_type.typename in ['float', 'double', 'int8', 'uint8', 'int16', 'uint16', 'int32', 'uint32', 'int64', 'uint64'] + ): + if len(field) == 0: + fieldstr = '[]' + else: + assert fieldstr.startswith('array(') + prefix = "array('X', " + suffix = ')' + fieldstr = fieldstr[len(prefix):-len(suffix)] + args.append(s[1:] + '=' + fieldstr) + return '%s(%s)' % ('.'.join(typename), ', '.join(args)) + + def __eq__(self, other): + if not isinstance(other, self.__class__): + return False + if self.mode != other.mode: + return False + if self.q != other.q: + return False + if self.dq != other.dq: + return False + if self.tau != other.tau: + return False + if self.kp != other.kp: + return False + if self.kd != other.kd: + return False + if all(self.reserve != other.reserve): + return False + return True + + @classmethod + def get_fields_and_field_types(cls): + from copy import copy + return copy(cls._fields_and_field_types) + + @property + def mode(self): + """Message field 'mode'.""" + return self._mode + + @mode.setter + def mode(self, value): + if __debug__: + assert \ + isinstance(value, int), \ + "The 'mode' field must be of type 'int'" + assert value >= 0 and value < 256, \ + "The 'mode' field must be an unsigned integer in [0, 255]" + self._mode = value + + @property + def q(self): + """Message field 'q'.""" + return self._q + + @q.setter + def q(self, value): + if __debug__: + assert \ + isinstance(value, float), \ + "The 'q' field must be of type 'float'" + self._q = value + + @property + def dq(self): + """Message field 'dq'.""" + return self._dq + + @dq.setter + def dq(self, value): + if __debug__: + assert \ + isinstance(value, float), \ + "The 'dq' field must be of type 'float'" + self._dq = value + + @property + def tau(self): + """Message field 'tau'.""" + return self._tau + + @tau.setter + def tau(self, value): + if __debug__: + assert \ + isinstance(value, float), \ + "The 'tau' field must be of type 'float'" + self._tau = value + + @property + def kp(self): + """Message field 'kp'.""" + return self._kp + + @kp.setter + def kp(self, value): + if __debug__: + assert \ + isinstance(value, float), \ + "The 'kp' field must be of type 'float'" + self._kp = value + + @property + def kd(self): + """Message field 'kd'.""" + return self._kd + + @kd.setter + def kd(self, value): + if __debug__: + assert \ + isinstance(value, float), \ + "The 'kd' field must be of type 'float'" + self._kd = value + + @property + def reserve(self): + """Message field 'reserve'.""" + return self._reserve + + @reserve.setter + def reserve(self, value): + if isinstance(value, numpy.ndarray): + assert value.dtype == numpy.uint32, \ + "The 'reserve' numpy.ndarray() must have the dtype of 'numpy.uint32'" + assert value.size == 3, \ + "The 'reserve' numpy.ndarray() must have a size of 3" + self._reserve = value + return + if __debug__: + from collections.abc import Sequence + from collections.abc import Set + from collections import UserList + from collections import UserString + assert \ + ((isinstance(value, Sequence) or + isinstance(value, Set) or + isinstance(value, UserList)) and + not isinstance(value, str) and + not isinstance(value, UserString) and + len(value) == 3 and + all(isinstance(v, int) for v in value) and + all(val >= 0 and val < 4294967296 for val in value)), \ + "The 'reserve' field must be a set or sequence with length 3 and each value of type 'int' and each unsigned integer in [0, 4294967295]" + self._reserve = numpy.array(value, dtype=numpy.uint32) diff --git a/python/unitree_go/msg/_motor_cmd_s.c b/python/unitree_go/msg/_motor_cmd_s.c new file mode 100644 index 00000000..64fa078a --- /dev/null +++ b/python/unitree_go/msg/_motor_cmd_s.c @@ -0,0 +1,243 @@ +// generated from rosidl_generator_py/resource/_idl_support.c.em +// with input from unitree_go:msg/MotorCmd.idl +// generated code does not contain a copyright notice +#define NPY_NO_DEPRECATED_API NPY_1_7_API_VERSION +#include +#include +#ifndef _WIN32 +# pragma GCC diagnostic push +# pragma GCC diagnostic ignored "-Wunused-function" +#endif +#include "numpy/ndarrayobject.h" +#ifndef _WIN32 +# pragma GCC diagnostic pop +#endif +#include "rosidl_runtime_c/visibility_control.h" +#include "unitree_go/msg/detail/motor_cmd__struct.h" +#include "unitree_go/msg/detail/motor_cmd__functions.h" + +#include "rosidl_runtime_c/primitives_sequence.h" +#include "rosidl_runtime_c/primitives_sequence_functions.h" + + +ROSIDL_GENERATOR_C_EXPORT +bool unitree_go__msg__motor_cmd__convert_from_py(PyObject * _pymsg, void * _ros_message) +{ + // check that the passed message is of the expected Python class + { + char full_classname_dest[35]; + { + char * class_name = NULL; + char * module_name = NULL; + { + PyObject * class_attr = PyObject_GetAttrString(_pymsg, "__class__"); + if (class_attr) { + PyObject * name_attr = PyObject_GetAttrString(class_attr, "__name__"); + if (name_attr) { + class_name = (char *)PyUnicode_1BYTE_DATA(name_attr); + Py_DECREF(name_attr); + } + PyObject * module_attr = PyObject_GetAttrString(class_attr, "__module__"); + if (module_attr) { + module_name = (char *)PyUnicode_1BYTE_DATA(module_attr); + Py_DECREF(module_attr); + } + Py_DECREF(class_attr); + } + } + if (!class_name || !module_name) { + return false; + } + snprintf(full_classname_dest, sizeof(full_classname_dest), "%s.%s", module_name, class_name); + } + assert(strncmp("unitree_go.msg._motor_cmd.MotorCmd", full_classname_dest, 34) == 0); + } + unitree_go__msg__MotorCmd * ros_message = _ros_message; + { // mode + PyObject * field = PyObject_GetAttrString(_pymsg, "mode"); + if (!field) { + return false; + } + assert(PyLong_Check(field)); + ros_message->mode = (uint8_t)PyLong_AsUnsignedLong(field); + Py_DECREF(field); + } + { // q + PyObject * field = PyObject_GetAttrString(_pymsg, "q"); + if (!field) { + return false; + } + assert(PyFloat_Check(field)); + ros_message->q = (float)PyFloat_AS_DOUBLE(field); + Py_DECREF(field); + } + { // dq + PyObject * field = PyObject_GetAttrString(_pymsg, "dq"); + if (!field) { + return false; + } + assert(PyFloat_Check(field)); + ros_message->dq = (float)PyFloat_AS_DOUBLE(field); + Py_DECREF(field); + } + { // tau + PyObject * field = PyObject_GetAttrString(_pymsg, "tau"); + if (!field) { + return false; + } + assert(PyFloat_Check(field)); + ros_message->tau = (float)PyFloat_AS_DOUBLE(field); + Py_DECREF(field); + } + { // kp + PyObject * field = PyObject_GetAttrString(_pymsg, "kp"); + if (!field) { + return false; + } + assert(PyFloat_Check(field)); + ros_message->kp = (float)PyFloat_AS_DOUBLE(field); + Py_DECREF(field); + } + { // kd + PyObject * field = PyObject_GetAttrString(_pymsg, "kd"); + if (!field) { + return false; + } + assert(PyFloat_Check(field)); + ros_message->kd = (float)PyFloat_AS_DOUBLE(field); + Py_DECREF(field); + } + { // reserve + PyObject * field = PyObject_GetAttrString(_pymsg, "reserve"); + if (!field) { + return false; + } + { + // TODO(dirk-thomas) use a better way to check the type before casting + assert(field->ob_type != NULL); + assert(field->ob_type->tp_name != NULL); + assert(strcmp(field->ob_type->tp_name, "numpy.ndarray") == 0); + PyArrayObject * seq_field = (PyArrayObject *)field; + Py_INCREF(seq_field); + assert(PyArray_NDIM(seq_field) == 1); + assert(PyArray_TYPE(seq_field) == NPY_UINT32); + Py_ssize_t size = 3; + uint32_t * dest = ros_message->reserve; + for (Py_ssize_t i = 0; i < size; ++i) { + uint32_t tmp = *(npy_uint32 *)PyArray_GETPTR1(seq_field, i); + memcpy(&dest[i], &tmp, sizeof(uint32_t)); + } + Py_DECREF(seq_field); + } + Py_DECREF(field); + } + + return true; +} + +ROSIDL_GENERATOR_C_EXPORT +PyObject * unitree_go__msg__motor_cmd__convert_to_py(void * raw_ros_message) +{ + /* NOTE(esteve): Call constructor of MotorCmd */ + PyObject * _pymessage = NULL; + { + PyObject * pymessage_module = PyImport_ImportModule("unitree_go.msg._motor_cmd"); + assert(pymessage_module); + PyObject * pymessage_class = PyObject_GetAttrString(pymessage_module, "MotorCmd"); + assert(pymessage_class); + Py_DECREF(pymessage_module); + _pymessage = PyObject_CallObject(pymessage_class, NULL); + Py_DECREF(pymessage_class); + if (!_pymessage) { + return NULL; + } + } + unitree_go__msg__MotorCmd * ros_message = (unitree_go__msg__MotorCmd *)raw_ros_message; + { // mode + PyObject * field = NULL; + field = PyLong_FromUnsignedLong(ros_message->mode); + { + int rc = PyObject_SetAttrString(_pymessage, "mode", field); + Py_DECREF(field); + if (rc) { + return NULL; + } + } + } + { // q + PyObject * field = NULL; + field = PyFloat_FromDouble(ros_message->q); + { + int rc = PyObject_SetAttrString(_pymessage, "q", field); + Py_DECREF(field); + if (rc) { + return NULL; + } + } + } + { // dq + PyObject * field = NULL; + field = PyFloat_FromDouble(ros_message->dq); + { + int rc = PyObject_SetAttrString(_pymessage, "dq", field); + Py_DECREF(field); + if (rc) { + return NULL; + } + } + } + { // tau + PyObject * field = NULL; + field = PyFloat_FromDouble(ros_message->tau); + { + int rc = PyObject_SetAttrString(_pymessage, "tau", field); + Py_DECREF(field); + if (rc) { + return NULL; + } + } + } + { // kp + PyObject * field = NULL; + field = PyFloat_FromDouble(ros_message->kp); + { + int rc = PyObject_SetAttrString(_pymessage, "kp", field); + Py_DECREF(field); + if (rc) { + return NULL; + } + } + } + { // kd + PyObject * field = NULL; + field = PyFloat_FromDouble(ros_message->kd); + { + int rc = PyObject_SetAttrString(_pymessage, "kd", field); + Py_DECREF(field); + if (rc) { + return NULL; + } + } + } + { // reserve + PyObject * field = NULL; + field = PyObject_GetAttrString(_pymessage, "reserve"); + if (!field) { + return NULL; + } + assert(field->ob_type != NULL); + assert(field->ob_type->tp_name != NULL); + assert(strcmp(field->ob_type->tp_name, "numpy.ndarray") == 0); + PyArrayObject * seq_field = (PyArrayObject *)field; + assert(PyArray_NDIM(seq_field) == 1); + assert(PyArray_TYPE(seq_field) == NPY_UINT32); + assert(sizeof(npy_uint32) == sizeof(uint32_t)); + npy_uint32 * dst = (npy_uint32 *)PyArray_GETPTR1(seq_field, 0); + uint32_t * src = &(ros_message->reserve[0]); + memcpy(dst, src, 3 * sizeof(uint32_t)); + Py_DECREF(field); + } + + // ownership of _pymessage is transferred to the caller + return _pymessage; +} diff --git a/python/unitree_go/msg/_motor_state.py b/python/unitree_go/msg/_motor_state.py new file mode 100644 index 00000000..739e8f01 --- /dev/null +++ b/python/unitree_go/msg/_motor_state.py @@ -0,0 +1,343 @@ +# generated from rosidl_generator_py/resource/_idl.py.em +# with input from unitree_go:msg/MotorState.idl +# generated code does not contain a copyright notice + + +# Import statements for member types + +# Member 'reserve' +import numpy # noqa: E402, I100 + +import rosidl_parser.definition # noqa: E402, I100 + + +class Metaclass_MotorState(type): + """Metaclass of message 'MotorState'.""" + + _CREATE_ROS_MESSAGE = None + _CONVERT_FROM_PY = None + _CONVERT_TO_PY = None + _DESTROY_ROS_MESSAGE = None + _TYPE_SUPPORT = None + + __constants = { + } + + @classmethod + def __import_type_support__(cls): + try: + from rosidl_generator_py import import_type_support + module = import_type_support('unitree_go') + except ImportError: + import logging + import traceback + logger = logging.getLogger( + 'unitree_go.msg.MotorState') + logger.debug( + 'Failed to import needed modules for type support:\n' + + traceback.format_exc()) + else: + cls._CREATE_ROS_MESSAGE = module.create_ros_message_msg__msg__motor_state + cls._CONVERT_FROM_PY = module.convert_from_py_msg__msg__motor_state + cls._CONVERT_TO_PY = module.convert_to_py_msg__msg__motor_state + cls._TYPE_SUPPORT = module.type_support_msg__msg__motor_state + cls._DESTROY_ROS_MESSAGE = module.destroy_ros_message_msg__msg__motor_state + + @classmethod + def __prepare__(cls, name, bases, **kwargs): + # list constant names here so that they appear in the help text of + # the message class under "Data and other attributes defined here:" + # as well as populate each message instance + return { + } + + +class MotorState(metaclass=Metaclass_MotorState): + """Message class 'MotorState'.""" + + __slots__ = [ + '_mode', + '_q', + '_dq', + '_ddq', + '_tau_est', + '_q_raw', + '_dq_raw', + '_ddq_raw', + '_temperature', + '_lost', + '_reserve', + ] + + _fields_and_field_types = { + 'mode': 'uint8', + 'q': 'float', + 'dq': 'float', + 'ddq': 'float', + 'tau_est': 'float', + 'q_raw': 'float', + 'dq_raw': 'float', + 'ddq_raw': 'float', + 'temperature': 'int8', + 'lost': 'uint32', + 'reserve': 'uint32[2]', + } + + SLOT_TYPES = ( + rosidl_parser.definition.BasicType('uint8'), # noqa: E501 + rosidl_parser.definition.BasicType('float'), # noqa: E501 + rosidl_parser.definition.BasicType('float'), # noqa: E501 + rosidl_parser.definition.BasicType('float'), # noqa: E501 + rosidl_parser.definition.BasicType('float'), # noqa: E501 + rosidl_parser.definition.BasicType('float'), # noqa: E501 + rosidl_parser.definition.BasicType('float'), # noqa: E501 + rosidl_parser.definition.BasicType('float'), # noqa: E501 + rosidl_parser.definition.BasicType('int8'), # noqa: E501 + rosidl_parser.definition.BasicType('uint32'), # noqa: E501 + rosidl_parser.definition.Array(rosidl_parser.definition.BasicType('uint32'), 2), # noqa: E501 + ) + + def __init__(self, **kwargs): + assert all('_' + key in self.__slots__ for key in kwargs.keys()), \ + 'Invalid arguments passed to constructor: %s' % \ + ', '.join(sorted(k for k in kwargs.keys() if '_' + k not in self.__slots__)) + self.mode = kwargs.get('mode', int()) + self.q = kwargs.get('q', float()) + self.dq = kwargs.get('dq', float()) + self.ddq = kwargs.get('ddq', float()) + self.tau_est = kwargs.get('tau_est', float()) + self.q_raw = kwargs.get('q_raw', float()) + self.dq_raw = kwargs.get('dq_raw', float()) + self.ddq_raw = kwargs.get('ddq_raw', float()) + self.temperature = kwargs.get('temperature', int()) + self.lost = kwargs.get('lost', int()) + if 'reserve' not in kwargs: + self.reserve = numpy.zeros(2, dtype=numpy.uint32) + else: + self.reserve = numpy.array(kwargs.get('reserve'), dtype=numpy.uint32) + assert self.reserve.shape == (2, ) + + def __repr__(self): + typename = self.__class__.__module__.split('.') + typename.pop() + typename.append(self.__class__.__name__) + args = [] + for s, t in zip(self.__slots__, self.SLOT_TYPES): + field = getattr(self, s) + fieldstr = repr(field) + # We use Python array type for fields that can be directly stored + # in them, and "normal" sequences for everything else. If it is + # a type that we store in an array, strip off the 'array' portion. + if ( + isinstance(t, rosidl_parser.definition.AbstractSequence) and + isinstance(t.value_type, rosidl_parser.definition.BasicType) and + t.value_type.typename in ['float', 'double', 'int8', 'uint8', 'int16', 'uint16', 'int32', 'uint32', 'int64', 'uint64'] + ): + if len(field) == 0: + fieldstr = '[]' + else: + assert fieldstr.startswith('array(') + prefix = "array('X', " + suffix = ')' + fieldstr = fieldstr[len(prefix):-len(suffix)] + args.append(s[1:] + '=' + fieldstr) + return '%s(%s)' % ('.'.join(typename), ', '.join(args)) + + def __eq__(self, other): + if not isinstance(other, self.__class__): + return False + if self.mode != other.mode: + return False + if self.q != other.q: + return False + if self.dq != other.dq: + return False + if self.ddq != other.ddq: + return False + if self.tau_est != other.tau_est: + return False + if self.q_raw != other.q_raw: + return False + if self.dq_raw != other.dq_raw: + return False + if self.ddq_raw != other.ddq_raw: + return False + if self.temperature != other.temperature: + return False + if self.lost != other.lost: + return False + if all(self.reserve != other.reserve): + return False + return True + + @classmethod + def get_fields_and_field_types(cls): + from copy import copy + return copy(cls._fields_and_field_types) + + @property + def mode(self): + """Message field 'mode'.""" + return self._mode + + @mode.setter + def mode(self, value): + if __debug__: + assert \ + isinstance(value, int), \ + "The 'mode' field must be of type 'int'" + assert value >= 0 and value < 256, \ + "The 'mode' field must be an unsigned integer in [0, 255]" + self._mode = value + + @property + def q(self): + """Message field 'q'.""" + return self._q + + @q.setter + def q(self, value): + if __debug__: + assert \ + isinstance(value, float), \ + "The 'q' field must be of type 'float'" + self._q = value + + @property + def dq(self): + """Message field 'dq'.""" + return self._dq + + @dq.setter + def dq(self, value): + if __debug__: + assert \ + isinstance(value, float), \ + "The 'dq' field must be of type 'float'" + self._dq = value + + @property + def ddq(self): + """Message field 'ddq'.""" + return self._ddq + + @ddq.setter + def ddq(self, value): + if __debug__: + assert \ + isinstance(value, float), \ + "The 'ddq' field must be of type 'float'" + self._ddq = value + + @property + def tau_est(self): + """Message field 'tau_est'.""" + return self._tau_est + + @tau_est.setter + def tau_est(self, value): + if __debug__: + assert \ + isinstance(value, float), \ + "The 'tau_est' field must be of type 'float'" + self._tau_est = value + + @property + def q_raw(self): + """Message field 'q_raw'.""" + return self._q_raw + + @q_raw.setter + def q_raw(self, value): + if __debug__: + assert \ + isinstance(value, float), \ + "The 'q_raw' field must be of type 'float'" + self._q_raw = value + + @property + def dq_raw(self): + """Message field 'dq_raw'.""" + return self._dq_raw + + @dq_raw.setter + def dq_raw(self, value): + if __debug__: + assert \ + isinstance(value, float), \ + "The 'dq_raw' field must be of type 'float'" + self._dq_raw = value + + @property + def ddq_raw(self): + """Message field 'ddq_raw'.""" + return self._ddq_raw + + @ddq_raw.setter + def ddq_raw(self, value): + if __debug__: + assert \ + isinstance(value, float), \ + "The 'ddq_raw' field must be of type 'float'" + self._ddq_raw = value + + @property + def temperature(self): + """Message field 'temperature'.""" + return self._temperature + + @temperature.setter + def temperature(self, value): + if __debug__: + assert \ + isinstance(value, int), \ + "The 'temperature' field must be of type 'int'" + assert value >= -128 and value < 128, \ + "The 'temperature' field must be an integer in [-128, 127]" + self._temperature = value + + @property + def lost(self): + """Message field 'lost'.""" + return self._lost + + @lost.setter + def lost(self, value): + if __debug__: + assert \ + isinstance(value, int), \ + "The 'lost' field must be of type 'int'" + assert value >= 0 and value < 4294967296, \ + "The 'lost' field must be an unsigned integer in [0, 4294967295]" + self._lost = value + + @property + def reserve(self): + """Message field 'reserve'.""" + return self._reserve + + @reserve.setter + def reserve(self, value): + if isinstance(value, numpy.ndarray): + assert value.dtype == numpy.uint32, \ + "The 'reserve' numpy.ndarray() must have the dtype of 'numpy.uint32'" + assert value.size == 2, \ + "The 'reserve' numpy.ndarray() must have a size of 2" + self._reserve = value + return + if __debug__: + from collections.abc import Sequence + from collections.abc import Set + from collections import UserList + from collections import UserString + assert \ + ((isinstance(value, Sequence) or + isinstance(value, Set) or + isinstance(value, UserList)) and + not isinstance(value, str) and + not isinstance(value, UserString) and + len(value) == 2 and + all(isinstance(v, int) for v in value) and + all(val >= 0 and val < 4294967296 for val in value)), \ + "The 'reserve' field must be a set or sequence with length 2 and each value of type 'int' and each unsigned integer in [0, 4294967295]" + self._reserve = numpy.array(value, dtype=numpy.uint32) diff --git a/python/unitree_go/msg/_motor_state_s.c b/python/unitree_go/msg/_motor_state_s.c new file mode 100644 index 00000000..56d9526d --- /dev/null +++ b/python/unitree_go/msg/_motor_state_s.c @@ -0,0 +1,323 @@ +// generated from rosidl_generator_py/resource/_idl_support.c.em +// with input from unitree_go:msg/MotorState.idl +// generated code does not contain a copyright notice +#define NPY_NO_DEPRECATED_API NPY_1_7_API_VERSION +#include +#include +#ifndef _WIN32 +# pragma GCC diagnostic push +# pragma GCC diagnostic ignored "-Wunused-function" +#endif +#include "numpy/ndarrayobject.h" +#ifndef _WIN32 +# pragma GCC diagnostic pop +#endif +#include "rosidl_runtime_c/visibility_control.h" +#include "unitree_go/msg/detail/motor_state__struct.h" +#include "unitree_go/msg/detail/motor_state__functions.h" + +#include "rosidl_runtime_c/primitives_sequence.h" +#include "rosidl_runtime_c/primitives_sequence_functions.h" + + +ROSIDL_GENERATOR_C_EXPORT +bool unitree_go__msg__motor_state__convert_from_py(PyObject * _pymsg, void * _ros_message) +{ + // check that the passed message is of the expected Python class + { + char full_classname_dest[39]; + { + char * class_name = NULL; + char * module_name = NULL; + { + PyObject * class_attr = PyObject_GetAttrString(_pymsg, "__class__"); + if (class_attr) { + PyObject * name_attr = PyObject_GetAttrString(class_attr, "__name__"); + if (name_attr) { + class_name = (char *)PyUnicode_1BYTE_DATA(name_attr); + Py_DECREF(name_attr); + } + PyObject * module_attr = PyObject_GetAttrString(class_attr, "__module__"); + if (module_attr) { + module_name = (char *)PyUnicode_1BYTE_DATA(module_attr); + Py_DECREF(module_attr); + } + Py_DECREF(class_attr); + } + } + if (!class_name || !module_name) { + return false; + } + snprintf(full_classname_dest, sizeof(full_classname_dest), "%s.%s", module_name, class_name); + } + assert(strncmp("unitree_go.msg._motor_state.MotorState", full_classname_dest, 38) == 0); + } + unitree_go__msg__MotorState * ros_message = _ros_message; + { // mode + PyObject * field = PyObject_GetAttrString(_pymsg, "mode"); + if (!field) { + return false; + } + assert(PyLong_Check(field)); + ros_message->mode = (uint8_t)PyLong_AsUnsignedLong(field); + Py_DECREF(field); + } + { // q + PyObject * field = PyObject_GetAttrString(_pymsg, "q"); + if (!field) { + return false; + } + assert(PyFloat_Check(field)); + ros_message->q = (float)PyFloat_AS_DOUBLE(field); + Py_DECREF(field); + } + { // dq + PyObject * field = PyObject_GetAttrString(_pymsg, "dq"); + if (!field) { + return false; + } + assert(PyFloat_Check(field)); + ros_message->dq = (float)PyFloat_AS_DOUBLE(field); + Py_DECREF(field); + } + { // ddq + PyObject * field = PyObject_GetAttrString(_pymsg, "ddq"); + if (!field) { + return false; + } + assert(PyFloat_Check(field)); + ros_message->ddq = (float)PyFloat_AS_DOUBLE(field); + Py_DECREF(field); + } + { // tau_est + PyObject * field = PyObject_GetAttrString(_pymsg, "tau_est"); + if (!field) { + return false; + } + assert(PyFloat_Check(field)); + ros_message->tau_est = (float)PyFloat_AS_DOUBLE(field); + Py_DECREF(field); + } + { // q_raw + PyObject * field = PyObject_GetAttrString(_pymsg, "q_raw"); + if (!field) { + return false; + } + assert(PyFloat_Check(field)); + ros_message->q_raw = (float)PyFloat_AS_DOUBLE(field); + Py_DECREF(field); + } + { // dq_raw + PyObject * field = PyObject_GetAttrString(_pymsg, "dq_raw"); + if (!field) { + return false; + } + assert(PyFloat_Check(field)); + ros_message->dq_raw = (float)PyFloat_AS_DOUBLE(field); + Py_DECREF(field); + } + { // ddq_raw + PyObject * field = PyObject_GetAttrString(_pymsg, "ddq_raw"); + if (!field) { + return false; + } + assert(PyFloat_Check(field)); + ros_message->ddq_raw = (float)PyFloat_AS_DOUBLE(field); + Py_DECREF(field); + } + { // temperature + PyObject * field = PyObject_GetAttrString(_pymsg, "temperature"); + if (!field) { + return false; + } + assert(PyLong_Check(field)); + ros_message->temperature = (int8_t)PyLong_AsLong(field); + Py_DECREF(field); + } + { // lost + PyObject * field = PyObject_GetAttrString(_pymsg, "lost"); + if (!field) { + return false; + } + assert(PyLong_Check(field)); + ros_message->lost = PyLong_AsUnsignedLong(field); + Py_DECREF(field); + } + { // reserve + PyObject * field = PyObject_GetAttrString(_pymsg, "reserve"); + if (!field) { + return false; + } + { + // TODO(dirk-thomas) use a better way to check the type before casting + assert(field->ob_type != NULL); + assert(field->ob_type->tp_name != NULL); + assert(strcmp(field->ob_type->tp_name, "numpy.ndarray") == 0); + PyArrayObject * seq_field = (PyArrayObject *)field; + Py_INCREF(seq_field); + assert(PyArray_NDIM(seq_field) == 1); + assert(PyArray_TYPE(seq_field) == NPY_UINT32); + Py_ssize_t size = 2; + uint32_t * dest = ros_message->reserve; + for (Py_ssize_t i = 0; i < size; ++i) { + uint32_t tmp = *(npy_uint32 *)PyArray_GETPTR1(seq_field, i); + memcpy(&dest[i], &tmp, sizeof(uint32_t)); + } + Py_DECREF(seq_field); + } + Py_DECREF(field); + } + + return true; +} + +ROSIDL_GENERATOR_C_EXPORT +PyObject * unitree_go__msg__motor_state__convert_to_py(void * raw_ros_message) +{ + /* NOTE(esteve): Call constructor of MotorState */ + PyObject * _pymessage = NULL; + { + PyObject * pymessage_module = PyImport_ImportModule("unitree_go.msg._motor_state"); + assert(pymessage_module); + PyObject * pymessage_class = PyObject_GetAttrString(pymessage_module, "MotorState"); + assert(pymessage_class); + Py_DECREF(pymessage_module); + _pymessage = PyObject_CallObject(pymessage_class, NULL); + Py_DECREF(pymessage_class); + if (!_pymessage) { + return NULL; + } + } + unitree_go__msg__MotorState * ros_message = (unitree_go__msg__MotorState *)raw_ros_message; + { // mode + PyObject * field = NULL; + field = PyLong_FromUnsignedLong(ros_message->mode); + { + int rc = PyObject_SetAttrString(_pymessage, "mode", field); + Py_DECREF(field); + if (rc) { + return NULL; + } + } + } + { // q + PyObject * field = NULL; + field = PyFloat_FromDouble(ros_message->q); + { + int rc = PyObject_SetAttrString(_pymessage, "q", field); + Py_DECREF(field); + if (rc) { + return NULL; + } + } + } + { // dq + PyObject * field = NULL; + field = PyFloat_FromDouble(ros_message->dq); + { + int rc = PyObject_SetAttrString(_pymessage, "dq", field); + Py_DECREF(field); + if (rc) { + return NULL; + } + } + } + { // ddq + PyObject * field = NULL; + field = PyFloat_FromDouble(ros_message->ddq); + { + int rc = PyObject_SetAttrString(_pymessage, "ddq", field); + Py_DECREF(field); + if (rc) { + return NULL; + } + } + } + { // tau_est + PyObject * field = NULL; + field = PyFloat_FromDouble(ros_message->tau_est); + { + int rc = PyObject_SetAttrString(_pymessage, "tau_est", field); + Py_DECREF(field); + if (rc) { + return NULL; + } + } + } + { // q_raw + PyObject * field = NULL; + field = PyFloat_FromDouble(ros_message->q_raw); + { + int rc = PyObject_SetAttrString(_pymessage, "q_raw", field); + Py_DECREF(field); + if (rc) { + return NULL; + } + } + } + { // dq_raw + PyObject * field = NULL; + field = PyFloat_FromDouble(ros_message->dq_raw); + { + int rc = PyObject_SetAttrString(_pymessage, "dq_raw", field); + Py_DECREF(field); + if (rc) { + return NULL; + } + } + } + { // ddq_raw + PyObject * field = NULL; + field = PyFloat_FromDouble(ros_message->ddq_raw); + { + int rc = PyObject_SetAttrString(_pymessage, "ddq_raw", field); + Py_DECREF(field); + if (rc) { + return NULL; + } + } + } + { // temperature + PyObject * field = NULL; + field = PyLong_FromLong(ros_message->temperature); + { + int rc = PyObject_SetAttrString(_pymessage, "temperature", field); + Py_DECREF(field); + if (rc) { + return NULL; + } + } + } + { // lost + PyObject * field = NULL; + field = PyLong_FromUnsignedLong(ros_message->lost); + { + int rc = PyObject_SetAttrString(_pymessage, "lost", field); + Py_DECREF(field); + if (rc) { + return NULL; + } + } + } + { // reserve + PyObject * field = NULL; + field = PyObject_GetAttrString(_pymessage, "reserve"); + if (!field) { + return NULL; + } + assert(field->ob_type != NULL); + assert(field->ob_type->tp_name != NULL); + assert(strcmp(field->ob_type->tp_name, "numpy.ndarray") == 0); + PyArrayObject * seq_field = (PyArrayObject *)field; + assert(PyArray_NDIM(seq_field) == 1); + assert(PyArray_TYPE(seq_field) == NPY_UINT32); + assert(sizeof(npy_uint32) == sizeof(uint32_t)); + npy_uint32 * dst = (npy_uint32 *)PyArray_GETPTR1(seq_field, 0); + uint32_t * src = &(ros_message->reserve[0]); + memcpy(dst, src, 2 * sizeof(uint32_t)); + Py_DECREF(field); + } + + // ownership of _pymessage is transferred to the caller + return _pymessage; +} diff --git a/python/unitree_go/msg/_path_point.py b/python/unitree_go/msg/_path_point.py new file mode 100644 index 00000000..1d702205 --- /dev/null +++ b/python/unitree_go/msg/_path_point.py @@ -0,0 +1,236 @@ +# generated from rosidl_generator_py/resource/_idl.py.em +# with input from unitree_go:msg/PathPoint.idl +# generated code does not contain a copyright notice + + +# Import statements for member types + +import rosidl_parser.definition # noqa: E402, I100 + + +class Metaclass_PathPoint(type): + """Metaclass of message 'PathPoint'.""" + + _CREATE_ROS_MESSAGE = None + _CONVERT_FROM_PY = None + _CONVERT_TO_PY = None + _DESTROY_ROS_MESSAGE = None + _TYPE_SUPPORT = None + + __constants = { + } + + @classmethod + def __import_type_support__(cls): + try: + from rosidl_generator_py import import_type_support + module = import_type_support('unitree_go') + except ImportError: + import logging + import traceback + logger = logging.getLogger( + 'unitree_go.msg.PathPoint') + logger.debug( + 'Failed to import needed modules for type support:\n' + + traceback.format_exc()) + else: + cls._CREATE_ROS_MESSAGE = module.create_ros_message_msg__msg__path_point + cls._CONVERT_FROM_PY = module.convert_from_py_msg__msg__path_point + cls._CONVERT_TO_PY = module.convert_to_py_msg__msg__path_point + cls._TYPE_SUPPORT = module.type_support_msg__msg__path_point + cls._DESTROY_ROS_MESSAGE = module.destroy_ros_message_msg__msg__path_point + + @classmethod + def __prepare__(cls, name, bases, **kwargs): + # list constant names here so that they appear in the help text of + # the message class under "Data and other attributes defined here:" + # as well as populate each message instance + return { + } + + +class PathPoint(metaclass=Metaclass_PathPoint): + """Message class 'PathPoint'.""" + + __slots__ = [ + '_t_from_start', + '_x', + '_y', + '_yaw', + '_vx', + '_vy', + '_vyaw', + ] + + _fields_and_field_types = { + 't_from_start': 'float', + 'x': 'float', + 'y': 'float', + 'yaw': 'float', + 'vx': 'float', + 'vy': 'float', + 'vyaw': 'float', + } + + SLOT_TYPES = ( + rosidl_parser.definition.BasicType('float'), # noqa: E501 + rosidl_parser.definition.BasicType('float'), # noqa: E501 + rosidl_parser.definition.BasicType('float'), # noqa: E501 + rosidl_parser.definition.BasicType('float'), # noqa: E501 + rosidl_parser.definition.BasicType('float'), # noqa: E501 + rosidl_parser.definition.BasicType('float'), # noqa: E501 + rosidl_parser.definition.BasicType('float'), # noqa: E501 + ) + + def __init__(self, **kwargs): + assert all('_' + key in self.__slots__ for key in kwargs.keys()), \ + 'Invalid arguments passed to constructor: %s' % \ + ', '.join(sorted(k for k in kwargs.keys() if '_' + k not in self.__slots__)) + self.t_from_start = kwargs.get('t_from_start', float()) + self.x = kwargs.get('x', float()) + self.y = kwargs.get('y', float()) + self.yaw = kwargs.get('yaw', float()) + self.vx = kwargs.get('vx', float()) + self.vy = kwargs.get('vy', float()) + self.vyaw = kwargs.get('vyaw', float()) + + def __repr__(self): + typename = self.__class__.__module__.split('.') + typename.pop() + typename.append(self.__class__.__name__) + args = [] + for s, t in zip(self.__slots__, self.SLOT_TYPES): + field = getattr(self, s) + fieldstr = repr(field) + # We use Python array type for fields that can be directly stored + # in them, and "normal" sequences for everything else. If it is + # a type that we store in an array, strip off the 'array' portion. + if ( + isinstance(t, rosidl_parser.definition.AbstractSequence) and + isinstance(t.value_type, rosidl_parser.definition.BasicType) and + t.value_type.typename in ['float', 'double', 'int8', 'uint8', 'int16', 'uint16', 'int32', 'uint32', 'int64', 'uint64'] + ): + if len(field) == 0: + fieldstr = '[]' + else: + assert fieldstr.startswith('array(') + prefix = "array('X', " + suffix = ')' + fieldstr = fieldstr[len(prefix):-len(suffix)] + args.append(s[1:] + '=' + fieldstr) + return '%s(%s)' % ('.'.join(typename), ', '.join(args)) + + def __eq__(self, other): + if not isinstance(other, self.__class__): + return False + if self.t_from_start != other.t_from_start: + return False + if self.x != other.x: + return False + if self.y != other.y: + return False + if self.yaw != other.yaw: + return False + if self.vx != other.vx: + return False + if self.vy != other.vy: + return False + if self.vyaw != other.vyaw: + return False + return True + + @classmethod + def get_fields_and_field_types(cls): + from copy import copy + return copy(cls._fields_and_field_types) + + @property + def t_from_start(self): + """Message field 't_from_start'.""" + return self._t_from_start + + @t_from_start.setter + def t_from_start(self, value): + if __debug__: + assert \ + isinstance(value, float), \ + "The 't_from_start' field must be of type 'float'" + self._t_from_start = value + + @property + def x(self): + """Message field 'x'.""" + return self._x + + @x.setter + def x(self, value): + if __debug__: + assert \ + isinstance(value, float), \ + "The 'x' field must be of type 'float'" + self._x = value + + @property + def y(self): + """Message field 'y'.""" + return self._y + + @y.setter + def y(self, value): + if __debug__: + assert \ + isinstance(value, float), \ + "The 'y' field must be of type 'float'" + self._y = value + + @property + def yaw(self): + """Message field 'yaw'.""" + return self._yaw + + @yaw.setter + def yaw(self, value): + if __debug__: + assert \ + isinstance(value, float), \ + "The 'yaw' field must be of type 'float'" + self._yaw = value + + @property + def vx(self): + """Message field 'vx'.""" + return self._vx + + @vx.setter + def vx(self, value): + if __debug__: + assert \ + isinstance(value, float), \ + "The 'vx' field must be of type 'float'" + self._vx = value + + @property + def vy(self): + """Message field 'vy'.""" + return self._vy + + @vy.setter + def vy(self, value): + if __debug__: + assert \ + isinstance(value, float), \ + "The 'vy' field must be of type 'float'" + self._vy = value + + @property + def vyaw(self): + """Message field 'vyaw'.""" + return self._vyaw + + @vyaw.setter + def vyaw(self, value): + if __debug__: + assert \ + isinstance(value, float), \ + "The 'vyaw' field must be of type 'float'" + self._vyaw = value diff --git a/python/unitree_go/msg/_path_point_s.c b/python/unitree_go/msg/_path_point_s.c new file mode 100644 index 00000000..6be16c7d --- /dev/null +++ b/python/unitree_go/msg/_path_point_s.c @@ -0,0 +1,218 @@ +// generated from rosidl_generator_py/resource/_idl_support.c.em +// with input from unitree_go:msg/PathPoint.idl +// generated code does not contain a copyright notice +#define NPY_NO_DEPRECATED_API NPY_1_7_API_VERSION +#include +#include +#ifndef _WIN32 +# pragma GCC diagnostic push +# pragma GCC diagnostic ignored "-Wunused-function" +#endif +#include "numpy/ndarrayobject.h" +#ifndef _WIN32 +# pragma GCC diagnostic pop +#endif +#include "rosidl_runtime_c/visibility_control.h" +#include "unitree_go/msg/detail/path_point__struct.h" +#include "unitree_go/msg/detail/path_point__functions.h" + + +ROSIDL_GENERATOR_C_EXPORT +bool unitree_go__msg__path_point__convert_from_py(PyObject * _pymsg, void * _ros_message) +{ + // check that the passed message is of the expected Python class + { + char full_classname_dest[37]; + { + char * class_name = NULL; + char * module_name = NULL; + { + PyObject * class_attr = PyObject_GetAttrString(_pymsg, "__class__"); + if (class_attr) { + PyObject * name_attr = PyObject_GetAttrString(class_attr, "__name__"); + if (name_attr) { + class_name = (char *)PyUnicode_1BYTE_DATA(name_attr); + Py_DECREF(name_attr); + } + PyObject * module_attr = PyObject_GetAttrString(class_attr, "__module__"); + if (module_attr) { + module_name = (char *)PyUnicode_1BYTE_DATA(module_attr); + Py_DECREF(module_attr); + } + Py_DECREF(class_attr); + } + } + if (!class_name || !module_name) { + return false; + } + snprintf(full_classname_dest, sizeof(full_classname_dest), "%s.%s", module_name, class_name); + } + assert(strncmp("unitree_go.msg._path_point.PathPoint", full_classname_dest, 36) == 0); + } + unitree_go__msg__PathPoint * ros_message = _ros_message; + { // t_from_start + PyObject * field = PyObject_GetAttrString(_pymsg, "t_from_start"); + if (!field) { + return false; + } + assert(PyFloat_Check(field)); + ros_message->t_from_start = (float)PyFloat_AS_DOUBLE(field); + Py_DECREF(field); + } + { // x + PyObject * field = PyObject_GetAttrString(_pymsg, "x"); + if (!field) { + return false; + } + assert(PyFloat_Check(field)); + ros_message->x = (float)PyFloat_AS_DOUBLE(field); + Py_DECREF(field); + } + { // y + PyObject * field = PyObject_GetAttrString(_pymsg, "y"); + if (!field) { + return false; + } + assert(PyFloat_Check(field)); + ros_message->y = (float)PyFloat_AS_DOUBLE(field); + Py_DECREF(field); + } + { // yaw + PyObject * field = PyObject_GetAttrString(_pymsg, "yaw"); + if (!field) { + return false; + } + assert(PyFloat_Check(field)); + ros_message->yaw = (float)PyFloat_AS_DOUBLE(field); + Py_DECREF(field); + } + { // vx + PyObject * field = PyObject_GetAttrString(_pymsg, "vx"); + if (!field) { + return false; + } + assert(PyFloat_Check(field)); + ros_message->vx = (float)PyFloat_AS_DOUBLE(field); + Py_DECREF(field); + } + { // vy + PyObject * field = PyObject_GetAttrString(_pymsg, "vy"); + if (!field) { + return false; + } + assert(PyFloat_Check(field)); + ros_message->vy = (float)PyFloat_AS_DOUBLE(field); + Py_DECREF(field); + } + { // vyaw + PyObject * field = PyObject_GetAttrString(_pymsg, "vyaw"); + if (!field) { + return false; + } + assert(PyFloat_Check(field)); + ros_message->vyaw = (float)PyFloat_AS_DOUBLE(field); + Py_DECREF(field); + } + + return true; +} + +ROSIDL_GENERATOR_C_EXPORT +PyObject * unitree_go__msg__path_point__convert_to_py(void * raw_ros_message) +{ + /* NOTE(esteve): Call constructor of PathPoint */ + PyObject * _pymessage = NULL; + { + PyObject * pymessage_module = PyImport_ImportModule("unitree_go.msg._path_point"); + assert(pymessage_module); + PyObject * pymessage_class = PyObject_GetAttrString(pymessage_module, "PathPoint"); + assert(pymessage_class); + Py_DECREF(pymessage_module); + _pymessage = PyObject_CallObject(pymessage_class, NULL); + Py_DECREF(pymessage_class); + if (!_pymessage) { + return NULL; + } + } + unitree_go__msg__PathPoint * ros_message = (unitree_go__msg__PathPoint *)raw_ros_message; + { // t_from_start + PyObject * field = NULL; + field = PyFloat_FromDouble(ros_message->t_from_start); + { + int rc = PyObject_SetAttrString(_pymessage, "t_from_start", field); + Py_DECREF(field); + if (rc) { + return NULL; + } + } + } + { // x + PyObject * field = NULL; + field = PyFloat_FromDouble(ros_message->x); + { + int rc = PyObject_SetAttrString(_pymessage, "x", field); + Py_DECREF(field); + if (rc) { + return NULL; + } + } + } + { // y + PyObject * field = NULL; + field = PyFloat_FromDouble(ros_message->y); + { + int rc = PyObject_SetAttrString(_pymessage, "y", field); + Py_DECREF(field); + if (rc) { + return NULL; + } + } + } + { // yaw + PyObject * field = NULL; + field = PyFloat_FromDouble(ros_message->yaw); + { + int rc = PyObject_SetAttrString(_pymessage, "yaw", field); + Py_DECREF(field); + if (rc) { + return NULL; + } + } + } + { // vx + PyObject * field = NULL; + field = PyFloat_FromDouble(ros_message->vx); + { + int rc = PyObject_SetAttrString(_pymessage, "vx", field); + Py_DECREF(field); + if (rc) { + return NULL; + } + } + } + { // vy + PyObject * field = NULL; + field = PyFloat_FromDouble(ros_message->vy); + { + int rc = PyObject_SetAttrString(_pymessage, "vy", field); + Py_DECREF(field); + if (rc) { + return NULL; + } + } + } + { // vyaw + PyObject * field = NULL; + field = PyFloat_FromDouble(ros_message->vyaw); + { + int rc = PyObject_SetAttrString(_pymessage, "vyaw", field); + Py_DECREF(field); + if (rc) { + return NULL; + } + } + } + + // ownership of _pymessage is transferred to the caller + return _pymessage; +} diff --git a/python/unitree_go/msg/_req.py b/python/unitree_go/msg/_req.py new file mode 100644 index 00000000..08276447 --- /dev/null +++ b/python/unitree_go/msg/_req.py @@ -0,0 +1,141 @@ +# generated from rosidl_generator_py/resource/_idl.py.em +# with input from unitree_go:msg/Req.idl +# generated code does not contain a copyright notice + + +# Import statements for member types + +import rosidl_parser.definition # noqa: E402, I100 + + +class Metaclass_Req(type): + """Metaclass of message 'Req'.""" + + _CREATE_ROS_MESSAGE = None + _CONVERT_FROM_PY = None + _CONVERT_TO_PY = None + _DESTROY_ROS_MESSAGE = None + _TYPE_SUPPORT = None + + __constants = { + } + + @classmethod + def __import_type_support__(cls): + try: + from rosidl_generator_py import import_type_support + module = import_type_support('unitree_go') + except ImportError: + import logging + import traceback + logger = logging.getLogger( + 'unitree_go.msg.Req') + logger.debug( + 'Failed to import needed modules for type support:\n' + + traceback.format_exc()) + else: + cls._CREATE_ROS_MESSAGE = module.create_ros_message_msg__msg__req + cls._CONVERT_FROM_PY = module.convert_from_py_msg__msg__req + cls._CONVERT_TO_PY = module.convert_to_py_msg__msg__req + cls._TYPE_SUPPORT = module.type_support_msg__msg__req + cls._DESTROY_ROS_MESSAGE = module.destroy_ros_message_msg__msg__req + + @classmethod + def __prepare__(cls, name, bases, **kwargs): + # list constant names here so that they appear in the help text of + # the message class under "Data and other attributes defined here:" + # as well as populate each message instance + return { + } + + +class Req(metaclass=Metaclass_Req): + """Message class 'Req'.""" + + __slots__ = [ + '_uuid', + '_body', + ] + + _fields_and_field_types = { + 'uuid': 'string', + 'body': 'string', + } + + SLOT_TYPES = ( + rosidl_parser.definition.UnboundedString(), # noqa: E501 + rosidl_parser.definition.UnboundedString(), # noqa: E501 + ) + + def __init__(self, **kwargs): + assert all('_' + key in self.__slots__ for key in kwargs.keys()), \ + 'Invalid arguments passed to constructor: %s' % \ + ', '.join(sorted(k for k in kwargs.keys() if '_' + k not in self.__slots__)) + self.uuid = kwargs.get('uuid', str()) + self.body = kwargs.get('body', str()) + + def __repr__(self): + typename = self.__class__.__module__.split('.') + typename.pop() + typename.append(self.__class__.__name__) + args = [] + for s, t in zip(self.__slots__, self.SLOT_TYPES): + field = getattr(self, s) + fieldstr = repr(field) + # We use Python array type for fields that can be directly stored + # in them, and "normal" sequences for everything else. If it is + # a type that we store in an array, strip off the 'array' portion. + if ( + isinstance(t, rosidl_parser.definition.AbstractSequence) and + isinstance(t.value_type, rosidl_parser.definition.BasicType) and + t.value_type.typename in ['float', 'double', 'int8', 'uint8', 'int16', 'uint16', 'int32', 'uint32', 'int64', 'uint64'] + ): + if len(field) == 0: + fieldstr = '[]' + else: + assert fieldstr.startswith('array(') + prefix = "array('X', " + suffix = ')' + fieldstr = fieldstr[len(prefix):-len(suffix)] + args.append(s[1:] + '=' + fieldstr) + return '%s(%s)' % ('.'.join(typename), ', '.join(args)) + + def __eq__(self, other): + if not isinstance(other, self.__class__): + return False + if self.uuid != other.uuid: + return False + if self.body != other.body: + return False + return True + + @classmethod + def get_fields_and_field_types(cls): + from copy import copy + return copy(cls._fields_and_field_types) + + @property + def uuid(self): + """Message field 'uuid'.""" + return self._uuid + + @uuid.setter + def uuid(self, value): + if __debug__: + assert \ + isinstance(value, str), \ + "The 'uuid' field must be of type 'str'" + self._uuid = value + + @property + def body(self): + """Message field 'body'.""" + return self._body + + @body.setter + def body(self, value): + if __debug__: + assert \ + isinstance(value, str), \ + "The 'body' field must be of type 'str'" + self._body = value diff --git a/python/unitree_go/msg/_req_s.c b/python/unitree_go/msg/_req_s.c new file mode 100644 index 00000000..856782f8 --- /dev/null +++ b/python/unitree_go/msg/_req_s.c @@ -0,0 +1,145 @@ +// generated from rosidl_generator_py/resource/_idl_support.c.em +// with input from unitree_go:msg/Req.idl +// generated code does not contain a copyright notice +#define NPY_NO_DEPRECATED_API NPY_1_7_API_VERSION +#include +#include +#ifndef _WIN32 +# pragma GCC diagnostic push +# pragma GCC diagnostic ignored "-Wunused-function" +#endif +#include "numpy/ndarrayobject.h" +#ifndef _WIN32 +# pragma GCC diagnostic pop +#endif +#include "rosidl_runtime_c/visibility_control.h" +#include "unitree_go/msg/detail/req__struct.h" +#include "unitree_go/msg/detail/req__functions.h" + +#include "rosidl_runtime_c/string.h" +#include "rosidl_runtime_c/string_functions.h" + + +ROSIDL_GENERATOR_C_EXPORT +bool unitree_go__msg__req__convert_from_py(PyObject * _pymsg, void * _ros_message) +{ + // check that the passed message is of the expected Python class + { + char full_classname_dest[24]; + { + char * class_name = NULL; + char * module_name = NULL; + { + PyObject * class_attr = PyObject_GetAttrString(_pymsg, "__class__"); + if (class_attr) { + PyObject * name_attr = PyObject_GetAttrString(class_attr, "__name__"); + if (name_attr) { + class_name = (char *)PyUnicode_1BYTE_DATA(name_attr); + Py_DECREF(name_attr); + } + PyObject * module_attr = PyObject_GetAttrString(class_attr, "__module__"); + if (module_attr) { + module_name = (char *)PyUnicode_1BYTE_DATA(module_attr); + Py_DECREF(module_attr); + } + Py_DECREF(class_attr); + } + } + if (!class_name || !module_name) { + return false; + } + snprintf(full_classname_dest, sizeof(full_classname_dest), "%s.%s", module_name, class_name); + } + assert(strncmp("unitree_go.msg._req.Req", full_classname_dest, 23) == 0); + } + unitree_go__msg__Req * ros_message = _ros_message; + { // uuid + PyObject * field = PyObject_GetAttrString(_pymsg, "uuid"); + if (!field) { + return false; + } + assert(PyUnicode_Check(field)); + PyObject * encoded_field = PyUnicode_AsUTF8String(field); + if (!encoded_field) { + Py_DECREF(field); + return false; + } + rosidl_runtime_c__String__assign(&ros_message->uuid, PyBytes_AS_STRING(encoded_field)); + Py_DECREF(encoded_field); + Py_DECREF(field); + } + { // body + PyObject * field = PyObject_GetAttrString(_pymsg, "body"); + if (!field) { + return false; + } + assert(PyUnicode_Check(field)); + PyObject * encoded_field = PyUnicode_AsUTF8String(field); + if (!encoded_field) { + Py_DECREF(field); + return false; + } + rosidl_runtime_c__String__assign(&ros_message->body, PyBytes_AS_STRING(encoded_field)); + Py_DECREF(encoded_field); + Py_DECREF(field); + } + + return true; +} + +ROSIDL_GENERATOR_C_EXPORT +PyObject * unitree_go__msg__req__convert_to_py(void * raw_ros_message) +{ + /* NOTE(esteve): Call constructor of Req */ + PyObject * _pymessage = NULL; + { + PyObject * pymessage_module = PyImport_ImportModule("unitree_go.msg._req"); + assert(pymessage_module); + PyObject * pymessage_class = PyObject_GetAttrString(pymessage_module, "Req"); + assert(pymessage_class); + Py_DECREF(pymessage_module); + _pymessage = PyObject_CallObject(pymessage_class, NULL); + Py_DECREF(pymessage_class); + if (!_pymessage) { + return NULL; + } + } + unitree_go__msg__Req * ros_message = (unitree_go__msg__Req *)raw_ros_message; + { // uuid + PyObject * field = NULL; + field = PyUnicode_DecodeUTF8( + ros_message->uuid.data, + strlen(ros_message->uuid.data), + "replace"); + if (!field) { + return NULL; + } + { + int rc = PyObject_SetAttrString(_pymessage, "uuid", field); + Py_DECREF(field); + if (rc) { + return NULL; + } + } + } + { // body + PyObject * field = NULL; + field = PyUnicode_DecodeUTF8( + ros_message->body.data, + strlen(ros_message->body.data), + "replace"); + if (!field) { + return NULL; + } + { + int rc = PyObject_SetAttrString(_pymessage, "body", field); + Py_DECREF(field); + if (rc) { + return NULL; + } + } + } + + // ownership of _pymessage is transferred to the caller + return _pymessage; +} diff --git a/python/unitree_go/msg/_res.py b/python/unitree_go/msg/_res.py new file mode 100644 index 00000000..da1126d2 --- /dev/null +++ b/python/unitree_go/msg/_res.py @@ -0,0 +1,178 @@ +# generated from rosidl_generator_py/resource/_idl.py.em +# with input from unitree_go:msg/Res.idl +# generated code does not contain a copyright notice + + +# Import statements for member types + +# Member 'data' +import array # noqa: E402, I100 + +import rosidl_parser.definition # noqa: E402, I100 + + +class Metaclass_Res(type): + """Metaclass of message 'Res'.""" + + _CREATE_ROS_MESSAGE = None + _CONVERT_FROM_PY = None + _CONVERT_TO_PY = None + _DESTROY_ROS_MESSAGE = None + _TYPE_SUPPORT = None + + __constants = { + } + + @classmethod + def __import_type_support__(cls): + try: + from rosidl_generator_py import import_type_support + module = import_type_support('unitree_go') + except ImportError: + import logging + import traceback + logger = logging.getLogger( + 'unitree_go.msg.Res') + logger.debug( + 'Failed to import needed modules for type support:\n' + + traceback.format_exc()) + else: + cls._CREATE_ROS_MESSAGE = module.create_ros_message_msg__msg__res + cls._CONVERT_FROM_PY = module.convert_from_py_msg__msg__res + cls._CONVERT_TO_PY = module.convert_to_py_msg__msg__res + cls._TYPE_SUPPORT = module.type_support_msg__msg__res + cls._DESTROY_ROS_MESSAGE = module.destroy_ros_message_msg__msg__res + + @classmethod + def __prepare__(cls, name, bases, **kwargs): + # list constant names here so that they appear in the help text of + # the message class under "Data and other attributes defined here:" + # as well as populate each message instance + return { + } + + +class Res(metaclass=Metaclass_Res): + """Message class 'Res'.""" + + __slots__ = [ + '_uuid', + '_data', + '_body', + ] + + _fields_and_field_types = { + 'uuid': 'string', + 'data': 'sequence', + 'body': 'string', + } + + SLOT_TYPES = ( + rosidl_parser.definition.UnboundedString(), # noqa: E501 + rosidl_parser.definition.UnboundedSequence(rosidl_parser.definition.BasicType('uint8')), # noqa: E501 + rosidl_parser.definition.UnboundedString(), # noqa: E501 + ) + + def __init__(self, **kwargs): + assert all('_' + key in self.__slots__ for key in kwargs.keys()), \ + 'Invalid arguments passed to constructor: %s' % \ + ', '.join(sorted(k for k in kwargs.keys() if '_' + k not in self.__slots__)) + self.uuid = kwargs.get('uuid', str()) + self.data = array.array('B', kwargs.get('data', [])) + self.body = kwargs.get('body', str()) + + def __repr__(self): + typename = self.__class__.__module__.split('.') + typename.pop() + typename.append(self.__class__.__name__) + args = [] + for s, t in zip(self.__slots__, self.SLOT_TYPES): + field = getattr(self, s) + fieldstr = repr(field) + # We use Python array type for fields that can be directly stored + # in them, and "normal" sequences for everything else. If it is + # a type that we store in an array, strip off the 'array' portion. + if ( + isinstance(t, rosidl_parser.definition.AbstractSequence) and + isinstance(t.value_type, rosidl_parser.definition.BasicType) and + t.value_type.typename in ['float', 'double', 'int8', 'uint8', 'int16', 'uint16', 'int32', 'uint32', 'int64', 'uint64'] + ): + if len(field) == 0: + fieldstr = '[]' + else: + assert fieldstr.startswith('array(') + prefix = "array('X', " + suffix = ')' + fieldstr = fieldstr[len(prefix):-len(suffix)] + args.append(s[1:] + '=' + fieldstr) + return '%s(%s)' % ('.'.join(typename), ', '.join(args)) + + def __eq__(self, other): + if not isinstance(other, self.__class__): + return False + if self.uuid != other.uuid: + return False + if self.data != other.data: + return False + if self.body != other.body: + return False + return True + + @classmethod + def get_fields_and_field_types(cls): + from copy import copy + return copy(cls._fields_and_field_types) + + @property + def uuid(self): + """Message field 'uuid'.""" + return self._uuid + + @uuid.setter + def uuid(self, value): + if __debug__: + assert \ + isinstance(value, str), \ + "The 'uuid' field must be of type 'str'" + self._uuid = value + + @property + def data(self): + """Message field 'data'.""" + return self._data + + @data.setter + def data(self, value): + if isinstance(value, array.array): + assert value.typecode == 'B', \ + "The 'data' array.array() must have the type code of 'B'" + self._data = value + return + if __debug__: + from collections.abc import Sequence + from collections.abc import Set + from collections import UserList + from collections import UserString + assert \ + ((isinstance(value, Sequence) or + isinstance(value, Set) or + isinstance(value, UserList)) and + not isinstance(value, str) and + not isinstance(value, UserString) and + all(isinstance(v, int) for v in value) and + all(val >= 0 and val < 256 for val in value)), \ + "The 'data' field must be a set or sequence and each value of type 'int' and each unsigned integer in [0, 255]" + self._data = array.array('B', value) + + @property + def body(self): + """Message field 'body'.""" + return self._body + + @body.setter + def body(self, value): + if __debug__: + assert \ + isinstance(value, str), \ + "The 'body' field must be of type 'str'" + self._body = value diff --git a/python/unitree_go/msg/_res_s.c b/python/unitree_go/msg/_res_s.c new file mode 100644 index 00000000..4e40f1d9 --- /dev/null +++ b/python/unitree_go/msg/_res_s.c @@ -0,0 +1,268 @@ +// generated from rosidl_generator_py/resource/_idl_support.c.em +// with input from unitree_go:msg/Res.idl +// generated code does not contain a copyright notice +#define NPY_NO_DEPRECATED_API NPY_1_7_API_VERSION +#include +#include +#ifndef _WIN32 +# pragma GCC diagnostic push +# pragma GCC diagnostic ignored "-Wunused-function" +#endif +#include "numpy/ndarrayobject.h" +#ifndef _WIN32 +# pragma GCC diagnostic pop +#endif +#include "rosidl_runtime_c/visibility_control.h" +#include "unitree_go/msg/detail/res__struct.h" +#include "unitree_go/msg/detail/res__functions.h" + +#include "rosidl_runtime_c/string.h" +#include "rosidl_runtime_c/string_functions.h" + +#include "rosidl_runtime_c/primitives_sequence.h" +#include "rosidl_runtime_c/primitives_sequence_functions.h" + + +ROSIDL_GENERATOR_C_EXPORT +bool unitree_go__msg__res__convert_from_py(PyObject * _pymsg, void * _ros_message) +{ + // check that the passed message is of the expected Python class + { + char full_classname_dest[24]; + { + char * class_name = NULL; + char * module_name = NULL; + { + PyObject * class_attr = PyObject_GetAttrString(_pymsg, "__class__"); + if (class_attr) { + PyObject * name_attr = PyObject_GetAttrString(class_attr, "__name__"); + if (name_attr) { + class_name = (char *)PyUnicode_1BYTE_DATA(name_attr); + Py_DECREF(name_attr); + } + PyObject * module_attr = PyObject_GetAttrString(class_attr, "__module__"); + if (module_attr) { + module_name = (char *)PyUnicode_1BYTE_DATA(module_attr); + Py_DECREF(module_attr); + } + Py_DECREF(class_attr); + } + } + if (!class_name || !module_name) { + return false; + } + snprintf(full_classname_dest, sizeof(full_classname_dest), "%s.%s", module_name, class_name); + } + assert(strncmp("unitree_go.msg._res.Res", full_classname_dest, 23) == 0); + } + unitree_go__msg__Res * ros_message = _ros_message; + { // uuid + PyObject * field = PyObject_GetAttrString(_pymsg, "uuid"); + if (!field) { + return false; + } + assert(PyUnicode_Check(field)); + PyObject * encoded_field = PyUnicode_AsUTF8String(field); + if (!encoded_field) { + Py_DECREF(field); + return false; + } + rosidl_runtime_c__String__assign(&ros_message->uuid, PyBytes_AS_STRING(encoded_field)); + Py_DECREF(encoded_field); + Py_DECREF(field); + } + { // data + PyObject * field = PyObject_GetAttrString(_pymsg, "data"); + if (!field) { + return false; + } + if (PyObject_CheckBuffer(field)) { + // Optimization for converting arrays of primitives + Py_buffer view; + int rc = PyObject_GetBuffer(field, &view, PyBUF_SIMPLE); + if (rc < 0) { + Py_DECREF(field); + return false; + } + Py_ssize_t size = view.len / sizeof(uint8_t); + if (!rosidl_runtime_c__uint8__Sequence__init(&(ros_message->data), size)) { + PyErr_SetString(PyExc_RuntimeError, "unable to create uint8__Sequence ros_message"); + PyBuffer_Release(&view); + Py_DECREF(field); + return false; + } + uint8_t * dest = ros_message->data.data; + rc = PyBuffer_ToContiguous(dest, &view, view.len, 'C'); + if (rc < 0) { + PyBuffer_Release(&view); + Py_DECREF(field); + return false; + } + PyBuffer_Release(&view); + } else { + PyObject * seq_field = PySequence_Fast(field, "expected a sequence in 'data'"); + if (!seq_field) { + Py_DECREF(field); + return false; + } + Py_ssize_t size = PySequence_Size(field); + if (-1 == size) { + Py_DECREF(seq_field); + Py_DECREF(field); + return false; + } + if (!rosidl_runtime_c__uint8__Sequence__init(&(ros_message->data), size)) { + PyErr_SetString(PyExc_RuntimeError, "unable to create uint8__Sequence ros_message"); + Py_DECREF(seq_field); + Py_DECREF(field); + return false; + } + uint8_t * dest = ros_message->data.data; + for (Py_ssize_t i = 0; i < size; ++i) { + PyObject * item = PySequence_Fast_GET_ITEM(seq_field, i); + if (!item) { + Py_DECREF(seq_field); + Py_DECREF(field); + return false; + } + assert(PyLong_Check(item)); + uint8_t tmp = (uint8_t)PyLong_AsUnsignedLong(item); + + memcpy(&dest[i], &tmp, sizeof(uint8_t)); + } + Py_DECREF(seq_field); + } + Py_DECREF(field); + } + { // body + PyObject * field = PyObject_GetAttrString(_pymsg, "body"); + if (!field) { + return false; + } + assert(PyUnicode_Check(field)); + PyObject * encoded_field = PyUnicode_AsUTF8String(field); + if (!encoded_field) { + Py_DECREF(field); + return false; + } + rosidl_runtime_c__String__assign(&ros_message->body, PyBytes_AS_STRING(encoded_field)); + Py_DECREF(encoded_field); + Py_DECREF(field); + } + + return true; +} + +ROSIDL_GENERATOR_C_EXPORT +PyObject * unitree_go__msg__res__convert_to_py(void * raw_ros_message) +{ + /* NOTE(esteve): Call constructor of Res */ + PyObject * _pymessage = NULL; + { + PyObject * pymessage_module = PyImport_ImportModule("unitree_go.msg._res"); + assert(pymessage_module); + PyObject * pymessage_class = PyObject_GetAttrString(pymessage_module, "Res"); + assert(pymessage_class); + Py_DECREF(pymessage_module); + _pymessage = PyObject_CallObject(pymessage_class, NULL); + Py_DECREF(pymessage_class); + if (!_pymessage) { + return NULL; + } + } + unitree_go__msg__Res * ros_message = (unitree_go__msg__Res *)raw_ros_message; + { // uuid + PyObject * field = NULL; + field = PyUnicode_DecodeUTF8( + ros_message->uuid.data, + strlen(ros_message->uuid.data), + "replace"); + if (!field) { + return NULL; + } + { + int rc = PyObject_SetAttrString(_pymessage, "uuid", field); + Py_DECREF(field); + if (rc) { + return NULL; + } + } + } + { // data + PyObject * field = NULL; + field = PyObject_GetAttrString(_pymessage, "data"); + if (!field) { + return NULL; + } + assert(field->ob_type != NULL); + assert(field->ob_type->tp_name != NULL); + assert(strcmp(field->ob_type->tp_name, "array.array") == 0); + // ensure that itemsize matches the sizeof of the ROS message field + PyObject * itemsize_attr = PyObject_GetAttrString(field, "itemsize"); + assert(itemsize_attr != NULL); + size_t itemsize = PyLong_AsSize_t(itemsize_attr); + Py_DECREF(itemsize_attr); + if (itemsize != sizeof(uint8_t)) { + PyErr_SetString(PyExc_RuntimeError, "itemsize doesn't match expectation"); + Py_DECREF(field); + return NULL; + } + // clear the array, poor approach to remove potential default values + Py_ssize_t length = PyObject_Length(field); + if (-1 == length) { + Py_DECREF(field); + return NULL; + } + if (length > 0) { + PyObject * pop = PyObject_GetAttrString(field, "pop"); + assert(pop != NULL); + for (Py_ssize_t i = 0; i < length; ++i) { + PyObject * ret = PyObject_CallFunctionObjArgs(pop, NULL); + if (!ret) { + Py_DECREF(pop); + Py_DECREF(field); + return NULL; + } + Py_DECREF(ret); + } + Py_DECREF(pop); + } + if (ros_message->data.size > 0) { + // populating the array.array using the frombytes method + PyObject * frombytes = PyObject_GetAttrString(field, "frombytes"); + assert(frombytes != NULL); + uint8_t * src = &(ros_message->data.data[0]); + PyObject * data = PyBytes_FromStringAndSize((const char *)src, ros_message->data.size * sizeof(uint8_t)); + assert(data != NULL); + PyObject * ret = PyObject_CallFunctionObjArgs(frombytes, data, NULL); + Py_DECREF(data); + Py_DECREF(frombytes); + if (!ret) { + Py_DECREF(field); + return NULL; + } + Py_DECREF(ret); + } + Py_DECREF(field); + } + { // body + PyObject * field = NULL; + field = PyUnicode_DecodeUTF8( + ros_message->body.data, + strlen(ros_message->body.data), + "replace"); + if (!field) { + return NULL; + } + { + int rc = PyObject_SetAttrString(_pymessage, "body", field); + Py_DECREF(field); + if (rc) { + return NULL; + } + } + } + + // ownership of _pymessage is transferred to the caller + return _pymessage; +} diff --git a/python/unitree_go/msg/_sport_mode_cmd.py b/python/unitree_go/msg/_sport_mode_cmd.py new file mode 100644 index 00000000..7bcaf061 --- /dev/null +++ b/python/unitree_go/msg/_sport_mode_cmd.py @@ -0,0 +1,415 @@ +# generated from rosidl_generator_py/resource/_idl.py.em +# with input from unitree_go:msg/SportModeCmd.idl +# generated code does not contain a copyright notice + + +# Import statements for member types + +# Member 'position' +# Member 'euler' +# Member 'velocity' +import numpy # noqa: E402, I100 + +import rosidl_parser.definition # noqa: E402, I100 + + +class Metaclass_SportModeCmd(type): + """Metaclass of message 'SportModeCmd'.""" + + _CREATE_ROS_MESSAGE = None + _CONVERT_FROM_PY = None + _CONVERT_TO_PY = None + _DESTROY_ROS_MESSAGE = None + _TYPE_SUPPORT = None + + __constants = { + } + + @classmethod + def __import_type_support__(cls): + try: + from rosidl_generator_py import import_type_support + module = import_type_support('unitree_go') + except ImportError: + import logging + import traceback + logger = logging.getLogger( + 'unitree_go.msg.SportModeCmd') + logger.debug( + 'Failed to import needed modules for type support:\n' + + traceback.format_exc()) + else: + cls._CREATE_ROS_MESSAGE = module.create_ros_message_msg__msg__sport_mode_cmd + cls._CONVERT_FROM_PY = module.convert_from_py_msg__msg__sport_mode_cmd + cls._CONVERT_TO_PY = module.convert_to_py_msg__msg__sport_mode_cmd + cls._TYPE_SUPPORT = module.type_support_msg__msg__sport_mode_cmd + cls._DESTROY_ROS_MESSAGE = module.destroy_ros_message_msg__msg__sport_mode_cmd + + from unitree_go.msg import BmsCmd + if BmsCmd.__class__._TYPE_SUPPORT is None: + BmsCmd.__class__.__import_type_support__() + + from unitree_go.msg import PathPoint + if PathPoint.__class__._TYPE_SUPPORT is None: + PathPoint.__class__.__import_type_support__() + + @classmethod + def __prepare__(cls, name, bases, **kwargs): + # list constant names here so that they appear in the help text of + # the message class under "Data and other attributes defined here:" + # as well as populate each message instance + return { + } + + +class SportModeCmd(metaclass=Metaclass_SportModeCmd): + """Message class 'SportModeCmd'.""" + + __slots__ = [ + '_mode', + '_gait_type', + '_speed_level', + '_foot_raise_height', + '_body_height', + '_position', + '_euler', + '_velocity', + '_yaw_speed', + '_bms_cmd', + '_path_point', + ] + + _fields_and_field_types = { + 'mode': 'uint8', + 'gait_type': 'uint8', + 'speed_level': 'uint8', + 'foot_raise_height': 'float', + 'body_height': 'float', + 'position': 'float[2]', + 'euler': 'float[3]', + 'velocity': 'float[2]', + 'yaw_speed': 'float', + 'bms_cmd': 'unitree_go/BmsCmd', + 'path_point': 'unitree_go/PathPoint[30]', + } + + SLOT_TYPES = ( + rosidl_parser.definition.BasicType('uint8'), # noqa: E501 + rosidl_parser.definition.BasicType('uint8'), # noqa: E501 + rosidl_parser.definition.BasicType('uint8'), # noqa: E501 + rosidl_parser.definition.BasicType('float'), # noqa: E501 + rosidl_parser.definition.BasicType('float'), # noqa: E501 + rosidl_parser.definition.Array(rosidl_parser.definition.BasicType('float'), 2), # noqa: E501 + rosidl_parser.definition.Array(rosidl_parser.definition.BasicType('float'), 3), # noqa: E501 + rosidl_parser.definition.Array(rosidl_parser.definition.BasicType('float'), 2), # noqa: E501 + rosidl_parser.definition.BasicType('float'), # noqa: E501 + rosidl_parser.definition.NamespacedType(['unitree_go', 'msg'], 'BmsCmd'), # noqa: E501 + rosidl_parser.definition.Array(rosidl_parser.definition.NamespacedType(['unitree_go', 'msg'], 'PathPoint'), 30), # noqa: E501 + ) + + def __init__(self, **kwargs): + assert all('_' + key in self.__slots__ for key in kwargs.keys()), \ + 'Invalid arguments passed to constructor: %s' % \ + ', '.join(sorted(k for k in kwargs.keys() if '_' + k not in self.__slots__)) + self.mode = kwargs.get('mode', int()) + self.gait_type = kwargs.get('gait_type', int()) + self.speed_level = kwargs.get('speed_level', int()) + self.foot_raise_height = kwargs.get('foot_raise_height', float()) + self.body_height = kwargs.get('body_height', float()) + if 'position' not in kwargs: + self.position = numpy.zeros(2, dtype=numpy.float32) + else: + self.position = numpy.array(kwargs.get('position'), dtype=numpy.float32) + assert self.position.shape == (2, ) + if 'euler' not in kwargs: + self.euler = numpy.zeros(3, dtype=numpy.float32) + else: + self.euler = numpy.array(kwargs.get('euler'), dtype=numpy.float32) + assert self.euler.shape == (3, ) + if 'velocity' not in kwargs: + self.velocity = numpy.zeros(2, dtype=numpy.float32) + else: + self.velocity = numpy.array(kwargs.get('velocity'), dtype=numpy.float32) + assert self.velocity.shape == (2, ) + self.yaw_speed = kwargs.get('yaw_speed', float()) + from unitree_go.msg import BmsCmd + self.bms_cmd = kwargs.get('bms_cmd', BmsCmd()) + from unitree_go.msg import PathPoint + self.path_point = kwargs.get( + 'path_point', + [PathPoint() for x in range(30)] + ) + + def __repr__(self): + typename = self.__class__.__module__.split('.') + typename.pop() + typename.append(self.__class__.__name__) + args = [] + for s, t in zip(self.__slots__, self.SLOT_TYPES): + field = getattr(self, s) + fieldstr = repr(field) + # We use Python array type for fields that can be directly stored + # in them, and "normal" sequences for everything else. If it is + # a type that we store in an array, strip off the 'array' portion. + if ( + isinstance(t, rosidl_parser.definition.AbstractSequence) and + isinstance(t.value_type, rosidl_parser.definition.BasicType) and + t.value_type.typename in ['float', 'double', 'int8', 'uint8', 'int16', 'uint16', 'int32', 'uint32', 'int64', 'uint64'] + ): + if len(field) == 0: + fieldstr = '[]' + else: + assert fieldstr.startswith('array(') + prefix = "array('X', " + suffix = ')' + fieldstr = fieldstr[len(prefix):-len(suffix)] + args.append(s[1:] + '=' + fieldstr) + return '%s(%s)' % ('.'.join(typename), ', '.join(args)) + + def __eq__(self, other): + if not isinstance(other, self.__class__): + return False + if self.mode != other.mode: + return False + if self.gait_type != other.gait_type: + return False + if self.speed_level != other.speed_level: + return False + if self.foot_raise_height != other.foot_raise_height: + return False + if self.body_height != other.body_height: + return False + if all(self.position != other.position): + return False + if all(self.euler != other.euler): + return False + if all(self.velocity != other.velocity): + return False + if self.yaw_speed != other.yaw_speed: + return False + if self.bms_cmd != other.bms_cmd: + return False + if self.path_point != other.path_point: + return False + return True + + @classmethod + def get_fields_and_field_types(cls): + from copy import copy + return copy(cls._fields_and_field_types) + + @property + def mode(self): + """Message field 'mode'.""" + return self._mode + + @mode.setter + def mode(self, value): + if __debug__: + assert \ + isinstance(value, int), \ + "The 'mode' field must be of type 'int'" + assert value >= 0 and value < 256, \ + "The 'mode' field must be an unsigned integer in [0, 255]" + self._mode = value + + @property + def gait_type(self): + """Message field 'gait_type'.""" + return self._gait_type + + @gait_type.setter + def gait_type(self, value): + if __debug__: + assert \ + isinstance(value, int), \ + "The 'gait_type' field must be of type 'int'" + assert value >= 0 and value < 256, \ + "The 'gait_type' field must be an unsigned integer in [0, 255]" + self._gait_type = value + + @property + def speed_level(self): + """Message field 'speed_level'.""" + return self._speed_level + + @speed_level.setter + def speed_level(self, value): + if __debug__: + assert \ + isinstance(value, int), \ + "The 'speed_level' field must be of type 'int'" + assert value >= 0 and value < 256, \ + "The 'speed_level' field must be an unsigned integer in [0, 255]" + self._speed_level = value + + @property + def foot_raise_height(self): + """Message field 'foot_raise_height'.""" + return self._foot_raise_height + + @foot_raise_height.setter + def foot_raise_height(self, value): + if __debug__: + assert \ + isinstance(value, float), \ + "The 'foot_raise_height' field must be of type 'float'" + self._foot_raise_height = value + + @property + def body_height(self): + """Message field 'body_height'.""" + return self._body_height + + @body_height.setter + def body_height(self, value): + if __debug__: + assert \ + isinstance(value, float), \ + "The 'body_height' field must be of type 'float'" + self._body_height = value + + @property + def position(self): + """Message field 'position'.""" + return self._position + + @position.setter + def position(self, value): + if isinstance(value, numpy.ndarray): + assert value.dtype == numpy.float32, \ + "The 'position' numpy.ndarray() must have the dtype of 'numpy.float32'" + assert value.size == 2, \ + "The 'position' numpy.ndarray() must have a size of 2" + self._position = value + return + if __debug__: + from collections.abc import Sequence + from collections.abc import Set + from collections import UserList + from collections import UserString + assert \ + ((isinstance(value, Sequence) or + isinstance(value, Set) or + isinstance(value, UserList)) and + not isinstance(value, str) and + not isinstance(value, UserString) and + len(value) == 2 and + all(isinstance(v, float) for v in value) and + True), \ + "The 'position' field must be a set or sequence with length 2 and each value of type 'float'" + self._position = numpy.array(value, dtype=numpy.float32) + + @property + def euler(self): + """Message field 'euler'.""" + return self._euler + + @euler.setter + def euler(self, value): + if isinstance(value, numpy.ndarray): + assert value.dtype == numpy.float32, \ + "The 'euler' numpy.ndarray() must have the dtype of 'numpy.float32'" + assert value.size == 3, \ + "The 'euler' numpy.ndarray() must have a size of 3" + self._euler = value + return + if __debug__: + from collections.abc import Sequence + from collections.abc import Set + from collections import UserList + from collections import UserString + assert \ + ((isinstance(value, Sequence) or + isinstance(value, Set) or + isinstance(value, UserList)) and + not isinstance(value, str) and + not isinstance(value, UserString) and + len(value) == 3 and + all(isinstance(v, float) for v in value) and + True), \ + "The 'euler' field must be a set or sequence with length 3 and each value of type 'float'" + self._euler = numpy.array(value, dtype=numpy.float32) + + @property + def velocity(self): + """Message field 'velocity'.""" + return self._velocity + + @velocity.setter + def velocity(self, value): + if isinstance(value, numpy.ndarray): + assert value.dtype == numpy.float32, \ + "The 'velocity' numpy.ndarray() must have the dtype of 'numpy.float32'" + assert value.size == 2, \ + "The 'velocity' numpy.ndarray() must have a size of 2" + self._velocity = value + return + if __debug__: + from collections.abc import Sequence + from collections.abc import Set + from collections import UserList + from collections import UserString + assert \ + ((isinstance(value, Sequence) or + isinstance(value, Set) or + isinstance(value, UserList)) and + not isinstance(value, str) and + not isinstance(value, UserString) and + len(value) == 2 and + all(isinstance(v, float) for v in value) and + True), \ + "The 'velocity' field must be a set or sequence with length 2 and each value of type 'float'" + self._velocity = numpy.array(value, dtype=numpy.float32) + + @property + def yaw_speed(self): + """Message field 'yaw_speed'.""" + return self._yaw_speed + + @yaw_speed.setter + def yaw_speed(self, value): + if __debug__: + assert \ + isinstance(value, float), \ + "The 'yaw_speed' field must be of type 'float'" + self._yaw_speed = value + + @property + def bms_cmd(self): + """Message field 'bms_cmd'.""" + return self._bms_cmd + + @bms_cmd.setter + def bms_cmd(self, value): + if __debug__: + from unitree_go.msg import BmsCmd + assert \ + isinstance(value, BmsCmd), \ + "The 'bms_cmd' field must be a sub message of type 'BmsCmd'" + self._bms_cmd = value + + @property + def path_point(self): + """Message field 'path_point'.""" + return self._path_point + + @path_point.setter + def path_point(self, value): + if __debug__: + from unitree_go.msg import PathPoint + from collections.abc import Sequence + from collections.abc import Set + from collections import UserList + from collections import UserString + assert \ + ((isinstance(value, Sequence) or + isinstance(value, Set) or + isinstance(value, UserList)) and + not isinstance(value, str) and + not isinstance(value, UserString) and + len(value) == 30 and + all(isinstance(v, PathPoint) for v in value) and + True), \ + "The 'path_point' field must be a set or sequence with length 30 and each value of type 'PathPoint'" + self._path_point = value diff --git a/python/unitree_go/msg/_sport_mode_cmd_s.c b/python/unitree_go/msg/_sport_mode_cmd_s.c new file mode 100644 index 00000000..81633317 --- /dev/null +++ b/python/unitree_go/msg/_sport_mode_cmd_s.c @@ -0,0 +1,409 @@ +// generated from rosidl_generator_py/resource/_idl_support.c.em +// with input from unitree_go:msg/SportModeCmd.idl +// generated code does not contain a copyright notice +#define NPY_NO_DEPRECATED_API NPY_1_7_API_VERSION +#include +#include +#ifndef _WIN32 +# pragma GCC diagnostic push +# pragma GCC diagnostic ignored "-Wunused-function" +#endif +#include "numpy/ndarrayobject.h" +#ifndef _WIN32 +# pragma GCC diagnostic pop +#endif +#include "rosidl_runtime_c/visibility_control.h" +#include "unitree_go/msg/detail/sport_mode_cmd__struct.h" +#include "unitree_go/msg/detail/sport_mode_cmd__functions.h" + +#include "rosidl_runtime_c/primitives_sequence.h" +#include "rosidl_runtime_c/primitives_sequence_functions.h" + +// Nested array functions includes +#include "unitree_go/msg/detail/path_point__functions.h" +// end nested array functions include +bool unitree_go__msg__bms_cmd__convert_from_py(PyObject * _pymsg, void * _ros_message); +PyObject * unitree_go__msg__bms_cmd__convert_to_py(void * raw_ros_message); +bool unitree_go__msg__path_point__convert_from_py(PyObject * _pymsg, void * _ros_message); +PyObject * unitree_go__msg__path_point__convert_to_py(void * raw_ros_message); + +ROSIDL_GENERATOR_C_EXPORT +bool unitree_go__msg__sport_mode_cmd__convert_from_py(PyObject * _pymsg, void * _ros_message) +{ + // check that the passed message is of the expected Python class + { + char full_classname_dest[44]; + { + char * class_name = NULL; + char * module_name = NULL; + { + PyObject * class_attr = PyObject_GetAttrString(_pymsg, "__class__"); + if (class_attr) { + PyObject * name_attr = PyObject_GetAttrString(class_attr, "__name__"); + if (name_attr) { + class_name = (char *)PyUnicode_1BYTE_DATA(name_attr); + Py_DECREF(name_attr); + } + PyObject * module_attr = PyObject_GetAttrString(class_attr, "__module__"); + if (module_attr) { + module_name = (char *)PyUnicode_1BYTE_DATA(module_attr); + Py_DECREF(module_attr); + } + Py_DECREF(class_attr); + } + } + if (!class_name || !module_name) { + return false; + } + snprintf(full_classname_dest, sizeof(full_classname_dest), "%s.%s", module_name, class_name); + } + assert(strncmp("unitree_go.msg._sport_mode_cmd.SportModeCmd", full_classname_dest, 43) == 0); + } + unitree_go__msg__SportModeCmd * ros_message = _ros_message; + { // mode + PyObject * field = PyObject_GetAttrString(_pymsg, "mode"); + if (!field) { + return false; + } + assert(PyLong_Check(field)); + ros_message->mode = (uint8_t)PyLong_AsUnsignedLong(field); + Py_DECREF(field); + } + { // gait_type + PyObject * field = PyObject_GetAttrString(_pymsg, "gait_type"); + if (!field) { + return false; + } + assert(PyLong_Check(field)); + ros_message->gait_type = (uint8_t)PyLong_AsUnsignedLong(field); + Py_DECREF(field); + } + { // speed_level + PyObject * field = PyObject_GetAttrString(_pymsg, "speed_level"); + if (!field) { + return false; + } + assert(PyLong_Check(field)); + ros_message->speed_level = (uint8_t)PyLong_AsUnsignedLong(field); + Py_DECREF(field); + } + { // foot_raise_height + PyObject * field = PyObject_GetAttrString(_pymsg, "foot_raise_height"); + if (!field) { + return false; + } + assert(PyFloat_Check(field)); + ros_message->foot_raise_height = (float)PyFloat_AS_DOUBLE(field); + Py_DECREF(field); + } + { // body_height + PyObject * field = PyObject_GetAttrString(_pymsg, "body_height"); + if (!field) { + return false; + } + assert(PyFloat_Check(field)); + ros_message->body_height = (float)PyFloat_AS_DOUBLE(field); + Py_DECREF(field); + } + { // position + PyObject * field = PyObject_GetAttrString(_pymsg, "position"); + if (!field) { + return false; + } + { + // TODO(dirk-thomas) use a better way to check the type before casting + assert(field->ob_type != NULL); + assert(field->ob_type->tp_name != NULL); + assert(strcmp(field->ob_type->tp_name, "numpy.ndarray") == 0); + PyArrayObject * seq_field = (PyArrayObject *)field; + Py_INCREF(seq_field); + assert(PyArray_NDIM(seq_field) == 1); + assert(PyArray_TYPE(seq_field) == NPY_FLOAT32); + Py_ssize_t size = 2; + float * dest = ros_message->position; + for (Py_ssize_t i = 0; i < size; ++i) { + float tmp = *(npy_float32 *)PyArray_GETPTR1(seq_field, i); + memcpy(&dest[i], &tmp, sizeof(float)); + } + Py_DECREF(seq_field); + } + Py_DECREF(field); + } + { // euler + PyObject * field = PyObject_GetAttrString(_pymsg, "euler"); + if (!field) { + return false; + } + { + // TODO(dirk-thomas) use a better way to check the type before casting + assert(field->ob_type != NULL); + assert(field->ob_type->tp_name != NULL); + assert(strcmp(field->ob_type->tp_name, "numpy.ndarray") == 0); + PyArrayObject * seq_field = (PyArrayObject *)field; + Py_INCREF(seq_field); + assert(PyArray_NDIM(seq_field) == 1); + assert(PyArray_TYPE(seq_field) == NPY_FLOAT32); + Py_ssize_t size = 3; + float * dest = ros_message->euler; + for (Py_ssize_t i = 0; i < size; ++i) { + float tmp = *(npy_float32 *)PyArray_GETPTR1(seq_field, i); + memcpy(&dest[i], &tmp, sizeof(float)); + } + Py_DECREF(seq_field); + } + Py_DECREF(field); + } + { // velocity + PyObject * field = PyObject_GetAttrString(_pymsg, "velocity"); + if (!field) { + return false; + } + { + // TODO(dirk-thomas) use a better way to check the type before casting + assert(field->ob_type != NULL); + assert(field->ob_type->tp_name != NULL); + assert(strcmp(field->ob_type->tp_name, "numpy.ndarray") == 0); + PyArrayObject * seq_field = (PyArrayObject *)field; + Py_INCREF(seq_field); + assert(PyArray_NDIM(seq_field) == 1); + assert(PyArray_TYPE(seq_field) == NPY_FLOAT32); + Py_ssize_t size = 2; + float * dest = ros_message->velocity; + for (Py_ssize_t i = 0; i < size; ++i) { + float tmp = *(npy_float32 *)PyArray_GETPTR1(seq_field, i); + memcpy(&dest[i], &tmp, sizeof(float)); + } + Py_DECREF(seq_field); + } + Py_DECREF(field); + } + { // yaw_speed + PyObject * field = PyObject_GetAttrString(_pymsg, "yaw_speed"); + if (!field) { + return false; + } + assert(PyFloat_Check(field)); + ros_message->yaw_speed = (float)PyFloat_AS_DOUBLE(field); + Py_DECREF(field); + } + { // bms_cmd + PyObject * field = PyObject_GetAttrString(_pymsg, "bms_cmd"); + if (!field) { + return false; + } + if (!unitree_go__msg__bms_cmd__convert_from_py(field, &ros_message->bms_cmd)) { + Py_DECREF(field); + return false; + } + Py_DECREF(field); + } + { // path_point + PyObject * field = PyObject_GetAttrString(_pymsg, "path_point"); + if (!field) { + return false; + } + PyObject * seq_field = PySequence_Fast(field, "expected a sequence in 'path_point'"); + if (!seq_field) { + Py_DECREF(field); + return false; + } + Py_ssize_t size = 30; + unitree_go__msg__PathPoint * dest = ros_message->path_point; + for (Py_ssize_t i = 0; i < size; ++i) { + if (!unitree_go__msg__path_point__convert_from_py(PySequence_Fast_GET_ITEM(seq_field, i), &dest[i])) { + Py_DECREF(seq_field); + Py_DECREF(field); + return false; + } + } + Py_DECREF(seq_field); + Py_DECREF(field); + } + + return true; +} + +ROSIDL_GENERATOR_C_EXPORT +PyObject * unitree_go__msg__sport_mode_cmd__convert_to_py(void * raw_ros_message) +{ + /* NOTE(esteve): Call constructor of SportModeCmd */ + PyObject * _pymessage = NULL; + { + PyObject * pymessage_module = PyImport_ImportModule("unitree_go.msg._sport_mode_cmd"); + assert(pymessage_module); + PyObject * pymessage_class = PyObject_GetAttrString(pymessage_module, "SportModeCmd"); + assert(pymessage_class); + Py_DECREF(pymessage_module); + _pymessage = PyObject_CallObject(pymessage_class, NULL); + Py_DECREF(pymessage_class); + if (!_pymessage) { + return NULL; + } + } + unitree_go__msg__SportModeCmd * ros_message = (unitree_go__msg__SportModeCmd *)raw_ros_message; + { // mode + PyObject * field = NULL; + field = PyLong_FromUnsignedLong(ros_message->mode); + { + int rc = PyObject_SetAttrString(_pymessage, "mode", field); + Py_DECREF(field); + if (rc) { + return NULL; + } + } + } + { // gait_type + PyObject * field = NULL; + field = PyLong_FromUnsignedLong(ros_message->gait_type); + { + int rc = PyObject_SetAttrString(_pymessage, "gait_type", field); + Py_DECREF(field); + if (rc) { + return NULL; + } + } + } + { // speed_level + PyObject * field = NULL; + field = PyLong_FromUnsignedLong(ros_message->speed_level); + { + int rc = PyObject_SetAttrString(_pymessage, "speed_level", field); + Py_DECREF(field); + if (rc) { + return NULL; + } + } + } + { // foot_raise_height + PyObject * field = NULL; + field = PyFloat_FromDouble(ros_message->foot_raise_height); + { + int rc = PyObject_SetAttrString(_pymessage, "foot_raise_height", field); + Py_DECREF(field); + if (rc) { + return NULL; + } + } + } + { // body_height + PyObject * field = NULL; + field = PyFloat_FromDouble(ros_message->body_height); + { + int rc = PyObject_SetAttrString(_pymessage, "body_height", field); + Py_DECREF(field); + if (rc) { + return NULL; + } + } + } + { // position + PyObject * field = NULL; + field = PyObject_GetAttrString(_pymessage, "position"); + if (!field) { + return NULL; + } + assert(field->ob_type != NULL); + assert(field->ob_type->tp_name != NULL); + assert(strcmp(field->ob_type->tp_name, "numpy.ndarray") == 0); + PyArrayObject * seq_field = (PyArrayObject *)field; + assert(PyArray_NDIM(seq_field) == 1); + assert(PyArray_TYPE(seq_field) == NPY_FLOAT32); + assert(sizeof(npy_float32) == sizeof(float)); + npy_float32 * dst = (npy_float32 *)PyArray_GETPTR1(seq_field, 0); + float * src = &(ros_message->position[0]); + memcpy(dst, src, 2 * sizeof(float)); + Py_DECREF(field); + } + { // euler + PyObject * field = NULL; + field = PyObject_GetAttrString(_pymessage, "euler"); + if (!field) { + return NULL; + } + assert(field->ob_type != NULL); + assert(field->ob_type->tp_name != NULL); + assert(strcmp(field->ob_type->tp_name, "numpy.ndarray") == 0); + PyArrayObject * seq_field = (PyArrayObject *)field; + assert(PyArray_NDIM(seq_field) == 1); + assert(PyArray_TYPE(seq_field) == NPY_FLOAT32); + assert(sizeof(npy_float32) == sizeof(float)); + npy_float32 * dst = (npy_float32 *)PyArray_GETPTR1(seq_field, 0); + float * src = &(ros_message->euler[0]); + memcpy(dst, src, 3 * sizeof(float)); + Py_DECREF(field); + } + { // velocity + PyObject * field = NULL; + field = PyObject_GetAttrString(_pymessage, "velocity"); + if (!field) { + return NULL; + } + assert(field->ob_type != NULL); + assert(field->ob_type->tp_name != NULL); + assert(strcmp(field->ob_type->tp_name, "numpy.ndarray") == 0); + PyArrayObject * seq_field = (PyArrayObject *)field; + assert(PyArray_NDIM(seq_field) == 1); + assert(PyArray_TYPE(seq_field) == NPY_FLOAT32); + assert(sizeof(npy_float32) == sizeof(float)); + npy_float32 * dst = (npy_float32 *)PyArray_GETPTR1(seq_field, 0); + float * src = &(ros_message->velocity[0]); + memcpy(dst, src, 2 * sizeof(float)); + Py_DECREF(field); + } + { // yaw_speed + PyObject * field = NULL; + field = PyFloat_FromDouble(ros_message->yaw_speed); + { + int rc = PyObject_SetAttrString(_pymessage, "yaw_speed", field); + Py_DECREF(field); + if (rc) { + return NULL; + } + } + } + { // bms_cmd + PyObject * field = NULL; + field = unitree_go__msg__bms_cmd__convert_to_py(&ros_message->bms_cmd); + if (!field) { + return NULL; + } + { + int rc = PyObject_SetAttrString(_pymessage, "bms_cmd", field); + Py_DECREF(field); + if (rc) { + return NULL; + } + } + } + { // path_point + PyObject * field = NULL; + size_t size = 30; + field = PyList_New(size); + if (!field) { + return NULL; + } + unitree_go__msg__PathPoint * item; + for (size_t i = 0; i < size; ++i) { + item = &(ros_message->path_point[i]); + PyObject * pyitem = unitree_go__msg__path_point__convert_to_py(item); + if (!pyitem) { + Py_DECREF(field); + return NULL; + } + int rc = PyList_SetItem(field, i, pyitem); + (void)rc; + assert(rc == 0); + } + assert(PySequence_Check(field)); + { + int rc = PyObject_SetAttrString(_pymessage, "path_point", field); + Py_DECREF(field); + if (rc) { + return NULL; + } + } + } + + // ownership of _pymessage is transferred to the caller + return _pymessage; +} diff --git a/python/unitree_go/msg/_sport_mode_state.py b/python/unitree_go/msg/_sport_mode_state.py new file mode 100644 index 00000000..a00aa275 --- /dev/null +++ b/python/unitree_go/msg/_sport_mode_state.py @@ -0,0 +1,546 @@ +# generated from rosidl_generator_py/resource/_idl.py.em +# with input from unitree_go:msg/SportModeState.idl +# generated code does not contain a copyright notice + + +# Import statements for member types + +# Member 'position' +# Member 'velocity' +# Member 'range_obstacle' +# Member 'foot_force' +# Member 'foot_position_body' +# Member 'foot_speed_body' +import numpy # noqa: E402, I100 + +import rosidl_parser.definition # noqa: E402, I100 + + +class Metaclass_SportModeState(type): + """Metaclass of message 'SportModeState'.""" + + _CREATE_ROS_MESSAGE = None + _CONVERT_FROM_PY = None + _CONVERT_TO_PY = None + _DESTROY_ROS_MESSAGE = None + _TYPE_SUPPORT = None + + __constants = { + } + + @classmethod + def __import_type_support__(cls): + try: + from rosidl_generator_py import import_type_support + module = import_type_support('unitree_go') + except ImportError: + import logging + import traceback + logger = logging.getLogger( + 'unitree_go.msg.SportModeState') + logger.debug( + 'Failed to import needed modules for type support:\n' + + traceback.format_exc()) + else: + cls._CREATE_ROS_MESSAGE = module.create_ros_message_msg__msg__sport_mode_state + cls._CONVERT_FROM_PY = module.convert_from_py_msg__msg__sport_mode_state + cls._CONVERT_TO_PY = module.convert_to_py_msg__msg__sport_mode_state + cls._TYPE_SUPPORT = module.type_support_msg__msg__sport_mode_state + cls._DESTROY_ROS_MESSAGE = module.destroy_ros_message_msg__msg__sport_mode_state + + from unitree_go.msg import IMUState + if IMUState.__class__._TYPE_SUPPORT is None: + IMUState.__class__.__import_type_support__() + + from unitree_go.msg import TimeSpec + if TimeSpec.__class__._TYPE_SUPPORT is None: + TimeSpec.__class__.__import_type_support__() + + @classmethod + def __prepare__(cls, name, bases, **kwargs): + # list constant names here so that they appear in the help text of + # the message class under "Data and other attributes defined here:" + # as well as populate each message instance + return { + } + + +class SportModeState(metaclass=Metaclass_SportModeState): + """Message class 'SportModeState'.""" + + __slots__ = [ + '_stamp', + '_error_code', + '_imu_state', + '_mode', + '_progress', + '_gait_type', + '_foot_raise_height', + '_position', + '_body_height', + '_velocity', + '_yaw_speed', + '_range_obstacle', + '_foot_force', + '_foot_position_body', + '_foot_speed_body', + ] + + _fields_and_field_types = { + 'stamp': 'unitree_go/TimeSpec', + 'error_code': 'uint32', + 'imu_state': 'unitree_go/IMUState', + 'mode': 'uint8', + 'progress': 'float', + 'gait_type': 'uint8', + 'foot_raise_height': 'float', + 'position': 'float[3]', + 'body_height': 'float', + 'velocity': 'float[3]', + 'yaw_speed': 'float', + 'range_obstacle': 'float[4]', + 'foot_force': 'int16[4]', + 'foot_position_body': 'float[12]', + 'foot_speed_body': 'float[12]', + } + + SLOT_TYPES = ( + rosidl_parser.definition.NamespacedType(['unitree_go', 'msg'], 'TimeSpec'), # noqa: E501 + rosidl_parser.definition.BasicType('uint32'), # noqa: E501 + rosidl_parser.definition.NamespacedType(['unitree_go', 'msg'], 'IMUState'), # noqa: E501 + rosidl_parser.definition.BasicType('uint8'), # noqa: E501 + rosidl_parser.definition.BasicType('float'), # noqa: E501 + rosidl_parser.definition.BasicType('uint8'), # noqa: E501 + rosidl_parser.definition.BasicType('float'), # noqa: E501 + rosidl_parser.definition.Array(rosidl_parser.definition.BasicType('float'), 3), # noqa: E501 + rosidl_parser.definition.BasicType('float'), # noqa: E501 + rosidl_parser.definition.Array(rosidl_parser.definition.BasicType('float'), 3), # noqa: E501 + rosidl_parser.definition.BasicType('float'), # noqa: E501 + rosidl_parser.definition.Array(rosidl_parser.definition.BasicType('float'), 4), # noqa: E501 + rosidl_parser.definition.Array(rosidl_parser.definition.BasicType('int16'), 4), # noqa: E501 + rosidl_parser.definition.Array(rosidl_parser.definition.BasicType('float'), 12), # noqa: E501 + rosidl_parser.definition.Array(rosidl_parser.definition.BasicType('float'), 12), # noqa: E501 + ) + + def __init__(self, **kwargs): + assert all('_' + key in self.__slots__ for key in kwargs.keys()), \ + 'Invalid arguments passed to constructor: %s' % \ + ', '.join(sorted(k for k in kwargs.keys() if '_' + k not in self.__slots__)) + from unitree_go.msg import TimeSpec + self.stamp = kwargs.get('stamp', TimeSpec()) + self.error_code = kwargs.get('error_code', int()) + from unitree_go.msg import IMUState + self.imu_state = kwargs.get('imu_state', IMUState()) + self.mode = kwargs.get('mode', int()) + self.progress = kwargs.get('progress', float()) + self.gait_type = kwargs.get('gait_type', int()) + self.foot_raise_height = kwargs.get('foot_raise_height', float()) + if 'position' not in kwargs: + self.position = numpy.zeros(3, dtype=numpy.float32) + else: + self.position = numpy.array(kwargs.get('position'), dtype=numpy.float32) + assert self.position.shape == (3, ) + self.body_height = kwargs.get('body_height', float()) + if 'velocity' not in kwargs: + self.velocity = numpy.zeros(3, dtype=numpy.float32) + else: + self.velocity = numpy.array(kwargs.get('velocity'), dtype=numpy.float32) + assert self.velocity.shape == (3, ) + self.yaw_speed = kwargs.get('yaw_speed', float()) + if 'range_obstacle' not in kwargs: + self.range_obstacle = numpy.zeros(4, dtype=numpy.float32) + else: + self.range_obstacle = numpy.array(kwargs.get('range_obstacle'), dtype=numpy.float32) + assert self.range_obstacle.shape == (4, ) + if 'foot_force' not in kwargs: + self.foot_force = numpy.zeros(4, dtype=numpy.int16) + else: + self.foot_force = numpy.array(kwargs.get('foot_force'), dtype=numpy.int16) + assert self.foot_force.shape == (4, ) + if 'foot_position_body' not in kwargs: + self.foot_position_body = numpy.zeros(12, dtype=numpy.float32) + else: + self.foot_position_body = numpy.array(kwargs.get('foot_position_body'), dtype=numpy.float32) + assert self.foot_position_body.shape == (12, ) + if 'foot_speed_body' not in kwargs: + self.foot_speed_body = numpy.zeros(12, dtype=numpy.float32) + else: + self.foot_speed_body = numpy.array(kwargs.get('foot_speed_body'), dtype=numpy.float32) + assert self.foot_speed_body.shape == (12, ) + + def __repr__(self): + typename = self.__class__.__module__.split('.') + typename.pop() + typename.append(self.__class__.__name__) + args = [] + for s, t in zip(self.__slots__, self.SLOT_TYPES): + field = getattr(self, s) + fieldstr = repr(field) + # We use Python array type for fields that can be directly stored + # in them, and "normal" sequences for everything else. If it is + # a type that we store in an array, strip off the 'array' portion. + if ( + isinstance(t, rosidl_parser.definition.AbstractSequence) and + isinstance(t.value_type, rosidl_parser.definition.BasicType) and + t.value_type.typename in ['float', 'double', 'int8', 'uint8', 'int16', 'uint16', 'int32', 'uint32', 'int64', 'uint64'] + ): + if len(field) == 0: + fieldstr = '[]' + else: + assert fieldstr.startswith('array(') + prefix = "array('X', " + suffix = ')' + fieldstr = fieldstr[len(prefix):-len(suffix)] + args.append(s[1:] + '=' + fieldstr) + return '%s(%s)' % ('.'.join(typename), ', '.join(args)) + + def __eq__(self, other): + if not isinstance(other, self.__class__): + return False + if self.stamp != other.stamp: + return False + if self.error_code != other.error_code: + return False + if self.imu_state != other.imu_state: + return False + if self.mode != other.mode: + return False + if self.progress != other.progress: + return False + if self.gait_type != other.gait_type: + return False + if self.foot_raise_height != other.foot_raise_height: + return False + if all(self.position != other.position): + return False + if self.body_height != other.body_height: + return False + if all(self.velocity != other.velocity): + return False + if self.yaw_speed != other.yaw_speed: + return False + if all(self.range_obstacle != other.range_obstacle): + return False + if all(self.foot_force != other.foot_force): + return False + if all(self.foot_position_body != other.foot_position_body): + return False + if all(self.foot_speed_body != other.foot_speed_body): + return False + return True + + @classmethod + def get_fields_and_field_types(cls): + from copy import copy + return copy(cls._fields_and_field_types) + + @property + def stamp(self): + """Message field 'stamp'.""" + return self._stamp + + @stamp.setter + def stamp(self, value): + if __debug__: + from unitree_go.msg import TimeSpec + assert \ + isinstance(value, TimeSpec), \ + "The 'stamp' field must be a sub message of type 'TimeSpec'" + self._stamp = value + + @property + def error_code(self): + """Message field 'error_code'.""" + return self._error_code + + @error_code.setter + def error_code(self, value): + if __debug__: + assert \ + isinstance(value, int), \ + "The 'error_code' field must be of type 'int'" + assert value >= 0 and value < 4294967296, \ + "The 'error_code' field must be an unsigned integer in [0, 4294967295]" + self._error_code = value + + @property + def imu_state(self): + """Message field 'imu_state'.""" + return self._imu_state + + @imu_state.setter + def imu_state(self, value): + if __debug__: + from unitree_go.msg import IMUState + assert \ + isinstance(value, IMUState), \ + "The 'imu_state' field must be a sub message of type 'IMUState'" + self._imu_state = value + + @property + def mode(self): + """Message field 'mode'.""" + return self._mode + + @mode.setter + def mode(self, value): + if __debug__: + assert \ + isinstance(value, int), \ + "The 'mode' field must be of type 'int'" + assert value >= 0 and value < 256, \ + "The 'mode' field must be an unsigned integer in [0, 255]" + self._mode = value + + @property + def progress(self): + """Message field 'progress'.""" + return self._progress + + @progress.setter + def progress(self, value): + if __debug__: + assert \ + isinstance(value, float), \ + "The 'progress' field must be of type 'float'" + self._progress = value + + @property + def gait_type(self): + """Message field 'gait_type'.""" + return self._gait_type + + @gait_type.setter + def gait_type(self, value): + if __debug__: + assert \ + isinstance(value, int), \ + "The 'gait_type' field must be of type 'int'" + assert value >= 0 and value < 256, \ + "The 'gait_type' field must be an unsigned integer in [0, 255]" + self._gait_type = value + + @property + def foot_raise_height(self): + """Message field 'foot_raise_height'.""" + return self._foot_raise_height + + @foot_raise_height.setter + def foot_raise_height(self, value): + if __debug__: + assert \ + isinstance(value, float), \ + "The 'foot_raise_height' field must be of type 'float'" + self._foot_raise_height = value + + @property + def position(self): + """Message field 'position'.""" + return self._position + + @position.setter + def position(self, value): + if isinstance(value, numpy.ndarray): + assert value.dtype == numpy.float32, \ + "The 'position' numpy.ndarray() must have the dtype of 'numpy.float32'" + assert value.size == 3, \ + "The 'position' numpy.ndarray() must have a size of 3" + self._position = value + return + if __debug__: + from collections.abc import Sequence + from collections.abc import Set + from collections import UserList + from collections import UserString + assert \ + ((isinstance(value, Sequence) or + isinstance(value, Set) or + isinstance(value, UserList)) and + not isinstance(value, str) and + not isinstance(value, UserString) and + len(value) == 3 and + all(isinstance(v, float) for v in value) and + True), \ + "The 'position' field must be a set or sequence with length 3 and each value of type 'float'" + self._position = numpy.array(value, dtype=numpy.float32) + + @property + def body_height(self): + """Message field 'body_height'.""" + return self._body_height + + @body_height.setter + def body_height(self, value): + if __debug__: + assert \ + isinstance(value, float), \ + "The 'body_height' field must be of type 'float'" + self._body_height = value + + @property + def velocity(self): + """Message field 'velocity'.""" + return self._velocity + + @velocity.setter + def velocity(self, value): + if isinstance(value, numpy.ndarray): + assert value.dtype == numpy.float32, \ + "The 'velocity' numpy.ndarray() must have the dtype of 'numpy.float32'" + assert value.size == 3, \ + "The 'velocity' numpy.ndarray() must have a size of 3" + self._velocity = value + return + if __debug__: + from collections.abc import Sequence + from collections.abc import Set + from collections import UserList + from collections import UserString + assert \ + ((isinstance(value, Sequence) or + isinstance(value, Set) or + isinstance(value, UserList)) and + not isinstance(value, str) and + not isinstance(value, UserString) and + len(value) == 3 and + all(isinstance(v, float) for v in value) and + True), \ + "The 'velocity' field must be a set or sequence with length 3 and each value of type 'float'" + self._velocity = numpy.array(value, dtype=numpy.float32) + + @property + def yaw_speed(self): + """Message field 'yaw_speed'.""" + return self._yaw_speed + + @yaw_speed.setter + def yaw_speed(self, value): + if __debug__: + assert \ + isinstance(value, float), \ + "The 'yaw_speed' field must be of type 'float'" + self._yaw_speed = value + + @property + def range_obstacle(self): + """Message field 'range_obstacle'.""" + return self._range_obstacle + + @range_obstacle.setter + def range_obstacle(self, value): + if isinstance(value, numpy.ndarray): + assert value.dtype == numpy.float32, \ + "The 'range_obstacle' numpy.ndarray() must have the dtype of 'numpy.float32'" + assert value.size == 4, \ + "The 'range_obstacle' numpy.ndarray() must have a size of 4" + self._range_obstacle = value + return + if __debug__: + from collections.abc import Sequence + from collections.abc import Set + from collections import UserList + from collections import UserString + assert \ + ((isinstance(value, Sequence) or + isinstance(value, Set) or + isinstance(value, UserList)) and + not isinstance(value, str) and + not isinstance(value, UserString) and + len(value) == 4 and + all(isinstance(v, float) for v in value) and + True), \ + "The 'range_obstacle' field must be a set or sequence with length 4 and each value of type 'float'" + self._range_obstacle = numpy.array(value, dtype=numpy.float32) + + @property + def foot_force(self): + """Message field 'foot_force'.""" + return self._foot_force + + @foot_force.setter + def foot_force(self, value): + if isinstance(value, numpy.ndarray): + assert value.dtype == numpy.int16, \ + "The 'foot_force' numpy.ndarray() must have the dtype of 'numpy.int16'" + assert value.size == 4, \ + "The 'foot_force' numpy.ndarray() must have a size of 4" + self._foot_force = value + return + if __debug__: + from collections.abc import Sequence + from collections.abc import Set + from collections import UserList + from collections import UserString + assert \ + ((isinstance(value, Sequence) or + isinstance(value, Set) or + isinstance(value, UserList)) and + not isinstance(value, str) and + not isinstance(value, UserString) and + len(value) == 4 and + all(isinstance(v, int) for v in value) and + all(val >= -32768 and val < 32768 for val in value)), \ + "The 'foot_force' field must be a set or sequence with length 4 and each value of type 'int' and each integer in [-32768, 32767]" + self._foot_force = numpy.array(value, dtype=numpy.int16) + + @property + def foot_position_body(self): + """Message field 'foot_position_body'.""" + return self._foot_position_body + + @foot_position_body.setter + def foot_position_body(self, value): + if isinstance(value, numpy.ndarray): + assert value.dtype == numpy.float32, \ + "The 'foot_position_body' numpy.ndarray() must have the dtype of 'numpy.float32'" + assert value.size == 12, \ + "The 'foot_position_body' numpy.ndarray() must have a size of 12" + self._foot_position_body = value + return + if __debug__: + from collections.abc import Sequence + from collections.abc import Set + from collections import UserList + from collections import UserString + assert \ + ((isinstance(value, Sequence) or + isinstance(value, Set) or + isinstance(value, UserList)) and + not isinstance(value, str) and + not isinstance(value, UserString) and + len(value) == 12 and + all(isinstance(v, float) for v in value) and + True), \ + "The 'foot_position_body' field must be a set or sequence with length 12 and each value of type 'float'" + self._foot_position_body = numpy.array(value, dtype=numpy.float32) + + @property + def foot_speed_body(self): + """Message field 'foot_speed_body'.""" + return self._foot_speed_body + + @foot_speed_body.setter + def foot_speed_body(self, value): + if isinstance(value, numpy.ndarray): + assert value.dtype == numpy.float32, \ + "The 'foot_speed_body' numpy.ndarray() must have the dtype of 'numpy.float32'" + assert value.size == 12, \ + "The 'foot_speed_body' numpy.ndarray() must have a size of 12" + self._foot_speed_body = value + return + if __debug__: + from collections.abc import Sequence + from collections.abc import Set + from collections import UserList + from collections import UserString + assert \ + ((isinstance(value, Sequence) or + isinstance(value, Set) or + isinstance(value, UserList)) and + not isinstance(value, str) and + not isinstance(value, UserString) and + len(value) == 12 and + all(isinstance(v, float) for v in value) and + True), \ + "The 'foot_speed_body' field must be a set or sequence with length 12 and each value of type 'float'" + self._foot_speed_body = numpy.array(value, dtype=numpy.float32) diff --git a/python/unitree_go/msg/_sport_mode_state_s.c b/python/unitree_go/msg/_sport_mode_state_s.c new file mode 100644 index 00000000..b25a50b7 --- /dev/null +++ b/python/unitree_go/msg/_sport_mode_state_s.c @@ -0,0 +1,527 @@ +// generated from rosidl_generator_py/resource/_idl_support.c.em +// with input from unitree_go:msg/SportModeState.idl +// generated code does not contain a copyright notice +#define NPY_NO_DEPRECATED_API NPY_1_7_API_VERSION +#include +#include +#ifndef _WIN32 +# pragma GCC diagnostic push +# pragma GCC diagnostic ignored "-Wunused-function" +#endif +#include "numpy/ndarrayobject.h" +#ifndef _WIN32 +# pragma GCC diagnostic pop +#endif +#include "rosidl_runtime_c/visibility_control.h" +#include "unitree_go/msg/detail/sport_mode_state__struct.h" +#include "unitree_go/msg/detail/sport_mode_state__functions.h" + +#include "rosidl_runtime_c/primitives_sequence.h" +#include "rosidl_runtime_c/primitives_sequence_functions.h" + +bool unitree_go__msg__time_spec__convert_from_py(PyObject * _pymsg, void * _ros_message); +PyObject * unitree_go__msg__time_spec__convert_to_py(void * raw_ros_message); +bool unitree_go__msg__imu_state__convert_from_py(PyObject * _pymsg, void * _ros_message); +PyObject * unitree_go__msg__imu_state__convert_to_py(void * raw_ros_message); + +ROSIDL_GENERATOR_C_EXPORT +bool unitree_go__msg__sport_mode_state__convert_from_py(PyObject * _pymsg, void * _ros_message) +{ + // check that the passed message is of the expected Python class + { + char full_classname_dest[48]; + { + char * class_name = NULL; + char * module_name = NULL; + { + PyObject * class_attr = PyObject_GetAttrString(_pymsg, "__class__"); + if (class_attr) { + PyObject * name_attr = PyObject_GetAttrString(class_attr, "__name__"); + if (name_attr) { + class_name = (char *)PyUnicode_1BYTE_DATA(name_attr); + Py_DECREF(name_attr); + } + PyObject * module_attr = PyObject_GetAttrString(class_attr, "__module__"); + if (module_attr) { + module_name = (char *)PyUnicode_1BYTE_DATA(module_attr); + Py_DECREF(module_attr); + } + Py_DECREF(class_attr); + } + } + if (!class_name || !module_name) { + return false; + } + snprintf(full_classname_dest, sizeof(full_classname_dest), "%s.%s", module_name, class_name); + } + assert(strncmp("unitree_go.msg._sport_mode_state.SportModeState", full_classname_dest, 47) == 0); + } + unitree_go__msg__SportModeState * ros_message = _ros_message; + { // stamp + PyObject * field = PyObject_GetAttrString(_pymsg, "stamp"); + if (!field) { + return false; + } + if (!unitree_go__msg__time_spec__convert_from_py(field, &ros_message->stamp)) { + Py_DECREF(field); + return false; + } + Py_DECREF(field); + } + { // error_code + PyObject * field = PyObject_GetAttrString(_pymsg, "error_code"); + if (!field) { + return false; + } + assert(PyLong_Check(field)); + ros_message->error_code = PyLong_AsUnsignedLong(field); + Py_DECREF(field); + } + { // imu_state + PyObject * field = PyObject_GetAttrString(_pymsg, "imu_state"); + if (!field) { + return false; + } + if (!unitree_go__msg__imu_state__convert_from_py(field, &ros_message->imu_state)) { + Py_DECREF(field); + return false; + } + Py_DECREF(field); + } + { // mode + PyObject * field = PyObject_GetAttrString(_pymsg, "mode"); + if (!field) { + return false; + } + assert(PyLong_Check(field)); + ros_message->mode = (uint8_t)PyLong_AsUnsignedLong(field); + Py_DECREF(field); + } + { // progress + PyObject * field = PyObject_GetAttrString(_pymsg, "progress"); + if (!field) { + return false; + } + assert(PyFloat_Check(field)); + ros_message->progress = (float)PyFloat_AS_DOUBLE(field); + Py_DECREF(field); + } + { // gait_type + PyObject * field = PyObject_GetAttrString(_pymsg, "gait_type"); + if (!field) { + return false; + } + assert(PyLong_Check(field)); + ros_message->gait_type = (uint8_t)PyLong_AsUnsignedLong(field); + Py_DECREF(field); + } + { // foot_raise_height + PyObject * field = PyObject_GetAttrString(_pymsg, "foot_raise_height"); + if (!field) { + return false; + } + assert(PyFloat_Check(field)); + ros_message->foot_raise_height = (float)PyFloat_AS_DOUBLE(field); + Py_DECREF(field); + } + { // position + PyObject * field = PyObject_GetAttrString(_pymsg, "position"); + if (!field) { + return false; + } + { + // TODO(dirk-thomas) use a better way to check the type before casting + assert(field->ob_type != NULL); + assert(field->ob_type->tp_name != NULL); + assert(strcmp(field->ob_type->tp_name, "numpy.ndarray") == 0); + PyArrayObject * seq_field = (PyArrayObject *)field; + Py_INCREF(seq_field); + assert(PyArray_NDIM(seq_field) == 1); + assert(PyArray_TYPE(seq_field) == NPY_FLOAT32); + Py_ssize_t size = 3; + float * dest = ros_message->position; + for (Py_ssize_t i = 0; i < size; ++i) { + float tmp = *(npy_float32 *)PyArray_GETPTR1(seq_field, i); + memcpy(&dest[i], &tmp, sizeof(float)); + } + Py_DECREF(seq_field); + } + Py_DECREF(field); + } + { // body_height + PyObject * field = PyObject_GetAttrString(_pymsg, "body_height"); + if (!field) { + return false; + } + assert(PyFloat_Check(field)); + ros_message->body_height = (float)PyFloat_AS_DOUBLE(field); + Py_DECREF(field); + } + { // velocity + PyObject * field = PyObject_GetAttrString(_pymsg, "velocity"); + if (!field) { + return false; + } + { + // TODO(dirk-thomas) use a better way to check the type before casting + assert(field->ob_type != NULL); + assert(field->ob_type->tp_name != NULL); + assert(strcmp(field->ob_type->tp_name, "numpy.ndarray") == 0); + PyArrayObject * seq_field = (PyArrayObject *)field; + Py_INCREF(seq_field); + assert(PyArray_NDIM(seq_field) == 1); + assert(PyArray_TYPE(seq_field) == NPY_FLOAT32); + Py_ssize_t size = 3; + float * dest = ros_message->velocity; + for (Py_ssize_t i = 0; i < size; ++i) { + float tmp = *(npy_float32 *)PyArray_GETPTR1(seq_field, i); + memcpy(&dest[i], &tmp, sizeof(float)); + } + Py_DECREF(seq_field); + } + Py_DECREF(field); + } + { // yaw_speed + PyObject * field = PyObject_GetAttrString(_pymsg, "yaw_speed"); + if (!field) { + return false; + } + assert(PyFloat_Check(field)); + ros_message->yaw_speed = (float)PyFloat_AS_DOUBLE(field); + Py_DECREF(field); + } + { // range_obstacle + PyObject * field = PyObject_GetAttrString(_pymsg, "range_obstacle"); + if (!field) { + return false; + } + { + // TODO(dirk-thomas) use a better way to check the type before casting + assert(field->ob_type != NULL); + assert(field->ob_type->tp_name != NULL); + assert(strcmp(field->ob_type->tp_name, "numpy.ndarray") == 0); + PyArrayObject * seq_field = (PyArrayObject *)field; + Py_INCREF(seq_field); + assert(PyArray_NDIM(seq_field) == 1); + assert(PyArray_TYPE(seq_field) == NPY_FLOAT32); + Py_ssize_t size = 4; + float * dest = ros_message->range_obstacle; + for (Py_ssize_t i = 0; i < size; ++i) { + float tmp = *(npy_float32 *)PyArray_GETPTR1(seq_field, i); + memcpy(&dest[i], &tmp, sizeof(float)); + } + Py_DECREF(seq_field); + } + Py_DECREF(field); + } + { // foot_force + PyObject * field = PyObject_GetAttrString(_pymsg, "foot_force"); + if (!field) { + return false; + } + { + // TODO(dirk-thomas) use a better way to check the type before casting + assert(field->ob_type != NULL); + assert(field->ob_type->tp_name != NULL); + assert(strcmp(field->ob_type->tp_name, "numpy.ndarray") == 0); + PyArrayObject * seq_field = (PyArrayObject *)field; + Py_INCREF(seq_field); + assert(PyArray_NDIM(seq_field) == 1); + assert(PyArray_TYPE(seq_field) == NPY_INT16); + Py_ssize_t size = 4; + int16_t * dest = ros_message->foot_force; + for (Py_ssize_t i = 0; i < size; ++i) { + int16_t tmp = *(npy_int16 *)PyArray_GETPTR1(seq_field, i); + memcpy(&dest[i], &tmp, sizeof(int16_t)); + } + Py_DECREF(seq_field); + } + Py_DECREF(field); + } + { // foot_position_body + PyObject * field = PyObject_GetAttrString(_pymsg, "foot_position_body"); + if (!field) { + return false; + } + { + // TODO(dirk-thomas) use a better way to check the type before casting + assert(field->ob_type != NULL); + assert(field->ob_type->tp_name != NULL); + assert(strcmp(field->ob_type->tp_name, "numpy.ndarray") == 0); + PyArrayObject * seq_field = (PyArrayObject *)field; + Py_INCREF(seq_field); + assert(PyArray_NDIM(seq_field) == 1); + assert(PyArray_TYPE(seq_field) == NPY_FLOAT32); + Py_ssize_t size = 12; + float * dest = ros_message->foot_position_body; + for (Py_ssize_t i = 0; i < size; ++i) { + float tmp = *(npy_float32 *)PyArray_GETPTR1(seq_field, i); + memcpy(&dest[i], &tmp, sizeof(float)); + } + Py_DECREF(seq_field); + } + Py_DECREF(field); + } + { // foot_speed_body + PyObject * field = PyObject_GetAttrString(_pymsg, "foot_speed_body"); + if (!field) { + return false; + } + { + // TODO(dirk-thomas) use a better way to check the type before casting + assert(field->ob_type != NULL); + assert(field->ob_type->tp_name != NULL); + assert(strcmp(field->ob_type->tp_name, "numpy.ndarray") == 0); + PyArrayObject * seq_field = (PyArrayObject *)field; + Py_INCREF(seq_field); + assert(PyArray_NDIM(seq_field) == 1); + assert(PyArray_TYPE(seq_field) == NPY_FLOAT32); + Py_ssize_t size = 12; + float * dest = ros_message->foot_speed_body; + for (Py_ssize_t i = 0; i < size; ++i) { + float tmp = *(npy_float32 *)PyArray_GETPTR1(seq_field, i); + memcpy(&dest[i], &tmp, sizeof(float)); + } + Py_DECREF(seq_field); + } + Py_DECREF(field); + } + + return true; +} + +ROSIDL_GENERATOR_C_EXPORT +PyObject * unitree_go__msg__sport_mode_state__convert_to_py(void * raw_ros_message) +{ + /* NOTE(esteve): Call constructor of SportModeState */ + PyObject * _pymessage = NULL; + { + PyObject * pymessage_module = PyImport_ImportModule("unitree_go.msg._sport_mode_state"); + assert(pymessage_module); + PyObject * pymessage_class = PyObject_GetAttrString(pymessage_module, "SportModeState"); + assert(pymessage_class); + Py_DECREF(pymessage_module); + _pymessage = PyObject_CallObject(pymessage_class, NULL); + Py_DECREF(pymessage_class); + if (!_pymessage) { + return NULL; + } + } + unitree_go__msg__SportModeState * ros_message = (unitree_go__msg__SportModeState *)raw_ros_message; + { // stamp + PyObject * field = NULL; + field = unitree_go__msg__time_spec__convert_to_py(&ros_message->stamp); + if (!field) { + return NULL; + } + { + int rc = PyObject_SetAttrString(_pymessage, "stamp", field); + Py_DECREF(field); + if (rc) { + return NULL; + } + } + } + { // error_code + PyObject * field = NULL; + field = PyLong_FromUnsignedLong(ros_message->error_code); + { + int rc = PyObject_SetAttrString(_pymessage, "error_code", field); + Py_DECREF(field); + if (rc) { + return NULL; + } + } + } + { // imu_state + PyObject * field = NULL; + field = unitree_go__msg__imu_state__convert_to_py(&ros_message->imu_state); + if (!field) { + return NULL; + } + { + int rc = PyObject_SetAttrString(_pymessage, "imu_state", field); + Py_DECREF(field); + if (rc) { + return NULL; + } + } + } + { // mode + PyObject * field = NULL; + field = PyLong_FromUnsignedLong(ros_message->mode); + { + int rc = PyObject_SetAttrString(_pymessage, "mode", field); + Py_DECREF(field); + if (rc) { + return NULL; + } + } + } + { // progress + PyObject * field = NULL; + field = PyFloat_FromDouble(ros_message->progress); + { + int rc = PyObject_SetAttrString(_pymessage, "progress", field); + Py_DECREF(field); + if (rc) { + return NULL; + } + } + } + { // gait_type + PyObject * field = NULL; + field = PyLong_FromUnsignedLong(ros_message->gait_type); + { + int rc = PyObject_SetAttrString(_pymessage, "gait_type", field); + Py_DECREF(field); + if (rc) { + return NULL; + } + } + } + { // foot_raise_height + PyObject * field = NULL; + field = PyFloat_FromDouble(ros_message->foot_raise_height); + { + int rc = PyObject_SetAttrString(_pymessage, "foot_raise_height", field); + Py_DECREF(field); + if (rc) { + return NULL; + } + } + } + { // position + PyObject * field = NULL; + field = PyObject_GetAttrString(_pymessage, "position"); + if (!field) { + return NULL; + } + assert(field->ob_type != NULL); + assert(field->ob_type->tp_name != NULL); + assert(strcmp(field->ob_type->tp_name, "numpy.ndarray") == 0); + PyArrayObject * seq_field = (PyArrayObject *)field; + assert(PyArray_NDIM(seq_field) == 1); + assert(PyArray_TYPE(seq_field) == NPY_FLOAT32); + assert(sizeof(npy_float32) == sizeof(float)); + npy_float32 * dst = (npy_float32 *)PyArray_GETPTR1(seq_field, 0); + float * src = &(ros_message->position[0]); + memcpy(dst, src, 3 * sizeof(float)); + Py_DECREF(field); + } + { // body_height + PyObject * field = NULL; + field = PyFloat_FromDouble(ros_message->body_height); + { + int rc = PyObject_SetAttrString(_pymessage, "body_height", field); + Py_DECREF(field); + if (rc) { + return NULL; + } + } + } + { // velocity + PyObject * field = NULL; + field = PyObject_GetAttrString(_pymessage, "velocity"); + if (!field) { + return NULL; + } + assert(field->ob_type != NULL); + assert(field->ob_type->tp_name != NULL); + assert(strcmp(field->ob_type->tp_name, "numpy.ndarray") == 0); + PyArrayObject * seq_field = (PyArrayObject *)field; + assert(PyArray_NDIM(seq_field) == 1); + assert(PyArray_TYPE(seq_field) == NPY_FLOAT32); + assert(sizeof(npy_float32) == sizeof(float)); + npy_float32 * dst = (npy_float32 *)PyArray_GETPTR1(seq_field, 0); + float * src = &(ros_message->velocity[0]); + memcpy(dst, src, 3 * sizeof(float)); + Py_DECREF(field); + } + { // yaw_speed + PyObject * field = NULL; + field = PyFloat_FromDouble(ros_message->yaw_speed); + { + int rc = PyObject_SetAttrString(_pymessage, "yaw_speed", field); + Py_DECREF(field); + if (rc) { + return NULL; + } + } + } + { // range_obstacle + PyObject * field = NULL; + field = PyObject_GetAttrString(_pymessage, "range_obstacle"); + if (!field) { + return NULL; + } + assert(field->ob_type != NULL); + assert(field->ob_type->tp_name != NULL); + assert(strcmp(field->ob_type->tp_name, "numpy.ndarray") == 0); + PyArrayObject * seq_field = (PyArrayObject *)field; + assert(PyArray_NDIM(seq_field) == 1); + assert(PyArray_TYPE(seq_field) == NPY_FLOAT32); + assert(sizeof(npy_float32) == sizeof(float)); + npy_float32 * dst = (npy_float32 *)PyArray_GETPTR1(seq_field, 0); + float * src = &(ros_message->range_obstacle[0]); + memcpy(dst, src, 4 * sizeof(float)); + Py_DECREF(field); + } + { // foot_force + PyObject * field = NULL; + field = PyObject_GetAttrString(_pymessage, "foot_force"); + if (!field) { + return NULL; + } + assert(field->ob_type != NULL); + assert(field->ob_type->tp_name != NULL); + assert(strcmp(field->ob_type->tp_name, "numpy.ndarray") == 0); + PyArrayObject * seq_field = (PyArrayObject *)field; + assert(PyArray_NDIM(seq_field) == 1); + assert(PyArray_TYPE(seq_field) == NPY_INT16); + assert(sizeof(npy_int16) == sizeof(int16_t)); + npy_int16 * dst = (npy_int16 *)PyArray_GETPTR1(seq_field, 0); + int16_t * src = &(ros_message->foot_force[0]); + memcpy(dst, src, 4 * sizeof(int16_t)); + Py_DECREF(field); + } + { // foot_position_body + PyObject * field = NULL; + field = PyObject_GetAttrString(_pymessage, "foot_position_body"); + if (!field) { + return NULL; + } + assert(field->ob_type != NULL); + assert(field->ob_type->tp_name != NULL); + assert(strcmp(field->ob_type->tp_name, "numpy.ndarray") == 0); + PyArrayObject * seq_field = (PyArrayObject *)field; + assert(PyArray_NDIM(seq_field) == 1); + assert(PyArray_TYPE(seq_field) == NPY_FLOAT32); + assert(sizeof(npy_float32) == sizeof(float)); + npy_float32 * dst = (npy_float32 *)PyArray_GETPTR1(seq_field, 0); + float * src = &(ros_message->foot_position_body[0]); + memcpy(dst, src, 12 * sizeof(float)); + Py_DECREF(field); + } + { // foot_speed_body + PyObject * field = NULL; + field = PyObject_GetAttrString(_pymessage, "foot_speed_body"); + if (!field) { + return NULL; + } + assert(field->ob_type != NULL); + assert(field->ob_type->tp_name != NULL); + assert(strcmp(field->ob_type->tp_name, "numpy.ndarray") == 0); + PyArrayObject * seq_field = (PyArrayObject *)field; + assert(PyArray_NDIM(seq_field) == 1); + assert(PyArray_TYPE(seq_field) == NPY_FLOAT32); + assert(sizeof(npy_float32) == sizeof(float)); + npy_float32 * dst = (npy_float32 *)PyArray_GETPTR1(seq_field, 0); + float * src = &(ros_message->foot_speed_body[0]); + memcpy(dst, src, 12 * sizeof(float)); + Py_DECREF(field); + } + + // ownership of _pymessage is transferred to the caller + return _pymessage; +} diff --git a/python/unitree_go/msg/_time_spec.py b/python/unitree_go/msg/_time_spec.py new file mode 100644 index 00000000..2282825e --- /dev/null +++ b/python/unitree_go/msg/_time_spec.py @@ -0,0 +1,145 @@ +# generated from rosidl_generator_py/resource/_idl.py.em +# with input from unitree_go:msg/TimeSpec.idl +# generated code does not contain a copyright notice + + +# Import statements for member types + +import rosidl_parser.definition # noqa: E402, I100 + + +class Metaclass_TimeSpec(type): + """Metaclass of message 'TimeSpec'.""" + + _CREATE_ROS_MESSAGE = None + _CONVERT_FROM_PY = None + _CONVERT_TO_PY = None + _DESTROY_ROS_MESSAGE = None + _TYPE_SUPPORT = None + + __constants = { + } + + @classmethod + def __import_type_support__(cls): + try: + from rosidl_generator_py import import_type_support + module = import_type_support('unitree_go') + except ImportError: + import logging + import traceback + logger = logging.getLogger( + 'unitree_go.msg.TimeSpec') + logger.debug( + 'Failed to import needed modules for type support:\n' + + traceback.format_exc()) + else: + cls._CREATE_ROS_MESSAGE = module.create_ros_message_msg__msg__time_spec + cls._CONVERT_FROM_PY = module.convert_from_py_msg__msg__time_spec + cls._CONVERT_TO_PY = module.convert_to_py_msg__msg__time_spec + cls._TYPE_SUPPORT = module.type_support_msg__msg__time_spec + cls._DESTROY_ROS_MESSAGE = module.destroy_ros_message_msg__msg__time_spec + + @classmethod + def __prepare__(cls, name, bases, **kwargs): + # list constant names here so that they appear in the help text of + # the message class under "Data and other attributes defined here:" + # as well as populate each message instance + return { + } + + +class TimeSpec(metaclass=Metaclass_TimeSpec): + """Message class 'TimeSpec'.""" + + __slots__ = [ + '_sec', + '_nanosec', + ] + + _fields_and_field_types = { + 'sec': 'int32', + 'nanosec': 'uint32', + } + + SLOT_TYPES = ( + rosidl_parser.definition.BasicType('int32'), # noqa: E501 + rosidl_parser.definition.BasicType('uint32'), # noqa: E501 + ) + + def __init__(self, **kwargs): + assert all('_' + key in self.__slots__ for key in kwargs.keys()), \ + 'Invalid arguments passed to constructor: %s' % \ + ', '.join(sorted(k for k in kwargs.keys() if '_' + k not in self.__slots__)) + self.sec = kwargs.get('sec', int()) + self.nanosec = kwargs.get('nanosec', int()) + + def __repr__(self): + typename = self.__class__.__module__.split('.') + typename.pop() + typename.append(self.__class__.__name__) + args = [] + for s, t in zip(self.__slots__, self.SLOT_TYPES): + field = getattr(self, s) + fieldstr = repr(field) + # We use Python array type for fields that can be directly stored + # in them, and "normal" sequences for everything else. If it is + # a type that we store in an array, strip off the 'array' portion. + if ( + isinstance(t, rosidl_parser.definition.AbstractSequence) and + isinstance(t.value_type, rosidl_parser.definition.BasicType) and + t.value_type.typename in ['float', 'double', 'int8', 'uint8', 'int16', 'uint16', 'int32', 'uint32', 'int64', 'uint64'] + ): + if len(field) == 0: + fieldstr = '[]' + else: + assert fieldstr.startswith('array(') + prefix = "array('X', " + suffix = ')' + fieldstr = fieldstr[len(prefix):-len(suffix)] + args.append(s[1:] + '=' + fieldstr) + return '%s(%s)' % ('.'.join(typename), ', '.join(args)) + + def __eq__(self, other): + if not isinstance(other, self.__class__): + return False + if self.sec != other.sec: + return False + if self.nanosec != other.nanosec: + return False + return True + + @classmethod + def get_fields_and_field_types(cls): + from copy import copy + return copy(cls._fields_and_field_types) + + @property + def sec(self): + """Message field 'sec'.""" + return self._sec + + @sec.setter + def sec(self, value): + if __debug__: + assert \ + isinstance(value, int), \ + "The 'sec' field must be of type 'int'" + assert value >= -2147483648 and value < 2147483648, \ + "The 'sec' field must be an integer in [-2147483648, 2147483647]" + self._sec = value + + @property + def nanosec(self): + """Message field 'nanosec'.""" + return self._nanosec + + @nanosec.setter + def nanosec(self, value): + if __debug__: + assert \ + isinstance(value, int), \ + "The 'nanosec' field must be of type 'int'" + assert value >= 0 and value < 4294967296, \ + "The 'nanosec' field must be an unsigned integer in [0, 4294967295]" + self._nanosec = value diff --git a/python/unitree_go/msg/_time_spec_s.c b/python/unitree_go/msg/_time_spec_s.c new file mode 100644 index 00000000..a9643e1a --- /dev/null +++ b/python/unitree_go/msg/_time_spec_s.c @@ -0,0 +1,118 @@ +// generated from rosidl_generator_py/resource/_idl_support.c.em +// with input from unitree_go:msg/TimeSpec.idl +// generated code does not contain a copyright notice +#define NPY_NO_DEPRECATED_API NPY_1_7_API_VERSION +#include +#include +#ifndef _WIN32 +# pragma GCC diagnostic push +# pragma GCC diagnostic ignored "-Wunused-function" +#endif +#include "numpy/ndarrayobject.h" +#ifndef _WIN32 +# pragma GCC diagnostic pop +#endif +#include "rosidl_runtime_c/visibility_control.h" +#include "unitree_go/msg/detail/time_spec__struct.h" +#include "unitree_go/msg/detail/time_spec__functions.h" + + +ROSIDL_GENERATOR_C_EXPORT +bool unitree_go__msg__time_spec__convert_from_py(PyObject * _pymsg, void * _ros_message) +{ + // check that the passed message is of the expected Python class + { + char full_classname_dest[35]; + { + char * class_name = NULL; + char * module_name = NULL; + { + PyObject * class_attr = PyObject_GetAttrString(_pymsg, "__class__"); + if (class_attr) { + PyObject * name_attr = PyObject_GetAttrString(class_attr, "__name__"); + if (name_attr) { + class_name = (char *)PyUnicode_1BYTE_DATA(name_attr); + Py_DECREF(name_attr); + } + PyObject * module_attr = PyObject_GetAttrString(class_attr, "__module__"); + if (module_attr) { + module_name = (char *)PyUnicode_1BYTE_DATA(module_attr); + Py_DECREF(module_attr); + } + Py_DECREF(class_attr); + } + } + if (!class_name || !module_name) { + return false; + } + snprintf(full_classname_dest, sizeof(full_classname_dest), "%s.%s", module_name, class_name); + } + assert(strncmp("unitree_go.msg._time_spec.TimeSpec", full_classname_dest, 34) == 0); + } + unitree_go__msg__TimeSpec * ros_message = _ros_message; + { // sec + PyObject * field = PyObject_GetAttrString(_pymsg, "sec"); + if (!field) { + return false; + } + assert(PyLong_Check(field)); + ros_message->sec = (int32_t)PyLong_AsLong(field); + Py_DECREF(field); + } + { // nanosec + PyObject * field = PyObject_GetAttrString(_pymsg, "nanosec"); + if (!field) { + return false; + } + assert(PyLong_Check(field)); + ros_message->nanosec = PyLong_AsUnsignedLong(field); + Py_DECREF(field); + } + + return true; +} + +ROSIDL_GENERATOR_C_EXPORT +PyObject * unitree_go__msg__time_spec__convert_to_py(void * raw_ros_message) +{ + /* NOTE(esteve): Call constructor of TimeSpec */ + PyObject * _pymessage = NULL; + { + PyObject * pymessage_module = PyImport_ImportModule("unitree_go.msg._time_spec"); + assert(pymessage_module); + PyObject * pymessage_class = PyObject_GetAttrString(pymessage_module, "TimeSpec"); + assert(pymessage_class); + Py_DECREF(pymessage_module); + _pymessage = PyObject_CallObject(pymessage_class, NULL); + Py_DECREF(pymessage_class); + if (!_pymessage) { + return NULL; + } + } + unitree_go__msg__TimeSpec * ros_message = (unitree_go__msg__TimeSpec *)raw_ros_message; + { // sec + PyObject * field = NULL; + field = PyLong_FromLong(ros_message->sec); + { + int rc = PyObject_SetAttrString(_pymessage, "sec", field); + Py_DECREF(field); + if (rc) { + return NULL; + } + } + } + { // nanosec + PyObject * field = NULL; + field = PyLong_FromUnsignedLong(ros_message->nanosec); + { + int rc = PyObject_SetAttrString(_pymessage, "nanosec", field); + Py_DECREF(field); + if (rc) { + return NULL; + } + } + } + + // ownership of _pymessage is transferred to the caller + return _pymessage; +} diff --git a/python/unitree_go/msg/_uwb_state.py b/python/unitree_go/msg/_uwb_state.py new file mode 100644 index 00000000..5ed22b5a --- /dev/null +++ b/python/unitree_go/msg/_uwb_state.py @@ -0,0 +1,484 @@ +# generated from rosidl_generator_py/resource/_idl.py.em +# with input from unitree_go:msg/UwbState.idl +# generated code does not contain a copyright notice + + +# Import statements for member types + +# Member 'version' +# Member 'joystick' +import numpy # noqa: E402, I100 + +import rosidl_parser.definition # noqa: E402, I100 + + +class Metaclass_UwbState(type): + """Metaclass of message 'UwbState'.""" + + _CREATE_ROS_MESSAGE = None + _CONVERT_FROM_PY = None + _CONVERT_TO_PY = None + _DESTROY_ROS_MESSAGE = None + _TYPE_SUPPORT = None + + __constants = { + } + + @classmethod + def __import_type_support__(cls): + try: + from rosidl_generator_py import import_type_support + module = import_type_support('unitree_go') + except ImportError: + import logging + import traceback + logger = logging.getLogger( + 'unitree_go.msg.UwbState') + logger.debug( + 'Failed to import needed modules for type support:\n' + + traceback.format_exc()) + else: + cls._CREATE_ROS_MESSAGE = module.create_ros_message_msg__msg__uwb_state + cls._CONVERT_FROM_PY = module.convert_from_py_msg__msg__uwb_state + cls._CONVERT_TO_PY = module.convert_to_py_msg__msg__uwb_state + cls._TYPE_SUPPORT = module.type_support_msg__msg__uwb_state + cls._DESTROY_ROS_MESSAGE = module.destroy_ros_message_msg__msg__uwb_state + + @classmethod + def __prepare__(cls, name, bases, **kwargs): + # list constant names here so that they appear in the help text of + # the message class under "Data and other attributes defined here:" + # as well as populate each message instance + return { + } + + +class UwbState(metaclass=Metaclass_UwbState): + """Message class 'UwbState'.""" + + __slots__ = [ + '_version', + '_channel', + '_joy_mode', + '_orientation_est', + '_pitch_est', + '_distance_est', + '_yaw_est', + '_tag_roll', + '_tag_pitch', + '_tag_yaw', + '_base_roll', + '_base_pitch', + '_base_yaw', + '_joystick', + '_error_state', + '_buttons', + '_enabled_from_app', + ] + + _fields_and_field_types = { + 'version': 'uint8[2]', + 'channel': 'uint8', + 'joy_mode': 'uint8', + 'orientation_est': 'float', + 'pitch_est': 'float', + 'distance_est': 'float', + 'yaw_est': 'float', + 'tag_roll': 'float', + 'tag_pitch': 'float', + 'tag_yaw': 'float', + 'base_roll': 'float', + 'base_pitch': 'float', + 'base_yaw': 'float', + 'joystick': 'float[2]', + 'error_state': 'uint8', + 'buttons': 'uint8', + 'enabled_from_app': 'uint8', + } + + SLOT_TYPES = ( + rosidl_parser.definition.Array(rosidl_parser.definition.BasicType('uint8'), 2), # noqa: E501 + rosidl_parser.definition.BasicType('uint8'), # noqa: E501 + rosidl_parser.definition.BasicType('uint8'), # noqa: E501 + rosidl_parser.definition.BasicType('float'), # noqa: E501 + rosidl_parser.definition.BasicType('float'), # noqa: E501 + rosidl_parser.definition.BasicType('float'), # noqa: E501 + rosidl_parser.definition.BasicType('float'), # noqa: E501 + rosidl_parser.definition.BasicType('float'), # noqa: E501 + rosidl_parser.definition.BasicType('float'), # noqa: E501 + rosidl_parser.definition.BasicType('float'), # noqa: E501 + rosidl_parser.definition.BasicType('float'), # noqa: E501 + rosidl_parser.definition.BasicType('float'), # noqa: E501 + rosidl_parser.definition.BasicType('float'), # noqa: E501 + rosidl_parser.definition.Array(rosidl_parser.definition.BasicType('float'), 2), # noqa: E501 + rosidl_parser.definition.BasicType('uint8'), # noqa: E501 + rosidl_parser.definition.BasicType('uint8'), # noqa: E501 + rosidl_parser.definition.BasicType('uint8'), # noqa: E501 + ) + + def __init__(self, **kwargs): + assert all('_' + key in self.__slots__ for key in kwargs.keys()), \ + 'Invalid arguments passed to constructor: %s' % \ + ', '.join(sorted(k for k in kwargs.keys() if '_' + k not in self.__slots__)) + if 'version' not in kwargs: + self.version = numpy.zeros(2, dtype=numpy.uint8) + else: + self.version = numpy.array(kwargs.get('version'), dtype=numpy.uint8) + assert self.version.shape == (2, ) + self.channel = kwargs.get('channel', int()) + self.joy_mode = kwargs.get('joy_mode', int()) + self.orientation_est = kwargs.get('orientation_est', float()) + self.pitch_est = kwargs.get('pitch_est', float()) + self.distance_est = kwargs.get('distance_est', float()) + self.yaw_est = kwargs.get('yaw_est', float()) + self.tag_roll = kwargs.get('tag_roll', float()) + self.tag_pitch = kwargs.get('tag_pitch', float()) + self.tag_yaw = kwargs.get('tag_yaw', float()) + self.base_roll = kwargs.get('base_roll', float()) + self.base_pitch = kwargs.get('base_pitch', float()) + self.base_yaw = kwargs.get('base_yaw', float()) + if 'joystick' not in kwargs: + self.joystick = numpy.zeros(2, dtype=numpy.float32) + else: + self.joystick = numpy.array(kwargs.get('joystick'), dtype=numpy.float32) + assert self.joystick.shape == (2, ) + self.error_state = kwargs.get('error_state', int()) + self.buttons = kwargs.get('buttons', int()) + self.enabled_from_app = kwargs.get('enabled_from_app', int()) + + def __repr__(self): + typename = self.__class__.__module__.split('.') + typename.pop() + typename.append(self.__class__.__name__) + args = [] + for s, t in zip(self.__slots__, self.SLOT_TYPES): + field = getattr(self, s) + fieldstr = repr(field) + # We use Python array type for fields that can be directly stored + # in them, and "normal" sequences for everything else. If it is + # a type that we store in an array, strip off the 'array' portion. + if ( + isinstance(t, rosidl_parser.definition.AbstractSequence) and + isinstance(t.value_type, rosidl_parser.definition.BasicType) and + t.value_type.typename in ['float', 'double', 'int8', 'uint8', 'int16', 'uint16', 'int32', 'uint32', 'int64', 'uint64'] + ): + if len(field) == 0: + fieldstr = '[]' + else: + assert fieldstr.startswith('array(') + prefix = "array('X', " + suffix = ')' + fieldstr = fieldstr[len(prefix):-len(suffix)] + args.append(s[1:] + '=' + fieldstr) + return '%s(%s)' % ('.'.join(typename), ', '.join(args)) + + def __eq__(self, other): + if not isinstance(other, self.__class__): + return False + if all(self.version != other.version): + return False + if self.channel != other.channel: + return False + if self.joy_mode != other.joy_mode: + return False + if self.orientation_est != other.orientation_est: + return False + if self.pitch_est != other.pitch_est: + return False + if self.distance_est != other.distance_est: + return False + if self.yaw_est != other.yaw_est: + return False + if self.tag_roll != other.tag_roll: + return False + if self.tag_pitch != other.tag_pitch: + return False + if self.tag_yaw != other.tag_yaw: + return False + if self.base_roll != other.base_roll: + return False + if self.base_pitch != other.base_pitch: + return False + if self.base_yaw != other.base_yaw: + return False + if all(self.joystick != other.joystick): + return False + if self.error_state != other.error_state: + return False + if self.buttons != other.buttons: + return False + if self.enabled_from_app != other.enabled_from_app: + return False + return True + + @classmethod + def get_fields_and_field_types(cls): + from copy import copy + return copy(cls._fields_and_field_types) + + @property + def version(self): + """Message field 'version'.""" + return self._version + + @version.setter + def version(self, value): + if isinstance(value, numpy.ndarray): + assert value.dtype == numpy.uint8, \ + "The 'version' numpy.ndarray() must have the dtype of 'numpy.uint8'" + assert value.size == 2, \ + "The 'version' numpy.ndarray() must have a size of 2" + self._version = value + return + if __debug__: + from collections.abc import Sequence + from collections.abc import Set + from collections import UserList + from collections import UserString + assert \ + ((isinstance(value, Sequence) or + isinstance(value, Set) or + isinstance(value, UserList)) and + not isinstance(value, str) and + not isinstance(value, UserString) and + len(value) == 2 and + all(isinstance(v, int) for v in value) and + all(val >= 0 and val < 256 for val in value)), \ + "The 'version' field must be a set or sequence with length 2 and each value of type 'int' and each unsigned integer in [0, 255]" + self._version = numpy.array(value, dtype=numpy.uint8) + + @property + def channel(self): + """Message field 'channel'.""" + return self._channel + + @channel.setter + def channel(self, value): + if __debug__: + assert \ + isinstance(value, int), \ + "The 'channel' field must be of type 'int'" + assert value >= 0 and value < 256, \ + "The 'channel' field must be an unsigned integer in [0, 255]" + self._channel = value + + @property + def joy_mode(self): + """Message field 'joy_mode'.""" + return self._joy_mode + + @joy_mode.setter + def joy_mode(self, value): + if __debug__: + assert \ + isinstance(value, int), \ + "The 'joy_mode' field must be of type 'int'" + assert value >= 0 and value < 256, \ + "The 'joy_mode' field must be an unsigned integer in [0, 255]" + self._joy_mode = value + + @property + def orientation_est(self): + """Message field 'orientation_est'.""" + return self._orientation_est + + @orientation_est.setter + def orientation_est(self, value): + if __debug__: + assert \ + isinstance(value, float), \ + "The 'orientation_est' field must be of type 'float'" + self._orientation_est = value + + @property + def pitch_est(self): + """Message field 'pitch_est'.""" + return self._pitch_est + + @pitch_est.setter + def pitch_est(self, value): + if __debug__: + assert \ + isinstance(value, float), \ + "The 'pitch_est' field must be of type 'float'" + self._pitch_est = value + + @property + def distance_est(self): + """Message field 'distance_est'.""" + return self._distance_est + + @distance_est.setter + def distance_est(self, value): + if __debug__: + assert \ + isinstance(value, float), \ + "The 'distance_est' field must be of type 'float'" + self._distance_est = value + + @property + def yaw_est(self): + """Message field 'yaw_est'.""" + return self._yaw_est + + @yaw_est.setter + def yaw_est(self, value): + if __debug__: + assert \ + isinstance(value, float), \ + "The 'yaw_est' field must be of type 'float'" + self._yaw_est = value + + @property + def tag_roll(self): + """Message field 'tag_roll'.""" + return self._tag_roll + + @tag_roll.setter + def tag_roll(self, value): + if __debug__: + assert \ + isinstance(value, float), \ + "The 'tag_roll' field must be of type 'float'" + self._tag_roll = value + + @property + def tag_pitch(self): + """Message field 'tag_pitch'.""" + return self._tag_pitch + + @tag_pitch.setter + def tag_pitch(self, value): + if __debug__: + assert \ + isinstance(value, float), \ + "The 'tag_pitch' field must be of type 'float'" + self._tag_pitch = value + + @property + def tag_yaw(self): + """Message field 'tag_yaw'.""" + return self._tag_yaw + + @tag_yaw.setter + def tag_yaw(self, value): + if __debug__: + assert \ + isinstance(value, float), \ + "The 'tag_yaw' field must be of type 'float'" + self._tag_yaw = value + + @property + def base_roll(self): + """Message field 'base_roll'.""" + return self._base_roll + + @base_roll.setter + def base_roll(self, value): + if __debug__: + assert \ + isinstance(value, float), \ + "The 'base_roll' field must be of type 'float'" + self._base_roll = value + + @property + def base_pitch(self): + """Message field 'base_pitch'.""" + return self._base_pitch + + @base_pitch.setter + def base_pitch(self, value): + if __debug__: + assert \ + isinstance(value, float), \ + "The 'base_pitch' field must be of type 'float'" + self._base_pitch = value + + @property + def base_yaw(self): + """Message field 'base_yaw'.""" + return self._base_yaw + + @base_yaw.setter + def base_yaw(self, value): + if __debug__: + assert \ + isinstance(value, float), \ + "The 'base_yaw' field must be of type 'float'" + self._base_yaw = value + + @property + def joystick(self): + """Message field 'joystick'.""" + return self._joystick + + @joystick.setter + def joystick(self, value): + if isinstance(value, numpy.ndarray): + assert value.dtype == numpy.float32, \ + "The 'joystick' numpy.ndarray() must have the dtype of 'numpy.float32'" + assert value.size == 2, \ + "The 'joystick' numpy.ndarray() must have a size of 2" + self._joystick = value + return + if __debug__: + from collections.abc import Sequence + from collections.abc import Set + from collections import UserList + from collections import UserString + assert \ + ((isinstance(value, Sequence) or + isinstance(value, Set) or + isinstance(value, UserList)) and + not isinstance(value, str) and + not isinstance(value, UserString) and + len(value) == 2 and + all(isinstance(v, float) for v in value) and + True), \ + "The 'joystick' field must be a set or sequence with length 2 and each value of type 'float'" + self._joystick = numpy.array(value, dtype=numpy.float32) + + @property + def error_state(self): + """Message field 'error_state'.""" + return self._error_state + + @error_state.setter + def error_state(self, value): + if __debug__: + assert \ + isinstance(value, int), \ + "The 'error_state' field must be of type 'int'" + assert value >= 0 and value < 256, \ + "The 'error_state' field must be an unsigned integer in [0, 255]" + self._error_state = value + + @property + def buttons(self): + """Message field 'buttons'.""" + return self._buttons + + @buttons.setter + def buttons(self, value): + if __debug__: + assert \ + isinstance(value, int), \ + "The 'buttons' field must be of type 'int'" + assert value >= 0 and value < 256, \ + "The 'buttons' field must be an unsigned integer in [0, 255]" + self._buttons = value + + @property + def enabled_from_app(self): + """Message field 'enabled_from_app'.""" + return self._enabled_from_app + + @enabled_from_app.setter + def enabled_from_app(self, value): + if __debug__: + assert \ + isinstance(value, int), \ + "The 'enabled_from_app' field must be of type 'int'" + assert value >= 0 and value < 256, \ + "The 'enabled_from_app' field must be an unsigned integer in [0, 255]" + self._enabled_from_app = value diff --git a/python/unitree_go/msg/_uwb_state_s.c b/python/unitree_go/msg/_uwb_state_s.c new file mode 100644 index 00000000..c5511631 --- /dev/null +++ b/python/unitree_go/msg/_uwb_state_s.c @@ -0,0 +1,465 @@ +// generated from rosidl_generator_py/resource/_idl_support.c.em +// with input from unitree_go:msg/UwbState.idl +// generated code does not contain a copyright notice +#define NPY_NO_DEPRECATED_API NPY_1_7_API_VERSION +#include +#include +#ifndef _WIN32 +# pragma GCC diagnostic push +# pragma GCC diagnostic ignored "-Wunused-function" +#endif +#include "numpy/ndarrayobject.h" +#ifndef _WIN32 +# pragma GCC diagnostic pop +#endif +#include "rosidl_runtime_c/visibility_control.h" +#include "unitree_go/msg/detail/uwb_state__struct.h" +#include "unitree_go/msg/detail/uwb_state__functions.h" + +#include "rosidl_runtime_c/primitives_sequence.h" +#include "rosidl_runtime_c/primitives_sequence_functions.h" + + +ROSIDL_GENERATOR_C_EXPORT +bool unitree_go__msg__uwb_state__convert_from_py(PyObject * _pymsg, void * _ros_message) +{ + // check that the passed message is of the expected Python class + { + char full_classname_dest[35]; + { + char * class_name = NULL; + char * module_name = NULL; + { + PyObject * class_attr = PyObject_GetAttrString(_pymsg, "__class__"); + if (class_attr) { + PyObject * name_attr = PyObject_GetAttrString(class_attr, "__name__"); + if (name_attr) { + class_name = (char *)PyUnicode_1BYTE_DATA(name_attr); + Py_DECREF(name_attr); + } + PyObject * module_attr = PyObject_GetAttrString(class_attr, "__module__"); + if (module_attr) { + module_name = (char *)PyUnicode_1BYTE_DATA(module_attr); + Py_DECREF(module_attr); + } + Py_DECREF(class_attr); + } + } + if (!class_name || !module_name) { + return false; + } + snprintf(full_classname_dest, sizeof(full_classname_dest), "%s.%s", module_name, class_name); + } + assert(strncmp("unitree_go.msg._uwb_state.UwbState", full_classname_dest, 34) == 0); + } + unitree_go__msg__UwbState * ros_message = _ros_message; + { // version + PyObject * field = PyObject_GetAttrString(_pymsg, "version"); + if (!field) { + return false; + } + { + // TODO(dirk-thomas) use a better way to check the type before casting + assert(field->ob_type != NULL); + assert(field->ob_type->tp_name != NULL); + assert(strcmp(field->ob_type->tp_name, "numpy.ndarray") == 0); + PyArrayObject * seq_field = (PyArrayObject *)field; + Py_INCREF(seq_field); + assert(PyArray_NDIM(seq_field) == 1); + assert(PyArray_TYPE(seq_field) == NPY_UINT8); + Py_ssize_t size = 2; + uint8_t * dest = ros_message->version; + for (Py_ssize_t i = 0; i < size; ++i) { + uint8_t tmp = *(npy_uint8 *)PyArray_GETPTR1(seq_field, i); + memcpy(&dest[i], &tmp, sizeof(uint8_t)); + } + Py_DECREF(seq_field); + } + Py_DECREF(field); + } + { // channel + PyObject * field = PyObject_GetAttrString(_pymsg, "channel"); + if (!field) { + return false; + } + assert(PyLong_Check(field)); + ros_message->channel = (uint8_t)PyLong_AsUnsignedLong(field); + Py_DECREF(field); + } + { // joy_mode + PyObject * field = PyObject_GetAttrString(_pymsg, "joy_mode"); + if (!field) { + return false; + } + assert(PyLong_Check(field)); + ros_message->joy_mode = (uint8_t)PyLong_AsUnsignedLong(field); + Py_DECREF(field); + } + { // orientation_est + PyObject * field = PyObject_GetAttrString(_pymsg, "orientation_est"); + if (!field) { + return false; + } + assert(PyFloat_Check(field)); + ros_message->orientation_est = (float)PyFloat_AS_DOUBLE(field); + Py_DECREF(field); + } + { // pitch_est + PyObject * field = PyObject_GetAttrString(_pymsg, "pitch_est"); + if (!field) { + return false; + } + assert(PyFloat_Check(field)); + ros_message->pitch_est = (float)PyFloat_AS_DOUBLE(field); + Py_DECREF(field); + } + { // distance_est + PyObject * field = PyObject_GetAttrString(_pymsg, "distance_est"); + if (!field) { + return false; + } + assert(PyFloat_Check(field)); + ros_message->distance_est = (float)PyFloat_AS_DOUBLE(field); + Py_DECREF(field); + } + { // yaw_est + PyObject * field = PyObject_GetAttrString(_pymsg, "yaw_est"); + if (!field) { + return false; + } + assert(PyFloat_Check(field)); + ros_message->yaw_est = (float)PyFloat_AS_DOUBLE(field); + Py_DECREF(field); + } + { // tag_roll + PyObject * field = PyObject_GetAttrString(_pymsg, "tag_roll"); + if (!field) { + return false; + } + assert(PyFloat_Check(field)); + ros_message->tag_roll = (float)PyFloat_AS_DOUBLE(field); + Py_DECREF(field); + } + { // tag_pitch + PyObject * field = PyObject_GetAttrString(_pymsg, "tag_pitch"); + if (!field) { + return false; + } + assert(PyFloat_Check(field)); + ros_message->tag_pitch = (float)PyFloat_AS_DOUBLE(field); + Py_DECREF(field); + } + { // tag_yaw + PyObject * field = PyObject_GetAttrString(_pymsg, "tag_yaw"); + if (!field) { + return false; + } + assert(PyFloat_Check(field)); + ros_message->tag_yaw = (float)PyFloat_AS_DOUBLE(field); + Py_DECREF(field); + } + { // base_roll + PyObject * field = PyObject_GetAttrString(_pymsg, "base_roll"); + if (!field) { + return false; + } + assert(PyFloat_Check(field)); + ros_message->base_roll = (float)PyFloat_AS_DOUBLE(field); + Py_DECREF(field); + } + { // base_pitch + PyObject * field = PyObject_GetAttrString(_pymsg, "base_pitch"); + if (!field) { + return false; + } + assert(PyFloat_Check(field)); + ros_message->base_pitch = (float)PyFloat_AS_DOUBLE(field); + Py_DECREF(field); + } + { // base_yaw + PyObject * field = PyObject_GetAttrString(_pymsg, "base_yaw"); + if (!field) { + return false; + } + assert(PyFloat_Check(field)); + ros_message->base_yaw = (float)PyFloat_AS_DOUBLE(field); + Py_DECREF(field); + } + { // joystick + PyObject * field = PyObject_GetAttrString(_pymsg, "joystick"); + if (!field) { + return false; + } + { + // TODO(dirk-thomas) use a better way to check the type before casting + assert(field->ob_type != NULL); + assert(field->ob_type->tp_name != NULL); + assert(strcmp(field->ob_type->tp_name, "numpy.ndarray") == 0); + PyArrayObject * seq_field = (PyArrayObject *)field; + Py_INCREF(seq_field); + assert(PyArray_NDIM(seq_field) == 1); + assert(PyArray_TYPE(seq_field) == NPY_FLOAT32); + Py_ssize_t size = 2; + float * dest = ros_message->joystick; + for (Py_ssize_t i = 0; i < size; ++i) { + float tmp = *(npy_float32 *)PyArray_GETPTR1(seq_field, i); + memcpy(&dest[i], &tmp, sizeof(float)); + } + Py_DECREF(seq_field); + } + Py_DECREF(field); + } + { // error_state + PyObject * field = PyObject_GetAttrString(_pymsg, "error_state"); + if (!field) { + return false; + } + assert(PyLong_Check(field)); + ros_message->error_state = (uint8_t)PyLong_AsUnsignedLong(field); + Py_DECREF(field); + } + { // buttons + PyObject * field = PyObject_GetAttrString(_pymsg, "buttons"); + if (!field) { + return false; + } + assert(PyLong_Check(field)); + ros_message->buttons = (uint8_t)PyLong_AsUnsignedLong(field); + Py_DECREF(field); + } + { // enabled_from_app + PyObject * field = PyObject_GetAttrString(_pymsg, "enabled_from_app"); + if (!field) { + return false; + } + assert(PyLong_Check(field)); + ros_message->enabled_from_app = (uint8_t)PyLong_AsUnsignedLong(field); + Py_DECREF(field); + } + + return true; +} + +ROSIDL_GENERATOR_C_EXPORT +PyObject * unitree_go__msg__uwb_state__convert_to_py(void * raw_ros_message) +{ + /* NOTE(esteve): Call constructor of UwbState */ + PyObject * _pymessage = NULL; + { + PyObject * pymessage_module = PyImport_ImportModule("unitree_go.msg._uwb_state"); + assert(pymessage_module); + PyObject * pymessage_class = PyObject_GetAttrString(pymessage_module, "UwbState"); + assert(pymessage_class); + Py_DECREF(pymessage_module); + _pymessage = PyObject_CallObject(pymessage_class, NULL); + Py_DECREF(pymessage_class); + if (!_pymessage) { + return NULL; + } + } + unitree_go__msg__UwbState * ros_message = (unitree_go__msg__UwbState *)raw_ros_message; + { // version + PyObject * field = NULL; + field = PyObject_GetAttrString(_pymessage, "version"); + if (!field) { + return NULL; + } + assert(field->ob_type != NULL); + assert(field->ob_type->tp_name != NULL); + assert(strcmp(field->ob_type->tp_name, "numpy.ndarray") == 0); + PyArrayObject * seq_field = (PyArrayObject *)field; + assert(PyArray_NDIM(seq_field) == 1); + assert(PyArray_TYPE(seq_field) == NPY_UINT8); + assert(sizeof(npy_uint8) == sizeof(uint8_t)); + npy_uint8 * dst = (npy_uint8 *)PyArray_GETPTR1(seq_field, 0); + uint8_t * src = &(ros_message->version[0]); + memcpy(dst, src, 2 * sizeof(uint8_t)); + Py_DECREF(field); + } + { // channel + PyObject * field = NULL; + field = PyLong_FromUnsignedLong(ros_message->channel); + { + int rc = PyObject_SetAttrString(_pymessage, "channel", field); + Py_DECREF(field); + if (rc) { + return NULL; + } + } + } + { // joy_mode + PyObject * field = NULL; + field = PyLong_FromUnsignedLong(ros_message->joy_mode); + { + int rc = PyObject_SetAttrString(_pymessage, "joy_mode", field); + Py_DECREF(field); + if (rc) { + return NULL; + } + } + } + { // orientation_est + PyObject * field = NULL; + field = PyFloat_FromDouble(ros_message->orientation_est); + { + int rc = PyObject_SetAttrString(_pymessage, "orientation_est", field); + Py_DECREF(field); + if (rc) { + return NULL; + } + } + } + { // pitch_est + PyObject * field = NULL; + field = PyFloat_FromDouble(ros_message->pitch_est); + { + int rc = PyObject_SetAttrString(_pymessage, "pitch_est", field); + Py_DECREF(field); + if (rc) { + return NULL; + } + } + } + { // distance_est + PyObject * field = NULL; + field = PyFloat_FromDouble(ros_message->distance_est); + { + int rc = PyObject_SetAttrString(_pymessage, "distance_est", field); + Py_DECREF(field); + if (rc) { + return NULL; + } + } + } + { // yaw_est + PyObject * field = NULL; + field = PyFloat_FromDouble(ros_message->yaw_est); + { + int rc = PyObject_SetAttrString(_pymessage, "yaw_est", field); + Py_DECREF(field); + if (rc) { + return NULL; + } + } + } + { // tag_roll + PyObject * field = NULL; + field = PyFloat_FromDouble(ros_message->tag_roll); + { + int rc = PyObject_SetAttrString(_pymessage, "tag_roll", field); + Py_DECREF(field); + if (rc) { + return NULL; + } + } + } + { // tag_pitch + PyObject * field = NULL; + field = PyFloat_FromDouble(ros_message->tag_pitch); + { + int rc = PyObject_SetAttrString(_pymessage, "tag_pitch", field); + Py_DECREF(field); + if (rc) { + return NULL; + } + } + } + { // tag_yaw + PyObject * field = NULL; + field = PyFloat_FromDouble(ros_message->tag_yaw); + { + int rc = PyObject_SetAttrString(_pymessage, "tag_yaw", field); + Py_DECREF(field); + if (rc) { + return NULL; + } + } + } + { // base_roll + PyObject * field = NULL; + field = PyFloat_FromDouble(ros_message->base_roll); + { + int rc = PyObject_SetAttrString(_pymessage, "base_roll", field); + Py_DECREF(field); + if (rc) { + return NULL; + } + } + } + { // base_pitch + PyObject * field = NULL; + field = PyFloat_FromDouble(ros_message->base_pitch); + { + int rc = PyObject_SetAttrString(_pymessage, "base_pitch", field); + Py_DECREF(field); + if (rc) { + return NULL; + } + } + } + { // base_yaw + PyObject * field = NULL; + field = PyFloat_FromDouble(ros_message->base_yaw); + { + int rc = PyObject_SetAttrString(_pymessage, "base_yaw", field); + Py_DECREF(field); + if (rc) { + return NULL; + } + } + } + { // joystick + PyObject * field = NULL; + field = PyObject_GetAttrString(_pymessage, "joystick"); + if (!field) { + return NULL; + } + assert(field->ob_type != NULL); + assert(field->ob_type->tp_name != NULL); + assert(strcmp(field->ob_type->tp_name, "numpy.ndarray") == 0); + PyArrayObject * seq_field = (PyArrayObject *)field; + assert(PyArray_NDIM(seq_field) == 1); + assert(PyArray_TYPE(seq_field) == NPY_FLOAT32); + assert(sizeof(npy_float32) == sizeof(float)); + npy_float32 * dst = (npy_float32 *)PyArray_GETPTR1(seq_field, 0); + float * src = &(ros_message->joystick[0]); + memcpy(dst, src, 2 * sizeof(float)); + Py_DECREF(field); + } + { // error_state + PyObject * field = NULL; + field = PyLong_FromUnsignedLong(ros_message->error_state); + { + int rc = PyObject_SetAttrString(_pymessage, "error_state", field); + Py_DECREF(field); + if (rc) { + return NULL; + } + } + } + { // buttons + PyObject * field = NULL; + field = PyLong_FromUnsignedLong(ros_message->buttons); + { + int rc = PyObject_SetAttrString(_pymessage, "buttons", field); + Py_DECREF(field); + if (rc) { + return NULL; + } + } + } + { // enabled_from_app + PyObject * field = NULL; + field = PyLong_FromUnsignedLong(ros_message->enabled_from_app); + { + int rc = PyObject_SetAttrString(_pymessage, "enabled_from_app", field); + Py_DECREF(field); + if (rc) { + return NULL; + } + } + } + + // ownership of _pymessage is transferred to the caller + return _pymessage; +} diff --git a/python/unitree_go/msg/_uwb_switch.py b/python/unitree_go/msg/_uwb_switch.py new file mode 100644 index 00000000..5d28dcb6 --- /dev/null +++ b/python/unitree_go/msg/_uwb_switch.py @@ -0,0 +1,124 @@ +# generated from rosidl_generator_py/resource/_idl.py.em +# with input from unitree_go:msg/UwbSwitch.idl +# generated code does not contain a copyright notice + + +# Import statements for member types + +import rosidl_parser.definition # noqa: E402, I100 + + +class Metaclass_UwbSwitch(type): + """Metaclass of message 'UwbSwitch'.""" + + _CREATE_ROS_MESSAGE = None + _CONVERT_FROM_PY = None + _CONVERT_TO_PY = None + _DESTROY_ROS_MESSAGE = None + _TYPE_SUPPORT = None + + __constants = { + } + + @classmethod + def __import_type_support__(cls): + try: + from rosidl_generator_py import import_type_support + module = import_type_support('unitree_go') + except ImportError: + import logging + import traceback + logger = logging.getLogger( + 'unitree_go.msg.UwbSwitch') + logger.debug( + 'Failed to import needed modules for type support:\n' + + traceback.format_exc()) + else: + cls._CREATE_ROS_MESSAGE = module.create_ros_message_msg__msg__uwb_switch + cls._CONVERT_FROM_PY = module.convert_from_py_msg__msg__uwb_switch + cls._CONVERT_TO_PY = module.convert_to_py_msg__msg__uwb_switch + cls._TYPE_SUPPORT = module.type_support_msg__msg__uwb_switch + cls._DESTROY_ROS_MESSAGE = module.destroy_ros_message_msg__msg__uwb_switch + + @classmethod + def __prepare__(cls, name, bases, **kwargs): + # list constant names here so that they appear in the help text of + # the message class under "Data and other attributes defined here:" + # as well as populate each message instance + return { + } + + +class UwbSwitch(metaclass=Metaclass_UwbSwitch): + """Message class 'UwbSwitch'.""" + + __slots__ = [ + '_enabled', + ] + + _fields_and_field_types = { + 'enabled': 'uint8', + } + + SLOT_TYPES = ( + rosidl_parser.definition.BasicType('uint8'), # noqa: E501 + ) + + def __init__(self, **kwargs): + assert all('_' + key in self.__slots__ for key in kwargs.keys()), \ + 'Invalid arguments passed to constructor: %s' % \ + ', '.join(sorted(k for k in kwargs.keys() if '_' + k not in self.__slots__)) + self.enabled = kwargs.get('enabled', int()) + + def __repr__(self): + typename = self.__class__.__module__.split('.') + typename.pop() + typename.append(self.__class__.__name__) + args = [] + for s, t in zip(self.__slots__, self.SLOT_TYPES): + field = getattr(self, s) + fieldstr = repr(field) + # We use Python array type for fields that can be directly stored + # in them, and "normal" sequences for everything else. If it is + # a type that we store in an array, strip off the 'array' portion. + if ( + isinstance(t, rosidl_parser.definition.AbstractSequence) and + isinstance(t.value_type, rosidl_parser.definition.BasicType) and + t.value_type.typename in ['float', 'double', 'int8', 'uint8', 'int16', 'uint16', 'int32', 'uint32', 'int64', 'uint64'] + ): + if len(field) == 0: + fieldstr = '[]' + else: + assert fieldstr.startswith('array(') + prefix = "array('X', " + suffix = ')' + fieldstr = fieldstr[len(prefix):-len(suffix)] + args.append(s[1:] + '=' + fieldstr) + return '%s(%s)' % ('.'.join(typename), ', '.join(args)) + + def __eq__(self, other): + if not isinstance(other, self.__class__): + return False + if self.enabled != other.enabled: + return False + return True + + @classmethod + def get_fields_and_field_types(cls): + from copy import copy + return copy(cls._fields_and_field_types) + + @property + def enabled(self): + """Message field 'enabled'.""" + return self._enabled + + @enabled.setter + def enabled(self, value): + if __debug__: + assert \ + isinstance(value, int), \ + "The 'enabled' field must be of type 'int'" + assert value >= 0 and value < 256, \ + "The 'enabled' field must be an unsigned integer in [0, 255]" + self._enabled = value diff --git a/python/unitree_go/msg/_uwb_switch_s.c b/python/unitree_go/msg/_uwb_switch_s.c new file mode 100644 index 00000000..64fae02c --- /dev/null +++ b/python/unitree_go/msg/_uwb_switch_s.c @@ -0,0 +1,98 @@ +// generated from rosidl_generator_py/resource/_idl_support.c.em +// with input from unitree_go:msg/UwbSwitch.idl +// generated code does not contain a copyright notice +#define NPY_NO_DEPRECATED_API NPY_1_7_API_VERSION +#include +#include +#ifndef _WIN32 +# pragma GCC diagnostic push +# pragma GCC diagnostic ignored "-Wunused-function" +#endif +#include "numpy/ndarrayobject.h" +#ifndef _WIN32 +# pragma GCC diagnostic pop +#endif +#include "rosidl_runtime_c/visibility_control.h" +#include "unitree_go/msg/detail/uwb_switch__struct.h" +#include "unitree_go/msg/detail/uwb_switch__functions.h" + + +ROSIDL_GENERATOR_C_EXPORT +bool unitree_go__msg__uwb_switch__convert_from_py(PyObject * _pymsg, void * _ros_message) +{ + // check that the passed message is of the expected Python class + { + char full_classname_dest[37]; + { + char * class_name = NULL; + char * module_name = NULL; + { + PyObject * class_attr = PyObject_GetAttrString(_pymsg, "__class__"); + if (class_attr) { + PyObject * name_attr = PyObject_GetAttrString(class_attr, "__name__"); + if (name_attr) { + class_name = (char *)PyUnicode_1BYTE_DATA(name_attr); + Py_DECREF(name_attr); + } + PyObject * module_attr = PyObject_GetAttrString(class_attr, "__module__"); + if (module_attr) { + module_name = (char *)PyUnicode_1BYTE_DATA(module_attr); + Py_DECREF(module_attr); + } + Py_DECREF(class_attr); + } + } + if (!class_name || !module_name) { + return false; + } + snprintf(full_classname_dest, sizeof(full_classname_dest), "%s.%s", module_name, class_name); + } + assert(strncmp("unitree_go.msg._uwb_switch.UwbSwitch", full_classname_dest, 36) == 0); + } + unitree_go__msg__UwbSwitch * ros_message = _ros_message; + { // enabled + PyObject * field = PyObject_GetAttrString(_pymsg, "enabled"); + if (!field) { + return false; + } + assert(PyLong_Check(field)); + ros_message->enabled = (uint8_t)PyLong_AsUnsignedLong(field); + Py_DECREF(field); + } + + return true; +} + +ROSIDL_GENERATOR_C_EXPORT +PyObject * unitree_go__msg__uwb_switch__convert_to_py(void * raw_ros_message) +{ + /* NOTE(esteve): Call constructor of UwbSwitch */ + PyObject * _pymessage = NULL; + { + PyObject * pymessage_module = PyImport_ImportModule("unitree_go.msg._uwb_switch"); + assert(pymessage_module); + PyObject * pymessage_class = PyObject_GetAttrString(pymessage_module, "UwbSwitch"); + assert(pymessage_class); + Py_DECREF(pymessage_module); + _pymessage = PyObject_CallObject(pymessage_class, NULL); + Py_DECREF(pymessage_class); + if (!_pymessage) { + return NULL; + } + } + unitree_go__msg__UwbSwitch * ros_message = (unitree_go__msg__UwbSwitch *)raw_ros_message; + { // enabled + PyObject * field = NULL; + field = PyLong_FromUnsignedLong(ros_message->enabled); + { + int rc = PyObject_SetAttrString(_pymessage, "enabled", field); + Py_DECREF(field); + if (rc) { + return NULL; + } + } + } + + // ownership of _pymessage is transferred to the caller + return _pymessage; +} diff --git a/python/unitree_go/msg/_wireless_controller.py b/python/unitree_go/msg/_wireless_controller.py new file mode 100644 index 00000000..ceeb80a4 --- /dev/null +++ b/python/unitree_go/msg/_wireless_controller.py @@ -0,0 +1,200 @@ +# generated from rosidl_generator_py/resource/_idl.py.em +# with input from unitree_go:msg/WirelessController.idl +# generated code does not contain a copyright notice + + +# Import statements for member types + +import rosidl_parser.definition # noqa: E402, I100 + + +class Metaclass_WirelessController(type): + """Metaclass of message 'WirelessController'.""" + + _CREATE_ROS_MESSAGE = None + _CONVERT_FROM_PY = None + _CONVERT_TO_PY = None + _DESTROY_ROS_MESSAGE = None + _TYPE_SUPPORT = None + + __constants = { + } + + @classmethod + def __import_type_support__(cls): + try: + from rosidl_generator_py import import_type_support + module = import_type_support('unitree_go') + except ImportError: + import logging + import traceback + logger = logging.getLogger( + 'unitree_go.msg.WirelessController') + logger.debug( + 'Failed to import needed modules for type support:\n' + + traceback.format_exc()) + else: + cls._CREATE_ROS_MESSAGE = module.create_ros_message_msg__msg__wireless_controller + cls._CONVERT_FROM_PY = module.convert_from_py_msg__msg__wireless_controller + cls._CONVERT_TO_PY = module.convert_to_py_msg__msg__wireless_controller + cls._TYPE_SUPPORT = module.type_support_msg__msg__wireless_controller + cls._DESTROY_ROS_MESSAGE = module.destroy_ros_message_msg__msg__wireless_controller + + @classmethod + def __prepare__(cls, name, bases, **kwargs): + # list constant names here so that they appear in the help text of + # the message class under "Data and other attributes defined here:" + # as well as populate each message instance + return { + } + + +class WirelessController(metaclass=Metaclass_WirelessController): + """Message class 'WirelessController'.""" + + __slots__ = [ + '_lx', + '_ly', + '_rx', + '_ry', + '_keys', + ] + + _fields_and_field_types = { + 'lx': 'float', + 'ly': 'float', + 'rx': 'float', + 'ry': 'float', + 'keys': 'uint16', + } + + SLOT_TYPES = ( + rosidl_parser.definition.BasicType('float'), # noqa: E501 + rosidl_parser.definition.BasicType('float'), # noqa: E501 + rosidl_parser.definition.BasicType('float'), # noqa: E501 + rosidl_parser.definition.BasicType('float'), # noqa: E501 + rosidl_parser.definition.BasicType('uint16'), # noqa: E501 + ) + + def __init__(self, **kwargs): + assert all('_' + key in self.__slots__ for key in kwargs.keys()), \ + 'Invalid arguments passed to constructor: %s' % \ + ', '.join(sorted(k for k in kwargs.keys() if '_' + k not in self.__slots__)) + self.lx = kwargs.get('lx', float()) + self.ly = kwargs.get('ly', float()) + self.rx = kwargs.get('rx', float()) + self.ry = kwargs.get('ry', float()) + self.keys = kwargs.get('keys', int()) + + def __repr__(self): + typename = self.__class__.__module__.split('.') + typename.pop() + typename.append(self.__class__.__name__) + args = [] + for s, t in zip(self.__slots__, self.SLOT_TYPES): + field = getattr(self, s) + fieldstr = repr(field) + # We use Python array type for fields that can be directly stored + # in them, and "normal" sequences for everything else. If it is + # a type that we store in an array, strip off the 'array' portion. + if ( + isinstance(t, rosidl_parser.definition.AbstractSequence) and + isinstance(t.value_type, rosidl_parser.definition.BasicType) and + t.value_type.typename in ['float', 'double', 'int8', 'uint8', 'int16', 'uint16', 'int32', 'uint32', 'int64', 'uint64'] + ): + if len(field) == 0: + fieldstr = '[]' + else: + assert fieldstr.startswith('array(') + prefix = "array('X', " + suffix = ')' + fieldstr = fieldstr[len(prefix):-len(suffix)] + args.append(s[1:] + '=' + fieldstr) + return '%s(%s)' % ('.'.join(typename), ', '.join(args)) + + def __eq__(self, other): + if not isinstance(other, self.__class__): + return False + if self.lx != other.lx: + return False + if self.ly != other.ly: + return False + if self.rx != other.rx: + return False + if self.ry != other.ry: + return False + if self.keys != other.keys: + return False + return True + + @classmethod + def get_fields_and_field_types(cls): + from copy import copy + return copy(cls._fields_and_field_types) + + @property + def lx(self): + """Message field 'lx'.""" + return self._lx + + @lx.setter + def lx(self, value): + if __debug__: + assert \ + isinstance(value, float), \ + "The 'lx' field must be of type 'float'" + self._lx = value + + @property + def ly(self): + """Message field 'ly'.""" + return self._ly + + @ly.setter + def ly(self, value): + if __debug__: + assert \ + isinstance(value, float), \ + "The 'ly' field must be of type 'float'" + self._ly = value + + @property + def rx(self): + """Message field 'rx'.""" + return self._rx + + @rx.setter + def rx(self, value): + if __debug__: + assert \ + isinstance(value, float), \ + "The 'rx' field must be of type 'float'" + self._rx = value + + @property + def ry(self): + """Message field 'ry'.""" + return self._ry + + @ry.setter + def ry(self, value): + if __debug__: + assert \ + isinstance(value, float), \ + "The 'ry' field must be of type 'float'" + self._ry = value + + @property + def keys(self): + """Message field 'keys'.""" + return self._keys + + @keys.setter + def keys(self, value): + if __debug__: + assert \ + isinstance(value, int), \ + "The 'keys' field must be of type 'int'" + assert value >= 0 and value < 65536, \ + "The 'keys' field must be an unsigned integer in [0, 65535]" + self._keys = value diff --git a/python/unitree_go/msg/_wireless_controller_s.c b/python/unitree_go/msg/_wireless_controller_s.c new file mode 100644 index 00000000..f02defc4 --- /dev/null +++ b/python/unitree_go/msg/_wireless_controller_s.c @@ -0,0 +1,178 @@ +// generated from rosidl_generator_py/resource/_idl_support.c.em +// with input from unitree_go:msg/WirelessController.idl +// generated code does not contain a copyright notice +#define NPY_NO_DEPRECATED_API NPY_1_7_API_VERSION +#include +#include +#ifndef _WIN32 +# pragma GCC diagnostic push +# pragma GCC diagnostic ignored "-Wunused-function" +#endif +#include "numpy/ndarrayobject.h" +#ifndef _WIN32 +# pragma GCC diagnostic pop +#endif +#include "rosidl_runtime_c/visibility_control.h" +#include "unitree_go/msg/detail/wireless_controller__struct.h" +#include "unitree_go/msg/detail/wireless_controller__functions.h" + + +ROSIDL_GENERATOR_C_EXPORT +bool unitree_go__msg__wireless_controller__convert_from_py(PyObject * _pymsg, void * _ros_message) +{ + // check that the passed message is of the expected Python class + { + char full_classname_dest[55]; + { + char * class_name = NULL; + char * module_name = NULL; + { + PyObject * class_attr = PyObject_GetAttrString(_pymsg, "__class__"); + if (class_attr) { + PyObject * name_attr = PyObject_GetAttrString(class_attr, "__name__"); + if (name_attr) { + class_name = (char *)PyUnicode_1BYTE_DATA(name_attr); + Py_DECREF(name_attr); + } + PyObject * module_attr = PyObject_GetAttrString(class_attr, "__module__"); + if (module_attr) { + module_name = (char *)PyUnicode_1BYTE_DATA(module_attr); + Py_DECREF(module_attr); + } + Py_DECREF(class_attr); + } + } + if (!class_name || !module_name) { + return false; + } + snprintf(full_classname_dest, sizeof(full_classname_dest), "%s.%s", module_name, class_name); + } + assert(strncmp("unitree_go.msg._wireless_controller.WirelessController", full_classname_dest, 54) == 0); + } + unitree_go__msg__WirelessController * ros_message = _ros_message; + { // lx + PyObject * field = PyObject_GetAttrString(_pymsg, "lx"); + if (!field) { + return false; + } + assert(PyFloat_Check(field)); + ros_message->lx = (float)PyFloat_AS_DOUBLE(field); + Py_DECREF(field); + } + { // ly + PyObject * field = PyObject_GetAttrString(_pymsg, "ly"); + if (!field) { + return false; + } + assert(PyFloat_Check(field)); + ros_message->ly = (float)PyFloat_AS_DOUBLE(field); + Py_DECREF(field); + } + { // rx + PyObject * field = PyObject_GetAttrString(_pymsg, "rx"); + if (!field) { + return false; + } + assert(PyFloat_Check(field)); + ros_message->rx = (float)PyFloat_AS_DOUBLE(field); + Py_DECREF(field); + } + { // ry + PyObject * field = PyObject_GetAttrString(_pymsg, "ry"); + if (!field) { + return false; + } + assert(PyFloat_Check(field)); + ros_message->ry = (float)PyFloat_AS_DOUBLE(field); + Py_DECREF(field); + } + { // keys + PyObject * field = PyObject_GetAttrString(_pymsg, "keys"); + if (!field) { + return false; + } + assert(PyLong_Check(field)); + ros_message->keys = (uint16_t)PyLong_AsUnsignedLong(field); + Py_DECREF(field); + } + + return true; +} + +ROSIDL_GENERATOR_C_EXPORT +PyObject * unitree_go__msg__wireless_controller__convert_to_py(void * raw_ros_message) +{ + /* NOTE(esteve): Call constructor of WirelessController */ + PyObject * _pymessage = NULL; + { + PyObject * pymessage_module = PyImport_ImportModule("unitree_go.msg._wireless_controller"); + assert(pymessage_module); + PyObject * pymessage_class = PyObject_GetAttrString(pymessage_module, "WirelessController"); + assert(pymessage_class); + Py_DECREF(pymessage_module); + _pymessage = PyObject_CallObject(pymessage_class, NULL); + Py_DECREF(pymessage_class); + if (!_pymessage) { + return NULL; + } + } + unitree_go__msg__WirelessController * ros_message = (unitree_go__msg__WirelessController *)raw_ros_message; + { // lx + PyObject * field = NULL; + field = PyFloat_FromDouble(ros_message->lx); + { + int rc = PyObject_SetAttrString(_pymessage, "lx", field); + Py_DECREF(field); + if (rc) { + return NULL; + } + } + } + { // ly + PyObject * field = NULL; + field = PyFloat_FromDouble(ros_message->ly); + { + int rc = PyObject_SetAttrString(_pymessage, "ly", field); + Py_DECREF(field); + if (rc) { + return NULL; + } + } + } + { // rx + PyObject * field = NULL; + field = PyFloat_FromDouble(ros_message->rx); + { + int rc = PyObject_SetAttrString(_pymessage, "rx", field); + Py_DECREF(field); + if (rc) { + return NULL; + } + } + } + { // ry + PyObject * field = NULL; + field = PyFloat_FromDouble(ros_message->ry); + { + int rc = PyObject_SetAttrString(_pymessage, "ry", field); + Py_DECREF(field); + if (rc) { + return NULL; + } + } + } + { // keys + PyObject * field = NULL; + field = PyLong_FromUnsignedLong(ros_message->keys); + { + int rc = PyObject_SetAttrString(_pymessage, "keys", field); + Py_DECREF(field); + if (rc) { + return NULL; + } + } + } + + // ownership of _pymessage is transferred to the caller + return _pymessage; +}