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

initial commit

Browse Source
This commit is contained in:
Pavel Lutskov
2019-01-17 11:30:41 +01:00
commit becede0124
5 changed files with 428 additions and 0 deletions
Download Patch File Download Diff File Expand all files Collapse all files

2
.gitignore vendored Normal file
View File

@@ -0,0 +1,2 @@
# Vim stuff
.*.sw*

210
CMakeLists.txt Normal file
View File

@@ -0,0 +1,210 @@
cmake_minimum_required(VERSION 2.8.3)
project(teleoperation)
## Add support for C++11, supported in ROS Kinetic and newer
add_definitions(-std=c++11)
# set(aruco_LIB /usr/local/lib/libaruco.so)
## Find catkin macros and libraries
## if COMPONENTS list like find_package(catkin REQUIRED COMPONENTS xyz)
## is used, also find other catkin packages
find_package(catkin REQUIRED COMPONENTS
cv_bridge
image_transport
roscpp
sensor_msgs
std_msgs
tf
aruco
)
## System dependencies are found with CMake's conventions
# find_package(Boost REQUIRED COMPONENTS system)
## Uncomment this if the package has a setup.py. This macro ensures
## modules and global scripts declared therein get installed
## See http://ros.org/doc/api/catkin/html/user_guide/setup_dot_py.html
# catkin_python_setup()
################################################
## Declare ROS messages, services and actions ##
################################################
## To declare and build messages, services or actions from within this
## package, follow these steps:
## * Let MSG_DEP_SET be the set of packages whose message types you use in
## your messages/services/actions (e.g. std_msgs, actionlib_msgs, ...).
## * In the file package.xml:
## * add a build_depend tag for "message_generation"
## * add a build_depend and a run_depend tag for each package in MSG_DEP_SET
## * If MSG_DEP_SET isn't empty the following dependency has been pulled in
## but can be declared for certainty nonetheless:
## * add a run_depend tag for "message_runtime"
## * In this file (CMakeLists.txt):
## * add "message_generation" and every package in MSG_DEP_SET to
## find_package(catkin REQUIRED COMPONENTS ...)
## * add "message_runtime" and every package in MSG_DEP_SET to
## catkin_package(CATKIN_DEPENDS ...)
## * uncomment the add_*_files sections below as needed
## and list every .msg/.srv/.action file to be processed
## * uncomment the generate_messages entry below
## * add every package in MSG_DEP_SET to generate_messages(DEPENDENCIES ...)
## Generate messages in the 'msg' folder
# add_message_files(
# FILES
# Message1.msg
# Message2.msg
# )
## Generate services in the 'srv' folder
# add_service_files(
# FILES
# Service1.srv
# Service2.srv
# )
## Generate actions in the 'action' folder
# add_action_files(
# FILES
# Action1.action
# Action2.action
# )
## Generate added messages and services with any dependencies listed here
# generate_messages(
# DEPENDENCIES
# sensor_msgs# std_msgs
# )
################################################
## Declare ROS dynamic reconfigure parameters ##
################################################
## To declare and build dynamic reconfigure parameters within this
## package, follow these steps:
## * In the file package.xml:
## * add a build_depend and a run_depend tag for "dynamic_reconfigure"
## * In this file (CMakeLists.txt):
## * add "dynamic_reconfigure" to
## find_package(catkin REQUIRED COMPONENTS ...)
## * uncomment the "generate_dynamic_reconfigure_options" section below
## and list every .cfg file to be processed
## Generate dynamic reconfigure parameters in the 'cfg' folder
# generate_dynamic_reconfigure_options(
# cfg/DynReconf1.cfg
# cfg/DynReconf2.cfg
# )
###################################
## catkin specific configuration ##
###################################
## The catkin_package macro generates cmake config files for your package
## Declare things to be passed to dependent projects
## INCLUDE_DIRS: uncomment this if you package contains header files
## LIBRARIES: libraries you create in this project that dependent projects also need
## CATKIN_DEPENDS: catkin_packages dependent projects also need
## DEPENDS: system dependencies of this project that dependent projects also need
catkin_package(
# INCLUDE_DIRS include
# LIBRARIES project_aruco
# CATKIN_DEPENDS cv_bridge image_transport roscpp sensor_msgs std_msgs
# DEPENDS system_lib
)
###########
## Build ##
###########
## Specify additional locations of header files
## Your package locations should be listed before other locations
# include_directories(include)
include_directories(
${catkin_INCLUDE_DIRS}
)
## Declare a C++ library
# add_library(${PROJECT_NAME}
# src/${PROJECT_NAME}/project_aruco.cpp
# )
## Add cmake target dependencies of the library
## as an example, code may need to be generated before libraries
## either from message generation or dynamic reconfigure
# add_dependencies(${PROJECT_NAME} ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS})
## Declare a C++ executable
## With catkin_make all packages are built within a single CMake context
## The recommended prefix ensures that target names across packages don't collide
# add_executable(${PROJECT_NAME}_node src/project_aruco_node.cpp)
## Rename C++ executable without prefix
## The above recommended prefix causes long target names, the following renames the
## target back to the shorter version for ease of user use
## e.g. "rosrun someones_pkg node" instead of "rosrun someones_pkg someones_pkg_node"
# set_target_properties(${PROJECT_NAME}_node PROPERTIES OUTPUT_NAME node PREFIX "")
## Add cmake target dependencies of the executable
## same as for the library above
# add_dependencies(${PROJECT_NAME}_node ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS})
## Specify libraries to link a library or executable target against
# target_link_libraries(${PROJECT_NAME}_node
# ${catkin_LIBRARIES}
# )
add_executable(aruco_detector src/aruco_detector.cpp)
add_executable(aruco_effector src/aruco_effector.cpp)
target_link_libraries(aruco_detector ${catkin_LIBRARIES} ${aruco_LIB})
target_link_libraries(aruco_effector ${catkin_LIBRARIES} ${aruco_LIB})
# add_dependencies(aruco tutorial_4_generate_messages_cpp)
#############
## Install ##
#############
# all install targets should use catkin DESTINATION variables
# See http://ros.org/doc/api/catkin/html/adv_user_guide/variables.html
## Mark executable scripts (Python etc.) for installation
## in contrast to setup.py, you can choose the destination
# install(PROGRAMS
# scripts/my_python_script
# DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION}
# )
## Mark executables and/or libraries for installation
# install(TARGETS ${PROJECT_NAME} ${PROJECT_NAME}_node
# ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION}
# LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION}
# RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION}
# )
## Mark cpp header files for installation
# install(DIRECTORY include/${PROJECT_NAME}/
# DESTINATION ${CATKIN_PACKAGE_INCLUDE_DESTINATION}
# FILES_MATCHING PATTERN "*.h"
# PATTERN ".svn" EXCLUDE
# )
## Mark other files for installation (e.g. launch and bag files, etc.)
# install(FILES
# # myfile1
# # myfile2
# DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}
# )
#############
## Testing ##
#############
## Add gtest based cpp test target and link libraries
# catkin_add_gtest(${PROJECT_NAME}-test test/test_project_aruco.cpp)
# if(TARGET ${PROJECT_NAME}-test)
# target_link_libraries(${PROJECT_NAME}-test ${PROJECT_NAME})
# endif()
## Add folders to be run by python nosetests
# catkin_add_nosetests(test)

62
package.xml Normal file
View File

@@ -0,0 +1,62 @@
<?xml version="1.0"?>
<package>
<name>teleoperation</name>
<version>0.0.0</version>
<description>The semester project package</description>
<!-- One maintainer tag required, multiple allowed, one person per tag -->
<!-- Example: -->
<!-- <maintainer email="jane.doe@example.com">Jane Doe</maintainer> -->
<maintainer email="hrs_e@todo.todo">hrs_e</maintainer>
<!-- One license tag required, multiple allowed, one license per tag -->
<!-- Commonly used license strings: -->
<!-- BSD, MIT, Boost Software License, GPLv2, GPLv3, LGPLv2.1, LGPLv3 -->
<license>TODO</license>
<!-- Url tags are optional, but multiple are allowed, one per tag -->
<!-- Optional attribute type can be: website, bugtracker, or repository -->
<!-- Example: -->
<!-- <url type="website">http://wiki.ros.org/project_aruco</url> -->
<!-- Author tags are optional, multiple are allowed, one per tag -->
<!-- Authors do not have to be maintainers, but could be -->
<!-- Example: -->
<!-- <author email="jane.doe@example.com">Jane Doe</author> -->
<!-- The *_depend tags are used to specify dependencies -->
<!-- Dependencies can be catkin packages or system dependencies -->
<!-- Examples: -->
<!-- Use build_depend for packages you need at compile time: -->
<!-- <build_depend>message_generation</build_depend> -->
<!-- Use buildtool_depend for build tool packages: -->
<!-- <buildtool_depend>catkin</buildtool_depend> -->
<!-- Use run_depend for packages you need at runtime: -->
<!-- <run_depend>message_runtime</run_depend> -->
<!-- Use test_depend for packages you need only for testing: -->
<!-- <test_depend>gtest</test_depend> -->
<buildtool_depend>catkin</buildtool_depend>
<build_depend>cv_bridge</build_depend>
<build_depend>image_transport</build_depend>
<build_depend>roscpp</build_depend>
<build_depend>sensor_msgs</build_depend>
<build_depend>std_msgs</build_depend>
<build_depend>aruco</build_depend>
<run_depend>cv_bridge</run_depend>
<run_depend>image_transport</run_depend>
<run_depend>roscpp</run_depend>
<run_depend>sensor_msgs</run_depend>
<run_depend>std_msgs</run_depend>
<run_depend>aruco</run_depend>
<!-- The export tag contains other, unspecified, tags -->
<export>
<!-- Other tools can request additional information be placed here -->
</export>
</package>

107
src/aruco_detector.cpp Normal file
View File

@@ -0,0 +1,107 @@
#include <ros/ros.h>
#include <vector>
#include <iostream>
#include <tf/transform_broadcaster.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>
static const std::string OPENCV_WINDOW = "Original image";
static const std::string ARUCO_WINDOW = "Aruco window";
class ImageConverter {
ros::NodeHandle nh_;
tf::TransformBroadcaster aruco_pub;
image_transport::ImageTransport it_;
image_transport::Subscriber image_sub_;
public:
ImageConverter() :
it_(nh_)
{
// Subscrive to input video feed
image_sub_ = it_.subscribe("/nao_robot/camera/front/image_raw", 1,
&ImageConverter::imageCb, this);
// Create output windows
cv::namedWindow(OPENCV_WINDOW);
cv::namedWindow(ARUCO_WINDOW);
}
~ImageConverter() {
// Clean Up
cv::destroyWindow(OPENCV_WINDOW);
cv::destroyWindow(ARUCO_WINDOW);
}
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
cv_ptr = cv_bridge::toCvShare(msg,
sensor_msgs::image_encodings::BGR8);
}
catch (cv_bridge::Exception& e)
{
ROS_ERROR("cv_bridge exception: %s", e.what());
return;
}
// 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) <<
274.139508945831, 0.0, 141.184472810944,
0.0, 275.741846757374, 106.693773654172,
0.0, 0.0, 1.0);
cv::Mat distortion = (cv::Mat_<double>(4, 1) <<
-0.0870160932911717,
0.128210165050533,
0.003379500659424,
-0.00106205540818586);
aruco::CameraParameters cam(cam_mat, distortion, cv_ptr->image.size());
aruco::MarkerDetector detector;
std::vector<aruco::Marker> markers;
float m_size = 0.15;
detector.detect(cv_ptr->image, markers, cam, m_size);
// 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>(0);
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);
id += "_frame";
tf::Transform aruco_tf;
aruco_tf.setIdentity();
aruco_tf.setOrigin(tf::Vector3(x, y, z));
this->aruco_pub.sendTransform(tf::StampedTransform(
aruco_tf, ros::Time::now(), "CameraTop_optical_frame",
id.c_str()));
}
cv::imshow(ARUCO_WINDOW, aruco_demo);
cv::waitKey(2);
}
};
int main(int argc, char** argv) {
ros::init(argc, argv, "aruco_detector");
ImageConverter ic;
ros::spin();
return 0;
}

47
src/aruco_effector.cpp Normal file
View File

@@ -0,0 +1,47 @@
#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;
}