Change tf/ pose orientation in package

asked 2019-02-24 22:46:03 -0500

IvyKK gravatar image

updated 2019-03-19 03:45:45 -0500

Hi, I have a problem while using lidar (without odometry) for navigation stack: the tf goes to the wrong orientation when I move the robot. i.e. when I move robot to right, the tf goes to left..

For the odometry, I'm using rf2o laser_to_odometry. Also, I rotated my lidar 90 degrees around z direction, and flipped upside down, and it scans only half range, from -180 to 0 degree.

The tf setup seems to be correct because I run the same tf for hector slam to create a map, and no problem with the pose direction. Also, the map created is not flipped.

Is there anything I could change in the package (may be in navigation stack or rf2o laser to odometry) to solve this, like by just adding code to mirror the tf ?

The following is the code (also can be found here) I find in CLaserOdometry2DNode.cpp of the rf2o package..Will that be something wrong here causing the tf is not read correctly in the package, or is that any way to edit this, to mirror the tf (left and right, i.e. mirror along y-axis)?

    // Set laser pose on the robot (through tF)
    // This allow estimation of the odometry with respect to the robot base reference system.
    tf::StampedTransform transform;
    transform.setIdentity();
    try
    {
      tf_listener.lookupTransform(base_frame_id, last_scan.header.frame_id, ros::Time(0), transform);
      retrieved = true;
    }
    catch (tf::TransformException &ex)
    {
      ROS_ERROR("%s",ex.what());
      ros::Duration(1.0).sleep();
      retrieved = false;
    }

    //TF:transform -> Eigen::Isometry3d

    const tf::Matrix3x3 &basis = transform.getBasis();
    Eigen::Matrix3d R;

    for(int r = 0; r < 3; r++)
      for(int c = 0; c < 3; c++)
        R(r,c) = basis[r][c];

    Pose3d laser_tf(R);

    const tf::Vector3 &t = transform.getOrigin();
    laser_tf.translation()(0) = t[0];
    laser_tf.translation()(1) = t[1];
    laser_tf.translation()(2) = t[2];

    setLaserPose(laser_tf);

    return retrieved;
  }

(I'm at a very beginner stage in ROS and coding) frustratingly need your help! Thanks in advance!

edit retag flag offensive close merge delete

Comments

You should always initialize your Eigen matrices to Identity, this will avoid nasty errors: Eigen::Matrix3d R(Eigen::Matrix3d::Identity()); (this is not the problem here). And also, use http://docs.ros.org/melodic/api/tf_co... to convert between tf/Eigen, again, this will avoid nasty errors when working on more complex transforms.

VictorLamoine gravatar image VictorLamoine  ( 2019-03-21 10:28:25 -0500 )edit

Thankss VictorLamoine for the comment. I tried out this. but it seems to be the same. If you don't mind, could you advice may be how to mirror the tf along the y-axis?

IvyKK gravatar image IvyKK  ( 2019-03-22 02:07:29 -0500 )edit