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

Started implementing main controller

Browse Source 
Also sort of implemented the navigation
Also tidy up the CMakeLists a little
This commit is contained in:
Pavel Lutskov
2019-01-17 16:15:05 +01:00
parent becede0124
commit fe09cce493
5 changed files with 100 additions and 192 deletions
Download Patch File Download Diff File Expand all files Collapse all files

14
src/aruco_detector.cpp
View File

@@ -1,6 +1,7 @@
#include <ros/ros.h>
#include <vector>
#include <iostream>
#include <sstream>
#include <tf/transform_broadcaster.h>
#include <image_transport/image_transport.h>
#include <cv_bridge/cv_bridge.h>
@@ -26,7 +27,7 @@ class ImageConverter {
it_(nh_)
{
// Subscrive to input video feed
image_sub_ = it_.subscribe("/nao_robot/camera/front/image_raw", 1,
image_sub_ = it_.subscribe("/usb_cam/image_raw", 1,
&ImageConverter::imageCb, this);
// Create output windows
@@ -42,7 +43,6 @@ class ImageConverter {
void imageCb(const sensor_msgs::ImageConstPtr& msg) {
cv_bridge::CvImageConstPtr cv_ptr;
ROS_INFO("IN IMAGE CB");
try
{
// Create OpenCV, share memory with original message
@@ -57,7 +57,6 @@ class ImageConverter {
// Output Original Image (bcs y not)
cv::imshow(OPENCV_WINDOW, cv_ptr->image);
ROS_INFO("OUTPUT STUFF");
// Do Aruco
cv::Mat cam_mat = (cv::Mat_<double>(3, 3) <<
@@ -83,7 +82,12 @@ class ImageConverter {
float y = markers[i].Tvec.at<float>(1);
float z = markers[i].Tvec.at<float>(2);
std::string id = "Aruco_";
id += std::to_string(markers[i].id);
// int to str basically
std::ostringstream ss;
ss << markers[i].id;
id += ss.str();
id += "_frame";
tf::Transform aruco_tf;
aruco_tf.setIdentity();
@@ -93,8 +97,6 @@ class ImageConverter {
id.c_str()));
}
cv::imshow(ARUCO_WINDOW, aruco_demo);
cv::waitKey(2);
}
};