Rosanswers logo

I saw many api's to convert between euler angles and quaternions but could not find an api which actually performs rotation. Basically i want to rotate a point from current orientation to new orientation.

Could some one please suggest how can i do it in ros groovy ??


I tried as below :

    geometry_msgs::Pose pose1, pose2, pose3;
// point P1 of triangle.
pose1.position.x = cones[0].x1 * map_resolution + map_origin_x ; 
pose1.position.y = cones[0].y1 * map_resolution + map_origin_y;
pose1.position.z = 0.0;
//some assumed orientation
pose1.orientation.x = 0.0;  
pose1.orientation.y = 0.0;
pose1.orientation.z = 1.0;
pose1.orientation.w = yaw; 

// point P2 of triangle.
pose2.position.x = cones[0].x2 * map_resolution + map_origin_x;
pose2.position.y = cones[0].y2 * map_resolution + map_origin_y;
pose2.position.z = 0.0;
//some assumed orientation
pose2.orientation.x = 0.0;  
pose2.orientation.y = 0.0;
pose2.orientation.z = 1.0;
pose2.orientation.w = yaw;

// point P3 of triangle.
pose3.position.x = cones[0].x4 * map_resolution + map_origin_x;
pose3.position.y = cones[0].y4 * map_resolution + map_origin_y;
pose3.position.z = 0.0;
//some assumed orientation
pose3.orientation.x = 0.0;  
pose3.orientation.y = 0.0;
pose3.orientation.z = 1.0;
pose3.orientation.w = yaw;

tf::Transform tf_pose1, tf_pose2, tf_pose3, tf_pose4, tf_pose5, tf_pose6, tf_pose7, tf_pose8, tf_pose9, tf_pose_origin;

// Original created Points:
tf_pose1 = tf::Transform::getIdentity();
tf::poseMsgToTF(pose1, tf_pose1);
br.sendTransform(tf::StampedTransform(tf_pose1, ros::Time::now(), "map", frame_id1));

tf_pose2 = tf::Transform::getIdentity();
tf::poseMsgToTF(pose2, tf_pose2);
br.sendTransform(tf::StampedTransform(tf_pose2, ros::Time::now(), "map", frame_id2));

tf_pose3 = tf::Transform::getIdentity();
tf::poseMsgToTF(pose3, tf_pose3);
br.sendTransform(tf::StampedTransform(tf_pose3, ros::Time::now(), "map", frame_id3));

// Actual given Pose
tf_pose_origin = tf::Transform::getIdentity();
tf::poseMsgToTF(poseArray.poses[0], tf_pose_origin);
br.sendTransform(tf::StampedTransform(tf_pose_origin, ros::Time::now(), "map", frame_id_origin));

// Rotated Points:

// Assign P1 to same as origin since its position and orientation is same as of given pose.
tf_pose4 = tf_pose_origin;
br.sendTransform(tf::StampedTransform(tf_pose4, ros::Time::now(), "map", frame_id4));

// rotate P2
tf_pose5 = tf_pose2.inverseTimes(tf_pose_origin);
br.sendTransform(tf::StampedTransform(tf_pose5, ros::Time::now(), "map", frame_id5));

// rotate P3
tf_pose6 = tf_pose3.inverseTimes(tf_pose_origin);
br.sendTransform(tf::StampedTransform(tf_pose6, ros::Time::now(), "map", frame_id6));

// Another try reversing the order of multiplication:

// Assign P1 to same as origin since its position and orientation is same as of given pose.
tf_pose7 = tf_pose_origin;
br.sendTransform(tf::StampedTransform(tf_pose7, ros::Time::now(), "map", frame_id7));

// rotate P2
tf_pose8 = tf_pose_origin.inverseTimes(tf_pose2);
br.sendTransform(tf::StampedTransform(tf_pose8, ros::Time::now(), "map", frame_id8));

// rotate P3
tf_pose9 = tf_pose_origin.inverseTimes(tf_pose3);
br.sendTransform(tf::StampedTransform(tf_pose9, ros::Time::now(), "map", frame_id9));

Even though the position of P2 and P3 changed they were not as expected. The triangle in total was not in the required orientation :( Any idea ??

Or atleast any idea how to create a triangle given a pose and height(assume distance between P2 and P3 is equal to distance between P1 and midpoint of P2 and P3)


class Cone { public: float x1, y1, x2, y2, x3, y3, x4, y4; float radius; float height; float angle; // not used currently float probability; int id; };

void generateCones(Cone cones[]) {
//std::cerr << "inside generateCones" << std::endl; for (int i = 0; i < num_poses; i++) { float pose_x = poseArray.poses[i].position.x; // This poseArray of type geometry_msgs::PoseArray has random poses generated in prior float pose_y = poseArray.poses[i].position.y; int map_x = (pose_x - map_origin_x) / map_resolution; // convert into map cell resolution int map_y = (pose_y - map_origin_y) / map_resolution;

cones[i].x1 = map_x;    // p1
cones[i].y1 = map_y;  // p1
cones[i].x3 = map_x;  // p_mid

if ((map_y + length) >= map_height)
    cones[i].y3 = map_height;  // p_mid
cones[i].y3 = map_y + length;  // p_mid

if ((map_x - radius) < 0)
    cones[i].x2 = 0.0;  // p2
cones[i].x2 = map_x - radius;  // p2

if ((map_y + length) >= map_height)
    cones[i].y2 = map_height;  // p2
cones[i].y2 = map_y + length;  // p2

if ((map_x - radius) >= map_width)
    cones[i].x4 = map_width;  // p3
cones[i].x4 = map_x + radius;  // p3

if ((map_y + length) >= map_height)
    cones[i].y4 = map_height;  // p3
cones[i].y4 = map_y + length;  // p3

cones[i].radius = radius; //fixed radius
cones[i].height = length; //fixed length
cones[i].id = i;
cones[i].probability = 0.0; }}

Cones[i] has just (x,y) values...

Now i need to rotate each cone according to the orientations in poseArray

Originally posted by kk on ROS Answers with karma: 225 on 2013-08-21

Post score: 1

Original comments

Comment by dornhege on 2013-08-21:
Maybe a different question: What is your input to which goal? Do you want global/map coordinates for your cones? It looks like you're using global inputs in the cones[0]. It's quite straight-forward to only define a cone robot-relative and then transform that to map coords (although just the inverse

Comment by kk on 2013-08-22:
I have updated some more code and details.


2 Answers 2


Rosanswers logo

Depends how you have your point and orientation.

If it is tf data types, you can just make a transform out of the orientation and transform the point with that. geometry_msgs can be easily transformed to tf with helper functions.

If it is just a 2d x,y and theta, I'd probably just do it by hand.

Originally posted by dornhege with karma: 31395 on 2013-08-21

This answer was NOT ACCEPTED on the original site

Post score: 1

Original comments

Comment by kk on 2013-08-21:
Thank you for the comment. Its is in geometry_msgs:Pose type.. plz check this http://answers.ros.org/question/73996/how-to-rotate-a-triangle-to-a-given-pose-orientation/ for more details of my query.

Comment by dornhege on 2013-08-21:
In that case, just convert them to tf data types. Check the API, the functions are called something like: tf::poseMsgToTF(msg, tfPose);. You can then use intuitive math like p2.inverseTimes(p1) to get relative poses.

Comment by kk on 2013-08-21:
I tried but not working correctly. I have updated the my question with some code snippets that i tried.


Rosanswers logo

I got it working. Instead of creating points in global frame, i created with respect to the given pose frame. And then looked up between the global and local frames to find the exact points.

Originally posted by kk with karma: 225 on 2013-08-24

This answer was ACCEPTED on the original site

Post score: 0


Your Answer

By clicking “Post Your Answer”, you agree to our terms of service and acknowledge you have read our privacy policy.