diff --git a/CHANGELOG.md b/CHANGELOG.md index 6b8b4f4..0cfb845 100644 --- 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 index 3d26507..111c0d6 100644 --- 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("/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("/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("/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 index cc7099c..2e2dac9 100644 --- a/mujoco_ros_sensors/test/sensors_world.xml +++ b/mujoco_ros_sensors/test/sensors_world.xml @@ -40,10 +40,13 @@ - - + + + + +