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?
Locked
