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

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;
}