how to rotate a point(x,y) from current orientation to a given orientation
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 ??
Update:
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)
Update:
class Cone { public: float x1, y1, x2, y2, x3, y3, x4, y4; float radius; float height; float angle; // not ...
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
I have updated some more code and details.