diff --git a/Ethereum smart contracts/example/motherbase.sol b/Ethereum smart contracts/example/motherbase.sol new file mode 100644 index 0000000..6897e3b --- /dev/null +++ b/Ethereum smart contracts/example/motherbase.sol @@ -0,0 +1,34 @@ +contract motherbase { + address creator; + int256 public homebaseLongitude; + int256 public homebaseLatitude; + mapping (address => bool) droneActiveOf; + address public airwayAdminAddr; + + + modifier creatorCheck { if (msg.sender == creator) _ } + + /* Functions */ + function motherbase(int256 _homebaseLongitude, + int256 _homebaseLatitude, + address _airwayAdminAddr) { + creator = msg.sender; + homebaseLatitude = _homebaseLatitude; + homebaseLongitude = _homebaseLongitude; + airwayAdminAddr = _airwayAdminAddr; + } + + function setDrone(address _droneAddr) creatorCheck returns(bool result) { + droneActiveOf[_droneAddr] = true; + result true; + } + + function inactiveDrone(address _droneAddr) creatorCheck returns(bool result) { + droneActiveOf[_droneAddr] = false; + result true; + } + + + + +} \ No newline at end of file diff --git a/ROS/aira_ros_bridge/lib/aira_bridge.js b/ROS/aira_ros_bridge/lib/aira_bridge.js index ff4611e..5627cbd 100644 --- a/ROS/aira_ros_bridge/lib/aira_bridge.js +++ b/ROS/aira_ros_bridge/lib/aira_bridge.js @@ -10,8 +10,7 @@ var ROSMSG = require('./rosmsg'); var web3 = new Web3(); web3.setProvider(new web3.providers.HttpProvider('http://localhost:8545')); -var ros = new ROSLIB.Ros({url : 'ws://localhost:9090'}); -ros.on('connection', function() {console.log('Connected to websocket server.')}); +var ros; /*** Functions ***/ function topicOn(accessor, contract_abi, fun) { @@ -32,7 +31,11 @@ function topicOn(accessor, contract_abi, fun) { } } -function bridge(contract_address) { +function bridge(contract_address, port) { + /* Init ROS connection instance */ + ros = new ROSLIB.Ros({url : 'ws://localhost:'+port}); + ros.on('connection', function() {console.log('Connected to websocket server.')}); + /* Load publishers */ var ros_contract = web3.eth.contract(ABI.ros_compatible).at(contract_address); diff --git a/ROS/aira_ros_bridge/lib/msg/dron_common_msgs/RouteRequest.js b/ROS/aira_ros_bridge/lib/msg/dron_common_msgs/RouteRequest.js new file mode 100644 index 0000000..916844c --- /dev/null +++ b/ROS/aira_ros_bridge/lib/msg/dron_common_msgs/RouteRequest.js @@ -0,0 +1,51 @@ +// Init libs +var ROSLIB = require('roslib'); +var Helpers = require('../helpers'); +var SatFix = require('../dron_common_msgs/SatFix'); +// Solidity message definition +var bytecode = '606060405260405161016f38038061016f83398101604052805160805191019060605182516000805482825581805290927f290decd9548b62a8d60345a988386fc84ba6bc95484008f6362f93160ef3e563918201929190608001821560bc579160200282015b8281111560bc5782518254600160a060020a03191617825560209290920191600191909101906066565b505080600160006101000a81548163ffffffff021916908302179055505050608c806100e36000396000f35b5060909291505b8082111560df578054600160a060020a031916815560010160c3565b509056606060405260e060020a6000350463af640d0f81146024578063b8a24252146032575b005b608260015463ffffffff1681565b60826004356000805482908110156002575080527f290decd9548b62a8d60345a988386fc84ba6bc95484008f6362f93160ef3e563015473ffffffffffffffffffffffffffffffffffffffff1681565b6060908152602090f3'; +var abi = [{"constant":true,"inputs":[],"name":"id","outputs":[{"name":"","type":"uint32"}],"type":"function"},{"constant":true,"inputs":[{"name":"","type":"uint256"}],"name":"checkpoints","outputs":[{"name":"","type":"address"}],"type":"function"},{"inputs":[{"name":"_checkpoints","type":"address[]"},{"name":"_id","type":"uint32"}],"type":"constructor"}]; +// JSON message converter +function eth2json(address, web3) { + var msg = Helpers.getContract(abi, address, web3); + var route = []; + var ix = 0; + while (msg.checkpoints(ix) != "0x") + route.push(SatFix.eth2json(msg.checkpoints(ix++), web3)); + return { + // Message fields START + id: parseInt(msg.id()), + checkpoints: route + // Message fields END + } +} +function mkArray(arr, obj, web3, fn) { + var count = 0; + arr.forEach(function(e, i, a) { + obj.ros2eth(e, web3, function(err, r) { + arr[i] = r; + ++count; + }); + }); + while (count != arr.length); + return arr; +} +// Setup exports +module.exports = { +/* + * This message converter should be autogenerated from + * ROS message definition language. + * TODO: converter implementation. + */ + abi: abi, + eth2json: eth2json, + eth2ros: function(msg, web3) {return new ROSLIB.Message(eth2json(msg, web3))}, + ros2eth: function(msg, web3, fun) { + var id = msg.id; + + mkArray(msg.checkpoints, SatFix, web3, function(checkpoints) { + var args = [id, checkpoints]; + Helpers.newContract(abi, bytecode, web3, args, fun); + }); + } +} diff --git a/ROS/aira_ros_bridge/lib/msg/dron_common_msgs/RouteResponse.js b/ROS/aira_ros_bridge/lib/msg/dron_common_msgs/RouteResponse.js new file mode 100644 index 0000000..ac5724e --- /dev/null +++ b/ROS/aira_ros_bridge/lib/msg/dron_common_msgs/RouteResponse.js @@ -0,0 +1,62 @@ +// Init libs +var ROSLIB = require('roslib'); +var Helpers = require('../helpers'); +var SatFix = require('../dron_common_msgs/SatFix'); +// Solidity message definition +var bytecode = '606060405260405161019038038061019083398101604052805160805160a051919290910160a05160008054610100850264ffffffff001960ff1990921687179190911617815582516001805482825592819052927fb10e2d527612073b26eecdfd717e6a320cf44b4afac2b0732d9fcbe2b7fa0cf69283019290608001821560c2579160200282015b8281111560c25782518254600160a060020a03191617825560209290920191908401906089565b505050505060a8806100e86000396000f35b5060b09291505b8082111560e4578054600160a060020a0319168155830160c9565b509056606060405260e060020a6000350463af640d0f8114602e578063b980c78f146042578063c1991219146093575b005b609e60005463ffffffff6101009091041681565b609e600435600180548290811015600257506000527fb10e2d527612073b26eecdfd717e6a320cf44b4afac2b0732d9fcbe2b7fa0cf6015473ffffffffffffffffffffffffffffffffffffffff1681565b609e60005460ff1681565b6060908152602090f3'; +var abi = [{"constant":true,"inputs":[],"name":"id","outputs":[{"name":"","type":"uint32"}],"type":"function"},{"constant":true,"inputs":[{"name":"","type":"uint256"}],"name":"route","outputs":[{"name":"","type":"address"}],"type":"function"},{"constant":true,"inputs":[],"name":"valid","outputs":[{"name":"","type":"bool"}],"type":"function"},{"inputs":[{"name":"_valid","type":"bool"},{"name":"_id","type":"uint32"},{"name":"_route","type":"address[]"}],"type":"constructor"}]; +// JSON message converter +function eth2json(address, web3) { + var msg = Helpers.getContract(abi, address, web3); + var route = []; + var val = true; + var ix = 0; + while (msg.route(ix) != "0x") + route.push(SatFix.eth2json(msg.route(ix++), web3)); + if (msg.valid() == "False") + val = false; // TODO: check + return { + // Message fields START + valid: val, + id: parseInt(msg.id()), + route: route + // Message fields END + } +} +function mkArray(arr, obj, web3, fn) { + var count = 0; + arr.forEach(function(e, i, a) { + console.log("Make new message: " + i); + obj.ros2eth(e, web3, function(err, r) { + arr[i] = r; + ++count; + console.log("Message created: " + i); + }); + }); + setTimeout(function cb() { + if (count == arr.length) + fn(arr); + else + setTimeout(cb, 1000); + }, 1000); +} +// Setup exports +module.exports = { +/* + * This message converter should be autogenerated from + * ROS message definition language. + * TODO: converter implementation. + */ + abi: abi, + eth2json: eth2json, + eth2ros: function(msg, web3) {return new ROSLIB.Message(eth2json(msg, web3))}, + ros2eth: function(msg, web3, fun) { + var valid = msg.valid; + var id = msg.id; + + mkArray(msg.route, SatFix, web3, function(route) { + var args = [valid, id, route]; + Helpers.newContract(abi, bytecode, web3, args, fun); + }); + } +} diff --git a/ROS/aira_ros_bridge/lib/msg/dron_common_msgs/SatFix.js b/ROS/aira_ros_bridge/lib/msg/dron_common_msgs/SatFix.js new file mode 100644 index 0000000..f934930 --- /dev/null +++ b/ROS/aira_ros_bridge/lib/msg/dron_common_msgs/SatFix.js @@ -0,0 +1,35 @@ +// Init libs +var ROSLIB = require('roslib'); +var Helpers = require('../helpers'); +// Solidity message definition +var bytecode = '6060604052604051606080608683395060c06040525160805160a05160009283556001919091556002556050908190603690396000f3606060405260e060020a60003504634fd7d76a8114602e578063589af69c146036578063d906896a14603e575b005b604660005481565b604660015481565b604660025481565b6060908152602090f3'; +var abi = [{"constant":true,"inputs":[],"name":"latitude","outputs":[{"name":"","type":"int256"}],"type":"function"},{"constant":true,"inputs":[],"name":"longitude","outputs":[{"name":"","type":"int256"}],"type":"function"},{"constant":true,"inputs":[],"name":"altitude","outputs":[{"name":"","type":"int256"}],"type":"function"},{"inputs":[{"name":"_latitude","type":"int256"},{"name":"_longitude","type":"int256"},{"name":"_altitude","type":"int256"}],"type":"constructor"}]; +// JSON message converter +function eth2json(address, web3) { + var msg = Helpers.getContract(abi, address, web3); + return { + // Message fields START + latitude: parseInt(msg.latitude()) / 1000000.0, + longitude: parseInt(msg.longitude()) / 1000000.0, + altitude: parseInt(msg.altitude()) / 1000000.0 + // Message fields END + }; +} +// Setup exports +module.exports = { +/* + * This message converter should be autogenerated from + * ROS message definition language. + * TODO: converter implementation. + */ + abi: abi, + eth2json: eth2json, + eth2ros: function(address, web3) {return new ROSLIB.Message(eth2json(address, web3))}, + ros2eth: function(msg, web3, fun) { + var latitude = msg.latitude * 1000000; + var longitude = msg.longitude * 1000000; + var altitude = msg.altitude * 1000000; + var args = [latitude, longitude, altitude]; + Helpers.newContract(abi, bytecode, web3, args, fun); +} +} diff --git a/ROS/aira_ros_bridge/lib/msg/std_msgs/Int64.js b/ROS/aira_ros_bridge/lib/msg/std_msgs/Int64.js index 54da837..a0dfd96 100644 --- a/ROS/aira_ros_bridge/lib/msg/std_msgs/Int64.js +++ b/ROS/aira_ros_bridge/lib/msg/std_msgs/Int64.js @@ -4,6 +4,11 @@ var Helpers = require('../helpers'); // Solidity data var bytecode = '60606040526040516020806085833950608060405251600080547801000000000000000000000000000000000000000000000000808402046001604060020a03199091161790555060328060536000396000f3606060405260e060020a600035046373d4a13a8114601a575b005b602560005460070b81565b60070b6060908152602090f3'; var abi = [{"constant":true,"inputs":[],"name":"data","outputs":[{"name":"","type":"int64"}],"type":"function"},{"inputs":[{"name":"_data","type":"int64"}],"type":"constructor"}]; +// JSON serialization +function eth2json(address, web3) { + var msg = Helpers.getContract(abi, address, web3); + return {data: parseInt(msg.data())} +} // Setup exports module.exports = { /* @@ -12,7 +17,8 @@ module.exports = { * TODO: converter implementation. */ abi: abi, - eth2ros: function(msg) {return new ROSLIB.Message({data: parseInt(msg.data())})}, + eth2json: eth2json, + eth2ros: function(msg, web3) {return new ROSLIB.Message(eth2json(msg, web3))}, ros2eth: function(msg, web3, fun) { // var msg_data = msg.data; diff --git a/ROS/aira_ros_bridge/lib/msg/std_msgs/String.js b/ROS/aira_ros_bridge/lib/msg/std_msgs/String.js index 54cfbd2..cfca964 100644 --- a/ROS/aira_ros_bridge/lib/msg/std_msgs/String.js +++ b/ROS/aira_ros_bridge/lib/msg/std_msgs/String.js @@ -4,6 +4,11 @@ var Helpers = require('../helpers'); // Solidity data var bytecode = '60606040526040516101d53803806101d5833981016040528051018060006000509080519060200190828054600181600116156101000203166002900490600052602060002090601f016020900481019282601f10608d57805160ff19168380011785555b50607c9291505b8082111560ba57838155600101606b565b505050610117806100be6000396000f35b828001600101855582156064579182015b828111156064578251826000505591602001919060010190609e565b509056606060405260e060020a600035046373d4a13a811461001b575b005b6100766000805460806020601f6002600019610100600187161502019094169390930492830181900402810160405260608281529291908282801561010f5780601f106100e45761010080835404028352916020019161010f565b60405180806020018281038252838181518152602001915080519060200190808383829060006004602084601f0104600f02600301f150905090810190601f1680156100d65780820380516001836020036101000a031916815260200191505b509250505060405180910390f35b820191906000526020600020905b8154815290600101906020018083116100f257829003601f168201915b50505050508156'; var abi = [{"constant":true,"inputs":[],"name":"data","outputs":[{"name":"","type":"string"}],"type":"function"},{"inputs":[{"name":"_data","type":"string"}],"type":"constructor"}]; +function eth2json(address, web3) { + var msg = Helpers.getContract(abi, address, web3); + return {data: msg.data()} +} + // Setup exports module.exports = { /* @@ -12,7 +17,8 @@ module.exports = { * TODO: converter implementation. */ abi: abi, - eth2ros: function(msg) {return new ROSLIB.Message({data: msg.data()})}, + eth2json: eth2json, + eth2ros: function(msg, web3) {return new ROSLIB.Message(eth2json(msg, web3))}, ros2eth: function(msg, web3, fun) { var args = [msg.data]; Helpers.newContract(abi, bytecode, web3, args, fun); diff --git a/ROS/aira_ros_bridge/lib/msg/std_msgs/UInt32.js b/ROS/aira_ros_bridge/lib/msg/std_msgs/UInt32.js new file mode 100644 index 0000000..588c4b4 --- /dev/null +++ b/ROS/aira_ros_bridge/lib/msg/std_msgs/UInt32.js @@ -0,0 +1,29 @@ +// Init libs +var ROSLIB = require('roslib'); +var Helpers = require('../helpers'); +// Solidity data +var bytecode = '606060405260405160208060638339506080604052516000805463ffffffff1916821790555060328060316000396000f3606060405260e060020a600035046373d4a13a8114601a575b005b602860005463ffffffff1681565b6060908152602090f3'; +var abi = [{"constant":true,"inputs":[],"name":"data","outputs":[{"name":"","type":"uint32"}],"type":"function"},{"inputs":[{"name":"_data","type":"uint32"}],"type":"constructor"}]; +// JSON serialization +function eth2json(address, web3) { + var msg = Helpers.getContract(abi, address, web3); + return {data: parseInt(msg.data())} +} +// Setup exports +module.exports = { +/* + * This message converter should be autogenerated from + * ROS message definition language. + * TODO: converter implementation. + */ + abi: abi, + eth2json: eth2json, + eth2ros: function(msg, web3) {return new ROSLIB.Message(eth2json(msg, web3))}, + ros2eth: function(msg, web3, fun) { + // + var msg_data = msg.data; + var args = [msg_data]; + // + Helpers.newContract(abi, bytecode, web3, args, fun); +} +} diff --git a/ROS/aira_ros_bridge/start.js b/ROS/aira_ros_bridge/start.js index e6ecc97..617d962 100644 --- a/ROS/aira_ros_bridge/start.js +++ b/ROS/aira_ros_bridge/start.js @@ -1,4 +1,4 @@ if (process.argv.length < 3) - console.log("Usage: " + process.argv[0] + " " + process.argv[1] + " ADDRESS"); + console.log("Usage: " + process.argv[0] + " " + process.argv[1] + " CONTRACT_ADDRESS BRIDGE_PORT"); else - require('./lib/aira_bridge')(process.argv[2]) + require('./lib/aira_bridge')(process.argv[2], process.argv[3]) diff --git a/ROS/aira_ros_messages/CMakeLists.txt b/ROS/aira_ros_messages/CMakeLists.txt new file mode 100644 index 0000000..26b6664 --- /dev/null +++ b/ROS/aira_ros_messages/CMakeLists.txt @@ -0,0 +1,190 @@ +cmake_minimum_required(VERSION 2.8.3) +project(aira_ros_messages) + +## 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 genmsg) + +## System dependencies are found with CMake's conventions +# find_package(Boost REQUIRED COMPONENTS system) + + +## Uncomment this if the package has a setup.py. This macro ensures +## modules and global scripts declared therein get installed +## See http://ros.org/doc/api/catkin/html/user_guide/setup_dot_py.html +catkin_python_setup() + +################################################ +## Declare ROS messages, services and actions ## +################################################ + +## To declare and build messages, services or actions from within this +## package, follow these steps: +## * Let MSG_DEP_SET be the set of packages whose message types you use in +## your messages/services/actions (e.g. std_msgs, actionlib_msgs, ...). +## * In the file package.xml: +## * add a build_depend tag for "message_generation" +## * add a build_depend and a run_depend tag for each package in MSG_DEP_SET +## * If MSG_DEP_SET isn't empty the following dependency has been pulled in +## but can be declared for certainty nonetheless: +## * add a run_depend tag for "message_runtime" +## * In this file (CMakeLists.txt): +## * add "message_generation" and every package in MSG_DEP_SET to +## find_package(catkin REQUIRED COMPONENTS ...) +## * add "message_runtime" and every package in MSG_DEP_SET to +## catkin_package(CATKIN_DEPENDS ...) +## * uncomment the add_*_files sections below as needed +## and list every .msg/.srv/.action file to be processed +## * uncomment the generate_messages entry below +## * add every package in MSG_DEP_SET to generate_messages(DEPENDENCIES ...) + +## Generate messages in the 'msg' folder +# add_message_files( +# FILES +# Message1.msg +# Message2.msg +# ) + +## Generate services in the 'srv' folder +# add_service_files( +# FILES +# Service1.srv +# Service2.srv +# ) + +## Generate actions in the 'action' folder +# add_action_files( +# FILES +# Action1.action +# Action2.action +# ) + +## Generate added messages and services with any dependencies listed here +# generate_messages( +# DEPENDENCIES +# 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 aira_ros_messages + CATKIN_DEPENDS genmsg + CFG_EXTRAS gensol-extras.cmake +) + +########### +## Build ## +########### + +## Specify additional locations of header files +## Your package locations should be listed before other locations +# include_directories(include) +include_directories( + ${catkin_INCLUDE_DIRS} +) + +## Declare a C++ library +# add_library(aira_ros_messages +# src/${PROJECT_NAME}/aira_ros_messages.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(aira_ros_messages ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS}) + +## Declare a C++ executable +# add_executable(aira_ros_messages_node src/aira_ros_messages_node.cpp) + +## Add cmake target dependencies of the executable +## same as for the library above +# add_dependencies(aira_ros_messages_node ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS}) + +## Specify libraries to link a library or executable target against +# target_link_libraries(aira_ros_messages_node +# ${catkin_LIBRARIES} +# ) + +############# +## Install ## +############# + +# all install targets should use catkin DESTINATION variables +# See http://ros.org/doc/api/catkin/html/adv_user_guide/variables.html + +file(WRITE ${CATKIN_DEVEL_PREFIX}/${GENMSG_LANGS_DESTINATION}/gensol "Solidity") +install(FILES ${CATKIN_DEVEL_PREFIX}/${GENMSG_LANGS_DESTINATION}/gensol + DESTINATION ${GENMSG_LANGS_DESTINATION}) + +add_subdirectory(scripts) + +## 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 aira_ros_messages aira_ros_messages_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_aira_ros_messages.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/ROS/aira_ros_messages/cmake/gensol-extras.cmake.em b/ROS/aira_ros_messages/cmake/gensol-extras.cmake.em new file mode 100644 index 0000000..fb60383 --- /dev/null +++ b/ROS/aira_ros_messages/cmake/gensol-extras.cmake.em @@ -0,0 +1,44 @@ +@[if DEVELSPACE]@ +# bin and template dir variables in develspace +set(GENSOL_BIN "@(CMAKE_CURRENT_SOURCE_DIR)/scripts/gen_sol.py") +set(GENSOL_TEMPLATE_DIR "@(CMAKE_CURRENT_SOURCE_DIR)/scripts") +@[else]@ +# bin and template dir variables in installspace +set(GENSOL_BIN "${gensol_DIR}/../../../@(CATKIN_PACKAGE_BIN_DESTINATION)/gen_sol.py") +set(GENSOL_TEMPLATE_DIR "${gensol_DIR}/..") +@[end if]@ + +# Generate .msg->.sol for Solidity +# The generated .sol files should be added ALL_GEN_OUTPUT_FILES_sol +macro(_generate_msg_sol ARG_PKG ARG_MSG ARG_IFLAGS ARG_MSG_DEPS ARG_GEN_OUTPUT_DIR) + file(MAKE_DIRECTORY ${ARG_GEN_OUTPUT_DIR}) + + #Create input and output filenames + get_filename_component(MSG_NAME ${ARG_MSG} NAME) + get_filename_component(MSG_SHORT_NAME ${ARG_MSG} NAME_WE) + + set(MSG_GENERATED_NAME ${MSG_SHORT_NAME}.sol) + set(GEN_OUTPUT_FILE ${ARG_GEN_OUTPUT_DIR}/${MSG_GENERATED_NAME}) + + assert(CATKIN_ENV) + add_custom_command(OUTPUT ${GEN_OUTPUT_FILE} + DEPENDS ${GENSOL_BIN} ${ARG_MSG} ${ARG_MSG_DEPS} "${GENSOL_TEMPLATE_DIR}/msg.sol.template" ${ARGN} + COMMAND ${CATKIN_ENV} ${PYTHON_EXECUTABLE} ${GENSOL_BIN} ${ARG_MSG} + ${ARG_IFLAGS} + -p ${ARG_PKG} + -o ${ARG_GEN_OUTPUT_DIR} + -e ${GENSOL_TEMPLATE_DIR} + COMMENT "Generating Solidity code from ${ARG_PKG}/${MSG_NAME}" + ) + list(APPEND ALL_GEN_OUTPUT_FILES_sol ${GEN_OUTPUT_FILE}) + + gencpp_append_include_dirs() +endmacro() + +#gensol uses the same program to generate srv and msg files, so call the same macro +macro(_generate_srv_sol ARG_PKG ARG_SRV ARG_IFLAGS ARG_MSG_DEPS ARG_GEN_OUTPUT_DIR) +endmacro() + +macro(_generate_module_sol) + # the macros, they do nothing +endmacro() diff --git a/ROS/aira_ros_messages/package.xml b/ROS/aira_ros_messages/package.xml new file mode 100644 index 0000000..52480f8 --- /dev/null +++ b/ROS/aira_ros_messages/package.xml @@ -0,0 +1,52 @@ + + + aira_ros_messages + 0.1.0 + The aira_ros_messages package + + + + + Alexander Krupenkin + + + + + + MIT + + + + + + + + + + + + + + + + + + + + + + + + + + catkin + genmsg + genmsg + + + + + + + + diff --git a/ROS/aira_ros_messages/scripts/CMakeLists.txt b/ROS/aira_ros_messages/scripts/CMakeLists.txt new file mode 100644 index 0000000..faef25d --- /dev/null +++ b/ROS/aira_ros_messages/scripts/CMakeLists.txt @@ -0,0 +1,7 @@ +install( + FILES msg.h.template + DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}) + +catkin_install_python( + PROGRAMS gen_sol.py + DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION}) diff --git a/ROS/aira_ros_messages/scripts/gen_sol.py b/ROS/aira_ros_messages/scripts/gen_sol.py new file mode 100755 index 0000000..9110d45 --- /dev/null +++ b/ROS/aira_ros_messages/scripts/gen_sol.py @@ -0,0 +1,14 @@ +#!/usr/bin/env python + +## ROS message source code generation for Solidity +## +## Converts ROS .msg files in a package into Solidity source code implementations. + +import genmsg.template_tools +import sys + +msg_template_map = { 'msg.sol.template':'@NAME@.sol' } + +if __name__ == "__main__": + genmsg.template_tools.generate_from_command_line_options(sys.argv, + msg_template_map, None) diff --git a/ROS/aira_ros_messages/scripts/msg.sol.template b/ROS/aira_ros_messages/scripts/msg.sol.template new file mode 100644 index 0000000..c655f45 --- /dev/null +++ b/ROS/aira_ros_messages/scripts/msg.sol.template @@ -0,0 +1,62 @@ +@############################################### +@# +@# ROS message source code gen for Solidity +@# +@# EmPy template for generating .sol files +@# +@############################################### +@# Start of Template +@# +@# Context: +@# - file_name_in (String) Source file +@# - spec (msggen.MsgSpec) Parsed specification of the .msg file +@# - md5sum (String) MD5Sum of the .msg specification +@############################################### +// Generated by gensol from file @(spec.package)/@(spec.short_name).msg +// DO NOT EDIT! + +@{ +import genmsg.msgs +import gensol +}@ + +@############################## +@# Generic Includes +@############################## +import 'ROS'; +@############################## +@# Includes for dependencies +@############################## +@{ +for field in spec.parsed_fields(): + if (not field.is_builtin): + if (field.is_header): + print('import \'std_msgs_Header\'') + else: + (package, name) = genmsg.names.package_resource_name(field.base_type) + package = package or spec.package # convert '' to package + print('import \'%s_%s\''%(package, name)) +}@ + +contract @(spec.short_name) is Message { + /*** Message fields ***/ +@[for field in spec.parsed_fields()] + @(gensol.msg_type_to_sol(field.type)) public @(field.name); +@[end for] + + /*** Constants ***/ +@[for constant in spec.constants]@ + @(gensol.msg_type_to_sol(constant.type)) constant @(constant.name); +@[end for] + + /*** Constructor ***/ + function @(spec.short_name) ( +@[for field in spec.parsed_fields()] + @(gensol.msg_type_to_sol(field.type)) _@(field.name), +@[end for] + ) { +@[for field in spec.parsed_fields()] + @(field.name) = _@(field.name); +@[end for] + } +} diff --git a/ROS/aira_ros_messages/setup.py b/ROS/aira_ros_messages/setup.py new file mode 100755 index 0000000..98f68cf --- /dev/null +++ b/ROS/aira_ros_messages/setup.py @@ -0,0 +1,12 @@ +#!/usr/bin/env python + +from distutils.core import setup +from catkin_pkg.python_setup import generate_distutils_setup + +d = generate_distutils_setup( + packages=['gensol'], + package_dir={'': 'src'}, + requires=['genmsg'] +) + +setup(**d) diff --git a/ROS/aira_ros_messages/src/gensol/__init__.py b/ROS/aira_ros_messages/src/gensol/__init__.py new file mode 100644 index 0000000..dea4ad6 --- /dev/null +++ b/ROS/aira_ros_messages/src/gensol/__init__.py @@ -0,0 +1,45 @@ +import genmsg.msgs + +try: + from cStringIO import StringIO #Python 2.x +except ImportError: + from io import StringIO #Python 3.x + +MSG_TYPE_TO_SOL = {'byte': 'int8', + 'char': 'uint8', + 'bool': 'bool', + 'uint8': 'uint8', + 'int8': 'int8', + 'uint16': 'uint16', + 'int16': 'int16', + 'uint32': 'uint32', + 'int32': 'int32', + 'uint64': 'uint64', + 'int64': 'int64', + 'float32': 'int128', + 'float64': 'int256', + 'string': 'string', + 'time': 'uint', + 'duration': 'uint'} + +def msg_type_to_sol(t): + (base_type, is_array, array_len) = genmsg.msgs.parse_type(t) + + sol_type = None + if genmsg.msgs.is_builtin(base_type): + sol_type = MSG_TYPE_TO_SOL[base_type] + elif (len(base_type.split('/')) == 1): + if (genmsg.msgs.is_header_type(base_type)): + sol_type = 'std_msgs_Header' + else: + pkg = base_type.split('/')[0] + msg = base_type.split('/')[1] + sol_type = '%s_%s'%(pkg, msg) + + if (is_array): + if (array_len is None): + return '%s[]'%sol_type + else: + print('Multidimentional arrays currently does not supported by generator!') + return None + return sol_type diff --git a/ROS/dron_common_msgs/CMakeLists.txt b/ROS/dron_common_msgs/CMakeLists.txt new file mode 100644 index 0000000..629763e --- /dev/null +++ b/ROS/dron_common_msgs/CMakeLists.txt @@ -0,0 +1,218 @@ +cmake_minimum_required(VERSION 2.8.3) +project(dron_common_msgs) + +## 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 + message_generation + std_msgs + mavros_msgs + sensor_msgs + 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( + DIRECTORY msg + FILES + SatFix.msg + PathEstimate.msg + PathCost.msg + RouteRequest.msg + RouteResponse.msg + LocalRouteRequest.msg + LocalRouteResponse.msg +) + +## Generate added messages and services with any dependencies listed here +generate_messages( + DEPENDENCIES + std_msgs + sensor_msgs + mavros_msgs + geometry_msgs +) + +## Generate messages in the 'msg' folder +# add_message_files( +# FILES +# Message1.msg +# Message2.msg +# ) + +## Generate services in the 'srv' folder +# add_service_files( +# FILES +# Service1.srv +# Service2.srv +# ) + +## Generate actions in the 'action' folder +# add_action_files( +# FILES +# Action1.action +# Action2.action +# ) + +## Generate added messages and services with any dependencies listed here +# generate_messages( +# DEPENDENCIES +# std_msgs +# ) + +################################################ +## Declare ROS dynamic reconfigure parameters ## +################################################ + +## To declare and build dynamic reconfigure parameters within this +## package, follow these steps: +## * In the file package.xml: +## * add a build_depend and a run_depend tag for "dynamic_reconfigure" +## * In this file (CMakeLists.txt): +## * add "dynamic_reconfigure" to +## find_package(catkin REQUIRED COMPONENTS ...) +## * uncomment the "generate_dynamic_reconfigure_options" section below +## and list every .cfg file to be processed + +## Generate dynamic reconfigure parameters in the 'cfg' folder +# generate_dynamic_reconfigure_options( +# cfg/DynReconf1.cfg +# cfg/DynReconf2.cfg +# ) + +################################### +## catkin specific configuration ## +################################### +## The catkin_package macro generates cmake config files for your package +## Declare things to be passed to dependent projects +## INCLUDE_DIRS: uncomment this if you package contains header files +## LIBRARIES: libraries you create in this project that dependent projects also need +## CATKIN_DEPENDS: catkin_packages dependent projects also need +## DEPENDS: system dependencies of this project that dependent projects also need +catkin_package( +# INCLUDE_DIRS include +# LIBRARIES dron_common_msgs +# CATKIN_DEPENDS message_generation message_runtime std_msgs +# DEPENDS system_lib + CATKIN_DEPENDS + message_runtime + std_msgs + sensor_msgs + mavros_msgs + geometry_msgs +) + +########### +## Build ## +########### + +## Specify additional locations of header files +## Your package locations should be listed before other locations +# include_directories(include) +include_directories( + ${catkin_INCLUDE_DIRS} +) + +## Declare a C++ library +# add_library(dron_common_msgs +# src/${PROJECT_NAME}/dron_common_msgs.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(dron_common_msgs ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS}) + +## Declare a C++ executable +# add_executable(dron_common_msgs_node src/dron_common_msgs_node.cpp) + +## Add cmake target dependencies of the executable +## same as for the library above +# add_dependencies(dron_common_msgs_node ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS}) + +## Specify libraries to link a library or executable target against +# target_link_libraries(dron_common_msgs_node +# ${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 dron_common_msgs dron_common_msgs_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_dron_common_msgs.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/ROS/dron_common_msgs/msg/LocalRouteRequest.msg b/ROS/dron_common_msgs/msg/LocalRouteRequest.msg new file mode 100644 index 0000000..da1be70 --- /dev/null +++ b/ROS/dron_common_msgs/msg/LocalRouteRequest.msg @@ -0,0 +1,2 @@ +uint32 id +geometry_msgs/Point[] checkpoints diff --git a/ROS/dron_common_msgs/msg/LocalRouteResponse.msg b/ROS/dron_common_msgs/msg/LocalRouteResponse.msg new file mode 100644 index 0000000..d33fff8 --- /dev/null +++ b/ROS/dron_common_msgs/msg/LocalRouteResponse.msg @@ -0,0 +1,3 @@ +bool valid +uint32 id +geometry_msgs/Point[] route diff --git a/ROS/dron_ros_tutorial/msg/PathCost.msg b/ROS/dron_common_msgs/msg/PathCost.msg similarity index 100% rename from ROS/dron_ros_tutorial/msg/PathCost.msg rename to ROS/dron_common_msgs/msg/PathCost.msg diff --git a/ROS/dron_ros_tutorial/msg/PathEstimate.msg b/ROS/dron_common_msgs/msg/PathEstimate.msg similarity index 71% rename from ROS/dron_ros_tutorial/msg/PathEstimate.msg rename to ROS/dron_common_msgs/msg/PathEstimate.msg index e373933..68bfa63 100644 --- a/ROS/dron_ros_tutorial/msg/PathEstimate.msg +++ b/ROS/dron_common_msgs/msg/PathEstimate.msg @@ -4,7 +4,7 @@ uint16 ident # Dron base -SatPosition base +SatFix base # Dron destination -SatPosition destination +SatFix destination diff --git a/ROS/dron_common_msgs/msg/RouteRequest.msg b/ROS/dron_common_msgs/msg/RouteRequest.msg new file mode 100644 index 0000000..f9ca32a --- /dev/null +++ b/ROS/dron_common_msgs/msg/RouteRequest.msg @@ -0,0 +1,2 @@ +uint32 id +SatFix[] checkpoints diff --git a/ROS/dron_common_msgs/msg/RouteResponse.msg b/ROS/dron_common_msgs/msg/RouteResponse.msg new file mode 100644 index 0000000..ee03256 --- /dev/null +++ b/ROS/dron_common_msgs/msg/RouteResponse.msg @@ -0,0 +1,3 @@ +bool valid +uint32 id +SatFix[] route diff --git a/ROS/dron_ros_tutorial/msg/SatPosition.msg b/ROS/dron_common_msgs/msg/SatFix.msg similarity index 64% rename from ROS/dron_ros_tutorial/msg/SatPosition.msg rename to ROS/dron_common_msgs/msg/SatFix.msg index a275f5f..af0e45d 100644 --- a/ROS/dron_ros_tutorial/msg/SatPosition.msg +++ b/ROS/dron_common_msgs/msg/SatFix.msg @@ -2,6 +2,8 @@ # Latitude [degrees]. Positive is north of equator; negative is south. float64 latitude - # Longitude [degrees]. Positive is east of prime meridian; negative is west. -float64 longitude \ No newline at end of file +float64 longitude +# Altitude [m]. Positive is above the WGS 84 ellipsoid +# (quiet NaN if no altitude is available). +float64 altitude diff --git a/ROS/dron_common_msgs/package.xml b/ROS/dron_common_msgs/package.xml new file mode 100644 index 0000000..0cf569f --- /dev/null +++ b/ROS/dron_common_msgs/package.xml @@ -0,0 +1,60 @@ + + + dron_common_msgs + 0.0.0 + The common messages for AIRA ROS drones + + + + + Alexander Krupenkin + + + + + + GPLv3 + + + + + + + + + + + + + + + + + + + + + + + + + + catkin + message_generation + std_msgs + sensor_msgs + mavros_msgs + geometry_msgs + message_runtime + std_msgs + sensor_msgs + mavros_msgs + geometry_msgs + + + + + + + + diff --git a/ROS/dron_ros_tutorial/CMakeLists.txt b/ROS/dron_ros_tutorial/CMakeLists.txt index 4ba1d5c..de792ae 100644 --- a/ROS/dron_ros_tutorial/CMakeLists.txt +++ b/ROS/dron_ros_tutorial/CMakeLists.txt @@ -7,9 +7,8 @@ project(dron_ros_tutorial) find_package(catkin REQUIRED COMPONENTS rospy std_msgs - sensor_msgs mavros_msgs - message_generation + dron_common_msgs ) ## System dependencies are found with CMake's conventions @@ -45,23 +44,6 @@ find_package(catkin REQUIRED COMPONENTS ## * 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( - DIRECTORY msg - FILES - SatPosition.msg - PathEstimate.msg - PathCost.msg -) - -## Generate added messages and services with any dependencies listed here -generate_messages( - DEPENDENCIES - std_msgs - sensor_msgs - mavros_msgs -) - ################################################ ## Declare ROS dynamic reconfigure parameters ## ################################################ @@ -96,7 +78,7 @@ catkin_package( # LIBRARIES dron_employee_ros # CATKIN_DEPENDS std_msgs # DEPENDS system_lib - CATKIN_DEPENDS message_runtime std_msgs sensor_msgs mavros_msgs + CATKIN_DEPENDS dron_common_msgs mavros_msgs std_msgs ) ########### diff --git a/ROS/dron_ros_tutorial/wiki/Simulation.md b/ROS/dron_ros_tutorial/doc/Simulation.md similarity index 85% rename from ROS/dron_ros_tutorial/wiki/Simulation.md rename to ROS/dron_ros_tutorial/doc/Simulation.md index d7eb5ed..f9d79dc 100644 --- a/ROS/dron_ros_tutorial/wiki/Simulation.md +++ b/ROS/dron_ros_tutorial/doc/Simulation.md @@ -3,13 +3,12 @@ ## Install ROS Install `ros-indigo-desktop-full` for your distro. -Install `ros-indigo-control_toolbox` for your distro. -## Install SITL Enviroment +## Install SITL enviroment ### MAVLink & Ardupilot ROS node install -Run `aira_iot/tools/mavlink_build_ws.sh mav_ws`, +Run `dron_ros_tutorial/tools/mavlink_build_ws.sh mav_ws`, this script build MAVLink catkin workspace for the ROS. ### ArduCopter SITL diff --git a/ROS/dron_ros_tutorial/doc/Tutorial.md b/ROS/dron_ros_tutorial/doc/Tutorial.md new file mode 100644 index 0000000..0e64583 --- /dev/null +++ b/ROS/dron_ros_tutorial/doc/Tutorial.md @@ -0,0 +1,456 @@ +# AIRA IoT ROS Tutorials + +## Simple publisher + +This example makes only one `Publisher` object that provide ability to +publish message to ROS topic. + +### Solidity contract + +See the [example contract](https://raw.githubusercontent.com/aira-dao/aira-IoT/master/Ethereum%20smart%20contracts/example/simple_publisher.sol): + +```JavaScript +import 'ROS'; +``` + +Importing the basic ROS integration contract library. +`ROS` library defines some structures as `Publisher`, `Subscriber`, `Message` and etc. + +```JavaScript +contract StdString is Message { + string public data; + function StdString(string _data) { + data = _data; + } +} +``` + +Message definition contract cotaints only one member `data` with simple type `string`. +ROS messages will be converted to `Message` contract and submitted by AIRA ROS Bridge, +or `Message` contracts will be converted to ROS message and published to associated topics. + +```JavaScript +contract SimplePub is ROSCompatible { +``` + +The next is a main contract definition, this contract should be child of `ROSCompatible` contract. +The `ROSCompatible` contract contrains service members that used by AIRA ROS Bridge. + +```JavaScript + Publisher myPub; + function SampleContract() { + // Create a new publisher by topic name and type + myPub = mkPublisher('/hello', 'std_msgs/String'); + } +``` + +In this code contract constructor creates `Publisher` by `ROSCompatible` public +method `mkPublisher` and arguments *topic name* and *message type*. `Publisher` +instance saved in private member for the future. + +```JavaScript + function echo(string str) { + // Publish new message by received string + myPub.publish(new StdString(str)); + } +} +``` + +The last method accepts `string` argument, packs this into `StdString` message container, +and call `publish` method of `Publisher` instance that publish ROS message to +associated topic. + +### ROS interaction + +The first, [create](http://chriseth.github.io/browser-solidity/) Web3 deploy instructions +for the [example contract](https://raw.githubusercontent.com/aira-dao/aira-IoT/master/Ethereum%20smart%20contracts/example/simple_publisher.sol). +And put into the `geth console` window. After some time you see: + + Contract mined! address: 0xc52db6edc294ecf434a322b93e09d87d1aae80ae transactionHash: 0x4882579adef61f44fcef5847e93a5c998c81c30c9f7345f5c48e0f613a6e4bac + +With contract address `0xc52db6edc294ecf434a322b93e09d87d1aae80ae` +you can start AIRA ROS Bridge now, full start instructions is [there](https://github.com/aira-dao/aira-IoT/tree/master/ROS/aira_ros_bridge): + +```bash +$ node start.js 0xc52db6edc294ecf434a322b93e09d87d1aae80ae +util.debug: Use console.error instead +DEBUG: ROSLib uses utf8 encoding by default.It would be more efficent to use ascii (if possible) +Contract: 0xc52db6edc294ecf434a322b93e09d87d1aae80ae +Publishers: +/hello :: std_msgs/String +Subscribers: +``` + +Described on the top contract have a simple ROS interface: + +Publishers: + +* `/hello` :: **std_msgs/String** + +Using `rostopic` tools try to subscribe to `/hello` topic: + +```bash +$ rostopic echo /hello +``` + +This tool should print received strings to screen. +Try to publish string by `sendTransaction` method in `geth console`: + +```JavaScript +> var contract = eth.contract(simple_publisher_abi).at("0xc52db6edc294ecf434a322b93e09d87d1aae80ae") +> contract.echo.sendTransaction("Hello world!", {from: eth.accounts[0], gas: 500000}) +``` + +After some time `rostopic` console show: + +```bash +WARNING: topic [/hello] does not appear to be published yet +data: Hello world! +--- +``` + +## Publisher/Subscriber combination + +The next example combine `Subscriber` with `Publisher` and can +subscribe to topics and pubish new messages too. + +Task is write simple integrator with ROS interface: + +* `/add` :: **std_msgs/Int64** - Input integer value +* `/value` :: **std_msgs/Int64** - Sum of all input values + +### Solidity contract + +See the [example](https://raw.githubusercontent.com/aira-dao/aira-IoT/master/Ethereum%20smart%20contracts/example/pubsub.sol): + +```JavaScript +contract Integrator is MessageHandler { + Publisher myPub; + int64 value; +``` + +Contract `Integrator` can handle incoming messages and have two private members: +`Publisher` and `int64`. `value` member used for storing current value and +publish into `myPub`. + +```JavaScript + function Integrator(Publisher _pub) { + myPub = _pub; + value = 0; + } +``` + +Constructor just copy publisher from arguments and init `value` by zero. + +```JavaScript + function incomingMessage(Message _msg) { + value += MsgInt64(_msg).data(); + myPub.publish(new MsgInt64(value)); + } +} +``` +This method of `MessageHandler` interface takes `Message` argument. +From incoming message taked `data` and added to `value` member. +The next `myPub` publish `value` wrapped by `MsgInt64` object. + +```JavaScript +contract PubSub is ROSCompatible { + function PubSub() { + var pub = mkPublisher("/value", "std_msgs/Int64"); + var hdl = new Integrator(pub); + mkSubscriber("/add", "std_msgs/Int64", hdl); + } +} +``` + +The last `PubSub` contract is `ROSCompatible` and have only one constructor. +Constructor create publisher, message handler with publisher as argument and +subscriber with message handler as argument. + +### ROS interaction + +Lets mine `PubSub` contract and start AIRA ROS Bridge with them address: + +```bash +$ node start.js 0x8dc4bb82cbbdd0980cc8bd57c7ddc9682185acb7 +util.debug: Use console.error instead +DEBUG: ROSLib uses utf8 encoding by default.It would be more efficent to use ascii (if possible) +Contract: 0x8dc4bb82cbbdd0980cc8bd57c7ddc9682185acb7 +Publishers: +/value :: std_msgs/Int64 +Subscribers: +/add :: std_msgs/Int64 +Connected to websocket server. +``` + +Open terminal and start `rostopic` message subscriber node: + +```bash +$ rostopic echo /value +``` + +The next publish by `rostopic` some messages to `/add`: + +```bash +$ rostopic pub /add std_msgs/Int64 'data: 10' +publishing and latching message. Press ctrl-C to terminate +``` + +```bash +$ rostopic pub /add std_msgs/Int64 'data: -2' +publishing and latching message. Press ctrl-C to terminate +``` + +After some time the subscriber `rostopic` says: + +```bash +$ rostopic echo /value +WARNING: topic [/value] does not appear to be published yet +data: 10 +--- +data: 8 +--- +``` + +## Dron GPS Destination contract + +The task have a two parts: + +* **path estimation** :: how cost to flight for this point? +* **flight** :: dron flight and return back. + +The [video](https://www.youtube.com/watch?v=nuf6JtocTTQ) of this task solution. + +The ROS interface of *dron* is: + +Subscribers: + +* `/path_estimation/path` :: **dron_ros_tutorial/PathEstimate** - estimate cost of path +* `/dron_employee/target` :: **dron_ros_tutorial/SatPosition** - move dron to target point and go back + +Publishers: + +* `/path_estimation/cost` :: **dron_ros_tutorial/PathCost** - cost of path in ethers +* `/dron_employee/homebase` :: **dron_ros_tutorial/SatPosition** - dron current position + +### Solidity contract + +See the [gps-destination.sol](https://raw.githubusercontent.com/aira-dao/aira-IoT/master/Ethereum%20smart%20contracts/example/gps-destination.sol) contract: + +```JavaScript +contract EstimateListener is MessageHandler { + GPSDestination parent; + function EstimateListener(GPSDestination _parent) { + parent = _parent; + } + + function incomingMessage(Message _msg) { + var cost = PathCost(_msg); + parent.setEstimateCost(cost.ident(), cost.cost()); + } +} +``` + +This contract is path cost message handler, this call `parent` contract `setEstimateCost` method every new message is coming. + +```JavaScript +contract HomebaseListener is MessageHandler { + GPSDestination parent; + + function HomebaseListener(GPSDestination _parent) { + parent = _parent; + } + + function incomingMessage(Message _msg) { + SatPosition pos = SatPosition(_msg); + parent.homebase(pos.longitude(), pos.latitude()); + } +} +``` + +This contract is homebase message handler, this call `parent` contract `homebase` method every new message is coming. + +```JavaScript +contract GPSDestination is ROSCompatible { +``` + +The `ROSCompatible` main contract is this. + +```JavaScript + EstimateListener estimateListener; + HomebaseListener homebaseListener; + Publisher estimatePub; + Publisher targetPub; +``` + +Private members for message handlers and publishers. + +```JavaScript + address public currentCustomer; + int256 public homebaseLongitude; + int256 public homebaseLatitude; + int256 public destinationLongitude; + int256 public destinationLatitude; + uint public estimatesActualBefore; +``` + +Common dron information e.g. current customer, home and destination points. + + mapping (address => uint) customerEstimatesOf; + +Customer to estimation request ID mapping. + +```JavaScript + struct Estimate { + address customerAddr; + int256 destinationLongitude; + int256 destinationLatitude; + uint cost; + uint actualBefore; + } + + Estimate[] public estimates; +``` + +`Estimate` structure definition and array for storing path estimation requests. + +```JavaScript + event DroneComeback(uint estimateID); + event EstimateCostReceive(uint estimateID, uint cost); +``` + +Two customer events: drone come back from mission, path cost is received from drone. + +```JavaScript + function GPSDestination(int256 _homebaseLongitude, + int256 _homebaseLatitude, + uint _estimatesActualBefore) { + homebaseLatitude = _homebaseLatitude; + homebaseLongitude = _homebaseLongitude; + estimatesActualBefore = _estimatesActualBefore * 1 minutes; + } +``` + +Constructor of contract, sets initial values of home point. + +```JavaScript + function initROS() returns (bool result) { + estimatePub = mkPublisher('/path_estimation/path', + 'dron_ros_tutorial/PathEstimate'); + targetPub = mkPublisher('/dron_employee/target', + 'dron_ros_tutorial/SatPosition'); +``` + +Create two publishers for path estimation topic and dron target topic and save into private members for the future use. + +```JavaScript + estimateListener = new EstimateListener(this); + homebaseListener = new HomebaseListener(this); +``` + +Create two message handlers for the path estimation and homebase cases. + +```JavaScript + mkSubscriber('/path_estimation/cost', + 'dron_ros_tutorial/PathCost', + estimateListener); + mkSubscriber('/dron_employee/homebase', + 'dron_ros_tutorial/SatPosition', + homebaseListener); +``` + +Create two subscribers with previously instantied handlers. + +```JavaScript + return true; + } +``` + +ROS communication initial method takes too much GAS and can not be placed in constructor. **Should be called after contract is created.** + +```JavaScript +function homebase(int256 _currentLongitude, int256 _currentLatitude) returns(bool result) { +``` + +Takes two arguments: longitude and altitude of dron home position and sets contract members for this. Also reset current customer information and emit `DroneComeback` event. + +```JavaScript +function setEstimateCost(uint _estimateID, uint _cost) returns(bool result) { +``` +Takes two arguments: path estimation request ID and cost in ethers, saves cost into `estimates` array and emit `EstimateCostReceive` event. + +```JavaScript +function setNewEstimate(int256 _destinationLongitude, + int256 _destinationLatitude) returns(uint estimateID) { +``` + +Takes two arguments: destination longitude and latitude and return path estimation request ID. This function create new item in `estimates` array with destination coords, timestamp and publish new message by `estimatePub` instance: + +```JavaScript + var base = new SatPosition(homebaseLatitude, homebaseLongitude); + var target = new SatPosition(_destinationLatitude, _destinationLongitude); + estimatePub.publish(new PathEstimate(uint16(estimateID), base, target)); +``` + +The last method is: + +```JavaScript +function takeFlight() returns(bool result) { + uint workEstimateID = customerEstimatesOf[msg.sender]; + Estimate e = estimates[workEstimateID]; + if(msg.value >= e.cost) { +``` + +This method checks amount of ethers sended with transaction to `takeFlight` method, when count is big or eq do publish the dron target: + +```JavaScript + targetPub.publish(new SatPosition(destinationLongitude, destinationLatitude)); +``` + +In other case - return money to sender: + +```JavaScript + } else msg.sender.send(msg.value); +``` + +### ROS interaction + +The *path estimation* task have solved by [path_estimator.py](https://github.com/aira-dao/aira-IoT/blob/master/ROS/dron_ros_tutorial/scripts/path_estimator.py) ROS node. + +And the *target motion* task have solved by [quad_controller.py](https://github.com/aira-dao/aira-IoT/blob/master/ROS/dron_ros_tutorial/scripts/quad_controller.py) ROS node. + +The package [dron_ros_tutorial](https://github.com/aira-dao/aira-IoT/tree/master/ROS/dron_ros_tutorial) is a valid ROS package and can be build by [catkin](http://wiki.ros.org/catkin/Tutorials/using_a_workspace). + +```bash +$ mkdir -p catkin_ws/src && cd catkin_ws/src && catkin_init_workspace +$ cp -r /path/to/dron_ros_tutorial . +$ cd .. && catkin_make +$ source devel/setup.bash +``` + +After building you can just launch `apm_sitl.launch` for [local simulation cycle](https://github.com/aira-dao/aira-IoT/blob/master/ROS/dron_ros_tutorial/doc/Simulation.md). + +```bash +$ launch dron_ros_tutorial apm_sitl.launch +``` + +The next is mine [gps-destination.sol](https://github.com/aira-dao/aira-IoT/blob/master/Ethereum%20smart%20contracts/example/gps-destination.sol) contract and taking the address. With contract address do call `initROS` method of contract for creating ROS interface of this. + +```JavaScript +> gpsdestionationContract.initROS.sendTransaction({from: eth.accounts[0], gas: 3000000}) +``` + +The last is start [AIRA ROS Bridge](https://github.com/aira-dao/aira-IoT/tree/master/ROS/aira_ros_bridge) with current contract address. + +```bash +$ node start.js 0x0ba4478113b307e5053366b4ef0d049801268a7e +util.debug: Use console.error instead +DEBUG: ROSLib uses utf8 encoding by default.It would be more efficent to use ascii (if possible) +Contract: 0x0ba4478113b307e5053366b4ef0d049801268a7e +Publishers: +/path_estimation/path :: dron_ros_tutorial/PathEstimate +/dron_employee/target :: dron_ros_tutorial/SatPosition +Subscribers: +/path_estimation/cost :: dron_ros_tutorial/PathCost +/dron_employee/homebase :: dron_ros_tutorial/SatPosition +``` diff --git a/ROS/dron_ros_tutorial/launch/apm_sitl.launch b/ROS/dron_ros_tutorial/launch/apm_sitl.launch index 5c49ec2..81c24f4 100644 --- a/ROS/dron_ros_tutorial/launch/apm_sitl.launch +++ b/ROS/dron_ros_tutorial/launch/apm_sitl.launch @@ -17,6 +17,6 @@ - - + + diff --git a/ROS/dron_ros_tutorial/package.xml b/ROS/dron_ros_tutorial/package.xml index 66849d1..259cdcc 100644 --- a/ROS/dron_ros_tutorial/package.xml +++ b/ROS/dron_ros_tutorial/package.xml @@ -40,14 +40,12 @@ catkin - message_generation + dron_common_msgs std_msgs - sensor_msgs mavros_msgs rospy - message_runtime + dron_common_msgs mavros_msgs - sensor_msgs std_msgs rospy diff --git a/ROS/dron_ros_tutorial/wiki/Mission.md b/ROS/dron_ros_tutorial/wiki/Mission.md deleted file mode 100644 index 0b48c23..0000000 --- a/ROS/dron_ros_tutorial/wiki/Mission.md +++ /dev/null @@ -1 +0,0 @@ -[https://github.com/epsilonorion/roscopter/wiki/Exploring-Waypoint-Control](Waypoint description) diff --git a/ROS/small_atc/CMakeLists.txt b/ROS/small_atc/CMakeLists.txt new file mode 100644 index 0000000..a664577 --- /dev/null +++ b/ROS/small_atc/CMakeLists.txt @@ -0,0 +1,221 @@ +cmake_minimum_required(VERSION 2.8.3) +project(small_atc) + +## 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 + dron_common_msgs + roslib + roscpp + rostime + cmake_modules +) + +## System dependencies are found with CMake's conventions +find_package(Boost REQUIRED COMPONENTS system) +find_package(octomap REQUIRED) +find_package(ompl REQUIRED) + +find_package(PkgConfig REQUIRED) +pkg_check_modules(LIBFCL REQUIRED fcl) +find_library(LIBFCL_LIBRARIES_FULL ${LIBFCL_LIBRARIES} ${LIBFCL_LIBRARY_DIRS}) +set(LIBFCL_LIBRARIES "${LIBFCL_LIBRARIES_FULL}") + + +## Uncomment this if the package has a setup.py. This macro ensures +## modules and global scripts declared therein get installed +## See http://ros.org/doc/api/catkin/html/user_guide/setup_dot_py.html +# catkin_python_setup() + +################################################ +## Declare ROS messages, services and actions ## +################################################ + +## To declare and build messages, services or actions from within this +## package, follow these steps: +## * Let MSG_DEP_SET be the set of packages whose message types you use in +## your messages/services/actions (e.g. std_msgs, actionlib_msgs, ...). +## * In the file package.xml: +## * add a build_depend tag for "message_generation" +## * add a build_depend and a run_depend tag for each package in MSG_DEP_SET +## * If MSG_DEP_SET isn't empty the following dependency has been pulled in +## but can be declared for certainty nonetheless: +## * add a run_depend tag for "message_runtime" +## * In this file (CMakeLists.txt): +## * add "message_generation" and every package in MSG_DEP_SET to +## find_package(catkin REQUIRED COMPONENTS ...) +## * add "message_runtime" and every package in MSG_DEP_SET to +## catkin_package(CATKIN_DEPENDS ...) +## * uncomment the add_*_files sections below as needed +## and list every .msg/.srv/.action file to be processed +## * uncomment the generate_messages entry below +## * add every package in MSG_DEP_SET to generate_messages(DEPENDENCIES ...) + +## Generate messages in the 'msg' folder +# add_message_files( +# FILES +# Message1.msg +# Message2.msg +# ) + +## Generate services in the 'srv' folder +# add_service_files( +# FILES +# Service1.srv +# Service2.srv +# ) + +## Generate actions in the 'action' folder +# add_action_files( +# FILES +# Action1.action +# Action2.action +# ) + +## Generate added messages and services with any dependencies listed here +# generate_messages( +# DEPENDENCIES +# std_msgs +# ) + +################################################ +## Declare ROS dynamic reconfigure parameters ## +################################################ + +## To declare and build dynamic reconfigure parameters within this +## package, follow these steps: +## * In the file package.xml: +## * add a build_depend and a run_depend tag for "dynamic_reconfigure" +## * In this file (CMakeLists.txt): +## * add "dynamic_reconfigure" to +## find_package(catkin REQUIRED COMPONENTS ...) +## * uncomment the "generate_dynamic_reconfigure_options" section below +## and list every .cfg file to be processed + +## Generate dynamic reconfigure parameters in the 'cfg' folder +# generate_dynamic_reconfigure_options( +# cfg/DynReconf1.cfg +# cfg/DynReconf2.cfg +# ) + +################################### +## catkin specific configuration ## +################################### +## The catkin_package macro generates cmake config files for your package +## Declare things to be passed to dependent projects +## INCLUDE_DIRS: uncomment this if you package contains header files +## LIBRARIES: libraries you create in this project that dependent projects also need +## CATKIN_DEPENDS: catkin_packages dependent projects also need +## DEPENDS: system dependencies of this project that dependent projects also need +catkin_package( + INCLUDE_DIRS + ${THIS_PACKAGE_INCLUDE_DIRS} + ${OCTOMAP_INCLUDE_DIRS} + LIBRARIES small_atc + ${OCTOMAP_LIBRARIES} + CATKIN_DEPENDS + dron_common_msgs + DEPENDS + system_lib +) + +########### +## Build ## +########### + +## Specify additional locations of header files +## Your package locations should be listed before other locations +include_directories(SYSTEM ${Boost_INCLUDE_DIR}) +include_directories( + include + ${catkin_INCLUDE_DIRS} + ${OCTOMAP_INCLUDE_DIRS} + ${OMPL_INCLUDE_DIRS} +) + +link_directories(${Boost_LIBRARY_DIRS}) +link_directories(${catkin_LIBRARY_DIRS}) + +## Declare a C++ library +add_library(small_atc + src/${PROJECT_NAME}/DynamicOctoMap.cpp + src/${PROJECT_NAME}/PlannerOMPL.cpp + src/${PROJECT_NAME}/SmallATC.cpp +) + +target_compile_features(small_atc PRIVATE cxx_range_for) + +target_link_libraries(small_atc + ${catkin_LIBRARIES} + ${Boost_LIBRARIES} + ${OCTOMAP_LIBRARIES} + ${OMPL_LIBRARIES} + ${LIBFCL_LIBRARIES} +) + +## 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(small_atc ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS}) + +## Declare a C++ executable +add_executable(small_atc_node src/small_atc_node.cpp) +target_compile_features(small_atc_node PRIVATE cxx_range_for) + +## Add cmake target dependencies of the executable +## same as for the library above +#add_dependencies(small_atc_node ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS}) + +## Specify libraries to link a library or executable target against +target_link_libraries(small_atc_node + ${catkin_LIBRARIES} + small_atc +) + +############# +## 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 small_atc small_atc_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" +) + +## 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_small_atc.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/ROS/small_atc/data/forest_hills.bt b/ROS/small_atc/data/forest_hills.bt new file mode 100644 index 0000000..0fc1bf6 Binary files /dev/null and b/ROS/small_atc/data/forest_hills.bt differ diff --git a/ROS/small_atc/data/octomap_found.patch b/ROS/small_atc/data/octomap_found.patch new file mode 100644 index 0000000..e09e8c0 --- /dev/null +++ b/ROS/small_atc/data/octomap_found.patch @@ -0,0 +1,24 @@ +*** fcl-0.3.2/CMakeLists.txt 2014-10-28 08:10:48.000000000 +0300 +--- fcl-0.3.2-r1/CMakeLists.txt 2015-12-18 17:04:36.210687684 +0300 +*************** +*** 49,57 **** + ${CMAKE_SHARED_LIBRARY_PREFIX}octomap${CMAKE_SHARED_LIBRARY_SUFFIX}) + if(OCTOMAP_INCLUDE_DIRS AND OCTOMAP_LIBRARY_DIRS) + set(OCTOMAP_LIBRARIES "octomap;octomath") + endif() + endif() +! if (OCTOMAP_INCLUDE_DIRS AND OCTOMAP_LIBRARY_DIRS) + include_directories(${OCTOMAP_INCLUDE_DIRS}) + link_directories(${OCTOMAP_LIBRARY_DIRS}) + set(FCL_HAVE_OCTOMAP 1) +--- 49,58 ---- + ${CMAKE_SHARED_LIBRARY_PREFIX}octomap${CMAKE_SHARED_LIBRARY_SUFFIX}) + if(OCTOMAP_INCLUDE_DIRS AND OCTOMAP_LIBRARY_DIRS) + set(OCTOMAP_LIBRARIES "octomap;octomath") ++ set(OCTOMAP_FOUND 1) + endif() + endif() +! if (OCTOMAP_FOUND) + include_directories(${OCTOMAP_INCLUDE_DIRS}) + link_directories(${OCTOMAP_LIBRARY_DIRS}) + set(FCL_HAVE_OCTOMAP 1) diff --git a/ROS/small_atc/data/tamalpais_park.bt b/ROS/small_atc/data/tamalpais_park.bt new file mode 100644 index 0000000..9335ed0 Binary files /dev/null and b/ROS/small_atc/data/tamalpais_park.bt differ diff --git a/ROS/small_atc/doc/las_to_octree.md b/ROS/small_atc/doc/las_to_octree.md new file mode 100644 index 0000000..8e8f4c7 --- /dev/null +++ b/ROS/small_atc/doc/las_to_octree.md @@ -0,0 +1,23 @@ +## Making the OctoMap by LASer data + +The first - get LASer data from opentopography.org. + +You need to install `liblas` and `octomap` libraries and tools. + + las2las --offset "-569807.28 -4108300.46 -423.55" --output points.las downloads/points.las + +Offset data for centering future map. + + las2txt --stdout --parse xyz --delimiter " " points.las > points.txt + +Serialize binary las to text points. + + echo NODE 0 0 300 0 0 0 > header.txt + cat header.txt points.txt > ocdata.txt + +Prepare laser scan data for octree creation. + + log2graph ocdata.txt ocdata.graph + graph2tree -res 3 -i ocdata.graph -o ocdata.bt + +Choose resolution by `-res` key. diff --git a/ROS/small_atc/include/small_atc/CollisionChecker.h b/ROS/small_atc/include/small_atc/CollisionChecker.h new file mode 100644 index 0000000..b14ae6b --- /dev/null +++ b/ROS/small_atc/include/small_atc/CollisionChecker.h @@ -0,0 +1,12 @@ +#ifndef __COLLISION_CHECKER_H__ +#define __COLLISION_CHECKER_H__ + +template +class CollisionChecker +{ +public: + //static CollisionChecker * instance() = 0; + virtual bool collide(const Object *a, const Object *b) const = 0; +}; + +#endif // __COLLISION_CHECKER_H__ diff --git a/ROS/small_atc/include/small_atc/CollisionCheckerFCL.h b/ROS/small_atc/include/small_atc/CollisionCheckerFCL.h new file mode 100644 index 0000000..6320041 --- /dev/null +++ b/ROS/small_atc/include/small_atc/CollisionCheckerFCL.h @@ -0,0 +1,28 @@ +#ifndef __COLLISION_CHECKER_FCL_H__ +#define __COLLISION_CHECKER_FCL_H__ + +#include "CollisionChecker.h" +#include "HasCollision.h" +#include + +class CollisionCheckerFCL : + public CollisionChecker +{ +public: + static CollisionChecker * instance() { + static CollisionChecker *inst; + if (inst == nullptr) + inst = new CollisionCheckerFCL; + return inst; + } + + bool collide(const fcl::CollisionObject *a, const fcl::CollisionObject *b) const { + fcl::CollisionRequest req; + fcl::CollisionResult res; + return fcl::collide(a, b, req, res); + } +}; + +typedef HasCollisionImpl HasCollisionFCL; + +#endif diff --git a/ROS/small_atc/include/small_atc/DynamicMap.h b/ROS/small_atc/include/small_atc/DynamicMap.h new file mode 100644 index 0000000..760472c --- /dev/null +++ b/ROS/small_atc/include/small_atc/DynamicMap.h @@ -0,0 +1,60 @@ +#ifndef __DYNAMIC_MAP_H__ +#define __DYNAMIC_MAP_H__ + +#include +#include +#include +#include +#include + +struct MapMetaData { + double resolution; + uint32_t range[3]; + dron_common_msgs::SatFix origin; +}; + +class DynamicMap +{ +public: + virtual ~DynamicMap() {} + + /*** + * Octomap message + * Used by visualisation + **/ + virtual octomap_msgs::Octomap getOctomapMsg() const = 0; + + /*** + * Map resolution getter + **/ + virtual double getResolution() const = 0; + + /*** + * Map update methods + **/ + virtual void drawAABB(const geometry_msgs::Point &AA, + const geometry_msgs::Point &BB) = 0; + virtual void drawSphere(const geometry_msgs::Point ¢er, double R) = 0; + + virtual void drawRoute(const std::vector &route, double R) { + for (size_t i = 0; i < route.size() - 1; i++) { + geometry_msgs::Point a = route[i], + b = route[i+1]; + double distance = sqrt(pow(b.x - a.x, 2) + + pow(b.y - a.y, 2) + + pow(b.z - a.z, 2)), + dx = (b.x - a.x) / distance, + dy = (b.y - a.y) / distance, + dz = (b.z - a.z) / distance; + geometry_msgs::Point center; + for (int j = 0; j < distance; j += 4) { + center.x = a.x + j * dx, + center.y = a.y + j * dy, + center.z = a.z + j * dz; + drawSphere(center, R); + } + } + } +}; + +#endif // __DYNAMIC_MAP_H__ diff --git a/ROS/small_atc/include/small_atc/DynamicOctoMap.h b/ROS/small_atc/include/small_atc/DynamicOctoMap.h new file mode 100644 index 0000000..5be7002 --- /dev/null +++ b/ROS/small_atc/include/small_atc/DynamicOctoMap.h @@ -0,0 +1,41 @@ +#ifndef __DYNAMIC_OCTO_MAP_H +#define __DYNAMIC_OCTO_MAP_H + +#include "DynamicMap.h" +#include "CollisionCheckerFCL.h" + +#include +#include + +class DynamicOctoMap : + public DynamicMap, + public HasCollisionImpl +{ +public: + DynamicOctoMap(int resolution=3); + DynamicOctoMap(const std::string &filename); + + /*** + * Octomap message + * Used by visualisation + **/ + octomap_msgs::Octomap getOctomapMsg() const; + + /*** + * Map resolution getter + **/ + double getResolution() const + { return octomap->getResolution(); } + + /*** + * Map update methods + **/ + void drawAABB(const geometry_msgs::Point &AA, + const geometry_msgs::Point &BB); + void drawSphere(const geometry_msgs::Point ¢er, double R); + +protected: + boost::shared_ptr octomap; +}; + +#endif diff --git a/ROS/small_atc/include/small_atc/GeoHelpers.h b/ROS/small_atc/include/small_atc/GeoHelpers.h new file mode 100644 index 0000000..bdb5fc6 --- /dev/null +++ b/ROS/small_atc/include/small_atc/GeoHelpers.h @@ -0,0 +1,40 @@ +#ifndef __GEO_HELPERS_H__ +#define __GEO_HELPERS_H__ + +#include "DynamicMap.h" + +#include +#include +#include + +using namespace geometry_msgs; +using namespace dron_common_msgs; + +/** + * Global position system coords to length converter + **/ +#define RADIUS_OF_EARTH 6371 + +inline double grad2len(double grad) +{ return M_PI * RADIUS_OF_EARTH * grad / 180; } + +inline double len2grad(double len) +{ return len * 180 / M_PI / RADIUS_OF_EARTH; } + +inline Point satFix2Point(const SatFix &fix, const MapMetaData &meta) { + Point pt; + pt.x = grad2len(meta.origin.latitude - fix.latitude) / meta.resolution; + pt.y = grad2len(meta.origin.longitude - fix.longitude) / meta.resolution; + pt.z = (meta.origin.altitude - fix.altitude) / meta.resolution; + return pt; +} + +inline SatFix point2SatFix(const Point &pt, const MapMetaData &meta) { + SatFix fix; + fix.latitude = meta.origin.latitude + len2grad(pt.x * meta.resolution); + fix.longitude = meta.origin.longitude + len2grad(pt.y * meta.resolution); + fix.latitude = meta.origin.altitude + pt.z * meta.resolution; + return fix; +} + +#endif diff --git a/ROS/small_atc/include/small_atc/HasCollision.h b/ROS/small_atc/include/small_atc/HasCollision.h new file mode 100644 index 0000000..e398ef8 --- /dev/null +++ b/ROS/small_atc/include/small_atc/HasCollision.h @@ -0,0 +1,43 @@ +#ifndef __HAS_COLLISION_H__ +#define __HAS_COLLISION_H__ + +#include "CollisionChecker.h" + +class HasCollision +{ +public: + /** + * Collision checking with two objects + **/ + virtual bool hasCollision(const HasCollision *object) const = 0; +}; + +template +class HasCollisionImpl : public HasCollision +{ +public: + HasCollisionImpl(Object *object = nullptr) + : collision_object(object) + {} + + /*** + * Collision check + **/ + virtual bool hasCollision(const HasCollision *object) const + { + auto inst = static_cast*>(object); + return Collider::instance()->collide(collision_object, inst->collision_object); + } + +protected: + /*** + * Collision object setter + **/ + void setCollisionObject(Object *object) + { collision_object = object; } + +private: + Object *collision_object; +}; + +#endif diff --git a/ROS/small_atc/include/small_atc/ObstacleProvider.h b/ROS/small_atc/include/small_atc/ObstacleProvider.h new file mode 100644 index 0000000..b787d5c --- /dev/null +++ b/ROS/small_atc/include/small_atc/ObstacleProvider.h @@ -0,0 +1,72 @@ +#ifndef __OBSTACLE_PROVIDER_H__ +#define __OBSTACLE_PROVIDER_H__ + +#include "HasCollision.h" +#include "DynamicMap.h" +#include + +typedef std::vector HasCollisionVector; + +class ObstacleProvider +{ +public: + /*** + * Get vector of collision objects + **/ + virtual HasCollisionVector getCollisionObjects() const = 0; +}; + +template +class ObstacleProviderImpl : public ObstacleProvider +{ +public: + ObstacleProviderImpl(ros::NodeHandle &node_handle) + : node(node_handle) {} + + ~ObstacleProviderImpl() { + for (auto i = obstacles.begin(); i != obstacles.end(); ++i) + delete i->second; + } + /*** + * All collision objects getter + **/ + HasCollisionVector getCollisionObjects() const { + HasCollisionVector vec; + for (auto i = obstacles.begin(); i != obstacles.end(); ++i) + vec.push_back(static_cast(i->second)); + return vec; + } + + /** + * Obstacle registration method, + * returns obstacle id + **/ + void addObstacle(int id, const Obstacle *obstacle) { + obstacles[id] = obstacle; + std::string pub_name = "obstacle/" + std::to_string(id); + publishers[id] = node.advertise(pub_name, 1); + } + + /** + * Remove obstacle with id from collider + **/ + bool removeObstacle(int id) { + delete obstacles[id]; + return obstacles.erase(id) + publishers.erase(id); + } + + void publishAll() { + for (auto k = obstacles.begin(); k != obstacles.end(); ++k) { + int id = k->first; + const Obstacle *map = k->second; + publishers[id].publish(map->getOctomapMsg()); + } + } + +private: + ros::NodeHandle &node; + std::map obstacles; + std::map publishers; +}; + +#endif diff --git a/ROS/small_atc/include/small_atc/Planner.h b/ROS/small_atc/include/small_atc/Planner.h new file mode 100644 index 0000000..98529ac --- /dev/null +++ b/ROS/small_atc/include/small_atc/Planner.h @@ -0,0 +1,28 @@ +#ifndef __PLANNER_H__ +#define __PLANNER_H__ + +#include "DynamicMap.h" +#include +#include + +using namespace geometry_msgs; + +class Planner +{ +public: + Planner(const MapMetaData &meta) : map_meta(meta) {} + virtual ~Planner() {}; + + /*** Map meta getter ***/ + MapMetaData getMapMetaData() const + { return map_meta; } + + /*** Planning in map coords ***/ + virtual std::vector plan(const Point &start, + const Point &goal) = 0; + +protected: + MapMetaData map_meta; +}; + +#endif // __PLANNER_H__ diff --git a/ROS/small_atc/include/small_atc/PlannerOMPL.h b/ROS/small_atc/include/small_atc/PlannerOMPL.h new file mode 100644 index 0000000..4871fae --- /dev/null +++ b/ROS/small_atc/include/small_atc/PlannerOMPL.h @@ -0,0 +1,43 @@ +#ifndef __PLANNER_OMPL_H__ +#define __PLANNER_OMPL_H__ + +#include "Planner.h" +#include "ObstacleProvider.h" + +#include +#include +#include +#include + +using namespace geometry_msgs; + +class PlannerOMPL : public Planner +{ +public: + PlannerOMPL(const ObstacleProvider *obstProvider, + const MapMetaData &meta); + ~PlannerOMPL() {} + + std::vector plan(const Point &start, const Point &goal); + +protected: + class ValidityChecker : + public ompl::base::StateValidityChecker { + public: + ValidityChecker(const ompl::base::SpaceInformationPtr &si, + const ObstacleProvider *obstProvider) + : ompl::base::StateValidityChecker(si) + , collider(obstProvider) {} + /*** + * Check current state by collision with other objects + ***/ + bool isValid(const ompl::base::State *state) const; + private: + const ObstacleProvider *collider; + }; + + ompl::base::StateSpacePtr space; + ompl::geometric::SimpleSetup ss; +}; + +#endif // __PLANNER_OMPL_H__ diff --git a/ROS/small_atc/include/small_atc/SmallATC.h b/ROS/small_atc/include/small_atc/SmallATC.h new file mode 100644 index 0000000..955acb2 --- /dev/null +++ b/ROS/small_atc/include/small_atc/SmallATC.h @@ -0,0 +1,37 @@ +#ifndef __SMALL_ATC_H__ +#define __SMALL_ATC_H__ + +#include "Planner.h" +#include "DynamicOctoMap.h" +#include "ObstacleProvider.h" + +#include +#include + +using namespace ros; +using namespace dron_common_msgs; + +class SmallATC +{ +public: + SmallATC(); + ~SmallATC(); + + /** + * Exec the Small Air Traffic Controller + **/ + int exec(); + +protected: + void requestHandler(const LocalRouteRequest::ConstPtr &msg); + +private: + ObstacleProviderImpl *obstacles; + Planner *atc_planner; + NodeHandle node_handle; + + Publisher route_response; + Subscriber route_request; +}; + +#endif diff --git a/ROS/small_atc/launch/atc.launch b/ROS/small_atc/launch/atc.launch new file mode 100644 index 0000000..8700a64 --- /dev/null +++ b/ROS/small_atc/launch/atc.launch @@ -0,0 +1,7 @@ + + + + + + + diff --git a/ROS/small_atc/launch/demo.launch b/ROS/small_atc/launch/demo.launch new file mode 100644 index 0000000..be7e57f --- /dev/null +++ b/ROS/small_atc/launch/demo.launch @@ -0,0 +1,13 @@ + + + + + + + + + + + + + diff --git a/ROS/small_atc/package.xml b/ROS/small_atc/package.xml new file mode 100644 index 0000000..f985f2b --- /dev/null +++ b/ROS/small_atc/package.xml @@ -0,0 +1,66 @@ + + + small_atc + 0.0.0 + The small Air Traffic Control + + + + + Alexander Krupenkin + + + + + + GPLv3 + + + + + + + + + + + + + + + + + + + + + + + + + + catkin + + roslib + roscpp + rostime + dron_common_msgs + octomap + fcl + boost + xmlrpcpp + roslib + roscpp + rostime + dron_common_msgs + octomap + fcl + boost + + + + + + + + diff --git a/ROS/small_atc/scripts/geo_helper.py b/ROS/small_atc/scripts/geo_helper.py new file mode 100755 index 0000000..f6ae4a6 --- /dev/null +++ b/ROS/small_atc/scripts/geo_helper.py @@ -0,0 +1,108 @@ +#!/usr/bin/env python2 +import rospy, math +from geometry_msgs.msg import Point, PoseStamped +from sensor_msgs.msg import NavSatFix +from dron_common_msgs.msg import * +from nav_msgs.msg import Path +from threading import Thread + +RADIUS_OF_EARTH = 6371.0 * 1000 # meters + +### +# TODO: Origin param +class Origin(object): + pass +origin = Origin() +origin.latitude = 37.874815 +origin.longitude = -122.616569 +origin.altitude = -1000 +origin.offset = (1060, 1930) +# +### + +def grad2len(grad): + return math.pi * RADIUS_OF_EARTH * grad / 180 + +def len2grad(len): + return len * 180 / math.pi / RADIUS_OF_EARTH + +def satFix2Point(fix): + pt = Point() + pt.x = grad2len(fix.longitude - origin.longitude) - origin.offset[0] + pt.y = grad2len(fix.latitude - origin.latitude) - origin.offset[1] + pt.z = (fix.altitude - origin.altitude) + return pt + +def point2SatFix(pt): + fix = SatFix() + fix.latitude = origin.latitude + len2grad(pt.y + origin.offset[1]) + fix.longitude = origin.longitude + len2grad(pt.x + origin.offset[0]) + fix.altitude = origin.altitude + pt.z + return fix + +def navSatFix2Pose(fix): + p = PoseStamped() + p.header.frame_id = "map" + p.pose.position = satFix2Point(fix) + return p + +def connect(topic_in, topic_out, handler): + pub = rospy.Publisher(topic_out[0], topic_out[1], queue_size=1) + def _handler(msg): + pub.publish(handler(msg)) + rospy.Subscriber(topic_in[0], topic_in[1], _handler) + +def requestHandler(msg): + req = LocalRouteRequest() + req.id = msg.id + req.checkpoints = map(satFix2Point, msg.checkpoints) + return req + +def responseHandler(msg): + res = RouteResponse() + res.valid = msg.valid + res.id = msg.id + res.route = map(point2SatFix, msg.route) + # Path publish + emitPath(msg.id, msg.route) + return res + +def emitPath(ident, pointList): + pub = rospy.Publisher("path/"+str(ident), Path, queue_size=1) + path = Path() + path.header.frame_id = "map" + + def point2ps(point): + ps = PoseStamped() + ps.header.frame_id = path.header.frame_id + ps.pose.position = point + return ps + + path.poses = [point2ps(p) for p in pointList] + + def path_publisher(): + while not rospy.is_shutdown(): + pub.publish(path) + rospy.sleep(1) + t = Thread(target=path_publisher) + t.start() + +if __name__ == '__main__': + rospy.init_node("atc_geo_helper") + connect(("route/request", RouteRequest), + ("route/request_local", LocalRouteRequest), + requestHandler) + connect(("route/response_local", LocalRouteResponse), + ("route/response", RouteResponse), + responseHandler) + + connect(("/dron_1/mavros/global_position/global", NavSatFix), + ("/dron_1/position", PoseStamped), + navSatFix2Pose) + connect(("/dron_2/mavros/global_position/global", NavSatFix), + ("/dron_2/position", PoseStamped), + navSatFix2Pose) + connect(("/dron_3/mavros/global_position/global", NavSatFix), + ("/dron_3/position", PoseStamped), + navSatFix2Pose) + rospy.spin() diff --git a/ROS/small_atc/src/small_atc/DynamicOctoMap.cpp b/ROS/small_atc/src/small_atc/DynamicOctoMap.cpp new file mode 100644 index 0000000..548d17f --- /dev/null +++ b/ROS/small_atc/src/small_atc/DynamicOctoMap.cpp @@ -0,0 +1,62 @@ +#include +#include +#include +#include +#include + +using namespace boost; + +DynamicOctoMap::DynamicOctoMap(int resolution) + : octomap(new octomap::OcTree(resolution)) { + shared_ptr geometry(new fcl::OcTree(octomap)); + setCollisionObject(new fcl::CollisionObject(geometry)); +} + +DynamicOctoMap::DynamicOctoMap(const std::string &filename) + : octomap(new octomap::OcTree(filename)) { + shared_ptr geometry(new fcl::OcTree(octomap)); + setCollisionObject(new fcl::CollisionObject(geometry)); +} + +octomap_msgs::Octomap DynamicOctoMap::getOctomapMsg() const +{ + octomap_msgs::Octomap msg; + octomap_msgs::binaryMapToMsg(*octomap, msg); + msg.header.stamp = ros::Time::now(); + msg.header.frame_id = "map"; + return msg; +} + +void DynamicOctoMap::drawAABB(const geometry_msgs::Point &AA, + const geometry_msgs::Point &BB) +{ + octomap::Pointcloud cloud; + for (int x = AA.x; x < BB.x; x++) + for (int z = AA.z; z < BB.z; z++) { + cloud.push_back(x, AA.y, z); + cloud.push_back(x, BB.y, z); + } + for (int y = AA.y; y < BB.y; y++) + for (int z = AA.z; z < BB.z; z++) { + cloud.push_back(AA.x, y, z); + cloud.push_back(BB.x, y, z); + } + for (int x = AA.x; x < BB.x; x++) + for (int y = AA.y; y < BB.y; y++) { + cloud.push_back(x, y, AA.z); + cloud.push_back(x, y, BB.z); + } + for (auto p : cloud) + octomap->updateNode(p, true); +} + +void DynamicOctoMap::drawSphere(const geometry_msgs::Point ¢er, double R) +{ + for (double fi = 0; fi < 2 * M_PI; fi += 0.1) + for (double teta = 0; teta < 2 * M_PI; teta += 0.1) { + octomap::point3d point(center.x + R * sin(teta) * cos(fi), + center.y + R * sin(teta) * sin(fi), + center.z + R * cos(teta)); + octomap->updateNode(point, true); + } +} diff --git a/ROS/small_atc/src/small_atc/PlannerOMPL.cpp b/ROS/small_atc/src/small_atc/PlannerOMPL.cpp new file mode 100644 index 0000000..b8bfcb8 --- /dev/null +++ b/ROS/small_atc/src/small_atc/PlannerOMPL.cpp @@ -0,0 +1,92 @@ +#include +#include +#include +#include +#include +#include + +namespace ob = ompl::base; +namespace og = ompl::geometric; + +PlannerOMPL::PlannerOMPL(const ObstacleProvider *obstProvider, + const MapMetaData &meta) + : Planner(meta) + , space(new ob::RealVectorStateSpace(3)) + , ss(space) +{ + // Set up the bounds + ob::RealVectorBounds bounds(3); + // TODO: set correct bounds by map meta + bounds.setLow(-2000); + bounds.setHigh(2000); + space->as()->setBounds(bounds); + // Making the validity checker + ob::StateValidityCheckerPtr checker( + new ValidityChecker(ss.getSpaceInformation(), obstProvider)); + ss.setStateValidityChecker(checker); +} + +std::vector +PlannerOMPL::plan(const geometry_msgs::Point &start, + const geometry_msgs::Point &goal) { + + ob::ScopedState start_space(space); + start_space[0] = start.x; + start_space[1] = start.y; + start_space[2] = start.z; + std::cout << "Start:" << std::endl; + start_space.print(std::cout); + + ob::ScopedState goal_space(space); + goal_space[0] = goal.x; + goal_space[1] = goal.y; + goal_space[2] = goal.z; + std::cout << "Goal:" << std::endl; + goal_space.print(std::cout); + + ss.setStartAndGoalStates(start_space, goal_space); + + // Making the OMPL planner + ob::PlannerPtr planner(new og::RRTstar(ss.getSpaceInformation())); + ss.setPlanner(planner); + ob::PlannerStatus solved = ss.solve(); + + std::vector path; + if (solved) { + ss.simplifySolution(); + std::cout << "------------" << std::endl; + std::cout << "Found solution:" << std::endl; + ss.getSolutionPath().print(std::cout); + + for (auto pt : ss.getSolutionPath().getStates()) { + ob::State *state = &(*pt); + typedef ob::RealVectorStateSpace::StateType type; + const type *s = static_cast(state); + + geometry_msgs::Point point; + point.x = s->values[0]; + point.y = s->values[1]; + point.z = s->values[2]; + path.push_back(point); + } + } + else + std::cout << "Solution is not found" << std::endl; + return path; +} + +bool PlannerOMPL::ValidityChecker::isValid(const ob::State *state) const { + typedef ob::RealVectorStateSpace::StateType type; + const type *s = static_cast(state); + // Making an object + fcl::Transform3f stateTrans(fcl::Vec3f(s->values[0], s->values[1], s->values[2])); + boost::shared_ptr dronModel(new fcl::Sphere(15)); + fcl::CollisionObject stateObject(dronModel, stateTrans); + HasCollisionFCL collisionObject(&stateObject); + // Check collision for all obstacles in collider + for (auto obstacle : collider->getCollisionObjects()) + if (obstacle->hasCollision(&collisionObject)) + return false; + return true; +} + diff --git a/ROS/small_atc/src/small_atc/SmallATC.cpp b/ROS/small_atc/src/small_atc/SmallATC.cpp new file mode 100644 index 0000000..87754e4 --- /dev/null +++ b/ROS/small_atc/src/small_atc/SmallATC.cpp @@ -0,0 +1,65 @@ +#include +#include +#include +#include +#include + +using namespace ros; +using namespace dron_common_msgs; + +SmallATC::SmallATC() { + MapMetaData map_meta; + // TODO: Load map metadata + obstacles = new ObstacleProviderImpl(node_handle); + atc_planner = new PlannerOMPL(obstacles, map_meta); + // Making the route request/response handlers + route_response = node_handle.advertise("route/response_local", 1); + route_request = node_handle.subscribe("route/request_local", 1, + &SmallATC::requestHandler, this); + // Adding the first obstacle - the topographic map + std::string filename; + if (param::get("~map_file", filename)) { + obstacles->addObstacle(0, new DynamicOctoMap(filename)); + ROS_INFO("Loaded topographic map: %s", filename.c_str()); + } +} + +SmallATC::~SmallATC() { + delete atc_planner; + delete obstacles; +} + +void SmallATC::requestHandler(const LocalRouteRequest::ConstPtr &msg) { + ROS_INFO("Start planning..."); + LocalRouteResponse response; + for (int i = 0; i < msg->checkpoints.size() - 1; i++) { + auto plan = atc_planner->plan(msg->checkpoints[i], + msg->checkpoints[i+1]); + response.route.insert(response.route.end(), plan.begin(), plan.end()); + } + response.valid = response.route.size() > 0; + response.id = msg->id; + if (response.valid) { + ROS_INFO("Plan is valid."); + // Register route + DynamicOctoMap *map = new DynamicOctoMap(); // TODO: resolution set + map->drawRoute(response.route, 25); + obstacles->addObstacle(response.id, map); + ROS_INFO("Plan registered with id=%d", response.id); + } else { + ROS_WARN("No valid plan!"); + } + // Publish response + route_response.publish(response); +} + +int SmallATC::exec() { + ros::Rate dur(1); + while (ros::ok()) { + // Publish all the obstacles with associated topics + obstacles->publishAll(); + ros::spinOnce(); + dur.sleep(); + } + return 0; +} diff --git a/ROS/small_atc/src/small_atc_node.cpp b/ROS/small_atc/src/small_atc_node.cpp new file mode 100644 index 0000000..67bf187 --- /dev/null +++ b/ROS/small_atc/src/small_atc_node.cpp @@ -0,0 +1,9 @@ +#include +#include + +int main(int argc, char *argv[]) { + ros::init(argc, argv, "small_atc"); + + SmallATC atc; + return atc.exec(); +} diff --git a/ROS/small_atc_dron/.gitignore b/ROS/small_atc_dron/.gitignore new file mode 100644 index 0000000..0d20b64 --- /dev/null +++ b/ROS/small_atc_dron/.gitignore @@ -0,0 +1 @@ +*.pyc diff --git a/ROS/small_atc_dron/CMakeLists.txt b/ROS/small_atc_dron/CMakeLists.txt new file mode 100644 index 0000000..26d2d7c --- /dev/null +++ b/ROS/small_atc_dron/CMakeLists.txt @@ -0,0 +1,155 @@ +cmake_minimum_required(VERSION 2.8.3) +project(small_atc_dron) + +## 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 + rospy + mavros_msgs + dron_common_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 ...) + +################################################ +## 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 dron_employee_ros +# CATKIN_DEPENDS std_msgs +# DEPENDS system_lib + CATKIN_DEPENDS message_runtime dron_common_msgs mavros_msgs +) + +########### +## Build ## +########### + +## Specify additional locations of header files +## Your package locations should be listed before other locations +# include_directories(include) +include_directories( + ${catkin_INCLUDE_DIRS} +) + +## Declare a C++ library +# add_library(dron_employee_ros +# src/${PROJECT_NAME}/dron_employee_ros.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(dron_employee_ros ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS}) + +## Declare a C++ executable +# add_executable(dron_employee_ros_node src/dron_employee_ros_node.cpp) + +## Add cmake target dependencies of the executable +## same as for the library above +# add_dependencies(dron_employee_ros_node ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS}) + +## Specify libraries to link a library or executable target against +# target_link_libraries(dron_employee_ros_node +# ${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 dron_employee_ros dron_employee_ros_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 +# ) + +############# +## Testing ## +############# + +## Add gtest based cpp test target and link libraries +# catkin_add_gtest(${PROJECT_NAME}-test test/test_dron_employee_ros.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/ROS/small_atc_dron/launch/apm_sitl.launch b/ROS/small_atc_dron/launch/apm_sitl.launch new file mode 100644 index 0000000..aea6cbc --- /dev/null +++ b/ROS/small_atc_dron/launch/apm_sitl.launch @@ -0,0 +1,27 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/ROS/small_atc_dron/launch/controller.launch b/ROS/small_atc_dron/launch/controller.launch new file mode 100644 index 0000000..395c226 --- /dev/null +++ b/ROS/small_atc_dron/launch/controller.launch @@ -0,0 +1,5 @@ + + + + + diff --git a/ROS/small_atc_dron/package.xml b/ROS/small_atc_dron/package.xml new file mode 100644 index 0000000..239100d --- /dev/null +++ b/ROS/small_atc_dron/package.xml @@ -0,0 +1,58 @@ + + + small_atc_dron + 0.1.0 + Dron interaction through Ethereum tutorial + + + + + Alexander Krupenkin + + + + + + GPLv3 + + + + + + + + + + + + + + + + + + + + + + + + + + catkin + dron_common_msgs + sensor_msgs + mavros_msgs + rospy + message_runtime + dron_common_msgs + mavros_msgs + rospy + + + + + + + + diff --git a/ROS/small_atc_dron/scripts/mission_wrapper.py b/ROS/small_atc_dron/scripts/mission_wrapper.py new file mode 100644 index 0000000..0178bd0 --- /dev/null +++ b/ROS/small_atc_dron/scripts/mission_wrapper.py @@ -0,0 +1,24 @@ +from mavros_msgs.msg import Waypoint, CommandCode + +def waypoint(lat, lon, alt, delay): + w = Waypoint() + w.frame = Waypoint.FRAME_GLOBAL + w.command = CommandCode.NAV_WAYPOINT + w.is_current = False + w.autocontinue = True + w.param1 = delay # Hold time in mession + w.param2 = 2 # Position trashold in meters + w.x_lat = lat + w.y_long = lon + w.z_alt = alt + return w + +def rtl(): + w = Waypoint() + w.command = CommandCode.NAV_RETURN_TO_LAUNCH + return w + +def waypointWrap(pointList): + wps = [waypoint(p.latitude, p.longitude, p.altitude, 10) for p in pointList] + wps.append(rtl()) + return wps diff --git a/ROS/small_atc_dron/scripts/quad_commander.py b/ROS/small_atc_dron/scripts/quad_commander.py new file mode 100644 index 0000000..b5ce89b --- /dev/null +++ b/ROS/small_atc_dron/scripts/quad_commander.py @@ -0,0 +1,26 @@ +from mavros_msgs.srv import * +import rospy + +def push_mission(waypoints): + rospy.wait_for_service('mavros/mission/push') + try: + service = rospy.ServiceProxy('mavros/mission/push', WaypointPush) + service(waypoints) + except rospy.ServiceException, e: + print('Service call failed: {0}'.format(e)) + +def set_mode(mode): + rospy.wait_for_service('mavros/set_mode') + try: + service = rospy.ServiceProxy('mavros/set_mode', SetMode) + service(0, mode) + except rospy.ServiceException, e: + print('Service call failed: {0}'.format(e)) + +def arming(): + rospy.wait_for_service('mavros/cmd/arming') + try: + service = rospy.ServiceProxy('mavros/cmd/arming', CommandBool) + service(True) + except rospy.ServiceException, e: + print('Service call failed: {0}'.format(e)) \ No newline at end of file diff --git a/ROS/small_atc_dron/scripts/quad_controller.py b/ROS/small_atc_dron/scripts/quad_controller.py new file mode 100755 index 0000000..8cac70e --- /dev/null +++ b/ROS/small_atc_dron/scripts/quad_controller.py @@ -0,0 +1,69 @@ +#!/usr/bin/env python + +from mission_wrapper import waypointWrap +from quad_commander import * + +from rospy import init_node, spin, sleep, Subscriber, Publisher +from dron_common_msgs.msg import * +from sensor_msgs.msg import NavSatFix +from std_msgs.msg import Bool +from threading import Thread +from Queue import Queue + +def drop_all(queue): + while not queue.empty(): + queue.get() + +def take_current(queue): + drop_all(queue) + return queue.get(True, None) + +def quad_controller(target_queue, position_queue, arming_queue): + target = target_queue.get(True, None) + print('Received #'+str(target.id)+' mission') + if not target.valid: + print('Route invalid!') + return + # Write a mission + push_mission(waypointWrap(target.route)) + print('Mission created') + # Set manual mode + set_mode('ACRO') + # Enable motors + arming() + # Set autopilot mode + set_mode('AUTO') + print('Flight!') + # Wainting for arming + sleep(5) + drop_all(arming_queue) + while arming_queue.get().data: + pass + print('Mission complete') + +def main(): + ''' The main routine ''' + init_node('dron_employee_ros') + # Create queues + target_queue = Queue() + position_queue = Queue() + arming_queue = Queue() + # Create controller thread + controller = Thread(target=quad_controller, + args=(target_queue, position_queue, arming_queue)) + + # Target message handler + def quad_target(msg): + if not controller.isAlive(): + controller.start() + target_queue.put(msg) + else: + print('Target already exist!') + + Subscriber('mavros/global_position/global', NavSatFix, position_queue.put) + Subscriber('armed', Bool, arming_queue.put) + Subscriber('target', RouteResponse, quad_target) + spin() + +if __name__ == '__main__': + main() diff --git a/ROS/small_atc_dron/scripts/state_publisher.py b/ROS/small_atc_dron/scripts/state_publisher.py new file mode 100755 index 0000000..02cd3a6 --- /dev/null +++ b/ROS/small_atc_dron/scripts/state_publisher.py @@ -0,0 +1,25 @@ +#!/usr/bin/env python + +from rospy import init_node, spin, Subscriber, Publisher +from mavros_msgs.msg import State +from std_msgs.msg import Bool + +def main(): + init_node('state_publisher') + # Create publishers + armed_pub = Publisher('armed', Bool, queue_size=1) + + # TODO: Battery charger monitor thread + #ready_pub = Publisher('/dron_employee/battery_full', Bool, queue_size=1) + + # State handler + def dron_sate(msg): + armed = Bool() + armed.data = msg.armed + armed_pub.publish(armed) + + Subscriber('mavros/state', State, dron_sate) + spin() + +if __name__ == '__main__': + main()