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

Interaction of the walker module and main control

Browse Source
This commit is contained in:
Pavel Lutskov
2019-01-17 17:30:43 +01:00
parent fe09cce493
commit 171d3c49ff
6 changed files with 91 additions and 33 deletions
Download Patch File Download Diff File Expand all files Collapse all files

18
src/aruco_detector.cpp
View File

@@ -81,6 +81,21 @@ class ImageConverter {
float x = markers[i].Tvec.at<float>(0);
float y = markers[i].Tvec.at<float>(1);
float z = markers[i].Tvec.at<float>(2);
// 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));
std::string id = "Aruco_";
// int to str basically
@@ -91,7 +106,10 @@ class ImageConverter {
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(), "CameraTop_optical_frame",
id.c_str()));