Sending goals programmatically: frame transform issue

[felipeduque]

Hi. I've created a service to RTAB-Map that's supposed to make the robot walk towards the XYZ of a given point in the Kinetic point cloud. Here's the code:

bool CoreWrapper::myFindObjectCallback(rtabmap_ros::MyFindObject::Request& req, rtabmap_ros::MyFindObject::Response& res)
{
	int arrayPosition = 240*pclMsg_.row_step + 320*pclMsg_.point_step;
	// compute position in array where x,y,z data start
	int arrayPosX = arrayPosition + pclMsg_.fields[0].offset; // X has an offset of 0
	int arrayPosY = arrayPosition + pclMsg_.fields[1].offset; // Y has an offset of 4
	int arrayPosZ = arrayPosition + pclMsg_.fields[2].offset; // Z has an offset of 8
	float x;
	float y;
	float z;
	memcpy(&x, &pclMsg_.data[arrayPosX], sizeof(float));
	memcpy(&y, &pclMsg_.data[arrayPosY], sizeof(float));
	memcpy(&z, &pclMsg_.data[arrayPosZ], sizeof(float));

	std::cout << x << " " << y << " " << z << std::endl;

	rtabmap::Transform cameraToMapTransform = rtabmap_ros::getTransform("/map", "/xtion_rgb_optical_frame", pclMsg_.header.stamp, tfListener_, waitForTransform_?waitForTransformDuration_:0.0);

	cv::Mat m = cv::Mat::eye(3, 4, CV_32FC1);
	m.at<float>(3) = x;
	m.at<float>(7) = y;
	m.at<float>(11) = z;

	std::cout << m << std::endl;

	currentMetricGoal_ = cameraToMapTransform * rtabmap::Transform(m);

	std::cout << currentMetricGoal_.prettyPrint() << std::endl;
	std::cout << currentMetricGoal_.dataMatrix() << std::endl;

	publishCurrentGoal(pclMsg_.header.stamp);
	publishLocalPath(pclMsg_.header.stamp);
	publishGlobalPath(pclMsg_.header.stamp);
        return true;
}

Right now, it should just move towards the center point of the pointcloud (point with coordinates (240, 320) in the pointcloud), whose associated XYZ coordinates are in the "/xtion_rgb_optical_frame" frame. However, when the service is called, "move_base" complains about the quaternion not being close enough to vertical. This is a sample of the goal that's sent to RTAB-Map:

xyz=-2.179723,-4.765885,1.189696 rpy=-1.577074,-0.000000,2.661634

In fact, the "RPY" part implies that the robot should roll along X-axis (roughly pi/2 radians), which doesn't seem correct to me. I've tried several different combinations of the transform (with inverse, changing the orders...). Is there anything wrong with my transforms? Am I missing anything?

[felipeduque]

I found a workaround to make things work. I forced "currentMetricGoal_" to become a 3 DoF transform (XY location, and rotation around Z axis). Working perfectly now. However, the real issue is: why has the node been created with such an impossible pose (involving rotation along X and/or Y axes)?

[matlabbe]

Difficult to know, but if you set Reg/Force3DoF parameter to true, all poses should be x y theta-only. If not, what is the input odometry, is it 3DoF?

EDIT: Note also that "/map" -> "/xtion_rgb_optical_frame" is not forced to be 3DoF, in particular if the camera is upside down.