Ask Your Question
0

Problem using TFs

asked 2013-10-29 22:49:44 -0600

LucieR gravatar image

updated 2014-01-28 17:18:23 -0600

ngrennan gravatar image

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)

edit retag flag offensive close merge delete

1 Answer

Sort by ยป oldest newest most voted
0

answered 2013-11-04 10:01:05 -0600

tfoote gravatar image

Please work on providing a more concise sample and include both your expected output and observed output.

From a quick look at your code. One thing pops out as incorrect usage of TransformListener, in that you're allocating it inside your callback such that it doesn't have time to build up it's cache of data. You should allocate it in a way that it will persist for the duration of your program.

edit flag offensive delete link more

Comments

I saw that problem too and even if I allocate it outside the callback (in the constructor for example) nothing changes. I'll print output time in both cases. And I can't do or more concise sample; only first part of code is the sample I have a problem on, the second one is working fine.

LucieR gravatar imageLucieR ( 2013-11-04 20:21:09 -0600 )edit

Your Answer

Please start posting anonymously - your entry will be published after you log in or create a new account.

Add Answer

Question Tools

1 follower

Stats

Asked: 2013-10-29 22:49:44 -0600

Seen: 240 times

Last updated: Nov 04 '13