get rotation between two frames

Question

Hi,everyone,

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;
        try
        {
            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;
        p_pub.publish(new_goal);

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!

---

Originally posted by chengwei on ROS Answers with karma: 51 on 2016-11-01

Post score: 0

---

Original comments

Comment by JoshMarino on 2016-11-01:
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).

Link

Comment by chengwei on 2016-11-01:
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?

Comment by chengwei on 2016-11-02:
Hi,JoshMarino,

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!

Comment by JoshMarino on 2016-11-02:
Line 24 will get transformation (translation and rotation) between two frames. Here can convert rotation matrix to angles or quaternion.

Comment by gvdhoorn on 2016-11-03:
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.

Thanks.

Answer 1

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.

Edit:

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?

---

Originally posted by Akif with karma: 3561 on 2016-11-02

This answer was ACCEPTED on the original site

Post score: 2

---

Original comments

Comment by chengwei on 2016-11-02:
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，

Comment by artemiialessandrini on 2019-11-27:
How does tf_ declared?

Comment by ParkerRobert on 2021-04-02:
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: http://wiki.ros.org/tf/Tutorials/Writing%20a%20tf%20listener%20%28C%2B%2B%29