feat: compound sensors (Twist, IMU, Wrench, Pose)

mujoco_ros/CHANGELOG.md

@@ -1,10 +1,14 @@
 <a name="unreleased"></a>
 ## 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).

mujoco_ros_sensors/CMakeLists.txt

@@ -47,6 +47,7 @@ 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}

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

mujoco_ros_sensors/include/mujoco_ros_sensors/mujoco_sensor_handler_plugin.h

@@ -71,7 +71,7 @@ class MujocoRosSensorsPlugin : public mujoco_ros::MujocoPlugin
 	std::normal_distribution<double> noise_dist_;
 
 	std::map<std::string, RosSensorInterfaceBase *> sensor_map_;
-	// std::vector<RosSensorInterfaceBase *> enabled_sensors_;
+	std::map<std::string, RosSensorInterfaceBase *> enabled_sensors_;
 
 	ros::ServiceServer register_noise_model_server_;
 

mujoco_ros_sensors/include/mujoco_ros_sensors/sensors.h

@@ -41,6 +41,9 @@
 #include <geometry_msgs/Vector3Stamped.h>
 #include <geometry_msgs/PointStamped.h>
 #include <geometry_msgs/QuaternionStamped.h>
+#include <geometry_msgs/WrenchStamped.h>
+#include <geometry_msgs/TwistStamped.h>
+#include <geometry_msgs/PoseStamped.h>
 #include <sensor_msgs/Imu.h>
 
 #include <mujoco_ros_sensors/serialization.h>
@@ -80,6 +83,12 @@ class RosSensorInterfaceBase
 	virtual void publish(const bool publish_gt, const mjData *data, std::normal_distribution<double> &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_;
@@ -92,6 +101,7 @@ class RosSensorInterfaceBase
 	mjtNum *sigma_noise_;
 
 	uint dofs_;
+	int type_ = -1;
 
 	uint8_t noise_set_ = 0; // 0 for unset, otherwise binary code for set dimensions
 };
@@ -100,8 +110,8 @@ template <typename T>
 class RosSensorInterface : public RosSensorInterfaceBase
 {
 public:
-	RosSensorInterface(const std::string &frame_id, const std::string &sensor_name, const int dofs, const int adr,
-	                   const mjtNum cutoff, bool eval_mode, ros::NodeHandle *sensors_nh);
+	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<double> &dist,
 	             std::mt19937 &gen) override;
@@ -113,6 +123,67 @@ class RosSensorInterface : public RosSensorInterfaceBase
 	mjtNum cutoff_;
 };
 
+class WrenchSensor : public RosSensorInterfaceBase
+{
+public:
+	WrenchSensor(const std::string &sensor_name, std::vector<RosSensorInterfaceBase *> &wrench_sensors, bool eval_mode,
+	             ros::NodeHandle *sensors_nh);
+
+	void publish(const bool publish_gt, const mjData *data, std::normal_distribution<double> &dist,
+	             std::mt19937 &gen) override;
+
+private:
+	RosSensorInterface<geometry_msgs::Vector3Stamped> *force_  = nullptr;
+	RosSensorInterface<geometry_msgs::Vector3Stamped> *torque_ = nullptr;
+};
+
+class TwistSensor : public RosSensorInterfaceBase
+{
+public:
+	TwistSensor(const std::string &sensor_name, std::vector<RosSensorInterfaceBase *> &twist_sensors, bool eval_mode,
+	            ros::NodeHandle *sensors_nh);
+
+	void publish(const bool publish_gt, const mjData *data, std::normal_distribution<double> &dist,
+	             std::mt19937 &gen) override;
+
+private:
+	RosSensorInterface<geometry_msgs::Vector3Stamped> *linear_velocity_  = nullptr;
+	RosSensorInterface<geometry_msgs::Vector3Stamped> *angular_velocity_ = nullptr;
+};
+
+class PoseSensor : public RosSensorInterfaceBase
+{
+public:
+	PoseSensor(const std::string &sensor_name, std::vector<RosSensorInterfaceBase *> &pose_sensors, bool eval_mode,
+	           ros::NodeHandle *sensors_nh);
+
+	void publish(const bool publish_gt, const mjData *data, std::normal_distribution<double> &dist,
+	             std::mt19937 &gen) override;
+
+private:
+	RosSensorInterface<geometry_msgs::PointStamped> *position_         = nullptr;
+	RosSensorInterface<geometry_msgs::QuaternionStamped> *orientation_ = nullptr;
+};
+
+class IMUSensor : public RosSensorInterfaceBase
+{
+public:
+	IMUSensor(const std::string &sensor_name, std::vector<RosSensorInterfaceBase *> &imu_sensors, bool eval_mode,
+	          ros::NodeHandle *sensors_nh);
+
+	void publish(const bool publish_gt, const mjData *data, std::normal_distribution<double> &dist,
+	             std::mt19937 &gen) override;
+
+private:
+	RosSensorInterface<geometry_msgs::Vector3Stamped> *linear_acceleration_  = nullptr;
+	RosSensorInterface<geometry_msgs::Vector3Stamped> *angular_acceleration_ = nullptr;
+	RosSensorInterface<geometry_msgs::QuaternionStamped> *orientation_       = nullptr;
+
+	mjtNum lin_cov_[9];
+	mjtNum ang_cov_[9];
+	mjtNum ori_cov_[9];
+};
+
 // template impl needs to be in header
 template <typename T>
 void RosSensorInterface<T>::publish(const bool publish_gt, const mjData *data, std::normal_distribution<double> &dist,
@@ -128,14 +199,15 @@ void RosSensorInterface<T>::publish(const bool publish_gt, const mjData *data, s
 
 template <typename T>
 RosSensorInterface<T>::RosSensorInterface(const std::string &frame_id, const std::string &sensor_name, const int dofs,
-                                          const int adr, const mjtNum cutoff, bool eval_mode,
+                                          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<T>(sensor_name, 1);
 	if (!eval_mode) {
 		gt_pub_ = sensors_nh->advertise<T>(sensor_name + "_GT", 1);
 	}
+	type_ = type;
 };
 
 template <typename T>
@@ -152,4 +224,6 @@ void RosSensorInterface<T>::addNoise(T &msg, std::normal_distribution<double> &d
 	mujoco_ros::sensors::addNoise(msg, mu_noise_, sigma_noise_, noise_set_, dist, gen);
 }
 
+extern const char *SENSOR_STRING[35];
+
 } // namespace mujoco_ros::sensors