From b95417293bca7121c567de4ae74c80fde215ec33 Mon Sep 17 00:00:00 2001 From: Pavel Lutskov Date: Tue, 29 Jan 2019 17:31:52 +0100 Subject: [PATCH] somehow managed to implement rotation cleanly --- include/teleoperation/utils.hpp | 13 ++++++ script/controller.py | 2 +- script/walker.py | 40 +++++++++++-------- src/aruco_detector.cpp | 71 ++++++++++++++++++--------------- 4 files changed, 77 insertions(+), 49 deletions(-) diff --git a/include/teleoperation/utils.hpp b/include/teleoperation/utils.hpp index 8459af5..54fe7b3 100644 --- a/include/teleoperation/utils.hpp +++ b/include/teleoperation/utils.hpp @@ -1,14 +1,27 @@ #ifndef _UTILS_HPP_ #define _UTILS_HPP_ +#define PI 3.14159265358979323846 + #include namespace { + template inline std::string stuff_to_str(T i) { std::ostringstream ss; ss << i; return ss.str(); } + + double radians(double degrees) { + return degrees / 180 * PI; + } + + double degrees(double radians) { + return radians / PI * 180; + } } + + #endif diff --git a/script/controller.py b/script/controller.py index b202fac..c07074c 100755 --- a/script/controller.py +++ b/script/controller.py @@ -21,11 +21,11 @@ def inform_controller_factory(who): def handle_request(r): + global STATE module = r.module message = r.message _state_old = STATE permission = False - global STATE if module == 'walker': if message == 'move': diff --git a/script/walker.py b/script/walker.py index e986477..063e2a7 100755 --- a/script/walker.py +++ b/script/walker.py @@ -10,10 +10,12 @@ from controller import inform_controller_factory #min #max -FW = 1.65, 1.45 -BK = 2.20, 2.40 -LT = -0.35, -0.53 -RT = 0.35, 0.53 +FW = -1.65, -1.45 +BK = -2.20, -2.40 +LT = 0.35, 0.53 +RT = -0.35, -0.53 +LR = 0.45, 0.85 +RR = -0.45, -0.85 VMIN = 0.3 VMAX = 1.0 @@ -39,34 +41,42 @@ if __name__ == '__main__': rospy.wait_for_service('inform_controller') ll = tf.TransformListener() mp = ALProxy('ALMotion', os.environ['NAO_IP'], 9559) - mp.wakeUp() + # mp.wakeUp() mp.moveInit() mp.setMoveArmsEnabled(False, False) while not rospy.is_shutdown(): sleep(0.3) try: - trans, rot = ll.lookupTransform('Aruco_0_frame', - 'odom', - rospy.Time(0)) + trans, q = ll.lookupTransform('odom', + 'Aruco_0_frame', + rospy.Time(0)) + rot = tf.transformations.euler_from_quaternion(q) + except Exception as e: mp.stopMove() _inform_controller('stop') continue # print trans + # print rot # continue movement = [0, 0, 0] - #-1 1 -1 1 - for i, dr in enumerate((BK, FW, RT, LT)): + ref_vec = trans[:2] + rot[2:] + #-1 1 -1 1 -1 1 + for i, dr in enumerate((BK, FW, RT, LT, RR, LR)): idx = i // 2 sign = 1 if (i % 2) else -1 - speed = _speed(trans[idx], dr) + speed = _speed(ref_vec[idx], dr) if speed: movement[idx] = sign * speed break + rospy.loginfo('WALKER: TRANS: {}'.format(trans)) + rospy.loginfo('WALKER: ROTTN: {}'.format(rot)) + rospy.loginfo('WALKER: MOVMT: {}\n'.format(movement)) + if not any(movement): rospy.logdebug('WALKER: STOP') _inform_controller('stop') @@ -77,10 +87,8 @@ if __name__ == '__main__': if not permission: mp.stopMove() - continue - - rospy.loginfo('WALKER: TRANS: {}'.format(trans)) - rospy.loginfo('WALKER: MOVMT: {}'.format(movement)) - mp.moveToward(*movement) + else: + # mp.moveToward(*movement) # DON'T DELETE THIS ONE + pass mp.rest() diff --git a/src/aruco_detector.cpp b/src/aruco_detector.cpp index 391c6e5..e996d54 100644 --- a/src/aruco_detector.cpp +++ b/src/aruco_detector.cpp @@ -10,6 +10,7 @@ #include #include #include +#include static const std::string ARUCO_WINDOW = "Aruco window"; @@ -25,9 +26,6 @@ class ImageConverter { ImageConverter() : it_(nh_) { - ros::Rate loop_rate(0.1); - loop_rate.sleep(); - // Subscribe to input video feed image_sub_ = it_.subscribe("/usb_cam/image_raw", 1, &ImageConverter::imageCb, this); @@ -75,39 +73,48 @@ class ImageConverter { // Draw and print aruco 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(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); - // 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)); + markers[i].draw(aruco_demo, cv::Scalar(0, 0, 255), 2); + + cv::Mat rot_cv; + cv::Rodrigues(markers[i].Rvec, rot_cv); + + tf::Matrix3x3 rot( + rot_cv.at(0, 0), + rot_cv.at(0, 1), + rot_cv.at(0, 2), + rot_cv.at(1, 0), + rot_cv.at(1, 1), + rot_cv.at(1, 2), + rot_cv.at(2, 0), + rot_cv.at(2, 1), + rot_cv.at(2, 2) + ); + tf::Vector3 trans( + markers[i].Tvec.at(0), + markers[i].Tvec.at(1), + markers[i].Tvec.at(2) + ); + tf::Matrix3x3 hmat( + 0, 0, -1, + 1, 0, 0, + 0, -1, 0 + ); + tf::Matrix3x3 another( + 0, 0, -1, + 1, 0, 0, + 0, -1, 0 + ); + + rot = hmat * rot * another; // This all somehow works as expected + trans = hmat * trans; + + tf::Transform aruco_tf(rot, trans); std::string id = "Aruco_"; - - // int to str basically - std::ostringstream ss; - ss << markers[i].id; - id += ss.str(); - + id += stuff_to_str(markers[i].id); 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(), "odom", id.c_str()));