CopterExpress/clover
1
0
Fork 0
mirror of https://github.com/CopterExpress/clover.git synced 2026-05-26 21:19:35 +00:00
Code Issues Packages Projects Releases Wiki Activity

Merge branch 'mergebranch' of ../clever_bundle into mergebranch

Browse Source
This commit is contained in:
urpylka
2018-02-05 21:23:09 +03:00
parent 8e0fb01fb0 5417b32b49
commit 59a1ce7e84
48 changed files with 2273 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 @@
/deploy/ros_lib/
*.pyc

4
.gitmodules vendored Normal file
View File

@@ -0,0 +1,4 @@
[submodule "clever/static"]
path = clever/static
url = https://github.com/CopterExpress/clever-rc.git
branch = build

210
aruco_pose/CMakeLists.txt Normal file
View File

@@ -0,0 +1,210 @@
cmake_minimum_required(VERSION 2.8.3)
project(aruco_pose)
add_definitions(-std=c++11 -Wall -g)
## Compile as C++11, supported in ROS Kinetic and newer
add_compile_options(-std=c++11)
## 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
nodelet
pluginlib
roscpp
image_transport
cv_bridge
tf
#tf2
#tf2_ros
#aruco_msgs
)
## 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
# Marker.msg
# MarkerArray.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
# std_msgs # Or other packages containing 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 aruco_pose
# CATKIN_DEPENDS other_catkin_pkg
# DEPENDS system_lib
)
###########
## Build ##
###########
## Specify additional locations of header files
## Your package locations should be listed before other locations
include_directories(
# include
${catkin_INCLUDE_DIRS}
)
## Declare a C++ library
add_library(${PROJECT_NAME}
src/aruco_pose.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/aruco_pose_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
link_directories(/opt/ros/kinetic/lib)
target_link_libraries(${PROJECT_NAME}
${catkin_LIBRARIES}
"/opt/ros/kinetic/lib/libopencv_aruco3.so" # TODO: fix launch fails with .so loading
)
#############
## 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_aruco_pose.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)

5
aruco_pose/nodelet_plugins.xml Normal file
View File

@@ -0,0 +1,5 @@
<library path="lib/libaruco_pose">
<class name="aruco_pose/aruco_pose" type="ArucoPose" base_class_type="nodelet::Nodelet">
<description/>
</class>
</library>

62
aruco_pose/package.xml Normal file
View File

@@ -0,0 +1,62 @@
<?xml version="1.0"?>
<package>
<name>aruco_pose</name>
<version>0.0.0</version>
<description>ArUco maps precise pose estimation nodelet</description>
<!-- One maintainer tag required, multiple allowed, one person per tag -->
<!-- Example: -->
<!-- <maintainer email="jane.doe@example.com">Jane Doe</maintainer> -->
<maintainer email="okalachev@gmail.com">Oleg Kalachev</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/aruco_pose</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>nodelet</build_depend>
<build_depend>roscpp</build_depend>
<build_depend>image_transport</build_depend>
<build_depend>cv_bridge</build_depend>
<build_depend>tf</build_depend>
<run_depend>nodelet</run_depend>
<run_depend>roscpp</run_depend>
<run_depend>image_transport</run_depend>
<run_depend>cv_bridge</run_depend>
<build_depend>tf</build_depend>
<!-- The export tag contains other, unspecified, tags -->
<export>
<nodelet plugin="${prefix}/nodelet_plugins.xml" />
<!-- Other tools can request additional information be placed here -->
</export>
</package>

292
aruco_pose/src/aruco_pose.cpp Normal file
View File

@@ -0,0 +1,292 @@
#include <algorithm>
#include <nodelet/nodelet.h>
#include <image_transport/image_transport.h>
#include <cv_bridge/cv_bridge.h>
#include <opencv2/imgproc/imgproc.hpp>
#include <pluginlib/class_list_macros.h>
#include <geometry_msgs/TransformStamped.h>
#include <geometry_msgs/PoseStamped.h>
#include <geometry_msgs/PoseWithCovarianceStamped.h>
#include <visualization_msgs/MarkerArray.h>
#include <tf/transform_datatypes.h>
#include <opencv2/calib3d/calib3d.hpp>
#include <opencv2/opencv.hpp>
#include <opencv2/aruco.hpp>
#include <opencv2/aruco/dictionary.hpp>
#include <stdio.h>
#include <tf/transform_broadcaster.h>
namespace aruco_pose {
class ArucoPose : public nodelet::Nodelet {
tf::TransformBroadcaster br;
cv::Ptr<cv::aruco::Dictionary> dictionary;
cv::Ptr<cv::aruco::DetectorParameters> parameters;
cv::Ptr<cv::aruco::Board> board;
std::string frame_id_;
image_transport::CameraSubscriber img_sub;
image_transport::Publisher img_pub;
ros::Publisher marker_pub;
ros::Publisher pose_pub;
ros::NodeHandle nh_, nh_priv_;
virtual void onInit();
void createBoard();
cv::Point3f getObjPointsCenter(cv::Mat objPoints);
void detect(const sensor_msgs::ImageConstPtr&, const sensor_msgs::CameraInfoConstPtr&);
void parseCameraInfo(const sensor_msgs::CameraInfoConstPtr&, cv::Mat&, cv::Mat&);
tf::Transform aruco2tf(cv::Mat rvec, cv::Mat tvec);
};
void ArucoPose::onInit() {
ROS_INFO("Initializing aruco_pose");
nh_ = getNodeHandle();
nh_priv_ = getPrivateNodeHandle();
nh_priv_.param("frame_id", frame_id_, std::string("aruco_map"));
dictionary = cv::aruco::getPredefinedDictionary(cv::aruco::DICT_4X4_1000);
parameters = cv::aruco::DetectorParameters::create();
try
{
createBoard();
}
catch (const std::exception &exc)
{
std::cerr << exc.what();
exit(0);
}
image_transport::ImageTransport it(nh_);
img_sub = it.subscribeCamera("image", 1, &ArucoPose::detect, this);
image_transport::ImageTransport it_priv(nh_priv_);
img_pub = it_priv.advertise("debug", 1);
pose_pub = nh_priv_.advertise<geometry_msgs::PoseStamped>("pose", 1);
ROS_INFO("aruco_pose nodelet inited");
}
cv::Ptr<cv::aruco::Board> createCustomGridBoard(int markersX, int markersY, float markerLength, float markerSeparationX, float markerSeparationY,
const cv::Ptr<cv::aruco::Dictionary> &dictionary, std::vector<int> ids) {
CV_Assert(markersX > 0 && markersY > 0 && markerLength > 0 && markerSeparationX > 0 && markerSeparationY > 0);
cv::Ptr<cv::aruco::Board> res = cv::makePtr<cv::aruco::Board>();
res->dictionary = dictionary;
size_t totalMarkers = (size_t) markersX * markersY;
res->ids = ids;
res->objPoints.reserve(totalMarkers);
// calculate Board objPoints
float maxY = (float)markersY * markerLength + (markersY - 1) * markerSeparationY;
for(int y = 0; y < markersY; y++) {
for(int x = 0; x < markersX; x++) {
std::vector< cv::Point3f > corners;
corners.resize(4);
corners[0] = cv::Point3f(x * (markerLength + markerSeparationX),
maxY - y * (markerLength + markerSeparationY), 0);
corners[1] = corners[0] + cv::Point3f(markerLength, 0, 0);
corners[2] = corners[0] + cv::Point3f(markerLength, -markerLength, 0);
corners[3] = corners[0] + cv::Point3f(0, -markerLength, 0);
res->objPoints.push_back(corners);
}
}
return res;
}
#include "fix.cpp"
void ArucoPose::createBoard()
{
static auto map_image_pub = nh_priv_.advertise<sensor_msgs::Image>("map_image", 1, true);
cv_bridge::CvImage map_image_msg;
cv::Mat map_image;
std::string type;
nh_priv_.param<std::string>("type", type, "gridboard");
if (type == "gridboard")
{
ROS_INFO("Initialize gridboard");
int markers_x, markers_y, first_marker;
float markers_side, markers_sep_x, markers_sep_y;
std::vector<int> marker_ids;
nh_priv_.param<int>("markers_x", markers_x, 10);
nh_priv_.param<int>("markers_y", markers_y, 10);
nh_priv_.param<int>("first_marker", first_marker, 0);
if (!nh_priv_.getParam("markers_side", markers_side))
{
ROS_ERROR("gridboard: required parameter ~markers_side is not set.");
exit(1);
}
if (!nh_priv_.getParam("markers_sep_x", markers_sep_x))
{
if (!nh_priv_.getParam("markers_sep", markers_sep_x))
{
ROS_ERROR("gridboard: ~markers_sep_x or ~markers_sep parameters are required");
exit(1);
}
}
if (!nh_priv_.getParam("markers_sep_y", markers_sep_y))
{
if (!nh_priv_.getParam("markers_sep", markers_sep_y))
{
ROS_ERROR("gridboard: ~markers_sep_y or ~markers_sep parameters are required");
exit(1);
}
}
if (nh_priv_.getParam("marker_ids", marker_ids))
{
if (markers_x * markers_y != marker_ids.size())
{
ROS_FATAL("~marker_ids length should be equal to ~markers_x * ~markers_y");
exit(1);
}
}
else
{
// Fill marker_ids automatically
marker_ids.resize(markers_x * markers_y);
for(int i = 0; i < markers_x * markers_y; i++)
{
marker_ids.at(i) = first_marker++;
}
}
// Create grid board
board = createCustomGridBoard(markers_x, markers_y, markers_side, markers_sep_x, markers_sep_y, dictionary, marker_ids);
// Publish map image for debugging
_drawPlanarBoard(board, cv::Size(2000, 2000), map_image, 50, 1);
cv::cvtColor(map_image, map_image, CV_GRAY2BGR);
map_image_msg.encoding = sensor_msgs::image_encodings::BGR8;
map_image_msg.image = map_image;
map_image_pub.publish(map_image_msg.toImageMsg());
}
else if (type == "custom")
{
// Not implemented yet
ROS_FATAL("Custom boards are not implemented yet.");
}
else
{
ROS_ERROR("Incorrect map type '%s'", type.c_str());
}
}
cv::Point3f ArucoPose::getObjPointsCenter(cv::Mat objPoints) {
float min_x = std::numeric_limits<float>::max();
float max_x = std::numeric_limits<float>::min();
float min_y = min_x, max_y = max_x;
for (int i = 0; i < objPoints.rows; i++) {
max_x = std::max(max_x, objPoints.at<float>(i, 0));
max_y = std::max(max_y, objPoints.at<float>(i, 1));
min_x = std::min(min_x, objPoints.at<float>(i, 0));
min_y = std::min(min_y, objPoints.at<float>(i, 1));
}
cv::Point3f res((min_x + max_x) / 2, (min_y + max_y) / 2, 0);
return res;
}
void ArucoPose::detect(const sensor_msgs::ImageConstPtr& msg, const sensor_msgs::CameraInfoConstPtr &cinfo) {
cv::Mat image = cv_bridge::toCvShare(msg, "bgr8")->image;
std::vector<int> markerIds;
std::vector<std::vector<cv::Point2f>> markerCorners;
std::vector<std::vector<cv::Point2f>> rejectedCandidates;
cv::aruco::detectMarkers(image, dictionary, markerCorners, markerIds, parameters, rejectedCandidates);
cv::Mat cameraMatrix(3, 3, CV_64F);
cv::Mat distCoeffs(8, 1, CV_64F);
parseCameraInfo(cinfo, cameraMatrix, distCoeffs);
if (markerIds.size() > 0) {
cv::Mat rvec, tvec, objPoints;
int valid = _estimatePoseBoard(markerCorners, markerIds, board, cameraMatrix, distCoeffs,
rvec, tvec, false, objPoints);
if (valid) {
// Send map transform
tf::StampedTransform transform(aruco2tf(rvec, tvec), msg->header.stamp, cinfo->header.frame_id, frame_id_);
br.sendTransform(transform);
// Publish map pose
static geometry_msgs::PoseStamped ps;
ps.header.frame_id = frame_id_;
ps.header.stamp = msg->header.stamp;
ps.pose.orientation.w = 1;
pose_pub.publish(ps);
// Send reference point
cv::Point3f ref = getObjPointsCenter(objPoints);
tf::Vector3 ref_vector3 = tf::Vector3(ref.x, ref.y, ref.z);
tf::Quaternion q(0, 0, 0);
static tf::StampedTransform ref_transform;
ref_transform.stamp_ = msg->header.stamp;
ref_transform.frame_id_ = frame_id_;
ref_transform.child_frame_id_ = "aruco_map_reference";
ref_transform.setOrigin(ref_vector3);
ref_transform.setRotation(q);
br.sendTransform(ref_transform);
if(img_pub.getNumSubscribers() > 0)
{
// Publish debug image
cv::aruco::drawDetectedMarkers(image, markerCorners, markerIds);
cv::aruco::drawAxis(image, cameraMatrix, distCoeffs, rvec, tvec, 0.3);
}
}
}
if (img_pub.getNumSubscribers() > 0)
{
cv_bridge::CvImage out_msg;
out_msg.header.frame_id = msg->header.frame_id;
out_msg.header.stamp = msg->header.stamp;
out_msg.encoding = sensor_msgs::image_encodings::BGR8;
out_msg.image = image;
img_pub.publish(out_msg.toImageMsg());
}
}
void ArucoPose::parseCameraInfo(const sensor_msgs::CameraInfoConstPtr &cinfo, cv::Mat &cameraMat, cv::Mat &distCoeffs) {
for (int i = 0; i < 3; ++i) {
for (int j = 0; j < 3; ++j) {
cameraMat.at<double>(i, j) = cinfo->K[3 * i + j];
}
}
for (int k = 0; k < cinfo->D.size(); k++) {
distCoeffs.at<double>(k) = cinfo->D[k];
}
}
tf::Transform ArucoPose::aruco2tf(cv::Mat rvec, cv::Mat tvec) {
cv::Mat rot;
cv::Rodrigues(rvec, rot);
tf::Matrix3x3 tf_rot(rot.at<double>(0,0), rot.at<double>(0,1), rot.at<double>(0,2),
rot.at<double>(1,0), rot.at<double>(1,1), rot.at<double>(1,2),
rot.at<double>(2,0), rot.at<double>(2,1), rot.at<double>(2,2));
tf::Vector3 tf_orig(tvec.at<double>(0,0), tvec.at<double>(1,0), tvec.at<double>(2,0));
return tf::Transform(tf_rot, tf_orig);
}
PLUGINLIB_EXPORT_CLASS(ArucoPose, nodelet::Nodelet)
}

145
aruco_pose/src/fix.cpp Normal file
View File

@@ -0,0 +1,145 @@
using namespace cv;
using namespace cv::aruco;
// Temporal fix!
// TODO: remove
// fix strange bug in our OpenCV version
void _getBoardObjectAndImagePoints(const Ptr<aruco::Board> &board, InputArrayOfArrays detectedCorners,
InputArray detectedIds, OutputArray objPoints, OutputArray imgPoints) {
CV_Assert(board->ids.size() == board->objPoints.size());
CV_Assert(detectedIds.total() == detectedCorners.total());
size_t nDetectedMarkers = detectedIds.total();
std::vector< Point3f > objPnts;
objPnts.reserve(nDetectedMarkers);
std::vector< Point2f > imgPnts;
imgPnts.reserve(nDetectedMarkers);
// look for detected markers that belong to the board and get their information
for(unsigned int i = 0; i < nDetectedMarkers; i++) {
int currentId = detectedIds.getMat().ptr< int >(0)[i];
for(unsigned int j = 0; j < board->ids.size(); j++) {
if(currentId == board->ids[j]) {
for(int p = 0; p < 4; p++) {
objPnts.push_back(board->objPoints[j][p]);
imgPnts.push_back(detectedCorners.getMat(i).ptr< Point2f >(0)[p]);
}
}
}
}
// create output
Mat(objPnts).copyTo(objPoints);
Mat(imgPnts).copyTo(imgPoints);
}
int _estimatePoseBoard(InputArrayOfArrays _corners, InputArray _ids, const Ptr<aruco::Board> &board,
InputArray _cameraMatrix, InputArray _distCoeffs, OutputArray _rvec,
OutputArray _tvec, bool useExtrinsicGuess, Mat &objPoints) {
CV_Assert(_corners.total() == _ids.total());
// get object and image points for the solvePnP function
Mat /*objPoints, */imgPoints;
_getBoardObjectAndImagePoints(board, _corners, _ids, objPoints, imgPoints);
CV_Assert(imgPoints.total() == objPoints.total());
if(objPoints.total() == 0) // 0 of the detected markers in board
return 0;
// std::cout << "objPoints: " << objPoints << std::endl;
// std::cout << "imgPoints: " << imgPoints << std::endl;
solvePnP(objPoints, imgPoints, _cameraMatrix, _distCoeffs, _rvec, _tvec, useExtrinsicGuess);
// divide by four since all the four corners are concatenated in the array for each marker
return (int)objPoints.total() / 4;
}
void _drawPlanarBoard(Board *_board, Size outSize, OutputArray _img, int marginSize,
int borderBits) {
CV_Assert(outSize.area() > 0);
CV_Assert(marginSize >= 0);
_img.create(outSize, CV_8UC1);
Mat out = _img.getMat();
out.setTo(Scalar::all(255));
out.adjustROI(-marginSize, -marginSize, -marginSize, -marginSize);
// calculate max and min values in XY plane
CV_Assert(_board->objPoints.size() > 0);
float minX, maxX, minY, maxY;
minX = maxX = _board->objPoints[0][0].x;
minY = maxY = _board->objPoints[0][0].y;
for(unsigned int i = 0; i < _board->objPoints.size(); i++) {
for(int j = 0; j < 4; j++) {
minX = min(minX, _board->objPoints[i][j].x);
maxX = max(maxX, _board->objPoints[i][j].x);
minY = min(minY, _board->objPoints[i][j].y);
maxY = max(maxY, _board->objPoints[i][j].y);
}
}
float sizeX = maxX - minX;
float sizeY = maxY - minY;
// proportion transformations
float xReduction = sizeX / float(out.cols);
float yReduction = sizeY / float(out.rows);
// determine the zone where the markers are placed
if(xReduction > yReduction) {
int nRows = int(sizeY / xReduction);
int rowsMargins = (out.rows - nRows) / 2;
out.adjustROI(-rowsMargins, -rowsMargins, 0, 0);
} else {
int nCols = int(sizeX / yReduction);
int colsMargins = (out.cols - nCols) / 2;
out.adjustROI(0, 0, -colsMargins, -colsMargins);
}
// now paint each marker
Dictionary &dictionary = *(_board->dictionary);
Mat marker;
Point2f outCorners[3];
Point2f inCorners[3];
for(unsigned int m = 0; m < _board->objPoints.size(); m++) {
// transform corners to markerZone coordinates
for(int j = 0; j < 3; j++) {
Point2f pf = Point2f(_board->objPoints[m][j].x, _board->objPoints[m][j].y);
// move top left to 0, 0
pf -= Point2f(minX, minY);
pf.x = pf.x / sizeX * float(out.cols);
pf.y = (1.0f - pf.y / sizeY) * float(out.rows);
outCorners[j] = pf;
}
// get marker
Size dst_sz(outCorners[2] - outCorners[0]); // assuming CCW order
dst_sz.width = dst_sz.height = std::min(dst_sz.width, dst_sz.height); //marker should be square
dictionary.drawMarker(_board->ids[m], dst_sz.width, marker, borderBits);
if((outCorners[0].y == outCorners[1].y) && (outCorners[1].x == outCorners[2].x)) {
// marker is aligned to image axes
marker.copyTo(out(Rect(outCorners[0], dst_sz)));
continue;
}
// interpolate tiny marker to marker position in markerZone
inCorners[0] = Point2f(-0.5f, -0.5f);
inCorners[1] = Point2f(marker.cols - 0.5f, -0.5f);
inCorners[2] = Point2f(marker.cols - 0.5f, marker.rows - 0.5f);
// remove perspective
Mat transformation = getAffineTransform(inCorners, outCorners);
warpAffine(marker, out, transformation, out.size(), INTER_LINEAR,
BORDER_TRANSPARENT);
}
}

228
clever/CMakeLists.txt Normal file
View File

@@ -0,0 +1,228 @@
cmake_minimum_required(VERSION 2.8.3)
project(clever)
## Compile as C++11, supported in ROS Kinetic and newer
add_compile_options(-std=c++11)
## 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
nodelet
pluginlib
roscpp
rospy
std_msgs
message_generation
geometry_msgs
sensor_msgs
geographic_msgs
tf
tf2
tf2_geometry_msgs
)
## 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
GetTelemetry.srv
Navigate.srv
SetPosition.srv
SetPositionYawRate.srv
SetPositionGlobal.srv
SetPositionGlobalYawRate.srv
SetVelocity.srv
SetVelocityYawRate.srv
SetAttitude.srv
SetAttitudeYawRate.srv
SetRatesYaw.srv
SetRates.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
std_msgs # Or other packages containing 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 clever
# CATKIN_DEPENDS other_catkin_pkg
# DEPENDS system_lib
)
###########
## Build ##
###########
## Specify additional locations of header files
## Your package locations should be listed before other locations
include_directories(
# include
${catkin_INCLUDE_DIRS}
)
## Declare a C++ library
add_library(fcu_horiz
src/fcu_horiz.cpp
)
add_library(aruco_vpe
src/aruco_vpe.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/clever_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(fcu_horiz
${catkin_LIBRARIES}
"/opt/ros/kinetic/lib/libtf2_ros.so"
)
target_link_libraries(aruco_vpe
${catkin_LIBRARIES}
)
#############
## 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_clever.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)

45
clever/camera_info/fisheye_cam_320.yaml Normal file
View File

@@ -0,0 +1,45 @@
image_width: 320
image_height: 240
distortion_model: plumb_bob
camera_name: raspicam
camera_matrix:
rows: 3
cols: 3
data:
- 166.23942373073172
- 0.
- 162.19011246829268
- 0.
- 166.5880923974026
- 109.82227735714285
- 0.
- 0.
- 1.
distortion_coefficients:
rows: 1
cols: 8
data: [ 2.15356885e-01, -1.17472846e-01, -3.06197672e-04,
-1.09444025e-04, -4.53657258e-03, 5.73090623e-01,
-1.27574577e-01, -2.86125589e-02, 0.00000000e+00,
0.00000000e+00, 0.00000000e+00, 0.00000000e+00,
0.00000000e+00, 0.00000000e+00]
rectification_matrix:
rows: 3
cols: 3
data: [1, 0, 0, 0, 1, 0, 0, 0, 1]
projection_matrix:
rows: 3
cols: 4
data:
- 166.23942373073172
- 0.
- 162.19011246829268
- 0.
- 0.
- 166.5880923974026
- 109.82227735714285
- 0.
- 0.
- 0.
- 1.
- 0.

45
clever/camera_info/fisheye_cam_640.yaml Normal file
View File

@@ -0,0 +1,45 @@
image_width: 640
image_height: 480
distortion_model: plumb_bob
camera_name: raspicam
camera_matrix:
rows: 3
cols: 3
data:
- 332.47884746146343
- 0.
- 324.38022493658536
- 0.
- 333.1761847948052
- 219.6445547142857
- 0.
- 0.
- 1.
distortion_coefficients:
rows: 1
cols: 8
data: [ 2.15356885e-01, -1.17472846e-01, -3.06197672e-04,
-1.09444025e-04, -4.53657258e-03, 5.73090623e-01,
-1.27574577e-01, -2.86125589e-02, 0.00000000e+00,
0.00000000e+00, 0.00000000e+00, 0.00000000e+00,
0.00000000e+00, 0.00000000e+00]
rectification_matrix:
rows: 3
cols: 3
data: [1, 0, 0, 0, 1, 0, 0, 0, 1]
projection_matrix:
rows: 3
cols: 4
data:
- 332.47884746146343
- 0.
- 324.38022493658536
- 0.
- 0.
- 333.1761847948052
- 219.6445547142857
- 0.
- 0.
- 0.
- 1.
- 0.

9
clever/launch/arduino.launch Normal file
View File

@@ -0,0 +1,9 @@
<launch>
<!-- Bridge to connected Arduino using rosserial -->
<arg name="device" default="/dev/serial/by-id/usb-1a86_USB2.0-Serial-if00-port0"/>
<!-- TODO: UART connection -->
<node pkg="rosserial_python" type="serial_node.py" name="serial_node" output="screen">
<param name="port" value="$(arg device)"/>
</node>
</launch>

24
clever/launch/aruco.launch Normal file
View File

@@ -0,0 +1,24 @@
<launch>
<remap from="image" to="main_camera/image_raw"/>
<remap from="camera_info" to="main_camera/camera_info"/>
<node pkg="nodelet" type="nodelet" name="aruco_pose" args="load aruco_pose/aruco_pose nodelet_manager" clear_params="true">
<param name="frame_id" value="aruco_map_raw"/>
<param name="type" value="gridboard"/>
<param name="markers_x" value="1"/>
<param name="markers_y" value="6"/>
<param name="first_marker" value="240"/>
<param name="markers_side" value="0.3362"/>
<param name="markers_sep" value="0.46"/>
<!-- Custom gridboard: -->
<!--<rosparam param="marker_ids">[6, 5, 4, 3, 2, 1]</rosparam>-->
</node>
<node pkg="nodelet" type="nodelet" name="aruco_vpe" args="load clever/aruco_vpe nodelet_manager" clear_params="true">
<param name="aruco_orientation" value="local_origin"/>
<!--<param name="aruco_orientation" value="local_origin_upside_down"/>-->
<param name="use_mocap" value="true"/>
</node>
</launch>

58
clever/launch/clever.launch Normal file
View File

@@ -0,0 +1,58 @@
<launch>
<arg name="fcu_conn" default="usb"/>
<arg name="fcu_ip" default="127.0.0.1"/>
<arg name="gcs_bridge" default="tcp"/>
<arg name="viz" default="true"/>
<arg name="web_server" default="false"/>
<arg name="web_video_server" default="true"/>
<arg name="rosbridge" default="true"/>
<arg name="main_camera" default="true"/>
<arg name="aruco" default="false"/>
<arg name="fpv_camera" default="false"/>
<arg name="fpv_camera_device" default="/dev/v4l/by-id/usb-HD_Camera_Manufacturer_USB_2.0_Camera-video-index0"/>
<arg name="arduino" default="false"/>
<!-- mavros -->
<include file="$(find clever)/launch/mavros.launch">
<arg name="fcu_conn" value="$(arg fcu_conn)"/>
<arg name="fcu_ip" value="$(arg fcu_ip)"/>
<arg name="gcs_bridge" value="$(arg gcs_bridge)"/>
<arg name="viz" value="$(arg viz)"/>
</include>
<!-- web server -->
<include file="$(find clever)/launch/web_server.launch" if="$(arg web_server)"/>
<!-- web video server -->
<node name="web_video_server" pkg="web_video_server" type="web_video_server" if="$(arg web_video_server)" required="false" respawn="true" respawn_delay="5"/>
<!-- aruco vpe -->
<include file="$(find clever)/launch/aruco.launch" if="$(arg aruco)"/>
<!-- main nodelet manager -->
<node pkg="nodelet" type="nodelet" name="nodelet_manager" args="manager" output="screen" clear_params="true">
<param name="num_worker_threads" value="2"/>
</node>
<node pkg="tf2_ros" type="static_transform_publisher" name="local_origin_upside_down_frame" args="0 0 0 3.1415926 3.1415926 0 local_origin local_origin_upside_down"/>
<!-- fcu_horiz frame -->
<node pkg="nodelet" type="nodelet" name="fcu_horiz" args="standalone clever/fcu_horiz" output="screen" clear_params="true"/>
<!-- simplified offboard control -->
<node name="simple_offboard" pkg="clever" type="simple_offboard.py" output="screen"/>
<!-- main camera -->
<include file="$(find clever)/launch/main_camera.launch" if="$(arg main_camera)"/>
<!-- rosbridge -->
<include file="$(find rosbridge_server)/launch/rosbridge_websocket.launch" if="$(arg rosbridge)"/>
<!-- FPV video streaming -->
<include file="$(find clever)/launch/fpv_camera.launch" if="$(arg fpv_camera)">
<arg name="device" value="$(arg fpv_camera_device)"/>
</include>
<!-- Arduino bridge -->
<include file="$(find clever)/launch/arduino.launch" if="$(arg arduino)"/>
</launch>

12
clever/launch/copter_visualization.launch Normal file
View File

@@ -0,0 +1,12 @@
<launch>
<remap to="mavros/local_position/pose" from="local_position"/>
<remap to="mavros/setpoint_position/local" from="local_setpoint"/>
<node name="copter_visualization" pkg="mavros_extras" type="copter_visualization"/>
<param name="copter_visualization/fixed_frame_id" value="local_origin"/>
<param name="copter_visualization/child_frame_id" value="fcu"/>
<param name="copter_visualization/marker_scale" value="1"/>
<param name="copter_visualization/max_track_size" value="500"/>
<param name="copter_visualization/num_rotors" value="4"/>
</launch>

6
clever/launch/fpv_camera.launch Normal file
View File

@@ -0,0 +1,6 @@
<launch>
<arg name="device" default="/dev/video1"/>
<arg name="port" default="9999"/>
<node name="fpv_camera" pkg="clever" type="fpv_camera" args="$(arg device) $(arg port)" output="screen"/>
</launch>

23
clever/launch/main_camera.launch Normal file
View File

@@ -0,0 +1,23 @@
<launch>
<!-- clever 2 -->
<!--<node pkg="tf2_ros" type="static_transform_publisher" name="main_camera_frame" args="0 0 -0.07 -1.5707963 0 3.1415926 fcu main_camera_optical"/>-->
<!-- clever 3 -->
<node pkg="tf2_ros" type="static_transform_publisher" name="main_camera_frame" args="0.05 0 -0.07 1.5707963 0 3.1415926 fcu main_camera_optical"/>
<!-- clever 3, upwards -->
<!--<node pkg="tf2_ros" type="static_transform_publisher" name="main_camera_frame" args="0.05 0 0.07 -1.5707963 0 0 fcu main_camera_optical"/>-->
<!-- camera node -->
<node pkg="nodelet" type="nodelet" name="main_camera" args="load cv_camera/CvCameraNodelet nodelet_manager" clear_params="true">
<param name="frame_id" value="main_camera_optical"/>
<param name="camera_info_url" value="file://$(find clever)/camera_info/fisheye_cam_320.yaml"/>
<!-- setting camera FPS -->
<param name="rate" value="100"/>
<param name="cv_cap_prop_fps" value="40"/>
<param name="image_width" value="320"/>
<param name="image_height" value="240"/>
</node>
</launch>

53
clever/launch/mavros.launch Normal file
View File

@@ -0,0 +1,53 @@
<launch>
<arg name="fcu_conn" default="uart"/>
<arg name="fcu_ip" default="127.0.0.1"/>
<arg name="gcs_bridge" default="tcp"/>
<arg name="viz" default="true"/>
<node pkg="mavros" type="mavros_node" name="mavros" required="false" clear_params="true" output="screen">
<!-- UART connection -->
<param name="fcu_url" value="/dev/ttyAMA0:921600" if="$(eval fcu_conn is None or fcu_conn == 'uart')"/>
<!-- USB connection -->
<param name="fcu_url" value="/dev/ttyACM0" if="$(eval fcu_conn == 'usb')"/>
<!-- sitl -->
<param name="fcu_url" value="udp://@$(arg fcu_ip):14557" if="$(eval fcu_conn == 'udp')"/>
<!-- gcs bridge -->
<param name="gcs_url" value="tcp-l://0.0.0.0:5760" if="$(eval gcs_bridge == 'tcp')"/>
<param name="gcs_url" value="udp://@192.168.11.14:14550" if="$(eval gcs_bridge == 'udp')"/> <!-- TODO: fix -->
<param name="gcs_url" value="" if="$(eval not gcs_bridge)"/>
<!-- default px4 params -->
<rosparam command="load" file="$(find mavros)/launch/px4_config.yaml"/>
<!-- additional params -->
<param name="local_position/frame_id" value="local_origin"/>
<param name="local_position/tf/send" value="true"/>
<param name="local_position/tf/frame_id" value="local_origin"/>
<param name="local_position/tf/child_frame_id" value="fcu"/>
<param name="global_position/tf/send" value="false"/>
<rosparam param="plugin_blacklist">
- safety_area
- image_pub
- vibration
- distance_sensor
- rangefinder
- 3dr_radio
- actuator_control
- hil_controls
- manual_control
- vfr_hud
- px4flow
- vision_speed_estimate
- fake_gps
- cam_imu_sync
- hil
- adsb
</rosparam>
</node>
<!-- Copter visualization -->
<include file="$(find clever)/launch/copter_visualization.launch" if="$(arg viz)"/>
</launch>

17
clever/launch/sitl.launch Normal file
View File

@@ -0,0 +1,17 @@
<launch>
<!-- Clever configuration for testing in sitl -->
<arg name="ip" default="127.0.0.1"/>
<arg name="rosbridge" default="true"/>
<include file="$(find clever)/launch/clever.launch">
<arg name="fcu_conn" value="udp"/>
<arg name="fcu_ip" value="$(arg ip)"/>
<arg name="gcs_bridge" value="false"/>
<arg name="web_video_server" default="false"/>
<arg name="main_camera" default="false"/>
<arg name="fpv_camera" default="false"/>
<arg name="rosbridge" value="$(arg rosbridge)"/>
<arg name="web_server" default="false"/>
<arg name="aruco" default="false"/>
</include>
</launch>

5
clever/launch/web_server.launch Normal file
View File

@@ -0,0 +1,5 @@
<launch>
<node name="web_server" pkg="clever" type="web_server.py" output="screen">
<param name="path" value="$(find clever)/static"/>
</node>
</launch>

10
clever/nodelet_plugins.xml Normal file
View File

@@ -0,0 +1,10 @@
<library path="lib/libfcu_horiz">
<class name="clever/fcu_horiz" type="FcuHoriz" base_class_type="nodelet::Nodelet">
<description/>
</class>
</library>
<library path="lib/libaruco_vpe">
<class name="clever/aruco_vpe" type="ArucoVPE" base_class_type="nodelet::Nodelet">
<description/>
</class>
</library>

56
clever/package.xml Normal file
View File

@@ -0,0 +1,56 @@
<?xml version="1.0"?>
<package>
<name>clever</name>
<version>0.0.1</version>
<description>The clever package</description>
<!-- One maintainer tag required, multiple allowed, one person per tag -->
<!-- Example: -->
<!-- <maintainer email="jane.doe@example.com">Jane Doe</maintainer> -->
<maintainer email="okalachev@gmail.com">Oleg Kalachev</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/clever</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>nodelet</build_depend>
<build_depend>roscpp</build_depend>
<run_depend>nodelet</run_depend>
<run_depend>roscpp</run_depend>
<!-- The export tag contains other, unspecified, tags -->
<export>
<nodelet plugin="${prefix}/nodelet_plugins.xml" />
<!-- Other tools can request additional information be placed here -->
</export>
</package>

2
clever/requirements.txt Normal file
View File

@@ -0,0 +1,2 @@
flask==0.12.2
geopy==1.11.0

133
clever/src/aruco_vpe.cpp Normal file
View File

@@ -0,0 +1,133 @@
#include <nodelet/nodelet.h>
#include <pluginlib/class_list_macros.h>
#include <tf/transform_datatypes.h>
#include <tf2/exceptions.h>
#include <tf2/convert.h>
#include <tf2_ros/transform_listener.h>
#include <tf2_ros/transform_broadcaster.h>
#include <tf2_ros/static_transform_broadcaster.h>
#include <tf2_geometry_msgs/tf2_geometry_msgs.h>
#include <geometry_msgs/TransformStamped.h>
#include <geometry_msgs/PoseStamped.h>
#include "util.h"
using namespace tf2_ros;
using geometry_msgs::PoseStamped;
using geometry_msgs::TransformStamped;
using std::string;
class ArucoVPE : public nodelet::Nodelet
{
public:
ArucoVPE() :
last_published_(0),
lookup_timeout_(0.05)
{}
private:
ros::Time last_published_;
ros::Duration lookup_timeout_;
ros::Publisher vision_position_pub_;
ros::Timer dummy_vision_timer_;
string aruco_orientation_;
bool reset_vpe_;
void onInit()
{
ros::NodeHandle& nh = getNodeHandle();
ros::NodeHandle& nh_priv = getPrivateNodeHandle();
nh_priv.param<string>("aruco_orientation", aruco_orientation_, "local_origin");
bool use_mocap;
nh_priv.param<bool>("use_mocap", use_mocap, false);
nh_priv.param<bool>("reset_vpe", reset_vpe_, !use_mocap);
static ros::Subscriber pose_sub = nh.subscribe("mavros/local_position/pose", 1, &ArucoVPE::handlePose, this);
static ros::Subscriber aruco_pose_sub = nh.subscribe("aruco_pose/pose", 1, &ArucoVPE::handleArucoPose, this);
vision_position_pub_ = nh.advertise<PoseStamped>(use_mocap ? "mavros/mocap/pose" : "mavros/vision_pose/pose", 1);
ROS_INFO("aruco orientation frame: %s", aruco_orientation_.c_str());
dummy_vision_timer_ = nh.createTimer(ros::Duration(0.5), &ArucoVPE::publishDummy, this);
ROS_INFO("Aruco VPE initialized");
}
void publishDummy(const ros::TimerEvent&)
{
// This is published to init FCU's position estimator
static PoseStamped ps;
ps.header.stamp = ros::Time::now();
ps.pose.orientation.w = 1;
vision_position_pub_.publish(ps);
}
void handlePose(const geometry_msgs::PoseStampedConstPtr& pose)
{
// local position is inited, stop posting dummy position
ROS_INFO_ONCE("Got local position, stop publishing zeroes");
dummy_vision_timer_.stop();
}
void handleArucoPose(const geometry_msgs::PoseStampedConstPtr& pose)
{
static TransformBroadcaster br;
static Buffer tf_buffer;
static TransformListener tfListener(tf_buffer);
static StaticTransformBroadcaster static_br;
static PoseStamped ps, vpe_raw, vpe;
TransformStamped t;
ros::Time stamp = pose->header.stamp;
double roll, pitch, yaw;
try
{
// Refine aruco map pose
// Reference in local origin
t = tf_buffer.lookupTransform(aruco_orientation_, "aruco_map_reference", stamp, lookup_timeout_);
quaternionToEuler(t.transform.rotation, roll, pitch, yaw);
eulerToQuaternion(t.transform.rotation, 0, 0, yaw);
t.child_frame_id = "aruco_map_reference_horiz";
br.sendTransform(t);
// Aruco map in reference
t = tf_buffer.lookupTransform("aruco_map_reference", "aruco_map_raw", stamp, lookup_timeout_);
t.header.frame_id = "aruco_map_reference_horiz";
t.child_frame_id = "aruco_map_vision";
br.sendTransform(t);
if (last_published_.toSec() == 0 || // no vpe has been posted
(reset_vpe_ && (ros::Time::now() - last_published_ > ros::Duration(2)))) // vpe origin outdated
{
ROS_INFO("Reset VPE");
t = tf_buffer.lookupTransform("local_origin", "aruco_map_vision", stamp, lookup_timeout_);
t.child_frame_id = "aruco_map";
static_br.sendTransform(t);
}
// Calculate VPE
ps.header.frame_id = "fcu_horiz";
ps.header.stamp = stamp;
ps.pose.orientation.w = 1;
tf_buffer.transform(ps, vpe_raw, "aruco_map_vision", lookup_timeout_);
vpe_raw.header.frame_id = "aruco_map";
tf_buffer.transform(vpe_raw, vpe, "local_origin", lookup_timeout_);
vision_position_pub_.publish(vpe);
last_published_ = stamp;
dummy_vision_timer_.stop();
}
catch (const tf2::TransformException& e)
{
ROS_WARN_THROTTLE(10, "Aruco VPE: failed to transform: %s", e.what());
}
}
};
PLUGINLIB_EXPORT_CLASS(ArucoVPE, nodelet::Nodelet)

40
clever/src/fcu_horiz.cpp Normal file
View File

@@ -0,0 +1,40 @@
#include <nodelet/nodelet.h>
#include <pluginlib/class_list_macros.h>
#include <tf2_ros/transform_broadcaster.h>
#include <geometry_msgs/TransformStamped.h>
#include <geometry_msgs/PoseStamped.h>
#include "util.h"
class FcuHoriz : public nodelet::Nodelet
{
geometry_msgs::TransformStamped t_;
void handlePose(const geometry_msgs::PoseStampedConstPtr& msg)
{
static tf2_ros::TransformBroadcaster br;
double roll, pitch, yaw;
t_.header.stamp = msg->header.stamp;
t_.header.frame_id = msg->header.frame_id;
t_.transform.translation.x = msg->pose.position.x;
t_.transform.translation.y = msg->pose.position.y;
t_.transform.translation.z = msg->pose.position.z;
// Warning: this is not thead-safe
quaternionToEuler(msg->pose.orientation, roll, pitch, yaw);
eulerToQuaternion(t_.transform.rotation, 0, 0, yaw);
br.sendTransform(t_);
}
void onInit()
{
t_.child_frame_id = "fcu_horiz";
t_.transform.rotation.w = 1;
static ros::Subscriber pose_sub = getNodeHandle().subscribe("mavros/local_position/pose", 1, &FcuHoriz::handlePose, this);
ROS_INFO("fcu_horiz initialized");
}
};
PLUGINLIB_EXPORT_CLASS(FcuHoriz, nodelet::Nodelet)

8
clever/src/fpv_camera Executable file
View File

@@ -0,0 +1,8 @@
#!/usr/bin/env bash
# Usage
# fpv_camera <video_device> <http port>
echo "Starting FPV camera $1 on :$2"
cd /home/pi/mjpg-streamer/mjpg-streamer-experimental
./mjpg_streamer -i "./input_uvc.so -d $1 -r 320x240 -f 30" -o "./output_http.so -w ./www -p $2"

36
clever/src/global_local.py Normal file
View File

@@ -0,0 +1,36 @@
import rospy
import math
import geopy
from geometry_msgs.msg import PoseStamped
from geopy.distance import VincentyDistance, vincenty
from sensor_msgs.msg import NavSatFix
def global_to_local(lat, lon):
# TODO: refactor
position_global = rospy.wait_for_message('mavros/global_position/global', NavSatFix, timeout=5)
pose = rospy.wait_for_message('mavros/local_position/pose', PoseStamped, timeout=5)
d = math.hypot(pose.pose.position.x, pose.pose.position.y)
bearing = math.degrees(math.atan2(-pose.pose.position.x, -pose.pose.position.y))
if bearing < 0:
bearing += 360
cur = geopy.Point(position_global.latitude, position_global.longitude)
origin = VincentyDistance(meters=d).destination(cur, bearing)
_origin = origin.latitude, origin.longitude
olat_tlon = origin.latitude, lon
tlat_olon = lat, origin.longitude
N = vincenty(_origin, tlat_olon)
if lat < origin.latitude:
N = -N
E = vincenty(_origin, olat_tlon)
if lon < origin.longitude:
E = -E
return E.meters, N.meters

466
clever/src/simple_offboard.py Executable file
View File

@@ -0,0 +1,466 @@
#!/usr/bin/env python
from __future__ import division
import rospy
from geometry_msgs.msg import TransformStamped, PoseStamped, Point, PointStamped, Vector3, Vector3Stamped, TwistStamped, QuaternionStamped
from sensor_msgs.msg import NavSatFix, BatteryState
import tf2_ros
import tf2_geometry_msgs
from mavros_msgs.msg import PositionTarget, AttitudeTarget, State
from mavros_msgs.srv import CommandBool, SetMode
from threading import Lock
import math
from global_local import global_to_local
from util import euler_from_orientation, vector3_from_point, orientation_from_euler
from std_srvs.srv import Trigger
from clever import srv
rospy.init_node('simple_offboard')
# TF2 stuff
tf_broadcaster = tf2_ros.TransformBroadcaster()
static_tf_broadcaster = tf2_ros.StaticTransformBroadcaster()
tf_buffer = tf2_ros.Buffer()
tf_listener = tf2_ros.TransformListener(tf_buffer)
position_pub = rospy.Publisher('/mavros/setpoint_raw/local', PositionTarget, queue_size=1)
attitude_pub = rospy.Publisher('/mavros/setpoint_raw/attitude', AttitudeTarget, queue_size=1)
target_pub = rospy.Publisher('~target', PoseStamped, queue_size=1)
arming = rospy.ServiceProxy('/mavros/cmd/arming', CommandBool, persistent=True)
set_mode = rospy.ServiceProxy('/mavros/set_mode', SetMode, persistent=True)
pose = None
global_position = None
velocity = None
state = None
battery = None
def pose_update(data):
global pose
pose = data
def global_position_update(data):
global global_position
global_position = data
def velocity_update(data):
global velocity
velocity = data
def state_update(data):
global state
state = data
def battery_update(data):
global battery
battery = data
rospy.Subscriber('/mavros/state', State, state_update)
rospy.Subscriber('/mavros/local_position/pose', PoseStamped, pose_update)
rospy.Subscriber('/mavros/local_position/velocity', TwistStamped, velocity_update)
rospy.Subscriber('/mavros/global_position/global', NavSatFix, global_position_update)
rospy.Subscriber('/mavros/battery', BatteryState, battery_update)
AUTO_OFFBOARD = rospy.get_param('~auto_offboard', True)
AUTO_ARM = AUTO_OFFBOARD and rospy.get_param('~auto_arm', True)
OFFBOARD_TIMEOUT = rospy.Duration(rospy.get_param('~offboard_timeout', 3))
ARM_TIMEOUT = rospy.Duration(rospy.get_param('~arm_timeout', 5))
TRANSFORM_TIMEOUT = rospy.Duration(rospy.get_param('~transform_timeout', 3))
SETPOINT_RATE = rospy.get_param('~setpoint_rate', 30)
def offboard_and_arm():
if AUTO_OFFBOARD and state.mode != 'OFFBOARD':
rospy.sleep(.3)
rospy.loginfo('Switch mode to OFFBOARD')
res = set_mode(base_mode=0, custom_mode='OFFBOARD')
start = rospy.get_rostime()
while True:
if state.mode == 'OFFBOARD':
break
if rospy.get_rostime() - start > OFFBOARD_TIMEOUT:
raise Exception('OFFBOARD request timed out')
if AUTO_ARM and not state.armed:
rospy.loginfo('Arming')
res = arming(True)
start = rospy.get_rostime()
while True:
if state.armed:
return True
if rospy.get_rostime() - start > ARM_TIMEOUT:
raise Exception('Arming timed out')
ps = PoseStamped()
vs = Vector3Stamped()
BRAKE_TIME = rospy.Duration(0)
def get_navigate_setpoint(stamp, start, finish, start_stamp, speed):
distance = math.sqrt((finish.z - start.z)**2 + (finish.x - start.x)**2 + (finish.y - start.y)**2)
time = rospy.Duration(distance / speed)
k = (stamp - start_stamp) / time
time_left = start_stamp + time - stamp
if BRAKE_TIME and time_left < BRAKE_TIME:
# time to brake
time_before_braking = time - BRAKE_TIME
brake_time_passed = (stamp - start_stamp - time_before_braking)
if brake_time_passed > 2 * BRAKE_TIME:
# finish
k = 1
else:
# brake!
k_before_braking = time_before_braking / time
k_after_braking = (speed * brake_time_passed.to_sec() - brake_time_passed.to_sec() ** 2 * speed / 4 / BRAKE_TIME.to_sec()) / distance
k = k_before_braking + k_after_braking
k = min(k, 1)
p = Point()
p.x = start.x + (finish.x - start.x) * k
p.y = start.y + (finish.y - start.y) * k
p.z = start.z + (finish.z - start.z) * k
return p
def get_publisher_and_message(req, stamp, continued=True, update_frame=True):
ps.header.stamp = stamp
vs.header.stamp = stamp
if isinstance(req, srv.NavigateRequest):
global current_nav_start, current_nav_start_stamp, current_nav_finish
if update_frame:
ps.header.frame_id = req.frame_id or 'local_origin'
ps.pose.position = Point(req.x, req.y, req.z)
ps.pose.orientation = orientation_from_euler(0, 0, req.yaw)
current_nav_finish = tf_buffer.transform(ps, 'local_origin', TRANSFORM_TIMEOUT)
if not continued:
current_nav_start = pose.pose.position
current_nav_start_stamp = stamp
setpoint = get_navigate_setpoint(stamp, current_nav_start, current_nav_finish.pose.position,
current_nav_start_stamp, req.speed)
msg = PositionTarget(coordinate_frame=PositionTarget.FRAME_LOCAL_NED,
type_mask=PositionTarget.IGNORE_VX + PositionTarget.IGNORE_VY + PositionTarget.IGNORE_VZ +
PositionTarget.IGNORE_AFX + PositionTarget.IGNORE_AFY + PositionTarget.IGNORE_AFZ +
PositionTarget.IGNORE_YAW_RATE,
position=setpoint,
yaw=euler_from_orientation(current_nav_finish.pose.orientation)[2] - math.pi / 2)
return position_pub, msg
elif isinstance(req, srv.SetPositionRequest):
ps.header.frame_id = req.frame_id or 'local_origin'
ps.pose.position = Point(req.x, req.y, req.z)
ps.pose.orientation = orientation_from_euler(0, 0, req.yaw)
pose_local = tf_buffer.transform(ps, 'local_origin', TRANSFORM_TIMEOUT)
msg = PositionTarget(coordinate_frame=PositionTarget.FRAME_LOCAL_NED,
type_mask=PositionTarget.IGNORE_VX + PositionTarget.IGNORE_VY + PositionTarget.IGNORE_VZ +
PositionTarget.IGNORE_AFX + PositionTarget.IGNORE_AFY + PositionTarget.IGNORE_AFZ +
PositionTarget.IGNORE_YAW_RATE,
position=pose_local.pose.position,
yaw=euler_from_orientation(pose_local.pose.orientation)[2] - math.pi / 2)
return position_pub, msg
elif isinstance(req, srv.SetPositionYawRateRequest):
ps.header.frame_id = req.frame_id or 'local_origin'
ps.pose.position = Point(req.x, req.y, req.z)
pose_local = tf_buffer.transform(ps, 'local_origin', TRANSFORM_TIMEOUT)
msg = PositionTarget(coordinate_frame=PositionTarget.FRAME_LOCAL_NED,
type_mask=PositionTarget.IGNORE_VX + PositionTarget.IGNORE_VY + PositionTarget.IGNORE_VZ +
PositionTarget.IGNORE_AFX + PositionTarget.IGNORE_AFY + PositionTarget.IGNORE_AFZ +
PositionTarget.IGNORE_YAW,
position=pose_local.pose.position,
yaw_rate=req.yaw_rate)
return position_pub, msg
elif isinstance(req, srv.SetPositionGlobalRequest):
x, y = global_to_local(req.lat, req.lon)
ps.header.frame_id = req.frame_id or 'local_origin'
ps.pose.position = Point(0, 0, req.z)
ps.pose.orientation = orientation_from_euler(0, 0, req.yaw)
pose_local = tf_buffer.transform(ps, 'local_origin', TRANSFORM_TIMEOUT)
pose_local.pose.position.x = x
pose_local.pose.position.y = y
msg = PositionTarget(coordinate_frame=PositionTarget.FRAME_LOCAL_NED,
type_mask=PositionTarget.IGNORE_VX + PositionTarget.IGNORE_VY + PositionTarget.IGNORE_VZ +
PositionTarget.IGNORE_AFX + PositionTarget.IGNORE_AFY + PositionTarget.IGNORE_AFZ +
PositionTarget.IGNORE_YAW_RATE,
position=pose_local.pose.position,
yaw=euler_from_orientation(pose_local.pose.orientation)[2] - math.pi / 2)
return position_pub, msg
elif isinstance(req, srv.SetPositionGlobalYawRateRequest):
x, y = global_to_local(req.lat, req.lon)
ps.header.frame_id = req.frame_id or 'local_origin'
ps.pose.position = Point(0, 0, req.z)
pose_local = tf_buffer.transform(ps, 'local_origin', TRANSFORM_TIMEOUT)
pose_local.pose.position.x = x
pose_local.pose.position.y = y
msg = PositionTarget(coordinate_frame=PositionTarget.FRAME_LOCAL_NED,
type_mask=PositionTarget.IGNORE_VX + PositionTarget.IGNORE_VY + PositionTarget.IGNORE_VZ +
PositionTarget.IGNORE_AFX + PositionTarget.IGNORE_AFY + PositionTarget.IGNORE_AFZ +
PositionTarget.IGNORE_YAW,
position=pose_local.pose.position,
yaw_rate=req.yaw_rate)
return position_pub, msg
elif isinstance(req, srv.SetVelocityRequest):
vs.vector = Vector3(req.vx, req.vy, req.vz)
vs.header.frame_id = req.frame_id or 'local_origin'
ps.header.frame_id = req.frame_id or 'local_origin'
ps.pose.orientation = orientation_from_euler(0, 0, req.yaw)
pose_local = tf_buffer.transform(ps, 'local_origin', TRANSFORM_TIMEOUT)
vector_local = tf_buffer.transform(vs, 'local_origin', TRANSFORM_TIMEOUT)
msg = PositionTarget(coordinate_frame=PositionTarget.FRAME_LOCAL_NED,
type_mask=PositionTarget.IGNORE_PX + PositionTarget.IGNORE_PY + PositionTarget.IGNORE_PZ +
PositionTarget.IGNORE_AFX + PositionTarget.IGNORE_AFY + PositionTarget.IGNORE_AFZ +
PositionTarget.IGNORE_YAW_RATE,
velocity=vector_local.vector,
yaw=euler_from_orientation(pose_local.pose.orientation)[2] - math.pi / 2)
return position_pub, msg
elif isinstance(req, srv.SetVelocityYawRateRequest):
vs.vector = Vector3(req.vx, req.vy, req.vz)
vs.header.frame_id = req.frame_id or 'local_origin'
vector_local = tf_buffer.transform(vs, 'local_origin', TRANSFORM_TIMEOUT)
msg = PositionTarget(coordinate_frame=PositionTarget.FRAME_LOCAL_NED,
type_mask=PositionTarget.IGNORE_PX + PositionTarget.IGNORE_PY + PositionTarget.IGNORE_PZ +
PositionTarget.IGNORE_AFX + PositionTarget.IGNORE_AFY + PositionTarget.IGNORE_AFZ +
PositionTarget.IGNORE_YAW,
velocity=vector_local.vector,
yaw_rate=req.yaw_rate)
return position_pub, msg
elif isinstance(req, srv.SetAttitudeRequest):
ps.header.frame_id = req.frame_id or 'local_origin'
ps.pose.orientation = orientation_from_euler(req.roll, req.pitch, req.yaw)
pose_local = tf_buffer.transform(ps, 'local_origin', TRANSFORM_TIMEOUT)
msg = AttitudeTarget(orientation=pose_local.pose.orientation,
thrust=req.thrust,
type_mask=AttitudeTarget.IGNORE_YAW_RATE + AttitudeTarget.IGNORE_PITCH_RATE +
AttitudeTarget.IGNORE_ROLL_RATE)
return attitude_pub, msg
elif isinstance(req, srv.SetAttitudeYawRateRequest):
msg = AttitudeTarget(orientation=orientation_from_euler(req.roll, req.pitch, 0),
thrust=req.thrust,
type_mask=AttitudeTarget.IGNORE_PITCH_RATE + AttitudeTarget.IGNORE_ROLL_RATE)
msg.body_rate.z = req.yaw_rate
return attitude_pub, msg
elif isinstance(req, srv.SetRatesYawRequest):
ps.header.frame_id = req.frame_id or 'local_origin'
ps.pose.orientation = orientation_from_euler(0, 0, req.yaw)
pose_local = tf_buffer.transform(ps, 'local_origin', TRANSFORM_TIMEOUT)
msg = AttitudeTarget(orientation=pose_local.pose.orientation,
thrust=req.thrust,
type_mask=AttitudeTarget.IGNORE_YAW_RATE,
body_rate=Vector3(req.roll_rate, req.pitch_rate, 0))
return attitude_pub, msg
elif isinstance(req, srv.SetRatesRequest):
msg = AttitudeTarget(thrust=req.thrust,
type_mask=AttitudeTarget.IGNORE_ATTITUDE,
body_rate=Vector3(req.roll_rate, req.pitch_rate, req.yaw_rate))
return attitude_pub, msg
current_pub = None
current_msg = None
current_req = None
current_nav_start = None
current_nav_finish = None
current_nav_start_stamp = None
handle_lock = Lock()
def handle(req):
global current_pub, current_msg, current_req
if not state or not state.connected:
rospy.logwarn('No connection to the FCU')
return {'message': 'No connection to the FCU'}
if isinstance(req, srv.NavigateRequest) and req.speed <= 0:
rospy.logwarn('Navigate speed must be greater than zero, %s passed')
return {'message': 'Navigate speed must be greater than zero, %s passed' % req.speed}
try:
with handle_lock:
stamp = rospy.get_rostime()
current_req = req
current_pub, current_msg = get_publisher_and_message(req, stamp, False)
rospy.loginfo('Topic: %s, message: %s', current_pub.name, current_msg)
current_msg.header.stamp = stamp
current_pub.publish(current_msg)
if req.auto_arm:
offboard_and_arm()
else:
if state.mode != 'OFFBOARD':
return {'message': 'Copter is not in OFFBOARD mode, use auto_arm?'}
if not state.armed:
return {'message': 'Copter is not armed, use auto_arm?'}
return {'success': True}
except Exception as e:
rospy.logerr(str(e))
return {'success': False, 'message': str(e)}
def release(req):
global current_pub
current_pub = None
rospy.loginfo('simple_offboard: release')
return {'success': True}
rospy.Service('navigate', srv.Navigate, handle)
rospy.Service('set_position', srv.SetPosition, handle)
rospy.Service('set_position/yaw_rate', srv.SetPositionYawRate, handle)
rospy.Service('set_position_global', srv.SetPositionGlobal, handle)
rospy.Service('set_position_global/yaw_rate', srv.SetPositionGlobalYawRate, handle)
rospy.Service('set_velocity', srv.SetVelocity, handle)
rospy.Service('set_velocity/yaw_rate', srv.SetVelocityYawRate, handle)
rospy.Service('set_attitude', srv.SetAttitude, handle)
rospy.Service('set_attitude/yaw_rate', srv.SetAttitudeYawRate, handle)
rospy.Service('set_rates', srv.SetRates, handle)
rospy.Service('set_rates/yaw', srv.SetRatesYaw, handle)
rospy.Service('release', Trigger, release)
def get_telemetry(req):
res = {
'frame_id': req.frame_id or 'local_origin',
'x': float('nan'),
'y': float('nan'),
'z': float('nan'),
'lat': float('nan'),
'lon': float('nan'),
'vx': float('nan'),
'vy': float('nan'),
'vz': float('nan'),
'pitch': float('nan'),
'roll': float('nan'),
'yaw': float('nan'),
'pitch_rate': float('nan'),
'roll_rate': float('nan'),
'yaw_rate': float('nan'),
'voltage': float('nan'),
'cell_voltage': float('nan')
}
frame_id = req.frame_id or 'local_origin'
stamp = rospy.get_rostime()
if pose:
p = tf_buffer.transform(pose, frame_id, TRANSFORM_TIMEOUT)
res['x'] = p.pose.position.x
res['y'] = p.pose.position.y
res['z'] = p.pose.position.z
# Get yaw in the request's frame_in
_, _, res['yaw'] = euler_from_orientation(p.pose.orientation)
# Calculate pitch and roll as angles between the pose and fcu_horiz
attitude_pose = tf_buffer.transform(pose, 'fcu_horiz', TRANSFORM_TIMEOUT)
res['roll'], res['pitch'], _ = euler_from_orientation(attitude_pose.pose.orientation)
if velocity:
v = Vector3Stamped()
v.header.stamp = velocity.header.stamp
v.header.frame_id = velocity.header.frame_id
v.vector = velocity.twist.linear
linear = tf_buffer.transform(v, frame_id, TRANSFORM_TIMEOUT)
res['vx'] = linear.vector.x
res['vy'] = linear.vector.y
res['vz'] = linear.vector.z
# TODO pitch_rate, roll_rate, yaw_rate
if global_position and stamp - global_position.header.stamp < rospy.Duration(5):
res['lat'] = global_position.latitude
res['lon'] = global_position.longitude
if state:
res['connected'] = state.connected
res['armed'] = state.armed
res['mode'] = state.mode
if battery:
res['voltage'] = battery.voltage
try:
res['cell_voltage'] = battery.cell_voltage[0]
except:
pass
return res
rospy.Service('get_telemetry', srv.GetTelemetry, get_telemetry)
rospy.loginfo('simple_offboard inited')
def start_loop():
global current_pub, current_msg, current_req
r = rospy.Rate(SETPOINT_RATE)
while not rospy.is_shutdown():
with handle_lock:
if current_pub is not None:
try:
stamp = rospy.get_rostime()
if getattr(current_req, 'update_frame', False) or isinstance(current_req, srv.NavigateRequest):
current_pub, current_msg = get_publisher_and_message(current_req, stamp, True,
getattr(current_req, 'update_frame', False))
current_msg.header.stamp = stamp
current_pub.publish(current_msg)
# For monitoring
if isinstance(current_msg, PositionTarget):
p = PoseStamped()
p.header.frame_id = 'local_origin'
p.header.stamp = stamp
p.pose.position = current_msg.position
p.pose.orientation = orientation_from_euler(0, 0, current_msg.yaw + math.pi / 2)
target_pub.publish(p)
except Exception as e:
rospy.logwarn_throttle(10, str(e))
r.sleep()
start_loop()

17
clever/src/util.h Normal file
View File

@@ -0,0 +1,17 @@
#pragma once
#include <tf/transform_datatypes.h>
#include <geometry_msgs/Quaternion.h>
inline void quaternionToEuler(geometry_msgs::Quaternion q, double& roll, double& pitch, double& yaw)
{
tf::Quaternion tfq(q.x, q.y, q.z, q.w);
tf::Matrix3x3 m(tfq);
m.getRPY(roll, pitch, yaw);
}
inline void eulerToQuaternion(geometry_msgs::Quaternion& q, double roll, double pitch, double yaw)
{
tf::Quaternion tfq(roll, pitch, yaw);
quaternionTFToMsg(tfq, q);
}

28
clever/src/util.py Normal file
View File

@@ -0,0 +1,28 @@
from geometry_msgs.msg import Quaternion, Vector3, Point
import tf.transformations as t
def orientation_from_quaternion(q):
return Quaternion(*q)
def orientation_from_euler(roll, pitch, yaw):
q = t.quaternion_from_euler(roll, pitch, yaw)
return orientation_from_quaternion(q)
def quaternion_from_orientation(o):
return o.x, o.y, o.z, o.w
def euler_from_orientation(o):
q = quaternion_from_orientation(o)
return t.euler_from_quaternion(q)
def vector3_from_point(p):
return Vector3(p.x, p.y, p.z)
def point_from_vector3(v):
return Point(v.x, v.y, v.z)

60
clever/src/web_server.py Executable file
View File

@@ -0,0 +1,60 @@
#!/usr/bin/env python
import rospy
import subprocess
import re
from flask import Flask, send_from_directory, send_file, request, jsonify
rospy.init_node('web_server', disable_signals=True)
port = rospy.get_param('~port', 7070)
host = rospy.get_param('~host', '0.0.0.0')
serve_path = rospy.get_param('~path')
app = Flask(__name__)
@app.route('/')
def serve_index():
return send_from_directory(serve_path, 'index.html')
@app.route('/<path:path>')
def serve_static(path):
print serve_path, path
return send_from_directory(serve_path, path)
@app.route('/wifi_data/')
def get_wifi_data():
cur_ip = request.remote_addr
ip_signal = get_ip_signal()
return jsonify({'ip': cur_ip, 'signal': ip_signal[cur_ip]}), 200
def get_ip_signal():
wlan_interface = 'wlan0'
# Getting info about wifi client connected to access point. From here we know MAC and signal level
iwl = subprocess.check_output(['sudo', 'iw', 'dev', 'wlan0', 'station', 'dump']).splitlines()
mac_signal = {}
cur_client = ''
for line in iwl:
if line.find('Station') != -1:
cur_client = re.search(r'([0-9A-F]{2}[:-]){5}([0-9A-F]{2})', line, re.I).group()
if line.find('signal') != -1:
sg = re.search(r'(\[-?\d*\])', line, re.I).group()
mac_signal[cur_client] = re.sub(r'[\[\]]', '', sg)
ip_signal = {}
# Getting ip-mac mapping
ip_mac = subprocess.check_output(['arp', '-i', wlan_interface]).splitlines()
for line in ip_mac:
mac = re.search(r'([0-9A-F]{2}[:-]){5}([0-9A-F]{2})', line, re.I)
if mac is not None:
mac = mac.group()
if mac in mac_signal:
ips = re.search(r'((2[0-5]|1[0-9]|[0-9])?[0-9]\.){3}((2[0-5]|1[0-9]|[0-9])?[0-9])', line, re.I).group()
ip_signal[ips] = mac_signal[mac]
return ip_signal
rospy.loginfo('Serving on %s:%s', host, port)
app.run(host=host, port=port, threaded=True)

22
clever/srv/GetTelemetry.srv Normal file
View File

@@ -0,0 +1,22 @@
string frame_id
---
string frame_id
bool connected
bool armed
string mode
float32 x
float32 y
float32 z
float32 lat
float32 lon
float32 vx
float32 vy
float32 vz
float32 pitch
float32 roll
float32 yaw
float32 pitch_rate
float32 roll_rate
float32 yaw_rate
float32 voltage
float32 cell_voltage

11
clever/srv/Navigate.srv Normal file
View File

@@ -0,0 +1,11 @@
float32 x
float32 y
float32 z
float32 yaw
float32 speed
string frame_id
bool update_frame
bool auto_arm
---
bool success
string message

10
clever/srv/SetAttitude.srv Normal file
View File

@@ -0,0 +1,10 @@
float32 pitch
float32 roll
float32 yaw
float32 thrust
string frame_id
bool update_frame
bool auto_arm
---
bool success
string message

8
clever/srv/SetAttitudeYawRate.srv Normal file
View File

@@ -0,0 +1,8 @@
float32 roll
float32 pitch
float32 yaw_rate
float32 thrust
bool auto_arm
---
bool success
string message

10
clever/srv/SetPosition.srv Normal file
View File

@@ -0,0 +1,10 @@
float32 x
float32 y
float32 z
float32 yaw
string frame_id
bool update_frame
bool auto_arm
---
bool success
string message

10
clever/srv/SetPositionGlobal.srv Normal file
View File

@@ -0,0 +1,10 @@
float32 lat
float32 lon
float32 z
float32 yaw
string frame_id
bool update_frame
bool auto_arm
---
bool success
string message

10
clever/srv/SetPositionGlobalYawRate.srv Normal file
View File

@@ -0,0 +1,10 @@
float32 lat
float32 lon
float32 z
float32 yaw_rate
string frame_id
bool update_frame
bool auto_arm
---
bool success
string message

10
clever/srv/SetPositionYawRate.srv Normal file
View File

@@ -0,0 +1,10 @@
float32 x
float32 y
float32 z
float32 yaw_rate
string frame_id
bool update_frame
bool auto_arm
---
bool success
string message

8
clever/srv/SetRates.srv Normal file
View File

@@ -0,0 +1,8 @@
float32 pitch_rate
float32 roll_rate
float32 yaw_rate
float32 thrust
bool auto_arm
---
bool success
string message

10
clever/srv/SetRatesYaw.srv Normal file
View File

@@ -0,0 +1,10 @@
float32 pitch_rate
float32 roll_rate
float32 yaw
float32 thrust
string frame_id
bool update_frame
bool auto_arm
---
bool success
string message

10
clever/srv/SetVelocity.srv Normal file
View File

@@ -0,0 +1,10 @@
float32 vx
float32 vy
float32 vz
float32 yaw
string frame_id
bool update_frame
bool auto_arm
---
bool success
string message

10
clever/srv/SetVelocityYawRate.srv Normal file
View File

@@ -0,0 +1,10 @@
float32 vx
float32 vy
float32 vz
float32 yaw_rate
string frame_id
bool update_frame
bool auto_arm
---
bool success
string message

1
clever/static Submodule

Submodule clever/static added at 54c530431f

12
deploy/clever.service Normal file
View File

@@ -0,0 +1,12 @@
[Unit]
Description=Clever ROS package
Requires=roscore.service
After=roscore.service
[Service]
EnvironmentFile=/home/pi/catkin_ws/src/clever/deploy/roscore.env
ExecStart=/opt/ros/kinetic/bin/roslaunch clever clever.launch --wait
Restart=on-abort
[Install]
WantedBy=multi-user.target

BIN
deploy/clever_arudino.tar.gz Normal file
View File

Binary file not shown.

9
deploy/generate_ros_lib Normal file
View File

@@ -0,0 +1,9 @@
#!/usr/bin/env bash
# This script generates ros_lib library for Arduino for using with rosseial_arduino:
# http://wiki.ros.org/rosserial_arduino/Tutorials
# https://copterexpress.gitbooks.io/clever/content/docs/arduino.html
rm -rf ros_lib
rosrun rosserial_arduino make_libraries.py .
tar czf clever_arudino.tar.gz ros_lib

10
deploy/roscore.env Normal file
View File

@@ -0,0 +1,10 @@
ROS_ROOT=/opt/ros/kinetic/share/ros
ROS_DISTRO=kinetic
ROS_PACKAGE_PATH=/home/pi/catkin_ws/src:/opt/ros/kinetic/share
ROS_PORT=11311
ROS_MASTER_URI=http://localhost:11311
CMAKE_PREFIX_PATH=/home/pi/catkin_ws/devel:/opt/ros/kinetic
PATH=/opt/ros/kinetic/bin:/usr/local/sbin:/usr/local/bin:/usr/sbin:/usr/bin:/sbin:/bin:/usr/games:/usr/local/games:/snap/bin
LD_LIBRARY_PATH=/opt/ros/kinetic/lib
PYTHONPATH=/home/pi/catkin_ws/devel/lib/python2.7/dist-packages:/opt/ros/kinetic/lib/python2.7/dist-packages
ROS_IP=192.168.11.1

11
deploy/roscore.service Normal file
View File

@@ -0,0 +1,11 @@
[Unit]
Description=Launcher for the ROS master, parameter server and rosout logging node
After=network.target
[Service]
EnvironmentFile=/home/pi/catkin_ws/src/clever/deploy/roscore.env
ExecStart=/opt/ros/kinetic/bin/roscore
Restart=on-abort
[Install]
WantedBy=multi-user.target