[eustf] fix bug in :set-transform

[Naoki-Hiraoka]

https://github.com/euslisp/jskeus/pull/589

:set-transform does not work correctly because the order of quaternion was incorrect.

before this PR

$ (setq tr (instance ros::transformer :init))
#<ros::transformer #X545e290>
$ (send tr :set-transform (instance ros::tf-cascaded-coords :init :pos #f(1 2 3) :frame-id "this_frame" :child-frame-id "child"))
$ (send tr :get-frame-strings)
("this_frame" "child")
$ (send tr :lookup-transform "this_frame" "child" (ros::time))
#<cascaded-coords #X5438430 this_frame  1000.0 2000.0 3000.0 / 3.142 0.0 3.142>
$ (send tr :lookup-transform "child"  "this_frame" (ros::time))
#<cascaded-coords #X54388e0 child  1000.0 -2000.0 3000.0 / 3.142 0.0 3.142>

after this PR

$ (setq tr (instance ros::transformer :init))
#<ros::transformer #X545e290>
$ (send tr :set-transform (instance ros::tf-cascaded-coords :init :pos #f(1 2 3) :frame-id "this_frame" :child-frame-id "child"))
$ (send tr :get-frame-strings)
("this_frame" "child")
$ (send tr :lookup-transform "this_frame" "child" (ros::time))
#<cascaded-coords #X5438430 this_frame  1000.0 2000.0 3000.0 / 0.0 0.0 0.0>
$ (send tr :lookup-transform "child"  "this_frame" (ros::time))
#<cascaded-coords #X54388e0 child  -1000.0 -2000.0 -3000.0 / 0.0 0.0 0.0>

[k-okada]

@Naoki-Hiraoka nice catch!

1) created test code, please check #654

2) this fix changes the behavior of all code using :set-transform, I searched under jsk-ros-pkg/jsk_demos and cound not find any code using this method. @jsk-ros-pkg/jsk_current_gakusei_kyoin

3) to pass #654, we need to multiply 0.001 to pos. how do you think?

diff --git a/roseus/eustf.cpp b/roseus/eustf.cpp
index 9c21c0d1..26f26c1e 100644
--- a/roseus/eustf.cpp
+++ b/roseus/eustf.cpp
@@ -149,8 +149,10 @@ pointer EUSTF_SETTRANSFORM(register context *ctx,int n,pointer *argv)
   std::string child_frame_id = std::string((char*)(argv[5]->c.str.chars));
   std::string authority= std::string((char*)(argv[6]->c.str.chars));
   tf::StampedTransform transform;
-  transform.setOrigin(tf::Vector3(pos[0], pos[1], pos[2]));
-  transform.setRotation(tf::Quaternion(rot[3], rot[0], rot[1], rot[2]));
+  transform.setOrigin(tf::Vector3(pos[0]/1000.0, pos[1]/1000.0, pos[2]/1000.0));
+  // rot (euslisp) -> (w, x, y, z)
+  // tfQuaternion  -> (x, y, z, w)
+  transform.setRotation(tf::Quaternion(rot[1], rot[2], rot[3], rot[0]));
   transform.frame_id_ = frame_id;
   transform.child_frame_id_ = child_frame_id;
   transform.stamp_.sec = stamp[0];

[Naoki-Hiraoka]

You're right! We need to multiply 0.001 to pos.

Since ros::tf-cascaded-coords is child class of cascaded-coords, the unit of ros::tf-cascaded-coords should be [mm]. (not [m])

after

$ (setq tr (instance ros::transformer :init))
#<ros::transformer #X545e290>
$ (send tr :set-transform (instance ros::tf-cascaded-coords :init :pos #f(1000 2000 3000) :frame-id "this_frame" :child-frame-id "child"))
$ (send tr :get-frame-strings)
("this_frame" "child")
$ (send tr :lookup-transform "this_frame" "child" (ros::time))
#<cascaded-coords #X5438430 this_frame  1000.0 2000.0 3000.0 / 0.0 0.0 0.0>
$ (send tr :lookup-transform "child"  "this_frame" (ros::time))
#<cascaded-coords #X54388e0 child  -1000.0 -2000.0 -3000.0 / 0.0 0.0 0.0>