pavel/teleoperation
1
0
Fork 0
Code Issues Pull Requests Releases Wiki Activity

somehow managed to implement rotation cleanly

Browse Source
This commit is contained in:
Pavel Lutskov
2019-01-29 17:31:52 +01:00
parent b6e7ab8aa4
commit b95417293b
4 changed files with 77 additions and 49 deletions
Download Patch File Download Diff File Expand all files Collapse all files

13
include/teleoperation/utils.hpp
View File

@@ -1,14 +1,27 @@
#ifndef _UTILS_HPP_
#define _UTILS_HPP_
#define PI 3.14159265358979323846
#include <sstream>
namespace {
template<typename T> 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

2
script/controller.py
View File

@@ -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':

38
script/walker.py
View File

@@ -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',
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()

71
src/aruco_detector.cpp
View File

@@ -10,6 +10,7 @@
#include <opencv2/highgui/highgui.hpp>
#include <opencv2/video/tracking.hpp>
#include <aruco/aruco.h>
#include <teleoperation/utils.hpp>
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<float>(2);
float y = markers[i].Tvec.at<float>(0);
float z = -markers[i].Tvec.at<float>(1);
// cv::Mat rot_mat;
// cv::Rodrigues(markers[i].Rvec, rot_mat);
// float roll = atan2(
// rot_mat.at<float>(2,1),
// rot_mat.at<float>(2,2));
// float pitch = -atan2(
// rot_mat.at<float>(2,0),
// sqrt(
// pow(rot_mat.at<float>(2,1),2) +
// pow(rot_mat.at<float>(2,2),2)));
// float yaw = atan2(
// rot_mat.at<float>(1,0),
// rot_mat.at<float>(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<float>(0, 0),
rot_cv.at<float>(0, 1),
rot_cv.at<float>(0, 2),
rot_cv.at<float>(1, 0),
rot_cv.at<float>(1, 1),
rot_cv.at<float>(1, 2),
rot_cv.at<float>(2, 0),
rot_cv.at<float>(2, 1),
rot_cv.at<float>(2, 2)
);
tf::Vector3 trans(
markers[i].Tvec.at<float>(0),
markers[i].Tvec.at<float>(1),
markers[i].Tvec.at<float>(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()));