Interaction of the walker module and main control

CMakeLists.txt

@@ -5,19 +5,35 @@ find_package(catkin REQUIRED COMPONENTS
   cv_bridge
   image_transport
   roscpp
+  rospy
   sensor_msgs
   std_msgs
   tf
   aruco
+  message_generation
 )
 
+add_service_files(
+    DIRECTORY srv
+    FILES
+    InformController.srv
+)
+
+generate_messages(DEPENDENCIES std_msgs)
 catkin_package(
+    CATKIN_DEPENDS message_runtime
 )
 
 include_directories(
   ${catkin_INCLUDE_DIRS}
 )
 
+catkin_install_python(PROGRAMS
+    ./script/controller.py
+    ./script/walker.py
+    DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION}
+)
+
 add_executable(aruco_detector src/aruco_detector.cpp)
 add_executable(aruco_effector src/aruco_effector.cpp)
 target_link_libraries(aruco_detector ${catkin_LIBRARIES} ${aruco_LIB})

package.xml

@@ -43,15 +43,19 @@
   <build_depend>cv_bridge</build_depend>
   <build_depend>image_transport</build_depend>
   <build_depend>roscpp</build_depend>
+  <build_depend>rospy</build_depend>
   <build_depend>sensor_msgs</build_depend>
   <build_depend>std_msgs</build_depend>
   <build_depend>aruco</build_depend>
+  <build_depend>message_generation</build_depend>
   <run_depend>cv_bridge</run_depend>
   <run_depend>image_transport</run_depend>
   <run_depend>roscpp</run_depend>
+  <run_depend>rospy</run_depend>
   <run_depend>sensor_msgs</run_depend>
   <run_depend>std_msgs</run_depend>
   <run_depend>aruco</run_depend>
+  <run_depend>message_runtime</run_depend>
 
 
   <!-- The export tag contains other, unspecified, tags -->

script/controller.py

@@ -1,34 +1,36 @@
-from time import sleep
+#! /usr/bin/env python
+import rospy
+
+from teleoperation.srv import InformController, InformControllerResponse
 
 
-handlers = {}
 STATE = 'idle'  # Also walk, imitate and fallen
 
 
-def fall_handler():
-    pass
-
-
-def walk_handler():
-    pass
-
-
-def imitation_handler():
-    pass
-
-
-def handle_transition(new_state):
+def handle_request(r):
+    module = r.module
+    message = r.message
     global STATE
+
+    if module == 'walker':
+        if message == 'move':
+            if STATE in ('idle', 'walk'):
+                STATE = 'walk'
+                permission = True
+            else:
+                permission = False
+        elif message == 'stop':
             if STATE == 'walk':
-        if new_state in ('fallen', 'idle'):
-            STATE = new_state
-    elif STATE == 'fallen':
-        if new_state == 'idle':
-            STATE = new_state
-    elif STATE == 'imitate':
-        STATE = new_state
+                STATE = 'idle'
+            permission = True
+    print 'Got request from %s to %s. Permission: %s. State is now: %s.' % (
+        module, message, permission, STATE
+    )
+    return InformControllerResponse(permission)
 
 
 if __name__ == '__main__':
-    pass
+    rospy.init_node('controller')
+    ic = rospy.Service('inform_controller', InformController, handle_request)
+    rospy.spin()
     # initialize stuff

script/walker.py

@@ -2,19 +2,27 @@
 import os
 from time import sleep
 
-from naoqi import ALProxy
-
 import rospy
 import tf
+from naoqi import ALProxy
 
-FW = -0.30
+from teleoperation.srv import InformController
+
+FW = -0.32
 BK = -0.55
 LT = -0.55
 RT = 0.0
 
 
+def _inform_controller(what):
+    inform_controller = rospy.ServiceProxy('inform_controller',
+                                           InformController)
+    return inform_controller('walker', what).permission
+
+
 if __name__ == '__main__':
     rospy.init_node('walker')
+    rospy.wait_for_service('inform_controller')
     ll = tf.TransformListener()
     mp = ALProxy('ALMotion', os.environ['NAO_IP'], 9559)
     mp.wakeUp()
@@ -28,18 +36,22 @@ if __name__ == '__main__':
                                             rospy.Time(0))
         except Exception as e:
             mp.stopMove()
-            # inform_controller('walker', 'stop')
+            _inform_controller('stop')
             continue
-        print trans
+
+        print trans, rot
         # continue
 
-        if BK < trans[2] < FW and LT < trans[0] < RT:
+        if (
+            BK < trans[2] < FW and
+            LT < trans[0] < RT
+            # CW < trans[1] < CC
+        ):
             mp.move(0, 0, 0)
-            # inform_controller('walker', 'stop')
+            _inform_controller('stop')
             continue
 
-        permission = True
-        # permission = inform_controller('walker', 'move')
+        permission = _inform_controller('move')
         if not permission:
             mp.stopMove()
             continue

src/aruco_detector.cpp

@@ -81,6 +81,21 @@ class ImageConverter {
             float x = markers[i].Tvec.at<float>(0);
             float y = markers[i].Tvec.at<float>(1);
             float z = markers[i].Tvec.at<float>(2);
+
+            // cv::Mat rot_mat;
+            // cv::Rodrigues(markers[i].Rvec, rot_mat);
+            // float roll = atan2(
+                    // rot_mat.at<float>(2,1),
+                    // rot_mat.at<float>(2,2));
+            // float pitch = -atan2(
+                    // rot_mat.at<float>(2,0),
+                    // sqrt(
+                        // pow(rot_mat.at<float>(2,1),2) +
+                        // pow(rot_mat.at<float>(2,2),2)));
+            // float yaw = atan2(
+                    // rot_mat.at<float>(1,0),
+                    // rot_mat.at<float>(0,0));
+
             std::string id = "Aruco_";
 
             // int to str basically
@@ -91,7 +106,10 @@ class ImageConverter {
             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(), "CameraTop_optical_frame",
                         id.c_str()));

srv/InformController.srv

@@ -0,0 +1,6 @@
+#CREATE request parameters
+string module
+string message
+-------------------
+#CREATE response parameters
+bool permission