How to make a maker parallel to a vector
Hi there, I'm trying to define a marker(cylinder) which connects two known frames visualized in rviz. I want this maker point from one frame to the other frame...
And the what I've gotten now is the transform between these two known frames. The first thing I come up with is make this marker parallel to this transform vector. But I was failed to implement this idea.
I would be appreciated if you can give some Pseudo Code (and the function names that will be perfect). OR, maybe a greater idea to make this thing up.
Codeofmine
marker.pose.position.x = ls_transform.getOrigin().x()/2;
marker.pose.position.y = ls_transform.getOrigin().y()/2;
marker.pose.position.z = ls_transform.getOrigin().z()/2;
marker.pose.orientation.x = ls_transform.getRotation().x();
marker.pose.orientation.y = ls_transform.getRotation().y();
marker.pose.orientation.z = ls_transform.getRotation().z();
marker.pose.orientation.w = ls_transform.getRotation().w();
And the marker is put in the middle of two frame, but the angle is not correct. (Sorry I can't load up my picture cause I don't got enough points...)
P.S. I set the markerframe follow the one of two frames(which means origin = 0). When I set the markerframe is my first frame, problem still occur.
Asked by willIamxue96 on 2018-06-03 02:14:35 UTC
Answers
The marker's pose is given by a pose (marker.pose) which is intrepreted relative to the frame that you pass in the header. So as you already have the transformation between the two frames, pass this transform as marker.pose and set the header.frame_id to you first frame:
visualization_msgs::Marker marker;
marker.header.frame_id = "frame1";
marker.type = visualization_msgs::Marker::CYLINDER;
marker.pose.position = [translation of other frame]/2
marker.pose.orientation = orientation of frame2 relative to frame1
marker.scale.x = marker.scale.y = 0.1; // diameter of cylinder
marker.scale.z = translation.z;
marker.color.a = marker.color.r = 1.0; // red
vis_pub.publish( marker );
// thanks for the hint that my first answer was wrong...
Asked by NEngelhard on 2018-06-03 06:40:03 UTC
Comments
Thanks for you attention, I followed your command and here is some of my code(due to the maximum limitation, I put result in question ):
Asked by willIamxue96 on 2018-06-03 07:19:11 UTC
It shouldn't be "orientation of frame2 relative to frame1" it should be "orientation of (the vector from the origin of frame1 to the origin of frame2) in the coordinate system of frame1. Hope this makes sense
Asked by PeteBlackerThe3rd on 2018-06-05 16:11:02 UTC
you mean marker.pose.orientation.x = frame1_to_frame2.x();
notfram2_to_frame1.x()
?
Asked by willIamxue96 on 2018-06-06 05:43:44 UTC
The simplest way to do this is to use an arrow marker and set it's head length to zero and it's head diameter to the same as it's shaft diameter. The reason this would be simpler is because it can be specified with two points avoiding the need to calculate the centre and orientation.
This is a bit of an abuse of the arrow marker but would look exactly the same as the cylinder and requires less code. I'll post a code example when I'm back at a computer.
Asked by PeteBlackerThe3rd on 2018-06-03 10:29:08 UTC
Comments
Cool man, I just got the same answer as you did. And I have done this setting z.scale = 0.01(It looks really like the cylinder way...).
But, there is another question, can I do this "start point-end point" philosophy with something custom? May be in mesh resource(.dae)?
Asked by willIamxue96 on 2018-06-05 11:41:54 UTC
Is this definition codes should be in DAE manually or they will generated from SolidWork naturally?
Asked by willIamxue96 on 2018-06-05 11:46:45 UTC
This start and end definition only works for arrows and line segments I'm afraid. If you want to position a DAE model in the same way you'll have to use the approach described by @NEngelhard below.
Asked by PeteBlackerThe3rd on 2018-06-05 12:34:15 UTC
Comments