diff --git a/.gitignore b/.gitignore new file mode 100644 index 00000000..960e17c6 --- /dev/null +++ b/.gitignore @@ -0,0 +1,2 @@ +/deploy/ros_lib/ +*.pyc diff --git a/.gitmodules b/.gitmodules new file mode 100644 index 00000000..d78ff0b3 --- /dev/null +++ b/.gitmodules @@ -0,0 +1,4 @@ +[submodule "clever/static"] + path = clever/static + url = https://github.com/CopterExpress/clever-rc.git + branch = build diff --git a/aruco_pose/CMakeLists.txt b/aruco_pose/CMakeLists.txt new file mode 100644 index 00000000..90403cf5 --- /dev/null +++ b/aruco_pose/CMakeLists.txt @@ -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) diff --git a/aruco_pose/nodelet_plugins.xml b/aruco_pose/nodelet_plugins.xml new file mode 100644 index 00000000..79d5ddad --- /dev/null +++ b/aruco_pose/nodelet_plugins.xml @@ -0,0 +1,5 @@ + + + + + diff --git a/aruco_pose/package.xml b/aruco_pose/package.xml new file mode 100644 index 00000000..b4da81c3 --- /dev/null +++ b/aruco_pose/package.xml @@ -0,0 +1,62 @@ + + + aruco_pose + 0.0.0 + ArUco maps precise pose estimation nodelet + + + + + Oleg Kalachev + + + + + + TODO + + + + + + + + + + + + + + + + + + + + + + + + + + catkin + + nodelet + roscpp + image_transport + cv_bridge + tf + + nodelet + roscpp + image_transport + cv_bridge + tf + + + + + + + + diff --git a/aruco_pose/src/aruco_pose.cpp b/aruco_pose/src/aruco_pose.cpp new file mode 100644 index 00000000..d6f4853f --- /dev/null +++ b/aruco_pose/src/aruco_pose.cpp @@ -0,0 +1,292 @@ +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + +namespace aruco_pose { + +class ArucoPose : public nodelet::Nodelet { + tf::TransformBroadcaster br; + cv::Ptr dictionary; + cv::Ptr parameters; + cv::Ptr 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("pose", 1); + + ROS_INFO("aruco_pose nodelet inited"); +} + +cv::Ptr createCustomGridBoard(int markersX, int markersY, float markerLength, float markerSeparationX, float markerSeparationY, + const cv::Ptr &dictionary, std::vector ids) { + + CV_Assert(markersX > 0 && markersY > 0 && markerLength > 0 && markerSeparationX > 0 && markerSeparationY > 0); + + cv::Ptr res = cv::makePtr(); + + 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("map_image", 1, true); + cv_bridge::CvImage map_image_msg; + cv::Mat map_image; + + std::string type; + + nh_priv_.param("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 marker_ids; + nh_priv_.param("markers_x", markers_x, 10); + nh_priv_.param("markers_y", markers_y, 10); + nh_priv_.param("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::max(); + float max_x = std::numeric_limits::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(i, 0)); + max_y = std::max(max_y, objPoints.at(i, 1)); + min_x = std::min(min_x, objPoints.at(i, 0)); + min_y = std::min(min_y, objPoints.at(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 markerIds; + std::vector> markerCorners; + std::vector> 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(i, j) = cinfo->K[3 * i + j]; + } + } + for (int k = 0; k < cinfo->D.size(); k++) { + distCoeffs.at(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(0,0), rot.at(0,1), rot.at(0,2), + rot.at(1,0), rot.at(1,1), rot.at(1,2), + rot.at(2,0), rot.at(2,1), rot.at(2,2)); + tf::Vector3 tf_orig(tvec.at(0,0), tvec.at(1,0), tvec.at(2,0)); + return tf::Transform(tf_rot, tf_orig); +} + +PLUGINLIB_EXPORT_CLASS(ArucoPose, nodelet::Nodelet) + +} diff --git a/aruco_pose/src/fix.cpp b/aruco_pose/src/fix.cpp new file mode 100644 index 00000000..c15bf3b7 --- /dev/null +++ b/aruco_pose/src/fix.cpp @@ -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 &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 &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); + } +} diff --git a/clever/CMakeLists.txt b/clever/CMakeLists.txt new file mode 100644 index 00000000..a32e4da9 --- /dev/null +++ b/clever/CMakeLists.txt @@ -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) diff --git a/clever/camera_info/fisheye_cam_320.yaml b/clever/camera_info/fisheye_cam_320.yaml new file mode 100644 index 00000000..76fcd8c2 --- /dev/null +++ b/clever/camera_info/fisheye_cam_320.yaml @@ -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. diff --git a/clever/camera_info/fisheye_cam_640.yaml b/clever/camera_info/fisheye_cam_640.yaml new file mode 100644 index 00000000..f664bb7c --- /dev/null +++ b/clever/camera_info/fisheye_cam_640.yaml @@ -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. diff --git a/clever/launch/arduino.launch b/clever/launch/arduino.launch new file mode 100644 index 00000000..593f38d2 --- /dev/null +++ b/clever/launch/arduino.launch @@ -0,0 +1,9 @@ + + + + + + + + + diff --git a/clever/launch/aruco.launch b/clever/launch/aruco.launch new file mode 100644 index 00000000..f9dc88c5 --- /dev/null +++ b/clever/launch/aruco.launch @@ -0,0 +1,24 @@ + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/clever/launch/clever.launch b/clever/launch/clever.launch new file mode 100644 index 00000000..c89f32f1 --- /dev/null +++ b/clever/launch/clever.launch @@ -0,0 +1,58 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/clever/launch/copter_visualization.launch b/clever/launch/copter_visualization.launch new file mode 100644 index 00000000..24006baf --- /dev/null +++ b/clever/launch/copter_visualization.launch @@ -0,0 +1,12 @@ + + + + + + + + + + + + diff --git a/clever/launch/fpv_camera.launch b/clever/launch/fpv_camera.launch new file mode 100644 index 00000000..399d403d --- /dev/null +++ b/clever/launch/fpv_camera.launch @@ -0,0 +1,6 @@ + + + + + + diff --git a/clever/launch/main_camera.launch b/clever/launch/main_camera.launch new file mode 100644 index 00000000..480ba38f --- /dev/null +++ b/clever/launch/main_camera.launch @@ -0,0 +1,23 @@ + + + + + + + + + + + + + + + + + + + + + + + diff --git a/clever/launch/mavros.launch b/clever/launch/mavros.launch new file mode 100644 index 00000000..ae2a1c8c --- /dev/null +++ b/clever/launch/mavros.launch @@ -0,0 +1,53 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + - 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 + + + + + + diff --git a/clever/launch/sitl.launch b/clever/launch/sitl.launch new file mode 100644 index 00000000..1e48052f --- /dev/null +++ b/clever/launch/sitl.launch @@ -0,0 +1,17 @@ + + + + + + + + + + + + + + + + + diff --git a/clever/launch/web_server.launch b/clever/launch/web_server.launch new file mode 100644 index 00000000..9d8944ac --- /dev/null +++ b/clever/launch/web_server.launch @@ -0,0 +1,5 @@ + + + + + diff --git a/clever/nodelet_plugins.xml b/clever/nodelet_plugins.xml new file mode 100644 index 00000000..9e1c2c67 --- /dev/null +++ b/clever/nodelet_plugins.xml @@ -0,0 +1,10 @@ + + + + + + + + + + diff --git a/clever/package.xml b/clever/package.xml new file mode 100644 index 00000000..5759150f --- /dev/null +++ b/clever/package.xml @@ -0,0 +1,56 @@ + + + clever + 0.0.1 + The clever package + + + + + Oleg Kalachev + + + + + + TODO + + + + + + + + + + + + + + + + + + + + + + + + + + catkin + + nodelet + roscpp + + nodelet + roscpp + + + + + + + + diff --git a/clever/requirements.txt b/clever/requirements.txt new file mode 100644 index 00000000..6cb6a548 --- /dev/null +++ b/clever/requirements.txt @@ -0,0 +1,2 @@ +flask==0.12.2 +geopy==1.11.0 diff --git a/clever/src/aruco_vpe.cpp b/clever/src/aruco_vpe.cpp new file mode 100644 index 00000000..122297c1 --- /dev/null +++ b/clever/src/aruco_vpe.cpp @@ -0,0 +1,133 @@ +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + +#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("aruco_orientation", aruco_orientation_, "local_origin"); + bool use_mocap; + nh_priv.param("use_mocap", use_mocap, false); + nh_priv.param("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(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) diff --git a/clever/src/fcu_horiz.cpp b/clever/src/fcu_horiz.cpp new file mode 100644 index 00000000..aa17c1e3 --- /dev/null +++ b/clever/src/fcu_horiz.cpp @@ -0,0 +1,40 @@ +#include +#include +#include +#include +#include + +#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) diff --git a/clever/src/fpv_camera b/clever/src/fpv_camera new file mode 100755 index 00000000..7ca4981d --- /dev/null +++ b/clever/src/fpv_camera @@ -0,0 +1,8 @@ +#!/usr/bin/env bash + +# Usage +# fpv_camera + +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" diff --git a/clever/src/global_local.py b/clever/src/global_local.py new file mode 100644 index 00000000..a4f8f047 --- /dev/null +++ b/clever/src/global_local.py @@ -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 diff --git a/clever/src/simple_offboard.py b/clever/src/simple_offboard.py new file mode 100755 index 00000000..a84b9367 --- /dev/null +++ b/clever/src/simple_offboard.py @@ -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() diff --git a/clever/src/util.h b/clever/src/util.h new file mode 100644 index 00000000..0eb7f1e9 --- /dev/null +++ b/clever/src/util.h @@ -0,0 +1,17 @@ +#pragma once + +#include +#include + +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); +} diff --git a/clever/src/util.py b/clever/src/util.py new file mode 100644 index 00000000..eafa72fd --- /dev/null +++ b/clever/src/util.py @@ -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) diff --git a/clever/src/web_server.py b/clever/src/web_server.py new file mode 100755 index 00000000..86874760 --- /dev/null +++ b/clever/src/web_server.py @@ -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('/') +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) diff --git a/clever/srv/GetTelemetry.srv b/clever/srv/GetTelemetry.srv new file mode 100644 index 00000000..b815ce89 --- /dev/null +++ b/clever/srv/GetTelemetry.srv @@ -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 diff --git a/clever/srv/Navigate.srv b/clever/srv/Navigate.srv new file mode 100644 index 00000000..e368f284 --- /dev/null +++ b/clever/srv/Navigate.srv @@ -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 diff --git a/clever/srv/SetAttitude.srv b/clever/srv/SetAttitude.srv new file mode 100644 index 00000000..05ba13db --- /dev/null +++ b/clever/srv/SetAttitude.srv @@ -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 diff --git a/clever/srv/SetAttitudeYawRate.srv b/clever/srv/SetAttitudeYawRate.srv new file mode 100644 index 00000000..e914cf7e --- /dev/null +++ b/clever/srv/SetAttitudeYawRate.srv @@ -0,0 +1,8 @@ +float32 roll +float32 pitch +float32 yaw_rate +float32 thrust +bool auto_arm +--- +bool success +string message diff --git a/clever/srv/SetPosition.srv b/clever/srv/SetPosition.srv new file mode 100644 index 00000000..421c4931 --- /dev/null +++ b/clever/srv/SetPosition.srv @@ -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 diff --git a/clever/srv/SetPositionGlobal.srv b/clever/srv/SetPositionGlobal.srv new file mode 100644 index 00000000..742f4d95 --- /dev/null +++ b/clever/srv/SetPositionGlobal.srv @@ -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 diff --git a/clever/srv/SetPositionGlobalYawRate.srv b/clever/srv/SetPositionGlobalYawRate.srv new file mode 100644 index 00000000..3fc42931 --- /dev/null +++ b/clever/srv/SetPositionGlobalYawRate.srv @@ -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 diff --git a/clever/srv/SetPositionYawRate.srv b/clever/srv/SetPositionYawRate.srv new file mode 100644 index 00000000..419621ca --- /dev/null +++ b/clever/srv/SetPositionYawRate.srv @@ -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 diff --git a/clever/srv/SetRates.srv b/clever/srv/SetRates.srv new file mode 100644 index 00000000..f6ebddf9 --- /dev/null +++ b/clever/srv/SetRates.srv @@ -0,0 +1,8 @@ +float32 pitch_rate +float32 roll_rate +float32 yaw_rate +float32 thrust +bool auto_arm +--- +bool success +string message diff --git a/clever/srv/SetRatesYaw.srv b/clever/srv/SetRatesYaw.srv new file mode 100644 index 00000000..df8950ac --- /dev/null +++ b/clever/srv/SetRatesYaw.srv @@ -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 diff --git a/clever/srv/SetVelocity.srv b/clever/srv/SetVelocity.srv new file mode 100644 index 00000000..01b818b2 --- /dev/null +++ b/clever/srv/SetVelocity.srv @@ -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 diff --git a/clever/srv/SetVelocityYawRate.srv b/clever/srv/SetVelocityYawRate.srv new file mode 100644 index 00000000..3155644e --- /dev/null +++ b/clever/srv/SetVelocityYawRate.srv @@ -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 diff --git a/clever/static b/clever/static new file mode 160000 index 00000000..54c53043 --- /dev/null +++ b/clever/static @@ -0,0 +1 @@ +Subproject commit 54c530431f1060cf21ceeb8f38a0ab914e570d8b diff --git a/deploy/clever.service b/deploy/clever.service new file mode 100644 index 00000000..282ab0af --- /dev/null +++ b/deploy/clever.service @@ -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 diff --git a/deploy/clever_arudino.tar.gz b/deploy/clever_arudino.tar.gz new file mode 100644 index 00000000..df92e77a Binary files /dev/null and b/deploy/clever_arudino.tar.gz differ diff --git a/deploy/generate_ros_lib b/deploy/generate_ros_lib new file mode 100644 index 00000000..b72c5602 --- /dev/null +++ b/deploy/generate_ros_lib @@ -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 diff --git a/deploy/roscore.env b/deploy/roscore.env new file mode 100644 index 00000000..f794e05b --- /dev/null +++ b/deploy/roscore.env @@ -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 \ No newline at end of file diff --git a/deploy/roscore.service b/deploy/roscore.service new file mode 100644 index 00000000..56f949ac --- /dev/null +++ b/deploy/roscore.service @@ -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