somehow managed to implement rotation cleanly

include/teleoperation/utils.hpp

@@ -1,14 +1,27 @@
 #ifndef _UTILS_HPP_
 #define _UTILS_HPP_
 
+#define PI 3.14159265358979323846
+
 #include <sstream>
 
 namespace {
+
     template<typename T> inline std::string stuff_to_str(T i) {
         std::ostringstream ss;
         ss << i;
         return ss.str();
     }
+
+    double radians(double degrees) {
+        return degrees / 180 * PI;
+    }
+
+    double degrees(double radians) {
+        return radians / PI * 180;
+    }
 }
 
+
+
 #endif

script/controller.py

@@ -21,11 +21,11 @@ def inform_controller_factory(who):
 
 
 def handle_request(r):
+    global STATE
     module = r.module
     message = r.message
     _state_old = STATE
     permission = False
-    global STATE
 
     if module == 'walker':
         if message == 'move':

script/walker.py

@@ -10,10 +10,12 @@ from controller import inform_controller_factory
 
 
       #min   #max
-FW =  1.65,  1.45
+FW = -1.65, -1.45
-BK =  2.20,  2.40
+BK = -2.20, -2.40
-LT = -0.35, -0.53
+LT =  0.35,  0.53
-RT =  0.35,  0.53
+RT = -0.35, -0.53
+LR =  0.45,  0.85
+RR = -0.45, -0.85
 
 VMIN = 0.3
 VMAX = 1.0
@@ -39,34 +41,42 @@ if __name__ == '__main__':
     rospy.wait_for_service('inform_controller')
     ll = tf.TransformListener()
     mp = ALProxy('ALMotion', os.environ['NAO_IP'], 9559)
-    mp.wakeUp()
+    # mp.wakeUp()
     mp.moveInit()
     mp.setMoveArmsEnabled(False, False)
 
     while not rospy.is_shutdown():
         sleep(0.3)
         try:
-            trans, rot = ll.lookupTransform('Aruco_0_frame',
+            trans, q = ll.lookupTransform('odom',
-                                            'odom',
+                                          'Aruco_0_frame',
-                                            rospy.Time(0))
+                                          rospy.Time(0))
+            rot = tf.transformations.euler_from_quaternion(q)
+
         except Exception as e:
             mp.stopMove()
             _inform_controller('stop')
             continue
 
         # print trans
+        # print rot
         # continue
 
         movement = [0, 0, 0]
-                               #-1   1  -1   1
+        ref_vec = trans[:2] + rot[2:]
-        for i, dr in enumerate((BK, FW, RT, LT)):
+                               #-1   1  -1   1  -1   1
+        for i, dr in enumerate((BK, FW, RT, LT, RR, LR)):
             idx = i // 2
             sign = 1 if (i % 2) else -1
-            speed = _speed(trans[idx], dr)
+            speed = _speed(ref_vec[idx], dr)
             if speed:
                 movement[idx] = sign * speed
                 break
 
+        rospy.loginfo('WALKER: TRANS: {}'.format(trans))
+        rospy.loginfo('WALKER: ROTTN: {}'.format(rot))
+        rospy.loginfo('WALKER: MOVMT: {}\n'.format(movement))
+
         if not any(movement):
             rospy.logdebug('WALKER: STOP')
             _inform_controller('stop')
@@ -77,10 +87,8 @@ if __name__ == '__main__':
 
         if not permission:
             mp.stopMove()
-            continue
+        else:
-
+            # mp.moveToward(*movement)  # DON'T DELETE THIS ONE
-        rospy.loginfo('WALKER: TRANS: {}'.format(trans))
+            pass
-        rospy.loginfo('WALKER: MOVMT: {}'.format(movement))
-        mp.moveToward(*movement)
 
     mp.rest()

src/aruco_detector.cpp

@@ -10,6 +10,7 @@
 #include <opencv2/highgui/highgui.hpp>
 #include <opencv2/video/tracking.hpp>
 #include <aruco/aruco.h>
+#include <teleoperation/utils.hpp>
 
 static const std::string ARUCO_WINDOW = "Aruco window";
 
@@ -25,9 +26,6 @@ class ImageConverter {
     ImageConverter() :
         it_(nh_)
     {
-        ros::Rate loop_rate(0.1);
-        loop_rate.sleep();
-
         // Subscribe to input video feed
         image_sub_ = it_.subscribe("/usb_cam/image_raw", 1,
                 &ImageConverter::imageCb, this);
@@ -75,39 +73,48 @@ class ImageConverter {
         // Draw and print aruco
         cv::Mat aruco_demo = cv_ptr->image.clone();
         for (int i = 0; i < markers.size(); i++) {
-            markers[i].draw(aruco_demo, cv::Scalar(0, 0, 255), 2);
-            float x = -markers[i].Tvec.at<float>(2);
-            float y = markers[i].Tvec.at<float>(0);
-            float z = -markers[i].Tvec.at<float>(1);
 
-            // cv::Mat rot_mat;
+            markers[i].draw(aruco_demo, cv::Scalar(0, 0, 255), 2);
-            // cv::Rodrigues(markers[i].Rvec, rot_mat);
+
-            // float roll = atan2(
+            cv::Mat rot_cv;
-                    // rot_mat.at<float>(2,1),
+            cv::Rodrigues(markers[i].Rvec, rot_cv);
-                    // rot_mat.at<float>(2,2));
+
-            // float pitch = -atan2(
+            tf::Matrix3x3 rot(
-                    // rot_mat.at<float>(2,0),
+                    rot_cv.at<float>(0, 0),
-                    // sqrt(
+                    rot_cv.at<float>(0, 1),
-                        // pow(rot_mat.at<float>(2,1),2) +
+                    rot_cv.at<float>(0, 2),
-                        // pow(rot_mat.at<float>(2,2),2)));
+                    rot_cv.at<float>(1, 0),
-            // float yaw = atan2(
+                    rot_cv.at<float>(1, 1),
-                    // rot_mat.at<float>(1,0),
+                    rot_cv.at<float>(1, 2),
-                    // rot_mat.at<float>(0,0));
+                    rot_cv.at<float>(2, 0),
+                    rot_cv.at<float>(2, 1),
+                    rot_cv.at<float>(2, 2)
+                    );
+            tf::Vector3 trans(
+                    markers[i].Tvec.at<float>(0),
+                    markers[i].Tvec.at<float>(1),
+                    markers[i].Tvec.at<float>(2)
+                    );
+            tf::Matrix3x3 hmat(
+                    0, 0, -1,
+                    1, 0, 0,
+                    0, -1, 0
+                    );
+            tf::Matrix3x3 another(
+                    0, 0, -1,
+                    1, 0, 0,
+                    0, -1, 0
+                    );
+
+            rot = hmat * rot * another;  // This all somehow works as expected
+            trans = hmat * trans;
+
+            tf::Transform aruco_tf(rot, trans);
 
             std::string id = "Aruco_";
-
+            id += stuff_to_str(markers[i].id);
-            // int to str basically
-            std::ostringstream ss;
-            ss << markers[i].id;
-            id += ss.str();
-
             id += "_frame";
-            tf::Transform aruco_tf;
+
-            aruco_tf.setIdentity();
-            // tf::Quaternion q;
-            // q.setRPY(roll, pitch, yaw);
-            aruco_tf.setOrigin(tf::Vector3(x, y, z));
-            // aruco_tf.setRotation(q);
             this->aruco_pub.sendTransform(tf::StampedTransform(
                         aruco_tf, ros::Time::now(), "odom",
                         id.c_str()));