Ask Your Question
0

get rotation between two frames

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

chengwei gravatar image

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

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!

edit retag flag offensive close merge delete

Comments

1

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

JoshMarino gravatar imageJoshMarino ( 2016-11-01 22:30:21 -0600 )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 imagechengwei ( 2016-11-01 23:11:11 -0600 )edit

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!

chengwei gravatar imagechengwei ( 2016-11-02 20:38:17 -0600 )edit

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

JoshMarino gravatar imageJoshMarino ( 2016-11-02 21:34:05 -0600 )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.

Thanks.

gvdhoorn gravatar imagegvdhoorn ( 2016-11-03 18:14:59 -0600 )edit

1 Answer

Sort by » oldest newest most voted
2

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

Akif gravatar image

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

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?

edit flag offensive delete link more

Comments

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 imagechengwei ( 2016-11-02 20:23:56 -0600 )edit

How does tf_ declared?

artemiialessandrini gravatar imageartemiialessandrini ( 2019-11-27 21:37:54 -0600 )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

2 followers

Stats

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

Seen: 3,968 times

Last updated: Nov 03 '16