diff --git a/README.md b/README.md index 90f2054..d843a1e 100644 --- a/README.md +++ b/README.md @@ -128,8 +128,10 @@ no build flag, no shim and no code change — the same code path they use agains - **Transport** is UDPv4 + shared memory by default. `./b run` gives the container `--network host --ipc host` precisely so both survive the docker boundary; set `K1_DDS_UDP_ONLY=1` (or `udp_only: true`) if only UDP works in your setup. -- **Topics** — sim publishes `rt/low_state` (IMU + serial/parallel motor state, 50 Hz), `rt/odometer_state`, - `rt/fall_down` (on change, ≥1 Hz keepalive) and `rt/battery_state` (constant SOC); sim subscribes to +- **Topics** — sim publishes `rt/low_state` (IMU + serial/parallel motor state, 50 Hz), `rt/odometer_state` + and `rt/odom` (the controller's odometry: ground truth, or drifting with the error model in + [`mujoco/config/odometry.yaml`](mujoco/config/odometry.yaml) enabled), `rt/head_pose`, `rt/fall_down` (on change, + ≥1 Hz keepalive) and `rt/battery_state` (constant SOC); sim subscribes to `rt/joint_ctrl` (`LowCmd`, PD-tracked at 1 kHz in CUSTOM mode). The RPC pair `rt/LocoApiTopicReq` / `rt/LocoApiTopicResp` serves `CHANGE_MODE`, `MOVE`, `ROTATE_HEAD`, `LIE_DOWN`, `GET_UP`, `GET_UP_WITH_MODE`, `VISUAL_KICK` and `GET_MODE`; unimplemented `api_id`s are accepted with a warning diff --git a/docs/K1_MUJOCO_SETUP.md b/docs/K1_MUJOCO_SETUP.md index f7e3ecf..fa1bae1 100644 --- a/docs/K1_MUJOCO_SETUP.md +++ b/docs/K1_MUJOCO_SETUP.md @@ -226,6 +226,7 @@ runs show whether the robot is actually moving. | `gains.yaml` | Per-joint PD stiffness/damping + ready pose (seeded from Booster's official K1 deploy config) | | `locomotion.yaml` | `module::Locomotion` — initial mode, prepare blend time, fall thresholds | | `dds.yaml` | `module::SdkBridge` — DDS domain, UDP-only fallback, battery SOC, unknown-RPC status | +| `odometry.yaml` | `module::SdkBridge` — odometry error model (scale, white noise, bias drift) on `rt/odometer_state` / `rt/odom`; off = ground truth | All are read at startup (`--config-dir` or `$K1SIM_CONFIG_DIR` to point elsewhere); `--model`/`--rtf` on the command line override the corresponding YAML value for one-off runs (this is what `K1_MODEL`/`K1_RTF` above diff --git a/mujoco/config/odometry.yaml b/mujoco/config/odometry.yaml new file mode 100644 index 0000000..4054633 --- /dev/null +++ b/mujoco/config/odometry.yaml @@ -0,0 +1,31 @@ +# Error model of the controller's odometry (module::SdkBridge OdometryModel), as published on +# rt/odometer_state and rt/odom. The sim's base state is ground truth; the real K1's odometry is +# the controller's estimate, which drifts. Per planar body axis (forward x, sideways y, yaw z): +# +# v_est = scale * v_true + bias + white noise, +# +# and the odometry pose is integrated from v_est. The white noise is a density, so the pose error +# grows the same whatever the publish rate: a density s gives a position (heading) random walk of +# s * sqrt(t). The bias is stationary (standard deviation bias_sigma) and wanders over +# bias_time_constant, so over a window of T seconds it drifts the pose steadily by about +# bias_sigma * T while T is short of the time constant, and like more random walk beyond it. +# +# The values below are untuned placeholders. Fit them from a real recording with motion capture +# (NUbots_K1 branch tumminello/k1_odom_record: record with the data/odometry_mocap role, then +# `./b nbs odometry_fit ` prints these values), then enable. + +enabled: false # false: publish the ground truth + +# Multiplicative error on the true body velocity [vx, vy, wz]; 1 is exact +scale: [1.0, 1.0, 1.0] + +# White noise density on the body velocity [m/sqrt(s), m/sqrt(s), rad/sqrt(s)] +velocity_noise_density: [0.02, 0.02, 0.02] + +# Standard deviation of the velocity bias [m/s, m/s, rad/s] +bias_sigma: [0.01, 0.01, 0.01] + +# Time over which the bias decorrelates [s] +bias_time_constant: [10.0, 10.0, 10.0] + +seed: 0 # random seed; 0 seeds randomly each run diff --git a/mujoco/idl/builtin_interfaces/msg/Time.idl b/mujoco/idl/builtin_interfaces/msg/Time.idl new file mode 100644 index 0000000..a09bb93 --- /dev/null +++ b/mujoco/idl/builtin_interfaces/msg/Time.idl @@ -0,0 +1,13 @@ +// builtin_interfaces::msg::dds_::Time_ — see PROTOCOL.md. +// Field layout verified against include/booster/idl/builtin_interfaces/Time.h. + +module builtin_interfaces { + module msg { + module dds_ { + struct Time_ { + long sec; + unsigned long nanosec; + }; + }; + }; +}; diff --git a/mujoco/idl/geometry_msgs/msg/PoseWithCovariance.idl b/mujoco/idl/geometry_msgs/msg/PoseWithCovariance.idl new file mode 100644 index 0000000..e4a949a --- /dev/null +++ b/mujoco/idl/geometry_msgs/msg/PoseWithCovariance.idl @@ -0,0 +1,15 @@ +// geometry_msgs::msg::dds_::PoseWithCovariance_ — see PROTOCOL.md. +// Field layout verified against include/booster/idl/geometry_msgs/PoseWithCovariance.h. + +#include "Pose.idl" + +module geometry_msgs { + module msg { + module dds_ { + struct PoseWithCovariance_ { + geometry_msgs::msg::dds_::Pose_ pose; + double covariance[36]; + }; + }; + }; +}; diff --git a/mujoco/idl/geometry_msgs/msg/Twist.idl b/mujoco/idl/geometry_msgs/msg/Twist.idl new file mode 100644 index 0000000..3c81ad9 --- /dev/null +++ b/mujoco/idl/geometry_msgs/msg/Twist.idl @@ -0,0 +1,15 @@ +// geometry_msgs::msg::dds_::Twist_ — see PROTOCOL.md. +// Field layout verified against include/booster/idl/geometry_msgs/Twist.h. + +#include "Vector3.idl" + +module geometry_msgs { + module msg { + module dds_ { + struct Twist_ { + geometry_msgs::msg::dds_::Vector3_ linear; + geometry_msgs::msg::dds_::Vector3_ angular; + }; + }; + }; +}; diff --git a/mujoco/idl/geometry_msgs/msg/TwistWithCovariance.idl b/mujoco/idl/geometry_msgs/msg/TwistWithCovariance.idl new file mode 100644 index 0000000..b2e92ee --- /dev/null +++ b/mujoco/idl/geometry_msgs/msg/TwistWithCovariance.idl @@ -0,0 +1,15 @@ +// geometry_msgs::msg::dds_::TwistWithCovariance_ — see PROTOCOL.md. +// Field layout verified against include/booster/idl/geometry_msgs/TwistWithCovariance.h. + +#include "Twist.idl" + +module geometry_msgs { + module msg { + module dds_ { + struct TwistWithCovariance_ { + geometry_msgs::msg::dds_::Twist_ twist; + double covariance[36]; + }; + }; + }; +}; diff --git a/mujoco/idl/geometry_msgs/msg/Vector3.idl b/mujoco/idl/geometry_msgs/msg/Vector3.idl new file mode 100644 index 0000000..7dcbb1c --- /dev/null +++ b/mujoco/idl/geometry_msgs/msg/Vector3.idl @@ -0,0 +1,14 @@ +// geometry_msgs::msg::dds_::Vector3_ — see PROTOCOL.md. +// Field layout verified against include/booster/idl/geometry_msgs/Vector3.h. + +module geometry_msgs { + module msg { + module dds_ { + struct Vector3_ { + double x; + double y; + double z; + }; + }; + }; +}; diff --git a/mujoco/idl/nav_msgs/msg/Odometry.idl b/mujoco/idl/nav_msgs/msg/Odometry.idl new file mode 100644 index 0000000..d7c6cde --- /dev/null +++ b/mujoco/idl/nav_msgs/msg/Odometry.idl @@ -0,0 +1,19 @@ +// nav_msgs::msg::dds_::Odometry_ — see PROTOCOL.md. +// Field layout verified against include/booster/idl/nav_msgs/Odometry.h. + +#include "Header.idl" +#include "PoseWithCovariance.idl" +#include "TwistWithCovariance.idl" + +module nav_msgs { + module msg { + module dds_ { + struct Odometry_ { + std_msgs::msg::dds_::Header_ header; + string child_frame_id; + geometry_msgs::msg::dds_::PoseWithCovariance_ pose; + geometry_msgs::msg::dds_::TwistWithCovariance_ twist; + }; + }; + }; +}; diff --git a/mujoco/idl/std_msgs/msg/Header.idl b/mujoco/idl/std_msgs/msg/Header.idl new file mode 100644 index 0000000..bfd9b2b --- /dev/null +++ b/mujoco/idl/std_msgs/msg/Header.idl @@ -0,0 +1,15 @@ +// std_msgs::msg::dds_::Header_ — see PROTOCOL.md. +// Field layout verified against include/booster/idl/std_msgs/Header.h. + +#include "Time.idl" + +module std_msgs { + module msg { + module dds_ { + struct Header_ { + builtin_interfaces::msg::dds_::Time_ stamp; + string frame_id; + }; + }; + }; +}; diff --git a/mujoco/idl_gen/builtin_interfaces/Time.cxx b/mujoco/idl_gen/builtin_interfaces/Time.cxx new file mode 100644 index 0000000..7766fbe --- /dev/null +++ b/mujoco/idl_gen/builtin_interfaces/Time.cxx @@ -0,0 +1,288 @@ +// Copyright 2016 Proyectos y Sistemas de Mantenimiento SL (eProsima). +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +/*! + * @file Time.cpp + * This source file contains the implementation of the described types in the IDL file. + * + * This file was generated by the tool fastddsgen. + */ + +#ifdef _WIN32 +// Remove linker warning LNK4221 on Visual Studio +namespace { +char dummy; +} // namespace +#endif // _WIN32 + +#include "Time.h" + +#include + + +#include +using namespace eprosima::fastcdr::exception; + +#include + +namespace helper { namespace internal { + +enum class Size { + UInt8, + UInt16, + UInt32, + UInt64, +}; + +constexpr Size get_size(int s) { + return (s <= 8 ) ? Size::UInt8: + (s <= 16) ? Size::UInt16: + (s <= 32) ? Size::UInt32: Size::UInt64; +} + +template +struct FindTypeH; + +template<> +struct FindTypeH { + using type = std::uint8_t; +}; + +template<> +struct FindTypeH { + using type = std::uint16_t; +}; + +template<> +struct FindTypeH { + using type = std::uint32_t; +}; + +template<> +struct FindTypeH { + using type = std::uint64_t; +}; +} + +template +struct FindType { + using type = typename internal::FindTypeH::type; +}; +} + +#define builtin_interfaces_msg_dds__Time__max_cdr_typesize 8ULL; + + +namespace builtin_interfaces { + +namespace msg { + +namespace dds_ { + + + +Time_::Time_() +{ + // long m_sec + m_sec = 0; + // unsigned long m_nanosec + m_nanosec = 0; + +} + +Time_::~Time_() +{ +} + +Time_::Time_( + const Time_& x) +{ + m_sec = x.m_sec; + + + m_nanosec = x.m_nanosec; + +} + +Time_::Time_( + Time_&& x) noexcept +{ + m_sec = x.m_sec; + + + m_nanosec = x.m_nanosec; + +} + +Time_& Time_::operator =( + const Time_& x) +{ + m_sec = x.m_sec; + + + m_nanosec = x.m_nanosec; + + return *this; +} + +Time_& Time_::operator =( + Time_&& x) noexcept +{ + m_sec = x.m_sec; + + + m_nanosec = x.m_nanosec; + + return *this; +} + +bool Time_::operator ==( + const Time_& x) const +{ + return (m_sec == x.m_sec && + m_nanosec == x.m_nanosec); +} + +bool Time_::operator !=( + const Time_& x) const +{ + return !(*this == x); +} + +size_t Time_::getMaxCdrSerializedSize( + size_t current_alignment) +{ + static_cast(current_alignment); + return builtin_interfaces_msg_dds__Time__max_cdr_typesize; +} + +size_t Time_::getCdrSerializedSize( + const Time_& data, + size_t current_alignment) +{ + (void)data; + size_t initial_alignment = current_alignment; + + current_alignment += 4 + eprosima::fastcdr::Cdr::alignment(current_alignment, 4); + + + current_alignment += 4 + eprosima::fastcdr::Cdr::alignment(current_alignment, 4); + + + return current_alignment - initial_alignment; +} + + +void Time_::serialize( + eprosima::fastcdr::Cdr& scdr) const +{ + scdr << m_sec; + + scdr << m_nanosec; + +} + +void Time_::deserialize( + eprosima::fastcdr::Cdr& dcdr) +{ + dcdr >> m_sec; + + + + dcdr >> m_nanosec; + + +} + + +bool Time_::isKeyDefined() +{ + return false; +} + +void Time_::serializeKey( + eprosima::fastcdr::Cdr& scdr) const +{ + (void) scdr; +} + +/*! + * @brief This function sets a value in member sec + * @param _sec New value for member sec + */ +void Time_::sec( + int32_t _sec) +{ + m_sec = _sec; +} + +/*! + * @brief This function returns the value of member sec + * @return Value of member sec + */ +int32_t Time_::sec() const +{ + return m_sec; +} + +/*! + * @brief This function returns a reference to member sec + * @return Reference to member sec + */ +int32_t& Time_::sec() +{ + return m_sec; +} + + +/*! + * @brief This function sets a value in member nanosec + * @param _nanosec New value for member nanosec + */ +void Time_::nanosec( + uint32_t _nanosec) +{ + m_nanosec = _nanosec; +} + +/*! + * @brief This function returns the value of member nanosec + * @return Value of member nanosec + */ +uint32_t Time_::nanosec() const +{ + return m_nanosec; +} + +/*! + * @brief This function returns a reference to member nanosec + * @return Reference to member nanosec + */ +uint32_t& Time_::nanosec() +{ + return m_nanosec; +} + + + + + +} // namespace dds_ + + +} // namespace msg + + +} // namespace builtin_interfaces + diff --git a/mujoco/idl_gen/builtin_interfaces/Time.h b/mujoco/idl_gen/builtin_interfaces/Time.h new file mode 100644 index 0000000..7296ac0 --- /dev/null +++ b/mujoco/idl_gen/builtin_interfaces/Time.h @@ -0,0 +1,237 @@ +// Copyright 2016 Proyectos y Sistemas de Mantenimiento SL (eProsima). +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +/*! + * @file Time.h + * This header file contains the declaration of the described types in the IDL file. + * + * This file was generated by the tool fastddsgen. + */ + +#ifndef _FAST_DDS_GENERATED_BUILTIN_INTERFACES_MSG_DDS__TIME_H_ +#define _FAST_DDS_GENERATED_BUILTIN_INTERFACES_MSG_DDS__TIME_H_ + + +#include + +#include +#include +#include +#include +#include +#include + +#if defined(_WIN32) +#if defined(EPROSIMA_USER_DLL_EXPORT) +#define eProsima_user_DllExport __declspec( dllexport ) +#else +#define eProsima_user_DllExport +#endif // EPROSIMA_USER_DLL_EXPORT +#else +#define eProsima_user_DllExport +#endif // _WIN32 + +#if defined(_WIN32) +#if defined(EPROSIMA_USER_DLL_EXPORT) +#if defined(TIME_SOURCE) +#define TIME_DllAPI __declspec( dllexport ) +#else +#define TIME_DllAPI __declspec( dllimport ) +#endif // TIME_SOURCE +#else +#define TIME_DllAPI +#endif // EPROSIMA_USER_DLL_EXPORT +#else +#define TIME_DllAPI +#endif // _WIN32 + +namespace eprosima { +namespace fastcdr { +class Cdr; +} // namespace fastcdr +} // namespace eprosima + + + +namespace builtin_interfaces { + namespace msg { + namespace dds_ { + + + /*! + * @brief This class represents the structure Time_ defined by the user in the IDL file. + * @ingroup Time + */ + class Time_ + { + public: + + /*! + * @brief Default constructor. + */ + eProsima_user_DllExport Time_(); + + /*! + * @brief Default destructor. + */ + eProsima_user_DllExport ~Time_(); + + /*! + * @brief Copy constructor. + * @param x Reference to the object builtin_interfaces::msg::dds_::Time_ that will be copied. + */ + eProsima_user_DllExport Time_( + const Time_& x); + + /*! + * @brief Move constructor. + * @param x Reference to the object builtin_interfaces::msg::dds_::Time_ that will be copied. + */ + eProsima_user_DllExport Time_( + Time_&& x) noexcept; + + /*! + * @brief Copy assignment. + * @param x Reference to the object builtin_interfaces::msg::dds_::Time_ that will be copied. + */ + eProsima_user_DllExport Time_& operator =( + const Time_& x); + + /*! + * @brief Move assignment. + * @param x Reference to the object builtin_interfaces::msg::dds_::Time_ that will be copied. + */ + eProsima_user_DllExport Time_& operator =( + Time_&& x) noexcept; + + /*! + * @brief Comparison operator. + * @param x builtin_interfaces::msg::dds_::Time_ object to compare. + */ + eProsima_user_DllExport bool operator ==( + const Time_& x) const; + + /*! + * @brief Comparison operator. + * @param x builtin_interfaces::msg::dds_::Time_ object to compare. + */ + eProsima_user_DllExport bool operator !=( + const Time_& x) const; + + /*! + * @brief This function sets a value in member sec + * @param _sec New value for member sec + */ + eProsima_user_DllExport void sec( + int32_t _sec); + + /*! + * @brief This function returns the value of member sec + * @return Value of member sec + */ + eProsima_user_DllExport int32_t sec() const; + + /*! + * @brief This function returns a reference to member sec + * @return Reference to member sec + */ + eProsima_user_DllExport int32_t& sec(); + + + /*! + * @brief This function sets a value in member nanosec + * @param _nanosec New value for member nanosec + */ + eProsima_user_DllExport void nanosec( + uint32_t _nanosec); + + /*! + * @brief This function returns the value of member nanosec + * @return Value of member nanosec + */ + eProsima_user_DllExport uint32_t nanosec() const; + + /*! + * @brief This function returns a reference to member nanosec + * @return Reference to member nanosec + */ + eProsima_user_DllExport uint32_t& nanosec(); + + + /*! + * @brief This function returns the maximum serialized size of an object + * depending on the buffer alignment. + * @param current_alignment Buffer alignment. + * @return Maximum serialized size. + */ + eProsima_user_DllExport static size_t getMaxCdrSerializedSize( + size_t current_alignment = 0); + + /*! + * @brief This function returns the serialized size of a data depending on the buffer alignment. + * @param data Data which is calculated its serialized size. + * @param current_alignment Buffer alignment. + * @return Serialized size. + */ + eProsima_user_DllExport static size_t getCdrSerializedSize( + const builtin_interfaces::msg::dds_::Time_& data, + size_t current_alignment = 0); + + + + /*! + * @brief This function serializes an object using CDR serialization. + * @param cdr CDR serialization object. + */ + eProsima_user_DllExport void serialize( + eprosima::fastcdr::Cdr& cdr) const; + + /*! + * @brief This function deserializes an object using CDR serialization. + * @param cdr CDR serialization object. + */ + eProsima_user_DllExport void deserialize( + eprosima::fastcdr::Cdr& cdr); + + + + + /*! + * @brief This function tells you if the Key has been defined for this type + */ + eProsima_user_DllExport static bool isKeyDefined(); + + /*! + * @brief This function serializes the key members of an object using CDR serialization. + * @param cdr CDR serialization object. + */ + eProsima_user_DllExport void serializeKey( + eprosima::fastcdr::Cdr& cdr) const; + + + private: + + int32_t m_sec; + uint32_t m_nanosec; + + }; + + } // namespace dds_ + } // namespace msg +} // namespace builtin_interfaces + +#endif // _FAST_DDS_GENERATED_BUILTIN_INTERFACES_MSG_DDS__TIME_H_ + + + diff --git a/mujoco/idl_gen/builtin_interfaces/TimeCdrAux.hpp b/mujoco/idl_gen/builtin_interfaces/TimeCdrAux.hpp new file mode 100644 index 0000000..2f50163 --- /dev/null +++ b/mujoco/idl_gen/builtin_interfaces/TimeCdrAux.hpp @@ -0,0 +1,51 @@ +// Copyright 2016 Proyectos y Sistemas de Mantenimiento SL (eProsima). +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +/*! + * @file TimeCdrAux.hpp + * This source file contains some definitions of CDR related functions. + * + * This file was generated by the tool fastddsgen. + */ + +#ifndef _FAST_DDS_GENERATED_BUILTIN_INTERFACES_MSG_DDS__TIMECDRAUX_HPP_ +#define _FAST_DDS_GENERATED_BUILTIN_INTERFACES_MSG_DDS__TIMECDRAUX_HPP_ + +#include "Time.h" + +constexpr uint32_t builtin_interfaces_msg_dds__Time__max_cdr_typesize {8UL}; +constexpr uint32_t builtin_interfaces_msg_dds__Time__max_key_cdr_typesize {0UL}; + + +namespace eprosima { +namespace fastcdr { + +class Cdr; +class CdrSizeCalculator; + + + +eProsima_user_DllExport void serialize_key( + eprosima::fastcdr::Cdr& scdr, + const builtin_interfaces::msg::dds_::Time_& data); + + + + + +} // namespace fastcdr +} // namespace eprosima + +#endif // _FAST_DDS_GENERATED_BUILTIN_INTERFACES_MSG_DDS__TIMECDRAUX_HPP_ + diff --git a/mujoco/idl_gen/builtin_interfaces/TimeCdrAux.ipp b/mujoco/idl_gen/builtin_interfaces/TimeCdrAux.ipp new file mode 100644 index 0000000..4b1d45b --- /dev/null +++ b/mujoco/idl_gen/builtin_interfaces/TimeCdrAux.ipp @@ -0,0 +1,139 @@ +// Copyright 2016 Proyectos y Sistemas de Mantenimiento SL (eProsima). +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +/*! + * @file TimeCdrAux.ipp + * This source file contains some declarations of CDR related functions. + * + * This file was generated by the tool fastddsgen. + */ + +#ifndef _FAST_DDS_GENERATED_BUILTIN_INTERFACES_MSG_DDS__TIMECDRAUX_IPP_ +#define _FAST_DDS_GENERATED_BUILTIN_INTERFACES_MSG_DDS__TIMECDRAUX_IPP_ + +#include "TimeCdrAux.hpp" + +#include +#include + + +#include +using namespace eprosima::fastcdr::exception; + +namespace eprosima { +namespace fastcdr { + + + +template<> +eProsima_user_DllExport size_t calculate_serialized_size( + eprosima::fastcdr::CdrSizeCalculator& calculator, + const builtin_interfaces::msg::dds_::Time_& data, + size_t& current_alignment) +{ + using namespace builtin_interfaces::msg::dds_; + + static_cast(data); + + eprosima::fastcdr::EncodingAlgorithmFlag previous_encoding = calculator.get_encoding(); + size_t calculated_size {calculator.begin_calculate_type_serialized_size( + eprosima::fastcdr::CdrVersion::XCDRv2 == calculator.get_cdr_version() ? + eprosima::fastcdr::EncodingAlgorithmFlag::PLAIN_CDR2 : + eprosima::fastcdr::EncodingAlgorithmFlag::PLAIN_CDR, + current_alignment)}; + + + calculated_size += calculator.calculate_member_serialized_size(eprosima::fastcdr::MemberId(0), + data.sec(), current_alignment); + + calculated_size += calculator.calculate_member_serialized_size(eprosima::fastcdr::MemberId(1), + data.nanosec(), current_alignment); + + + calculated_size += calculator.end_calculate_type_serialized_size(previous_encoding, current_alignment); + + return calculated_size; +} + +template<> +eProsima_user_DllExport void serialize( + eprosima::fastcdr::Cdr& scdr, + const builtin_interfaces::msg::dds_::Time_& data) +{ + using namespace builtin_interfaces::msg::dds_; + + eprosima::fastcdr::Cdr::state current_state(scdr); + scdr.begin_serialize_type(current_state, + eprosima::fastcdr::CdrVersion::XCDRv2 == scdr.get_cdr_version() ? + eprosima::fastcdr::EncodingAlgorithmFlag::PLAIN_CDR2 : + eprosima::fastcdr::EncodingAlgorithmFlag::PLAIN_CDR); + + scdr + << eprosima::fastcdr::MemberId(0) << data.sec() + << eprosima::fastcdr::MemberId(1) << data.nanosec() +; + scdr.end_serialize_type(current_state); +} + +template<> +eProsima_user_DllExport void deserialize( + eprosima::fastcdr::Cdr& cdr, + builtin_interfaces::msg::dds_::Time_& data) +{ + using namespace builtin_interfaces::msg::dds_; + + cdr.deserialize_type(eprosima::fastcdr::CdrVersion::XCDRv2 == cdr.get_cdr_version() ? + eprosima::fastcdr::EncodingAlgorithmFlag::PLAIN_CDR2 : + eprosima::fastcdr::EncodingAlgorithmFlag::PLAIN_CDR, + [&data](eprosima::fastcdr::Cdr& dcdr, const eprosima::fastcdr::MemberId& mid) -> bool + { + bool ret_value = true; + switch (mid.id) + { + case 0: + dcdr >> data.sec(); + break; + + case 1: + dcdr >> data.nanosec(); + break; + + default: + ret_value = false; + break; + } + return ret_value; + }); +} + +void serialize_key( + eprosima::fastcdr::Cdr& scdr, + const builtin_interfaces::msg::dds_::Time_& data) +{ + using namespace builtin_interfaces::msg::dds_; + + static_cast(scdr); + static_cast(data); +} + + + + + + +} // namespace fastcdr +} // namespace eprosima + +#endif // _FAST_DDS_GENERATED_BUILTIN_INTERFACES_MSG_DDS__TIMECDRAUX_IPP_ + diff --git a/mujoco/idl_gen/builtin_interfaces/TimePubSubTypes.cxx b/mujoco/idl_gen/builtin_interfaces/TimePubSubTypes.cxx new file mode 100644 index 0000000..e0de1ac --- /dev/null +++ b/mujoco/idl_gen/builtin_interfaces/TimePubSubTypes.cxx @@ -0,0 +1,233 @@ +// Copyright 2016 Proyectos y Sistemas de Mantenimiento SL (eProsima). +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +/*! + * @file TimePubSubTypes.cpp + * This header file contains the implementation of the serialization functions. + * + * This file was generated by the tool fastddsgen. + */ + + +#include + +#include "TimePubSubTypes.h" +#include "TimeCdrAux.hpp" + +using SerializedPayload_t = eprosima::fastrtps::rtps::SerializedPayload_t; +using InstanceHandle_t = eprosima::fastrtps::rtps::InstanceHandle_t; +using DataRepresentationId_t = eprosima::fastdds::dds::DataRepresentationId_t; + +namespace builtin_interfaces { + namespace msg { + namespace dds_ { + + + Time_PubSubType::Time_PubSubType() + { + setName("builtin_interfaces::msg::dds_::Time_"); + uint32_t type_size = + #if FASTCDR_VERSION_MAJOR == 1 + static_cast(Time_::getMaxCdrSerializedSize()); + #else + builtin_interfaces_msg_dds__Time__max_cdr_typesize; + #endif + type_size += static_cast(eprosima::fastcdr::Cdr::alignment(type_size, 4)); /* possible submessage alignment */ + m_typeSize = type_size + 4; /*encapsulation*/ + m_isGetKeyDefined = false; + uint32_t keyLength = builtin_interfaces_msg_dds__Time__max_key_cdr_typesize > 16 ? builtin_interfaces_msg_dds__Time__max_key_cdr_typesize : 16; + m_keyBuffer = reinterpret_cast(malloc(keyLength)); + memset(m_keyBuffer, 0, keyLength); + } + + Time_PubSubType::~Time_PubSubType() + { + if (m_keyBuffer != nullptr) + { + free(m_keyBuffer); + } + } + + bool Time_PubSubType::serialize( + void* data, + SerializedPayload_t* payload, + DataRepresentationId_t data_representation) + { + Time_* p_type = static_cast(data); + + // Object that manages the raw buffer. + eprosima::fastcdr::FastBuffer fastbuffer(reinterpret_cast(payload->data), payload->max_size); + // Object that serializes the data. + eprosima::fastcdr::Cdr ser(fastbuffer, eprosima::fastcdr::Cdr::DEFAULT_ENDIAN, + data_representation == DataRepresentationId_t::XCDR_DATA_REPRESENTATION ? + eprosima::fastcdr::CdrVersion::XCDRv1 : eprosima::fastcdr::CdrVersion::XCDRv2); + payload->encapsulation = ser.endianness() == eprosima::fastcdr::Cdr::BIG_ENDIANNESS ? CDR_BE : CDR_LE; + #if FASTCDR_VERSION_MAJOR > 1 + ser.set_encoding_flag( + data_representation == DataRepresentationId_t::XCDR_DATA_REPRESENTATION ? + eprosima::fastcdr::EncodingAlgorithmFlag::PLAIN_CDR : + eprosima::fastcdr::EncodingAlgorithmFlag::PLAIN_CDR2); + #endif // FASTCDR_VERSION_MAJOR > 1 + + try + { + // Serialize encapsulation + ser.serialize_encapsulation(); + // Serialize the object. + ser << *p_type; + } + catch (eprosima::fastcdr::exception::Exception& /*exception*/) + { + return false; + } + + // Get the serialized length + #if FASTCDR_VERSION_MAJOR == 1 + payload->length = static_cast(ser.getSerializedDataLength()); + #else + payload->length = static_cast(ser.get_serialized_data_length()); + #endif // FASTCDR_VERSION_MAJOR == 1 + return true; + } + + bool Time_PubSubType::deserialize( + SerializedPayload_t* payload, + void* data) + { + try + { + // Convert DATA to pointer of your type + Time_* p_type = static_cast(data); + + // Object that manages the raw buffer. + eprosima::fastcdr::FastBuffer fastbuffer(reinterpret_cast(payload->data), payload->length); + + // Object that deserializes the data. + eprosima::fastcdr::Cdr deser(fastbuffer, eprosima::fastcdr::Cdr::DEFAULT_ENDIAN + #if FASTCDR_VERSION_MAJOR == 1 + , eprosima::fastcdr::Cdr::CdrType::DDS_CDR + #endif // FASTCDR_VERSION_MAJOR == 1 + ); + + // Deserialize encapsulation. + deser.read_encapsulation(); + payload->encapsulation = deser.endianness() == eprosima::fastcdr::Cdr::BIG_ENDIANNESS ? CDR_BE : CDR_LE; + + // Deserialize the object. + deser >> *p_type; + } + catch (eprosima::fastcdr::exception::Exception& /*exception*/) + { + return false; + } + + return true; + } + + std::function Time_PubSubType::getSerializedSizeProvider( + void* data, + DataRepresentationId_t data_representation) + { + return [data, data_representation]() -> uint32_t + { + #if FASTCDR_VERSION_MAJOR == 1 + static_cast(data_representation); + return static_cast(type::getCdrSerializedSize(*static_cast(data))) + + 4u /*encapsulation*/; + #else + try + { + eprosima::fastcdr::CdrSizeCalculator calculator( + data_representation == DataRepresentationId_t::XCDR_DATA_REPRESENTATION ? + eprosima::fastcdr::CdrVersion::XCDRv1 :eprosima::fastcdr::CdrVersion::XCDRv2); + size_t current_alignment {0}; + return static_cast(calculator.calculate_serialized_size( + *static_cast(data), current_alignment)) + + 4u /*encapsulation*/; + } + catch (eprosima::fastcdr::exception::Exception& /*exception*/) + { + return 0; + } + #endif // FASTCDR_VERSION_MAJOR == 1 + }; + } + + void* Time_PubSubType::createData() + { + return reinterpret_cast(new Time_()); + } + + void Time_PubSubType::deleteData( + void* data) + { + delete(reinterpret_cast(data)); + } + + bool Time_PubSubType::getKey( + void* data, + InstanceHandle_t* handle, + bool force_md5) + { + if (!m_isGetKeyDefined) + { + return false; + } + + Time_* p_type = static_cast(data); + + // Object that manages the raw buffer. + eprosima::fastcdr::FastBuffer fastbuffer(reinterpret_cast(m_keyBuffer), + builtin_interfaces_msg_dds__Time__max_key_cdr_typesize); + + // Object that serializes the data. + eprosima::fastcdr::Cdr ser(fastbuffer, eprosima::fastcdr::Cdr::BIG_ENDIANNESS, eprosima::fastcdr::CdrVersion::XCDRv1); + #if FASTCDR_VERSION_MAJOR == 1 + p_type->serializeKey(ser); + #else + eprosima::fastcdr::serialize_key(ser, *p_type); + #endif // FASTCDR_VERSION_MAJOR == 1 + if (force_md5 || builtin_interfaces_msg_dds__Time__max_key_cdr_typesize > 16) + { + m_md5.init(); + #if FASTCDR_VERSION_MAJOR == 1 + m_md5.update(m_keyBuffer, static_cast(ser.getSerializedDataLength())); + #else + m_md5.update(m_keyBuffer, static_cast(ser.get_serialized_data_length())); + #endif // FASTCDR_VERSION_MAJOR == 1 + m_md5.finalize(); + for (uint8_t i = 0; i < 16; ++i) + { + handle->value[i] = m_md5.digest[i]; + } + } + else + { + for (uint8_t i = 0; i < 16; ++i) + { + handle->value[i] = m_keyBuffer[i]; + } + } + return true; + } + + + } //End of namespace dds_ + + + } //End of namespace msg + + +} //End of namespace builtin_interfaces + diff --git a/mujoco/idl_gen/builtin_interfaces/TimePubSubTypes.h b/mujoco/idl_gen/builtin_interfaces/TimePubSubTypes.h new file mode 100644 index 0000000..ae14992 --- /dev/null +++ b/mujoco/idl_gen/builtin_interfaces/TimePubSubTypes.h @@ -0,0 +1,196 @@ +// Copyright 2016 Proyectos y Sistemas de Mantenimiento SL (eProsima). +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +/*! + * @file TimePubSubTypes.h + * This header file contains the declaration of the serialization functions. + * + * This file was generated by the tool fastddsgen. + */ + + +#ifndef _FAST_DDS_GENERATED_BUILTIN_INTERFACES_MSG_DDS__TIME_PUBSUBTYPES_H_ +#define _FAST_DDS_GENERATED_BUILTIN_INTERFACES_MSG_DDS__TIME_PUBSUBTYPES_H_ + +#include +#include +#include +#include +#include + +#include "Time.h" + + +#if !defined(GEN_API_VER) || (GEN_API_VER != 2) +#error \ + Generated Time is not compatible with current installed Fast DDS. Please, regenerate it with fastddsgen. +#endif // GEN_API_VER + +namespace builtin_interfaces +{ + namespace msg + { + namespace dds_ + { + + + + #ifndef SWIG + namespace detail { + + template + struct Time__rob + { + friend constexpr typename Tag::type get( + Tag) + { + return M; + } + + }; + + struct Time__f + { + typedef uint32_t Time_::* type; + friend constexpr type get( + Time__f); + }; + + template struct Time__rob; + + template + inline size_t constexpr Time__offset_of() + { + return ((::size_t) &reinterpret_cast((((T*)0)->*get(Tag())))); + } + + } // namespace detail + #endif // ifndef SWIG + + + /*! + * @brief This class represents the TopicDataType of the type Time_ defined by the user in the IDL file. + * @ingroup Time + */ + class Time_PubSubType : public eprosima::fastdds::dds::TopicDataType + { + public: + + typedef Time_ type; + + eProsima_user_DllExport Time_PubSubType(); + + eProsima_user_DllExport ~Time_PubSubType() override; + + eProsima_user_DllExport bool serialize( + void* data, + eprosima::fastrtps::rtps::SerializedPayload_t* payload) override + { + return serialize(data, payload, eprosima::fastdds::dds::DEFAULT_DATA_REPRESENTATION); + } + + eProsima_user_DllExport bool serialize( + void* data, + eprosima::fastrtps::rtps::SerializedPayload_t* payload, + eprosima::fastdds::dds::DataRepresentationId_t data_representation) override; + + eProsima_user_DllExport bool deserialize( + eprosima::fastrtps::rtps::SerializedPayload_t* payload, + void* data) override; + + eProsima_user_DllExport std::function getSerializedSizeProvider( + void* data) override + { + return getSerializedSizeProvider(data, eprosima::fastdds::dds::DEFAULT_DATA_REPRESENTATION); + } + + eProsima_user_DllExport std::function getSerializedSizeProvider( + void* data, + eprosima::fastdds::dds::DataRepresentationId_t data_representation) override; + + eProsima_user_DllExport bool getKey( + void* data, + eprosima::fastrtps::rtps::InstanceHandle_t* ihandle, + bool force_md5 = false) override; + + eProsima_user_DllExport void* createData() override; + + eProsima_user_DllExport void deleteData( + void* data) override; + + #ifdef TOPIC_DATA_TYPE_API_HAS_IS_BOUNDED + eProsima_user_DllExport inline bool is_bounded() const override + { + return true; + } + + #endif // TOPIC_DATA_TYPE_API_HAS_IS_BOUNDED + + #ifdef TOPIC_DATA_TYPE_API_HAS_IS_PLAIN + eProsima_user_DllExport inline bool is_plain() const override + { + return is_plain_xcdrv1_impl(); + } + + eProsima_user_DllExport inline bool is_plain( + eprosima::fastdds::dds::DataRepresentationId_t data_representation) const override + { + if(data_representation == eprosima::fastdds::dds::DataRepresentationId_t::XCDR2_DATA_REPRESENTATION) + { + return is_plain_xcdrv2_impl(); + } + else + { + return is_plain_xcdrv1_impl(); + } + } + + #endif // TOPIC_DATA_TYPE_API_HAS_IS_PLAIN + + #ifdef TOPIC_DATA_TYPE_API_HAS_CONSTRUCT_SAMPLE + eProsima_user_DllExport inline bool construct_sample( + void* memory) const override + { + new (memory) Time_(); + return true; + } + + #endif // TOPIC_DATA_TYPE_API_HAS_CONSTRUCT_SAMPLE + + MD5 m_md5; + unsigned char* m_keyBuffer; + + private: + + static constexpr bool is_plain_xcdrv1_impl() + { + return 8ULL == + (detail::Time__offset_of() + + sizeof(uint32_t)); + } + + static constexpr bool is_plain_xcdrv2_impl() + { + return 8ULL == + (detail::Time__offset_of() + + sizeof(uint32_t)); + } + + }; + } + } +} + +#endif // _FAST_DDS_GENERATED_BUILTIN_INTERFACES_MSG_DDS__TIME_PUBSUBTYPES_H_ + diff --git a/mujoco/idl_gen/geometry_msgs/PoseWithCovariance.cxx b/mujoco/idl_gen/geometry_msgs/PoseWithCovariance.cxx new file mode 100644 index 0000000..267c230 --- /dev/null +++ b/mujoco/idl_gen/geometry_msgs/PoseWithCovariance.cxx @@ -0,0 +1,313 @@ +// Copyright 2016 Proyectos y Sistemas de Mantenimiento SL (eProsima). +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +/*! + * @file PoseWithCovariance.cpp + * This source file contains the implementation of the described types in the IDL file. + * + * This file was generated by the tool fastddsgen. + */ + +#ifdef _WIN32 +// Remove linker warning LNK4221 on Visual Studio +namespace { +char dummy; +} // namespace +#endif // _WIN32 + +#include "PoseWithCovariance.h" + +#include + + +#include +using namespace eprosima::fastcdr::exception; + +#include + +namespace helper { namespace internal { + +enum class Size { + UInt8, + UInt16, + UInt32, + UInt64, +}; + +constexpr Size get_size(int s) { + return (s <= 8 ) ? Size::UInt8: + (s <= 16) ? Size::UInt16: + (s <= 32) ? Size::UInt32: Size::UInt64; +} + +template +struct FindTypeH; + +template<> +struct FindTypeH { + using type = std::uint8_t; +}; + +template<> +struct FindTypeH { + using type = std::uint16_t; +}; + +template<> +struct FindTypeH { + using type = std::uint32_t; +}; + +template<> +struct FindTypeH { + using type = std::uint64_t; +}; +} + +template +struct FindType { + using type = typename internal::FindTypeH::type; +}; +} + +#define geometry_msgs_msg_dds__Quaternion__max_cdr_typesize 32ULL; +#define geometry_msgs_msg_dds__Point__max_cdr_typesize 24ULL; +#define geometry_msgs_msg_dds__Pose__max_cdr_typesize 56ULL; +#define geometry_msgs_msg_dds__PoseWithCovariance__max_cdr_typesize 344ULL; + + +namespace geometry_msgs { + +namespace msg { + +namespace dds_ { + + + +PoseWithCovariance_::PoseWithCovariance_() +{ + // geometry_msgs::msg::dds_::Pose_ m_pose + + // double m_covariance + memset(&m_covariance, 0, ((36)) * 8); + +} + +PoseWithCovariance_::~PoseWithCovariance_() +{ +} + +PoseWithCovariance_::PoseWithCovariance_( + const PoseWithCovariance_& x) +{ + m_pose = x.m_pose; + + + m_covariance = x.m_covariance; + +} + +PoseWithCovariance_::PoseWithCovariance_( + PoseWithCovariance_&& x) noexcept +{ + m_pose = std::move(x.m_pose); + + + m_covariance = std::move(x.m_covariance); + +} + +PoseWithCovariance_& PoseWithCovariance_::operator =( + const PoseWithCovariance_& x) +{ + m_pose = x.m_pose; + + + m_covariance = x.m_covariance; + + return *this; +} + +PoseWithCovariance_& PoseWithCovariance_::operator =( + PoseWithCovariance_&& x) noexcept +{ + m_pose = std::move(x.m_pose); + + + m_covariance = std::move(x.m_covariance); + + return *this; +} + +bool PoseWithCovariance_::operator ==( + const PoseWithCovariance_& x) const +{ + return (m_pose == x.m_pose && + m_covariance == x.m_covariance); +} + +bool PoseWithCovariance_::operator !=( + const PoseWithCovariance_& x) const +{ + return !(*this == x); +} + +size_t PoseWithCovariance_::getMaxCdrSerializedSize( + size_t current_alignment) +{ + static_cast(current_alignment); + return geometry_msgs_msg_dds__PoseWithCovariance__max_cdr_typesize; +} + +size_t PoseWithCovariance_::getCdrSerializedSize( + const PoseWithCovariance_& data, + size_t current_alignment) +{ + (void)data; + size_t initial_alignment = current_alignment; + + current_alignment += geometry_msgs::msg::dds_::Pose_::getCdrSerializedSize(data.pose(), current_alignment); + + + current_alignment += (((36)) * 8) + eprosima::fastcdr::Cdr::alignment(current_alignment, 8); + + + + return current_alignment - initial_alignment; +} + + +void PoseWithCovariance_::serialize( + eprosima::fastcdr::Cdr& scdr) const +{ + scdr << m_pose; + + scdr << m_covariance; + + +} + +void PoseWithCovariance_::deserialize( + eprosima::fastcdr::Cdr& dcdr) +{ + dcdr >> m_pose; + + + + dcdr >> m_covariance; + + +} + + +bool PoseWithCovariance_::isKeyDefined() +{ + return false; +} + +void PoseWithCovariance_::serializeKey( + eprosima::fastcdr::Cdr& scdr) const +{ + (void) scdr; +} + +/*! + * @brief This function copies the value in member pose + * @param _pose New value to be copied in member pose + */ +void PoseWithCovariance_::pose( + const geometry_msgs::msg::dds_::Pose_& _pose) +{ + m_pose = _pose; +} + +/*! + * @brief This function moves the value in member pose + * @param _pose New value to be moved in member pose + */ +void PoseWithCovariance_::pose( + geometry_msgs::msg::dds_::Pose_&& _pose) +{ + m_pose = std::move(_pose); +} + +/*! + * @brief This function returns a constant reference to member pose + * @return Constant reference to member pose + */ +const geometry_msgs::msg::dds_::Pose_& PoseWithCovariance_::pose() const +{ + return m_pose; +} + +/*! + * @brief This function returns a reference to member pose + * @return Reference to member pose + */ +geometry_msgs::msg::dds_::Pose_& PoseWithCovariance_::pose() +{ + return m_pose; +} + + +/*! + * @brief This function copies the value in member covariance + * @param _covariance New value to be copied in member covariance + */ +void PoseWithCovariance_::covariance( + const std::array& _covariance) +{ + m_covariance = _covariance; +} + +/*! + * @brief This function moves the value in member covariance + * @param _covariance New value to be moved in member covariance + */ +void PoseWithCovariance_::covariance( + std::array&& _covariance) +{ + m_covariance = std::move(_covariance); +} + +/*! + * @brief This function returns a constant reference to member covariance + * @return Constant reference to member covariance + */ +const std::array& PoseWithCovariance_::covariance() const +{ + return m_covariance; +} + +/*! + * @brief This function returns a reference to member covariance + * @return Reference to member covariance + */ +std::array& PoseWithCovariance_::covariance() +{ + return m_covariance; +} + + + + + +} // namespace dds_ + + +} // namespace msg + + +} // namespace geometry_msgs + diff --git a/mujoco/idl_gen/geometry_msgs/PoseWithCovariance.h b/mujoco/idl_gen/geometry_msgs/PoseWithCovariance.h new file mode 100644 index 0000000..eefb9fd --- /dev/null +++ b/mujoco/idl_gen/geometry_msgs/PoseWithCovariance.h @@ -0,0 +1,252 @@ +// Copyright 2016 Proyectos y Sistemas de Mantenimiento SL (eProsima). +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +/*! + * @file PoseWithCovariance.h + * This header file contains the declaration of the described types in the IDL file. + * + * This file was generated by the tool fastddsgen. + */ + +#ifndef _FAST_DDS_GENERATED_GEOMETRY_MSGS_MSG_DDS__POSEWITHCOVARIANCE_H_ +#define _FAST_DDS_GENERATED_GEOMETRY_MSGS_MSG_DDS__POSEWITHCOVARIANCE_H_ + +#include "Pose.h" + +#include + +#include +#include +#include +#include +#include +#include + +#if defined(_WIN32) +#if defined(EPROSIMA_USER_DLL_EXPORT) +#define eProsima_user_DllExport __declspec( dllexport ) +#else +#define eProsima_user_DllExport +#endif // EPROSIMA_USER_DLL_EXPORT +#else +#define eProsima_user_DllExport +#endif // _WIN32 + +#if defined(_WIN32) +#if defined(EPROSIMA_USER_DLL_EXPORT) +#if defined(POSEWITHCOVARIANCE_SOURCE) +#define POSEWITHCOVARIANCE_DllAPI __declspec( dllexport ) +#else +#define POSEWITHCOVARIANCE_DllAPI __declspec( dllimport ) +#endif // POSEWITHCOVARIANCE_SOURCE +#else +#define POSEWITHCOVARIANCE_DllAPI +#endif // EPROSIMA_USER_DLL_EXPORT +#else +#define POSEWITHCOVARIANCE_DllAPI +#endif // _WIN32 + +namespace eprosima { +namespace fastcdr { +class Cdr; +} // namespace fastcdr +} // namespace eprosima + + + +namespace geometry_msgs { + namespace msg { + namespace dds_ { + + + /*! + * @brief This class represents the structure PoseWithCovariance_ defined by the user in the IDL file. + * @ingroup PoseWithCovariance + */ + class PoseWithCovariance_ + { + public: + + /*! + * @brief Default constructor. + */ + eProsima_user_DllExport PoseWithCovariance_(); + + /*! + * @brief Default destructor. + */ + eProsima_user_DllExport ~PoseWithCovariance_(); + + /*! + * @brief Copy constructor. + * @param x Reference to the object geometry_msgs::msg::dds_::PoseWithCovariance_ that will be copied. + */ + eProsima_user_DllExport PoseWithCovariance_( + const PoseWithCovariance_& x); + + /*! + * @brief Move constructor. + * @param x Reference to the object geometry_msgs::msg::dds_::PoseWithCovariance_ that will be copied. + */ + eProsima_user_DllExport PoseWithCovariance_( + PoseWithCovariance_&& x) noexcept; + + /*! + * @brief Copy assignment. + * @param x Reference to the object geometry_msgs::msg::dds_::PoseWithCovariance_ that will be copied. + */ + eProsima_user_DllExport PoseWithCovariance_& operator =( + const PoseWithCovariance_& x); + + /*! + * @brief Move assignment. + * @param x Reference to the object geometry_msgs::msg::dds_::PoseWithCovariance_ that will be copied. + */ + eProsima_user_DllExport PoseWithCovariance_& operator =( + PoseWithCovariance_&& x) noexcept; + + /*! + * @brief Comparison operator. + * @param x geometry_msgs::msg::dds_::PoseWithCovariance_ object to compare. + */ + eProsima_user_DllExport bool operator ==( + const PoseWithCovariance_& x) const; + + /*! + * @brief Comparison operator. + * @param x geometry_msgs::msg::dds_::PoseWithCovariance_ object to compare. + */ + eProsima_user_DllExport bool operator !=( + const PoseWithCovariance_& x) const; + + /*! + * @brief This function copies the value in member pose + * @param _pose New value to be copied in member pose + */ + eProsima_user_DllExport void pose( + const geometry_msgs::msg::dds_::Pose_& _pose); + + /*! + * @brief This function moves the value in member pose + * @param _pose New value to be moved in member pose + */ + eProsima_user_DllExport void pose( + geometry_msgs::msg::dds_::Pose_&& _pose); + + /*! + * @brief This function returns a constant reference to member pose + * @return Constant reference to member pose + */ + eProsima_user_DllExport const geometry_msgs::msg::dds_::Pose_& pose() const; + + /*! + * @brief This function returns a reference to member pose + * @return Reference to member pose + */ + eProsima_user_DllExport geometry_msgs::msg::dds_::Pose_& pose(); + + + /*! + * @brief This function copies the value in member covariance + * @param _covariance New value to be copied in member covariance + */ + eProsima_user_DllExport void covariance( + const std::array& _covariance); + + /*! + * @brief This function moves the value in member covariance + * @param _covariance New value to be moved in member covariance + */ + eProsima_user_DllExport void covariance( + std::array&& _covariance); + + /*! + * @brief This function returns a constant reference to member covariance + * @return Constant reference to member covariance + */ + eProsima_user_DllExport const std::array& covariance() const; + + /*! + * @brief This function returns a reference to member covariance + * @return Reference to member covariance + */ + eProsima_user_DllExport std::array& covariance(); + + + /*! + * @brief This function returns the maximum serialized size of an object + * depending on the buffer alignment. + * @param current_alignment Buffer alignment. + * @return Maximum serialized size. + */ + eProsima_user_DllExport static size_t getMaxCdrSerializedSize( + size_t current_alignment = 0); + + /*! + * @brief This function returns the serialized size of a data depending on the buffer alignment. + * @param data Data which is calculated its serialized size. + * @param current_alignment Buffer alignment. + * @return Serialized size. + */ + eProsima_user_DllExport static size_t getCdrSerializedSize( + const geometry_msgs::msg::dds_::PoseWithCovariance_& data, + size_t current_alignment = 0); + + + + /*! + * @brief This function serializes an object using CDR serialization. + * @param cdr CDR serialization object. + */ + eProsima_user_DllExport void serialize( + eprosima::fastcdr::Cdr& cdr) const; + + /*! + * @brief This function deserializes an object using CDR serialization. + * @param cdr CDR serialization object. + */ + eProsima_user_DllExport void deserialize( + eprosima::fastcdr::Cdr& cdr); + + + + + /*! + * @brief This function tells you if the Key has been defined for this type + */ + eProsima_user_DllExport static bool isKeyDefined(); + + /*! + * @brief This function serializes the key members of an object using CDR serialization. + * @param cdr CDR serialization object. + */ + eProsima_user_DllExport void serializeKey( + eprosima::fastcdr::Cdr& cdr) const; + + + private: + + geometry_msgs::msg::dds_::Pose_ m_pose; + std::array m_covariance; + + }; + + } // namespace dds_ + } // namespace msg +} // namespace geometry_msgs + +#endif // _FAST_DDS_GENERATED_GEOMETRY_MSGS_MSG_DDS__POSEWITHCOVARIANCE_H_ + + + diff --git a/mujoco/idl_gen/geometry_msgs/PoseWithCovarianceCdrAux.hpp b/mujoco/idl_gen/geometry_msgs/PoseWithCovarianceCdrAux.hpp new file mode 100644 index 0000000..77dcabf --- /dev/null +++ b/mujoco/idl_gen/geometry_msgs/PoseWithCovarianceCdrAux.hpp @@ -0,0 +1,51 @@ +// Copyright 2016 Proyectos y Sistemas de Mantenimiento SL (eProsima). +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +/*! + * @file PoseWithCovarianceCdrAux.hpp + * This source file contains some definitions of CDR related functions. + * + * This file was generated by the tool fastddsgen. + */ + +#ifndef _FAST_DDS_GENERATED_GEOMETRY_MSGS_MSG_DDS__POSEWITHCOVARIANCECDRAUX_HPP_ +#define _FAST_DDS_GENERATED_GEOMETRY_MSGS_MSG_DDS__POSEWITHCOVARIANCECDRAUX_HPP_ + +#include "PoseWithCovariance.h" + +constexpr uint32_t geometry_msgs_msg_dds__PoseWithCovariance__max_cdr_typesize {344UL}; +constexpr uint32_t geometry_msgs_msg_dds__PoseWithCovariance__max_key_cdr_typesize {0UL}; + + +namespace eprosima { +namespace fastcdr { + +class Cdr; +class CdrSizeCalculator; + + + +eProsima_user_DllExport void serialize_key( + eprosima::fastcdr::Cdr& scdr, + const geometry_msgs::msg::dds_::PoseWithCovariance_& data); + + + + + +} // namespace fastcdr +} // namespace eprosima + +#endif // _FAST_DDS_GENERATED_GEOMETRY_MSGS_MSG_DDS__POSEWITHCOVARIANCECDRAUX_HPP_ + diff --git a/mujoco/idl_gen/geometry_msgs/PoseWithCovarianceCdrAux.ipp b/mujoco/idl_gen/geometry_msgs/PoseWithCovarianceCdrAux.ipp new file mode 100644 index 0000000..b843a05 --- /dev/null +++ b/mujoco/idl_gen/geometry_msgs/PoseWithCovarianceCdrAux.ipp @@ -0,0 +1,139 @@ +// Copyright 2016 Proyectos y Sistemas de Mantenimiento SL (eProsima). +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +/*! + * @file PoseWithCovarianceCdrAux.ipp + * This source file contains some declarations of CDR related functions. + * + * This file was generated by the tool fastddsgen. + */ + +#ifndef _FAST_DDS_GENERATED_GEOMETRY_MSGS_MSG_DDS__POSEWITHCOVARIANCECDRAUX_IPP_ +#define _FAST_DDS_GENERATED_GEOMETRY_MSGS_MSG_DDS__POSEWITHCOVARIANCECDRAUX_IPP_ + +#include "PoseWithCovarianceCdrAux.hpp" + +#include +#include + + +#include +using namespace eprosima::fastcdr::exception; + +namespace eprosima { +namespace fastcdr { + + + +template<> +eProsima_user_DllExport size_t calculate_serialized_size( + eprosima::fastcdr::CdrSizeCalculator& calculator, + const geometry_msgs::msg::dds_::PoseWithCovariance_& data, + size_t& current_alignment) +{ + using namespace geometry_msgs::msg::dds_; + + static_cast(data); + + eprosima::fastcdr::EncodingAlgorithmFlag previous_encoding = calculator.get_encoding(); + size_t calculated_size {calculator.begin_calculate_type_serialized_size( + eprosima::fastcdr::CdrVersion::XCDRv2 == calculator.get_cdr_version() ? + eprosima::fastcdr::EncodingAlgorithmFlag::PLAIN_CDR2 : + eprosima::fastcdr::EncodingAlgorithmFlag::PLAIN_CDR, + current_alignment)}; + + + calculated_size += calculator.calculate_member_serialized_size(eprosima::fastcdr::MemberId(0), + data.pose(), current_alignment); + + calculated_size += calculator.calculate_member_serialized_size(eprosima::fastcdr::MemberId(1), + data.covariance(), current_alignment); + + + calculated_size += calculator.end_calculate_type_serialized_size(previous_encoding, current_alignment); + + return calculated_size; +} + +template<> +eProsima_user_DllExport void serialize( + eprosima::fastcdr::Cdr& scdr, + const geometry_msgs::msg::dds_::PoseWithCovariance_& data) +{ + using namespace geometry_msgs::msg::dds_; + + eprosima::fastcdr::Cdr::state current_state(scdr); + scdr.begin_serialize_type(current_state, + eprosima::fastcdr::CdrVersion::XCDRv2 == scdr.get_cdr_version() ? + eprosima::fastcdr::EncodingAlgorithmFlag::PLAIN_CDR2 : + eprosima::fastcdr::EncodingAlgorithmFlag::PLAIN_CDR); + + scdr + << eprosima::fastcdr::MemberId(0) << data.pose() + << eprosima::fastcdr::MemberId(1) << data.covariance() +; + scdr.end_serialize_type(current_state); +} + +template<> +eProsima_user_DllExport void deserialize( + eprosima::fastcdr::Cdr& cdr, + geometry_msgs::msg::dds_::PoseWithCovariance_& data) +{ + using namespace geometry_msgs::msg::dds_; + + cdr.deserialize_type(eprosima::fastcdr::CdrVersion::XCDRv2 == cdr.get_cdr_version() ? + eprosima::fastcdr::EncodingAlgorithmFlag::PLAIN_CDR2 : + eprosima::fastcdr::EncodingAlgorithmFlag::PLAIN_CDR, + [&data](eprosima::fastcdr::Cdr& dcdr, const eprosima::fastcdr::MemberId& mid) -> bool + { + bool ret_value = true; + switch (mid.id) + { + case 0: + dcdr >> data.pose(); + break; + + case 1: + dcdr >> data.covariance(); + break; + + default: + ret_value = false; + break; + } + return ret_value; + }); +} + +void serialize_key( + eprosima::fastcdr::Cdr& scdr, + const geometry_msgs::msg::dds_::PoseWithCovariance_& data) +{ + using namespace geometry_msgs::msg::dds_; + + static_cast(scdr); + static_cast(data); +} + + + + + + +} // namespace fastcdr +} // namespace eprosima + +#endif // _FAST_DDS_GENERATED_GEOMETRY_MSGS_MSG_DDS__POSEWITHCOVARIANCECDRAUX_IPP_ + diff --git a/mujoco/idl_gen/geometry_msgs/PoseWithCovariancePubSubTypes.cxx b/mujoco/idl_gen/geometry_msgs/PoseWithCovariancePubSubTypes.cxx new file mode 100644 index 0000000..8fd289e --- /dev/null +++ b/mujoco/idl_gen/geometry_msgs/PoseWithCovariancePubSubTypes.cxx @@ -0,0 +1,233 @@ +// Copyright 2016 Proyectos y Sistemas de Mantenimiento SL (eProsima). +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +/*! + * @file PoseWithCovariancePubSubTypes.cpp + * This header file contains the implementation of the serialization functions. + * + * This file was generated by the tool fastddsgen. + */ + + +#include + +#include "PoseWithCovariancePubSubTypes.h" +#include "PoseWithCovarianceCdrAux.hpp" + +using SerializedPayload_t = eprosima::fastrtps::rtps::SerializedPayload_t; +using InstanceHandle_t = eprosima::fastrtps::rtps::InstanceHandle_t; +using DataRepresentationId_t = eprosima::fastdds::dds::DataRepresentationId_t; + +namespace geometry_msgs { + namespace msg { + namespace dds_ { + + + PoseWithCovariance_PubSubType::PoseWithCovariance_PubSubType() + { + setName("geometry_msgs::msg::dds_::PoseWithCovariance_"); + uint32_t type_size = + #if FASTCDR_VERSION_MAJOR == 1 + static_cast(PoseWithCovariance_::getMaxCdrSerializedSize()); + #else + geometry_msgs_msg_dds__PoseWithCovariance__max_cdr_typesize; + #endif + type_size += static_cast(eprosima::fastcdr::Cdr::alignment(type_size, 4)); /* possible submessage alignment */ + m_typeSize = type_size + 4; /*encapsulation*/ + m_isGetKeyDefined = false; + uint32_t keyLength = geometry_msgs_msg_dds__PoseWithCovariance__max_key_cdr_typesize > 16 ? geometry_msgs_msg_dds__PoseWithCovariance__max_key_cdr_typesize : 16; + m_keyBuffer = reinterpret_cast(malloc(keyLength)); + memset(m_keyBuffer, 0, keyLength); + } + + PoseWithCovariance_PubSubType::~PoseWithCovariance_PubSubType() + { + if (m_keyBuffer != nullptr) + { + free(m_keyBuffer); + } + } + + bool PoseWithCovariance_PubSubType::serialize( + void* data, + SerializedPayload_t* payload, + DataRepresentationId_t data_representation) + { + PoseWithCovariance_* p_type = static_cast(data); + + // Object that manages the raw buffer. + eprosima::fastcdr::FastBuffer fastbuffer(reinterpret_cast(payload->data), payload->max_size); + // Object that serializes the data. + eprosima::fastcdr::Cdr ser(fastbuffer, eprosima::fastcdr::Cdr::DEFAULT_ENDIAN, + data_representation == DataRepresentationId_t::XCDR_DATA_REPRESENTATION ? + eprosima::fastcdr::CdrVersion::XCDRv1 : eprosima::fastcdr::CdrVersion::XCDRv2); + payload->encapsulation = ser.endianness() == eprosima::fastcdr::Cdr::BIG_ENDIANNESS ? CDR_BE : CDR_LE; + #if FASTCDR_VERSION_MAJOR > 1 + ser.set_encoding_flag( + data_representation == DataRepresentationId_t::XCDR_DATA_REPRESENTATION ? + eprosima::fastcdr::EncodingAlgorithmFlag::PLAIN_CDR : + eprosima::fastcdr::EncodingAlgorithmFlag::PLAIN_CDR2); + #endif // FASTCDR_VERSION_MAJOR > 1 + + try + { + // Serialize encapsulation + ser.serialize_encapsulation(); + // Serialize the object. + ser << *p_type; + } + catch (eprosima::fastcdr::exception::Exception& /*exception*/) + { + return false; + } + + // Get the serialized length + #if FASTCDR_VERSION_MAJOR == 1 + payload->length = static_cast(ser.getSerializedDataLength()); + #else + payload->length = static_cast(ser.get_serialized_data_length()); + #endif // FASTCDR_VERSION_MAJOR == 1 + return true; + } + + bool PoseWithCovariance_PubSubType::deserialize( + SerializedPayload_t* payload, + void* data) + { + try + { + // Convert DATA to pointer of your type + PoseWithCovariance_* p_type = static_cast(data); + + // Object that manages the raw buffer. + eprosima::fastcdr::FastBuffer fastbuffer(reinterpret_cast(payload->data), payload->length); + + // Object that deserializes the data. + eprosima::fastcdr::Cdr deser(fastbuffer, eprosima::fastcdr::Cdr::DEFAULT_ENDIAN + #if FASTCDR_VERSION_MAJOR == 1 + , eprosima::fastcdr::Cdr::CdrType::DDS_CDR + #endif // FASTCDR_VERSION_MAJOR == 1 + ); + + // Deserialize encapsulation. + deser.read_encapsulation(); + payload->encapsulation = deser.endianness() == eprosima::fastcdr::Cdr::BIG_ENDIANNESS ? CDR_BE : CDR_LE; + + // Deserialize the object. + deser >> *p_type; + } + catch (eprosima::fastcdr::exception::Exception& /*exception*/) + { + return false; + } + + return true; + } + + std::function PoseWithCovariance_PubSubType::getSerializedSizeProvider( + void* data, + DataRepresentationId_t data_representation) + { + return [data, data_representation]() -> uint32_t + { + #if FASTCDR_VERSION_MAJOR == 1 + static_cast(data_representation); + return static_cast(type::getCdrSerializedSize(*static_cast(data))) + + 4u /*encapsulation*/; + #else + try + { + eprosima::fastcdr::CdrSizeCalculator calculator( + data_representation == DataRepresentationId_t::XCDR_DATA_REPRESENTATION ? + eprosima::fastcdr::CdrVersion::XCDRv1 :eprosima::fastcdr::CdrVersion::XCDRv2); + size_t current_alignment {0}; + return static_cast(calculator.calculate_serialized_size( + *static_cast(data), current_alignment)) + + 4u /*encapsulation*/; + } + catch (eprosima::fastcdr::exception::Exception& /*exception*/) + { + return 0; + } + #endif // FASTCDR_VERSION_MAJOR == 1 + }; + } + + void* PoseWithCovariance_PubSubType::createData() + { + return reinterpret_cast(new PoseWithCovariance_()); + } + + void PoseWithCovariance_PubSubType::deleteData( + void* data) + { + delete(reinterpret_cast(data)); + } + + bool PoseWithCovariance_PubSubType::getKey( + void* data, + InstanceHandle_t* handle, + bool force_md5) + { + if (!m_isGetKeyDefined) + { + return false; + } + + PoseWithCovariance_* p_type = static_cast(data); + + // Object that manages the raw buffer. + eprosima::fastcdr::FastBuffer fastbuffer(reinterpret_cast(m_keyBuffer), + geometry_msgs_msg_dds__PoseWithCovariance__max_key_cdr_typesize); + + // Object that serializes the data. + eprosima::fastcdr::Cdr ser(fastbuffer, eprosima::fastcdr::Cdr::BIG_ENDIANNESS, eprosima::fastcdr::CdrVersion::XCDRv1); + #if FASTCDR_VERSION_MAJOR == 1 + p_type->serializeKey(ser); + #else + eprosima::fastcdr::serialize_key(ser, *p_type); + #endif // FASTCDR_VERSION_MAJOR == 1 + if (force_md5 || geometry_msgs_msg_dds__PoseWithCovariance__max_key_cdr_typesize > 16) + { + m_md5.init(); + #if FASTCDR_VERSION_MAJOR == 1 + m_md5.update(m_keyBuffer, static_cast(ser.getSerializedDataLength())); + #else + m_md5.update(m_keyBuffer, static_cast(ser.get_serialized_data_length())); + #endif // FASTCDR_VERSION_MAJOR == 1 + m_md5.finalize(); + for (uint8_t i = 0; i < 16; ++i) + { + handle->value[i] = m_md5.digest[i]; + } + } + else + { + for (uint8_t i = 0; i < 16; ++i) + { + handle->value[i] = m_keyBuffer[i]; + } + } + return true; + } + + + } //End of namespace dds_ + + + } //End of namespace msg + + +} //End of namespace geometry_msgs + diff --git a/mujoco/idl_gen/geometry_msgs/PoseWithCovariancePubSubTypes.h b/mujoco/idl_gen/geometry_msgs/PoseWithCovariancePubSubTypes.h new file mode 100644 index 0000000..9dcd73c --- /dev/null +++ b/mujoco/idl_gen/geometry_msgs/PoseWithCovariancePubSubTypes.h @@ -0,0 +1,197 @@ +// Copyright 2016 Proyectos y Sistemas de Mantenimiento SL (eProsima). +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +/*! + * @file PoseWithCovariancePubSubTypes.h + * This header file contains the declaration of the serialization functions. + * + * This file was generated by the tool fastddsgen. + */ + + +#ifndef _FAST_DDS_GENERATED_GEOMETRY_MSGS_MSG_DDS__POSEWITHCOVARIANCE_PUBSUBTYPES_H_ +#define _FAST_DDS_GENERATED_GEOMETRY_MSGS_MSG_DDS__POSEWITHCOVARIANCE_PUBSUBTYPES_H_ + +#include +#include +#include +#include +#include + +#include "PoseWithCovariance.h" + +#include "PosePubSubTypes.h" + +#if !defined(GEN_API_VER) || (GEN_API_VER != 2) +#error \ + Generated PoseWithCovariance is not compatible with current installed Fast DDS. Please, regenerate it with fastddsgen. +#endif // GEN_API_VER + +namespace geometry_msgs +{ + namespace msg + { + namespace dds_ + { + + + + #ifndef SWIG + namespace detail { + + template + struct PoseWithCovariance__rob + { + friend constexpr typename Tag::type get( + Tag) + { + return M; + } + + }; + + struct PoseWithCovariance__f + { + typedef std::array PoseWithCovariance_::* type; + friend constexpr type get( + PoseWithCovariance__f); + }; + + template struct PoseWithCovariance__rob; + + template + inline size_t constexpr PoseWithCovariance__offset_of() + { + return ((::size_t) &reinterpret_cast((((T*)0)->*get(Tag())))); + } + + } // namespace detail + #endif // ifndef SWIG + + + /*! + * @brief This class represents the TopicDataType of the type PoseWithCovariance_ defined by the user in the IDL file. + * @ingroup PoseWithCovariance + */ + class PoseWithCovariance_PubSubType : public eprosima::fastdds::dds::TopicDataType + { + public: + + typedef PoseWithCovariance_ type; + + eProsima_user_DllExport PoseWithCovariance_PubSubType(); + + eProsima_user_DllExport ~PoseWithCovariance_PubSubType() override; + + eProsima_user_DllExport bool serialize( + void* data, + eprosima::fastrtps::rtps::SerializedPayload_t* payload) override + { + return serialize(data, payload, eprosima::fastdds::dds::DEFAULT_DATA_REPRESENTATION); + } + + eProsima_user_DllExport bool serialize( + void* data, + eprosima::fastrtps::rtps::SerializedPayload_t* payload, + eprosima::fastdds::dds::DataRepresentationId_t data_representation) override; + + eProsima_user_DllExport bool deserialize( + eprosima::fastrtps::rtps::SerializedPayload_t* payload, + void* data) override; + + eProsima_user_DllExport std::function getSerializedSizeProvider( + void* data) override + { + return getSerializedSizeProvider(data, eprosima::fastdds::dds::DEFAULT_DATA_REPRESENTATION); + } + + eProsima_user_DllExport std::function getSerializedSizeProvider( + void* data, + eprosima::fastdds::dds::DataRepresentationId_t data_representation) override; + + eProsima_user_DllExport bool getKey( + void* data, + eprosima::fastrtps::rtps::InstanceHandle_t* ihandle, + bool force_md5 = false) override; + + eProsima_user_DllExport void* createData() override; + + eProsima_user_DllExport void deleteData( + void* data) override; + + #ifdef TOPIC_DATA_TYPE_API_HAS_IS_BOUNDED + eProsima_user_DllExport inline bool is_bounded() const override + { + return true; + } + + #endif // TOPIC_DATA_TYPE_API_HAS_IS_BOUNDED + + #ifdef TOPIC_DATA_TYPE_API_HAS_IS_PLAIN + eProsima_user_DllExport inline bool is_plain() const override + { + return is_plain_xcdrv1_impl(); + } + + eProsima_user_DllExport inline bool is_plain( + eprosima::fastdds::dds::DataRepresentationId_t data_representation) const override + { + if(data_representation == eprosima::fastdds::dds::DataRepresentationId_t::XCDR2_DATA_REPRESENTATION) + { + return is_plain_xcdrv2_impl(); + } + else + { + return is_plain_xcdrv1_impl(); + } + } + + #endif // TOPIC_DATA_TYPE_API_HAS_IS_PLAIN + + #ifdef TOPIC_DATA_TYPE_API_HAS_CONSTRUCT_SAMPLE + eProsima_user_DllExport inline bool construct_sample( + void* memory) const override + { + new (memory) PoseWithCovariance_(); + return true; + } + + #endif // TOPIC_DATA_TYPE_API_HAS_CONSTRUCT_SAMPLE + + MD5 m_md5; + unsigned char* m_keyBuffer; + + private: + + static constexpr bool is_plain_xcdrv1_impl() + { + return 344ULL == + (detail::PoseWithCovariance__offset_of() + + sizeof(std::array)); + } + + static constexpr bool is_plain_xcdrv2_impl() + { + return 344ULL == + (detail::PoseWithCovariance__offset_of() + + sizeof(std::array)); + } + + }; + } + } +} + +#endif // _FAST_DDS_GENERATED_GEOMETRY_MSGS_MSG_DDS__POSEWITHCOVARIANCE_PUBSUBTYPES_H_ + diff --git a/mujoco/idl_gen/geometry_msgs/Twist.cxx b/mujoco/idl_gen/geometry_msgs/Twist.cxx new file mode 100644 index 0000000..d6da739 --- /dev/null +++ b/mujoco/idl_gen/geometry_msgs/Twist.cxx @@ -0,0 +1,309 @@ +// Copyright 2016 Proyectos y Sistemas de Mantenimiento SL (eProsima). +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +/*! + * @file Twist.cpp + * This source file contains the implementation of the described types in the IDL file. + * + * This file was generated by the tool fastddsgen. + */ + +#ifdef _WIN32 +// Remove linker warning LNK4221 on Visual Studio +namespace { +char dummy; +} // namespace +#endif // _WIN32 + +#include "Twist.h" + +#include + + +#include +using namespace eprosima::fastcdr::exception; + +#include + +namespace helper { namespace internal { + +enum class Size { + UInt8, + UInt16, + UInt32, + UInt64, +}; + +constexpr Size get_size(int s) { + return (s <= 8 ) ? Size::UInt8: + (s <= 16) ? Size::UInt16: + (s <= 32) ? Size::UInt32: Size::UInt64; +} + +template +struct FindTypeH; + +template<> +struct FindTypeH { + using type = std::uint8_t; +}; + +template<> +struct FindTypeH { + using type = std::uint16_t; +}; + +template<> +struct FindTypeH { + using type = std::uint32_t; +}; + +template<> +struct FindTypeH { + using type = std::uint64_t; +}; +} + +template +struct FindType { + using type = typename internal::FindTypeH::type; +}; +} + +#define geometry_msgs_msg_dds__Vector3__max_cdr_typesize 24ULL; +#define geometry_msgs_msg_dds__Twist__max_cdr_typesize 48ULL; + + +namespace geometry_msgs { + +namespace msg { + +namespace dds_ { + + + +Twist_::Twist_() +{ + // geometry_msgs::msg::dds_::Vector3_ m_linear + + // geometry_msgs::msg::dds_::Vector3_ m_angular + + +} + +Twist_::~Twist_() +{ +} + +Twist_::Twist_( + const Twist_& x) +{ + m_linear = x.m_linear; + + + m_angular = x.m_angular; + +} + +Twist_::Twist_( + Twist_&& x) noexcept +{ + m_linear = std::move(x.m_linear); + + + m_angular = std::move(x.m_angular); + +} + +Twist_& Twist_::operator =( + const Twist_& x) +{ + m_linear = x.m_linear; + + + m_angular = x.m_angular; + + return *this; +} + +Twist_& Twist_::operator =( + Twist_&& x) noexcept +{ + m_linear = std::move(x.m_linear); + + + m_angular = std::move(x.m_angular); + + return *this; +} + +bool Twist_::operator ==( + const Twist_& x) const +{ + return (m_linear == x.m_linear && + m_angular == x.m_angular); +} + +bool Twist_::operator !=( + const Twist_& x) const +{ + return !(*this == x); +} + +size_t Twist_::getMaxCdrSerializedSize( + size_t current_alignment) +{ + static_cast(current_alignment); + return geometry_msgs_msg_dds__Twist__max_cdr_typesize; +} + +size_t Twist_::getCdrSerializedSize( + const Twist_& data, + size_t current_alignment) +{ + (void)data; + size_t initial_alignment = current_alignment; + + current_alignment += geometry_msgs::msg::dds_::Vector3_::getCdrSerializedSize(data.linear(), current_alignment); + + + current_alignment += geometry_msgs::msg::dds_::Vector3_::getCdrSerializedSize(data.angular(), current_alignment); + + + return current_alignment - initial_alignment; +} + + +void Twist_::serialize( + eprosima::fastcdr::Cdr& scdr) const +{ + scdr << m_linear; + + scdr << m_angular; + +} + +void Twist_::deserialize( + eprosima::fastcdr::Cdr& dcdr) +{ + dcdr >> m_linear; + + + + dcdr >> m_angular; + + +} + + +bool Twist_::isKeyDefined() +{ + return false; +} + +void Twist_::serializeKey( + eprosima::fastcdr::Cdr& scdr) const +{ + (void) scdr; +} + +/*! + * @brief This function copies the value in member linear + * @param _linear New value to be copied in member linear + */ +void Twist_::linear( + const geometry_msgs::msg::dds_::Vector3_& _linear) +{ + m_linear = _linear; +} + +/*! + * @brief This function moves the value in member linear + * @param _linear New value to be moved in member linear + */ +void Twist_::linear( + geometry_msgs::msg::dds_::Vector3_&& _linear) +{ + m_linear = std::move(_linear); +} + +/*! + * @brief This function returns a constant reference to member linear + * @return Constant reference to member linear + */ +const geometry_msgs::msg::dds_::Vector3_& Twist_::linear() const +{ + return m_linear; +} + +/*! + * @brief This function returns a reference to member linear + * @return Reference to member linear + */ +geometry_msgs::msg::dds_::Vector3_& Twist_::linear() +{ + return m_linear; +} + + +/*! + * @brief This function copies the value in member angular + * @param _angular New value to be copied in member angular + */ +void Twist_::angular( + const geometry_msgs::msg::dds_::Vector3_& _angular) +{ + m_angular = _angular; +} + +/*! + * @brief This function moves the value in member angular + * @param _angular New value to be moved in member angular + */ +void Twist_::angular( + geometry_msgs::msg::dds_::Vector3_&& _angular) +{ + m_angular = std::move(_angular); +} + +/*! + * @brief This function returns a constant reference to member angular + * @return Constant reference to member angular + */ +const geometry_msgs::msg::dds_::Vector3_& Twist_::angular() const +{ + return m_angular; +} + +/*! + * @brief This function returns a reference to member angular + * @return Reference to member angular + */ +geometry_msgs::msg::dds_::Vector3_& Twist_::angular() +{ + return m_angular; +} + + + + + +} // namespace dds_ + + +} // namespace msg + + +} // namespace geometry_msgs + diff --git a/mujoco/idl_gen/geometry_msgs/Twist.h b/mujoco/idl_gen/geometry_msgs/Twist.h new file mode 100644 index 0000000..64989d1 --- /dev/null +++ b/mujoco/idl_gen/geometry_msgs/Twist.h @@ -0,0 +1,252 @@ +// Copyright 2016 Proyectos y Sistemas de Mantenimiento SL (eProsima). +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +/*! + * @file Twist.h + * This header file contains the declaration of the described types in the IDL file. + * + * This file was generated by the tool fastddsgen. + */ + +#ifndef _FAST_DDS_GENERATED_GEOMETRY_MSGS_MSG_DDS__TWIST_H_ +#define _FAST_DDS_GENERATED_GEOMETRY_MSGS_MSG_DDS__TWIST_H_ + +#include "Vector3.h" + +#include + +#include +#include +#include +#include +#include +#include + +#if defined(_WIN32) +#if defined(EPROSIMA_USER_DLL_EXPORT) +#define eProsima_user_DllExport __declspec( dllexport ) +#else +#define eProsima_user_DllExport +#endif // EPROSIMA_USER_DLL_EXPORT +#else +#define eProsima_user_DllExport +#endif // _WIN32 + +#if defined(_WIN32) +#if defined(EPROSIMA_USER_DLL_EXPORT) +#if defined(TWIST_SOURCE) +#define TWIST_DllAPI __declspec( dllexport ) +#else +#define TWIST_DllAPI __declspec( dllimport ) +#endif // TWIST_SOURCE +#else +#define TWIST_DllAPI +#endif // EPROSIMA_USER_DLL_EXPORT +#else +#define TWIST_DllAPI +#endif // _WIN32 + +namespace eprosima { +namespace fastcdr { +class Cdr; +} // namespace fastcdr +} // namespace eprosima + + + +namespace geometry_msgs { + namespace msg { + namespace dds_ { + + + /*! + * @brief This class represents the structure Twist_ defined by the user in the IDL file. + * @ingroup Twist + */ + class Twist_ + { + public: + + /*! + * @brief Default constructor. + */ + eProsima_user_DllExport Twist_(); + + /*! + * @brief Default destructor. + */ + eProsima_user_DllExport ~Twist_(); + + /*! + * @brief Copy constructor. + * @param x Reference to the object geometry_msgs::msg::dds_::Twist_ that will be copied. + */ + eProsima_user_DllExport Twist_( + const Twist_& x); + + /*! + * @brief Move constructor. + * @param x Reference to the object geometry_msgs::msg::dds_::Twist_ that will be copied. + */ + eProsima_user_DllExport Twist_( + Twist_&& x) noexcept; + + /*! + * @brief Copy assignment. + * @param x Reference to the object geometry_msgs::msg::dds_::Twist_ that will be copied. + */ + eProsima_user_DllExport Twist_& operator =( + const Twist_& x); + + /*! + * @brief Move assignment. + * @param x Reference to the object geometry_msgs::msg::dds_::Twist_ that will be copied. + */ + eProsima_user_DllExport Twist_& operator =( + Twist_&& x) noexcept; + + /*! + * @brief Comparison operator. + * @param x geometry_msgs::msg::dds_::Twist_ object to compare. + */ + eProsima_user_DllExport bool operator ==( + const Twist_& x) const; + + /*! + * @brief Comparison operator. + * @param x geometry_msgs::msg::dds_::Twist_ object to compare. + */ + eProsima_user_DllExport bool operator !=( + const Twist_& x) const; + + /*! + * @brief This function copies the value in member linear + * @param _linear New value to be copied in member linear + */ + eProsima_user_DllExport void linear( + const geometry_msgs::msg::dds_::Vector3_& _linear); + + /*! + * @brief This function moves the value in member linear + * @param _linear New value to be moved in member linear + */ + eProsima_user_DllExport void linear( + geometry_msgs::msg::dds_::Vector3_&& _linear); + + /*! + * @brief This function returns a constant reference to member linear + * @return Constant reference to member linear + */ + eProsima_user_DllExport const geometry_msgs::msg::dds_::Vector3_& linear() const; + + /*! + * @brief This function returns a reference to member linear + * @return Reference to member linear + */ + eProsima_user_DllExport geometry_msgs::msg::dds_::Vector3_& linear(); + + + /*! + * @brief This function copies the value in member angular + * @param _angular New value to be copied in member angular + */ + eProsima_user_DllExport void angular( + const geometry_msgs::msg::dds_::Vector3_& _angular); + + /*! + * @brief This function moves the value in member angular + * @param _angular New value to be moved in member angular + */ + eProsima_user_DllExport void angular( + geometry_msgs::msg::dds_::Vector3_&& _angular); + + /*! + * @brief This function returns a constant reference to member angular + * @return Constant reference to member angular + */ + eProsima_user_DllExport const geometry_msgs::msg::dds_::Vector3_& angular() const; + + /*! + * @brief This function returns a reference to member angular + * @return Reference to member angular + */ + eProsima_user_DllExport geometry_msgs::msg::dds_::Vector3_& angular(); + + + /*! + * @brief This function returns the maximum serialized size of an object + * depending on the buffer alignment. + * @param current_alignment Buffer alignment. + * @return Maximum serialized size. + */ + eProsima_user_DllExport static size_t getMaxCdrSerializedSize( + size_t current_alignment = 0); + + /*! + * @brief This function returns the serialized size of a data depending on the buffer alignment. + * @param data Data which is calculated its serialized size. + * @param current_alignment Buffer alignment. + * @return Serialized size. + */ + eProsima_user_DllExport static size_t getCdrSerializedSize( + const geometry_msgs::msg::dds_::Twist_& data, + size_t current_alignment = 0); + + + + /*! + * @brief This function serializes an object using CDR serialization. + * @param cdr CDR serialization object. + */ + eProsima_user_DllExport void serialize( + eprosima::fastcdr::Cdr& cdr) const; + + /*! + * @brief This function deserializes an object using CDR serialization. + * @param cdr CDR serialization object. + */ + eProsima_user_DllExport void deserialize( + eprosima::fastcdr::Cdr& cdr); + + + + + /*! + * @brief This function tells you if the Key has been defined for this type + */ + eProsima_user_DllExport static bool isKeyDefined(); + + /*! + * @brief This function serializes the key members of an object using CDR serialization. + * @param cdr CDR serialization object. + */ + eProsima_user_DllExport void serializeKey( + eprosima::fastcdr::Cdr& cdr) const; + + + private: + + geometry_msgs::msg::dds_::Vector3_ m_linear; + geometry_msgs::msg::dds_::Vector3_ m_angular; + + }; + + } // namespace dds_ + } // namespace msg +} // namespace geometry_msgs + +#endif // _FAST_DDS_GENERATED_GEOMETRY_MSGS_MSG_DDS__TWIST_H_ + + + diff --git a/mujoco/idl_gen/geometry_msgs/TwistCdrAux.hpp b/mujoco/idl_gen/geometry_msgs/TwistCdrAux.hpp new file mode 100644 index 0000000..cff1e80 --- /dev/null +++ b/mujoco/idl_gen/geometry_msgs/TwistCdrAux.hpp @@ -0,0 +1,51 @@ +// Copyright 2016 Proyectos y Sistemas de Mantenimiento SL (eProsima). +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +/*! + * @file TwistCdrAux.hpp + * This source file contains some definitions of CDR related functions. + * + * This file was generated by the tool fastddsgen. + */ + +#ifndef _FAST_DDS_GENERATED_GEOMETRY_MSGS_MSG_DDS__TWISTCDRAUX_HPP_ +#define _FAST_DDS_GENERATED_GEOMETRY_MSGS_MSG_DDS__TWISTCDRAUX_HPP_ + +#include "Twist.h" + +constexpr uint32_t geometry_msgs_msg_dds__Twist__max_cdr_typesize {48UL}; +constexpr uint32_t geometry_msgs_msg_dds__Twist__max_key_cdr_typesize {0UL}; + + +namespace eprosima { +namespace fastcdr { + +class Cdr; +class CdrSizeCalculator; + + + +eProsima_user_DllExport void serialize_key( + eprosima::fastcdr::Cdr& scdr, + const geometry_msgs::msg::dds_::Twist_& data); + + + + + +} // namespace fastcdr +} // namespace eprosima + +#endif // _FAST_DDS_GENERATED_GEOMETRY_MSGS_MSG_DDS__TWISTCDRAUX_HPP_ + diff --git a/mujoco/idl_gen/geometry_msgs/TwistCdrAux.ipp b/mujoco/idl_gen/geometry_msgs/TwistCdrAux.ipp new file mode 100644 index 0000000..88c03d1 --- /dev/null +++ b/mujoco/idl_gen/geometry_msgs/TwistCdrAux.ipp @@ -0,0 +1,139 @@ +// Copyright 2016 Proyectos y Sistemas de Mantenimiento SL (eProsima). +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +/*! + * @file TwistCdrAux.ipp + * This source file contains some declarations of CDR related functions. + * + * This file was generated by the tool fastddsgen. + */ + +#ifndef _FAST_DDS_GENERATED_GEOMETRY_MSGS_MSG_DDS__TWISTCDRAUX_IPP_ +#define _FAST_DDS_GENERATED_GEOMETRY_MSGS_MSG_DDS__TWISTCDRAUX_IPP_ + +#include "TwistCdrAux.hpp" + +#include +#include + + +#include +using namespace eprosima::fastcdr::exception; + +namespace eprosima { +namespace fastcdr { + + + +template<> +eProsima_user_DllExport size_t calculate_serialized_size( + eprosima::fastcdr::CdrSizeCalculator& calculator, + const geometry_msgs::msg::dds_::Twist_& data, + size_t& current_alignment) +{ + using namespace geometry_msgs::msg::dds_; + + static_cast(data); + + eprosima::fastcdr::EncodingAlgorithmFlag previous_encoding = calculator.get_encoding(); + size_t calculated_size {calculator.begin_calculate_type_serialized_size( + eprosima::fastcdr::CdrVersion::XCDRv2 == calculator.get_cdr_version() ? + eprosima::fastcdr::EncodingAlgorithmFlag::PLAIN_CDR2 : + eprosima::fastcdr::EncodingAlgorithmFlag::PLAIN_CDR, + current_alignment)}; + + + calculated_size += calculator.calculate_member_serialized_size(eprosima::fastcdr::MemberId(0), + data.linear(), current_alignment); + + calculated_size += calculator.calculate_member_serialized_size(eprosima::fastcdr::MemberId(1), + data.angular(), current_alignment); + + + calculated_size += calculator.end_calculate_type_serialized_size(previous_encoding, current_alignment); + + return calculated_size; +} + +template<> +eProsima_user_DllExport void serialize( + eprosima::fastcdr::Cdr& scdr, + const geometry_msgs::msg::dds_::Twist_& data) +{ + using namespace geometry_msgs::msg::dds_; + + eprosima::fastcdr::Cdr::state current_state(scdr); + scdr.begin_serialize_type(current_state, + eprosima::fastcdr::CdrVersion::XCDRv2 == scdr.get_cdr_version() ? + eprosima::fastcdr::EncodingAlgorithmFlag::PLAIN_CDR2 : + eprosima::fastcdr::EncodingAlgorithmFlag::PLAIN_CDR); + + scdr + << eprosima::fastcdr::MemberId(0) << data.linear() + << eprosima::fastcdr::MemberId(1) << data.angular() +; + scdr.end_serialize_type(current_state); +} + +template<> +eProsima_user_DllExport void deserialize( + eprosima::fastcdr::Cdr& cdr, + geometry_msgs::msg::dds_::Twist_& data) +{ + using namespace geometry_msgs::msg::dds_; + + cdr.deserialize_type(eprosima::fastcdr::CdrVersion::XCDRv2 == cdr.get_cdr_version() ? + eprosima::fastcdr::EncodingAlgorithmFlag::PLAIN_CDR2 : + eprosima::fastcdr::EncodingAlgorithmFlag::PLAIN_CDR, + [&data](eprosima::fastcdr::Cdr& dcdr, const eprosima::fastcdr::MemberId& mid) -> bool + { + bool ret_value = true; + switch (mid.id) + { + case 0: + dcdr >> data.linear(); + break; + + case 1: + dcdr >> data.angular(); + break; + + default: + ret_value = false; + break; + } + return ret_value; + }); +} + +void serialize_key( + eprosima::fastcdr::Cdr& scdr, + const geometry_msgs::msg::dds_::Twist_& data) +{ + using namespace geometry_msgs::msg::dds_; + + static_cast(scdr); + static_cast(data); +} + + + + + + +} // namespace fastcdr +} // namespace eprosima + +#endif // _FAST_DDS_GENERATED_GEOMETRY_MSGS_MSG_DDS__TWISTCDRAUX_IPP_ + diff --git a/mujoco/idl_gen/geometry_msgs/TwistPubSubTypes.cxx b/mujoco/idl_gen/geometry_msgs/TwistPubSubTypes.cxx new file mode 100644 index 0000000..71792a3 --- /dev/null +++ b/mujoco/idl_gen/geometry_msgs/TwistPubSubTypes.cxx @@ -0,0 +1,233 @@ +// Copyright 2016 Proyectos y Sistemas de Mantenimiento SL (eProsima). +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +/*! + * @file TwistPubSubTypes.cpp + * This header file contains the implementation of the serialization functions. + * + * This file was generated by the tool fastddsgen. + */ + + +#include + +#include "TwistPubSubTypes.h" +#include "TwistCdrAux.hpp" + +using SerializedPayload_t = eprosima::fastrtps::rtps::SerializedPayload_t; +using InstanceHandle_t = eprosima::fastrtps::rtps::InstanceHandle_t; +using DataRepresentationId_t = eprosima::fastdds::dds::DataRepresentationId_t; + +namespace geometry_msgs { + namespace msg { + namespace dds_ { + + + Twist_PubSubType::Twist_PubSubType() + { + setName("geometry_msgs::msg::dds_::Twist_"); + uint32_t type_size = + #if FASTCDR_VERSION_MAJOR == 1 + static_cast(Twist_::getMaxCdrSerializedSize()); + #else + geometry_msgs_msg_dds__Twist__max_cdr_typesize; + #endif + type_size += static_cast(eprosima::fastcdr::Cdr::alignment(type_size, 4)); /* possible submessage alignment */ + m_typeSize = type_size + 4; /*encapsulation*/ + m_isGetKeyDefined = false; + uint32_t keyLength = geometry_msgs_msg_dds__Twist__max_key_cdr_typesize > 16 ? geometry_msgs_msg_dds__Twist__max_key_cdr_typesize : 16; + m_keyBuffer = reinterpret_cast(malloc(keyLength)); + memset(m_keyBuffer, 0, keyLength); + } + + Twist_PubSubType::~Twist_PubSubType() + { + if (m_keyBuffer != nullptr) + { + free(m_keyBuffer); + } + } + + bool Twist_PubSubType::serialize( + void* data, + SerializedPayload_t* payload, + DataRepresentationId_t data_representation) + { + Twist_* p_type = static_cast(data); + + // Object that manages the raw buffer. + eprosima::fastcdr::FastBuffer fastbuffer(reinterpret_cast(payload->data), payload->max_size); + // Object that serializes the data. + eprosima::fastcdr::Cdr ser(fastbuffer, eprosima::fastcdr::Cdr::DEFAULT_ENDIAN, + data_representation == DataRepresentationId_t::XCDR_DATA_REPRESENTATION ? + eprosima::fastcdr::CdrVersion::XCDRv1 : eprosima::fastcdr::CdrVersion::XCDRv2); + payload->encapsulation = ser.endianness() == eprosima::fastcdr::Cdr::BIG_ENDIANNESS ? CDR_BE : CDR_LE; + #if FASTCDR_VERSION_MAJOR > 1 + ser.set_encoding_flag( + data_representation == DataRepresentationId_t::XCDR_DATA_REPRESENTATION ? + eprosima::fastcdr::EncodingAlgorithmFlag::PLAIN_CDR : + eprosima::fastcdr::EncodingAlgorithmFlag::PLAIN_CDR2); + #endif // FASTCDR_VERSION_MAJOR > 1 + + try + { + // Serialize encapsulation + ser.serialize_encapsulation(); + // Serialize the object. + ser << *p_type; + } + catch (eprosima::fastcdr::exception::Exception& /*exception*/) + { + return false; + } + + // Get the serialized length + #if FASTCDR_VERSION_MAJOR == 1 + payload->length = static_cast(ser.getSerializedDataLength()); + #else + payload->length = static_cast(ser.get_serialized_data_length()); + #endif // FASTCDR_VERSION_MAJOR == 1 + return true; + } + + bool Twist_PubSubType::deserialize( + SerializedPayload_t* payload, + void* data) + { + try + { + // Convert DATA to pointer of your type + Twist_* p_type = static_cast(data); + + // Object that manages the raw buffer. + eprosima::fastcdr::FastBuffer fastbuffer(reinterpret_cast(payload->data), payload->length); + + // Object that deserializes the data. + eprosima::fastcdr::Cdr deser(fastbuffer, eprosima::fastcdr::Cdr::DEFAULT_ENDIAN + #if FASTCDR_VERSION_MAJOR == 1 + , eprosima::fastcdr::Cdr::CdrType::DDS_CDR + #endif // FASTCDR_VERSION_MAJOR == 1 + ); + + // Deserialize encapsulation. + deser.read_encapsulation(); + payload->encapsulation = deser.endianness() == eprosima::fastcdr::Cdr::BIG_ENDIANNESS ? CDR_BE : CDR_LE; + + // Deserialize the object. + deser >> *p_type; + } + catch (eprosima::fastcdr::exception::Exception& /*exception*/) + { + return false; + } + + return true; + } + + std::function Twist_PubSubType::getSerializedSizeProvider( + void* data, + DataRepresentationId_t data_representation) + { + return [data, data_representation]() -> uint32_t + { + #if FASTCDR_VERSION_MAJOR == 1 + static_cast(data_representation); + return static_cast(type::getCdrSerializedSize(*static_cast(data))) + + 4u /*encapsulation*/; + #else + try + { + eprosima::fastcdr::CdrSizeCalculator calculator( + data_representation == DataRepresentationId_t::XCDR_DATA_REPRESENTATION ? + eprosima::fastcdr::CdrVersion::XCDRv1 :eprosima::fastcdr::CdrVersion::XCDRv2); + size_t current_alignment {0}; + return static_cast(calculator.calculate_serialized_size( + *static_cast(data), current_alignment)) + + 4u /*encapsulation*/; + } + catch (eprosima::fastcdr::exception::Exception& /*exception*/) + { + return 0; + } + #endif // FASTCDR_VERSION_MAJOR == 1 + }; + } + + void* Twist_PubSubType::createData() + { + return reinterpret_cast(new Twist_()); + } + + void Twist_PubSubType::deleteData( + void* data) + { + delete(reinterpret_cast(data)); + } + + bool Twist_PubSubType::getKey( + void* data, + InstanceHandle_t* handle, + bool force_md5) + { + if (!m_isGetKeyDefined) + { + return false; + } + + Twist_* p_type = static_cast(data); + + // Object that manages the raw buffer. + eprosima::fastcdr::FastBuffer fastbuffer(reinterpret_cast(m_keyBuffer), + geometry_msgs_msg_dds__Twist__max_key_cdr_typesize); + + // Object that serializes the data. + eprosima::fastcdr::Cdr ser(fastbuffer, eprosima::fastcdr::Cdr::BIG_ENDIANNESS, eprosima::fastcdr::CdrVersion::XCDRv1); + #if FASTCDR_VERSION_MAJOR == 1 + p_type->serializeKey(ser); + #else + eprosima::fastcdr::serialize_key(ser, *p_type); + #endif // FASTCDR_VERSION_MAJOR == 1 + if (force_md5 || geometry_msgs_msg_dds__Twist__max_key_cdr_typesize > 16) + { + m_md5.init(); + #if FASTCDR_VERSION_MAJOR == 1 + m_md5.update(m_keyBuffer, static_cast(ser.getSerializedDataLength())); + #else + m_md5.update(m_keyBuffer, static_cast(ser.get_serialized_data_length())); + #endif // FASTCDR_VERSION_MAJOR == 1 + m_md5.finalize(); + for (uint8_t i = 0; i < 16; ++i) + { + handle->value[i] = m_md5.digest[i]; + } + } + else + { + for (uint8_t i = 0; i < 16; ++i) + { + handle->value[i] = m_keyBuffer[i]; + } + } + return true; + } + + + } //End of namespace dds_ + + + } //End of namespace msg + + +} //End of namespace geometry_msgs + diff --git a/mujoco/idl_gen/geometry_msgs/TwistPubSubTypes.h b/mujoco/idl_gen/geometry_msgs/TwistPubSubTypes.h new file mode 100644 index 0000000..defb4e8 --- /dev/null +++ b/mujoco/idl_gen/geometry_msgs/TwistPubSubTypes.h @@ -0,0 +1,197 @@ +// Copyright 2016 Proyectos y Sistemas de Mantenimiento SL (eProsima). +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +/*! + * @file TwistPubSubTypes.h + * This header file contains the declaration of the serialization functions. + * + * This file was generated by the tool fastddsgen. + */ + + +#ifndef _FAST_DDS_GENERATED_GEOMETRY_MSGS_MSG_DDS__TWIST_PUBSUBTYPES_H_ +#define _FAST_DDS_GENERATED_GEOMETRY_MSGS_MSG_DDS__TWIST_PUBSUBTYPES_H_ + +#include +#include +#include +#include +#include + +#include "Twist.h" + +#include "Vector3PubSubTypes.h" + +#if !defined(GEN_API_VER) || (GEN_API_VER != 2) +#error \ + Generated Twist is not compatible with current installed Fast DDS. Please, regenerate it with fastddsgen. +#endif // GEN_API_VER + +namespace geometry_msgs +{ + namespace msg + { + namespace dds_ + { + + + + #ifndef SWIG + namespace detail { + + template + struct Twist__rob + { + friend constexpr typename Tag::type get( + Tag) + { + return M; + } + + }; + + struct Twist__f + { + typedef geometry_msgs::msg::dds_::Vector3_ Twist_::* type; + friend constexpr type get( + Twist__f); + }; + + template struct Twist__rob; + + template + inline size_t constexpr Twist__offset_of() + { + return ((::size_t) &reinterpret_cast((((T*)0)->*get(Tag())))); + } + + } // namespace detail + #endif // ifndef SWIG + + + /*! + * @brief This class represents the TopicDataType of the type Twist_ defined by the user in the IDL file. + * @ingroup Twist + */ + class Twist_PubSubType : public eprosima::fastdds::dds::TopicDataType + { + public: + + typedef Twist_ type; + + eProsima_user_DllExport Twist_PubSubType(); + + eProsima_user_DllExport ~Twist_PubSubType() override; + + eProsima_user_DllExport bool serialize( + void* data, + eprosima::fastrtps::rtps::SerializedPayload_t* payload) override + { + return serialize(data, payload, eprosima::fastdds::dds::DEFAULT_DATA_REPRESENTATION); + } + + eProsima_user_DllExport bool serialize( + void* data, + eprosima::fastrtps::rtps::SerializedPayload_t* payload, + eprosima::fastdds::dds::DataRepresentationId_t data_representation) override; + + eProsima_user_DllExport bool deserialize( + eprosima::fastrtps::rtps::SerializedPayload_t* payload, + void* data) override; + + eProsima_user_DllExport std::function getSerializedSizeProvider( + void* data) override + { + return getSerializedSizeProvider(data, eprosima::fastdds::dds::DEFAULT_DATA_REPRESENTATION); + } + + eProsima_user_DllExport std::function getSerializedSizeProvider( + void* data, + eprosima::fastdds::dds::DataRepresentationId_t data_representation) override; + + eProsima_user_DllExport bool getKey( + void* data, + eprosima::fastrtps::rtps::InstanceHandle_t* ihandle, + bool force_md5 = false) override; + + eProsima_user_DllExport void* createData() override; + + eProsima_user_DllExport void deleteData( + void* data) override; + + #ifdef TOPIC_DATA_TYPE_API_HAS_IS_BOUNDED + eProsima_user_DllExport inline bool is_bounded() const override + { + return true; + } + + #endif // TOPIC_DATA_TYPE_API_HAS_IS_BOUNDED + + #ifdef TOPIC_DATA_TYPE_API_HAS_IS_PLAIN + eProsima_user_DllExport inline bool is_plain() const override + { + return is_plain_xcdrv1_impl(); + } + + eProsima_user_DllExport inline bool is_plain( + eprosima::fastdds::dds::DataRepresentationId_t data_representation) const override + { + if(data_representation == eprosima::fastdds::dds::DataRepresentationId_t::XCDR2_DATA_REPRESENTATION) + { + return is_plain_xcdrv2_impl(); + } + else + { + return is_plain_xcdrv1_impl(); + } + } + + #endif // TOPIC_DATA_TYPE_API_HAS_IS_PLAIN + + #ifdef TOPIC_DATA_TYPE_API_HAS_CONSTRUCT_SAMPLE + eProsima_user_DllExport inline bool construct_sample( + void* memory) const override + { + new (memory) Twist_(); + return true; + } + + #endif // TOPIC_DATA_TYPE_API_HAS_CONSTRUCT_SAMPLE + + MD5 m_md5; + unsigned char* m_keyBuffer; + + private: + + static constexpr bool is_plain_xcdrv1_impl() + { + return 48ULL == + (detail::Twist__offset_of() + + sizeof(geometry_msgs::msg::dds_::Vector3_)); + } + + static constexpr bool is_plain_xcdrv2_impl() + { + return 48ULL == + (detail::Twist__offset_of() + + sizeof(geometry_msgs::msg::dds_::Vector3_)); + } + + }; + } + } +} + +#endif // _FAST_DDS_GENERATED_GEOMETRY_MSGS_MSG_DDS__TWIST_PUBSUBTYPES_H_ + diff --git a/mujoco/idl_gen/geometry_msgs/TwistWithCovariance.cxx b/mujoco/idl_gen/geometry_msgs/TwistWithCovariance.cxx new file mode 100644 index 0000000..462570d --- /dev/null +++ b/mujoco/idl_gen/geometry_msgs/TwistWithCovariance.cxx @@ -0,0 +1,312 @@ +// Copyright 2016 Proyectos y Sistemas de Mantenimiento SL (eProsima). +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +/*! + * @file TwistWithCovariance.cpp + * This source file contains the implementation of the described types in the IDL file. + * + * This file was generated by the tool fastddsgen. + */ + +#ifdef _WIN32 +// Remove linker warning LNK4221 on Visual Studio +namespace { +char dummy; +} // namespace +#endif // _WIN32 + +#include "TwistWithCovariance.h" + +#include + + +#include +using namespace eprosima::fastcdr::exception; + +#include + +namespace helper { namespace internal { + +enum class Size { + UInt8, + UInt16, + UInt32, + UInt64, +}; + +constexpr Size get_size(int s) { + return (s <= 8 ) ? Size::UInt8: + (s <= 16) ? Size::UInt16: + (s <= 32) ? Size::UInt32: Size::UInt64; +} + +template +struct FindTypeH; + +template<> +struct FindTypeH { + using type = std::uint8_t; +}; + +template<> +struct FindTypeH { + using type = std::uint16_t; +}; + +template<> +struct FindTypeH { + using type = std::uint32_t; +}; + +template<> +struct FindTypeH { + using type = std::uint64_t; +}; +} + +template +struct FindType { + using type = typename internal::FindTypeH::type; +}; +} + +#define geometry_msgs_msg_dds__Vector3__max_cdr_typesize 24ULL; +#define geometry_msgs_msg_dds__TwistWithCovariance__max_cdr_typesize 336ULL; +#define geometry_msgs_msg_dds__Twist__max_cdr_typesize 48ULL; + + +namespace geometry_msgs { + +namespace msg { + +namespace dds_ { + + + +TwistWithCovariance_::TwistWithCovariance_() +{ + // geometry_msgs::msg::dds_::Twist_ m_twist + + // double m_covariance + memset(&m_covariance, 0, ((36)) * 8); + +} + +TwistWithCovariance_::~TwistWithCovariance_() +{ +} + +TwistWithCovariance_::TwistWithCovariance_( + const TwistWithCovariance_& x) +{ + m_twist = x.m_twist; + + + m_covariance = x.m_covariance; + +} + +TwistWithCovariance_::TwistWithCovariance_( + TwistWithCovariance_&& x) noexcept +{ + m_twist = std::move(x.m_twist); + + + m_covariance = std::move(x.m_covariance); + +} + +TwistWithCovariance_& TwistWithCovariance_::operator =( + const TwistWithCovariance_& x) +{ + m_twist = x.m_twist; + + + m_covariance = x.m_covariance; + + return *this; +} + +TwistWithCovariance_& TwistWithCovariance_::operator =( + TwistWithCovariance_&& x) noexcept +{ + m_twist = std::move(x.m_twist); + + + m_covariance = std::move(x.m_covariance); + + return *this; +} + +bool TwistWithCovariance_::operator ==( + const TwistWithCovariance_& x) const +{ + return (m_twist == x.m_twist && + m_covariance == x.m_covariance); +} + +bool TwistWithCovariance_::operator !=( + const TwistWithCovariance_& x) const +{ + return !(*this == x); +} + +size_t TwistWithCovariance_::getMaxCdrSerializedSize( + size_t current_alignment) +{ + static_cast(current_alignment); + return geometry_msgs_msg_dds__TwistWithCovariance__max_cdr_typesize; +} + +size_t TwistWithCovariance_::getCdrSerializedSize( + const TwistWithCovariance_& data, + size_t current_alignment) +{ + (void)data; + size_t initial_alignment = current_alignment; + + current_alignment += geometry_msgs::msg::dds_::Twist_::getCdrSerializedSize(data.twist(), current_alignment); + + + current_alignment += (((36)) * 8) + eprosima::fastcdr::Cdr::alignment(current_alignment, 8); + + + + return current_alignment - initial_alignment; +} + + +void TwistWithCovariance_::serialize( + eprosima::fastcdr::Cdr& scdr) const +{ + scdr << m_twist; + + scdr << m_covariance; + + +} + +void TwistWithCovariance_::deserialize( + eprosima::fastcdr::Cdr& dcdr) +{ + dcdr >> m_twist; + + + + dcdr >> m_covariance; + + +} + + +bool TwistWithCovariance_::isKeyDefined() +{ + return false; +} + +void TwistWithCovariance_::serializeKey( + eprosima::fastcdr::Cdr& scdr) const +{ + (void) scdr; +} + +/*! + * @brief This function copies the value in member twist + * @param _twist New value to be copied in member twist + */ +void TwistWithCovariance_::twist( + const geometry_msgs::msg::dds_::Twist_& _twist) +{ + m_twist = _twist; +} + +/*! + * @brief This function moves the value in member twist + * @param _twist New value to be moved in member twist + */ +void TwistWithCovariance_::twist( + geometry_msgs::msg::dds_::Twist_&& _twist) +{ + m_twist = std::move(_twist); +} + +/*! + * @brief This function returns a constant reference to member twist + * @return Constant reference to member twist + */ +const geometry_msgs::msg::dds_::Twist_& TwistWithCovariance_::twist() const +{ + return m_twist; +} + +/*! + * @brief This function returns a reference to member twist + * @return Reference to member twist + */ +geometry_msgs::msg::dds_::Twist_& TwistWithCovariance_::twist() +{ + return m_twist; +} + + +/*! + * @brief This function copies the value in member covariance + * @param _covariance New value to be copied in member covariance + */ +void TwistWithCovariance_::covariance( + const std::array& _covariance) +{ + m_covariance = _covariance; +} + +/*! + * @brief This function moves the value in member covariance + * @param _covariance New value to be moved in member covariance + */ +void TwistWithCovariance_::covariance( + std::array&& _covariance) +{ + m_covariance = std::move(_covariance); +} + +/*! + * @brief This function returns a constant reference to member covariance + * @return Constant reference to member covariance + */ +const std::array& TwistWithCovariance_::covariance() const +{ + return m_covariance; +} + +/*! + * @brief This function returns a reference to member covariance + * @return Reference to member covariance + */ +std::array& TwistWithCovariance_::covariance() +{ + return m_covariance; +} + + + + + +} // namespace dds_ + + +} // namespace msg + + +} // namespace geometry_msgs + diff --git a/mujoco/idl_gen/geometry_msgs/TwistWithCovariance.h b/mujoco/idl_gen/geometry_msgs/TwistWithCovariance.h new file mode 100644 index 0000000..e263013 --- /dev/null +++ b/mujoco/idl_gen/geometry_msgs/TwistWithCovariance.h @@ -0,0 +1,252 @@ +// Copyright 2016 Proyectos y Sistemas de Mantenimiento SL (eProsima). +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +/*! + * @file TwistWithCovariance.h + * This header file contains the declaration of the described types in the IDL file. + * + * This file was generated by the tool fastddsgen. + */ + +#ifndef _FAST_DDS_GENERATED_GEOMETRY_MSGS_MSG_DDS__TWISTWITHCOVARIANCE_H_ +#define _FAST_DDS_GENERATED_GEOMETRY_MSGS_MSG_DDS__TWISTWITHCOVARIANCE_H_ + +#include "Twist.h" + +#include + +#include +#include +#include +#include +#include +#include + +#if defined(_WIN32) +#if defined(EPROSIMA_USER_DLL_EXPORT) +#define eProsima_user_DllExport __declspec( dllexport ) +#else +#define eProsima_user_DllExport +#endif // EPROSIMA_USER_DLL_EXPORT +#else +#define eProsima_user_DllExport +#endif // _WIN32 + +#if defined(_WIN32) +#if defined(EPROSIMA_USER_DLL_EXPORT) +#if defined(TWISTWITHCOVARIANCE_SOURCE) +#define TWISTWITHCOVARIANCE_DllAPI __declspec( dllexport ) +#else +#define TWISTWITHCOVARIANCE_DllAPI __declspec( dllimport ) +#endif // TWISTWITHCOVARIANCE_SOURCE +#else +#define TWISTWITHCOVARIANCE_DllAPI +#endif // EPROSIMA_USER_DLL_EXPORT +#else +#define TWISTWITHCOVARIANCE_DllAPI +#endif // _WIN32 + +namespace eprosima { +namespace fastcdr { +class Cdr; +} // namespace fastcdr +} // namespace eprosima + + + +namespace geometry_msgs { + namespace msg { + namespace dds_ { + + + /*! + * @brief This class represents the structure TwistWithCovariance_ defined by the user in the IDL file. + * @ingroup TwistWithCovariance + */ + class TwistWithCovariance_ + { + public: + + /*! + * @brief Default constructor. + */ + eProsima_user_DllExport TwistWithCovariance_(); + + /*! + * @brief Default destructor. + */ + eProsima_user_DllExport ~TwistWithCovariance_(); + + /*! + * @brief Copy constructor. + * @param x Reference to the object geometry_msgs::msg::dds_::TwistWithCovariance_ that will be copied. + */ + eProsima_user_DllExport TwistWithCovariance_( + const TwistWithCovariance_& x); + + /*! + * @brief Move constructor. + * @param x Reference to the object geometry_msgs::msg::dds_::TwistWithCovariance_ that will be copied. + */ + eProsima_user_DllExport TwistWithCovariance_( + TwistWithCovariance_&& x) noexcept; + + /*! + * @brief Copy assignment. + * @param x Reference to the object geometry_msgs::msg::dds_::TwistWithCovariance_ that will be copied. + */ + eProsima_user_DllExport TwistWithCovariance_& operator =( + const TwistWithCovariance_& x); + + /*! + * @brief Move assignment. + * @param x Reference to the object geometry_msgs::msg::dds_::TwistWithCovariance_ that will be copied. + */ + eProsima_user_DllExport TwistWithCovariance_& operator =( + TwistWithCovariance_&& x) noexcept; + + /*! + * @brief Comparison operator. + * @param x geometry_msgs::msg::dds_::TwistWithCovariance_ object to compare. + */ + eProsima_user_DllExport bool operator ==( + const TwistWithCovariance_& x) const; + + /*! + * @brief Comparison operator. + * @param x geometry_msgs::msg::dds_::TwistWithCovariance_ object to compare. + */ + eProsima_user_DllExport bool operator !=( + const TwistWithCovariance_& x) const; + + /*! + * @brief This function copies the value in member twist + * @param _twist New value to be copied in member twist + */ + eProsima_user_DllExport void twist( + const geometry_msgs::msg::dds_::Twist_& _twist); + + /*! + * @brief This function moves the value in member twist + * @param _twist New value to be moved in member twist + */ + eProsima_user_DllExport void twist( + geometry_msgs::msg::dds_::Twist_&& _twist); + + /*! + * @brief This function returns a constant reference to member twist + * @return Constant reference to member twist + */ + eProsima_user_DllExport const geometry_msgs::msg::dds_::Twist_& twist() const; + + /*! + * @brief This function returns a reference to member twist + * @return Reference to member twist + */ + eProsima_user_DllExport geometry_msgs::msg::dds_::Twist_& twist(); + + + /*! + * @brief This function copies the value in member covariance + * @param _covariance New value to be copied in member covariance + */ + eProsima_user_DllExport void covariance( + const std::array& _covariance); + + /*! + * @brief This function moves the value in member covariance + * @param _covariance New value to be moved in member covariance + */ + eProsima_user_DllExport void covariance( + std::array&& _covariance); + + /*! + * @brief This function returns a constant reference to member covariance + * @return Constant reference to member covariance + */ + eProsima_user_DllExport const std::array& covariance() const; + + /*! + * @brief This function returns a reference to member covariance + * @return Reference to member covariance + */ + eProsima_user_DllExport std::array& covariance(); + + + /*! + * @brief This function returns the maximum serialized size of an object + * depending on the buffer alignment. + * @param current_alignment Buffer alignment. + * @return Maximum serialized size. + */ + eProsima_user_DllExport static size_t getMaxCdrSerializedSize( + size_t current_alignment = 0); + + /*! + * @brief This function returns the serialized size of a data depending on the buffer alignment. + * @param data Data which is calculated its serialized size. + * @param current_alignment Buffer alignment. + * @return Serialized size. + */ + eProsima_user_DllExport static size_t getCdrSerializedSize( + const geometry_msgs::msg::dds_::TwistWithCovariance_& data, + size_t current_alignment = 0); + + + + /*! + * @brief This function serializes an object using CDR serialization. + * @param cdr CDR serialization object. + */ + eProsima_user_DllExport void serialize( + eprosima::fastcdr::Cdr& cdr) const; + + /*! + * @brief This function deserializes an object using CDR serialization. + * @param cdr CDR serialization object. + */ + eProsima_user_DllExport void deserialize( + eprosima::fastcdr::Cdr& cdr); + + + + + /*! + * @brief This function tells you if the Key has been defined for this type + */ + eProsima_user_DllExport static bool isKeyDefined(); + + /*! + * @brief This function serializes the key members of an object using CDR serialization. + * @param cdr CDR serialization object. + */ + eProsima_user_DllExport void serializeKey( + eprosima::fastcdr::Cdr& cdr) const; + + + private: + + geometry_msgs::msg::dds_::Twist_ m_twist; + std::array m_covariance; + + }; + + } // namespace dds_ + } // namespace msg +} // namespace geometry_msgs + +#endif // _FAST_DDS_GENERATED_GEOMETRY_MSGS_MSG_DDS__TWISTWITHCOVARIANCE_H_ + + + diff --git a/mujoco/idl_gen/geometry_msgs/TwistWithCovarianceCdrAux.hpp b/mujoco/idl_gen/geometry_msgs/TwistWithCovarianceCdrAux.hpp new file mode 100644 index 0000000..297ad3b --- /dev/null +++ b/mujoco/idl_gen/geometry_msgs/TwistWithCovarianceCdrAux.hpp @@ -0,0 +1,52 @@ +// Copyright 2016 Proyectos y Sistemas de Mantenimiento SL (eProsima). +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +/*! + * @file TwistWithCovarianceCdrAux.hpp + * This source file contains some definitions of CDR related functions. + * + * This file was generated by the tool fastddsgen. + */ + +#ifndef _FAST_DDS_GENERATED_GEOMETRY_MSGS_MSG_DDS__TWISTWITHCOVARIANCECDRAUX_HPP_ +#define _FAST_DDS_GENERATED_GEOMETRY_MSGS_MSG_DDS__TWISTWITHCOVARIANCECDRAUX_HPP_ + +#include "TwistWithCovariance.h" + +constexpr uint32_t geometry_msgs_msg_dds__TwistWithCovariance__max_cdr_typesize {336UL}; +constexpr uint32_t geometry_msgs_msg_dds__TwistWithCovariance__max_key_cdr_typesize {0UL}; + + + +namespace eprosima { +namespace fastcdr { + +class Cdr; +class CdrSizeCalculator; + + + +eProsima_user_DllExport void serialize_key( + eprosima::fastcdr::Cdr& scdr, + const geometry_msgs::msg::dds_::TwistWithCovariance_& data); + + + + + +} // namespace fastcdr +} // namespace eprosima + +#endif // _FAST_DDS_GENERATED_GEOMETRY_MSGS_MSG_DDS__TWISTWITHCOVARIANCECDRAUX_HPP_ + diff --git a/mujoco/idl_gen/geometry_msgs/TwistWithCovarianceCdrAux.ipp b/mujoco/idl_gen/geometry_msgs/TwistWithCovarianceCdrAux.ipp new file mode 100644 index 0000000..a3fc729 --- /dev/null +++ b/mujoco/idl_gen/geometry_msgs/TwistWithCovarianceCdrAux.ipp @@ -0,0 +1,139 @@ +// Copyright 2016 Proyectos y Sistemas de Mantenimiento SL (eProsima). +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +/*! + * @file TwistWithCovarianceCdrAux.ipp + * This source file contains some declarations of CDR related functions. + * + * This file was generated by the tool fastddsgen. + */ + +#ifndef _FAST_DDS_GENERATED_GEOMETRY_MSGS_MSG_DDS__TWISTWITHCOVARIANCECDRAUX_IPP_ +#define _FAST_DDS_GENERATED_GEOMETRY_MSGS_MSG_DDS__TWISTWITHCOVARIANCECDRAUX_IPP_ + +#include "TwistWithCovarianceCdrAux.hpp" + +#include +#include + + +#include +using namespace eprosima::fastcdr::exception; + +namespace eprosima { +namespace fastcdr { + + + +template<> +eProsima_user_DllExport size_t calculate_serialized_size( + eprosima::fastcdr::CdrSizeCalculator& calculator, + const geometry_msgs::msg::dds_::TwistWithCovariance_& data, + size_t& current_alignment) +{ + using namespace geometry_msgs::msg::dds_; + + static_cast(data); + + eprosima::fastcdr::EncodingAlgorithmFlag previous_encoding = calculator.get_encoding(); + size_t calculated_size {calculator.begin_calculate_type_serialized_size( + eprosima::fastcdr::CdrVersion::XCDRv2 == calculator.get_cdr_version() ? + eprosima::fastcdr::EncodingAlgorithmFlag::PLAIN_CDR2 : + eprosima::fastcdr::EncodingAlgorithmFlag::PLAIN_CDR, + current_alignment)}; + + + calculated_size += calculator.calculate_member_serialized_size(eprosima::fastcdr::MemberId(0), + data.twist(), current_alignment); + + calculated_size += calculator.calculate_member_serialized_size(eprosima::fastcdr::MemberId(1), + data.covariance(), current_alignment); + + + calculated_size += calculator.end_calculate_type_serialized_size(previous_encoding, current_alignment); + + return calculated_size; +} + +template<> +eProsima_user_DllExport void serialize( + eprosima::fastcdr::Cdr& scdr, + const geometry_msgs::msg::dds_::TwistWithCovariance_& data) +{ + using namespace geometry_msgs::msg::dds_; + + eprosima::fastcdr::Cdr::state current_state(scdr); + scdr.begin_serialize_type(current_state, + eprosima::fastcdr::CdrVersion::XCDRv2 == scdr.get_cdr_version() ? + eprosima::fastcdr::EncodingAlgorithmFlag::PLAIN_CDR2 : + eprosima::fastcdr::EncodingAlgorithmFlag::PLAIN_CDR); + + scdr + << eprosima::fastcdr::MemberId(0) << data.twist() + << eprosima::fastcdr::MemberId(1) << data.covariance() +; + scdr.end_serialize_type(current_state); +} + +template<> +eProsima_user_DllExport void deserialize( + eprosima::fastcdr::Cdr& cdr, + geometry_msgs::msg::dds_::TwistWithCovariance_& data) +{ + using namespace geometry_msgs::msg::dds_; + + cdr.deserialize_type(eprosima::fastcdr::CdrVersion::XCDRv2 == cdr.get_cdr_version() ? + eprosima::fastcdr::EncodingAlgorithmFlag::PLAIN_CDR2 : + eprosima::fastcdr::EncodingAlgorithmFlag::PLAIN_CDR, + [&data](eprosima::fastcdr::Cdr& dcdr, const eprosima::fastcdr::MemberId& mid) -> bool + { + bool ret_value = true; + switch (mid.id) + { + case 0: + dcdr >> data.twist(); + break; + + case 1: + dcdr >> data.covariance(); + break; + + default: + ret_value = false; + break; + } + return ret_value; + }); +} + +void serialize_key( + eprosima::fastcdr::Cdr& scdr, + const geometry_msgs::msg::dds_::TwistWithCovariance_& data) +{ + using namespace geometry_msgs::msg::dds_; + + static_cast(scdr); + static_cast(data); +} + + + + + + +} // namespace fastcdr +} // namespace eprosima + +#endif // _FAST_DDS_GENERATED_GEOMETRY_MSGS_MSG_DDS__TWISTWITHCOVARIANCECDRAUX_IPP_ + diff --git a/mujoco/idl_gen/geometry_msgs/TwistWithCovariancePubSubTypes.cxx b/mujoco/idl_gen/geometry_msgs/TwistWithCovariancePubSubTypes.cxx new file mode 100644 index 0000000..4c24e80 --- /dev/null +++ b/mujoco/idl_gen/geometry_msgs/TwistWithCovariancePubSubTypes.cxx @@ -0,0 +1,233 @@ +// Copyright 2016 Proyectos y Sistemas de Mantenimiento SL (eProsima). +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +/*! + * @file TwistWithCovariancePubSubTypes.cpp + * This header file contains the implementation of the serialization functions. + * + * This file was generated by the tool fastddsgen. + */ + + +#include + +#include "TwistWithCovariancePubSubTypes.h" +#include "TwistWithCovarianceCdrAux.hpp" + +using SerializedPayload_t = eprosima::fastrtps::rtps::SerializedPayload_t; +using InstanceHandle_t = eprosima::fastrtps::rtps::InstanceHandle_t; +using DataRepresentationId_t = eprosima::fastdds::dds::DataRepresentationId_t; + +namespace geometry_msgs { + namespace msg { + namespace dds_ { + + + TwistWithCovariance_PubSubType::TwistWithCovariance_PubSubType() + { + setName("geometry_msgs::msg::dds_::TwistWithCovariance_"); + uint32_t type_size = + #if FASTCDR_VERSION_MAJOR == 1 + static_cast(TwistWithCovariance_::getMaxCdrSerializedSize()); + #else + geometry_msgs_msg_dds__TwistWithCovariance__max_cdr_typesize; + #endif + type_size += static_cast(eprosima::fastcdr::Cdr::alignment(type_size, 4)); /* possible submessage alignment */ + m_typeSize = type_size + 4; /*encapsulation*/ + m_isGetKeyDefined = false; + uint32_t keyLength = geometry_msgs_msg_dds__TwistWithCovariance__max_key_cdr_typesize > 16 ? geometry_msgs_msg_dds__TwistWithCovariance__max_key_cdr_typesize : 16; + m_keyBuffer = reinterpret_cast(malloc(keyLength)); + memset(m_keyBuffer, 0, keyLength); + } + + TwistWithCovariance_PubSubType::~TwistWithCovariance_PubSubType() + { + if (m_keyBuffer != nullptr) + { + free(m_keyBuffer); + } + } + + bool TwistWithCovariance_PubSubType::serialize( + void* data, + SerializedPayload_t* payload, + DataRepresentationId_t data_representation) + { + TwistWithCovariance_* p_type = static_cast(data); + + // Object that manages the raw buffer. + eprosima::fastcdr::FastBuffer fastbuffer(reinterpret_cast(payload->data), payload->max_size); + // Object that serializes the data. + eprosima::fastcdr::Cdr ser(fastbuffer, eprosima::fastcdr::Cdr::DEFAULT_ENDIAN, + data_representation == DataRepresentationId_t::XCDR_DATA_REPRESENTATION ? + eprosima::fastcdr::CdrVersion::XCDRv1 : eprosima::fastcdr::CdrVersion::XCDRv2); + payload->encapsulation = ser.endianness() == eprosima::fastcdr::Cdr::BIG_ENDIANNESS ? CDR_BE : CDR_LE; + #if FASTCDR_VERSION_MAJOR > 1 + ser.set_encoding_flag( + data_representation == DataRepresentationId_t::XCDR_DATA_REPRESENTATION ? + eprosima::fastcdr::EncodingAlgorithmFlag::PLAIN_CDR : + eprosima::fastcdr::EncodingAlgorithmFlag::PLAIN_CDR2); + #endif // FASTCDR_VERSION_MAJOR > 1 + + try + { + // Serialize encapsulation + ser.serialize_encapsulation(); + // Serialize the object. + ser << *p_type; + } + catch (eprosima::fastcdr::exception::Exception& /*exception*/) + { + return false; + } + + // Get the serialized length + #if FASTCDR_VERSION_MAJOR == 1 + payload->length = static_cast(ser.getSerializedDataLength()); + #else + payload->length = static_cast(ser.get_serialized_data_length()); + #endif // FASTCDR_VERSION_MAJOR == 1 + return true; + } + + bool TwistWithCovariance_PubSubType::deserialize( + SerializedPayload_t* payload, + void* data) + { + try + { + // Convert DATA to pointer of your type + TwistWithCovariance_* p_type = static_cast(data); + + // Object that manages the raw buffer. + eprosima::fastcdr::FastBuffer fastbuffer(reinterpret_cast(payload->data), payload->length); + + // Object that deserializes the data. + eprosima::fastcdr::Cdr deser(fastbuffer, eprosima::fastcdr::Cdr::DEFAULT_ENDIAN + #if FASTCDR_VERSION_MAJOR == 1 + , eprosima::fastcdr::Cdr::CdrType::DDS_CDR + #endif // FASTCDR_VERSION_MAJOR == 1 + ); + + // Deserialize encapsulation. + deser.read_encapsulation(); + payload->encapsulation = deser.endianness() == eprosima::fastcdr::Cdr::BIG_ENDIANNESS ? CDR_BE : CDR_LE; + + // Deserialize the object. + deser >> *p_type; + } + catch (eprosima::fastcdr::exception::Exception& /*exception*/) + { + return false; + } + + return true; + } + + std::function TwistWithCovariance_PubSubType::getSerializedSizeProvider( + void* data, + DataRepresentationId_t data_representation) + { + return [data, data_representation]() -> uint32_t + { + #if FASTCDR_VERSION_MAJOR == 1 + static_cast(data_representation); + return static_cast(type::getCdrSerializedSize(*static_cast(data))) + + 4u /*encapsulation*/; + #else + try + { + eprosima::fastcdr::CdrSizeCalculator calculator( + data_representation == DataRepresentationId_t::XCDR_DATA_REPRESENTATION ? + eprosima::fastcdr::CdrVersion::XCDRv1 :eprosima::fastcdr::CdrVersion::XCDRv2); + size_t current_alignment {0}; + return static_cast(calculator.calculate_serialized_size( + *static_cast(data), current_alignment)) + + 4u /*encapsulation*/; + } + catch (eprosima::fastcdr::exception::Exception& /*exception*/) + { + return 0; + } + #endif // FASTCDR_VERSION_MAJOR == 1 + }; + } + + void* TwistWithCovariance_PubSubType::createData() + { + return reinterpret_cast(new TwistWithCovariance_()); + } + + void TwistWithCovariance_PubSubType::deleteData( + void* data) + { + delete(reinterpret_cast(data)); + } + + bool TwistWithCovariance_PubSubType::getKey( + void* data, + InstanceHandle_t* handle, + bool force_md5) + { + if (!m_isGetKeyDefined) + { + return false; + } + + TwistWithCovariance_* p_type = static_cast(data); + + // Object that manages the raw buffer. + eprosima::fastcdr::FastBuffer fastbuffer(reinterpret_cast(m_keyBuffer), + geometry_msgs_msg_dds__TwistWithCovariance__max_key_cdr_typesize); + + // Object that serializes the data. + eprosima::fastcdr::Cdr ser(fastbuffer, eprosima::fastcdr::Cdr::BIG_ENDIANNESS, eprosima::fastcdr::CdrVersion::XCDRv1); + #if FASTCDR_VERSION_MAJOR == 1 + p_type->serializeKey(ser); + #else + eprosima::fastcdr::serialize_key(ser, *p_type); + #endif // FASTCDR_VERSION_MAJOR == 1 + if (force_md5 || geometry_msgs_msg_dds__TwistWithCovariance__max_key_cdr_typesize > 16) + { + m_md5.init(); + #if FASTCDR_VERSION_MAJOR == 1 + m_md5.update(m_keyBuffer, static_cast(ser.getSerializedDataLength())); + #else + m_md5.update(m_keyBuffer, static_cast(ser.get_serialized_data_length())); + #endif // FASTCDR_VERSION_MAJOR == 1 + m_md5.finalize(); + for (uint8_t i = 0; i < 16; ++i) + { + handle->value[i] = m_md5.digest[i]; + } + } + else + { + for (uint8_t i = 0; i < 16; ++i) + { + handle->value[i] = m_keyBuffer[i]; + } + } + return true; + } + + + } //End of namespace dds_ + + + } //End of namespace msg + + +} //End of namespace geometry_msgs + diff --git a/mujoco/idl_gen/geometry_msgs/TwistWithCovariancePubSubTypes.h b/mujoco/idl_gen/geometry_msgs/TwistWithCovariancePubSubTypes.h new file mode 100644 index 0000000..5302fbd --- /dev/null +++ b/mujoco/idl_gen/geometry_msgs/TwistWithCovariancePubSubTypes.h @@ -0,0 +1,197 @@ +// Copyright 2016 Proyectos y Sistemas de Mantenimiento SL (eProsima). +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +/*! + * @file TwistWithCovariancePubSubTypes.h + * This header file contains the declaration of the serialization functions. + * + * This file was generated by the tool fastddsgen. + */ + + +#ifndef _FAST_DDS_GENERATED_GEOMETRY_MSGS_MSG_DDS__TWISTWITHCOVARIANCE_PUBSUBTYPES_H_ +#define _FAST_DDS_GENERATED_GEOMETRY_MSGS_MSG_DDS__TWISTWITHCOVARIANCE_PUBSUBTYPES_H_ + +#include +#include +#include +#include +#include + +#include "TwistWithCovariance.h" + +#include "TwistPubSubTypes.h" + +#if !defined(GEN_API_VER) || (GEN_API_VER != 2) +#error \ + Generated TwistWithCovariance is not compatible with current installed Fast DDS. Please, regenerate it with fastddsgen. +#endif // GEN_API_VER + +namespace geometry_msgs +{ + namespace msg + { + namespace dds_ + { + + + + #ifndef SWIG + namespace detail { + + template + struct TwistWithCovariance__rob + { + friend constexpr typename Tag::type get( + Tag) + { + return M; + } + + }; + + struct TwistWithCovariance__f + { + typedef std::array TwistWithCovariance_::* type; + friend constexpr type get( + TwistWithCovariance__f); + }; + + template struct TwistWithCovariance__rob; + + template + inline size_t constexpr TwistWithCovariance__offset_of() + { + return ((::size_t) &reinterpret_cast((((T*)0)->*get(Tag())))); + } + + } // namespace detail + #endif // ifndef SWIG + + + /*! + * @brief This class represents the TopicDataType of the type TwistWithCovariance_ defined by the user in the IDL file. + * @ingroup TwistWithCovariance + */ + class TwistWithCovariance_PubSubType : public eprosima::fastdds::dds::TopicDataType + { + public: + + typedef TwistWithCovariance_ type; + + eProsima_user_DllExport TwistWithCovariance_PubSubType(); + + eProsima_user_DllExport ~TwistWithCovariance_PubSubType() override; + + eProsima_user_DllExport bool serialize( + void* data, + eprosima::fastrtps::rtps::SerializedPayload_t* payload) override + { + return serialize(data, payload, eprosima::fastdds::dds::DEFAULT_DATA_REPRESENTATION); + } + + eProsima_user_DllExport bool serialize( + void* data, + eprosima::fastrtps::rtps::SerializedPayload_t* payload, + eprosima::fastdds::dds::DataRepresentationId_t data_representation) override; + + eProsima_user_DllExport bool deserialize( + eprosima::fastrtps::rtps::SerializedPayload_t* payload, + void* data) override; + + eProsima_user_DllExport std::function getSerializedSizeProvider( + void* data) override + { + return getSerializedSizeProvider(data, eprosima::fastdds::dds::DEFAULT_DATA_REPRESENTATION); + } + + eProsima_user_DllExport std::function getSerializedSizeProvider( + void* data, + eprosima::fastdds::dds::DataRepresentationId_t data_representation) override; + + eProsima_user_DllExport bool getKey( + void* data, + eprosima::fastrtps::rtps::InstanceHandle_t* ihandle, + bool force_md5 = false) override; + + eProsima_user_DllExport void* createData() override; + + eProsima_user_DllExport void deleteData( + void* data) override; + + #ifdef TOPIC_DATA_TYPE_API_HAS_IS_BOUNDED + eProsima_user_DllExport inline bool is_bounded() const override + { + return true; + } + + #endif // TOPIC_DATA_TYPE_API_HAS_IS_BOUNDED + + #ifdef TOPIC_DATA_TYPE_API_HAS_IS_PLAIN + eProsima_user_DllExport inline bool is_plain() const override + { + return is_plain_xcdrv1_impl(); + } + + eProsima_user_DllExport inline bool is_plain( + eprosima::fastdds::dds::DataRepresentationId_t data_representation) const override + { + if(data_representation == eprosima::fastdds::dds::DataRepresentationId_t::XCDR2_DATA_REPRESENTATION) + { + return is_plain_xcdrv2_impl(); + } + else + { + return is_plain_xcdrv1_impl(); + } + } + + #endif // TOPIC_DATA_TYPE_API_HAS_IS_PLAIN + + #ifdef TOPIC_DATA_TYPE_API_HAS_CONSTRUCT_SAMPLE + eProsima_user_DllExport inline bool construct_sample( + void* memory) const override + { + new (memory) TwistWithCovariance_(); + return true; + } + + #endif // TOPIC_DATA_TYPE_API_HAS_CONSTRUCT_SAMPLE + + MD5 m_md5; + unsigned char* m_keyBuffer; + + private: + + static constexpr bool is_plain_xcdrv1_impl() + { + return 336ULL == + (detail::TwistWithCovariance__offset_of() + + sizeof(std::array)); + } + + static constexpr bool is_plain_xcdrv2_impl() + { + return 336ULL == + (detail::TwistWithCovariance__offset_of() + + sizeof(std::array)); + } + + }; + } + } +} + +#endif // _FAST_DDS_GENERATED_GEOMETRY_MSGS_MSG_DDS__TWISTWITHCOVARIANCE_PUBSUBTYPES_H_ + diff --git a/mujoco/idl_gen/geometry_msgs/Vector3.cxx b/mujoco/idl_gen/geometry_msgs/Vector3.cxx new file mode 100644 index 0000000..b9f24b9 --- /dev/null +++ b/mujoco/idl_gen/geometry_msgs/Vector3.cxx @@ -0,0 +1,341 @@ +// Copyright 2016 Proyectos y Sistemas de Mantenimiento SL (eProsima). +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +/*! + * @file Vector3.cpp + * This source file contains the implementation of the described types in the IDL file. + * + * This file was generated by the tool fastddsgen. + */ + +#ifdef _WIN32 +// Remove linker warning LNK4221 on Visual Studio +namespace { +char dummy; +} // namespace +#endif // _WIN32 + +#include "Vector3.h" + +#include + + +#include +using namespace eprosima::fastcdr::exception; + +#include + +namespace helper { namespace internal { + +enum class Size { + UInt8, + UInt16, + UInt32, + UInt64, +}; + +constexpr Size get_size(int s) { + return (s <= 8 ) ? Size::UInt8: + (s <= 16) ? Size::UInt16: + (s <= 32) ? Size::UInt32: Size::UInt64; +} + +template +struct FindTypeH; + +template<> +struct FindTypeH { + using type = std::uint8_t; +}; + +template<> +struct FindTypeH { + using type = std::uint16_t; +}; + +template<> +struct FindTypeH { + using type = std::uint32_t; +}; + +template<> +struct FindTypeH { + using type = std::uint64_t; +}; +} + +template +struct FindType { + using type = typename internal::FindTypeH::type; +}; +} + +#define geometry_msgs_msg_dds__Vector3__max_cdr_typesize 24ULL; + + +namespace geometry_msgs { + +namespace msg { + +namespace dds_ { + + + +Vector3_::Vector3_() +{ + // double m_x + m_x = 0.0; + // double m_y + m_y = 0.0; + // double m_z + m_z = 0.0; + +} + +Vector3_::~Vector3_() +{ +} + +Vector3_::Vector3_( + const Vector3_& x) +{ + m_x = x.m_x; + + + m_y = x.m_y; + + + m_z = x.m_z; + +} + +Vector3_::Vector3_( + Vector3_&& x) noexcept +{ + m_x = x.m_x; + + + m_y = x.m_y; + + + m_z = x.m_z; + +} + +Vector3_& Vector3_::operator =( + const Vector3_& x) +{ + m_x = x.m_x; + + + m_y = x.m_y; + + + m_z = x.m_z; + + return *this; +} + +Vector3_& Vector3_::operator =( + Vector3_&& x) noexcept +{ + m_x = x.m_x; + + + m_y = x.m_y; + + + m_z = x.m_z; + + return *this; +} + +bool Vector3_::operator ==( + const Vector3_& x) const +{ + return (m_x == x.m_x && + m_y == x.m_y && + m_z == x.m_z); +} + +bool Vector3_::operator !=( + const Vector3_& x) const +{ + return !(*this == x); +} + +size_t Vector3_::getMaxCdrSerializedSize( + size_t current_alignment) +{ + static_cast(current_alignment); + return geometry_msgs_msg_dds__Vector3__max_cdr_typesize; +} + +size_t Vector3_::getCdrSerializedSize( + const Vector3_& data, + size_t current_alignment) +{ + (void)data; + size_t initial_alignment = current_alignment; + + current_alignment += 8 + eprosima::fastcdr::Cdr::alignment(current_alignment, 8); + + + current_alignment += 8 + eprosima::fastcdr::Cdr::alignment(current_alignment, 8); + + + current_alignment += 8 + eprosima::fastcdr::Cdr::alignment(current_alignment, 8); + + + return current_alignment - initial_alignment; +} + + +void Vector3_::serialize( + eprosima::fastcdr::Cdr& scdr) const +{ + scdr << m_x; + + scdr << m_y; + + scdr << m_z; + +} + +void Vector3_::deserialize( + eprosima::fastcdr::Cdr& dcdr) +{ + dcdr >> m_x; + + + + dcdr >> m_y; + + + + dcdr >> m_z; + + +} + + +bool Vector3_::isKeyDefined() +{ + return false; +} + +void Vector3_::serializeKey( + eprosima::fastcdr::Cdr& scdr) const +{ + (void) scdr; +} + +/*! + * @brief This function sets a value in member x + * @param _x New value for member x + */ +void Vector3_::x( + double _x) +{ + m_x = _x; +} + +/*! + * @brief This function returns the value of member x + * @return Value of member x + */ +double Vector3_::x() const +{ + return m_x; +} + +/*! + * @brief This function returns a reference to member x + * @return Reference to member x + */ +double& Vector3_::x() +{ + return m_x; +} + + +/*! + * @brief This function sets a value in member y + * @param _y New value for member y + */ +void Vector3_::y( + double _y) +{ + m_y = _y; +} + +/*! + * @brief This function returns the value of member y + * @return Value of member y + */ +double Vector3_::y() const +{ + return m_y; +} + +/*! + * @brief This function returns a reference to member y + * @return Reference to member y + */ +double& Vector3_::y() +{ + return m_y; +} + + +/*! + * @brief This function sets a value in member z + * @param _z New value for member z + */ +void Vector3_::z( + double _z) +{ + m_z = _z; +} + +/*! + * @brief This function returns the value of member z + * @return Value of member z + */ +double Vector3_::z() const +{ + return m_z; +} + +/*! + * @brief This function returns a reference to member z + * @return Reference to member z + */ +double& Vector3_::z() +{ + return m_z; +} + + + + + +} // namespace dds_ + + +} // namespace msg + + +} // namespace geometry_msgs + diff --git a/mujoco/idl_gen/geometry_msgs/Vector3.h b/mujoco/idl_gen/geometry_msgs/Vector3.h new file mode 100644 index 0000000..98a76cf --- /dev/null +++ b/mujoco/idl_gen/geometry_msgs/Vector3.h @@ -0,0 +1,258 @@ +// Copyright 2016 Proyectos y Sistemas de Mantenimiento SL (eProsima). +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +/*! + * @file Vector3.h + * This header file contains the declaration of the described types in the IDL file. + * + * This file was generated by the tool fastddsgen. + */ + +#ifndef _FAST_DDS_GENERATED_GEOMETRY_MSGS_MSG_DDS__VECTOR3_H_ +#define _FAST_DDS_GENERATED_GEOMETRY_MSGS_MSG_DDS__VECTOR3_H_ + + +#include + +#include +#include +#include +#include +#include +#include + +#if defined(_WIN32) +#if defined(EPROSIMA_USER_DLL_EXPORT) +#define eProsima_user_DllExport __declspec( dllexport ) +#else +#define eProsima_user_DllExport +#endif // EPROSIMA_USER_DLL_EXPORT +#else +#define eProsima_user_DllExport +#endif // _WIN32 + +#if defined(_WIN32) +#if defined(EPROSIMA_USER_DLL_EXPORT) +#if defined(VECTOR3_SOURCE) +#define VECTOR3_DllAPI __declspec( dllexport ) +#else +#define VECTOR3_DllAPI __declspec( dllimport ) +#endif // VECTOR3_SOURCE +#else +#define VECTOR3_DllAPI +#endif // EPROSIMA_USER_DLL_EXPORT +#else +#define VECTOR3_DllAPI +#endif // _WIN32 + +namespace eprosima { +namespace fastcdr { +class Cdr; +} // namespace fastcdr +} // namespace eprosima + + + +namespace geometry_msgs { + namespace msg { + namespace dds_ { + + + /*! + * @brief This class represents the structure Vector3_ defined by the user in the IDL file. + * @ingroup Vector3 + */ + class Vector3_ + { + public: + + /*! + * @brief Default constructor. + */ + eProsima_user_DllExport Vector3_(); + + /*! + * @brief Default destructor. + */ + eProsima_user_DllExport ~Vector3_(); + + /*! + * @brief Copy constructor. + * @param x Reference to the object geometry_msgs::msg::dds_::Vector3_ that will be copied. + */ + eProsima_user_DllExport Vector3_( + const Vector3_& x); + + /*! + * @brief Move constructor. + * @param x Reference to the object geometry_msgs::msg::dds_::Vector3_ that will be copied. + */ + eProsima_user_DllExport Vector3_( + Vector3_&& x) noexcept; + + /*! + * @brief Copy assignment. + * @param x Reference to the object geometry_msgs::msg::dds_::Vector3_ that will be copied. + */ + eProsima_user_DllExport Vector3_& operator =( + const Vector3_& x); + + /*! + * @brief Move assignment. + * @param x Reference to the object geometry_msgs::msg::dds_::Vector3_ that will be copied. + */ + eProsima_user_DllExport Vector3_& operator =( + Vector3_&& x) noexcept; + + /*! + * @brief Comparison operator. + * @param x geometry_msgs::msg::dds_::Vector3_ object to compare. + */ + eProsima_user_DllExport bool operator ==( + const Vector3_& x) const; + + /*! + * @brief Comparison operator. + * @param x geometry_msgs::msg::dds_::Vector3_ object to compare. + */ + eProsima_user_DllExport bool operator !=( + const Vector3_& x) const; + + /*! + * @brief This function sets a value in member x + * @param _x New value for member x + */ + eProsima_user_DllExport void x( + double _x); + + /*! + * @brief This function returns the value of member x + * @return Value of member x + */ + eProsima_user_DllExport double x() const; + + /*! + * @brief This function returns a reference to member x + * @return Reference to member x + */ + eProsima_user_DllExport double& x(); + + + /*! + * @brief This function sets a value in member y + * @param _y New value for member y + */ + eProsima_user_DllExport void y( + double _y); + + /*! + * @brief This function returns the value of member y + * @return Value of member y + */ + eProsima_user_DllExport double y() const; + + /*! + * @brief This function returns a reference to member y + * @return Reference to member y + */ + eProsima_user_DllExport double& y(); + + + /*! + * @brief This function sets a value in member z + * @param _z New value for member z + */ + eProsima_user_DllExport void z( + double _z); + + /*! + * @brief This function returns the value of member z + * @return Value of member z + */ + eProsima_user_DllExport double z() const; + + /*! + * @brief This function returns a reference to member z + * @return Reference to member z + */ + eProsima_user_DllExport double& z(); + + + /*! + * @brief This function returns the maximum serialized size of an object + * depending on the buffer alignment. + * @param current_alignment Buffer alignment. + * @return Maximum serialized size. + */ + eProsima_user_DllExport static size_t getMaxCdrSerializedSize( + size_t current_alignment = 0); + + /*! + * @brief This function returns the serialized size of a data depending on the buffer alignment. + * @param data Data which is calculated its serialized size. + * @param current_alignment Buffer alignment. + * @return Serialized size. + */ + eProsima_user_DllExport static size_t getCdrSerializedSize( + const geometry_msgs::msg::dds_::Vector3_& data, + size_t current_alignment = 0); + + + + /*! + * @brief This function serializes an object using CDR serialization. + * @param cdr CDR serialization object. + */ + eProsima_user_DllExport void serialize( + eprosima::fastcdr::Cdr& cdr) const; + + /*! + * @brief This function deserializes an object using CDR serialization. + * @param cdr CDR serialization object. + */ + eProsima_user_DllExport void deserialize( + eprosima::fastcdr::Cdr& cdr); + + + + + /*! + * @brief This function tells you if the Key has been defined for this type + */ + eProsima_user_DllExport static bool isKeyDefined(); + + /*! + * @brief This function serializes the key members of an object using CDR serialization. + * @param cdr CDR serialization object. + */ + eProsima_user_DllExport void serializeKey( + eprosima::fastcdr::Cdr& cdr) const; + + + private: + + double m_x; + double m_y; + double m_z; + + }; + + } // namespace dds_ + } // namespace msg +} // namespace geometry_msgs + +#endif // _FAST_DDS_GENERATED_GEOMETRY_MSGS_MSG_DDS__VECTOR3_H_ + + + diff --git a/mujoco/idl_gen/geometry_msgs/Vector3CdrAux.hpp b/mujoco/idl_gen/geometry_msgs/Vector3CdrAux.hpp new file mode 100644 index 0000000..cf1af8c --- /dev/null +++ b/mujoco/idl_gen/geometry_msgs/Vector3CdrAux.hpp @@ -0,0 +1,51 @@ +// Copyright 2016 Proyectos y Sistemas de Mantenimiento SL (eProsima). +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +/*! + * @file Vector3CdrAux.hpp + * This source file contains some definitions of CDR related functions. + * + * This file was generated by the tool fastddsgen. + */ + +#ifndef _FAST_DDS_GENERATED_GEOMETRY_MSGS_MSG_DDS__VECTOR3CDRAUX_HPP_ +#define _FAST_DDS_GENERATED_GEOMETRY_MSGS_MSG_DDS__VECTOR3CDRAUX_HPP_ + +#include "Vector3.h" + +constexpr uint32_t geometry_msgs_msg_dds__Vector3__max_cdr_typesize {24UL}; +constexpr uint32_t geometry_msgs_msg_dds__Vector3__max_key_cdr_typesize {0UL}; + + +namespace eprosima { +namespace fastcdr { + +class Cdr; +class CdrSizeCalculator; + + + +eProsima_user_DllExport void serialize_key( + eprosima::fastcdr::Cdr& scdr, + const geometry_msgs::msg::dds_::Vector3_& data); + + + + + +} // namespace fastcdr +} // namespace eprosima + +#endif // _FAST_DDS_GENERATED_GEOMETRY_MSGS_MSG_DDS__VECTOR3CDRAUX_HPP_ + diff --git a/mujoco/idl_gen/geometry_msgs/Vector3CdrAux.ipp b/mujoco/idl_gen/geometry_msgs/Vector3CdrAux.ipp new file mode 100644 index 0000000..9bf2b9a --- /dev/null +++ b/mujoco/idl_gen/geometry_msgs/Vector3CdrAux.ipp @@ -0,0 +1,147 @@ +// Copyright 2016 Proyectos y Sistemas de Mantenimiento SL (eProsima). +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +/*! + * @file Vector3CdrAux.ipp + * This source file contains some declarations of CDR related functions. + * + * This file was generated by the tool fastddsgen. + */ + +#ifndef _FAST_DDS_GENERATED_GEOMETRY_MSGS_MSG_DDS__VECTOR3CDRAUX_IPP_ +#define _FAST_DDS_GENERATED_GEOMETRY_MSGS_MSG_DDS__VECTOR3CDRAUX_IPP_ + +#include "Vector3CdrAux.hpp" + +#include +#include + + +#include +using namespace eprosima::fastcdr::exception; + +namespace eprosima { +namespace fastcdr { + + + +template<> +eProsima_user_DllExport size_t calculate_serialized_size( + eprosima::fastcdr::CdrSizeCalculator& calculator, + const geometry_msgs::msg::dds_::Vector3_& data, + size_t& current_alignment) +{ + using namespace geometry_msgs::msg::dds_; + + static_cast(data); + + eprosima::fastcdr::EncodingAlgorithmFlag previous_encoding = calculator.get_encoding(); + size_t calculated_size {calculator.begin_calculate_type_serialized_size( + eprosima::fastcdr::CdrVersion::XCDRv2 == calculator.get_cdr_version() ? + eprosima::fastcdr::EncodingAlgorithmFlag::PLAIN_CDR2 : + eprosima::fastcdr::EncodingAlgorithmFlag::PLAIN_CDR, + current_alignment)}; + + + calculated_size += calculator.calculate_member_serialized_size(eprosima::fastcdr::MemberId(0), + data.x(), current_alignment); + + calculated_size += calculator.calculate_member_serialized_size(eprosima::fastcdr::MemberId(1), + data.y(), current_alignment); + + calculated_size += calculator.calculate_member_serialized_size(eprosima::fastcdr::MemberId(2), + data.z(), current_alignment); + + + calculated_size += calculator.end_calculate_type_serialized_size(previous_encoding, current_alignment); + + return calculated_size; +} + +template<> +eProsima_user_DllExport void serialize( + eprosima::fastcdr::Cdr& scdr, + const geometry_msgs::msg::dds_::Vector3_& data) +{ + using namespace geometry_msgs::msg::dds_; + + eprosima::fastcdr::Cdr::state current_state(scdr); + scdr.begin_serialize_type(current_state, + eprosima::fastcdr::CdrVersion::XCDRv2 == scdr.get_cdr_version() ? + eprosima::fastcdr::EncodingAlgorithmFlag::PLAIN_CDR2 : + eprosima::fastcdr::EncodingAlgorithmFlag::PLAIN_CDR); + + scdr + << eprosima::fastcdr::MemberId(0) << data.x() + << eprosima::fastcdr::MemberId(1) << data.y() + << eprosima::fastcdr::MemberId(2) << data.z() +; + scdr.end_serialize_type(current_state); +} + +template<> +eProsima_user_DllExport void deserialize( + eprosima::fastcdr::Cdr& cdr, + geometry_msgs::msg::dds_::Vector3_& data) +{ + using namespace geometry_msgs::msg::dds_; + + cdr.deserialize_type(eprosima::fastcdr::CdrVersion::XCDRv2 == cdr.get_cdr_version() ? + eprosima::fastcdr::EncodingAlgorithmFlag::PLAIN_CDR2 : + eprosima::fastcdr::EncodingAlgorithmFlag::PLAIN_CDR, + [&data](eprosima::fastcdr::Cdr& dcdr, const eprosima::fastcdr::MemberId& mid) -> bool + { + bool ret_value = true; + switch (mid.id) + { + case 0: + dcdr >> data.x(); + break; + + case 1: + dcdr >> data.y(); + break; + + case 2: + dcdr >> data.z(); + break; + + default: + ret_value = false; + break; + } + return ret_value; + }); +} + +void serialize_key( + eprosima::fastcdr::Cdr& scdr, + const geometry_msgs::msg::dds_::Vector3_& data) +{ + using namespace geometry_msgs::msg::dds_; + + static_cast(scdr); + static_cast(data); +} + + + + + + +} // namespace fastcdr +} // namespace eprosima + +#endif // _FAST_DDS_GENERATED_GEOMETRY_MSGS_MSG_DDS__VECTOR3CDRAUX_IPP_ + diff --git a/mujoco/idl_gen/geometry_msgs/Vector3PubSubTypes.cxx b/mujoco/idl_gen/geometry_msgs/Vector3PubSubTypes.cxx new file mode 100644 index 0000000..a8a59b9 --- /dev/null +++ b/mujoco/idl_gen/geometry_msgs/Vector3PubSubTypes.cxx @@ -0,0 +1,233 @@ +// Copyright 2016 Proyectos y Sistemas de Mantenimiento SL (eProsima). +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +/*! + * @file Vector3PubSubTypes.cpp + * This header file contains the implementation of the serialization functions. + * + * This file was generated by the tool fastddsgen. + */ + + +#include + +#include "Vector3PubSubTypes.h" +#include "Vector3CdrAux.hpp" + +using SerializedPayload_t = eprosima::fastrtps::rtps::SerializedPayload_t; +using InstanceHandle_t = eprosima::fastrtps::rtps::InstanceHandle_t; +using DataRepresentationId_t = eprosima::fastdds::dds::DataRepresentationId_t; + +namespace geometry_msgs { + namespace msg { + namespace dds_ { + + + Vector3_PubSubType::Vector3_PubSubType() + { + setName("geometry_msgs::msg::dds_::Vector3_"); + uint32_t type_size = + #if FASTCDR_VERSION_MAJOR == 1 + static_cast(Vector3_::getMaxCdrSerializedSize()); + #else + geometry_msgs_msg_dds__Vector3__max_cdr_typesize; + #endif + type_size += static_cast(eprosima::fastcdr::Cdr::alignment(type_size, 4)); /* possible submessage alignment */ + m_typeSize = type_size + 4; /*encapsulation*/ + m_isGetKeyDefined = false; + uint32_t keyLength = geometry_msgs_msg_dds__Vector3__max_key_cdr_typesize > 16 ? geometry_msgs_msg_dds__Vector3__max_key_cdr_typesize : 16; + m_keyBuffer = reinterpret_cast(malloc(keyLength)); + memset(m_keyBuffer, 0, keyLength); + } + + Vector3_PubSubType::~Vector3_PubSubType() + { + if (m_keyBuffer != nullptr) + { + free(m_keyBuffer); + } + } + + bool Vector3_PubSubType::serialize( + void* data, + SerializedPayload_t* payload, + DataRepresentationId_t data_representation) + { + Vector3_* p_type = static_cast(data); + + // Object that manages the raw buffer. + eprosima::fastcdr::FastBuffer fastbuffer(reinterpret_cast(payload->data), payload->max_size); + // Object that serializes the data. + eprosima::fastcdr::Cdr ser(fastbuffer, eprosima::fastcdr::Cdr::DEFAULT_ENDIAN, + data_representation == DataRepresentationId_t::XCDR_DATA_REPRESENTATION ? + eprosima::fastcdr::CdrVersion::XCDRv1 : eprosima::fastcdr::CdrVersion::XCDRv2); + payload->encapsulation = ser.endianness() == eprosima::fastcdr::Cdr::BIG_ENDIANNESS ? CDR_BE : CDR_LE; + #if FASTCDR_VERSION_MAJOR > 1 + ser.set_encoding_flag( + data_representation == DataRepresentationId_t::XCDR_DATA_REPRESENTATION ? + eprosima::fastcdr::EncodingAlgorithmFlag::PLAIN_CDR : + eprosima::fastcdr::EncodingAlgorithmFlag::PLAIN_CDR2); + #endif // FASTCDR_VERSION_MAJOR > 1 + + try + { + // Serialize encapsulation + ser.serialize_encapsulation(); + // Serialize the object. + ser << *p_type; + } + catch (eprosima::fastcdr::exception::Exception& /*exception*/) + { + return false; + } + + // Get the serialized length + #if FASTCDR_VERSION_MAJOR == 1 + payload->length = static_cast(ser.getSerializedDataLength()); + #else + payload->length = static_cast(ser.get_serialized_data_length()); + #endif // FASTCDR_VERSION_MAJOR == 1 + return true; + } + + bool Vector3_PubSubType::deserialize( + SerializedPayload_t* payload, + void* data) + { + try + { + // Convert DATA to pointer of your type + Vector3_* p_type = static_cast(data); + + // Object that manages the raw buffer. + eprosima::fastcdr::FastBuffer fastbuffer(reinterpret_cast(payload->data), payload->length); + + // Object that deserializes the data. + eprosima::fastcdr::Cdr deser(fastbuffer, eprosima::fastcdr::Cdr::DEFAULT_ENDIAN + #if FASTCDR_VERSION_MAJOR == 1 + , eprosima::fastcdr::Cdr::CdrType::DDS_CDR + #endif // FASTCDR_VERSION_MAJOR == 1 + ); + + // Deserialize encapsulation. + deser.read_encapsulation(); + payload->encapsulation = deser.endianness() == eprosima::fastcdr::Cdr::BIG_ENDIANNESS ? CDR_BE : CDR_LE; + + // Deserialize the object. + deser >> *p_type; + } + catch (eprosima::fastcdr::exception::Exception& /*exception*/) + { + return false; + } + + return true; + } + + std::function Vector3_PubSubType::getSerializedSizeProvider( + void* data, + DataRepresentationId_t data_representation) + { + return [data, data_representation]() -> uint32_t + { + #if FASTCDR_VERSION_MAJOR == 1 + static_cast(data_representation); + return static_cast(type::getCdrSerializedSize(*static_cast(data))) + + 4u /*encapsulation*/; + #else + try + { + eprosima::fastcdr::CdrSizeCalculator calculator( + data_representation == DataRepresentationId_t::XCDR_DATA_REPRESENTATION ? + eprosima::fastcdr::CdrVersion::XCDRv1 :eprosima::fastcdr::CdrVersion::XCDRv2); + size_t current_alignment {0}; + return static_cast(calculator.calculate_serialized_size( + *static_cast(data), current_alignment)) + + 4u /*encapsulation*/; + } + catch (eprosima::fastcdr::exception::Exception& /*exception*/) + { + return 0; + } + #endif // FASTCDR_VERSION_MAJOR == 1 + }; + } + + void* Vector3_PubSubType::createData() + { + return reinterpret_cast(new Vector3_()); + } + + void Vector3_PubSubType::deleteData( + void* data) + { + delete(reinterpret_cast(data)); + } + + bool Vector3_PubSubType::getKey( + void* data, + InstanceHandle_t* handle, + bool force_md5) + { + if (!m_isGetKeyDefined) + { + return false; + } + + Vector3_* p_type = static_cast(data); + + // Object that manages the raw buffer. + eprosima::fastcdr::FastBuffer fastbuffer(reinterpret_cast(m_keyBuffer), + geometry_msgs_msg_dds__Vector3__max_key_cdr_typesize); + + // Object that serializes the data. + eprosima::fastcdr::Cdr ser(fastbuffer, eprosima::fastcdr::Cdr::BIG_ENDIANNESS, eprosima::fastcdr::CdrVersion::XCDRv1); + #if FASTCDR_VERSION_MAJOR == 1 + p_type->serializeKey(ser); + #else + eprosima::fastcdr::serialize_key(ser, *p_type); + #endif // FASTCDR_VERSION_MAJOR == 1 + if (force_md5 || geometry_msgs_msg_dds__Vector3__max_key_cdr_typesize > 16) + { + m_md5.init(); + #if FASTCDR_VERSION_MAJOR == 1 + m_md5.update(m_keyBuffer, static_cast(ser.getSerializedDataLength())); + #else + m_md5.update(m_keyBuffer, static_cast(ser.get_serialized_data_length())); + #endif // FASTCDR_VERSION_MAJOR == 1 + m_md5.finalize(); + for (uint8_t i = 0; i < 16; ++i) + { + handle->value[i] = m_md5.digest[i]; + } + } + else + { + for (uint8_t i = 0; i < 16; ++i) + { + handle->value[i] = m_keyBuffer[i]; + } + } + return true; + } + + + } //End of namespace dds_ + + + } //End of namespace msg + + +} //End of namespace geometry_msgs + diff --git a/mujoco/idl_gen/geometry_msgs/Vector3PubSubTypes.h b/mujoco/idl_gen/geometry_msgs/Vector3PubSubTypes.h new file mode 100644 index 0000000..a7045eb --- /dev/null +++ b/mujoco/idl_gen/geometry_msgs/Vector3PubSubTypes.h @@ -0,0 +1,196 @@ +// Copyright 2016 Proyectos y Sistemas de Mantenimiento SL (eProsima). +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +/*! + * @file Vector3PubSubTypes.h + * This header file contains the declaration of the serialization functions. + * + * This file was generated by the tool fastddsgen. + */ + + +#ifndef _FAST_DDS_GENERATED_GEOMETRY_MSGS_MSG_DDS__VECTOR3_PUBSUBTYPES_H_ +#define _FAST_DDS_GENERATED_GEOMETRY_MSGS_MSG_DDS__VECTOR3_PUBSUBTYPES_H_ + +#include +#include +#include +#include +#include + +#include "Vector3.h" + + +#if !defined(GEN_API_VER) || (GEN_API_VER != 2) +#error \ + Generated Vector3 is not compatible with current installed Fast DDS. Please, regenerate it with fastddsgen. +#endif // GEN_API_VER + +namespace geometry_msgs +{ + namespace msg + { + namespace dds_ + { + + + + #ifndef SWIG + namespace detail { + + template + struct Vector3__rob + { + friend constexpr typename Tag::type get( + Tag) + { + return M; + } + + }; + + struct Vector3__f + { + typedef double Vector3_::* type; + friend constexpr type get( + Vector3__f); + }; + + template struct Vector3__rob; + + template + inline size_t constexpr Vector3__offset_of() + { + return ((::size_t) &reinterpret_cast((((T*)0)->*get(Tag())))); + } + + } // namespace detail + #endif // ifndef SWIG + + + /*! + * @brief This class represents the TopicDataType of the type Vector3_ defined by the user in the IDL file. + * @ingroup Vector3 + */ + class Vector3_PubSubType : public eprosima::fastdds::dds::TopicDataType + { + public: + + typedef Vector3_ type; + + eProsima_user_DllExport Vector3_PubSubType(); + + eProsima_user_DllExport ~Vector3_PubSubType() override; + + eProsima_user_DllExport bool serialize( + void* data, + eprosima::fastrtps::rtps::SerializedPayload_t* payload) override + { + return serialize(data, payload, eprosima::fastdds::dds::DEFAULT_DATA_REPRESENTATION); + } + + eProsima_user_DllExport bool serialize( + void* data, + eprosima::fastrtps::rtps::SerializedPayload_t* payload, + eprosima::fastdds::dds::DataRepresentationId_t data_representation) override; + + eProsima_user_DllExport bool deserialize( + eprosima::fastrtps::rtps::SerializedPayload_t* payload, + void* data) override; + + eProsima_user_DllExport std::function getSerializedSizeProvider( + void* data) override + { + return getSerializedSizeProvider(data, eprosima::fastdds::dds::DEFAULT_DATA_REPRESENTATION); + } + + eProsima_user_DllExport std::function getSerializedSizeProvider( + void* data, + eprosima::fastdds::dds::DataRepresentationId_t data_representation) override; + + eProsima_user_DllExport bool getKey( + void* data, + eprosima::fastrtps::rtps::InstanceHandle_t* ihandle, + bool force_md5 = false) override; + + eProsima_user_DllExport void* createData() override; + + eProsima_user_DllExport void deleteData( + void* data) override; + + #ifdef TOPIC_DATA_TYPE_API_HAS_IS_BOUNDED + eProsima_user_DllExport inline bool is_bounded() const override + { + return true; + } + + #endif // TOPIC_DATA_TYPE_API_HAS_IS_BOUNDED + + #ifdef TOPIC_DATA_TYPE_API_HAS_IS_PLAIN + eProsima_user_DllExport inline bool is_plain() const override + { + return is_plain_xcdrv1_impl(); + } + + eProsima_user_DllExport inline bool is_plain( + eprosima::fastdds::dds::DataRepresentationId_t data_representation) const override + { + if(data_representation == eprosima::fastdds::dds::DataRepresentationId_t::XCDR2_DATA_REPRESENTATION) + { + return is_plain_xcdrv2_impl(); + } + else + { + return is_plain_xcdrv1_impl(); + } + } + + #endif // TOPIC_DATA_TYPE_API_HAS_IS_PLAIN + + #ifdef TOPIC_DATA_TYPE_API_HAS_CONSTRUCT_SAMPLE + eProsima_user_DllExport inline bool construct_sample( + void* memory) const override + { + new (memory) Vector3_(); + return true; + } + + #endif // TOPIC_DATA_TYPE_API_HAS_CONSTRUCT_SAMPLE + + MD5 m_md5; + unsigned char* m_keyBuffer; + + private: + + static constexpr bool is_plain_xcdrv1_impl() + { + return 24ULL == + (detail::Vector3__offset_of() + + sizeof(double)); + } + + static constexpr bool is_plain_xcdrv2_impl() + { + return 24ULL == + (detail::Vector3__offset_of() + + sizeof(double)); + } + + }; + } + } +} + +#endif // _FAST_DDS_GENERATED_GEOMETRY_MSGS_MSG_DDS__VECTOR3_PUBSUBTYPES_H_ + diff --git a/mujoco/idl_gen/nav_msgs/Odometry.cxx b/mujoco/idl_gen/nav_msgs/Odometry.cxx new file mode 100644 index 0000000..9e7ef5e --- /dev/null +++ b/mujoco/idl_gen/nav_msgs/Odometry.cxx @@ -0,0 +1,443 @@ +// Copyright 2016 Proyectos y Sistemas de Mantenimiento SL (eProsima). +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +/*! + * @file Odometry.cpp + * This source file contains the implementation of the described types in the IDL file. + * + * This file was generated by the tool fastddsgen. + */ + +#ifdef _WIN32 +// Remove linker warning LNK4221 on Visual Studio +namespace { +char dummy; +} // namespace +#endif // _WIN32 + +#include "Odometry.h" + +#include + + +#include +using namespace eprosima::fastcdr::exception; + +#include + +namespace helper { namespace internal { + +enum class Size { + UInt8, + UInt16, + UInt32, + UInt64, +}; + +constexpr Size get_size(int s) { + return (s <= 8 ) ? Size::UInt8: + (s <= 16) ? Size::UInt16: + (s <= 32) ? Size::UInt32: Size::UInt64; +} + +template +struct FindTypeH; + +template<> +struct FindTypeH { + using type = std::uint8_t; +}; + +template<> +struct FindTypeH { + using type = std::uint16_t; +}; + +template<> +struct FindTypeH { + using type = std::uint32_t; +}; + +template<> +struct FindTypeH { + using type = std::uint64_t; +}; +} + +template +struct FindType { + using type = typename internal::FindTypeH::type; +}; +} + +#define geometry_msgs_msg_dds__Quaternion__max_cdr_typesize 32ULL; +#define geometry_msgs_msg_dds__Vector3__max_cdr_typesize 24ULL; +#define builtin_interfaces_msg_dds__Time__max_cdr_typesize 8ULL; +#define geometry_msgs_msg_dds__TwistWithCovariance__max_cdr_typesize 336ULL; +#define std_msgs_msg_dds__Header__max_cdr_typesize 268ULL; +#define geometry_msgs_msg_dds__Point__max_cdr_typesize 24ULL; +#define geometry_msgs_msg_dds__Pose__max_cdr_typesize 56ULL; +#define geometry_msgs_msg_dds__Twist__max_cdr_typesize 48ULL; +#define nav_msgs_msg_dds__Odometry__max_cdr_typesize 1208ULL; +#define geometry_msgs_msg_dds__PoseWithCovariance__max_cdr_typesize 344ULL; + + +namespace nav_msgs { + +namespace msg { + +namespace dds_ { + + + +Odometry_::Odometry_() +{ + // std_msgs::msg::dds_::Header_ m_header + + // /type_d() m_child_frame_id + + // geometry_msgs::msg::dds_::PoseWithCovariance_ m_pose + + // geometry_msgs::msg::dds_::TwistWithCovariance_ m_twist + + +} + +Odometry_::~Odometry_() +{ +} + +Odometry_::Odometry_( + const Odometry_& x) +{ + m_header = x.m_header; + + + m_child_frame_id = x.m_child_frame_id; + + + m_pose = x.m_pose; + + + m_twist = x.m_twist; + +} + +Odometry_::Odometry_( + Odometry_&& x) noexcept +{ + m_header = std::move(x.m_header); + + + m_child_frame_id = std::move(x.m_child_frame_id); + + + m_pose = std::move(x.m_pose); + + + m_twist = std::move(x.m_twist); + +} + +Odometry_& Odometry_::operator =( + const Odometry_& x) +{ + m_header = x.m_header; + + + m_child_frame_id = x.m_child_frame_id; + + + m_pose = x.m_pose; + + + m_twist = x.m_twist; + + return *this; +} + +Odometry_& Odometry_::operator =( + Odometry_&& x) noexcept +{ + m_header = std::move(x.m_header); + + + m_child_frame_id = std::move(x.m_child_frame_id); + + + m_pose = std::move(x.m_pose); + + + m_twist = std::move(x.m_twist); + + return *this; +} + +bool Odometry_::operator ==( + const Odometry_& x) const +{ + return (m_header == x.m_header && + m_child_frame_id == x.m_child_frame_id && + m_pose == x.m_pose && + m_twist == x.m_twist); +} + +bool Odometry_::operator !=( + const Odometry_& x) const +{ + return !(*this == x); +} + +size_t Odometry_::getMaxCdrSerializedSize( + size_t current_alignment) +{ + static_cast(current_alignment); + return nav_msgs_msg_dds__Odometry__max_cdr_typesize; +} + +size_t Odometry_::getCdrSerializedSize( + const Odometry_& data, + size_t current_alignment) +{ + (void)data; + size_t initial_alignment = current_alignment; + + current_alignment += std_msgs::msg::dds_::Header_::getCdrSerializedSize(data.header(), current_alignment); + + + current_alignment += 4 + eprosima::fastcdr::Cdr::alignment(current_alignment, 4) + data.child_frame_id().size() + 1; + + + current_alignment += geometry_msgs::msg::dds_::PoseWithCovariance_::getCdrSerializedSize(data.pose(), current_alignment); + + + current_alignment += geometry_msgs::msg::dds_::TwistWithCovariance_::getCdrSerializedSize(data.twist(), current_alignment); + + + return current_alignment - initial_alignment; +} + + +void Odometry_::serialize( + eprosima::fastcdr::Cdr& scdr) const +{ + scdr << m_header; + + scdr << m_child_frame_id.c_str(); + + scdr << m_pose; + + scdr << m_twist; + +} + +void Odometry_::deserialize( + eprosima::fastcdr::Cdr& dcdr) +{ + dcdr >> m_header; + + + + dcdr >> m_child_frame_id; + + + + dcdr >> m_pose; + + + + dcdr >> m_twist; + + +} + + +bool Odometry_::isKeyDefined() +{ + return false; +} + +void Odometry_::serializeKey( + eprosima::fastcdr::Cdr& scdr) const +{ + (void) scdr; +} + +/*! + * @brief This function copies the value in member header + * @param _header New value to be copied in member header + */ +void Odometry_::header( + const std_msgs::msg::dds_::Header_& _header) +{ + m_header = _header; +} + +/*! + * @brief This function moves the value in member header + * @param _header New value to be moved in member header + */ +void Odometry_::header( + std_msgs::msg::dds_::Header_&& _header) +{ + m_header = std::move(_header); +} + +/*! + * @brief This function returns a constant reference to member header + * @return Constant reference to member header + */ +const std_msgs::msg::dds_::Header_& Odometry_::header() const +{ + return m_header; +} + +/*! + * @brief This function returns a reference to member header + * @return Reference to member header + */ +std_msgs::msg::dds_::Header_& Odometry_::header() +{ + return m_header; +} + + +/*! + * @brief This function copies the value in member child_frame_id + * @param _child_frame_id New value to be copied in member child_frame_id + */ +void Odometry_::child_frame_id( + const std::string& _child_frame_id) +{ + m_child_frame_id = _child_frame_id; +} + +/*! + * @brief This function moves the value in member child_frame_id + * @param _child_frame_id New value to be moved in member child_frame_id + */ +void Odometry_::child_frame_id( + std::string&& _child_frame_id) +{ + m_child_frame_id = std::move(_child_frame_id); +} + +/*! + * @brief This function returns a constant reference to member child_frame_id + * @return Constant reference to member child_frame_id + */ +const std::string& Odometry_::child_frame_id() const +{ + return m_child_frame_id; +} + +/*! + * @brief This function returns a reference to member child_frame_id + * @return Reference to member child_frame_id + */ +std::string& Odometry_::child_frame_id() +{ + return m_child_frame_id; +} + + +/*! + * @brief This function copies the value in member pose + * @param _pose New value to be copied in member pose + */ +void Odometry_::pose( + const geometry_msgs::msg::dds_::PoseWithCovariance_& _pose) +{ + m_pose = _pose; +} + +/*! + * @brief This function moves the value in member pose + * @param _pose New value to be moved in member pose + */ +void Odometry_::pose( + geometry_msgs::msg::dds_::PoseWithCovariance_&& _pose) +{ + m_pose = std::move(_pose); +} + +/*! + * @brief This function returns a constant reference to member pose + * @return Constant reference to member pose + */ +const geometry_msgs::msg::dds_::PoseWithCovariance_& Odometry_::pose() const +{ + return m_pose; +} + +/*! + * @brief This function returns a reference to member pose + * @return Reference to member pose + */ +geometry_msgs::msg::dds_::PoseWithCovariance_& Odometry_::pose() +{ + return m_pose; +} + + +/*! + * @brief This function copies the value in member twist + * @param _twist New value to be copied in member twist + */ +void Odometry_::twist( + const geometry_msgs::msg::dds_::TwistWithCovariance_& _twist) +{ + m_twist = _twist; +} + +/*! + * @brief This function moves the value in member twist + * @param _twist New value to be moved in member twist + */ +void Odometry_::twist( + geometry_msgs::msg::dds_::TwistWithCovariance_&& _twist) +{ + m_twist = std::move(_twist); +} + +/*! + * @brief This function returns a constant reference to member twist + * @return Constant reference to member twist + */ +const geometry_msgs::msg::dds_::TwistWithCovariance_& Odometry_::twist() const +{ + return m_twist; +} + +/*! + * @brief This function returns a reference to member twist + * @return Reference to member twist + */ +geometry_msgs::msg::dds_::TwistWithCovariance_& Odometry_::twist() +{ + return m_twist; +} + + + + + +} // namespace dds_ + + +} // namespace msg + + +} // namespace nav_msgs + diff --git a/mujoco/idl_gen/nav_msgs/Odometry.h b/mujoco/idl_gen/nav_msgs/Odometry.h new file mode 100644 index 0000000..b7a4fb9 --- /dev/null +++ b/mujoco/idl_gen/nav_msgs/Odometry.h @@ -0,0 +1,310 @@ +// Copyright 2016 Proyectos y Sistemas de Mantenimiento SL (eProsima). +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +/*! + * @file Odometry.h + * This header file contains the declaration of the described types in the IDL file. + * + * This file was generated by the tool fastddsgen. + */ + +#ifndef _FAST_DDS_GENERATED_NAV_MSGS_MSG_DDS__ODOMETRY_H_ +#define _FAST_DDS_GENERATED_NAV_MSGS_MSG_DDS__ODOMETRY_H_ + +#include "Header.h" +#include "PoseWithCovariance.h" +#include "TwistWithCovariance.h" + +#include + +#include +#include +#include +#include +#include +#include + +#if defined(_WIN32) +#if defined(EPROSIMA_USER_DLL_EXPORT) +#define eProsima_user_DllExport __declspec( dllexport ) +#else +#define eProsima_user_DllExport +#endif // EPROSIMA_USER_DLL_EXPORT +#else +#define eProsima_user_DllExport +#endif // _WIN32 + +#if defined(_WIN32) +#if defined(EPROSIMA_USER_DLL_EXPORT) +#if defined(ODOMETRY_SOURCE) +#define ODOMETRY_DllAPI __declspec( dllexport ) +#else +#define ODOMETRY_DllAPI __declspec( dllimport ) +#endif // ODOMETRY_SOURCE +#else +#define ODOMETRY_DllAPI +#endif // EPROSIMA_USER_DLL_EXPORT +#else +#define ODOMETRY_DllAPI +#endif // _WIN32 + +namespace eprosima { +namespace fastcdr { +class Cdr; +} // namespace fastcdr +} // namespace eprosima + + + +namespace nav_msgs { + namespace msg { + namespace dds_ { + + + /*! + * @brief This class represents the structure Odometry_ defined by the user in the IDL file. + * @ingroup Odometry + */ + class Odometry_ + { + public: + + /*! + * @brief Default constructor. + */ + eProsima_user_DllExport Odometry_(); + + /*! + * @brief Default destructor. + */ + eProsima_user_DllExport ~Odometry_(); + + /*! + * @brief Copy constructor. + * @param x Reference to the object nav_msgs::msg::dds_::Odometry_ that will be copied. + */ + eProsima_user_DllExport Odometry_( + const Odometry_& x); + + /*! + * @brief Move constructor. + * @param x Reference to the object nav_msgs::msg::dds_::Odometry_ that will be copied. + */ + eProsima_user_DllExport Odometry_( + Odometry_&& x) noexcept; + + /*! + * @brief Copy assignment. + * @param x Reference to the object nav_msgs::msg::dds_::Odometry_ that will be copied. + */ + eProsima_user_DllExport Odometry_& operator =( + const Odometry_& x); + + /*! + * @brief Move assignment. + * @param x Reference to the object nav_msgs::msg::dds_::Odometry_ that will be copied. + */ + eProsima_user_DllExport Odometry_& operator =( + Odometry_&& x) noexcept; + + /*! + * @brief Comparison operator. + * @param x nav_msgs::msg::dds_::Odometry_ object to compare. + */ + eProsima_user_DllExport bool operator ==( + const Odometry_& x) const; + + /*! + * @brief Comparison operator. + * @param x nav_msgs::msg::dds_::Odometry_ object to compare. + */ + eProsima_user_DllExport bool operator !=( + const Odometry_& x) const; + + /*! + * @brief This function copies the value in member header + * @param _header New value to be copied in member header + */ + eProsima_user_DllExport void header( + const std_msgs::msg::dds_::Header_& _header); + + /*! + * @brief This function moves the value in member header + * @param _header New value to be moved in member header + */ + eProsima_user_DllExport void header( + std_msgs::msg::dds_::Header_&& _header); + + /*! + * @brief This function returns a constant reference to member header + * @return Constant reference to member header + */ + eProsima_user_DllExport const std_msgs::msg::dds_::Header_& header() const; + + /*! + * @brief This function returns a reference to member header + * @return Reference to member header + */ + eProsima_user_DllExport std_msgs::msg::dds_::Header_& header(); + + + /*! + * @brief This function copies the value in member child_frame_id + * @param _child_frame_id New value to be copied in member child_frame_id + */ + eProsima_user_DllExport void child_frame_id( + const std::string& _child_frame_id); + + /*! + * @brief This function moves the value in member child_frame_id + * @param _child_frame_id New value to be moved in member child_frame_id + */ + eProsima_user_DllExport void child_frame_id( + std::string&& _child_frame_id); + + /*! + * @brief This function returns a constant reference to member child_frame_id + * @return Constant reference to member child_frame_id + */ + eProsima_user_DllExport const std::string& child_frame_id() const; + + /*! + * @brief This function returns a reference to member child_frame_id + * @return Reference to member child_frame_id + */ + eProsima_user_DllExport std::string& child_frame_id(); + + + /*! + * @brief This function copies the value in member pose + * @param _pose New value to be copied in member pose + */ + eProsima_user_DllExport void pose( + const geometry_msgs::msg::dds_::PoseWithCovariance_& _pose); + + /*! + * @brief This function moves the value in member pose + * @param _pose New value to be moved in member pose + */ + eProsima_user_DllExport void pose( + geometry_msgs::msg::dds_::PoseWithCovariance_&& _pose); + + /*! + * @brief This function returns a constant reference to member pose + * @return Constant reference to member pose + */ + eProsima_user_DllExport const geometry_msgs::msg::dds_::PoseWithCovariance_& pose() const; + + /*! + * @brief This function returns a reference to member pose + * @return Reference to member pose + */ + eProsima_user_DllExport geometry_msgs::msg::dds_::PoseWithCovariance_& pose(); + + + /*! + * @brief This function copies the value in member twist + * @param _twist New value to be copied in member twist + */ + eProsima_user_DllExport void twist( + const geometry_msgs::msg::dds_::TwistWithCovariance_& _twist); + + /*! + * @brief This function moves the value in member twist + * @param _twist New value to be moved in member twist + */ + eProsima_user_DllExport void twist( + geometry_msgs::msg::dds_::TwistWithCovariance_&& _twist); + + /*! + * @brief This function returns a constant reference to member twist + * @return Constant reference to member twist + */ + eProsima_user_DllExport const geometry_msgs::msg::dds_::TwistWithCovariance_& twist() const; + + /*! + * @brief This function returns a reference to member twist + * @return Reference to member twist + */ + eProsima_user_DllExport geometry_msgs::msg::dds_::TwistWithCovariance_& twist(); + + + /*! + * @brief This function returns the maximum serialized size of an object + * depending on the buffer alignment. + * @param current_alignment Buffer alignment. + * @return Maximum serialized size. + */ + eProsima_user_DllExport static size_t getMaxCdrSerializedSize( + size_t current_alignment = 0); + + /*! + * @brief This function returns the serialized size of a data depending on the buffer alignment. + * @param data Data which is calculated its serialized size. + * @param current_alignment Buffer alignment. + * @return Serialized size. + */ + eProsima_user_DllExport static size_t getCdrSerializedSize( + const nav_msgs::msg::dds_::Odometry_& data, + size_t current_alignment = 0); + + + + /*! + * @brief This function serializes an object using CDR serialization. + * @param cdr CDR serialization object. + */ + eProsima_user_DllExport void serialize( + eprosima::fastcdr::Cdr& cdr) const; + + /*! + * @brief This function deserializes an object using CDR serialization. + * @param cdr CDR serialization object. + */ + eProsima_user_DllExport void deserialize( + eprosima::fastcdr::Cdr& cdr); + + + + + /*! + * @brief This function tells you if the Key has been defined for this type + */ + eProsima_user_DllExport static bool isKeyDefined(); + + /*! + * @brief This function serializes the key members of an object using CDR serialization. + * @param cdr CDR serialization object. + */ + eProsima_user_DllExport void serializeKey( + eprosima::fastcdr::Cdr& cdr) const; + + + private: + + std_msgs::msg::dds_::Header_ m_header; + std::string m_child_frame_id; + geometry_msgs::msg::dds_::PoseWithCovariance_ m_pose; + geometry_msgs::msg::dds_::TwistWithCovariance_ m_twist; + + }; + + } // namespace dds_ + } // namespace msg +} // namespace nav_msgs + +#endif // _FAST_DDS_GENERATED_NAV_MSGS_MSG_DDS__ODOMETRY_H_ + + + diff --git a/mujoco/idl_gen/nav_msgs/OdometryCdrAux.hpp b/mujoco/idl_gen/nav_msgs/OdometryCdrAux.hpp new file mode 100644 index 0000000..73d078a --- /dev/null +++ b/mujoco/idl_gen/nav_msgs/OdometryCdrAux.hpp @@ -0,0 +1,52 @@ +// Copyright 2016 Proyectos y Sistemas de Mantenimiento SL (eProsima). +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +/*! + * @file OdometryCdrAux.hpp + * This source file contains some definitions of CDR related functions. + * + * This file was generated by the tool fastddsgen. + */ + +#ifndef _FAST_DDS_GENERATED_NAV_MSGS_MSG_DDS__ODOMETRYCDRAUX_HPP_ +#define _FAST_DDS_GENERATED_NAV_MSGS_MSG_DDS__ODOMETRYCDRAUX_HPP_ + +#include "Odometry.h" + +constexpr uint32_t nav_msgs_msg_dds__Odometry__max_cdr_typesize {1208UL}; +constexpr uint32_t nav_msgs_msg_dds__Odometry__max_key_cdr_typesize {0UL}; + + + +namespace eprosima { +namespace fastcdr { + +class Cdr; +class CdrSizeCalculator; + + + +eProsima_user_DllExport void serialize_key( + eprosima::fastcdr::Cdr& scdr, + const nav_msgs::msg::dds_::Odometry_& data); + + + + + +} // namespace fastcdr +} // namespace eprosima + +#endif // _FAST_DDS_GENERATED_NAV_MSGS_MSG_DDS__ODOMETRYCDRAUX_HPP_ + diff --git a/mujoco/idl_gen/nav_msgs/OdometryCdrAux.ipp b/mujoco/idl_gen/nav_msgs/OdometryCdrAux.ipp new file mode 100644 index 0000000..9c2acb5 --- /dev/null +++ b/mujoco/idl_gen/nav_msgs/OdometryCdrAux.ipp @@ -0,0 +1,155 @@ +// Copyright 2016 Proyectos y Sistemas de Mantenimiento SL (eProsima). +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +/*! + * @file OdometryCdrAux.ipp + * This source file contains some declarations of CDR related functions. + * + * This file was generated by the tool fastddsgen. + */ + +#ifndef _FAST_DDS_GENERATED_NAV_MSGS_MSG_DDS__ODOMETRYCDRAUX_IPP_ +#define _FAST_DDS_GENERATED_NAV_MSGS_MSG_DDS__ODOMETRYCDRAUX_IPP_ + +#include "OdometryCdrAux.hpp" + +#include +#include + + +#include +using namespace eprosima::fastcdr::exception; + +namespace eprosima { +namespace fastcdr { + + + +template<> +eProsima_user_DllExport size_t calculate_serialized_size( + eprosima::fastcdr::CdrSizeCalculator& calculator, + const nav_msgs::msg::dds_::Odometry_& data, + size_t& current_alignment) +{ + using namespace nav_msgs::msg::dds_; + + static_cast(data); + + eprosima::fastcdr::EncodingAlgorithmFlag previous_encoding = calculator.get_encoding(); + size_t calculated_size {calculator.begin_calculate_type_serialized_size( + eprosima::fastcdr::CdrVersion::XCDRv2 == calculator.get_cdr_version() ? + eprosima::fastcdr::EncodingAlgorithmFlag::PLAIN_CDR2 : + eprosima::fastcdr::EncodingAlgorithmFlag::PLAIN_CDR, + current_alignment)}; + + + calculated_size += calculator.calculate_member_serialized_size(eprosima::fastcdr::MemberId(0), + data.header(), current_alignment); + + calculated_size += calculator.calculate_member_serialized_size(eprosima::fastcdr::MemberId(1), + data.child_frame_id(), current_alignment); + + calculated_size += calculator.calculate_member_serialized_size(eprosima::fastcdr::MemberId(2), + data.pose(), current_alignment); + + calculated_size += calculator.calculate_member_serialized_size(eprosima::fastcdr::MemberId(3), + data.twist(), current_alignment); + + + calculated_size += calculator.end_calculate_type_serialized_size(previous_encoding, current_alignment); + + return calculated_size; +} + +template<> +eProsima_user_DllExport void serialize( + eprosima::fastcdr::Cdr& scdr, + const nav_msgs::msg::dds_::Odometry_& data) +{ + using namespace nav_msgs::msg::dds_; + + eprosima::fastcdr::Cdr::state current_state(scdr); + scdr.begin_serialize_type(current_state, + eprosima::fastcdr::CdrVersion::XCDRv2 == scdr.get_cdr_version() ? + eprosima::fastcdr::EncodingAlgorithmFlag::PLAIN_CDR2 : + eprosima::fastcdr::EncodingAlgorithmFlag::PLAIN_CDR); + + scdr + << eprosima::fastcdr::MemberId(0) << data.header() + << eprosima::fastcdr::MemberId(1) << data.child_frame_id() + << eprosima::fastcdr::MemberId(2) << data.pose() + << eprosima::fastcdr::MemberId(3) << data.twist() +; + scdr.end_serialize_type(current_state); +} + +template<> +eProsima_user_DllExport void deserialize( + eprosima::fastcdr::Cdr& cdr, + nav_msgs::msg::dds_::Odometry_& data) +{ + using namespace nav_msgs::msg::dds_; + + cdr.deserialize_type(eprosima::fastcdr::CdrVersion::XCDRv2 == cdr.get_cdr_version() ? + eprosima::fastcdr::EncodingAlgorithmFlag::PLAIN_CDR2 : + eprosima::fastcdr::EncodingAlgorithmFlag::PLAIN_CDR, + [&data](eprosima::fastcdr::Cdr& dcdr, const eprosima::fastcdr::MemberId& mid) -> bool + { + bool ret_value = true; + switch (mid.id) + { + case 0: + dcdr >> data.header(); + break; + + case 1: + dcdr >> data.child_frame_id(); + break; + + case 2: + dcdr >> data.pose(); + break; + + case 3: + dcdr >> data.twist(); + break; + + default: + ret_value = false; + break; + } + return ret_value; + }); +} + +void serialize_key( + eprosima::fastcdr::Cdr& scdr, + const nav_msgs::msg::dds_::Odometry_& data) +{ + using namespace nav_msgs::msg::dds_; + + static_cast(scdr); + static_cast(data); +} + + + + + + +} // namespace fastcdr +} // namespace eprosima + +#endif // _FAST_DDS_GENERATED_NAV_MSGS_MSG_DDS__ODOMETRYCDRAUX_IPP_ + diff --git a/mujoco/idl_gen/nav_msgs/OdometryPubSubTypes.cxx b/mujoco/idl_gen/nav_msgs/OdometryPubSubTypes.cxx new file mode 100644 index 0000000..2a2e45d --- /dev/null +++ b/mujoco/idl_gen/nav_msgs/OdometryPubSubTypes.cxx @@ -0,0 +1,233 @@ +// Copyright 2016 Proyectos y Sistemas de Mantenimiento SL (eProsima). +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +/*! + * @file OdometryPubSubTypes.cpp + * This header file contains the implementation of the serialization functions. + * + * This file was generated by the tool fastddsgen. + */ + + +#include + +#include "OdometryPubSubTypes.h" +#include "OdometryCdrAux.hpp" + +using SerializedPayload_t = eprosima::fastrtps::rtps::SerializedPayload_t; +using InstanceHandle_t = eprosima::fastrtps::rtps::InstanceHandle_t; +using DataRepresentationId_t = eprosima::fastdds::dds::DataRepresentationId_t; + +namespace nav_msgs { + namespace msg { + namespace dds_ { + + + Odometry_PubSubType::Odometry_PubSubType() + { + setName("nav_msgs::msg::dds_::Odometry_"); + uint32_t type_size = + #if FASTCDR_VERSION_MAJOR == 1 + static_cast(Odometry_::getMaxCdrSerializedSize()); + #else + nav_msgs_msg_dds__Odometry__max_cdr_typesize; + #endif + type_size += static_cast(eprosima::fastcdr::Cdr::alignment(type_size, 4)); /* possible submessage alignment */ + m_typeSize = type_size + 4; /*encapsulation*/ + m_isGetKeyDefined = false; + uint32_t keyLength = nav_msgs_msg_dds__Odometry__max_key_cdr_typesize > 16 ? nav_msgs_msg_dds__Odometry__max_key_cdr_typesize : 16; + m_keyBuffer = reinterpret_cast(malloc(keyLength)); + memset(m_keyBuffer, 0, keyLength); + } + + Odometry_PubSubType::~Odometry_PubSubType() + { + if (m_keyBuffer != nullptr) + { + free(m_keyBuffer); + } + } + + bool Odometry_PubSubType::serialize( + void* data, + SerializedPayload_t* payload, + DataRepresentationId_t data_representation) + { + Odometry_* p_type = static_cast(data); + + // Object that manages the raw buffer. + eprosima::fastcdr::FastBuffer fastbuffer(reinterpret_cast(payload->data), payload->max_size); + // Object that serializes the data. + eprosima::fastcdr::Cdr ser(fastbuffer, eprosima::fastcdr::Cdr::DEFAULT_ENDIAN, + data_representation == DataRepresentationId_t::XCDR_DATA_REPRESENTATION ? + eprosima::fastcdr::CdrVersion::XCDRv1 : eprosima::fastcdr::CdrVersion::XCDRv2); + payload->encapsulation = ser.endianness() == eprosima::fastcdr::Cdr::BIG_ENDIANNESS ? CDR_BE : CDR_LE; + #if FASTCDR_VERSION_MAJOR > 1 + ser.set_encoding_flag( + data_representation == DataRepresentationId_t::XCDR_DATA_REPRESENTATION ? + eprosima::fastcdr::EncodingAlgorithmFlag::PLAIN_CDR : + eprosima::fastcdr::EncodingAlgorithmFlag::PLAIN_CDR2); + #endif // FASTCDR_VERSION_MAJOR > 1 + + try + { + // Serialize encapsulation + ser.serialize_encapsulation(); + // Serialize the object. + ser << *p_type; + } + catch (eprosima::fastcdr::exception::Exception& /*exception*/) + { + return false; + } + + // Get the serialized length + #if FASTCDR_VERSION_MAJOR == 1 + payload->length = static_cast(ser.getSerializedDataLength()); + #else + payload->length = static_cast(ser.get_serialized_data_length()); + #endif // FASTCDR_VERSION_MAJOR == 1 + return true; + } + + bool Odometry_PubSubType::deserialize( + SerializedPayload_t* payload, + void* data) + { + try + { + // Convert DATA to pointer of your type + Odometry_* p_type = static_cast(data); + + // Object that manages the raw buffer. + eprosima::fastcdr::FastBuffer fastbuffer(reinterpret_cast(payload->data), payload->length); + + // Object that deserializes the data. + eprosima::fastcdr::Cdr deser(fastbuffer, eprosima::fastcdr::Cdr::DEFAULT_ENDIAN + #if FASTCDR_VERSION_MAJOR == 1 + , eprosima::fastcdr::Cdr::CdrType::DDS_CDR + #endif // FASTCDR_VERSION_MAJOR == 1 + ); + + // Deserialize encapsulation. + deser.read_encapsulation(); + payload->encapsulation = deser.endianness() == eprosima::fastcdr::Cdr::BIG_ENDIANNESS ? CDR_BE : CDR_LE; + + // Deserialize the object. + deser >> *p_type; + } + catch (eprosima::fastcdr::exception::Exception& /*exception*/) + { + return false; + } + + return true; + } + + std::function Odometry_PubSubType::getSerializedSizeProvider( + void* data, + DataRepresentationId_t data_representation) + { + return [data, data_representation]() -> uint32_t + { + #if FASTCDR_VERSION_MAJOR == 1 + static_cast(data_representation); + return static_cast(type::getCdrSerializedSize(*static_cast(data))) + + 4u /*encapsulation*/; + #else + try + { + eprosima::fastcdr::CdrSizeCalculator calculator( + data_representation == DataRepresentationId_t::XCDR_DATA_REPRESENTATION ? + eprosima::fastcdr::CdrVersion::XCDRv1 :eprosima::fastcdr::CdrVersion::XCDRv2); + size_t current_alignment {0}; + return static_cast(calculator.calculate_serialized_size( + *static_cast(data), current_alignment)) + + 4u /*encapsulation*/; + } + catch (eprosima::fastcdr::exception::Exception& /*exception*/) + { + return 0; + } + #endif // FASTCDR_VERSION_MAJOR == 1 + }; + } + + void* Odometry_PubSubType::createData() + { + return reinterpret_cast(new Odometry_()); + } + + void Odometry_PubSubType::deleteData( + void* data) + { + delete(reinterpret_cast(data)); + } + + bool Odometry_PubSubType::getKey( + void* data, + InstanceHandle_t* handle, + bool force_md5) + { + if (!m_isGetKeyDefined) + { + return false; + } + + Odometry_* p_type = static_cast(data); + + // Object that manages the raw buffer. + eprosima::fastcdr::FastBuffer fastbuffer(reinterpret_cast(m_keyBuffer), + nav_msgs_msg_dds__Odometry__max_key_cdr_typesize); + + // Object that serializes the data. + eprosima::fastcdr::Cdr ser(fastbuffer, eprosima::fastcdr::Cdr::BIG_ENDIANNESS, eprosima::fastcdr::CdrVersion::XCDRv1); + #if FASTCDR_VERSION_MAJOR == 1 + p_type->serializeKey(ser); + #else + eprosima::fastcdr::serialize_key(ser, *p_type); + #endif // FASTCDR_VERSION_MAJOR == 1 + if (force_md5 || nav_msgs_msg_dds__Odometry__max_key_cdr_typesize > 16) + { + m_md5.init(); + #if FASTCDR_VERSION_MAJOR == 1 + m_md5.update(m_keyBuffer, static_cast(ser.getSerializedDataLength())); + #else + m_md5.update(m_keyBuffer, static_cast(ser.get_serialized_data_length())); + #endif // FASTCDR_VERSION_MAJOR == 1 + m_md5.finalize(); + for (uint8_t i = 0; i < 16; ++i) + { + handle->value[i] = m_md5.digest[i]; + } + } + else + { + for (uint8_t i = 0; i < 16; ++i) + { + handle->value[i] = m_keyBuffer[i]; + } + } + return true; + } + + + } //End of namespace dds_ + + + } //End of namespace msg + + +} //End of namespace nav_msgs + diff --git a/mujoco/idl_gen/nav_msgs/OdometryPubSubTypes.h b/mujoco/idl_gen/nav_msgs/OdometryPubSubTypes.h new file mode 100644 index 0000000..edce4c8 --- /dev/null +++ b/mujoco/idl_gen/nav_msgs/OdometryPubSubTypes.h @@ -0,0 +1,144 @@ +// Copyright 2016 Proyectos y Sistemas de Mantenimiento SL (eProsima). +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +/*! + * @file OdometryPubSubTypes.h + * This header file contains the declaration of the serialization functions. + * + * This file was generated by the tool fastddsgen. + */ + + +#ifndef _FAST_DDS_GENERATED_NAV_MSGS_MSG_DDS__ODOMETRY_PUBSUBTYPES_H_ +#define _FAST_DDS_GENERATED_NAV_MSGS_MSG_DDS__ODOMETRY_PUBSUBTYPES_H_ + +#include +#include +#include +#include +#include + +#include "Odometry.h" + +#include "HeaderPubSubTypes.h" +#include "PoseWithCovariancePubSubTypes.h" +#include "TwistWithCovariancePubSubTypes.h" + +#if !defined(GEN_API_VER) || (GEN_API_VER != 2) +#error \ + Generated Odometry is not compatible with current installed Fast DDS. Please, regenerate it with fastddsgen. +#endif // GEN_API_VER + +namespace nav_msgs +{ + namespace msg + { + namespace dds_ + { + + + + /*! + * @brief This class represents the TopicDataType of the type Odometry_ defined by the user in the IDL file. + * @ingroup Odometry + */ + class Odometry_PubSubType : public eprosima::fastdds::dds::TopicDataType + { + public: + + typedef Odometry_ type; + + eProsima_user_DllExport Odometry_PubSubType(); + + eProsima_user_DllExport ~Odometry_PubSubType() override; + + eProsima_user_DllExport bool serialize( + void* data, + eprosima::fastrtps::rtps::SerializedPayload_t* payload) override + { + return serialize(data, payload, eprosima::fastdds::dds::DEFAULT_DATA_REPRESENTATION); + } + + eProsima_user_DllExport bool serialize( + void* data, + eprosima::fastrtps::rtps::SerializedPayload_t* payload, + eprosima::fastdds::dds::DataRepresentationId_t data_representation) override; + + eProsima_user_DllExport bool deserialize( + eprosima::fastrtps::rtps::SerializedPayload_t* payload, + void* data) override; + + eProsima_user_DllExport std::function getSerializedSizeProvider( + void* data) override + { + return getSerializedSizeProvider(data, eprosima::fastdds::dds::DEFAULT_DATA_REPRESENTATION); + } + + eProsima_user_DllExport std::function getSerializedSizeProvider( + void* data, + eprosima::fastdds::dds::DataRepresentationId_t data_representation) override; + + eProsima_user_DllExport bool getKey( + void* data, + eprosima::fastrtps::rtps::InstanceHandle_t* ihandle, + bool force_md5 = false) override; + + eProsima_user_DllExport void* createData() override; + + eProsima_user_DllExport void deleteData( + void* data) override; + + #ifdef TOPIC_DATA_TYPE_API_HAS_IS_BOUNDED + eProsima_user_DllExport inline bool is_bounded() const override + { + return false; + } + + #endif // TOPIC_DATA_TYPE_API_HAS_IS_BOUNDED + + #ifdef TOPIC_DATA_TYPE_API_HAS_IS_PLAIN + eProsima_user_DllExport inline bool is_plain() const override + { + return false; + } + + eProsima_user_DllExport inline bool is_plain( + eprosima::fastdds::dds::DataRepresentationId_t data_representation) const override + { + static_cast(data_representation); + return false; + } + + #endif // TOPIC_DATA_TYPE_API_HAS_IS_PLAIN + + #ifdef TOPIC_DATA_TYPE_API_HAS_CONSTRUCT_SAMPLE + eProsima_user_DllExport inline bool construct_sample( + void* memory) const override + { + static_cast(memory); + return false; + } + + #endif // TOPIC_DATA_TYPE_API_HAS_CONSTRUCT_SAMPLE + + MD5 m_md5; + unsigned char* m_keyBuffer; + + }; + } + } +} + +#endif // _FAST_DDS_GENERATED_NAV_MSGS_MSG_DDS__ODOMETRY_PUBSUBTYPES_H_ + diff --git a/mujoco/idl_gen/std_msgs/Header.cxx b/mujoco/idl_gen/std_msgs/Header.cxx new file mode 100644 index 0000000..fc56cc5 --- /dev/null +++ b/mujoco/idl_gen/std_msgs/Header.cxx @@ -0,0 +1,309 @@ +// Copyright 2016 Proyectos y Sistemas de Mantenimiento SL (eProsima). +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +/*! + * @file Header.cpp + * This source file contains the implementation of the described types in the IDL file. + * + * This file was generated by the tool fastddsgen. + */ + +#ifdef _WIN32 +// Remove linker warning LNK4221 on Visual Studio +namespace { +char dummy; +} // namespace +#endif // _WIN32 + +#include "Header.h" + +#include + + +#include +using namespace eprosima::fastcdr::exception; + +#include + +namespace helper { namespace internal { + +enum class Size { + UInt8, + UInt16, + UInt32, + UInt64, +}; + +constexpr Size get_size(int s) { + return (s <= 8 ) ? Size::UInt8: + (s <= 16) ? Size::UInt16: + (s <= 32) ? Size::UInt32: Size::UInt64; +} + +template +struct FindTypeH; + +template<> +struct FindTypeH { + using type = std::uint8_t; +}; + +template<> +struct FindTypeH { + using type = std::uint16_t; +}; + +template<> +struct FindTypeH { + using type = std::uint32_t; +}; + +template<> +struct FindTypeH { + using type = std::uint64_t; +}; +} + +template +struct FindType { + using type = typename internal::FindTypeH::type; +}; +} + +#define builtin_interfaces_msg_dds__Time__max_cdr_typesize 8ULL; +#define std_msgs_msg_dds__Header__max_cdr_typesize 268ULL; + + +namespace std_msgs { + +namespace msg { + +namespace dds_ { + + + +Header_::Header_() +{ + // builtin_interfaces::msg::dds_::Time_ m_stamp + + // /type_d() m_frame_id + + +} + +Header_::~Header_() +{ +} + +Header_::Header_( + const Header_& x) +{ + m_stamp = x.m_stamp; + + + m_frame_id = x.m_frame_id; + +} + +Header_::Header_( + Header_&& x) noexcept +{ + m_stamp = std::move(x.m_stamp); + + + m_frame_id = std::move(x.m_frame_id); + +} + +Header_& Header_::operator =( + const Header_& x) +{ + m_stamp = x.m_stamp; + + + m_frame_id = x.m_frame_id; + + return *this; +} + +Header_& Header_::operator =( + Header_&& x) noexcept +{ + m_stamp = std::move(x.m_stamp); + + + m_frame_id = std::move(x.m_frame_id); + + return *this; +} + +bool Header_::operator ==( + const Header_& x) const +{ + return (m_stamp == x.m_stamp && + m_frame_id == x.m_frame_id); +} + +bool Header_::operator !=( + const Header_& x) const +{ + return !(*this == x); +} + +size_t Header_::getMaxCdrSerializedSize( + size_t current_alignment) +{ + static_cast(current_alignment); + return std_msgs_msg_dds__Header__max_cdr_typesize; +} + +size_t Header_::getCdrSerializedSize( + const Header_& data, + size_t current_alignment) +{ + (void)data; + size_t initial_alignment = current_alignment; + + current_alignment += builtin_interfaces::msg::dds_::Time_::getCdrSerializedSize(data.stamp(), current_alignment); + + + current_alignment += 4 + eprosima::fastcdr::Cdr::alignment(current_alignment, 4) + data.frame_id().size() + 1; + + + return current_alignment - initial_alignment; +} + + +void Header_::serialize( + eprosima::fastcdr::Cdr& scdr) const +{ + scdr << m_stamp; + + scdr << m_frame_id.c_str(); + +} + +void Header_::deserialize( + eprosima::fastcdr::Cdr& dcdr) +{ + dcdr >> m_stamp; + + + + dcdr >> m_frame_id; + + +} + + +bool Header_::isKeyDefined() +{ + return false; +} + +void Header_::serializeKey( + eprosima::fastcdr::Cdr& scdr) const +{ + (void) scdr; +} + +/*! + * @brief This function copies the value in member stamp + * @param _stamp New value to be copied in member stamp + */ +void Header_::stamp( + const builtin_interfaces::msg::dds_::Time_& _stamp) +{ + m_stamp = _stamp; +} + +/*! + * @brief This function moves the value in member stamp + * @param _stamp New value to be moved in member stamp + */ +void Header_::stamp( + builtin_interfaces::msg::dds_::Time_&& _stamp) +{ + m_stamp = std::move(_stamp); +} + +/*! + * @brief This function returns a constant reference to member stamp + * @return Constant reference to member stamp + */ +const builtin_interfaces::msg::dds_::Time_& Header_::stamp() const +{ + return m_stamp; +} + +/*! + * @brief This function returns a reference to member stamp + * @return Reference to member stamp + */ +builtin_interfaces::msg::dds_::Time_& Header_::stamp() +{ + return m_stamp; +} + + +/*! + * @brief This function copies the value in member frame_id + * @param _frame_id New value to be copied in member frame_id + */ +void Header_::frame_id( + const std::string& _frame_id) +{ + m_frame_id = _frame_id; +} + +/*! + * @brief This function moves the value in member frame_id + * @param _frame_id New value to be moved in member frame_id + */ +void Header_::frame_id( + std::string&& _frame_id) +{ + m_frame_id = std::move(_frame_id); +} + +/*! + * @brief This function returns a constant reference to member frame_id + * @return Constant reference to member frame_id + */ +const std::string& Header_::frame_id() const +{ + return m_frame_id; +} + +/*! + * @brief This function returns a reference to member frame_id + * @return Reference to member frame_id + */ +std::string& Header_::frame_id() +{ + return m_frame_id; +} + + + + + +} // namespace dds_ + + +} // namespace msg + + +} // namespace std_msgs + diff --git a/mujoco/idl_gen/std_msgs/Header.h b/mujoco/idl_gen/std_msgs/Header.h new file mode 100644 index 0000000..91bdf94 --- /dev/null +++ b/mujoco/idl_gen/std_msgs/Header.h @@ -0,0 +1,252 @@ +// Copyright 2016 Proyectos y Sistemas de Mantenimiento SL (eProsima). +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +/*! + * @file Header.h + * This header file contains the declaration of the described types in the IDL file. + * + * This file was generated by the tool fastddsgen. + */ + +#ifndef _FAST_DDS_GENERATED_STD_MSGS_MSG_DDS__HEADER_H_ +#define _FAST_DDS_GENERATED_STD_MSGS_MSG_DDS__HEADER_H_ + +#include "Time.h" + +#include + +#include +#include +#include +#include +#include +#include + +#if defined(_WIN32) +#if defined(EPROSIMA_USER_DLL_EXPORT) +#define eProsima_user_DllExport __declspec( dllexport ) +#else +#define eProsima_user_DllExport +#endif // EPROSIMA_USER_DLL_EXPORT +#else +#define eProsima_user_DllExport +#endif // _WIN32 + +#if defined(_WIN32) +#if defined(EPROSIMA_USER_DLL_EXPORT) +#if defined(HEADER_SOURCE) +#define HEADER_DllAPI __declspec( dllexport ) +#else +#define HEADER_DllAPI __declspec( dllimport ) +#endif // HEADER_SOURCE +#else +#define HEADER_DllAPI +#endif // EPROSIMA_USER_DLL_EXPORT +#else +#define HEADER_DllAPI +#endif // _WIN32 + +namespace eprosima { +namespace fastcdr { +class Cdr; +} // namespace fastcdr +} // namespace eprosima + + + +namespace std_msgs { + namespace msg { + namespace dds_ { + + + /*! + * @brief This class represents the structure Header_ defined by the user in the IDL file. + * @ingroup Header + */ + class Header_ + { + public: + + /*! + * @brief Default constructor. + */ + eProsima_user_DllExport Header_(); + + /*! + * @brief Default destructor. + */ + eProsima_user_DllExport ~Header_(); + + /*! + * @brief Copy constructor. + * @param x Reference to the object std_msgs::msg::dds_::Header_ that will be copied. + */ + eProsima_user_DllExport Header_( + const Header_& x); + + /*! + * @brief Move constructor. + * @param x Reference to the object std_msgs::msg::dds_::Header_ that will be copied. + */ + eProsima_user_DllExport Header_( + Header_&& x) noexcept; + + /*! + * @brief Copy assignment. + * @param x Reference to the object std_msgs::msg::dds_::Header_ that will be copied. + */ + eProsima_user_DllExport Header_& operator =( + const Header_& x); + + /*! + * @brief Move assignment. + * @param x Reference to the object std_msgs::msg::dds_::Header_ that will be copied. + */ + eProsima_user_DllExport Header_& operator =( + Header_&& x) noexcept; + + /*! + * @brief Comparison operator. + * @param x std_msgs::msg::dds_::Header_ object to compare. + */ + eProsima_user_DllExport bool operator ==( + const Header_& x) const; + + /*! + * @brief Comparison operator. + * @param x std_msgs::msg::dds_::Header_ object to compare. + */ + eProsima_user_DllExport bool operator !=( + const Header_& x) const; + + /*! + * @brief This function copies the value in member stamp + * @param _stamp New value to be copied in member stamp + */ + eProsima_user_DllExport void stamp( + const builtin_interfaces::msg::dds_::Time_& _stamp); + + /*! + * @brief This function moves the value in member stamp + * @param _stamp New value to be moved in member stamp + */ + eProsima_user_DllExport void stamp( + builtin_interfaces::msg::dds_::Time_&& _stamp); + + /*! + * @brief This function returns a constant reference to member stamp + * @return Constant reference to member stamp + */ + eProsima_user_DllExport const builtin_interfaces::msg::dds_::Time_& stamp() const; + + /*! + * @brief This function returns a reference to member stamp + * @return Reference to member stamp + */ + eProsima_user_DllExport builtin_interfaces::msg::dds_::Time_& stamp(); + + + /*! + * @brief This function copies the value in member frame_id + * @param _frame_id New value to be copied in member frame_id + */ + eProsima_user_DllExport void frame_id( + const std::string& _frame_id); + + /*! + * @brief This function moves the value in member frame_id + * @param _frame_id New value to be moved in member frame_id + */ + eProsima_user_DllExport void frame_id( + std::string&& _frame_id); + + /*! + * @brief This function returns a constant reference to member frame_id + * @return Constant reference to member frame_id + */ + eProsima_user_DllExport const std::string& frame_id() const; + + /*! + * @brief This function returns a reference to member frame_id + * @return Reference to member frame_id + */ + eProsima_user_DllExport std::string& frame_id(); + + + /*! + * @brief This function returns the maximum serialized size of an object + * depending on the buffer alignment. + * @param current_alignment Buffer alignment. + * @return Maximum serialized size. + */ + eProsima_user_DllExport static size_t getMaxCdrSerializedSize( + size_t current_alignment = 0); + + /*! + * @brief This function returns the serialized size of a data depending on the buffer alignment. + * @param data Data which is calculated its serialized size. + * @param current_alignment Buffer alignment. + * @return Serialized size. + */ + eProsima_user_DllExport static size_t getCdrSerializedSize( + const std_msgs::msg::dds_::Header_& data, + size_t current_alignment = 0); + + + + /*! + * @brief This function serializes an object using CDR serialization. + * @param cdr CDR serialization object. + */ + eProsima_user_DllExport void serialize( + eprosima::fastcdr::Cdr& cdr) const; + + /*! + * @brief This function deserializes an object using CDR serialization. + * @param cdr CDR serialization object. + */ + eProsima_user_DllExport void deserialize( + eprosima::fastcdr::Cdr& cdr); + + + + + /*! + * @brief This function tells you if the Key has been defined for this type + */ + eProsima_user_DllExport static bool isKeyDefined(); + + /*! + * @brief This function serializes the key members of an object using CDR serialization. + * @param cdr CDR serialization object. + */ + eProsima_user_DllExport void serializeKey( + eprosima::fastcdr::Cdr& cdr) const; + + + private: + + builtin_interfaces::msg::dds_::Time_ m_stamp; + std::string m_frame_id; + + }; + + } // namespace dds_ + } // namespace msg +} // namespace std_msgs + +#endif // _FAST_DDS_GENERATED_STD_MSGS_MSG_DDS__HEADER_H_ + + + diff --git a/mujoco/idl_gen/std_msgs/HeaderCdrAux.hpp b/mujoco/idl_gen/std_msgs/HeaderCdrAux.hpp new file mode 100644 index 0000000..f7fed1f --- /dev/null +++ b/mujoco/idl_gen/std_msgs/HeaderCdrAux.hpp @@ -0,0 +1,51 @@ +// Copyright 2016 Proyectos y Sistemas de Mantenimiento SL (eProsima). +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +/*! + * @file HeaderCdrAux.hpp + * This source file contains some definitions of CDR related functions. + * + * This file was generated by the tool fastddsgen. + */ + +#ifndef _FAST_DDS_GENERATED_STD_MSGS_MSG_DDS__HEADERCDRAUX_HPP_ +#define _FAST_DDS_GENERATED_STD_MSGS_MSG_DDS__HEADERCDRAUX_HPP_ + +#include "Header.h" + +constexpr uint32_t std_msgs_msg_dds__Header__max_cdr_typesize {268UL}; +constexpr uint32_t std_msgs_msg_dds__Header__max_key_cdr_typesize {0UL}; + + +namespace eprosima { +namespace fastcdr { + +class Cdr; +class CdrSizeCalculator; + + + +eProsima_user_DllExport void serialize_key( + eprosima::fastcdr::Cdr& scdr, + const std_msgs::msg::dds_::Header_& data); + + + + + +} // namespace fastcdr +} // namespace eprosima + +#endif // _FAST_DDS_GENERATED_STD_MSGS_MSG_DDS__HEADERCDRAUX_HPP_ + diff --git a/mujoco/idl_gen/std_msgs/HeaderCdrAux.ipp b/mujoco/idl_gen/std_msgs/HeaderCdrAux.ipp new file mode 100644 index 0000000..628d73f --- /dev/null +++ b/mujoco/idl_gen/std_msgs/HeaderCdrAux.ipp @@ -0,0 +1,139 @@ +// Copyright 2016 Proyectos y Sistemas de Mantenimiento SL (eProsima). +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +/*! + * @file HeaderCdrAux.ipp + * This source file contains some declarations of CDR related functions. + * + * This file was generated by the tool fastddsgen. + */ + +#ifndef _FAST_DDS_GENERATED_STD_MSGS_MSG_DDS__HEADERCDRAUX_IPP_ +#define _FAST_DDS_GENERATED_STD_MSGS_MSG_DDS__HEADERCDRAUX_IPP_ + +#include "HeaderCdrAux.hpp" + +#include +#include + + +#include +using namespace eprosima::fastcdr::exception; + +namespace eprosima { +namespace fastcdr { + + + +template<> +eProsima_user_DllExport size_t calculate_serialized_size( + eprosima::fastcdr::CdrSizeCalculator& calculator, + const std_msgs::msg::dds_::Header_& data, + size_t& current_alignment) +{ + using namespace std_msgs::msg::dds_; + + static_cast(data); + + eprosima::fastcdr::EncodingAlgorithmFlag previous_encoding = calculator.get_encoding(); + size_t calculated_size {calculator.begin_calculate_type_serialized_size( + eprosima::fastcdr::CdrVersion::XCDRv2 == calculator.get_cdr_version() ? + eprosima::fastcdr::EncodingAlgorithmFlag::PLAIN_CDR2 : + eprosima::fastcdr::EncodingAlgorithmFlag::PLAIN_CDR, + current_alignment)}; + + + calculated_size += calculator.calculate_member_serialized_size(eprosima::fastcdr::MemberId(0), + data.stamp(), current_alignment); + + calculated_size += calculator.calculate_member_serialized_size(eprosima::fastcdr::MemberId(1), + data.frame_id(), current_alignment); + + + calculated_size += calculator.end_calculate_type_serialized_size(previous_encoding, current_alignment); + + return calculated_size; +} + +template<> +eProsima_user_DllExport void serialize( + eprosima::fastcdr::Cdr& scdr, + const std_msgs::msg::dds_::Header_& data) +{ + using namespace std_msgs::msg::dds_; + + eprosima::fastcdr::Cdr::state current_state(scdr); + scdr.begin_serialize_type(current_state, + eprosima::fastcdr::CdrVersion::XCDRv2 == scdr.get_cdr_version() ? + eprosima::fastcdr::EncodingAlgorithmFlag::PLAIN_CDR2 : + eprosima::fastcdr::EncodingAlgorithmFlag::PLAIN_CDR); + + scdr + << eprosima::fastcdr::MemberId(0) << data.stamp() + << eprosima::fastcdr::MemberId(1) << data.frame_id() +; + scdr.end_serialize_type(current_state); +} + +template<> +eProsima_user_DllExport void deserialize( + eprosima::fastcdr::Cdr& cdr, + std_msgs::msg::dds_::Header_& data) +{ + using namespace std_msgs::msg::dds_; + + cdr.deserialize_type(eprosima::fastcdr::CdrVersion::XCDRv2 == cdr.get_cdr_version() ? + eprosima::fastcdr::EncodingAlgorithmFlag::PLAIN_CDR2 : + eprosima::fastcdr::EncodingAlgorithmFlag::PLAIN_CDR, + [&data](eprosima::fastcdr::Cdr& dcdr, const eprosima::fastcdr::MemberId& mid) -> bool + { + bool ret_value = true; + switch (mid.id) + { + case 0: + dcdr >> data.stamp(); + break; + + case 1: + dcdr >> data.frame_id(); + break; + + default: + ret_value = false; + break; + } + return ret_value; + }); +} + +void serialize_key( + eprosima::fastcdr::Cdr& scdr, + const std_msgs::msg::dds_::Header_& data) +{ + using namespace std_msgs::msg::dds_; + + static_cast(scdr); + static_cast(data); +} + + + + + + +} // namespace fastcdr +} // namespace eprosima + +#endif // _FAST_DDS_GENERATED_STD_MSGS_MSG_DDS__HEADERCDRAUX_IPP_ + diff --git a/mujoco/idl_gen/std_msgs/HeaderPubSubTypes.cxx b/mujoco/idl_gen/std_msgs/HeaderPubSubTypes.cxx new file mode 100644 index 0000000..6c6cfb4 --- /dev/null +++ b/mujoco/idl_gen/std_msgs/HeaderPubSubTypes.cxx @@ -0,0 +1,233 @@ +// Copyright 2016 Proyectos y Sistemas de Mantenimiento SL (eProsima). +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +/*! + * @file HeaderPubSubTypes.cpp + * This header file contains the implementation of the serialization functions. + * + * This file was generated by the tool fastddsgen. + */ + + +#include + +#include "HeaderPubSubTypes.h" +#include "HeaderCdrAux.hpp" + +using SerializedPayload_t = eprosima::fastrtps::rtps::SerializedPayload_t; +using InstanceHandle_t = eprosima::fastrtps::rtps::InstanceHandle_t; +using DataRepresentationId_t = eprosima::fastdds::dds::DataRepresentationId_t; + +namespace std_msgs { + namespace msg { + namespace dds_ { + + + Header_PubSubType::Header_PubSubType() + { + setName("std_msgs::msg::dds_::Header_"); + uint32_t type_size = + #if FASTCDR_VERSION_MAJOR == 1 + static_cast(Header_::getMaxCdrSerializedSize()); + #else + std_msgs_msg_dds__Header__max_cdr_typesize; + #endif + type_size += static_cast(eprosima::fastcdr::Cdr::alignment(type_size, 4)); /* possible submessage alignment */ + m_typeSize = type_size + 4; /*encapsulation*/ + m_isGetKeyDefined = false; + uint32_t keyLength = std_msgs_msg_dds__Header__max_key_cdr_typesize > 16 ? std_msgs_msg_dds__Header__max_key_cdr_typesize : 16; + m_keyBuffer = reinterpret_cast(malloc(keyLength)); + memset(m_keyBuffer, 0, keyLength); + } + + Header_PubSubType::~Header_PubSubType() + { + if (m_keyBuffer != nullptr) + { + free(m_keyBuffer); + } + } + + bool Header_PubSubType::serialize( + void* data, + SerializedPayload_t* payload, + DataRepresentationId_t data_representation) + { + Header_* p_type = static_cast(data); + + // Object that manages the raw buffer. + eprosima::fastcdr::FastBuffer fastbuffer(reinterpret_cast(payload->data), payload->max_size); + // Object that serializes the data. + eprosima::fastcdr::Cdr ser(fastbuffer, eprosima::fastcdr::Cdr::DEFAULT_ENDIAN, + data_representation == DataRepresentationId_t::XCDR_DATA_REPRESENTATION ? + eprosima::fastcdr::CdrVersion::XCDRv1 : eprosima::fastcdr::CdrVersion::XCDRv2); + payload->encapsulation = ser.endianness() == eprosima::fastcdr::Cdr::BIG_ENDIANNESS ? CDR_BE : CDR_LE; + #if FASTCDR_VERSION_MAJOR > 1 + ser.set_encoding_flag( + data_representation == DataRepresentationId_t::XCDR_DATA_REPRESENTATION ? + eprosima::fastcdr::EncodingAlgorithmFlag::PLAIN_CDR : + eprosima::fastcdr::EncodingAlgorithmFlag::PLAIN_CDR2); + #endif // FASTCDR_VERSION_MAJOR > 1 + + try + { + // Serialize encapsulation + ser.serialize_encapsulation(); + // Serialize the object. + ser << *p_type; + } + catch (eprosima::fastcdr::exception::Exception& /*exception*/) + { + return false; + } + + // Get the serialized length + #if FASTCDR_VERSION_MAJOR == 1 + payload->length = static_cast(ser.getSerializedDataLength()); + #else + payload->length = static_cast(ser.get_serialized_data_length()); + #endif // FASTCDR_VERSION_MAJOR == 1 + return true; + } + + bool Header_PubSubType::deserialize( + SerializedPayload_t* payload, + void* data) + { + try + { + // Convert DATA to pointer of your type + Header_* p_type = static_cast(data); + + // Object that manages the raw buffer. + eprosima::fastcdr::FastBuffer fastbuffer(reinterpret_cast(payload->data), payload->length); + + // Object that deserializes the data. + eprosima::fastcdr::Cdr deser(fastbuffer, eprosima::fastcdr::Cdr::DEFAULT_ENDIAN + #if FASTCDR_VERSION_MAJOR == 1 + , eprosima::fastcdr::Cdr::CdrType::DDS_CDR + #endif // FASTCDR_VERSION_MAJOR == 1 + ); + + // Deserialize encapsulation. + deser.read_encapsulation(); + payload->encapsulation = deser.endianness() == eprosima::fastcdr::Cdr::BIG_ENDIANNESS ? CDR_BE : CDR_LE; + + // Deserialize the object. + deser >> *p_type; + } + catch (eprosima::fastcdr::exception::Exception& /*exception*/) + { + return false; + } + + return true; + } + + std::function Header_PubSubType::getSerializedSizeProvider( + void* data, + DataRepresentationId_t data_representation) + { + return [data, data_representation]() -> uint32_t + { + #if FASTCDR_VERSION_MAJOR == 1 + static_cast(data_representation); + return static_cast(type::getCdrSerializedSize(*static_cast(data))) + + 4u /*encapsulation*/; + #else + try + { + eprosima::fastcdr::CdrSizeCalculator calculator( + data_representation == DataRepresentationId_t::XCDR_DATA_REPRESENTATION ? + eprosima::fastcdr::CdrVersion::XCDRv1 :eprosima::fastcdr::CdrVersion::XCDRv2); + size_t current_alignment {0}; + return static_cast(calculator.calculate_serialized_size( + *static_cast(data), current_alignment)) + + 4u /*encapsulation*/; + } + catch (eprosima::fastcdr::exception::Exception& /*exception*/) + { + return 0; + } + #endif // FASTCDR_VERSION_MAJOR == 1 + }; + } + + void* Header_PubSubType::createData() + { + return reinterpret_cast(new Header_()); + } + + void Header_PubSubType::deleteData( + void* data) + { + delete(reinterpret_cast(data)); + } + + bool Header_PubSubType::getKey( + void* data, + InstanceHandle_t* handle, + bool force_md5) + { + if (!m_isGetKeyDefined) + { + return false; + } + + Header_* p_type = static_cast(data); + + // Object that manages the raw buffer. + eprosima::fastcdr::FastBuffer fastbuffer(reinterpret_cast(m_keyBuffer), + std_msgs_msg_dds__Header__max_key_cdr_typesize); + + // Object that serializes the data. + eprosima::fastcdr::Cdr ser(fastbuffer, eprosima::fastcdr::Cdr::BIG_ENDIANNESS, eprosima::fastcdr::CdrVersion::XCDRv1); + #if FASTCDR_VERSION_MAJOR == 1 + p_type->serializeKey(ser); + #else + eprosima::fastcdr::serialize_key(ser, *p_type); + #endif // FASTCDR_VERSION_MAJOR == 1 + if (force_md5 || std_msgs_msg_dds__Header__max_key_cdr_typesize > 16) + { + m_md5.init(); + #if FASTCDR_VERSION_MAJOR == 1 + m_md5.update(m_keyBuffer, static_cast(ser.getSerializedDataLength())); + #else + m_md5.update(m_keyBuffer, static_cast(ser.get_serialized_data_length())); + #endif // FASTCDR_VERSION_MAJOR == 1 + m_md5.finalize(); + for (uint8_t i = 0; i < 16; ++i) + { + handle->value[i] = m_md5.digest[i]; + } + } + else + { + for (uint8_t i = 0; i < 16; ++i) + { + handle->value[i] = m_keyBuffer[i]; + } + } + return true; + } + + + } //End of namespace dds_ + + + } //End of namespace msg + + +} //End of namespace std_msgs + diff --git a/mujoco/idl_gen/std_msgs/HeaderPubSubTypes.h b/mujoco/idl_gen/std_msgs/HeaderPubSubTypes.h new file mode 100644 index 0000000..20f2d28 --- /dev/null +++ b/mujoco/idl_gen/std_msgs/HeaderPubSubTypes.h @@ -0,0 +1,142 @@ +// Copyright 2016 Proyectos y Sistemas de Mantenimiento SL (eProsima). +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +/*! + * @file HeaderPubSubTypes.h + * This header file contains the declaration of the serialization functions. + * + * This file was generated by the tool fastddsgen. + */ + + +#ifndef _FAST_DDS_GENERATED_STD_MSGS_MSG_DDS__HEADER_PUBSUBTYPES_H_ +#define _FAST_DDS_GENERATED_STD_MSGS_MSG_DDS__HEADER_PUBSUBTYPES_H_ + +#include +#include +#include +#include +#include + +#include "Header.h" + +#include "TimePubSubTypes.h" + +#if !defined(GEN_API_VER) || (GEN_API_VER != 2) +#error \ + Generated Header is not compatible with current installed Fast DDS. Please, regenerate it with fastddsgen. +#endif // GEN_API_VER + +namespace std_msgs +{ + namespace msg + { + namespace dds_ + { + + + + /*! + * @brief This class represents the TopicDataType of the type Header_ defined by the user in the IDL file. + * @ingroup Header + */ + class Header_PubSubType : public eprosima::fastdds::dds::TopicDataType + { + public: + + typedef Header_ type; + + eProsima_user_DllExport Header_PubSubType(); + + eProsima_user_DllExport ~Header_PubSubType() override; + + eProsima_user_DllExport bool serialize( + void* data, + eprosima::fastrtps::rtps::SerializedPayload_t* payload) override + { + return serialize(data, payload, eprosima::fastdds::dds::DEFAULT_DATA_REPRESENTATION); + } + + eProsima_user_DllExport bool serialize( + void* data, + eprosima::fastrtps::rtps::SerializedPayload_t* payload, + eprosima::fastdds::dds::DataRepresentationId_t data_representation) override; + + eProsima_user_DllExport bool deserialize( + eprosima::fastrtps::rtps::SerializedPayload_t* payload, + void* data) override; + + eProsima_user_DllExport std::function getSerializedSizeProvider( + void* data) override + { + return getSerializedSizeProvider(data, eprosima::fastdds::dds::DEFAULT_DATA_REPRESENTATION); + } + + eProsima_user_DllExport std::function getSerializedSizeProvider( + void* data, + eprosima::fastdds::dds::DataRepresentationId_t data_representation) override; + + eProsima_user_DllExport bool getKey( + void* data, + eprosima::fastrtps::rtps::InstanceHandle_t* ihandle, + bool force_md5 = false) override; + + eProsima_user_DllExport void* createData() override; + + eProsima_user_DllExport void deleteData( + void* data) override; + + #ifdef TOPIC_DATA_TYPE_API_HAS_IS_BOUNDED + eProsima_user_DllExport inline bool is_bounded() const override + { + return false; + } + + #endif // TOPIC_DATA_TYPE_API_HAS_IS_BOUNDED + + #ifdef TOPIC_DATA_TYPE_API_HAS_IS_PLAIN + eProsima_user_DllExport inline bool is_plain() const override + { + return false; + } + + eProsima_user_DllExport inline bool is_plain( + eprosima::fastdds::dds::DataRepresentationId_t data_representation) const override + { + static_cast(data_representation); + return false; + } + + #endif // TOPIC_DATA_TYPE_API_HAS_IS_PLAIN + + #ifdef TOPIC_DATA_TYPE_API_HAS_CONSTRUCT_SAMPLE + eProsima_user_DllExport inline bool construct_sample( + void* memory) const override + { + static_cast(memory); + return false; + } + + #endif // TOPIC_DATA_TYPE_API_HAS_CONSTRUCT_SAMPLE + + MD5 m_md5; + unsigned char* m_keyBuffer; + + }; + } + } +} + +#endif // _FAST_DDS_GENERATED_STD_MSGS_MSG_DDS__HEADER_PUBSUBTYPES_H_ + diff --git a/mujoco/module/SdkBridge/CMakeLists.txt b/mujoco/module/SdkBridge/CMakeLists.txt index 9ff18da..8b94ccd 100644 --- a/mujoco/module/SdkBridge/CMakeLists.txt +++ b/mujoco/module/SdkBridge/CMakeLists.txt @@ -6,11 +6,17 @@ file(GLOB k1sim_idl_srcs CONFIGURE_DEPENDS ${K1SIM_IDL_GEN_DIR}/booster_interface/*.cxx ${K1SIM_IDL_GEN_DIR}/booster_msgs/*.cxx ${K1SIM_IDL_GEN_DIR}/geometry_msgs/*.cxx + ${K1SIM_IDL_GEN_DIR}/builtin_interfaces/*.cxx + ${K1SIM_IDL_GEN_DIR}/std_msgs/*.cxx + ${K1SIM_IDL_GEN_DIR}/nav_msgs/*.cxx ) file(GLOB k1sim_idl_ipp CONFIGURE_DEPENDS ${K1SIM_IDL_GEN_DIR}/booster_interface/*.ipp ${K1SIM_IDL_GEN_DIR}/booster_msgs/*.ipp ${K1SIM_IDL_GEN_DIR}/geometry_msgs/*.ipp + ${K1SIM_IDL_GEN_DIR}/builtin_interfaces/*.ipp + ${K1SIM_IDL_GEN_DIR}/std_msgs/*.ipp + ${K1SIM_IDL_GEN_DIR}/nav_msgs/*.ipp ) # Each CdrAux.ipp holds explicit fastcdr template specializations # (calculate_serialized_size/serialize/deserialize/serialize_key for that exact @@ -25,7 +31,8 @@ add_library(k1sim_idl STATIC ${k1sim_idl_srcs} ${k1sim_idl_ipp}) target_include_directories( k1sim_idl PUBLIC ${K1SIM_IDL_GEN_DIR}/booster_interface ${K1SIM_IDL_GEN_DIR}/booster_msgs - ${K1SIM_IDL_GEN_DIR}/geometry_msgs + ${K1SIM_IDL_GEN_DIR}/geometry_msgs ${K1SIM_IDL_GEN_DIR}/builtin_interfaces + ${K1SIM_IDL_GEN_DIR}/std_msgs ${K1SIM_IDL_GEN_DIR}/nav_msgs ) target_link_libraries(k1sim_idl PUBLIC fastrtps fastcdr) @@ -34,6 +41,7 @@ add_library( k1sim_module_sdkbridge STATIC src/SdkBridge.cpp src/DdsParticipant.cpp + src/OdometryModel.cpp src/StatePublisher.cpp src/RpcServer.cpp src/RpcDispatch.cpp diff --git a/mujoco/module/SdkBridge/PROTOCOL.md b/mujoco/module/SdkBridge/PROTOCOL.md index c74354b..8d1f28f 100644 --- a/mujoco/module/SdkBridge/PROTOCOL.md +++ b/mujoco/module/SdkBridge/PROTOCOL.md @@ -86,7 +86,8 @@ fastddsgen maps to unsigned `octet`/`uint8_t` in the generated C++ — this is t | 2 | `y` | `float` | | 3 | `theta` | `float` | -Topic `rt/odometer_state`. Planar ground-truth pose (m, m, rad). +Topic `rt/odometer_state`. Planar pose (m, m, rad): the base's ground truth, or with +`config/odometry.yaml` enabled, the odometry error model's drifting estimate of it. ### `geometry_msgs::msg::dds_::Pose_` (geometry_msgs/Pose.h) | # | field | type | @@ -98,9 +99,30 @@ Topic `rt/head_pose`. Not an SDK API topic: it is what NUbots_K1's `input::K1Sen subscribes to (reliable) for the head pose in the yaw-only base footprint frame, which it composes with `rt/odometer_state` to place the camera and torso in the world. The head frame is the robot's, 0.08 m above the `Head_pitch` joint along the head z-axis, not the -`Head_2` body origin: K1Sensors' `Hhp` removes that offset (see `shared/sim/HeadPose.hpp`). Layout and registered name verified against the SDK NUbots_K1 builds against -(`d5d8f7ae`): `geometry_msgs::msg::Pose().getName()` returns -`geometry_msgs::msg::dds_::Pose_`. +`Head_2` body origin: K1Sensors' `Hhp` removes that offset (see `shared/sim/HeadPose.hpp`). +Layout and registered name verified against the SDK NUbots_K1 builds against (`d5d8f7ae`): +`geometry_msgs::msg::Pose().getName()` returns `geometry_msgs::msg::dds_::Pose_`. + +### `nav_msgs::msg::dds_::Odometry_` (nav_msgs/Odometry.h) +| # | field | type | +|---|---|---| +| 1 | `header` | `std_msgs::msg::dds_::Header_` (`stamp` : `builtin_interfaces::msg::dds_::Time_` {`sec` : `int32`, `nanosec` : `uint32`}, `frame_id` : `string`) | +| 2 | `child_frame_id` | `string` | +| 3 | `pose` | `geometry_msgs::msg::dds_::PoseWithCovariance_` (`pose` : `Pose_`, `covariance` : `double[36]`) | +| 4 | `twist` | `geometry_msgs::msg::dds_::TwistWithCovariance_` (`twist` : `Twist_` {`linear`, `angular` : `Vector3_` {`x`, `y`, `z` : `double`}}, `covariance` : `double[36]`) | + +Topic `rt/odom` (`kTopicRosOdometer`, in the SDK from the 1.7.0 firmware). The controller's +full ROS odometry: `rt/odometer_state` carries only its planar pose, this adds the velocity, +which NUbots_K1's `platform::Booster::HardwareIO` subscribes to for `Sensors.vTw`. Booster +does not document the frames; the sim follows the ROS nav_msgs convention: pose in +`frame_id` "odom" (the world), twist in `child_frame_id` "base_link" (the root body's frame, +rotated from MuJoCo's world-frame velocities), header stamped with wall-clock time. The +planar parts (x, y, yaw and vx, vy, wz) carry `config/odometry.yaml`'s error model when it is +enabled, the same estimate as `rt/odometer_state`, with the twist covariance's vx, vy and wz +diagonal its per-sample white noise variance; otherwise everything is ground truth and the +covariances are zero. HardwareIO logs the frames of the +first message it receives, which shows which convention the real robot uses. Layouts and +registered names verified against the SDK (`d5d8f7ae`) with `getName()`, as for `Pose_`. ### `booster_interface::msg::dds_::FallDownState_` (FallDownState.h) Enum `FallDownStateType : unsigned long` (C++ `uint32_t`) = `{IS_READY=0, IS_FALLING=1, @@ -199,6 +221,9 @@ booster_interface::msg::dds_::MotorCmd_ booster_msgs::msg::dds_::RpcReqMsg_ booster_msgs::msg::dds_::RpcRespMsg_ geometry_msgs::msg::dds_::Pose_ (+ nested Point_, Quaternion_; rt/head_pose) +nav_msgs::msg::dds_::Odometry_ (+ nested std_msgs Header_, builtin_interfaces Time_, + geometry_msgs PoseWithCovariance_, TwistWithCovariance_, + Twist_, Vector3_; rt/odom) ``` Also present (not used by us): `booster_interface::msg::dds_::RemoteControllerState_`, @@ -309,7 +334,7 @@ Per the lead's explicit spec (de-risks RPC correctness over squeezing out best-e UDP savings — reliability never hurts a loopback/`--network host` transport, and the SDK's own defaults would silently degrade to best-effort which is a *safe* superset to widen, not narrow), `module::SdkBridge` uses: -- **State writers** (`low_state`, `odometer_state`, `head_pose`, `fall_down`, +- **State writers** (`low_state`, `odometer_state`, `odom`, `head_pose`, `fall_down`, `battery_state`, `button_event`): `RELIABLE` + `VOLATILE` durability + `KEEP_LAST(5)` history. `head_pose` must be `RELIABLE`: K1Sensors creates its reader with `reliable = true`, which a best-effort writer would not match. diff --git a/mujoco/module/SdkBridge/src/OdometryModel.cpp b/mujoco/module/SdkBridge/src/OdometryModel.cpp new file mode 100644 index 0000000..131704d --- /dev/null +++ b/mujoco/module/SdkBridge/src/OdometryModel.cpp @@ -0,0 +1,77 @@ +#include "module/SdkBridge/src/OdometryModel.hpp" + +#include + +namespace k1sim::module::sdkbridge { + +namespace { + +// Avoid M_PI: not reliably available under this project's -std=c++17 build +constexpr double kPi = 3.14159265358979323846; + +std::array triple(const YAML::Node& node, const std::array& fallback) { + if (!node) { + return fallback; + } + return {node[0].as(), node[1].as(), node[2].as()}; +} + +} // namespace + +OdometryModel::Config OdometryModel::load_config(const YAML::Node& node) { + Config cfg; + cfg.enabled = node["enabled"].as(cfg.enabled); + cfg.scale = triple(node["scale"], cfg.scale); + cfg.velocity_noise_density = triple(node["velocity_noise_density"], cfg.velocity_noise_density); + cfg.bias_sigma = triple(node["bias_sigma"], cfg.bias_sigma); + cfg.bias_time_constant = triple(node["bias_time_constant"], cfg.bias_time_constant); + cfg.seed = node["seed"].as(cfg.seed); + return cfg; +} + +OdometryModel::OdometryModel(const Config& config) + : config_(config), rng_(config.seed != 0 ? config.seed : std::random_device{}()) {} + +const OdometryModel::Estimate& OdometryModel::update(double t, + double x, + double y, + double yaw, + double vx_world, + double vy_world, + double wz) { + // The true velocity in the yaw-only body frame + const double c = std::cos(yaw), s = std::sin(yaw); + const std::array v_true{c * vx_world + s * vy_world, -s * vx_world + c * vy_world, wz}; + + const double dt = t - t_; + if (!config_.enabled || !initialised_ || dt <= 0.0) { + estimate_ = Estimate{x, y, yaw, v_true, {}}; + for (std::size_t i = 0; i < 3; ++i) { + bias_[i] = config_.bias_sigma[i] * normal_(rng_); + } + initialised_ = true; + t_ = t; + return estimate_; + } + t_ = t; + + for (std::size_t i = 0; i < 3; ++i) { + // Exact discretisation of the Gauss-Markov bias, stationary at bias_sigma + const double a = std::exp(-dt / config_.bias_time_constant[i]); + bias_[i] = a * bias_[i] + config_.bias_sigma[i] * std::sqrt(1.0 - a * a) * normal_(rng_); + // A white noise density sigma is a per-sample standard deviation of sigma / sqrt(dt) + const double sample_sigma = config_.velocity_noise_density[i] / std::sqrt(dt); + estimate_.velocity[i] = config_.scale[i] * v_true[i] + bias_[i] + sample_sigma * normal_(rng_); + estimate_.velocity_variance[i] = sample_sigma * sample_sigma; + } + + // Integrate the pose from the estimated velocity, rotating it by the midpoint heading + const double yaw_mid = estimate_.yaw + 0.5 * estimate_.velocity[2] * dt; + const double cm = std::cos(yaw_mid), sm = std::sin(yaw_mid); + estimate_.x += (cm * estimate_.velocity[0] - sm * estimate_.velocity[1]) * dt; + estimate_.y += (sm * estimate_.velocity[0] + cm * estimate_.velocity[1]) * dt; + estimate_.yaw = std::remainder(estimate_.yaw + estimate_.velocity[2] * dt, 2.0 * kPi); + return estimate_; +} + +} // namespace k1sim::module::sdkbridge diff --git a/mujoco/module/SdkBridge/src/OdometryModel.hpp b/mujoco/module/SdkBridge/src/OdometryModel.hpp new file mode 100644 index 0000000..1908c7e --- /dev/null +++ b/mujoco/module/SdkBridge/src/OdometryModel.hpp @@ -0,0 +1,72 @@ +#ifndef K1SIM_MODULE_SDKBRIDGE_ODOMETRYMODEL_HPP +#define K1SIM_MODULE_SDKBRIDGE_ODOMETRYMODEL_HPP + +#include +#include +#include + +#include + +// The robot's own odometry, as rt/odometer_state and rt/odom report it. The sim's base state is +// ground truth, but the real controller's odometry is an estimate that drifts; this corrupts the +// planar body velocity (forward, sideways, yaw rate) with the usual odometry error terms and +// integrates the pose from it, so the pose and the twist stay consistent with each other. +// +// Per axis: v_est = scale * v_true + bias + white noise, where the bias is a first-order +// Gauss-Markov process: stationary, with standard deviation bias_sigma, decorrelating over +// bias_time_constant. The white noise is a density, so the pose error it causes grows the same way +// whatever rate the state is published at: a white noise density s gives a position (or heading) +// random walk of s * sqrt(t). Over a window of T seconds the pose error variance is +// +// s^2 T + 2 bias_sigma^2 tau^2 (T / tau - 1 + exp(-T / tau)), +// +// the bias drifting the pose steadily (like bias_sigma * T) over windows shorter than tau and +// adding to the random walk over longer ones. + +namespace k1sim::module::sdkbridge { + +class OdometryModel { +public: + struct Config { + bool enabled = false; // false: publish the ground truth unchanged + // Multiplicative error on the true body velocity (vx, vy, wz); 1 is exact + std::array scale{1.0, 1.0, 1.0}; + // White noise density on the body velocity [m/sqrt(s), m/sqrt(s), rad/sqrt(s)] + std::array velocity_noise_density{0.0, 0.0, 0.0}; + // Standard deviation of the velocity bias [m/s, m/s, rad/s] + std::array bias_sigma{0.0, 0.0, 0.0}; + // Time over which the bias decorrelates [s] + std::array bias_time_constant{10.0, 10.0, 10.0}; + uint64_t seed = 0; // 0 seeds from std::random_device + }; + + // Planar pose in the odometry world frame, and the body-frame velocity that moved it + struct Estimate { + double x = 0.0, y = 0.0, yaw = 0.0; + std::array velocity{}; // body frame (vx, vy, wz) + std::array velocity_variance{}; // of this sample's velocity error + }; + + static Config load_config(const YAML::Node& node); + + explicit OdometryModel(const Config& config); + + // Advance to time t from the base's true planar pose and world-frame planar velocity. The + // first call, and any call that goes back in time (a sim reset), starts again from the truth, + // with a fresh bias drawn from its stationary distribution. + const Estimate& update(double t, double x, double y, double yaw, double vx_world, double vy_world, double wz); + +private: + Config config_; + std::mt19937_64 rng_; + std::normal_distribution normal_{0.0, 1.0}; + + bool initialised_ = false; + double t_ = 0.0; + std::array bias_{}; + Estimate estimate_; +}; + +} // namespace k1sim::module::sdkbridge + +#endif // K1SIM_MODULE_SDKBRIDGE_ODOMETRYMODEL_HPP diff --git a/mujoco/module/SdkBridge/src/SdkBridge.cpp b/mujoco/module/SdkBridge/src/SdkBridge.cpp index 8ef7cc5..ff34874 100644 --- a/mujoco/module/SdkBridge/src/SdkBridge.cpp +++ b/mujoco/module/SdkBridge/src/SdkBridge.cpp @@ -26,13 +26,17 @@ SdkBridge::SdkBridge(std::unique_ptr environment) : Reacto const double battery_soc = cfg["battery_soc"].as(100.0); const int64_t unknown_api_status = cfg["unknown_api_status"].as(0); + const auto odometry = sdkbridge::OdometryModel::load_config(config::load("odometry.yaml")); + dds_ = std::make_unique(domain, udp_only); - state_publisher_ = std::make_unique(*dds_, battery_soc); + state_publisher_ = std::make_unique(*dds_, battery_soc, odometry); rpc_server_ = std::make_unique(*dds_, *this, unknown_api_status); log("SdkBridge ready (DDS domain", domain, udp_only ? "UDP-only" : "UDP+SHM", + ", odometry", + odometry.enabled ? "with the config/odometry.yaml error model" : "ground truth", ")"); }); diff --git a/mujoco/module/SdkBridge/src/StatePublisher.cpp b/mujoco/module/SdkBridge/src/StatePublisher.cpp index 0a45989..e8e1aa1 100644 --- a/mujoco/module/SdkBridge/src/StatePublisher.cpp +++ b/mujoco/module/SdkBridge/src/StatePublisher.cpp @@ -1,5 +1,6 @@ #include "module/SdkBridge/src/StatePublisher.hpp" +#include #include #include @@ -13,6 +14,8 @@ #include "LowStatePubSubTypes.h" #include "Odometer.h" #include "OdometerPubSubTypes.h" +#include "Odometry.h" +#include "OdometryPubSubTypes.h" #include "Pose.h" #include "PosePubSubTypes.h" @@ -30,6 +33,29 @@ double yaw_from_quat(const std::array& q) { return std::atan2(2.0 * (w * z + x * y), 1.0 - 2.0 * (y * y + z * z)); } +// Avoid M_PI: not reliably available under this project's -std=c++17 build +constexpr double kPi = 3.14159265358979323846; + +// Hamilton product a * b of {w, x, y, z} quaternions +std::array quat_mul(const std::array& a, const std::array& b) { + return {a[0] * b[0] - a[1] * b[1] - a[2] * b[2] - a[3] * b[3], + a[0] * b[1] + a[1] * b[0] + a[2] * b[3] - a[3] * b[2], + a[0] * b[2] - a[1] * b[3] + a[2] * b[0] + a[3] * b[1], + a[0] * b[3] + a[1] * b[2] - a[2] * b[1] + a[3] * b[0]}; +} + +// A world-frame vector expressed in the body frame of the {w, x, y, z} orientation q: R(q)^T v. +std::array world_to_body(const std::array& q, const std::array& v) { + const double w = q[0], x = q[1], y = q[2], z = q[3]; + // Rows of R^T are the columns of R + const double r[3][3] = {{1 - 2 * (y * y + z * z), 2 * (x * y + w * z), 2 * (x * z - w * y)}, + {2 * (x * y - w * z), 1 - 2 * (x * x + z * z), 2 * (y * z + w * x)}, + {2 * (x * z + w * y), 2 * (y * z - w * x), 1 - 2 * (x * x + y * y)}}; + return {r[0][0] * v[0] + r[0][1] * v[1] + r[0][2] * v[2], + r[1][0] * v[0] + r[1][1] * v[1] + r[1][2] * v[2], + r[2][0] * v[0] + r[2][1] * v[1] + r[2][2] * v[2]}; +} + booster_interface::msg::dds_::MotorState_ to_motor_state(const k1sim::message::JointState& joint) { booster_interface::msg::dds_::MotorState_ m; m.mode(1); @@ -45,18 +71,22 @@ booster_interface::msg::dds_::MotorState_ to_motor_state(const k1sim::message::J } // namespace -StatePublisher::StatePublisher(DdsParticipant& dds, double battery_soc) : battery_soc_(battery_soc) { +StatePublisher::StatePublisher(DdsParticipant& dds, double battery_soc, const OdometryModel::Config& odometry) + : battery_soc_(battery_soc), odometry_(odometry) { using booster_interface::msg::dds_::BatteryState_PubSubType; using booster_interface::msg::dds_::ButtonEventMsg_PubSubType; using booster_interface::msg::dds_::FallDownState_PubSubType; using booster_interface::msg::dds_::LowState_PubSubType; using booster_interface::msg::dds_::Odometer_PubSubType; using geometry_msgs::msg::dds_::Pose_PubSubType; + using nav_msgs::msg::dds_::Odometry_PubSubType; low_state_writer_ = dds.create_writer(k1sim::booster::TOPIC_LOW_STATE, DdsParticipant::state_writer_qos()); odometer_writer_ = dds.create_writer(k1sim::booster::TOPIC_ODOMETER_STATE, DdsParticipant::state_writer_qos()); + ros_odometry_writer_ = dds.create_writer(k1sim::booster::TOPIC_ROS_ODOMETER, + DdsParticipant::state_writer_qos()); head_pose_writer_ = dds.create_writer(k1sim::booster::TOPIC_HEAD_POSE, DdsParticipant::state_writer_qos()); fall_down_writer_ = @@ -95,13 +125,65 @@ void StatePublisher::publish(const k1sim::message::SimStateUpdate& update) { low_state.motor_state_parallel(legs); low_state_writer_->write(&low_state); + // The controller's odometry: the base's planar pose and velocity as OdometryModel corrupts + // them (the ground truth when it is disabled), published on both odometry topics below. + const double yaw_true = yaw_from_quat(update.base.quat); + const OdometryModel::Estimate& est = odometry_.update(update.sim_time, + update.base.x, + update.base.y, + yaw_true, + update.base.lin_vel[0], + update.base.lin_vel[1], + update.base.ang_vel[2]); + // --- rt/odometer_state --- booster_interface::msg::dds_::Odometer_ odom; - odom.x(static_cast(update.base.x)); - odom.y(static_cast(update.base.y)); - odom.theta(static_cast(yaw_from_quat(update.base.quat))); + odom.x(static_cast(est.x)); + odom.y(static_cast(est.y)); + odom.theta(static_cast(est.yaw)); odometer_writer_->write(&odom); + // --- rt/odom --- The same base odometry with its velocity: pose in "odom" (the world), twist + // in the body frame "base_link", per the ROS nav_msgs convention (twist in child_frame_id). + // The odometry model's planar errors are applied on top of the base's true 3D pose and twist: + // its position and heading error to the pose, its velocity error to the twist, with the twist + // covariance its white noise (zero, like the pose covariance, when the model is disabled). + const double cy = std::cos(yaw_true), sy = std::sin(yaw_true); + const std::array v_true_planar{cy * update.base.lin_vel[0] + sy * update.base.lin_vel[1], + -sy * update.base.lin_vel[0] + cy * update.base.lin_vel[1], + update.base.ang_vel[2]}; + const double half_dyaw = 0.5 * std::remainder(est.yaw - yaw_true, 2.0 * kPi); + const std::array q = quat_mul({std::cos(half_dyaw), 0.0, 0.0, std::sin(half_dyaw)}, update.base.quat); + + nav_msgs::msg::dds_::Odometry_ ros_odom; + const auto stamp = std::chrono::system_clock::now().time_since_epoch(); + const auto sec = std::chrono::duration_cast(stamp); + ros_odom.header().stamp().sec(static_cast(sec.count())); + ros_odom.header().stamp().nanosec( + static_cast(std::chrono::duration_cast(stamp - sec).count())); + ros_odom.header().frame_id("odom"); + ros_odom.child_frame_id("base_link"); + ros_odom.pose().pose().position().x(est.x); + ros_odom.pose().pose().position().y(est.y); + ros_odom.pose().pose().position().z(update.base.z); + ros_odom.pose().pose().orientation().w(q[0]); + ros_odom.pose().pose().orientation().x(q[1]); + ros_odom.pose().pose().orientation().y(q[2]); + ros_odom.pose().pose().orientation().z(q[3]); + const auto linear = world_to_body(update.base.quat, update.base.lin_vel); + const auto angular = world_to_body(update.base.quat, update.base.ang_vel); + ros_odom.twist().twist().linear().x(linear[0] + est.velocity[0] - v_true_planar[0]); + ros_odom.twist().twist().linear().y(linear[1] + est.velocity[1] - v_true_planar[1]); + ros_odom.twist().twist().linear().z(linear[2]); + ros_odom.twist().twist().angular().x(angular[0]); + ros_odom.twist().twist().angular().y(angular[1]); + ros_odom.twist().twist().angular().z(angular[2] + est.velocity[2] - v_true_planar[2]); + // Row-major 6x6 over (vx, vy, vz, wx, wy, wz) + ros_odom.twist().covariance()[0] = est.velocity_variance[0]; + ros_odom.twist().covariance()[7] = est.velocity_variance[1]; + ros_odom.twist().covariance()[35] = est.velocity_variance[2]; + ros_odometry_writer_->write(&ros_odom); + // --- rt/head_pose --- The head frame in the yaw-only base footprint frame, which // K1Sensors composes with the odometry above to place the camera and torso in the world. if (update.head.valid) { diff --git a/mujoco/module/SdkBridge/src/StatePublisher.hpp b/mujoco/module/SdkBridge/src/StatePublisher.hpp index ac3253a..64574e7 100644 --- a/mujoco/module/SdkBridge/src/StatePublisher.hpp +++ b/mujoco/module/SdkBridge/src/StatePublisher.hpp @@ -4,6 +4,7 @@ #include #include "module/SdkBridge/src/DdsParticipant.hpp" +#include "module/SdkBridge/src/OdometryModel.hpp" #include "shared/message/SimMessages.hpp" // Builds and publishes the Booster SDK state topics from k1sim's internal @@ -13,11 +14,12 @@ namespace k1sim::module::sdkbridge { class StatePublisher { public: - // battery_soc comes from config/dds.yaml (constant, published at 1 Hz). - StatePublisher(DdsParticipant& dds, double battery_soc); + // battery_soc comes from config/dds.yaml (constant, published at 1 Hz), odometry from + // config/odometry.yaml (the error model of rt/odometer_state and rt/odom). + StatePublisher(DdsParticipant& dds, double battery_soc, const OdometryModel::Config& odometry); // Called at the LowState cadence (50 Hz, on>): writes - // rt/low_state, rt/odometer_state and rt/head_pose every call, and rt/fall_down + // rt/low_state, rt/odometer_state, rt/odom and rt/head_pose every call, and rt/fall_down // whenever fall_state changes or >=1s has elapsed since the last publish (keepalive). void publish(const k1sim::message::SimStateUpdate& update); @@ -27,12 +29,14 @@ class StatePublisher { private: eprosima::fastdds::dds::DataWriter* low_state_writer_ = nullptr; eprosima::fastdds::dds::DataWriter* odometer_writer_ = nullptr; + eprosima::fastdds::dds::DataWriter* ros_odometry_writer_ = nullptr; eprosima::fastdds::dds::DataWriter* head_pose_writer_ = nullptr; eprosima::fastdds::dds::DataWriter* fall_down_writer_ = nullptr; eprosima::fastdds::dds::DataWriter* battery_writer_ = nullptr; eprosima::fastdds::dds::DataWriter* button_event_writer_ = nullptr; // created, never published double battery_soc_; + OdometryModel odometry_; bool have_last_fall_state_ = false; int last_fall_state_ = -1; diff --git a/mujoco/module/SdkBridge/test_support/SyntheticState.cpp b/mujoco/module/SdkBridge/test_support/SyntheticState.cpp index cb158b5..f47f4a9 100644 --- a/mujoco/module/SdkBridge/test_support/SyntheticState.cpp +++ b/mujoco/module/SdkBridge/test_support/SyntheticState.cpp @@ -85,6 +85,9 @@ SyntheticState::SyntheticState(std::unique_ptr environment update->base.z = 0.53; const double half = yaw_ * 0.5; update->base.quat = {std::cos(half), 0.0, 0.0, std::sin(half)}; + // World-frame velocity of the commanded body-frame walk + update->base.lin_vel = {vx_ * cos_yaw - vy_ * sin_yaw, vx_ * sin_yaw + vy_ * cos_yaw, 0.0}; + update->base.ang_vel = {0.0, 0.0, vyaw_}; // Standing head frame in the footprint frame: 0.33 m above the base (the Head_pitch // joint 0.248 m above the Trunk plus the 0.08 m head frame offset, see diff --git a/mujoco/shared/k1/BoosterApi.hpp b/mujoco/shared/k1/BoosterApi.hpp index 8205653..2d668fd 100644 --- a/mujoco/shared/k1/BoosterApi.hpp +++ b/mujoco/shared/k1/BoosterApi.hpp @@ -16,6 +16,9 @@ inline constexpr const char* TOPIC_BATTERY_STATE = "rt/battery_state"; inline constexpr const char* TOPIC_BUTTON_EVENT = "rt/button_event"; inline constexpr const char* TOPIC_RPC_REQUEST = "rt/LocoApiTopicReq"; inline constexpr const char* TOPIC_RPC_RESPONSE = "rt/LocoApiTopicResp"; +// The controller's full ROS odometry (nav_msgs Odometry: pose and twist), kTopicRosOdometer. Only +// in the SDK from the 1.7.0 firmware (booster_robotics_sdk @ d5d8f7ae). +inline constexpr const char* TOPIC_ROS_ODOMETER = "rt/odom"; // Not an SDK constant: the robot's head pose (geometry_msgs Pose), the topic NUbots' // K1Sensors subscribes to (K1Sensors.yaml head_pose.topic). inline constexpr const char* TOPIC_HEAD_POSE = "rt/head_pose"; diff --git a/mujoco/test/contract/host_client/sdk_client_check.cpp b/mujoco/test/contract/host_client/sdk_client_check.cpp index 9526669..5576cfd 100644 --- a/mujoco/test/contract/host_client/sdk_client_check.cpp +++ b/mujoco/test/contract/host_client/sdk_client_check.cpp @@ -13,6 +13,9 @@ // - rt/head_pose received at least once with a finite, standing-height pose (the // topic NUbots' K1Sensors places the camera and torso from). Only when the SDK // ships booster/idl/geometry_msgs/Pose.h; SKIP printed otherwise. +// - rt/odom (nav_msgs Odometry, the source of NUbots' Sensors.vTw) received at +// least once with finite pose and twist, reporting its frames. Only when the SDK +// ships booster/idl/nav_msgs/Odometry.h (1.7.0 firmware); SKIP printed otherwise. // - rt/battery_state received at least once during the 5s window (published at // 1 Hz via NUClear on>) with soc in (0, 100]. // Only when the SDK being built against ships booster/idl/b1/BatteryState.h — @@ -49,6 +52,10 @@ #include #define K1SIM_CHECK_HEAD_POSE 1 #endif +#if __has_include() + #include + #define K1SIM_CHECK_ROS_ODOMETRY 1 +#endif #include #include #include @@ -105,6 +112,27 @@ void HeadPoseHandler(const void* msg) { } #endif +#ifdef K1SIM_CHECK_ROS_ODOMETRY +std::atomic g_ros_odom_count{0}; +std::atomic g_ros_odom_finite{true}; +std::string g_ros_odom_frames; // written once, before the count is first incremented +void RosOdometryHandler(const void* msg) { + const auto* odom = static_cast(msg); + const auto& p = odom->pose().pose().position(); + const auto& twist = odom->twist().twist(); + if (g_ros_odom_count.load() == 0) { + g_ros_odom_frames = "'" + odom->header().frame_id() + "' -> '" + odom->child_frame_id() + "'"; + } + for (double v : {p.x(), p.y(), p.z(), twist.linear().x(), twist.linear().y(), twist.linear().z(), + twist.angular().x(), twist.angular().y(), twist.angular().z()}) { + if (!std::isfinite(v)) { + g_ros_odom_finite.store(false, std::memory_order_relaxed); + } + } + g_ros_odom_count.fetch_add(1, std::memory_order_relaxed); +} +#endif + #ifdef K1SIM_CHECK_BATTERY std::atomic g_battery_count{0}; std::atomic g_battery_soc{-1.0f}; @@ -153,6 +181,10 @@ int main() { ChannelSubscriber head_pose_sub("rt/head_pose", HeadPoseHandler); head_pose_sub.InitChannel(); #endif +#ifdef K1SIM_CHECK_ROS_ODOMETRY + ChannelSubscriber ros_odom_sub(booster::robot::b1::kTopicRosOdometer, RosOdometryHandler); + ros_odom_sub.InitChannel(); +#endif #ifdef K1SIM_CHECK_BATTERY ChannelSubscriber battery_sub("rt/battery_state", BatteryHandler); battery_sub.InitChannel(); @@ -191,6 +223,15 @@ int main() { #else std::printf("[SKIP] rt/head_pose checks (SDK build lacks booster/idl/geometry_msgs/Pose.h)\n"); #endif +#ifdef K1SIM_CHECK_ROS_ODOMETRY + std::printf("odom: %llu samples in window, frames %s\n", + static_cast(g_ros_odom_count.load()), + g_ros_odom_frames.c_str()); + check(g_ros_odom_count.load() > 0, "rt/odom received at least once"); + check(g_ros_odom_finite.load(), "rt/odom pose and twist are finite"); +#else + std::printf("[SKIP] rt/odom checks (SDK build lacks booster/idl/nav_msgs/Odometry.h)\n"); +#endif #ifdef K1SIM_CHECK_BATTERY std::printf("battery_state: %llu samples in window, last soc=%.1f\n", static_cast(g_battery_count.load()), diff --git a/mujoco/test/unit/test_odometry_model.cpp b/mujoco/test/unit/test_odometry_model.cpp new file mode 100644 index 0000000..a0ee382 --- /dev/null +++ b/mujoco/test/unit/test_odometry_model.cpp @@ -0,0 +1,120 @@ +// OdometryModel, the error model of rt/odometer_state and rt/odom (config/odometry.yaml). +// +// Asserts: +// - disabled, it passes the ground truth through unchanged. +// - a scale error scales the distance walked and the angle turned. +// - white noise density s spreads the position after T seconds standing still with standard +// deviation s * sqrt(T), and a Gauss-Markov bias (sigma b, time constant tau) with +// sqrt(2 b^2 tau^2 (T / tau - 1 + exp(-T / tau))), each within 15% over 400 seeded runs. +// - the same seed gives the same odometry, and going back in time (a sim reset) restarts from +// the ground truth. +#include +#include +#include + +#include "module/SdkBridge/src/OdometryModel.hpp" + +using k1sim::module::sdkbridge::OdometryModel; + +namespace { + +constexpr double kDt = 0.02; // the 50 Hz state publish rate + +int failures = 0; + +void check(bool cond, const char* what, double got, double want) { + std::printf("[%s] %s: got %.4f, want %.4f\n", cond ? "PASS" : "FAIL", what, got, want); + failures += cond ? 0 : 1; +} + +// Walk at a constant body velocity (vx, wz) for `seconds`, returning the final estimate +OdometryModel::Estimate walk(OdometryModel& model, double vx, double wz, double seconds) { + double x = 0, y = 0, yaw = 0; + OdometryModel::Estimate est = model.update(0.0, x, y, yaw, 0.0, 0.0, 0.0); + for (int i = 1; i <= int(std::lround(seconds / kDt)); ++i) { + const double vxw = vx * std::cos(yaw), vyw = vx * std::sin(yaw); + x += vxw * kDt; + y += vyw * kDt; + yaw += wz * kDt; + est = model.update(i * kDt, x, y, yaw, vxw, vyw, wz); + } + return est; +} + +// Standard deviation of the final x after standing still for `seconds`, over `runs` seeds +double spread(OdometryModel::Config cfg, double seconds, int runs) { + double sum = 0, sum2 = 0; + for (int r = 0; r < runs; ++r) { + cfg.seed = 1000 + r; + OdometryModel model(cfg); + const double x = walk(model, 0.0, 0.0, seconds).x; + sum += x; + sum2 += x * x; + } + const double mean = sum / runs; + return std::sqrt(sum2 / runs - mean * mean); +} + +} // namespace + +int main() { + { + OdometryModel::Config cfg; + cfg.enabled = false; + cfg.velocity_noise_density = {0.5, 0.5, 0.5}; + OdometryModel model(cfg); + model.update(0.0, 0, 0, 0, 0, 0, 0); + const auto& est = model.update(0.02, 1.25, -0.5, 0.3, 0.4, 0.1, 0.2); + check(est.x == 1.25 && est.y == -0.5 && est.yaw == 0.3, "disabled passes the pose through", est.x, 1.25); + } + { + OdometryModel::Config cfg; + cfg.enabled = true; + cfg.scale = {1.1, 1.0, 1.0}; + OdometryModel model(cfg); + const auto est = walk(model, 0.3, 0.0, 10.0); + check(std::abs(est.x - 3.3) < 1e-6, "forward scale 1.1 over 3 m walked", est.x, 3.3); + } + { + OdometryModel::Config cfg; + cfg.enabled = true; + cfg.scale = {1.0, 1.0, 0.9}; + OdometryModel model(cfg); + const auto est = walk(model, 0.0, 0.5, 4.0); + check(std::abs(est.yaw - 1.8) < 1e-6, "yaw scale 0.9 over 2 rad turned", est.yaw, 1.8); + } + { + OdometryModel::Config cfg; + cfg.enabled = true; + cfg.velocity_noise_density = {0.02, 0.0, 0.0}; + const double got = spread(cfg, 10.0, 400); + const double want = 0.02 * std::sqrt(10.0); + check(std::abs(got / want - 1.0) < 0.15, "white noise position spread after 10 s", got, want); + } + { + OdometryModel::Config cfg; + cfg.enabled = true; + cfg.bias_sigma = {0.01, 0.0, 0.0}; + cfg.bias_time_constant = {5.0, 5.0, 5.0}; + const double T = 10.0, tau = 5.0, b = 0.01; + const double got = spread(cfg, T, 400); + const double want = std::sqrt(2.0 * b * b * tau * tau * (T / tau - 1.0 + std::exp(-T / tau))); + check(std::abs(got / want - 1.0) < 0.15, "Gauss-Markov bias position drift after 10 s", got, want); + } + { + OdometryModel::Config cfg; + cfg.enabled = true; + cfg.velocity_noise_density = {0.05, 0.05, 0.05}; + cfg.seed = 7; + OdometryModel a(cfg), b(cfg); + const double xa = walk(a, 0.3, 0.2, 5.0).x, xb = walk(b, 0.3, 0.2, 5.0).x; + check(xa == xb, "same seed, same odometry", xa, xb); + + // A sim reset rewinds time: the estimate restarts from the ground truth + const auto& est = a.update(0.0, 2.0, 1.0, 0.5, 0, 0, 0); + check(est.x == 2.0 && est.y == 1.0 && est.yaw == 0.5, "a sim reset restarts from the truth", est.x, 2.0); + } + + std::printf("test_odometry_model: %s\n", failures == 0 ? "PASS" : "FAIL"); + return failures == 0 ? 0 : 1; +}