started with imitation

CMakeLists.txt

@@ -31,12 +31,11 @@ include_directories(
 catkin_install_python(PROGRAMS
     ./script/controller.py
     ./script/walker.py
-    ./script/fall_manager.py
+    ./script/fall_detector.py
+    ./script/imitator.py
     DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION}
 )
 
 add_executable(aruco_detector src/aruco_detector.cpp)
-add_executable(aruco_effector src/aruco_effector.cpp)
 add_executable(speech src/speech.cpp)
 target_link_libraries(aruco_detector ${catkin_LIBRARIES} ${aruco_LIB})
-target_link_libraries(aruco_effector ${catkin_LIBRARIES} ${aruco_LIB})

script/controller.py

@@ -44,6 +44,18 @@ def handle_request(r):
         elif message == 'recovered':
             STATE = 'idle'
 
+    elif module == 'imitator':
+        if message == 'imitate':
+            if STATE in ('imitate', 'idle'):
+                STATE = 'imitate'
+                permission = True
+            else:
+                permission = False
+        elif message == 'stop':
+            if STATE == 'imitate':
+                STATE = 'idle'
+            permission = True
+
     print 'Got request from %s to %s. Permission: %s. State is now: %s.' % (
         module, message, permission, STATE
     )

script/imitator.py

@@ -0,0 +1,56 @@
+#! /usr/bin/env python
+import os
+from time import sleep
+from math import atan, degrees, radians
+
+import rospy
+import tf
+import numpy as np
+from naoqi import ALProxy
+
+from controller import inform_controller_factory
+
+
+_inform_controller = inform_controller_factory('imitator')
+_FRAME_TORSO = 0
+
+
+if __name__ == '__main__':
+    rospy.init_node('imitator')
+    rospy.wait_for_service('inform_controller')
+    ll = tf.TransformListener()
+    mp = ALProxy('ALMotion', os.environ['NAO_IP'], 9559)
+    mp.wakeUp()
+    mp.setAngles(['LElbowRoll', 'RElbowRoll'], [0, 0], 0.5)
+
+    while not rospy.is_shutdown():
+        sleep(0.1)
+        for i, eff in enumerate(['L',
+                                 'R'], 1):
+            try:
+                trans, _ = ll.lookupTransform(
+                    'Aruco_0_frame',
+                    'Aruco_{}_frame'.format(i),
+                    rospy.Time(0)
+                )
+            except Exception as e:
+                print e
+                continue
+            permission = _inform_controller('imitate')
+            if not permission:
+                continue
+            roll = atan(trans[1] / abs(trans[0]))
+            pitch = atan(-trans[2] / abs(trans[0]))
+
+            # sign = 1 if roll > 0 else -1
+            # roll -= sign * radians(10)
+            print degrees(roll)
+            mp.setAngles(['{}ShoulderRoll'.format(eff),
+                          '{}ShoulderPitch'.format(eff)], [roll, pitch], 0.3)
+            # targ = np.array(trans)
+            # targ = targ / np.linalg.norm(targ) * 0.3
+            # mp.setPositions(eff, _FRAME_TORSO,
+                            # targ.tolist() + [0, 0, 0], 0.5, 7)
+            # print eff, targ
+
+    mp.rest()

script/walker.py

@@ -33,7 +33,7 @@ if __name__ == '__main__':
                                             rospy.Time(0))
         except Exception as e:
             mp.stopMove()
-            print _inform_controller('stop')
+            _inform_controller('stop')
             continue
 
         print trans, rot
@@ -44,8 +44,8 @@ if __name__ == '__main__':
             LT < trans[0] < RT
             # CW < trans[1] < CC
         ):
-            mp.move(0, 0, 0)
             _inform_controller('stop')
+            mp.move(0, 0, 0)
             continue
 
         permission = _inform_controller('move')

src/aruco_detector.cpp

@@ -11,7 +11,7 @@
 #include <opencv2/video/tracking.hpp>
 #include <aruco/aruco.h>
 
-static const std::string OPENCV_WINDOW = "Original image";
+// static const std::string OPENCV_WINDOW = "Original image";
 static const std::string ARUCO_WINDOW = "Aruco window";
 
 class ImageConverter {
@@ -31,13 +31,13 @@ class ImageConverter {
                 &ImageConverter::imageCb, this);
 
         // Create output windows
-        cv::namedWindow(OPENCV_WINDOW);
+        // cv::namedWindow(OPENCV_WINDOW);
         cv::namedWindow(ARUCO_WINDOW);
     }
 
     ~ImageConverter() {
         // Clean Up
-        cv::destroyWindow(OPENCV_WINDOW);
+        // cv::destroyWindow(OPENCV_WINDOW);
         cv::destroyWindow(ARUCO_WINDOW);
     }
 
@@ -56,18 +56,18 @@ class ImageConverter {
         }
 
         // Output Original Image (bcs y not)
-        cv::imshow(OPENCV_WINDOW, cv_ptr->image);
+        // cv::imshow(OPENCV_WINDOW, cv_ptr->image);
 
         // Do Aruco
         cv::Mat cam_mat = (cv::Mat_<double>(3, 3) <<
-                274.139508945831, 0.0, 141.184472810944,
+                690., 0.0, 320,
-                0.0, 275.741846757374, 106.693773654172,
+                0.0, 690., 225,
                 0.0, 0.0, 1.0);
         cv::Mat distortion = (cv::Mat_<double>(4, 1) <<
-                -0.0870160932911717,
+                -0.05,
-                0.128210165050533,
+                 0,
-                0.003379500659424,
+                 0,
-                -0.00106205540818586);
+                -0.0015);
         aruco::CameraParameters cam(cam_mat, distortion, cv_ptr->image.size());
         aruco::MarkerDetector detector;
         std::vector<aruco::Marker> markers;
@@ -78,9 +78,9 @@ class ImageConverter {
         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>(0);
+            float x = -markers[i].Tvec.at<float>(2);
-            float y = markers[i].Tvec.at<float>(1);
+            float y = markers[i].Tvec.at<float>(0);
-            float z = markers[i].Tvec.at<float>(2);
+            float z = -markers[i].Tvec.at<float>(1);
 
             // cv::Mat rot_mat;
             // cv::Rodrigues(markers[i].Rvec, rot_mat);
@@ -111,7 +111,7 @@ class ImageConverter {
             aruco_tf.setOrigin(tf::Vector3(x, y, z));
             // aruco_tf.setRotation(q);
             this->aruco_pub.sendTransform(tf::StampedTransform(
-                        aruco_tf, ros::Time::now(), "CameraTop_optical_frame",
+                        aruco_tf, ros::Time::now(), "odom",
                         id.c_str()));
         }
         cv::imshow(ARUCO_WINDOW, aruco_demo);

src/aruco_effector.cpp

@@ -1,47 +0,0 @@
-#include <ros/ros.h>
-#include <vector>
-#include <iostream>
-#include <tf/transform_broadcaster.h>
-#include <tf/transform_listener.h>
-#include <image_transport/image_transport.h>
-#include <cv_bridge/cv_bridge.h>
-#include <sensor_msgs/image_encodings.h>
-#include <opencv2/imgproc/imgproc.hpp>
-#include <opencv2/highgui/highgui.hpp>
-#include <opencv2/video/tracking.hpp>
-#include <aruco/aruco.h>
-
-int main(int argc, char** argv) {
-    ros::init(argc, argv, "aruco_effector");
-    ros::NodeHandle node;
-
-    tf::TransformListener tflisten;
-    ros::Rate rate(1.0);
-    while (node.ok()) {
-        tf::StampedTransform l_transform, r_transform;
-        try {
-            tflisten.lookupTransform("Aruco_1_frame", "Aruco_0_frame",
-                    ros::Time(0), l_transform);
-            ROS_INFO("LEFT FRAME");
-            std::cout << l_transform.getOrigin().x() << std::endl;
-            std::cout << l_transform.getOrigin().y() << std::endl;
-            std::cout << l_transform.getOrigin().z() << std::endl;
-        }
-        catch (tf::TransformException &ex) {
-            // ROS_ERROR("%s", ex.what());
-        }
-        try {
-            tflisten.lookupTransform("Aruco_2_frame", "Aruco_0_frame",
-                    ros::Time(0), r_transform);
-            ROS_INFO("RIGHT FRAME");
-            std::cout << r_transform.getOrigin().x() << std::endl;
-            std::cout << r_transform.getOrigin().y() << std::endl;
-            std::cout << r_transform.getOrigin().z() << std::endl;
-        }
-        catch (tf::TransformException &ex) {
-            // ROS_ERROR("%s", ex.what());
-        }
-        rate.sleep();
-    }
-    return 0;
-}