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

started with imitation

Browse Source
This commit is contained in:
Pavel Lutskov
2019-01-24 17:48:16 +01:00
parent 99bfaa924c
commit fd36b56631
6 changed files with 86 additions and 66 deletions
Download Patch File Download Diff File Expand all files Collapse all files

5
CMakeLists.txt
View File

@@ -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})

12
script/controller.py
View File

@@ -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
)

56
script/imitator.py Executable file
View File

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

4
script/walker.py
View File

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

28
src/aruco_detector.cpp
View File

@@ -11,7 +11,7 @@
#include <opencv2/video/tracking.hpp>
#include <aruco/aruco.h>
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_<double>(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_<double>(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<aruco::Marker> 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<float>(0);
float y = markers[i].Tvec.at<float>(1);
float z = markers[i].Tvec.at<float>(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);
@@ -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);

47
src/aruco_effector.cpp
View File

@@ -1,47 +0,0 @@
#include <ros/ros.h>
#include <vector>
#include <iostream>
#include <tf/transform_broadcaster.h>
#include <tf/transform_listener.h>
#include <image_transport/image_transport.h>
#include <cv_bridge/cv_bridge.h>
#include <sensor_msgs/image_encodings.h>
#include <opencv2/imgproc/imgproc.hpp>
#include <opencv2/highgui/highgui.hpp>
#include <opencv2/video/tracking.hpp>
#include <aruco/aruco.h>
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;
}