Started implementing main controller

Also sort of implemented the navigation
Also tidy up the CMakeLists a little

.gitignore

@@ -1,2 +1,5 @@
 # Vim stuff
 .*.sw*
+
+# Some files
+experiments.py

CMakeLists.txt

@@ -1,13 +1,6 @@
 cmake_minimum_required(VERSION 2.8.3)
 project(teleoperation)
 
-## Add support for C++11, supported in ROS Kinetic and newer
-add_definitions(-std=c++11)
-# set(aruco_LIB /usr/local/lib/libaruco.so)
-
-## Find catkin macros and libraries
-## if COMPONENTS list like find_package(catkin REQUIRED COMPONENTS xyz)
-## is used, also find other catkin packages
 find_package(catkin REQUIRED COMPONENTS
   cv_bridge
   image_transport
@@ -18,193 +11,14 @@ find_package(catkin REQUIRED COMPONENTS
   aruco
 )
 
-## System dependencies are found with CMake's conventions
-# find_package(Boost REQUIRED COMPONENTS system)
-
-
-## Uncomment this if the package has a setup.py. This macro ensures
-## modules and global scripts declared therein get installed
-## See http://ros.org/doc/api/catkin/html/user_guide/setup_dot_py.html
-# catkin_python_setup()
-
-################################################
-## Declare ROS messages, services and actions ##
-################################################
-
-## To declare and build messages, services or actions from within this
-## package, follow these steps:
-## * Let MSG_DEP_SET be the set of packages whose message types you use in
-##   your messages/services/actions (e.g. std_msgs, actionlib_msgs, ...).
-## * In the file package.xml:
-##   * add a build_depend tag for "message_generation"
-##   * add a build_depend and a run_depend tag for each package in MSG_DEP_SET
-##   * If MSG_DEP_SET isn't empty the following dependency has been pulled in
-##     but can be declared for certainty nonetheless:
-##     * add a run_depend tag for "message_runtime"
-## * In this file (CMakeLists.txt):
-##   * add "message_generation" and every package in MSG_DEP_SET to
-##     find_package(catkin REQUIRED COMPONENTS ...)
-##   * add "message_runtime" and every package in MSG_DEP_SET to
-##     catkin_package(CATKIN_DEPENDS ...)
-##   * uncomment the add_*_files sections below as needed
-##     and list every .msg/.srv/.action file to be processed
-##   * uncomment the generate_messages entry below
-##   * add every package in MSG_DEP_SET to generate_messages(DEPENDENCIES ...)
-
-## Generate messages in the 'msg' folder
-# add_message_files(
-#   FILES
-#   Message1.msg
-#   Message2.msg
-# )
-
-## Generate services in the 'srv' folder
-# add_service_files(
-#   FILES
-#   Service1.srv
-#   Service2.srv
-# )
-
-## Generate actions in the 'action' folder
-# add_action_files(
-#   FILES
-#   Action1.action
-#   Action2.action
-# )
-
-## Generate added messages and services with any dependencies listed here
-# generate_messages(
-#   DEPENDENCIES
-#   sensor_msgs#   std_msgs
-# )
-
-################################################
-## Declare ROS dynamic reconfigure parameters ##
-################################################
-
-## To declare and build dynamic reconfigure parameters within this
-## package, follow these steps:
-## * In the file package.xml:
-##   * add a build_depend and a run_depend tag for "dynamic_reconfigure"
-## * In this file (CMakeLists.txt):
-##   * add "dynamic_reconfigure" to
-##     find_package(catkin REQUIRED COMPONENTS ...)
-##   * uncomment the "generate_dynamic_reconfigure_options" section below
-##     and list every .cfg file to be processed
-
-## Generate dynamic reconfigure parameters in the 'cfg' folder
-# generate_dynamic_reconfigure_options(
-#   cfg/DynReconf1.cfg
-#   cfg/DynReconf2.cfg
-# )
-
-###################################
-## catkin specific configuration ##
-###################################
-## The catkin_package macro generates cmake config files for your package
-## Declare things to be passed to dependent projects
-## INCLUDE_DIRS: uncomment this if you package contains header files
-## LIBRARIES: libraries you create in this project that dependent projects also need
-## CATKIN_DEPENDS: catkin_packages dependent projects also need
-## DEPENDS: system dependencies of this project that dependent projects also need
 catkin_package(
-#  INCLUDE_DIRS include
-#  LIBRARIES project_aruco
-#  CATKIN_DEPENDS cv_bridge image_transport roscpp sensor_msgs std_msgs
-#  DEPENDS system_lib
 )
 
-###########
-## Build ##
-###########
-
-## Specify additional locations of header files
-## Your package locations should be listed before other locations
-# include_directories(include)
 include_directories(
   ${catkin_INCLUDE_DIRS}
 )
 
-## Declare a C++ library
-# add_library(${PROJECT_NAME}
-#   src/${PROJECT_NAME}/project_aruco.cpp
-# )
-
-## Add cmake target dependencies of the library
-## as an example, code may need to be generated before libraries
-## either from message generation or dynamic reconfigure
-# add_dependencies(${PROJECT_NAME} ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS})
-
-## Declare a C++ executable
-## With catkin_make all packages are built within a single CMake context
-## The recommended prefix ensures that target names across packages don't collide
-# add_executable(${PROJECT_NAME}_node src/project_aruco_node.cpp)
-
-## Rename C++ executable without prefix
-## The above recommended prefix causes long target names, the following renames the
-## target back to the shorter version for ease of user use
-## e.g. "rosrun someones_pkg node" instead of "rosrun someones_pkg someones_pkg_node"
-# set_target_properties(${PROJECT_NAME}_node PROPERTIES OUTPUT_NAME node PREFIX "")
-
-## Add cmake target dependencies of the executable
-## same as for the library above
-# add_dependencies(${PROJECT_NAME}_node ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS})
-
-## Specify libraries to link a library or executable target against
-# target_link_libraries(${PROJECT_NAME}_node
-#   ${catkin_LIBRARIES}
-# )
-
 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})
 target_link_libraries(aruco_effector ${catkin_LIBRARIES} ${aruco_LIB})
-# add_dependencies(aruco tutorial_4_generate_messages_cpp)
-
-#############
-## Install ##
-#############
-
-# all install targets should use catkin DESTINATION variables
-# See http://ros.org/doc/api/catkin/html/adv_user_guide/variables.html
-
-## Mark executable scripts (Python etc.) for installation
-## in contrast to setup.py, you can choose the destination
-# install(PROGRAMS
-#   scripts/my_python_script
-#   DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION}
-# )
-
-## Mark executables and/or libraries for installation
-# install(TARGETS ${PROJECT_NAME} ${PROJECT_NAME}_node
-#   ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION}
-#   LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION}
-#   RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION}
-# )
-
-## Mark cpp header files for installation
-# install(DIRECTORY include/${PROJECT_NAME}/
-#   DESTINATION ${CATKIN_PACKAGE_INCLUDE_DESTINATION}
-#   FILES_MATCHING PATTERN "*.h"
-#   PATTERN ".svn" EXCLUDE
-# )
-
-## Mark other files for installation (e.g. launch and bag files, etc.)
-# install(FILES
-#   # myfile1
-#   # myfile2
-#   DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}
-# )
-
-#############
-## Testing ##
-#############
-
-## Add gtest based cpp test target and link libraries
-# catkin_add_gtest(${PROJECT_NAME}-test test/test_project_aruco.cpp)
-# if(TARGET ${PROJECT_NAME}-test)
-#   target_link_libraries(${PROJECT_NAME}-test ${PROJECT_NAME})
-# endif()
-
-## Add folders to be run by python nosetests
-# catkin_add_nosetests(test)

script/controller.py

@@ -0,0 +1,34 @@
+from time import sleep
+
+
+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):
+    global STATE
+    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
+
+
+if __name__ == '__main__':
+    pass
+    # initialize stuff

script/walker.py

@@ -0,0 +1,55 @@
+#! /usr/bin/env python
+import os
+from time import sleep
+
+from naoqi import ALProxy
+
+import rospy
+import tf
+
+FW = -0.30
+BK = -0.55
+LT = -0.55
+RT = 0.0
+
+
+if __name__ == '__main__':
+    rospy.init_node('walker')
+    ll = tf.TransformListener()
+    mp = ALProxy('ALMotion', os.environ['NAO_IP'], 9559)
+    mp.wakeUp()
+    mp.moveInit()
+
+    while not rospy.is_shutdown():
+        sleep(0.3)
+        try:
+            trans, rot = ll.lookupTransform('Aruco_0_frame',
+                                            'CameraTop_optical_frame',
+                                            rospy.Time(0))
+        except Exception as e:
+            mp.stopMove()
+            # inform_controller('walker', 'stop')
+            continue
+        print trans
+        # continue
+
+        if BK < trans[2] < FW and LT < trans[0] < RT:
+            mp.move(0, 0, 0)
+            # inform_controller('walker', 'stop')
+            continue
+
+        permission = True
+        # permission = inform_controller('walker', 'move')
+        if not permission:
+            mp.stopMove()
+            continue
+        if trans[2] < BK:  # backwards
+            mp.move(-1, 0, 0)
+        elif FW < trans[2]:  # forwards
+            mp.move(1, 0, 0)
+        elif RT < trans[0]:  # right
+            mp.move(0, -1, 0)
+        elif trans[0] < LT:  # left
+            mp.move(0, 1, 0)
+
+    mp.rest()

src/aruco_detector.cpp

@@ -1,6 +1,7 @@
 #include <ros/ros.h>
 #include <vector>
 #include <iostream>
+#include <sstream>
 #include <tf/transform_broadcaster.h>
 #include <image_transport/image_transport.h>
 #include <cv_bridge/cv_bridge.h>
@@ -26,7 +27,7 @@ class ImageConverter {
         it_(nh_)
     {
         // Subscrive to input video feed
-        image_sub_ = it_.subscribe("/nao_robot/camera/front/image_raw", 1,
+        image_sub_ = it_.subscribe("/usb_cam/image_raw", 1,
                 &ImageConverter::imageCb, this);
 
         // Create output windows
@@ -42,7 +43,6 @@ class ImageConverter {
 
     void imageCb(const sensor_msgs::ImageConstPtr& msg) {
         cv_bridge::CvImageConstPtr cv_ptr;
-        ROS_INFO("IN IMAGE CB");
         try
         {
             // Create OpenCV, share memory with original message
@@ -57,7 +57,6 @@ class ImageConverter {
 
         // Output Original Image (bcs y not)
         cv::imshow(OPENCV_WINDOW, cv_ptr->image);
-        ROS_INFO("OUTPUT STUFF");
 
         // Do Aruco
         cv::Mat cam_mat = (cv::Mat_<double>(3, 3) <<
@@ -83,7 +82,12 @@ class ImageConverter {
             float y = markers[i].Tvec.at<float>(1);
             float z = markers[i].Tvec.at<float>(2);
             std::string id = "Aruco_";
-            id += std::to_string(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();
@@ -93,8 +97,6 @@ class ImageConverter {
                         id.c_str()));
         }
         cv::imshow(ARUCO_WINDOW, aruco_demo);
-
-
         cv::waitKey(2);
     }
 };