diff --git a/mujoco_ros/CHANGELOG.md b/mujoco_ros/CHANGELOG.md index 9a9e85ef..f60ef46b 100644 --- a/mujoco_ros/CHANGELOG.md +++ b/mujoco_ros/CHANGELOG.md @@ -1,14 +1,23 @@ ## Unreleased +### Added +* Compound sensors (for now limited to IMU, Pose, Twist, and Wrench). + ### Fixed * Repaired SIGINT handler callback. `C-c` in the roslaunch terminal now shuts down the MuJoCo ROS node instead of escalating to SIGTERM. * Added actionlib to the list of mujoco_ros' dependencies. +* Fixed bug where cutoff was not correctly applied to noisy sensors. +* Fixed missing FRAMEANGVEL serialization in sensors plugin. ### Changed * replaced `boost::shared_ptr` with `std::shared_ptr` or `std::unique_ptr` wherever possible (ROS 1 fast intra-process message-passing requires boost::shared_ptr). * replaced `shared_ptr` with `unique_ptr` wherever possible. * replaced smart pointer constructor initialization with `make_shared` or `make_unique` wherever possible. +* **Sensors overhaul:** +* * Increased sensor unit test speed by parallelly buffering and running computations for statistics. +* * Renamed sensor unit tests to serialization types for more clarity. +* * Replaced `SensorConfig` with `RosSensorInterfaceBase` and derived, templated `RosSensorInterface` classes with specialized serializers and noise management. ## [0.8.0] - 2023-10-23 diff --git a/mujoco_ros_sensors/CMakeLists.txt b/mujoco_ros_sensors/CMakeLists.txt index 74f163ed..fd5c1485 100644 --- a/mujoco_ros_sensors/CMakeLists.txt +++ b/mujoco_ros_sensors/CMakeLists.txt @@ -46,6 +46,8 @@ include_directories( add_library(${PROJECT_NAME} src/mujoco_sensor_handler_plugin.cpp + src/serialization.cpp + src/compound_sensors.cpp ) target_link_libraries(${PROJECT_NAME} ${catkin_LIBRARIES} diff --git a/mujoco_ros_sensors/config/example_sensors.yaml b/mujoco_ros_sensors/config/example_sensors.yaml index 47c3333b..cc03e862 100644 --- a/mujoco_ros_sensors/config/example_sensors.yaml +++ b/mujoco_ros_sensors/config/example_sensors.yaml @@ -1,2 +1,24 @@ MujocoPlugins: - type: mujoco_ros_sensors/MujocoRosSensorsPlugin + compound_sensors: + - name: test_imu + type: IMUSensor + sensors: + - linacc_EE + - gyro_EE + - quat_EE + - name: test_wrench + type: WrenchSensor + sensors: + - force_EE + - torque_EE + - name: test_twist + type: TwistSensor + sensors: + - linvel_EE + - angvel_EE + - name: test_pose + type: PoseSensor + sensors: + - pos_joint1 + - quat_joint1 diff --git a/mujoco_ros_sensors/include/mujoco_ros_sensors/mujoco_sensor_handler_plugin.h b/mujoco_ros_sensors/include/mujoco_ros_sensors/mujoco_sensor_handler_plugin.h index af12876d..e19406bd 100644 --- a/mujoco_ros_sensors/include/mujoco_ros_sensors/mujoco_sensor_handler_plugin.h +++ b/mujoco_ros_sensors/include/mujoco_ros_sensors/mujoco_sensor_handler_plugin.h @@ -38,7 +38,7 @@ #pragma once -#include +#include #include #include @@ -46,36 +46,12 @@ #include +#include + #include namespace mujoco_ros::sensors { -struct SensorConfig -{ -public: - SensorConfig() : frame_id(""){}; - SensorConfig(std::string frame_id) : frame_id(std::move(frame_id)){}; - - void setFrameId(const std::string &frame_id) { this->frame_id = frame_id; }; - - void registerPub(const ros::Publisher &pub) { value_pub = pub; }; - - void registerGTPub(const ros::Publisher &pub) { gt_pub = pub; }; - - std::string frame_id; - - ros::Publisher gt_pub; - ros::Publisher value_pub; - - // Noise params - double mean[3]; - double sigma[3]; - - uint8_t is_set = 0; // 0 for unset, otherwise binary code for combination of dims -}; - -using SensorConfigPtr = std::unique_ptr; - class MujocoRosSensorsPlugin : public mujoco_ros::MujocoPlugin { public: @@ -91,10 +67,11 @@ class MujocoRosSensorsPlugin : public mujoco_ros::MujocoPlugin private: ros::NodeHandle sensors_nh_; void initSensors(const mjModel *model, mjData *data); - std::mt19937 rand_generator = std::mt19937(std::random_device{}()); - std::normal_distribution noise_dist; + std::mt19937 rand_generator_ = std::mt19937(std::random_device{}()); + std::normal_distribution noise_dist_; - std::map sensor_map_; + std::map sensor_map_; + std::map enabled_sensors_; ros::ServiceServer register_noise_model_server_; diff --git a/mujoco_ros_sensors/include/mujoco_ros_sensors/sensors.h b/mujoco_ros_sensors/include/mujoco_ros_sensors/sensors.h new file mode 100644 index 00000000..2197a732 --- /dev/null +++ b/mujoco_ros_sensors/include/mujoco_ros_sensors/sensors.h @@ -0,0 +1,229 @@ +/********************************************************************* + * Software License Agreement (BSD License) + * + * Copyright (c) 2023, Bielefeld University + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above + * copyright notice, this list of conditions and the following + * disclaimer in the documentation and/or other materials provided + * with the distribution. + * * Neither the name of Bielefeld University nor the names of its + * contributors may be used to endorse or promote products derived + * from this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; + * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + *********************************************************************/ + +/* Authors: David P. Leins */ + +#pragma once + +#include + +#include +#include +#include +#include +#include +#include +#include + +#include +#include + +#include +#include + +#include + +namespace mujoco_ros::sensors { + +class RosSensorInterfaceBase +{ +public: + RosSensorInterfaceBase(const std::string &frame_id, const std::string &sensor_name, const int dofs) + : frame_id_(frame_id), sensor_name_(sensor_name), dofs_(dofs) + { + if (dofs_ == 0) + return; + + mu_noise_ = new mjtNum[dofs]; + sigma_noise_ = new mjtNum[dofs]; + }; + + void setNoise(const mjtNum *mu_noise, const mjtNum *sigma_noise, uint8_t noise_set); + + ~RosSensorInterfaceBase() + { + if (dofs_ == 0) + return; + + delete[] mu_noise_; + delete[] sigma_noise_; + }; + + virtual void publish(const bool publish_gt, const mjData *data, std::normal_distribution &dist, + std::mt19937 &gen) = 0; + + void unregisterPublishers(bool eval_mode); + + int getType() const { return type_; } + + std::string getFrameId() const { return frame_id_; } + +protected: + std::string frame_id_; + std::string sensor_name_; + + ros::Time last_pub_; + ros::Publisher value_pub_; + ros::Publisher gt_pub_; + + mjtNum *mu_noise_; + mjtNum *sigma_noise_; + + uint dofs_; + int type_ = -1; + + uint8_t noise_set_ = 0; // 0 for unset, otherwise binary code for set dimensions +}; + +template +class RosSensorInterface : public RosSensorInterfaceBase +{ +public: + RosSensorInterface(const std::string &frame_id, const std::string &sensor_name, const int dofs, const int type, + const int adr, const mjtNum cutoff, bool eval_mode, ros::NodeHandle *sensors_nh); + + void publish(const bool publish_gt, const mjData *data, std::normal_distribution &dist, + std::mt19937 &gen) override; + T serialize(const mjData *data); + void addNoise(T &msg, std::normal_distribution &dist, std::mt19937 &gen); + +private: + int adr_; + mjtNum cutoff_; +}; + +class WrenchSensor : public RosSensorInterfaceBase +{ +public: + WrenchSensor(const std::string &sensor_name, std::vector &wrench_sensors, bool eval_mode, + ros::NodeHandle *sensors_nh); + + void publish(const bool publish_gt, const mjData *data, std::normal_distribution &dist, + std::mt19937 &gen) override; + +private: + RosSensorInterface *force_ = nullptr; + RosSensorInterface *torque_ = nullptr; +}; + +class TwistSensor : public RosSensorInterfaceBase +{ +public: + TwistSensor(const std::string &sensor_name, std::vector &twist_sensors, bool eval_mode, + ros::NodeHandle *sensors_nh); + + void publish(const bool publish_gt, const mjData *data, std::normal_distribution &dist, + std::mt19937 &gen) override; + +private: + RosSensorInterface *linear_velocity_ = nullptr; + RosSensorInterface *angular_velocity_ = nullptr; +}; + +class PoseSensor : public RosSensorInterfaceBase +{ +public: + PoseSensor(const std::string &sensor_name, std::vector &pose_sensors, bool eval_mode, + ros::NodeHandle *sensors_nh); + + void publish(const bool publish_gt, const mjData *data, std::normal_distribution &dist, + std::mt19937 &gen) override; + +private: + RosSensorInterface *position_ = nullptr; + RosSensorInterface *orientation_ = nullptr; +}; + +class IMUSensor : public RosSensorInterfaceBase +{ +public: + IMUSensor(const std::string &sensor_name, std::vector &imu_sensors, bool eval_mode, + ros::NodeHandle *sensors_nh); + + void publish(const bool publish_gt, const mjData *data, std::normal_distribution &dist, + std::mt19937 &gen) override; + +private: + RosSensorInterface *linear_acceleration_ = nullptr; + RosSensorInterface *angular_acceleration_ = nullptr; + RosSensorInterface *orientation_ = nullptr; + + mjtNum lin_cov_[9]; + mjtNum ang_cov_[9]; + mjtNum ori_cov_[9]; +}; + +// template impl needs to be in header +template +void RosSensorInterface::publish(const bool publish_gt, const mjData *data, std::normal_distribution &dist, + std::mt19937 &gen) +{ + T msg = serialize(data); + if (publish_gt) { + gt_pub_.publish(msg); + } + addNoise(msg, dist, gen); + value_pub_.publish(msg); +} + +template +RosSensorInterface::RosSensorInterface(const std::string &frame_id, const std::string &sensor_name, const int dofs, + const int type, const int adr, const mjtNum cutoff, bool eval_mode, + ros::NodeHandle *sensors_nh) + : RosSensorInterfaceBase(frame_id, sensor_name, dofs), adr_(adr), cutoff_(cutoff) +{ + value_pub_ = sensors_nh->advertise(sensor_name, 1); + if (!eval_mode) { + gt_pub_ = sensors_nh->advertise(sensor_name + "_GT", 1); + } + type_ = type; +}; + +template +T RosSensorInterface::serialize(const mjData *data) +{ + T msg; + mujoco_ros::sensors::serialize(msg, data, frame_id_, adr_, cutoff_); + return msg; +} + +template +void RosSensorInterface::addNoise(T &msg, std::normal_distribution &dist, std::mt19937 &gen) +{ + mujoco_ros::sensors::addNoise(msg, mu_noise_, sigma_noise_, noise_set_, dist, gen); +} + +extern const char *SENSOR_STRING[35]; + +} // namespace mujoco_ros::sensors diff --git a/mujoco_ros_sensors/include/mujoco_ros_sensors/serialization.h b/mujoco_ros_sensors/include/mujoco_ros_sensors/serialization.h new file mode 100644 index 00000000..8a6c77e3 --- /dev/null +++ b/mujoco_ros_sensors/include/mujoco_ros_sensors/serialization.h @@ -0,0 +1,86 @@ +/********************************************************************* + * Software License Agreement (BSD License) + * + * Copyright (c) 2023, Bielefeld University + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above + * copyright notice, this list of conditions and the following + * disclaimer in the documentation and/or other materials provided + * with the distribution. + * * Neither the name of Bielefeld University nor the names of its + * contributors may be used to endorse or promote products derived + * from this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; + * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + *********************************************************************/ + +/* Authors: David P. Leins */ + +#pragma once + +#include +#include + +#include +#include + +#include +#include + +#include + +#include +#include + +#include + +namespace mujoco_ros::sensors { + +void serialize(geometry_msgs::QuaternionStamped &msg, const mjData *data, const std::string &frame_id, const int &adr, + const mjtNum &cutoff); +void serialize(geometry_msgs::Vector3Stamped &msg, const mjData *data, const std::string &frame_id, const int &adr, + const mjtNum &cutoff); +void serialize(geometry_msgs::PointStamped &msg, const mjData *data, const std::string &frame_id, const int &adr, + const mjtNum &cutoff); +void serialize(mujoco_ros_msgs::ScalarStamped &msg, const mjData *data, const std::string &frame_id, const int &adr, + const mjtNum &cutoff); + +void addNoise(geometry_msgs::Quaternion &msg, const mjtNum *mu_noise, const mjtNum *sigma_noise, + const uint8_t &noise_set, std::normal_distribution &dist, std::mt19937 &gen); +void addNoise(geometry_msgs::QuaternionStamped &msg, const mjtNum *mu_noise, const mjtNum *sigma_noise, + const uint8_t &noise_set, std::normal_distribution &dist, std::mt19937 &gen); + +void addNoise(geometry_msgs::Vector3 &msg, const mjtNum *mu_noise, const mjtNum *sigma_noise, const uint8_t &noise_set, + std::normal_distribution &dist, std::mt19937 &gen); +void addNoise(geometry_msgs::Vector3Stamped &msg, const mjtNum *mu_noise, const mjtNum *sigma_noise, + const uint8_t &noise_set, std::normal_distribution &dist, std::mt19937 &gen); + +void addNoise(geometry_msgs::Point &msg, const mjtNum *mu_noise, const mjtNum *sigma_noise, const uint8_t &noise_set, + std::normal_distribution &dist, std::mt19937 &gen); +void addNoise(geometry_msgs::PointStamped &msg, const mjtNum *mu_noise, const mjtNum *sigma_noise, + const uint8_t &noise_set, std::normal_distribution &dist, std::mt19937 &gen); + +void addNoise(double &value, const mjtNum *mu_noise, const mjtNum *sigma_noise, const uint8_t &noise_set, + std::normal_distribution &dist, std::mt19937 &gen); +void addNoise(mujoco_ros_msgs::ScalarStamped &msg, const mjtNum *mu_noise, const mjtNum *sigma_noise, + const uint8_t &noise_set, std::normal_distribution &dist, std::mt19937 &gen); + +} // namespace mujoco_ros::sensors diff --git a/mujoco_ros_sensors/src/compound_sensors.cpp b/mujoco_ros_sensors/src/compound_sensors.cpp new file mode 100644 index 00000000..8dabe21d --- /dev/null +++ b/mujoco_ros_sensors/src/compound_sensors.cpp @@ -0,0 +1,314 @@ +/** + * Software License Agreement (BSD 3-Clause License) + * + * Copyright (c) 2023, Bielefeld University + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * + * * Redistributions in binary form must reproduce the above + * copyright notice, this list of conditions and the following + * disclaimer in the documentation and/or other materials provided + * with the distribution. + * + * * Neither the name of Bielefeld University nor the names of its + * contributors may be used to endorse or promote products derived + * from this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; + * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + */ + +/* Authors: David P. Leins */ + +#include +#include + +#include + +namespace mujoco_ros::sensors { + +void RosSensorInterfaceBase::unregisterPublishers(bool eval_mode) +{ + value_pub_.shutdown(); + if (!eval_mode) { + gt_pub_.shutdown(); + } +} + +/////////// Wrench /////////// + +WrenchSensor::WrenchSensor(const std::string &sensor_name, std::vector &wrench_sensors, + bool eval_mode, ros::NodeHandle *sensors_nh) + : RosSensorInterfaceBase("world", sensor_name, 0) +{ + value_pub_ = sensors_nh->advertise(sensor_name, 1); + if (!eval_mode) { + gt_pub_ = sensors_nh->advertise(sensor_name + "_GT", 1); + } + + frame_id_ = wrench_sensors[0]->getFrameId(); + for (const auto &sensor : wrench_sensors) { + if (frame_id_ != sensor->getFrameId()) { + ROS_ERROR_STREAM("Wrench sensor " << sensor_name << " has sensors with different frame ids. Expected " + << frame_id_ << ", but got " << sensor->getFrameId() << " instead"); + throw std::runtime_error("Invalid wrench sensor configuration."); + } + if (sensor->getType() == mjtSensor::mjSENS_FORCE) { + if (force_ != nullptr) { + ROS_ERROR_STREAM("Wrench sensor " << sensor_name << " has multiple force sensors."); + throw std::runtime_error("Invalid wrench sensor configuration."); + } + force_ = dynamic_cast *>(sensor); + } else if (sensor->getType() == mjtSensor::mjSENS_TORQUE) { + if (torque_ != nullptr) { + ROS_ERROR_STREAM("Wrench sensor " << sensor_name << " has multiple torque sensors."); + throw std::runtime_error("Invalid wrench sensor configuration."); + } + torque_ = dynamic_cast *>(sensor); + } else { + ROS_ERROR_STREAM("Wrench sensor " << sensor_name << " has an invalid sensor type (" + << SENSOR_STRING[sensor->getType()] << ")."); + throw std::runtime_error("Invalid wrench sensor configuration."); + } + } +} + +void WrenchSensor::publish(const bool publish_gt, const mjData *data, std::normal_distribution &dist, + std::mt19937 &gen) +{ + geometry_msgs::WrenchStamped msg; + msg.header.stamp = ros::Time::now(); + msg.header.frame_id = frame_id_; + + auto f = force_->serialize(data); + auto t = torque_->serialize(data); + + msg.wrench.force = f.vector; + msg.wrench.torque = t.vector; + + if (publish_gt) { + gt_pub_.publish(msg); + } + + force_->addNoise(f, dist, gen); + torque_->addNoise(t, dist, gen); + + value_pub_.publish(msg); +} + +/////////// Twist /////////// + +TwistSensor::TwistSensor(const std::string &sensor_name, std::vector &twist_sensors, + bool eval_mode, ros::NodeHandle *sensors_nh) + : RosSensorInterfaceBase("world", sensor_name, 0) +{ + value_pub_ = sensors_nh->advertise(sensor_name, 1); + if (!eval_mode) { + gt_pub_ = sensors_nh->advertise(sensor_name + "_GT", 1); + } + + frame_id_ = twist_sensors[0]->getFrameId(); + for (const auto &sensor : twist_sensors) { + if (frame_id_ != sensor->getFrameId()) { + ROS_ERROR_STREAM("Twist sensor " << sensor_name << " has sensors with different frame ids. Expected " + << frame_id_ << ", but got " << sensor->getFrameId() << " instead"); + throw std::runtime_error("Invalid twist sensor configuration."); + } + if (sensor->getType() == mjtSensor::mjSENS_FRAMELINVEL) { + if (linear_velocity_ != nullptr) { + ROS_ERROR_STREAM("Twist sensor " << sensor_name << " has multiple linear velocity sensors."); + throw std::runtime_error("Invalid twist sensor configuration."); + } + linear_velocity_ = dynamic_cast *>(sensor); + } else if (sensor->getType() == mjtSensor::mjSENS_FRAMEANGVEL) { + if (angular_velocity_ != nullptr) { + ROS_ERROR_STREAM("Twist sensor " << sensor_name << " has multiple angular velocity sensors."); + throw std::runtime_error("Invalid twist sensor configuration."); + } + angular_velocity_ = dynamic_cast *>(sensor); + } else { + ROS_ERROR_STREAM("Twist sensor " << sensor_name << " has an invalid sensor type (" + << SENSOR_STRING[sensor->getType()] << ")."); + throw std::runtime_error("Invalid twist sensor configuration."); + } + } +} + +void TwistSensor::publish(const bool publish_gt, const mjData *data, std::normal_distribution &dist, + std::mt19937 &gen) +{ + geometry_msgs::TwistStamped msg; + msg.header.stamp = ros::Time::now(); + msg.header.frame_id = frame_id_; + + auto lv = linear_velocity_->serialize(data); + auto av = angular_velocity_->serialize(data); + + msg.twist.linear = lv.vector; + msg.twist.angular = av.vector; + + if (publish_gt) { + gt_pub_.publish(msg); + } + + linear_velocity_->addNoise(lv, dist, gen); + angular_velocity_->addNoise(av, dist, gen); + + value_pub_.publish(msg); +} + +/////////// Pose /////////// + +PoseSensor::PoseSensor(const std::string &sensor_name, std::vector &pose_sensors, + bool eval_mode, ros::NodeHandle *sensors_nh) + : RosSensorInterfaceBase("world", sensor_name, 0) +{ + value_pub_ = sensors_nh->advertise(sensor_name, 1); + if (!eval_mode) { + gt_pub_ = sensors_nh->advertise(sensor_name + "_GT", 1); + } + + frame_id_ = pose_sensors[0]->getFrameId(); + for (const auto &sensor : pose_sensors) { + if (frame_id_ != sensor->getFrameId()) { + ROS_ERROR_STREAM("Pose sensor " << sensor_name << " has sensors with different frame ids. Expected " + << frame_id_ << ", but got " << sensor->getFrameId() << " instead"); + throw std::runtime_error("Invalid pose sensor configuration."); + } + if (sensor->getType() == mjtSensor::mjSENS_FRAMEPOS) { + if (position_ != nullptr) { + ROS_ERROR_STREAM("Pose sensor " << sensor_name << " has multiple position sensors."); + throw std::runtime_error("Invalid pose sensor configuration."); + } + position_ = dynamic_cast *>(sensor); + } else if (sensor->getType() == mjtSensor::mjSENS_FRAMEQUAT) { + if (orientation_ != nullptr) { + ROS_ERROR_STREAM("Pose sensor " << sensor_name << " has multiple orientation sensors."); + throw std::runtime_error("Invalid pose sensor configuration."); + } + orientation_ = dynamic_cast *>(sensor); + } else { + ROS_ERROR_STREAM("Pose sensor " << sensor_name << " has an invalid sensor type (" + << SENSOR_STRING[sensor->getType()] << ")."); + throw std::runtime_error("Invalid pose sensor configuration."); + } + } +} + +void PoseSensor::publish(const bool publish_gt, const mjData *data, std::normal_distribution &dist, + std::mt19937 &gen) +{ + geometry_msgs::PoseStamped msg; + msg.header.stamp = ros::Time::now(); + msg.header.frame_id = frame_id_; + + auto p = position_->serialize(data); + auto o = orientation_->serialize(data); + + msg.pose.position = p.point; + msg.pose.orientation = o.quaternion; + + if (publish_gt) { + gt_pub_.publish(msg); + } + + position_->addNoise(p, dist, gen); + orientation_->addNoise(o, dist, gen); + + value_pub_.publish(msg); +} + +/////////// IMU /////////// + +IMUSensor::IMUSensor(const std::string &sensor_name, std::vector &imu_sensors, bool eval_mode, + ros::NodeHandle *sensors_nh) + : RosSensorInterfaceBase("world", sensor_name, 0) +{ + value_pub_ = sensors_nh->advertise(sensor_name, 1); + if (!eval_mode) { + gt_pub_ = sensors_nh->advertise(sensor_name + "_GT", 1); + } + + if (imu_sensors.size() != 3) { + ROS_ERROR_STREAM("IMU sensor " << sensor_name << " has " << imu_sensors.size() + << " sensors, but exactly 3 are required."); + throw std::runtime_error("Invalid IMU sensor configuration."); + } + + frame_id_ = imu_sensors[0]->getFrameId(); + for (const auto &sensor : imu_sensors) { + if (frame_id_ != sensor->getFrameId()) { + ROS_ERROR_STREAM("IMU sensor " << sensor_name << " has sensors with different frame ids. Expected " + << frame_id_ << ", but got " << sensor->getFrameId() << " instead"); + throw std::runtime_error("Invalid IMU sensor configuration."); + } + if (sensor->getType() == mjtSensor::mjSENS_FRAMELINACC || sensor->getType() == mjtSensor::mjSENS_ACCELEROMETER) { + if (linear_acceleration_ != nullptr) { + ROS_ERROR_STREAM("IMU sensor " << sensor_name << " has multiple linear acceleration sensors."); + throw std::runtime_error("Invalid IMU sensor configuration."); + } + linear_acceleration_ = dynamic_cast *>(sensor); + } else if (sensor->getType() == mjtSensor::mjSENS_FRAMEANGACC || sensor->getType() == mjtSensor::mjSENS_GYRO || + sensor->getType() == mjtSensor::mjSENS_BALLANGVEL) { + if (angular_acceleration_ != nullptr) { + ROS_ERROR_STREAM("IMU sensor " << sensor_name << " has multiple angular acceleration sensors."); + throw std::runtime_error("Invalid IMU sensor configuration."); + } + angular_acceleration_ = dynamic_cast *>(sensor); + } else if (sensor->getType() == mjtSensor::mjSENS_FRAMEQUAT || sensor->getType() == mjtSensor::mjSENS_BALLQUAT) { + if (orientation_ != nullptr) { + ROS_ERROR_STREAM("IMU sensor " << sensor_name << " has multiple orientation sensors."); + throw std::runtime_error("Invalid IMU sensor configuration."); + } + orientation_ = dynamic_cast *>(sensor); + } else { + ROS_ERROR_STREAM("IMU sensor " << sensor_name << " has an invalid sensor type (" + << SENSOR_STRING[sensor->getType()] << ")."); + throw std::runtime_error("Invalid IMU sensor configuration."); + } + } +} + +void IMUSensor::publish(const bool publish_gt, const mjData *data, std::normal_distribution &dist, + std::mt19937 &gen) +{ + sensor_msgs::Imu msg; + msg.header.stamp = ros::Time::now(); + msg.header.frame_id = frame_id_; + + auto la = linear_acceleration_->serialize(data); + auto aa = angular_acceleration_->serialize(data); + auto o = orientation_->serialize(data); + + msg.linear_acceleration = la.vector; + msg.angular_velocity = aa.vector; + msg.orientation = o.quaternion; + + if (publish_gt) { + gt_pub_.publish(msg); + } + + linear_acceleration_->addNoise(la, dist, gen); + angular_acceleration_->addNoise(aa, dist, gen); + orientation_->addNoise(o, dist, gen); + + value_pub_.publish(msg); +} + +} // namespace mujoco_ros::sensors diff --git a/mujoco_ros_sensors/src/mujoco_sensor_handler_plugin.cpp b/mujoco_ros_sensors/src/mujoco_sensor_handler_plugin.cpp index 0a524a55..2418bd97 100644 --- a/mujoco_ros_sensors/src/mujoco_sensor_handler_plugin.cpp +++ b/mujoco_ros_sensors/src/mujoco_sensor_handler_plugin.cpp @@ -37,6 +37,7 @@ /* Authors: David P. Leins */ #include +#include #include @@ -45,14 +46,44 @@ #include #include -#include - #include +#include + namespace mujoco_ros::sensors { +void RosSensorInterfaceBase::setNoise(const mjtNum *mu_noise, const mjtNum *sigma_noise, uint8_t noise_set) +{ + uint noise_count = noise_set - ((noise_set >> 1) & 033333333333) - ((noise_set >> 2) & 011111111111); + noise_count = ((noise_count + (noise_count >> 3)) & 030707070707) % 63; + if (noise_count > dofs_) { + throw std::runtime_error("Too many noise parameters provided for sensor " + sensor_name_); + } + + int noise_idx = 0; + if (noise_set & 0x01) { + mu_noise_[noise_idx] = mu_noise[noise_idx]; + sigma_noise_[noise_idx] = sigma_noise[noise_idx]; + noise_idx++; + } + if (noise_set & 0x02) { + mu_noise_[noise_idx] = mu_noise[noise_idx]; + sigma_noise_[noise_idx] = sigma_noise[noise_idx]; + noise_idx++; + } + if (noise_set & 0x04) { + mu_noise_[noise_idx] = mu_noise[noise_idx]; + sigma_noise_[noise_idx] = sigma_noise[noise_idx]; + } + + noise_set_ = noise_set_ | noise_set; +} + MujocoRosSensorsPlugin::~MujocoRosSensorsPlugin() { + for (auto &sensor : sensor_map_) { + delete sensor.second; + } sensor_map_.clear(); ROS_DEBUG_STREAM_NAMED("sensors", "Shutting down service " << register_noise_model_server_.getService()); register_noise_model_server_.shutdown(); @@ -62,7 +93,7 @@ bool MujocoRosSensorsPlugin::load(const mjModel *model, mjData *data) { ROS_INFO_NAMED("sensors", "Loading sensors plugin ..."); if (env_ptr_->settings_.eval_mode) { - ROS_WARN_NAMED("sensors", "Evalutaion mode is active, ground truth topics won't be available!"); + ROS_WARN_NAMED("sensors", "Evaluation mode is active, ground truth topics won't be available!"); } else { ROS_WARN_NAMED("sensors", "Train mode is active, ground truth topics will be available!"); } @@ -109,7 +140,7 @@ bool MujocoRosSensorsPlugin::load(const mjModel *model, mjData *data) } sensors_nh_ = ros::NodeHandle("/" + sensors_namespace); - noise_dist = std::normal_distribution(0.0, 1.0); + noise_dist_ = std::normal_distribution(0.0, 1.0); initSensors(model, data); ROS_INFO_NAMED("sensors", "All sensors initialized"); @@ -132,42 +163,31 @@ bool MujocoRosSensorsPlugin::registerNoiseModelsCB(mujoco_ros_msgs::RegisterSens ROS_DEBUG_NAMED("mujoco", "Hash valid, request authorized."); } - int noise_idx; + resp.success = true; for (const mujoco_ros_msgs::SensorNoiseModel &noise_model : req.noise_models) { ROS_WARN_STREAM_NAMED("sensors", "registering noise model for " << noise_model.sensor_name); - noise_idx = 0; - const std::map::const_iterator &pos = sensor_map_.find(noise_model.sensor_name); + const std::map::const_iterator &pos = + sensor_map_.find(noise_model.sensor_name); if (pos == sensor_map_.end()) { ROS_WARN_STREAM_NAMED("sensors", "No sensor with name '" << noise_model.sensor_name << "' was registered on init. Can not apply noise model"); + resp.success = false; continue; } - const SensorConfigPtr &config = pos->second; - ROS_DEBUG_STREAM_COND_NAMED(config->is_set > 0, "sensors", "Overriding current noise model with newly provided"); - - if (noise_model.set_flag & 0x01) { - config->mean[noise_idx] = noise_model.mean[noise_idx]; - config->sigma[noise_idx] = noise_model.std[noise_idx]; - noise_idx += 1; - } - if (noise_model.set_flag & 0x02) { - config->mean[noise_idx] = noise_model.mean[noise_idx]; - config->sigma[noise_idx] = noise_model.std[noise_idx]; - noise_idx += 1; - } - if (noise_model.set_flag & 0x04) { - config->mean[noise_idx] = noise_model.mean[noise_idx]; - config->sigma[noise_idx] = noise_model.std[noise_idx]; + RosSensorInterfaceBase *sensor = pos->second; + try { + sensor->setNoise(noise_model.mean.data(), noise_model.std.data(), noise_model.set_flag); + } catch (const std::runtime_error &e) { + ROS_ERROR_STREAM_NAMED("sensors", "Error while setting noise model for sensor " << noise_model.sensor_name + << ": " << e.what()); + resp.success = false; + return true; } - - config->is_set = config->is_set | noise_model.set_flag; } - resp.success = true; - return true; } @@ -175,262 +195,11 @@ void MujocoRosSensorsPlugin::lastStageCallback(const mjModel *model, mjData *dat { std::string sensor_name; - int adr, type, noise_idx; - mjtNum cutoff; - double noise = 0.0; - - for (int n = 0; n < model->nsensor; n++) { - adr = model->sensor_adr[n]; - type = model->sensor_type[n]; - cutoff = (model->sensor_cutoff[n] > 0 ? model->sensor_cutoff[n] : 1); - noise_idx = 0; - - if (model->names[model->name_sensoradr[n]]) { - sensor_name = mj_id2name(const_cast(model), mjOBJ_SENSOR, n); - } else { - continue; - } - - if (sensor_map_.find(sensor_name) == sensor_map_.end()) - continue; - - SensorConfigPtr &config = sensor_map_[sensor_name]; - - switch (type) { - { - case mjSENS_FRAMELINVEL: - case mjSENS_FRAMELINACC: - case mjSENS_FRAMEANGACC: - case mjSENS_SUBTREECOM: - case mjSENS_SUBTREELINVEL: - case mjSENS_SUBTREEANGMOM: - case mjSENS_ACCELEROMETER: - case mjSENS_VELOCIMETER: - case mjSENS_GYRO: - case mjSENS_FORCE: - case mjSENS_TORQUE: - case mjSENS_MAGNETOMETER: - case mjSENS_BALLANGVEL: - case mjSENS_FRAMEXAXIS: - case mjSENS_FRAMEYAXIS: - case mjSENS_FRAMEZAXIS: - geometry_msgs::Vector3Stamped msg; - msg.header.frame_id = config->frame_id; - msg.header.stamp = ros::Time::now(); - - // No noise configured - if (config->is_set == 0) { - msg.vector.x = static_cast(data->sensordata[adr] / cutoff); - msg.vector.y = static_cast(data->sensordata[adr + 1] / cutoff); - msg.vector.z = static_cast(data->sensordata[adr + 2] / cutoff); - - config->value_pub.publish(msg); - - if (!env_ptr_->settings_.eval_mode) { - config->gt_pub.publish(msg); - } - } else { // Noise at least in one dim - if (config->is_set & 0x01) { - // shift and scale standard normal to desired distribution - noise = noise_dist(rand_generator) * config->sigma[noise_idx] + config->mean[noise_idx]; - noise_idx += 1; - } else { - noise = 0; - } - msg.vector.x = static_cast(data->sensordata[adr] + noise / cutoff); - - if (config->is_set & 0x02) { - // shift and scale standard normal to desired distribution - noise = noise_dist(rand_generator) * config->sigma[noise_idx] + config->mean[noise_idx]; - noise_idx += 1; - } else { - noise = 0; - } - msg.vector.y = (float)(data->sensordata[adr + 1] + noise / cutoff); - - if (config->is_set & 0x04) { - // shift and scale standard normal to desired distribution - noise = noise_dist(rand_generator) * config->sigma[noise_idx] + config->mean[noise_idx]; - } else { - noise = 0; - } - msg.vector.z = (float)(data->sensordata[adr + 2] + noise / cutoff); - - config->value_pub.publish(msg); - - if (!env_ptr_->settings_.eval_mode) { - msg.vector.x = static_cast(data->sensordata[adr] / cutoff); - msg.vector.y = static_cast(data->sensordata[adr + 1] / cutoff); - msg.vector.z = static_cast(data->sensordata[adr + 2] / cutoff); - - config->gt_pub.publish(msg); - } - } - break; - } - - case mjSENS_FRAMEPOS: { - geometry_msgs::PointStamped msg; - msg.header.frame_id = config->frame_id; - msg.header.stamp = ros::Time::now(); - - // No noise configured - if (config->is_set == 0) { - msg.point.x = static_cast(data->sensordata[adr] / cutoff); - msg.point.y = static_cast(data->sensordata[adr + 1] / cutoff); - msg.point.z = static_cast(data->sensordata[adr + 2] / cutoff); - - config->value_pub.publish(msg); - - if (!env_ptr_->settings_.eval_mode) { - config->gt_pub.publish(msg); - } - } else { // Noise at least in one dim - if (config->is_set & 0x01) { - // shift and scale standard normal to desired distribution - noise = noise_dist(rand_generator) * config->sigma[noise_idx] + config->mean[noise_idx]; - noise_idx += 1; - } else { - noise = 0; - } - msg.point.x = static_cast(data->sensordata[adr] + noise / cutoff); - - if (config->is_set & 0x02) { - // shift and scale standard normal to desired distribution - noise = noise_dist(rand_generator) * config->sigma[noise_idx] + config->mean[noise_idx]; - noise_idx += 1; - } else { - noise = 0; - } - msg.point.y = static_cast(data->sensordata[adr + 1] + noise / cutoff); - - if (config->is_set & 0x04) { - // shift and scale standard normal to desired distribution - noise = noise_dist(rand_generator) * config->sigma[noise_idx] + config->mean[noise_idx]; - } else { - noise = 0; - } - msg.point.z = static_cast(data->sensordata[adr + 2] + noise / cutoff); - - config->value_pub.publish(msg); - - if (!env_ptr_->settings_.eval_mode) { - msg.point.x = static_cast(data->sensordata[adr] / cutoff); - msg.point.y = static_cast(data->sensordata[adr + 1] / cutoff); - msg.point.z = static_cast(data->sensordata[adr + 2] / cutoff); - - config->gt_pub.publish(msg); - } - } - break; - } - - { - case mjSENS_TOUCH: - case mjSENS_RANGEFINDER: - case mjSENS_JOINTPOS: - case mjSENS_JOINTVEL: - case mjSENS_TENDONPOS: - case mjSENS_TENDONVEL: - case mjSENS_ACTUATORPOS: - case mjSENS_ACTUATORVEL: - case mjSENS_ACTUATORFRC: - case mjSENS_JOINTLIMITPOS: - case mjSENS_JOINTLIMITVEL: - case mjSENS_JOINTLIMITFRC: - case mjSENS_TENDONLIMITPOS: - case mjSENS_TENDONLIMITVEL: - case mjSENS_TENDONLIMITFRC: - mujoco_ros_msgs::ScalarStamped msg; - msg.header.frame_id = config->frame_id; - msg.header.stamp = ros::Time::now(); - - // No noise configured - if (config->is_set == 0) { - msg.value = static_cast(data->sensordata[adr] / cutoff); - - config->value_pub.publish(msg); - - if (!env_ptr_->settings_.eval_mode) { - config->gt_pub.publish(msg); - } - } else { // Noise set - // shift and scale standard normal to desired distribution - noise = noise_dist(rand_generator) * config->sigma[0] + config->mean[0]; - msg.value = static_cast(data->sensordata[adr] + noise / cutoff); - - config->value_pub.publish(msg); - - if (!env_ptr_->settings_.eval_mode) { - msg.value = static_cast(data->sensordata[adr] / cutoff); - - config->gt_pub.publish(msg); - } - } - break; - } - - case mjSENS_BALLQUAT: { - case mjSENS_FRAMEQUAT: - geometry_msgs::QuaternionStamped msg; - tf2::Quaternion q_orig, q_rot; - msg.header.frame_id = config->frame_id; - msg.header.stamp = ros::Time::now(); - - msg.quaternion.w = static_cast(data->sensordata[adr] / cutoff); - msg.quaternion.x = static_cast(data->sensordata[adr + 1] / cutoff); - msg.quaternion.y = static_cast(data->sensordata[adr + 2] / cutoff); - msg.quaternion.z = static_cast(data->sensordata[adr + 3] / cutoff); - - if (!env_ptr_->settings_.eval_mode) { - config->gt_pub.publish(msg); - } - - if (config->is_set == 0) { - config->value_pub.publish(msg); - } else { - tf2::fromMsg(msg.quaternion, q_orig); - q_orig.normalize(); - - double r, p, y; - - if (config->is_set & 0x01) { - // shift and scale standard normal to desired distribution - r = noise_dist(rand_generator) * config->sigma[noise_idx] + config->mean[noise_idx]; - noise_idx += 1; - } else { - r = 0; - } - if (config->is_set & 0x02) { - // shift and scale standard normal to desired distribution - p = noise_dist(rand_generator) * config->sigma[noise_idx] + config->mean[noise_idx]; - noise_idx += 1; - } else { - p = 0; - } - if (config->is_set & 0x04) { - // shift and scale standard normal to desired distribution - y = noise_dist(rand_generator) * config->sigma[noise_idx] + config->mean[noise_idx]; - } else { - y = 0; - } - - q_rot.setRPY(r, p, y); - q_rot.normalize(); - - msg.quaternion = tf2::toMsg((q_rot * q_orig).normalize()); - config->value_pub.publish(msg); - } - break; - } - - default: - ROS_ERROR_STREAM_NAMED( - "sensors", - "Sensor publisher and frame_id defined but type can't be serialized. This shouldn't happen! (" - << sensor_name << " of type " << type << ")"); - break; - } + std::map::iterator pos = enabled_sensors_.begin(); + while (pos != enabled_sensors_.end()) { + RosSensorInterfaceBase *sensor = pos->second; + sensor->publish(!env_ptr_->settings_.eval_mode, data, noise_dist_, rand_generator_); + pos++; } } @@ -441,6 +210,8 @@ void MujocoRosSensorsPlugin::initSensors(const mjModel *model, mjData *data) int site_id = model->sensor_objid[n]; int parent_id = model->site_bodyid[site_id]; int type = model->sensor_type[n]; + mjtNum cutoff = (model->sensor_cutoff[n] > 0 ? model->sensor_cutoff[n] : 1); + int adr = model->sensor_adr[n]; site = mj_id2name(const_cast(model), model->sensor_objtype[n], site_id); @@ -452,16 +223,16 @@ void MujocoRosSensorsPlugin::initSensors(const mjModel *model, mjData *data) continue; } - // Global frame sensors - bool global_frame = false; - frame_id = "world"; - SensorConfigPtr config; + bool matched = false; + frame_id = "world"; switch (type) { + // Global or relative frame 3 DoF sensors { case mjSENS_FRAMEXAXIS: case mjSENS_FRAMEYAXIS: case mjSENS_FRAMEZAXIS: case mjSENS_FRAMELINVEL: + case mjSENS_FRAMEANGVEL: case mjSENS_FRAMELINACC: case mjSENS_FRAMEANGACC: int refid = model->sensor_refid[n]; @@ -476,27 +247,20 @@ void MujocoRosSensorsPlugin::initSensors(const mjModel *model, mjData *data) << reftype << " and ref_frame " << frame_id); } - config = std::make_unique(frame_id); - config->registerPub(sensors_nh_.advertise(sensor_name, 1, true)); - if (!env_ptr_->settings_.eval_mode) { - config->registerGTPub( - sensors_nh_.advertise(sensor_name + "_GT", 1, true)); - } - sensor_map_[sensor_name] = std::move(config); + sensor_map_[sensor_name] = new RosSensorInterface( + frame_id, sensor_name, 3, type, adr, cutoff, env_ptr_->settings_.eval_mode, &sensors_nh_); + matched = true; break; } + // Global frame 3 DoF sensors case mjSENS_SUBTREECOM: case mjSENS_SUBTREELINVEL: case mjSENS_SUBTREEANGMOM: - config = std::make_unique(frame_id); - config->registerPub(sensors_nh_.advertise(sensor_name, 1, true)); - if (!env_ptr_->settings_.eval_mode) { - config->registerGTPub( - sensors_nh_.advertise(sensor_name + "_GT", 1, true)); - } - sensor_map_[sensor_name] = std::move(config); - global_frame = true; + sensor_map_[sensor_name] = new RosSensorInterface( + frame_id, sensor_name, 3, type, adr, cutoff, env_ptr_->settings_.eval_mode, &sensors_nh_); + matched = true; break; + // Global or local frame position sensors (1 DoF) { case mjSENS_FRAMEPOS: int refid = model->sensor_refid[n]; @@ -511,42 +275,35 @@ void MujocoRosSensorsPlugin::initSensors(const mjModel *model, mjData *data) << refid << " and type " << reftype << " and ref_frame " << frame_id); } - config = std::make_unique(frame_id); - config->registerPub(sensors_nh_.advertise(sensor_name, 1, true)); - if (!env_ptr_->settings_.eval_mode) { - config->registerGTPub( - sensors_nh_.advertise(sensor_name + "_GT", 1, true)); - } - sensor_map_[sensor_name] = std::move(config); - global_frame = true; + sensor_map_[sensor_name] = new RosSensorInterface( + frame_id, sensor_name, 3, type, adr, cutoff, env_ptr_->settings_.eval_mode, &sensors_nh_); + matched = true; break; } + // Global frame quaternion sensors (4 DoF) case mjSENS_BALLQUAT: case mjSENS_FRAMEQUAT: - config = std::make_unique(frame_id); - config->registerPub(sensors_nh_.advertise(sensor_name, 1, true)); - if (!env_ptr_->settings_.eval_mode) { - config->registerGTPub( - sensors_nh_.advertise(sensor_name + "_GT", 1, true)); - } - sensor_map_[sensor_name] = std::move(config); - global_frame = true; + sensor_map_[sensor_name] = new RosSensorInterface( + frame_id, sensor_name, 3, type, adr, cutoff, env_ptr_->settings_.eval_mode, &sensors_nh_); + matched = true; break; } - // Check if sensor is in global frame and already setup - if (global_frame || frame_id != "world") { + // If the sensor is already registered and we can skip checking the remaining types + if (matched) { ROS_DEBUG_STREAM_NAMED("sensors", "Setting up sensor " << sensor_name << " on site " << site << " (frame_id: " << frame_id << ") of type " << SENSOR_STRING[type]); continue; } + // The remaining sensors all measure in the site's parent body frame frame_id = mj_id2name(const_cast(model), mjOBJ_BODY, parent_id); ROS_DEBUG_STREAM_NAMED("sensors", "Setting up sensor " << sensor_name << " on site " << site << " (frame_id: " << frame_id << ") of type " << SENSOR_STRING[type]); switch (type) { + // 3D-vector type sensors case mjSENS_ACCELEROMETER: case mjSENS_VELOCIMETER: case mjSENS_GYRO: @@ -554,15 +311,11 @@ void MujocoRosSensorsPlugin::initSensors(const mjModel *model, mjData *data) case mjSENS_TORQUE: case mjSENS_MAGNETOMETER: case mjSENS_BALLANGVEL: - config = std::make_unique(frame_id); - config->registerPub(sensors_nh_.advertise(sensor_name, 1, true)); - if (!env_ptr_->settings_.eval_mode) { - config->registerGTPub( - sensors_nh_.advertise(sensor_name + "_GT", 1, true)); - } - sensor_map_[sensor_name] = std::move(config); + sensor_map_[sensor_name] = new RosSensorInterface( + frame_id, sensor_name, 3, type, adr, cutoff, env_ptr_->settings_.eval_mode, &sensors_nh_); break; + // Scalar type sensors case mjSENS_TOUCH: case mjSENS_RANGEFINDER: case mjSENS_JOINTPOS: @@ -578,13 +331,8 @@ void MujocoRosSensorsPlugin::initSensors(const mjModel *model, mjData *data) case mjSENS_TENDONLIMITPOS: case mjSENS_TENDONLIMITVEL: case mjSENS_TENDONLIMITFRC: - config = std::make_unique(frame_id); - config->registerPub(sensors_nh_.advertise(sensor_name, 1, true)); - if (!env_ptr_->settings_.eval_mode) { - config->registerGTPub( - sensors_nh_.advertise(sensor_name + "_GT", 1, true)); - } - sensor_map_[sensor_name] = std::move(config); + sensor_map_[sensor_name] = new RosSensorInterface( + frame_id, sensor_name, 1, type, adr, cutoff, env_ptr_->settings_.eval_mode, &sensors_nh_); break; default: @@ -593,6 +341,88 @@ void MujocoRosSensorsPlugin::initSensors(const mjModel *model, mjData *data) break; } } + + // Copy all found sensors into enabled_sensors_ map + enabled_sensors_ = sensor_map_; + + if (!rosparam_config_.hasMember("compound_sensors")) { + ROS_DEBUG_NAMED("sensors", "No compound sensors configured"); + return; + } + ROS_DEBUG_STREAM("Processing compound sensors (n = " << rosparam_config_["compound_sensors"].size() << ")"); + for (uint i = 0; i < rosparam_config_["compound_sensors"].size(); i++) { + const auto compound_sensor = rosparam_config_["compound_sensors"][i]; + + // Check if definition contains name, type, and sensors, otherwise skip + if (!compound_sensor.hasMember("name")) { + ROS_ERROR_STREAM_NAMED("sensors", "Compound sensor definition " << i << " has no name. Skipping"); + continue; + } + std::string compound_sensor_name = static_cast(compound_sensor["name"]); + if (!compound_sensor.hasMember("type") || + (compound_sensor["type"] != "IMUSensor" && compound_sensor["type"] != "WrenchSensor" && + compound_sensor["type"] != "TwistSensor" && compound_sensor["type"] != "PoseSensor")) { + ROS_ERROR_STREAM_NAMED("sensors", + "Compound sensor '" << compound_sensor_name << "' has no valid type. Skipping"); + continue; + } + if (!compound_sensor.hasMember("sensors")) { + ROS_ERROR_STREAM_NAMED("sensors", "Compound sensor '" << compound_sensor_name << "' has no sensors. Skipping"); + continue; + } + + ROS_DEBUG_STREAM_NAMED("sensors", "Setting up compound sensor " << compound_sensor_name); + + std::vector compound_sensor_interfaces; + for (uint j = 0; j < compound_sensor["sensors"].size(); j++) { + const auto &sub_sensor = compound_sensor["sensors"][j]; + ROS_DEBUG_STREAM("Processing sub sensor " << sub_sensor << " of compound sensor " << compound_sensor_name); + const std::map::const_iterator &pos = sensor_map_.find(sub_sensor); + if (pos == sensor_map_.end()) { + ROS_ERROR_STREAM_NAMED("sensors", "No sensor with name '" << sub_sensor + << "' was registered on init. Skipping " + "compound sensor"); + continue; + } + // Remove sensor used in compound sensor from enabled_sensors_ map + compound_sensor_interfaces.emplace_back(pos->second); + enabled_sensors_.erase(sub_sensor); + // Unregister publishers for sensor + pos->second->unregisterPublishers(env_ptr_->settings_.eval_mode); + ROS_DEBUG_STREAM("Unregistered publishers for sensor " << sub_sensor); + } + + // TODO(dleins): Use factory pattern for better maintainability or is that overkill? + RosSensorInterfaceBase *compound_sensor_inst = nullptr; + try { + if (compound_sensor["type"] == "IMUSensor") { + compound_sensor_inst = new IMUSensor(compound_sensor_name, compound_sensor_interfaces, + env_ptr_->settings_.eval_mode, &sensors_nh_); + } else if (compound_sensor["type"] == "WrenchSensor") { + compound_sensor_inst = new WrenchSensor(compound_sensor_name, compound_sensor_interfaces, + env_ptr_->settings_.eval_mode, &sensors_nh_); + } else if (compound_sensor["type"] == "TwistSensor") { + compound_sensor_inst = new TwistSensor(compound_sensor_name, compound_sensor_interfaces, + env_ptr_->settings_.eval_mode, &sensors_nh_); + } else if (compound_sensor["type"] == "PoseSensor") { + compound_sensor_inst = new PoseSensor(compound_sensor_name, compound_sensor_interfaces, + env_ptr_->settings_.eval_mode, &sensors_nh_); + } + } catch (const std::runtime_error &e) { + ROS_ERROR_STREAM_NAMED("sensors", + "Error while setting up compound sensor " << compound_sensor_name << ": " << e.what()); + continue; + } + + // TODO(dleins): is further composition sensible? Alphabetical sorting of yaml could cause problems + // Add compound sensor to enabled sensors and register for deletion (and further composition ?) + enabled_sensors_[compound_sensor_name] = compound_sensor_inst; + sensor_map_[compound_sensor_name] = compound_sensor_inst; + ROS_DEBUG_STREAM_NAMED("sensors", "Added compound sensor " << compound_sensor_name << " to enabled sensors"); + } + + ROS_DEBUG_STREAM_NAMED("sensors", "Enabled sensors: " << enabled_sensors_.size()); + ROS_DEBUG_STREAM_NAMED("sensors", "All sensors: " << sensor_map_.size()); } // Nothing to do on reset diff --git a/mujoco_ros_sensors/src/serialization.cpp b/mujoco_ros_sensors/src/serialization.cpp new file mode 100644 index 00000000..53624a62 --- /dev/null +++ b/mujoco_ros_sensors/src/serialization.cpp @@ -0,0 +1,213 @@ +/********************************************************************* + * Software License Agreement (BSD License) + * + * Copyright (c) 2023, Bielefeld University + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above + * copyright notice, this list of conditions and the following + * disclaimer in the documentation and/or other materials provided + * with the distribution. + * * Neither the name of Bielefeld University nor the names of its + * contributors may be used to endorse or promote products derived + * from this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; + * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + *********************************************************************/ + +/* Authors: David P. Leins */ + +#include +#include + +namespace mujoco_ros::sensors { + +void serialize(geometry_msgs::QuaternionStamped &msg, const mjData *data, const std::string &frame_id, const int &adr, + const mjtNum &cutoff) +{ + msg.header.stamp = ros::Time::now(); + msg.header.frame_id = frame_id; + msg.quaternion.w = static_cast(data->sensordata[adr] / cutoff); + msg.quaternion.x = static_cast(data->sensordata[adr + 1] / cutoff); + msg.quaternion.y = static_cast(data->sensordata[adr + 2] / cutoff); + msg.quaternion.z = static_cast(data->sensordata[adr + 3] / cutoff); +} + +void serialize(geometry_msgs::Vector3Stamped &msg, const mjData *data, const std::string &frame_id, const int &adr, + const mjtNum &cutoff) +{ + msg.header.stamp = ros::Time::now(); + msg.header.frame_id = frame_id; + msg.vector.x = static_cast(data->sensordata[adr] / cutoff); + msg.vector.y = static_cast(data->sensordata[adr + 1] / cutoff); + msg.vector.z = static_cast(data->sensordata[adr + 2] / cutoff); +} + +void serialize(geometry_msgs::PointStamped &msg, const mjData *data, const std::string &frame_id, const int &adr, + const mjtNum &cutoff) +{ + msg.header.stamp = ros::Time::now(); + msg.header.frame_id = frame_id; + msg.point.x = static_cast(data->sensordata[adr] / cutoff); + msg.point.y = static_cast(data->sensordata[adr + 1] / cutoff); + msg.point.z = static_cast(data->sensordata[adr + 2] / cutoff); +} + +void serialize(mujoco_ros_msgs::ScalarStamped &msg, const mjData *data, const std::string &frame_id, const int &adr, + const mjtNum &cutoff) +{ + msg.header.stamp = ros::Time::now(); + msg.header.frame_id = frame_id; + msg.value = static_cast(data->sensordata[adr] / cutoff); +} + +///////////// Noise addition /////////////// + +////// Quaternion ////// + +void addNoise(geometry_msgs::Quaternion &msg, const mjtNum *mu_noise, const mjtNum *sigma_noise, + const uint8_t &noise_set, std::normal_distribution &dist, std::mt19937 &gen) +{ + if (noise_set == 0) { // no noise, leave unchanged + return; + } + + // For correct application, generate noise in roll, pitch, yaw, then convert to quaternion + tf2::Quaternion q, q_noise; + tf2::fromMsg(msg, q); + q.normalize(); + + double r = 0., p = 0., y = 0.; + int noise_idx = 0; + if (noise_set & 0x01) { // Noise in roll + r = dist(gen) * sigma_noise[noise_idx] + mu_noise[noise_idx]; + noise_idx++; + } + if (noise_set & 0x02) { // Noise in pitch + p = dist(gen) * sigma_noise[noise_idx] + mu_noise[noise_idx]; + noise_idx++; + } + if (noise_set & 0x04) { // Noise in yaw + y = dist(gen) * sigma_noise[noise_idx] + mu_noise[noise_idx]; + } + q_noise.setRPY(r, p, y); + + // Rotate original quaternion by noise quaternion + msg = tf2::toMsg((q_noise * q).normalize()); +} + +void addNoise(geometry_msgs::QuaternionStamped &msg, const mjtNum *mu_noise, const mjtNum *sigma_noise, + const uint8_t &noise_set, std::normal_distribution &dist, std::mt19937 &gen) +{ + if (noise_set == 0) { // no noise, leave unchanged + return; + } + addNoise(msg.quaternion, mu_noise, sigma_noise, noise_set, dist, gen); +} + +////// Vector3 ////// + +void addNoise(geometry_msgs::Vector3 &msg, const mjtNum *mu_noise, const mjtNum *sigma_noise, const uint8_t &noise_set, + std::normal_distribution &dist, std::mt19937 &gen) +{ + if (noise_set == 0) { // no noise, leave unchanged + return; + } + + int noise_idx = 0; + if (noise_set & 0x01) { // Noise in x + msg.x += dist(gen) * sigma_noise[noise_idx] + mu_noise[noise_idx]; + noise_idx++; + } + if (noise_set & 0x02) { // Noise in y + msg.y += dist(gen) * sigma_noise[noise_idx] + mu_noise[noise_idx]; + noise_idx++; + } + if (noise_set & 0x04) { // Noise in z + msg.z += dist(gen) * sigma_noise[noise_idx] + mu_noise[noise_idx]; + } +} + +void addNoise(geometry_msgs::Vector3Stamped &msg, const mjtNum *mu_noise, const mjtNum *sigma_noise, + const uint8_t &noise_set, std::normal_distribution &dist, std::mt19937 &gen) +{ + if (noise_set == 0) { // no noise, leave unchanged + return; + } + addNoise(msg.vector, mu_noise, sigma_noise, noise_set, dist, gen); +} + +////// Point ////// + +void addNoise(geometry_msgs::Point &msg, const mjtNum *mu_noise, const mjtNum *sigma_noise, const uint8_t &noise_set, + std::normal_distribution &dist, std::mt19937 &gen) +{ + if (noise_set == 0) { // no noise, leave unchanged + return; + } + + int noise_idx = 0; + if (noise_set & 0x01) { // Noise in x + msg.x += dist(gen) * sigma_noise[noise_idx] + mu_noise[noise_idx]; + noise_idx++; + } + if (noise_set & 0x02) { // Noise in y + msg.y += dist(gen) * sigma_noise[noise_idx] + mu_noise[noise_idx]; + noise_idx++; + } + if (noise_set & 0x04) { // Noise in z + msg.z += dist(gen) * sigma_noise[noise_idx] + mu_noise[noise_idx]; + } +} + +void addNoise(geometry_msgs::PointStamped &msg, const mjtNum *mu_noise, const mjtNum *sigma_noise, + const uint8_t &noise_set, std::normal_distribution &dist, std::mt19937 &gen) +{ + if (noise_set == 0) { // no noise, leave unchanged + return; + } + addNoise(msg.point, mu_noise, sigma_noise, noise_set, dist, gen); +} + +////// Scalar ////// + +void addNoise(double &value, const mjtNum *mu_noise, const mjtNum *sigma_noise, const uint8_t &noise_set, + std::normal_distribution &dist, std::mt19937 &gen) +{ + if (noise_set == 0) { // no noise, leave unchanged + return; + } + + if (noise_set & 0x01) { // Noise in x + // shift and scale standard normal to desired distribution + value += dist(gen) * sigma_noise[0] + mu_noise[0]; + } +} + +void addNoise(mujoco_ros_msgs::ScalarStamped &msg, const mjtNum *mu_noise, const mjtNum *sigma_noise, + const uint8_t &noise_set, std::normal_distribution &dist, std::mt19937 &gen) +{ + if (noise_set == 0) { // no noise, leave unchanged + return; + } + addNoise(msg.value, mu_noise, sigma_noise, noise_set, dist, gen); +} + +} // namespace mujoco_ros::sensors diff --git a/mujoco_ros_sensors/test/mujoco_sensors_test.cpp b/mujoco_ros_sensors/test/mujoco_sensors_test.cpp index 3d265079..a31565d2 100644 --- a/mujoco_ros_sensors/test/mujoco_sensors_test.cpp +++ b/mujoco_ros_sensors/test/mujoco_sensors_test.cpp @@ -54,7 +54,8 @@ #include #include #include -#include + +#include using namespace mujoco_ros; namespace mju = ::mujoco::sample_util; @@ -278,7 +279,7 @@ void compare_vectors(std::vector a, std::vector b, double tol, b } } -TEST_F(TrainEnvFixture, Sensor3DOF) +TEST_F(TrainEnvFixture, Vector3Sensor) { int n_sensor; getSensorByName("vel_EE", m, n_sensor); @@ -316,39 +317,55 @@ TEST_F(TrainEnvFixture, Sensor3DOF) nh->serviceClient("/sensors/register_noise_models"); EXPECT_TRUE(client.call(srv)) << "Service call failed!"; - // Pause sim for synchronous message + // Pause sim for synchronous messages env_ptr->settings_.run.store(0); - msgPtr = ros::topic::waitForMessage("/vel_EE"); - msgPtr_GT = ros::topic::waitForMessage("/vel_EE_GT"); - // GT == Sensor reading compare_vectors({ msgPtr_GT->vector.x, msgPtr_GT->vector.y, msgPtr_GT->vector.z }, { d->sensordata[adr] / cutoff, d->sensordata[adr + 1] / cutoff, d->sensordata[adr + 2] / cutoff }, 0.0001, true); + // Create message buffers + std::deque value_buf; + std::deque gt_buf; + + // Create callbacks to fill message buffers + auto value_cb = [&value_buf](auto msg) { value_buf.push_back(msg); }; + + auto gt_cb = [>_buf](auto msg) { gt_buf.push_back(msg); }; + + auto value_sub = nh->subscribe("/vel_EE", 1, value_cb); + auto gt_sub = nh->subscribe("/vel_EE_GT", 1, gt_cb); + + env_ptr->settings_.env_steps_request.store(1000); + int n = 0; double means[6] = { 0.0 }; double deltas[6] = { 0.0 }; double variances[6] = { 0.0 }; - for (int i = 0; i <= 1000; i++) { - env_ptr->settings_.env_steps_request.store(1); - - while (env_ptr->settings_.env_steps_request.load() != 0) { + while (n < 1000) { + if (value_buf.empty() || gt_buf.empty()) { + if (env_ptr->settings_.env_steps_request.load() == 0) { + env_ptr->settings_.env_steps_request.store(1000 - n); // Request missing steps + } std::this_thread::sleep_for(std::chrono::milliseconds(1)); + continue; } - msgPtr = ros::topic::waitForMessage("/vel_EE"); - msgPtr_GT = ros::topic::waitForMessage("/vel_EE_GT"); + + auto msg = value_buf.front(); + auto msg_GT = gt_buf.front(); + value_buf.pop_front(); + gt_buf.pop_front(); n += 1; - deltas[0] = (msgPtr->vector.x - msgPtr_GT->vector.x) - means[0]; - deltas[1] = (msgPtr->vector.y - msgPtr_GT->vector.y) - means[1]; - deltas[2] = (msgPtr->vector.z - msgPtr_GT->vector.z) - means[2]; + deltas[0] = (msg->vector.x - msg_GT->vector.x) - means[0]; + deltas[1] = (msg->vector.y - msg_GT->vector.y) - means[1]; + deltas[2] = (msg->vector.z - msg_GT->vector.z) - means[2]; - deltas[3] = msgPtr_GT->vector.x - means[3]; - deltas[4] = msgPtr_GT->vector.y - means[4]; - deltas[5] = msgPtr_GT->vector.z - means[5]; + deltas[3] = msg_GT->vector.x - means[3]; + deltas[4] = msg_GT->vector.y - means[4]; + deltas[5] = msg_GT->vector.z - means[5]; means[0] += deltas[0] / n; means[1] += deltas[1] / n; @@ -358,13 +375,13 @@ TEST_F(TrainEnvFixture, Sensor3DOF) means[4] += deltas[4] / n; means[5] += deltas[5] / n; - variances[0] += deltas[0] * ((msgPtr->vector.x - msgPtr_GT->vector.x) - means[0]); - variances[1] += deltas[1] * ((msgPtr->vector.y - msgPtr_GT->vector.y) - means[1]); - variances[2] += deltas[2] * ((msgPtr->vector.z - msgPtr_GT->vector.z) - means[2]); + variances[0] += deltas[0] * ((msg->vector.x - msg_GT->vector.x) - means[0]); + variances[1] += deltas[1] * ((msg->vector.y - msg_GT->vector.y) - means[1]); + variances[2] += deltas[2] * ((msg->vector.z - msg_GT->vector.z) - means[2]); - variances[3] += deltas[3] * (msgPtr_GT->vector.x - means[3]); - variances[4] += deltas[4] * (msgPtr_GT->vector.y - means[4]); - variances[5] += deltas[5] * (msgPtr_GT->vector.z - means[5]); + variances[3] += deltas[3] * (msg_GT->vector.x - means[3]); + variances[4] += deltas[4] * (msg_GT->vector.y - means[4]); + variances[5] += deltas[5] * (msg_GT->vector.z - means[5]); } variances[0] /= n - 1; @@ -391,7 +408,7 @@ TEST_F(TrainEnvFixture, Sensor3DOF) EXPECT_EQ(variances[5], 0); } -TEST_F(TrainEnvFixture, Framepos) +TEST_F(TrainEnvFixture, PointSensor) { int n_sensor; getSensorByName("immovable_pos", m, n_sensor); @@ -428,41 +445,55 @@ TEST_F(TrainEnvFixture, Framepos) ros::ServiceClient client = nh->serviceClient("/sensors/register_noise_models"); EXPECT_TRUE(client.call(srv)) << "Service call failed!"; - // sensor_plugin->registerNoiseModelsCB(srv.request, srv.response); // Pause sim for synchronous message env_ptr->settings_.run.store(0); - msgPtr = ros::topic::waitForMessage("/immovable_pos"); - msgPtr_GT = ros::topic::waitForMessage("/immovable_pos_GT"); - // GT == Sensor reading compare_vectors({ msgPtr_GT->point.x, msgPtr_GT->point.y, msgPtr_GT->point.z }, { d->sensordata[adr] / cutoff, d->sensordata[adr + 1] / cutoff, d->sensordata[adr + 2] / cutoff }, 0.0001, true); + // Create message buffers + std::deque value_buf; + std::deque gt_buf; + + // Create callbacks to fill message buffers + auto value_cb = [&value_buf](auto msg) { value_buf.push_back(msg); }; + auto gt_cb = [>_buf](auto msg) { gt_buf.push_back(msg); }; + + auto value_sub = nh->subscribe("/immovable_pos", 1, value_cb); + auto gt_sub = nh->subscribe("/immovable_pos_GT", 1, gt_cb); + + env_ptr->settings_.env_steps_request.store(1000); + int n = 0; double means[6] = { 0.0 }; double deltas[6] = { 0.0 }; double variances[6] = { 0.0 }; - for (int i = 0; i <= 1000; i++) { - env_ptr->settings_.env_steps_request.store(1); - - while (env_ptr->settings_.env_steps_request.load() != 0) { + while (n < 1000) { + if (value_buf.empty() || gt_buf.empty()) { + if (env_ptr->settings_.env_steps_request.load() == 0) { + env_ptr->settings_.env_steps_request.store(1000 - n); // Request missing steps + } std::this_thread::sleep_for(std::chrono::milliseconds(1)); + continue; } - msgPtr = ros::topic::waitForMessage("/immovable_pos"); - msgPtr_GT = ros::topic::waitForMessage("/immovable_pos_GT"); + + auto msg = value_buf.front(); + auto msg_GT = gt_buf.front(); + value_buf.pop_front(); + gt_buf.pop_front(); n += 1; - deltas[0] = (msgPtr->point.x - msgPtr_GT->point.x) - means[0]; - deltas[1] = (msgPtr->point.y - msgPtr_GT->point.y) - means[1]; - deltas[2] = (msgPtr->point.z - msgPtr_GT->point.z) - means[2]; + deltas[0] = (msg->point.x - msg_GT->point.x) - means[0]; + deltas[1] = (msg->point.y - msg_GT->point.y) - means[1]; + deltas[2] = (msg->point.z - msg_GT->point.z) - means[2]; - deltas[3] = msgPtr_GT->point.x - means[3]; - deltas[4] = msgPtr_GT->point.y - means[4]; - deltas[5] = msgPtr_GT->point.z - means[5]; + deltas[3] = msg_GT->point.x - means[3]; + deltas[4] = msg_GT->point.y - means[4]; + deltas[5] = msg_GT->point.z - means[5]; means[0] += deltas[0] / n; means[1] += deltas[1] / n; @@ -472,13 +503,13 @@ TEST_F(TrainEnvFixture, Framepos) means[4] += deltas[4] / n; means[5] += deltas[5] / n; - variances[0] += deltas[0] * ((msgPtr->point.x - msgPtr_GT->point.x) - means[0]); - variances[1] += deltas[1] * ((msgPtr->point.y - msgPtr_GT->point.y) - means[1]); - variances[2] += deltas[2] * ((msgPtr->point.z - msgPtr_GT->point.z) - means[2]); + variances[0] += deltas[0] * ((msg->point.x - msg_GT->point.x) - means[0]); + variances[1] += deltas[1] * ((msg->point.y - msg_GT->point.y) - means[1]); + variances[2] += deltas[2] * ((msg->point.z - msg_GT->point.z) - means[2]); - variances[3] += deltas[3] * (msgPtr_GT->point.x - means[3]); - variances[4] += deltas[4] * (msgPtr_GT->point.y - means[4]); - variances[5] += deltas[5] * (msgPtr_GT->point.z - means[5]); + variances[3] += deltas[3] * (msg_GT->point.x - means[3]); + variances[4] += deltas[4] * (msg_GT->point.y - means[4]); + variances[5] += deltas[5] * (msg_GT->point.z - means[5]); } variances[0] /= n - 1; @@ -505,7 +536,7 @@ TEST_F(TrainEnvFixture, Framepos) EXPECT_EQ(variances[5], 0); } -TEST_F(TrainEnvFixture, scalar_stamped) +TEST_F(TrainEnvFixture, ScalarSensor) { int n_sensor; getSensorByName("vel_joint2", m, n_sensor); @@ -540,38 +571,53 @@ TEST_F(TrainEnvFixture, scalar_stamped) nh->serviceClient("/sensors/register_noise_models"); EXPECT_TRUE(client.call(srv)) << "Service call failed!"; + // GT == Sensor reading + EXPECT_NEAR(msgPtr_GT->value, d->sensordata[adr] / cutoff, 0.0001) << "GT differs from actual sensor value"; + // Pause sim for synchronous message env_ptr->settings_.run.store(0); - msgPtr_GT = ros::topic::waitForMessage("/vel_joint2_GT"); - msgPtr = ros::topic::waitForMessage("/vel_joint2"); + // Create message buffers + std::deque value_buf; + std::deque gt_buf; - // GT == Sensor reading - EXPECT_NEAR(msgPtr_GT->value, d->sensordata[adr] / cutoff, 0.0001) << "GT differs from actual sensor value"; + // Create callbacks to fill message buffers + auto value_cb = [&value_buf](auto msg) { value_buf.push_back(msg); }; + auto gt_cb = [>_buf](auto msg) { gt_buf.push_back(msg); }; + + auto value_sub = nh->subscribe("/vel_joint2", 1, value_cb); + auto gt_sub = nh->subscribe("/vel_joint2_GT", 1, gt_cb); + + env_ptr->settings_.env_steps_request.store(1000); int n = 0; double means[2] = { 0.0 }; double deltas[2] = { 0.0 }; double variances[2] = { 0.0 }; - for (int i = 0; i <= 1000; i++) { - env_ptr->settings_.env_steps_request.store(1); - - while (env_ptr->settings_.env_steps_request.load() != 0) { + while (n < 1000) { + if (value_buf.empty() || gt_buf.empty()) { + if (env_ptr->settings_.env_steps_request.load() == 0) { + env_ptr->settings_.env_steps_request.store(1000 - n); // Request missing steps + } std::this_thread::sleep_for(std::chrono::milliseconds(1)); + continue; } - msgPtr_GT = ros::topic::waitForMessage("/vel_joint2_GT"); - msgPtr = ros::topic::waitForMessage("/vel_joint2"); + + auto msg = value_buf.front(); + auto msg_GT = gt_buf.front(); + value_buf.pop_front(); + gt_buf.pop_front(); n += 1; - deltas[0] = (msgPtr->value - msgPtr_GT->value) - means[0]; - deltas[1] = msgPtr_GT->value - means[1]; + deltas[0] = (msg->value - msg_GT->value) - means[0]; + deltas[1] = msg_GT->value - means[1]; means[0] += deltas[0] / n; means[1] += deltas[1] / n; - variances[0] += deltas[0] * ((msgPtr->value - msgPtr_GT->value) - means[0]); - variances[1] += deltas[1] * (msgPtr_GT->value - means[1]); + variances[0] += deltas[0] * ((msg->value - msg_GT->value) - means[0]); + variances[1] += deltas[1] * (msg_GT->value - means[1]); } variances[0] /= n - 1; @@ -584,7 +630,7 @@ TEST_F(TrainEnvFixture, scalar_stamped) EXPECT_EQ(variances[1], 0); } -TEST_F(TrainEnvFixture, quaternion) +TEST_F(TrainEnvFixture, QuaternionSensor) { int n_sensor; getSensorByName("immovable_quat", m, n_sensor); @@ -624,9 +670,6 @@ TEST_F(TrainEnvFixture, quaternion) // Pause sim for synchronous message env_ptr->settings_.run.store(0); - msgPtr = ros::topic::waitForMessage("/immovable_quat"); - msgPtr_GT = ros::topic::waitForMessage("/immovable_quat_GT"); - // GT == Sensor reading compare_vectors( { msgPtr_GT->quaternion.w, msgPtr_GT->quaternion.x, msgPtr_GT->quaternion.y, msgPtr_GT->quaternion.z }, @@ -634,6 +677,17 @@ TEST_F(TrainEnvFixture, quaternion) d->sensordata[adr + 3] / cutoff }, 0.0001, true); + // Create message buffers + std::deque value_buf; + std::deque gt_buf; + + // Create callbacks to fill message buffers + auto value_cb = [&value_buf](auto msg) { value_buf.push_back(msg); }; + auto gt_cb = [>_buf](auto msg) { gt_buf.push_back(msg); }; + + auto value_sub = nh->subscribe("/immovable_quat", 1, value_cb); + auto gt_sub = nh->subscribe("/immovable_quat_GT", 1, gt_cb); + int n = 0; double means[6] = { 0.0 }; double deltas[6] = { 0.0 }; @@ -643,19 +697,24 @@ TEST_F(TrainEnvFixture, quaternion) tf2::Matrix3x3 m; double r, p, y, R, P, Y; - for (int i = 0; i <= 1000; i++) { - env_ptr->settings_.env_steps_request.store(1); - - while (env_ptr->settings_.env_steps_request.load() != 0) { + while (n < 1000) { + if (value_buf.empty() || gt_buf.empty()) { + if (env_ptr->settings_.env_steps_request.load() == 0) { + env_ptr->settings_.env_steps_request.store(1000 - n); // Request missing steps + } std::this_thread::sleep_for(std::chrono::milliseconds(1)); + continue; } - msgPtr = ros::topic::waitForMessage("/immovable_quat"); - msgPtr_GT = ros::topic::waitForMessage("/immovable_quat_GT"); + + auto msg = value_buf.front(); + auto msg_GT = gt_buf.front(); + value_buf.pop_front(); + gt_buf.pop_front(); n += 1; - tf2::fromMsg(msgPtr->quaternion, q); - tf2::fromMsg(msgPtr_GT->quaternion, q_GT); + tf2::fromMsg(msg->quaternion, q); + tf2::fromMsg(msg_GT->quaternion, q_GT); m = tf2::Matrix3x3(q); m.getRPY(r, p, y); diff --git a/mujoco_ros_sensors/test/sensors_world.xml b/mujoco_ros_sensors/test/sensors_world.xml index bbc674e5..c421eb0e 100644 --- a/mujoco_ros_sensors/test/sensors_world.xml +++ b/mujoco_ros_sensors/test/sensors_world.xml @@ -21,11 +21,12 @@ - + + + - @@ -43,7 +44,23 @@ - + + + + + + + + + + + + + + + + +