ROS Answers: Open Source Q&A Forum - RSS feedhttps://answers.ros.org/questions/Open source question and answer forum written in Python and DjangoenROS Answers is licensed under Creative Commons Attribution 3.0Fri, 10 Feb 2017 07:11:32 -0600how to rotate a point(x,y) from current orientation to a given orientationhttps://answers.ros.org/question/74089/how-to-rotate-a-pointxy-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 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
else
cones[i].y3 = map_y + length; // p_mid
if ((map_x - radius) < 0)
cones[i].x2 = 0.0; // p2
else
cones[i].x2 = map_x - radius; // p2
if ((map_y + length) >= map_height)
cones[i].y2 = map_height; // p2
else
cones[i].y2 = map_y + length; // p2
if ((map_x - radius) >= map_width)
cones[i].x4 = map_width; // p3
else
cones[i].x4 = map_x + radius; // p3
if ((map_y + length) >= map_height)
cones[i].y4 = map_height; // p3
else
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
Wed, 21 Aug 2013 02:03:49 -0500https://answers.ros.org/question/74089/how-to-rotate-a-pointxy-from-current-orientation-to-a-given-orientation/Comment by dornhege for <div class="snippet"><p>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. </p>
<p>Could some one please suggest how can i do it in ros groovy ?? </p>
<p><strong>Update:</strong></p>
<p>I tried as below :</p>
<pre><code> 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));
</code></pre>
<p>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 ??</p>
<p>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)</p>
<p><strong>Update:</strong></p>
<p>class Cone
{
public:
float x1, y1, x2, y2, x3, y3, x4, y4;
float radius;
float height;
float angle; // not ...<span class="expander"> <a>(more)</a></span></p></div>https://answers.ros.org/question/74089/how-to-rotate-a-pointxy-from-current-orientation-to-a-given-orientation/?comment=74254#post-id-74254Maybe 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 inverseWed, 21 Aug 2013 23:04:14 -0500https://answers.ros.org/question/74089/how-to-rotate-a-pointxy-from-current-orientation-to-a-given-orientation/?comment=74254#post-id-74254Comment by kk for <div class="snippet"><p>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. </p>
<p>Could some one please suggest how can i do it in ros groovy ?? </p>
<p><strong>Update:</strong></p>
<p>I tried as below :</p>
<pre><code> 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));
</code></pre>
<p>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 ??</p>
<p>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)</p>
<p><strong>Update:</strong></p>
<p>class Cone
{
public:
float x1, y1, x2, y2, x3, y3, x4, y4;
float radius;
float height;
float angle; // not ...<span class="expander"> <a>(more)</a></span></p></div>https://answers.ros.org/question/74089/how-to-rotate-a-pointxy-from-current-orientation-to-a-given-orientation/?comment=74281#post-id-74281I have updated some more code and details.Thu, 22 Aug 2013 01:54:13 -0500https://answers.ros.org/question/74089/how-to-rotate-a-pointxy-from-current-orientation-to-a-given-orientation/?comment=74281#post-id-74281Answer by dornhege for <div class="snippet"><p>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. </p>
<p>Could some one please suggest how can i do it in ros groovy ?? </p>
<p><strong>Update:</strong></p>
<p>I tried as below :</p>
<pre><code> 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));
</code></pre>
<p>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 ??</p>
<p>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)</p>
<p><strong>Update:</strong></p>
<p>class Cone
{
public:
float x1, y1, x2, y2, x3, y3, x4, y4;
float radius;
float height;
float angle; // not ...<span class="expander"> <a>(more)</a></span></p></div> https://answers.ros.org/question/74089/how-to-rotate-a-pointxy-from-current-orientation-to-a-given-orientation/?answer=74125#post-id-74125Depends 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.Wed, 21 Aug 2013 06:14:01 -0500https://answers.ros.org/question/74089/how-to-rotate-a-pointxy-from-current-orientation-to-a-given-orientation/?answer=74125#post-id-74125Comment by dornhege for <p>Depends how you have your point and orientation.</p>
<p>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.</p>
<p>If it is just a 2d x,y and theta, I'd probably just do it by hand.</p>
https://answers.ros.org/question/74089/how-to-rotate-a-pointxy-from-current-orientation-to-a-given-orientation/?comment=74134#post-id-74134In 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.Wed, 21 Aug 2013 06:46:03 -0500https://answers.ros.org/question/74089/how-to-rotate-a-pointxy-from-current-orientation-to-a-given-orientation/?comment=74134#post-id-74134Comment by kk for <p>Depends how you have your point and orientation.</p>
<p>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.</p>
<p>If it is just a 2d x,y and theta, I'd probably just do it by hand.</p>
https://answers.ros.org/question/74089/how-to-rotate-a-pointxy-from-current-orientation-to-a-given-orientation/?comment=74131#post-id-74131Thank 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.Wed, 21 Aug 2013 06:19:21 -0500https://answers.ros.org/question/74089/how-to-rotate-a-pointxy-from-current-orientation-to-a-given-orientation/?comment=74131#post-id-74131Comment by kk for <p>Depends how you have your point and orientation.</p>
<p>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.</p>
<p>If it is just a 2d x,y and theta, I'd probably just do it by hand.</p>
https://answers.ros.org/question/74089/how-to-rotate-a-pointxy-from-current-orientation-to-a-given-orientation/?comment=74194#post-id-74194I tried but not working correctly. I have updated the my question with some code snippets that i tried.Wed, 21 Aug 2013 13:04:12 -0500https://answers.ros.org/question/74089/how-to-rotate-a-pointxy-from-current-orientation-to-a-given-orientation/?comment=74194#post-id-74194Answer by Wiew for <div class="snippet"><p>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. </p>
<p>Could some one please suggest how can i do it in ros groovy ?? </p>
<p><strong>Update:</strong></p>
<p>I tried as below :</p>
<pre><code> 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));
</code></pre>
<p>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 ??</p>
<p>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)</p>
<p><strong>Update:</strong></p>
<p>class Cone
{
public:
float x1, y1, x2, y2, x3, y3, x4, y4;
float radius;
float height;
float angle; // not ...<span class="expander"> <a>(more)</a></span></p></div> https://answers.ros.org/question/74089/how-to-rotate-a-pointxy-from-current-orientation-to-a-given-orientation/?answer=254331#post-id-254331In my research i used a DCM Matrix /Matrix of direct cosine/. This matrix provides (2D or 3D rotation) of point. This method is very simply, but you need matrix calculations. On wikipedia you are able to find how to transform data. In our paper the example of matixies and calculations are shown.
<code> Blockquote
https://www.researchgate.net/publication/312140651_The_Depth_Map_Construction_from_a_3D_Point_Cloud
</code> BlockquoteFri, 10 Feb 2017 07:11:32 -0600https://answers.ros.org/question/74089/how-to-rotate-a-pointxy-from-current-orientation-to-a-given-orientation/?answer=254331#post-id-254331Answer by kk for <div class="snippet"><p>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. </p>
<p>Could some one please suggest how can i do it in ros groovy ?? </p>
<p><strong>Update:</strong></p>
<p>I tried as below :</p>
<pre><code> 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));
</code></pre>
<p>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 ??</p>
<p>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)</p>
<p><strong>Update:</strong></p>
<p>class Cone
{
public:
float x1, y1, x2, y2, x3, y3, x4, y4;
float radius;
float height;
float angle; // not ...<span class="expander"> <a>(more)</a></span></p></div> https://answers.ros.org/question/74089/how-to-rotate-a-pointxy-from-current-orientation-to-a-given-orientation/?answer=74623#post-id-74623I 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. Sat, 24 Aug 2013 05:42:57 -0500https://answers.ros.org/question/74089/how-to-rotate-a-pointxy-from-current-orientation-to-a-given-orientation/?answer=74623#post-id-74623