0
$\begingroup$

Rosanswers logo

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) as the 4th argument. So the transform is ok but takes around 1sec. With the head nodes, everything is fluid and nice and we can have a good tracking with the head. Here is the working source code:

typedef package_head_controller::PointHeadAction HeadAction;
typedef actionlib::SimpleActionClient<HeadAction> PointHeadClient;

class PointHead
{
    private:
        ros::NodeHandle _node;
        PointHeadClient* _pointHeadClient;

    public:
        PointHead(const ros::NodeHandle &n);
        ~PointHead();

        void lookAt(std::string frameId, float x, float y, float z,
                    float tilt, float pan);

        void callbackGoal(const package_msgs::PointHead::ConstPtr& msg);
};


PointHead::PointHead(const ros::NodeHandle &n): _node(n)
{
    // Initialize the client
    _pointHeadClient = new PointHeadClient("/head_controller/point_head_action", true);

    while(!_pointHeadClient->waitForServer(ros::Duration(5.0)))
    {
        ROS_INFO("Waiting for the point_head_action server to come up");
    }
}

PointHead::~PointHead()
{
    delete _pointHeadClient;
}

void PointHead::lookAt(std::string frameId, float x, float y, float z,
                       float tilt, float pan)
{
    package_head_controller::PointHeadGoal goal;

    // The target point, expressed in the requested frame
    geometry_msgs::PointStamped point;
    point.header.frame_id = frameId;
    point.point.x = x;
    point.point.y = y;
    point.point.z = z;
    point.header.stamp = ros::Time::now();
    goal.target = point;
    goal.tilt = tilt;
    goal.pan = pan;

    goal.pointingFrame = "camera_link";
    _pointHeadClient->sendGoal(goal);
}

void PointHead::callbackGoal(const package_msgs::PointHead::ConstPtr& msg)
{
    if(msg->point.x != 0 || msg->point.y != 0 || msg->point.z != 0)
    {
        lookAt("camera_link", msg->point.x, msg->point.y,
               msg->point.z, msg->tilt, msg->pan);
    }
}


int main(int argc, char** argv)
{
    ros::init(argc, argv, "point_head");
    ros::NodeHandle n;

    PointHead pointHead(n);
    ros::Subscriber goalSub = n.subscribe("/head_controller/point_head/goal",
                                          1, &PointHead::callbackGoal,
                                          &pointHead);
    ros::spin();
}

typedef actionlib::ActionServer<package_head_controller::PointHeadAction> Server;
typedef Server::GoalHandle GoalHandle;

class HeadAction
{
    private:
        std::string _panLink;
        std::string _tiltLink;

        ros::NodeHandle _node;
        tf::TransformListener _tf;

        ros::Publisher _commandMotors;

        Server _actionServer;
        bool _hasActiveGoal;
        GoalHandle _activeGoal;

        tf::Stamped<tf::Point> _targetInPan;
        std::string _panParent;

        void cancelCallback(GoalHandle gh);

    public:
        HeadAction(const ros::NodeHandle &n);
        ~HeadAction();

        void goalCallback(GoalHandle gh);
};

HeadAction::HeadAction(const ros::NodeHandle &n):
    _node(n), _hasActiveGoal(false),
    _actionServer(_node, "/head_controller/point_head_action",
                  boost::bind(&HeadAction::goalCallback, this, _1),
                  boost::bind(&HeadAction::cancelCallback, this, _1))
{
    ros::NodeHandle pn("~");
    pn.param("pan_link", _panLink, std::string("link_2_left"));
    pn.param("tilt_link", _tiltLink, std::string("linf_3_left"));

    _commandMotors = _node.advertise<package_msgs::PointHeadCommand>
        ("/head_controller/point_head/result", 1000);
}

HeadAction::~HeadAction()
{

}

void HeadAction::goalCallback(GoalHandle gh)
{
    // Need to know that name of the pan_link's parent.
    if(_panParent.empty())
    {
        for(int i = 0; i < 10; ++i)
        {
            try
            {
                _tf.getParent(_panLink, ros::Time(), _panParent);
                break;
            }
            catch(const tf::TransformException &ex)
            {
            }
            ros::Duration(0.01).sleep();
        }

        if(_panParent.empty())
        {
            ROS_ERROR("Could not get parent of %s in the TF tree",
                      _panLink.c_str());
            gh.setRejected();
            return;
        }
    }

    std::vector<double> q_goal(2);  // [pan, tilt]

    // Transform the target point into the pan and tilt links.
    const geometry_msgs::PointStamped &target = gh.getGoal()->target;
    bool ret1 = false, ret2 = false;
    try
    {
        std::string error_msg;
        ret1 = _tf.waitForTransform(_panParent, target.header.frame_id,
                                    target.header.stamp,
                                    ros::Duration(5.0),
                                    ros::Duration(0.01), &error_msg);
        ret2 = _tf.waitForTransform(_panLink, target.header.frame_id,
                                    target.header.stamp,
                                    ros::Duration(5.0),
                                    ros::Duration(0.01), &error_msg);

        // Perform calculation to determine the desired joint angles

        // Transform the target into the pan and tilt frames
        tf::Stamped<tf::Point> targetPoint, targetInTilt;
        tf::pointStampedMsgToTF(target, targetPoint);
        _tf.transformPoint(_panParent, targetPoint, _targetInPan);
        _tf.transformPoint(_panLink, targetPoint, targetInTilt);

        // Compute the desired joint positions.
        q_goal[0] = atan2(_targetInPan.y(), _targetInPan.x());
        q_goal[1] = atan2(-targetInTilt.z(),
                          sqrt(pow(targetInTilt.x(),2) +
                               pow(targetInTilt.y(),2)));
        // Remove false positives problem
        if(q_goal[1] > 0.5)
        {
            q_goal[0] = 0.0;
            q_goal[1] = 0.0;
        }

        if(gh.getGoal()->tilt != 0.0)
        {
            q_goal[1] = gh.getGoal()->tilt;
        }
        if(gh.getGoal()->pan != 0.0)
        {
            q_goal[0] = gh.getGoal()->pan;
        }

        package_msgs::PointHeadCommand command;
        command.header.stamp = ros::Time::now();
        command.id = target.header.seq;
        command.pan = q_goal[0];
        command.tilt = q_goal[1];
        _commandMotors.publish(command);


    }
    catch(const tf::TransformException &ex)
    {
        ROS_ERROR("Transform failure (%d,%d): %s",
                  ret1, ret2, ex.what());
        gh.setRejected();
        return;
    }

    // Remove the previous goal and keep this one in memory
    if(_hasActiveGoal)
    {
        _activeGoal.setCanceled();
        _hasActiveGoal = false;
    }

    gh.setAccepted();
    _activeGoal = gh;
    _hasActiveGoal = true;
}

void HeadAction::cancelCallback(GoalHandle gh)
{

}


int main(int argc, char** argv)
{
    ros::init(argc, argv, "point_head_action");
    ros::NodeHandle node;

    HeadAction headAction(node);

    ros::spin();
    return 0;
}

I can't find the problem with the look_at node using transform and I'm wondering what's the problem observed with the stamp argument. Is there a "standard" way of using TFs. Do we forget to do something?

If you see any mistake of the way we are using TFs in the look_at node, please let me know.

Thanks,

Lucie


Originally posted by LucieR on ROS Answers with karma: 1 on 2013-10-29

Post score: 0

$\endgroup$

1 Answer 1

0
$\begingroup$

Rosanswers logo

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.


Originally posted by tfoote with karma: 58457 on 2013-11-04

This answer was ACCEPTED on the original site

Post score: 0


Original comments

Comment by LucieR on 2013-11-04:
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.

$\endgroup$

Your Answer

By clicking “Post Your Answer”, you agree to our terms of service and acknowledge you have read our privacy policy.