Problem using TFs

edit

Hello,

I'm working on a robot based on a trutlebot2 and with some motors and a kinect as a head. I would like to use TFs to do some transforms but I have some problems using them.

In our project, we have nodes for the head based on the PR2 code and this is working pretty well.

On the other hand, we want to use the TFs in another node to transform a point from camera_link frame to base_link frame. Here is the code:

class LookAt
{
    private:
        ros::Publisher _handPositionPub;
        int _lastHandId;

        ros::Publisher _transformedHandPositionPub;
        ros::Publisher _initHandPositionPub;

        geometry_msgs::PointStamped _lastHandPosition;

    public:
        LookAt(ros::NodeHandle& nodeHandle)
        {
            _handPositionPub = nodeHandle.advertise<package_msgs::PointHead>(
                "/head_controller/point_head/goal", 1000);
            _initHandPositionPub =
                nodeHandle.advertise<geometry_msgs::PointStamped>(
                    "/ui/init_hand_position", 1000, true);
            _transformedHandPositionPub =
                nodeHandle.advertise<geometry_msgs::PointStamped>(
                    "/ui/hand_position", 1000, true);
            _lastHandId = -1;
        }

        void handDetectionCallback(const package_msgs::HandPosition::ConstPtr& msg)
        {
            int id = msg->nId;
            package_msgs::PointHead handPosition;
            handPosition.point = msg->point;
            bool isHandLost = msg->lostHand;
            bool isInit = false;

            if(_lastHandId != -1)
            {
                if(_lastHandId != id)
                {
                    return;
                }
                transformCamFrameToRobotFrame(msg);
            }
            else
            {
                transformCamFrameToRobotFrame(msg);
                _initHandPositionPub.publish(_lastHandPosition);
                _lastHandId = id;
                isInit = true;
            }

            if(!isHandLost)
            {
                if(!isInit)
                {
                    _transformedHandPositionPub.publish(_lastHandPosition);
                }
                _handPositionPub.publish(handPosition);
            }
        }

        void transformCamFrameToRobotFrame(const package_msgs::HandPositionConstPtr& msg)
        {
            package_msgs::PointHead handPosition;
            handPosition.point = msg->point;
            tf::TransformListener listener;
            std::string error_msg;
            std::string parent;

            bool ret1 = false;
            try
            {
                ret1 = listener.waitForTransform("base_footprint", "camera_link",
                                                 msg->header.stamp,
                                                 ros::Duration(5.0),
                                                 ros::Duration(0.01),
                                                 &error_msg);
                std::cout << "waitForTranform " << ret1 << std::endl;

                std::string targetFrame = "base_footprint";

                geometry_msgs::PointStamped targetPoint;
                targetPoint.header.frame_id = "camera_link";
                targetPoint.point.x = handPosition.point.x;
                targetPoint.point.y = handPosition.point.y;
                targetPoint.point.z = handPosition.point.z;

                ros::Time currentTransform = ros::Time::now();
                listener.getLatestCommonTime(targetPoint.header.frame_id,
                                             "base_footprint",
                                             currentTransform,
                                             NULL);
                targetPoint.header.stamp = currentTransform;

                listener.transformPoint(targetFrame, targetPoint,
                                        _lastHandPosition);
            }
            catch(const tf::TransformException &ex)
            {
                ROS_ERROR("Transform failure %d: %s",
                          ret1, ex.what());
                return;
            }
        }
};

int main(int argc, char** argv)
{
    ros::init(argc, argv, "look_at");

    ros::NodeHandle nodeHandle;
    LookAt lookAt(nodeHandle);
    ros::Subscriber handSubscriber = nodeHandle.subscribe(
        "/tracker/hand/info", 1000, &LookAt::handDetectionCallback, &lookAt);
    ros::spin();

    return 0;
}

The msg is a custom message containing:

Header header
geometry_msgs/Point point     # Position
int32 nId                     # Id of the hand
bool lostHand                 # True if the hand has just been lost

And when the message is sent from the hand tracker, msg->header.stamp is set to ros::Time::now().

The problem is that each time the waitForTransform function is called, it returns false and no exception is raised. When we look at the topic where we publish the info, we have a message each 5sec (5.01-5.03). When we set the first duration (4th argument) to 1.0, we have a message each 1sec (1.01-1.09) but the result is the same. If I understood, this means the transform between the 2 frames can't be made.

When we replace msg->header.stamp by ros::Time() or ros::Time(0), the waitForTransform function returns true and it takes about 1sec to get message in the topic with ros::Duration(5.0 ... (more)