get rotation between two frames

edit

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!