fix: body quaternion fetching in GetBodyStateCB

fixes #36

Author: DavidPL1

CHANGELOG.md

@@ -14,6 +14,7 @@ Loading and reset times are reported in the server debug log. All plugin stats c
 * *mujoco_ros_control*: fixed sometimes using wrong joint id in default hardware interface (would only be correct, if the joints appear first and in the same order in the compiled MuJoCo model).
 * *mujoco_ros_sensors*: now skipping user sensors, as they should be handled in separate, dedicated plugins.
 * When loading takes more than 0.25 seconds the simulation is no longer automatically paused.
+* Fixed fetching of body quaternion in `get_body_state` service.
 
 ### Changed
 * Moved `mujoco_ros::Viewer::Clock` definition to `mujoco_ros::Clock` (into common_types.h).

mujoco_ros/src/callbacks.cpp

@@ -413,10 +413,10 @@ bool MujocoEnv::getBodyStateCB(mujoco_ros_msgs::GetBodyState::Request &req,
 		resp.state.pose.pose.position.x    = data_->xpos[body_id * 3];
 		resp.state.pose.pose.position.y    = data_->xpos[body_id * 3 + 1];
 		resp.state.pose.pose.position.z    = data_->xpos[body_id * 3 + 2];
-		resp.state.pose.pose.orientation.w = data_->xquat[body_id * 3];
-		resp.state.pose.pose.orientation.x = data_->xquat[body_id * 3 + 1];
-		resp.state.pose.pose.orientation.y = data_->xquat[body_id * 3 + 2];
-		resp.state.pose.pose.orientation.z = data_->xquat[body_id * 3 + 3];
+		resp.state.pose.pose.orientation.w = data_->xquat[body_id * 4];
+		resp.state.pose.pose.orientation.x = data_->xquat[body_id * 4 + 1];
+		resp.state.pose.pose.orientation.y = data_->xquat[body_id * 4 + 2];
+		resp.state.pose.pose.orientation.z = data_->xquat[body_id * 4 + 3];
 
 		resp.state.twist.header          = std_msgs::Header();
 		resp.state.twist.header.frame_id = "world";

mujoco_ros/test/ros_interface_test.cpp

@@ -538,6 +538,54 @@ TEST_F(PendulumEnvFixture, SetBodyStateCallback)
 	compare_qvel(d, m->jnt_dofadr[id_free], "ball_freejoint", { 0.0, 0.0, 0.0, 0.0, 0.0, 0.0 });
 }
 
+TEST_F(PendulumEnvFixture, GetBodyStateInvalidName)
+{
+	EXPECT_FALSE(env_ptr->settings_.run) << "Simulation should be paused!";
+	EXPECT_NEAR(env_ptr->getDataPtr()->time, 0, 1e-6) << "Simulation time should be 0.0!";
+	EXPECT_TRUE(ros::service::exists(env_ptr->getHandleNamespace() + "/set_body_state", true))
+	    << "Set body state service should be available!";
+	EXPECT_TRUE(ros::service::exists(env_ptr->getHandleNamespace() + "/get_body_state", true))
+	    << "Get body state service should be available!";
+
+	mujoco_ros_msgs::GetBodyState g_srv;
+	// wrong body name
+	g_srv.request.name = "unknown";
+	EXPECT_TRUE(ros::service::call(env_ptr->getHandleNamespace() + "/get_body_state", g_srv))
+	    << "get body state service call failed!";
+	EXPECT_FALSE(g_srv.response.success);
+}
+
+TEST_F(PendulumEnvFixture, GetBodyStateStaticBody)
+{
+	EXPECT_FALSE(env_ptr->settings_.run) << "Simulation should be paused!";
+	EXPECT_NEAR(env_ptr->getDataPtr()->time, 0, 1e-6) << "Simulation time should be 0.0!";
+	EXPECT_TRUE(ros::service::exists(env_ptr->getHandleNamespace() + "/set_body_state", true))
+	    << "Set body state service should be available!";
+	EXPECT_TRUE(ros::service::exists(env_ptr->getHandleNamespace() + "/get_body_state", true))
+	    << "Get body state service should be available!";
+
+	mujoco_ros_msgs::GetBodyState g_srv;
+	g_srv.request.name = "immovable";
+	EXPECT_TRUE(ros::service::call(env_ptr->getHandleNamespace() + "/get_body_state", g_srv))
+	    << "get body state service call failed!";
+	EXPECT_TRUE(g_srv.response.success);
+	EXPECT_EQ(g_srv.response.state.name, "immovable");
+	EXPECT_DOUBLE_EQ(g_srv.response.state.mass, 0.10239999741315842);
+	EXPECT_DOUBLE_EQ(g_srv.response.state.pose.pose.position.x, 0.56428);
+	EXPECT_DOUBLE_EQ(g_srv.response.state.pose.pose.position.y, 0.221972);
+	EXPECT_DOUBLE_EQ(g_srv.response.state.pose.pose.position.z, 0.6);
+	EXPECT_DOUBLE_EQ(g_srv.response.state.pose.pose.orientation.w, 1.0);
+	EXPECT_DOUBLE_EQ(g_srv.response.state.pose.pose.orientation.x, 0.0);
+	EXPECT_DOUBLE_EQ(g_srv.response.state.pose.pose.orientation.y, 0.0);
+	EXPECT_DOUBLE_EQ(g_srv.response.state.pose.pose.orientation.z, 0.0);
+	EXPECT_DOUBLE_EQ(g_srv.response.state.twist.twist.linear.x, 0.0);
+	EXPECT_DOUBLE_EQ(g_srv.response.state.twist.twist.linear.y, 0.0);
+	EXPECT_DOUBLE_EQ(g_srv.response.state.twist.twist.linear.z, 0.0);
+	EXPECT_DOUBLE_EQ(g_srv.response.state.twist.twist.angular.x, 0.0);
+	EXPECT_DOUBLE_EQ(g_srv.response.state.twist.twist.angular.y, 0.0);
+	EXPECT_DOUBLE_EQ(g_srv.response.state.twist.twist.angular.z, 0.0);
+}
+
 TEST_F(PendulumEnvFixture, GetBodyStateCallback)
 {
 	EXPECT_FALSE(env_ptr->settings_.run) << "Simulation should be paused!";
@@ -572,8 +620,6 @@ TEST_F(PendulumEnvFixture, GetBodyStateCallback)
 	EXPECT_TRUE(srv.response.success);
 
 	mujoco_ros_msgs::GetBodyState g_srv;
-	// wrong body name
-	g_srv.request.name = "unknown";
 
 	// correct request
 	g_srv.request.name = "body_ball";
@@ -582,7 +628,8 @@ TEST_F(PendulumEnvFixture, GetBodyStateCallback)
 	EXPECT_TRUE(g_srv.response.success);
 	EXPECT_EQ(g_srv.response.state.mass, srv.request.state.mass);
 	EXPECT_EQ(g_srv.response.state.name, srv.request.state.name);
-	EXPECT_EQ(g_srv.response.state.pose.pose, srv.request.state.pose.pose);
+	EXPECT_EQ(g_srv.response.state.pose.pose.position, srv.request.state.pose.pose.position);
+	EXPECT_EQ(g_srv.response.state.pose.pose.orientation, srv.request.state.pose.pose.orientation);
 	EXPECT_EQ(g_srv.response.state.twist.twist, srv.request.state.twist.twist);
 
 	// TODO(dleins): tests for bodies with a non-freejoint, no joint, and multiple joints (cannot set position but read!)