Ask Your Question

get rotation between two frames

asked 2016-11-01 22:06:17 -0500

chengwei gravatar image

updated 2016-11-03 02:55:57 -0500


I have two frames, one is global frame("map"), another is local frame ("laser"). And I have the transform between them.

I want to publish a Navigation Goal, and the Goal is obtaining from the laserscan. The Goal's position is obtaining from a laserscan range float ra = scan_msg->ranges[t] ,and the Goal's orientation is get from a angle float angle = scan_msg->angle_min +t * scan_msg->angle_increment,here the t is a constant(specified).

Thank you in advance!

edit 11-03

    tf::StampedTransform transform;
    geometry_msgs::PoseStamped new_goal;
     geometry_msgs::PointStamped position_in, position_out;
    int id = 50;
            listener->waitForTransform("/map", "/laser", scan_msg->header.stamp, ros::Duration(10.0));
            listener->lookupTransform("/map", "/laser",  scan_msg->header.stamp, transform);
        catch (tf::TransformException& ex)
            ROS_ERROR("Received an exception trying to transform a point from \"laser\" to \"map\": %s", ex.what());

        float angle = scan_msg->angle_min + id * scan_msg->angle_increment;
        pt.x = ra * cos(angle);
        pt.y = ra * sin(angle);

        position_in.header = scan_msg->header;
        position_in.point.x = pt.x;
        position_in.point.y = pt.y;
        listener->transformPoint("map", position_in, position_out);

        // header
        new_goal.header.frame_id = "map";
        new_goal.header.stamp = ros::Time::now();
        // position
        new_goal.pose.position.x = position_out.point.x;
        new_goal.pose.position.y = position_out.point.y;
        new_goal.pose.position.z = 0.0;

        tf::Quaternion q = transform.getRotation();
        double yaw = tf::getYaw(q);
        double angle_goal = yaw + angle_orientation;

        geometry_msgs::Quaternion goal_quat = tf::createQuaternionMsgFromYaw(angle_goal);
        new_goal.pose.orientation = goal_quat;

I solve the problem in two steps.

First, transform the point position from /laser to /map, as listener->transformPoint("map", position_in, position_out);

Second, calculate the Yaw from /laser to /map as double angle_goal = yaw + angle_orientation;

I need to do some test and verify.

If somebody have good Suggestions, please let me know.

Thank you!

edit retag flag offensive close merge delete



If you have the transformation matrix, the rotation matrix is the first 3x3 elements. From the rotation matrix, you can convert it to quaternion (or Euler angles).


JoshMarino gravatar image JoshMarino  ( 2016-11-01 22:30:21 -0500 )edit

I want to publish a goal position to the robot,and I want get the position from a laserscan as fellows.

float ra = scan_msg->ranges[t];
float angle = scan_msg->angle_min + i * scan_msg->angle_increment;

I could transform coordinate from "base_link" to "map" , how to transform the angle?

chengwei gravatar image chengwei  ( 2016-11-01 23:11:11 -0500 )edit


Sorry for my poor English,

I want to publish a Navigation Goal,now I want to transform a Orientation from a local frame(laser) to a global frame (map), and I get the Orientation as a angle angle = scan_msg->angle_min + t * scan_msg->angle_increment, any suggest is acceptable!

chengwei gravatar image chengwei  ( 2016-11-02 20:38:17 -0500 )edit

Line 24 will get transformation (translation and rotation) between two frames. Here can convert rotation matrix to angles or quaternion.

JoshMarino gravatar image JoshMarino  ( 2016-11-02 21:34:05 -0500 )edit

Could you please post your last edit as an answer, and then accept your own answer? That shows much more clearly that your question was answered than if you close it.


gvdhoorn gravatar image gvdhoorn  ( 2016-11-03 18:14:59 -0500 )edit

1 Answer

Sort by » oldest newest most voted

answered 2016-11-02 03:39:32 -0500

Akif gravatar image

updated 2016-11-03 02:22:05 -0500

tf::Transform has methods for obtaining rotations like getRotation().

Though, you can multiply Transforms directly to have angular and linear transformations. No need to separate them.


I'm not sure if I get you correct but you can transform your laser scan to map using TF like this;

tf::StampedTransform transform;
ros::Time t = ros::Time(0);
tf_->waitForTransform("map", "laser", t, ros::Duration(1.0));
tf_->lookupTransform("map", "laser", t, transform);

then, you can get x, y and theta like;

double x = transform.getOrigin().getX();
double y = transform.getOrigin().getY();
double th = tf::getYaw(transform.getRotation());

Is this what you ask for?

edit flag offensive delete link more


Hi,Akif, I'm sorry for my poor English. I want to publish a Navigation Goal, and the Goal's position is obtaining from a laserscan range float ra = scan_msg->ranges[t] ,and the Goal's orientation is get from a angle float angle = scan_msg->angle_min + t * scan_msg->angle_increment

chengwei gravatar image chengwei  ( 2016-11-02 20:23:56 -0500 )edit

How does tf_ declared?

artemiialessandrini gravatar image artemiialessandrini  ( 2019-11-27 21:37:54 -0500 )edit

First include the correct header:

#include <tf/transform_listener.h>

Followed by defining the following:

tf::TransformListener *tf_ = new tf::TransformListener;
tf_->waitForTransform("map", "laser", t, ros::Duration(1.0));
tf_->lookupTransform("map", "laser", t, transform);

For more information, you can refer to:

ParkerRobert gravatar image ParkerRobert  ( 2021-04-02 04:44:35 -0500 )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



Asked: 2016-11-01 22:06:17 -0500

Seen: 5,976 times

Last updated: Nov 03 '16