test: improve mujoco_ros_sensors coverage

ubi-agni · Aug 5, 2024 · 2cecaf8 · 2cecaf8
1 parent 6529e8c
commit 2cecaf8
Show file tree

Hide file tree

Showing 3 changed files with 80 additions and 5 deletions.
diff --git a/CHANGELOG.md b/CHANGELOG.md
@@ -16,6 +16,7 @@ Loading and reset times are reported in the server debug log. All plugin stats c
 
 ### Changed
 * Moved `mujoco_ros::Viewer::Clock` definition to `mujoco_ros::Clock` (into common_types.h).
+* Increased test coverage of `mujoco_ros_sensors` plugin.
 
 Contributors: @DavidPL1
 

diff --git a/mujoco_ros_sensors/test/mujoco_sensors_test.cpp b/mujoco_ros_sensors/test/mujoco_sensors_test.cpp
@@ -303,9 +303,11 @@ TEST_F(TrainEnvFixture, Sensor3DOF)
 	mujoco_ros_msgs::SensorNoiseModel noise_model;
 	noise_model.mean.emplace_back(0.0);
 	noise_model.mean.emplace_back(1.0);
+	noise_model.mean.emplace_back(0.0);
 	noise_model.std.emplace_back(0.025);
 	noise_model.std.emplace_back(0.0);
-	noise_model.set_flag    = 3;
+	noise_model.std.emplace_back(0.0);
+	noise_model.set_flag    = 7;
 	noise_model.sensor_name = "vel_EE";
 
 	mujoco_ros_msgs::RegisterSensorNoiseModels srv;
@@ -416,9 +418,11 @@ TEST_F(TrainEnvFixture, Framepos)
 	mujoco_ros_msgs::SensorNoiseModel noise_model;
 	noise_model.mean.emplace_back(0.0);
 	noise_model.mean.emplace_back(1.0);
+	noise_model.mean.emplace_back(0.0);
 	noise_model.std.emplace_back(0.025);
 	noise_model.std.emplace_back(0.0);
-	noise_model.set_flag    = 3;
+	noise_model.std.emplace_back(0.0);
+	noise_model.set_flag    = 7;
 	noise_model.sensor_name = "immovable_pos";
 
 	mujoco_ros_msgs::RegisterSensorNoiseModels srv;
@@ -608,9 +612,13 @@ TEST_F(TrainEnvFixture, quaternion)
 	    { msgPtr->quaternion.w, msgPtr->quaternion.x, msgPtr->quaternion.y, msgPtr->quaternion.z }, 0.0001, true);
 
 	mujoco_ros_msgs::SensorNoiseModel noise_model;
+	noise_model.mean.emplace_back(0.0);
+	noise_model.mean.emplace_back(0.0);
 	noise_model.mean.emplace_back(1.0);
+	noise_model.std.emplace_back(0.0);
+	noise_model.std.emplace_back(0.0);
 	noise_model.std.emplace_back(0.025);
-	noise_model.set_flag    = 4;
+	noise_model.set_flag    = 7;
 	noise_model.sensor_name = "immovable_quat";
 
 	mujoco_ros_msgs::RegisterSensorNoiseModels srv;
@@ -710,3 +718,66 @@ TEST_F(TrainEnvFixture, quaternion)
 	EXPECT_EQ(variances[4], 0);
 	EXPECT_EQ(variances[5], 0);
 }
+
+TEST_F(EvalEnvFixture, NoAdmEvalNoiseModelChange)
+{
+	mujoco_ros_msgs::SensorNoiseModel noise_model;
+	noise_model.mean.emplace_back(0.0);
+	noise_model.mean.emplace_back(1.0);
+	noise_model.std.emplace_back(0.025);
+	noise_model.std.emplace_back(0.0);
+	noise_model.set_flag    = 3;
+	noise_model.sensor_name = "vel_EE";
+
+	mujoco_ros_msgs::RegisterSensorNoiseModels srv;
+	srv.request.noise_models.emplace_back(noise_model);
+	srv.request.admin_hash = "some_wrong_hash";
+
+	ros::ServiceClient client =
+	    nh->serviceClient<mujoco_ros_msgs::RegisterSensorNoiseModels>("/sensors/register_noise_models");
+	EXPECT_TRUE(client.call(srv)) << "Service call failed!";
+
+	EXPECT_EQ(srv.response.success, false) << "Service call should have failed!";
+}
+
+TEST_F(EvalEnvFixture, AllowedEvalNoiseModelChange)
+{
+	mujoco_ros_msgs::SensorNoiseModel noise_model;
+	noise_model.mean.emplace_back(0.0);
+	noise_model.mean.emplace_back(1.0);
+	noise_model.std.emplace_back(0.025);
+	noise_model.std.emplace_back(0.0);
+	noise_model.set_flag    = 3;
+	noise_model.sensor_name = "vel_EE";
+
+	mujoco_ros_msgs::RegisterSensorNoiseModels srv;
+	srv.request.noise_models.emplace_back(noise_model);
+	srv.request.admin_hash = "some_hash";
+
+	ros::ServiceClient client =
+	    nh->serviceClient<mujoco_ros_msgs::RegisterSensorNoiseModels>("/sensors/register_noise_models");
+	EXPECT_TRUE(client.call(srv)) << "Service call failed!";
+
+	EXPECT_EQ(srv.response.success, true) << "Service call should have succeeded!";
+}
+
+TEST_F(TrainEnvFixture, UnknownSensorAddNoise)
+{
+	mujoco_ros_msgs::SensorNoiseModel noise_model;
+	noise_model.mean.emplace_back(0.0);
+	noise_model.mean.emplace_back(1.0);
+	noise_model.std.emplace_back(0.025);
+	noise_model.std.emplace_back(0.0);
+	noise_model.set_flag    = 3;
+	noise_model.sensor_name = "unknown_sensor";
+
+	mujoco_ros_msgs::RegisterSensorNoiseModels srv;
+	srv.request.noise_models.emplace_back(noise_model);
+	srv.request.admin_hash = "example_hash";
+
+	ros::ServiceClient client =
+	    nh->serviceClient<mujoco_ros_msgs::RegisterSensorNoiseModels>("/sensors/register_noise_models");
+	EXPECT_TRUE(client.call(srv)) << "Service call failed!";
+
+	EXPECT_EQ(srv.response.success, true) << "Service call should have succeeded!";
+}
diff --git a/mujoco_ros_sensors/test/sensors_world.xml b/mujoco_ros_sensors/test/sensors_world.xml
@@ -40,10 +40,13 @@
     </worldbody>
 
     <sensor>
-        <framepos  name="immovable_pos"  objtype="xbody" objname="immovable" />
-        <framequat name="immovable_quat" objtype="xbody" objname="immovable" />
+        <framepos   name="immovable_pos"  objtype="xbody" objname="immovable" />
+        <framequat  name="immovable_quat" objtype="xbody" objname="immovable" />
+        <framexaxis name="framex_EE"      objtype="site"  objname="joint2_site" />
 
         <velocimeter name="vel_EE" site="joint2_site" />
         <jointvel    name="vel_joint2"  joint="joint2" />
+
+        <subtreecom  name="subtree_end_link" body="end_link" />
     </sensor>
 </mujoco>