From 171d3c49ff57e537ac18f21096bb41cb805dd9da Mon Sep 17 00:00:00 2001 From: Pavel Lutskov Date: Thu, 17 Jan 2019 17:30:43 +0100 Subject: [PATCH] Interaction of the walker module and main control --- CMakeLists.txt | 16 +++++++++++++ package.xml | 4 ++++ script/controller.py | 50 +++++++++++++++++++++------------------- script/walker.py | 30 ++++++++++++++++-------- src/aruco_detector.cpp | 18 +++++++++++++++ srv/InformController.srv | 6 +++++ 6 files changed, 91 insertions(+), 33 deletions(-) mode change 100644 => 100755 script/controller.py create mode 100644 srv/InformController.srv diff --git a/CMakeLists.txt b/CMakeLists.txt index ac60717..40331b6 100644 --- a/CMakeLists.txt +++ b/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}) diff --git a/package.xml b/package.xml index d562f18..5cd2dc2 100644 --- a/package.xml +++ b/package.xml @@ -43,15 +43,19 @@ cv_bridge image_transport roscpp + rospy sensor_msgs std_msgs aruco + message_generation cv_bridge image_transport roscpp + rospy sensor_msgs std_msgs aruco + message_runtime diff --git a/script/controller.py b/script/controller.py old mode 100644 new mode 100755 index bba4333..6f9325f --- a/script/controller.py +++ b/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 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 module == 'walker': + if message == 'move': + if STATE in ('idle', 'walk'): + STATE = 'walk' + permission = True + else: + permission = False + elif message == 'stop': + if STATE == 'walk': + 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 diff --git a/script/walker.py b/script/walker.py index ff865dd..534e8b4 100755 --- a/script/walker.py +++ b/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 diff --git a/src/aruco_detector.cpp b/src/aruco_detector.cpp index fcba43e..b739080 100644 --- a/src/aruco_detector.cpp +++ b/src/aruco_detector.cpp @@ -81,6 +81,21 @@ class ImageConverter { float x = markers[i].Tvec.at(0); float y = markers[i].Tvec.at(1); float z = markers[i].Tvec.at(2); + + // cv::Mat rot_mat; + // cv::Rodrigues(markers[i].Rvec, rot_mat); + // float roll = atan2( + // rot_mat.at(2,1), + // rot_mat.at(2,2)); + // float pitch = -atan2( + // rot_mat.at(2,0), + // sqrt( + // pow(rot_mat.at(2,1),2) + + // pow(rot_mat.at(2,2),2))); + // float yaw = atan2( + // rot_mat.at(1,0), + // rot_mat.at(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())); diff --git a/srv/InformController.srv b/srv/InformController.srv new file mode 100644 index 0000000..f2b8481 --- /dev/null +++ b/srv/InformController.srv @@ -0,0 +1,6 @@ +#CREATE request parameters +string module +string message +------------------- +#CREATE response parameters +bool permission