diff --git a/.gitignore b/.gitignore index 4d61f1b..42b4aab 100644 --- a/.gitignore +++ b/.gitignore @@ -1,2 +1,5 @@ # Vim stuff .*.sw* + +# Some files +experiments.py diff --git a/CMakeLists.txt b/CMakeLists.txt index f7a67ff..ac60717 100644 --- a/CMakeLists.txt +++ b/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) diff --git a/script/controller.py b/script/controller.py new file mode 100644 index 0000000..bba4333 --- /dev/null +++ b/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 diff --git a/script/walker.py b/script/walker.py new file mode 100755 index 0000000..ff865dd --- /dev/null +++ b/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() diff --git a/src/aruco_detector.cpp b/src/aruco_detector.cpp index 7564bed..fcba43e 100644 --- a/src/aruco_detector.cpp +++ b/src/aruco_detector.cpp @@ -1,6 +1,7 @@ #include #include #include +#include #include #include #include @@ -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_(3, 3) << @@ -83,7 +82,12 @@ class ImageConverter { float y = markers[i].Tvec.at(1); float z = markers[i].Tvec.at(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); } };