# get rotation between two frames

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).

edit 11-03

    tf::StampedTransform transform;
geometry_msgs::PoseStamped new_goal;
geometry_msgs::PointStamped position_in, position_out;
int id = 50;
try
{
}
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.point.x = pt.x;
position_in.point.y = pt.y;
listener->transformPoint("map", position_in, position_out);

// 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 close merge delete

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).

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?

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!

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

Thanks.

Sort by » oldest newest most voted

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?

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