Transform a point - problem on rotation

asked 2017-02-03 07:34:51 -0500

metRo_ gravatar image

updated 2017-02-03 10:45:58 -0500

Hello,

I have a point on base_link (robot frame). When the robot move I want to compensate the point position so it stays on the same position (odom). Per example at t=0 the point is at 0,0 (base_link). At t=1 the robot is at 1,0 (odom) so my point should be at -1,0 (base_link). Suppose the odometry is perfect.

https://youtu.be/K0w0yqEq44g

As you can see the two squares are moving okay until the robot start to rotate then my code start moving the squares on the wrong way.

How I'm trying to do it:

void odometryCallback(const nav_msgs::Odometry& msg){

    static tf::TransformBroadcaster br;

    tf::StampedTransform transform;
    try{
        listener->lookupTransform("odom", "base_link",
                                 ros::Time(0), transform);
    }
    catch (tf::TransformException ex){
        ROS_ERROR("%s",ex.what());
        return;
    }

    double delta_x = transform.getOrigin().x() - last_transform.getOrigin().x();
    double delta_y = transform.getOrigin().y() - last_transform.getOrigin().y();
    double delta_theta = tf::getYaw(transform.getRotation())-tf::getYaw(last_transform.getRotation());

    last_transform = transform;

    if(std::isnan(delta_theta))
        return;

    convertArrayToPointCloud();
    for(int i=0;i<point_cloud_transformed.points.size();i++){
        point_cloud_transformed.points.at(i) = rotMat(point_cloud_transformed.points.at(i),-delta_theta);
        point_cloud_transformed.points.at(i).x = point_cloud_transformed.points.at(i).x + delta_x*cos(delta_theta);
        point_cloud_transformed.points.at(i).y = point_cloud_transformed.points.at(i).y - delta_y*sin(delta_theta);
    }

    pub_cloud.publish(point_cloud_transformed);

    pointCloudToArray();

}

It works on some directions only, so I suppose I'm doing something wrong about the delta_theta but can't find what :s

edit retag flag offensive close merge delete

Comments

Hi! ┬┐Could you find the solution to your issue? I am having the same problem.

fandrade gravatar image fandrade  ( 2017-11-23 11:17:40 -0500 )edit