diff --git a/CMakeLists.txt b/CMakeLists.txt index ff06705..c432e84 100644 --- a/CMakeLists.txt +++ b/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}) diff --git a/script/controller.py b/script/controller.py index 3eb3696..7f42c72 100755 --- a/script/controller.py +++ b/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 ) diff --git a/script/imitator.py b/script/imitator.py new file mode 100755 index 0000000..4dc46f4 --- /dev/null +++ b/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() diff --git a/script/walker.py b/script/walker.py index 89382f8..c2fd1b7 100755 --- a/script/walker.py +++ b/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') diff --git a/src/aruco_detector.cpp b/src/aruco_detector.cpp index b739080..3743413 100644 --- a/src/aruco_detector.cpp +++ b/src/aruco_detector.cpp @@ -11,7 +11,7 @@ #include #include -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_(3, 3) << - 274.139508945831, 0.0, 141.184472810944, - 0.0, 275.741846757374, 106.693773654172, + 690., 0.0, 320, + 0.0, 690., 225, 0.0, 0.0, 1.0); cv::Mat distortion = (cv::Mat_(4, 1) << - -0.0870160932911717, - 0.128210165050533, - 0.003379500659424, - -0.00106205540818586); + -0.05, + 0, + 0, + -0.0015); aruco::CameraParameters cam(cam_mat, distortion, cv_ptr->image.size()); aruco::MarkerDetector detector; std::vector 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(0); - float y = markers[i].Tvec.at(1); - float z = markers[i].Tvec.at(2); + float x = -markers[i].Tvec.at(2); + float y = markers[i].Tvec.at(0); + float z = -markers[i].Tvec.at(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); diff --git a/src/aruco_effector.cpp b/src/aruco_effector.cpp deleted file mode 100644 index 7b2daf2..0000000 --- a/src/aruco_effector.cpp +++ /dev/null @@ -1,47 +0,0 @@ -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include - -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; -}