Code clean and readme file commit
This commit is contained in:
Vivek Shankar Varadharajan
parent
a9b1a7598a
commit
5e9b6df1ad
164
CMakeLists.txt
164
CMakeLists.txt
|
|
@ -1,11 +1,6 @@
|
|||
cmake_minimum_required(VERSION 2.8.3)
|
||||
project(rosbuzz)
|
||||
|
||||
|
||||
|
||||
## 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
|
||||
roscpp
|
||||
std_msgs
|
||||
|
|
@ -13,186 +8,33 @@ find_package(catkin REQUIRED COMPONENTS
|
|||
sensor_msgs
|
||||
)
|
||||
|
||||
|
||||
## System dependencies are found with CMake's conventions
|
||||
# find_package(Boost REQUIRED COMPONENTS system)
|
||||
|
||||
|
||||
################################################
|
||||
## 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
|
||||
# neighbour_pos.msg
|
||||
# Message2.msg
|
||||
# )
|
||||
|
||||
## Generate services in the 'srv' folder
|
||||
# add_service_files(
|
||||
# FILES
|
||||
# Service1.srv
|
||||
# Service2.srv
|
||||
# )
|
||||
|
||||
## Generate actions in the 'action' folder
|
||||
# add_action_files(
|
||||
# FILES
|
||||
# Action1.action
|
||||
# Action2.action
|
||||
# )
|
||||
|
||||
## Generate added messages and services with any dependencies listed here
|
||||
# generate_messages(
|
||||
# DEPENDENCIES
|
||||
# sensor_msgs
|
||||
# )
|
||||
|
||||
################################################
|
||||
## 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(
|
||||
#CATKIN_DEPENDS message_runtime
|
||||
# INCLUDE_DIRS include
|
||||
# LIBRARIES buzzros
|
||||
# CATKIN_DEPENDS Buzz roscpp st_msgs
|
||||
# DEPENDS system_lib
|
||||
)
|
||||
|
||||
###########
|
||||
## Build ##
|
||||
###########
|
||||
|
||||
## Specify additional locations of header files
|
||||
## Your package locations should be listed before other locations
|
||||
# include_directories(include)
|
||||
include_directories(
|
||||
${catkin_INCLUDE_DIRS}
|
||||
)
|
||||
|
||||
## Declare a C++ library
|
||||
# add_library(buzzros
|
||||
# src/${PROJECT_NAME}/buzzros.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(buzzros ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS})
|
||||
|
||||
## Declare a C++ executable
|
||||
# add_executable(buzzros_node src/buzzros_node.cpp)
|
||||
|
||||
## Add cmake target dependencies of the executable
|
||||
## same as for the library above
|
||||
# add_dependencies(buzzros_node ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS})
|
||||
|
||||
## Specify libraries to link a library or executable target against
|
||||
# target_link_libraries(buzzros_node
|
||||
# ${catkin_LIBRARIES}
|
||||
# )
|
||||
|
||||
# Executables
|
||||
add_executable(rosbuzz_node src/rosbuzz.cpp
|
||||
src/buzz_utility.cpp
|
||||
src/buzzuav_closures.cpp
|
||||
src/uav_utility.cpp)
|
||||
target_link_libraries(rosbuzz_node ${catkin_LIBRARIES} /usr/local/lib/libbuzz.so /usr/local/lib/libbuzzdbg.so -lpthread)
|
||||
#add_dependencies(rosbuzz_node rosbuzz_generate_messages_cpp)
|
||||
#############
|
||||
## Install ##
|
||||
#############
|
||||
|
||||
# all install targets should use catkin DESTINATION variables
|
||||
# See http://ros.org/doc/api/catkin/html/adv_user_guide/variables.html
|
||||
|
||||
## Mark executable scripts (Python etc.) for installation
|
||||
## in contrast to setup.py, you can choose the destination
|
||||
# install(PROGRAMS
|
||||
# scripts/my_python_script
|
||||
# DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION}
|
||||
# )
|
||||
|
||||
## Mark executables and/or libraries for installation
|
||||
# Executables and libraries for installation to do
|
||||
install(TARGETS rosbuzz_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_buzzros.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)
|
||||
|
||||
|
||||
|
||||
|
|
|
|||
|
|
@ -1,19 +1,19 @@
|
|||
<?xml version="1.0"?>
|
||||
<package>
|
||||
<name>rosbuzz</name>
|
||||
<version>0.0.0</version>
|
||||
<version>0.1.0</version>
|
||||
<description>The rosbuzz package</description>
|
||||
|
||||
<!-- One maintainer tag required, multiple allowed, one person per tag -->
|
||||
<!-- Example: -->
|
||||
<!-- <maintainer email="jane.doe@example.com">Jane Doe</maintainer> -->
|
||||
<maintainer email="vivek@todo.todo">vivek</maintainer>
|
||||
<maintainer email="vivek-shankar.varadharajan@polymtl.ca">vivek</maintainer>
|
||||
|
||||
|
||||
<!-- One license tag required, multiple allowed, one license per tag -->
|
||||
<!-- Commonly used license strings: -->
|
||||
<!-- BSD, MIT, Boost Software License, GPLv2, GPLv3, LGPLv2.1, LGPLv3 -->
|
||||
<license>TODO</license>
|
||||
<license>MIT</license>
|
||||
|
||||
|
||||
<!-- Url tags are optional, but mutiple are allowed, one per tag -->
|
||||
|
|
@ -25,7 +25,7 @@
|
|||
<!-- Author tags are optional, mutiple are allowed, one per tag -->
|
||||
<!-- Authors do not have to be maintianers, but could be -->
|
||||
<!-- Example: -->
|
||||
<!-- <author email="jane.doe@example.com">Jane Doe</author> -->
|
||||
<author email="vivek-shankar.varadharajan@polymtl.ca">vivek</author>
|
||||
|
||||
|
||||
<!-- The *_depend tags are used to specify dependencies -->
|
||||
|
|
|
|||
|
|
@ -0,0 +1,85 @@
|
|||
ROS Implemenation of Buzz
|
||||
=========================
|
||||
|
||||
What is Buzz?
|
||||
=============
|
||||
|
||||
Buzz is a novel programming language for heterogeneous robots swarms.
|
||||
|
||||
Buzz advocates a compositional approach, by offering primitives to define swarm behaviors both in a bottom-up and in a top-down fashion.
|
||||
|
||||
Bottom-up primitives include robot-wise commands and manipulation of neighborhood data through mapping/reducing/filtering operations.
|
||||
|
||||
Top-down primitives allow for the dynamic management of robot teams, and for sharing information globally across the swarm.
|
||||
|
||||
Self-organization results from the fact that the Buzz run-time platform is purely distributed.
|
||||
|
||||
The language can be extended to add new primitives (thus supporting heterogeneous robot swarms) and can be laid on top of other frameworks, such as ROS.
|
||||
|
||||
More information is available at http://the.swarming.buzz/wiki/doku.php?id=start.
|
||||
|
||||
Downloading ROS Package
|
||||
=======================
|
||||
|
||||
$ git clone
|
||||
|
||||
Requirements
|
||||
------------
|
||||
* Buzz : You can download the development sources through git:
|
||||
|
||||
$ git clone https://github.com/MISTLab/Buzz.git buzz
|
||||
|
||||
* ROS binary distribution (Indigo or higher) with catkin
|
||||
|
||||
You need the following package:
|
||||
|
||||
* mavros_msgs : You can install using apt-get:
|
||||
|
||||
$ sudo apt-get install ros-"distro"-mavros ros-"distro"-mavros-extras
|
||||
|
||||
Compilation
|
||||
-----------
|
||||
|
||||
To compile the ros package, execute the following:
|
||||
|
||||
$ cd catkin_ws
|
||||
$ catkin_make
|
||||
$ source devel/setup.bash
|
||||
|
||||
Run
|
||||
===
|
||||
|
||||
$ rosrun rosbuzz rosbuzz_node
|
||||
|
||||
Publisher
|
||||
----------
|
||||
|
||||
* Messages from Buzz (BVM):
|
||||
The package publishes mavros_msgs/Mavlink message with a topic name of "outMavlink".
|
||||
|
||||
Subscribers
|
||||
-----------
|
||||
|
||||
* Current position of the Robot:
|
||||
The package subscribes' to sensor_msgs/NavSatFix message with a topic name of "current_pos".
|
||||
|
||||
* Messages to Buzz (BVM):
|
||||
The package subscribes' to mavros_msgs/Mavlink message with a topic name of "inMavlink".
|
||||
|
||||
* Battery status:
|
||||
The package subscribes' to mavros_msgs/BatteryStatus message with a topic name of "battery_state".
|
||||
|
||||
Service
|
||||
-------
|
||||
|
||||
* Remote Controller command:
|
||||
The package offers service using mavros_msgs/CommandInt service with name "djicmd_rc".
|
||||
|
||||
Client
|
||||
------
|
||||
|
||||
* Flight controller client:
|
||||
This package is a client of mavros_msgs/CommandInt service with name "djicmd".
|
||||
|
||||
|
||||
|
||||
|
|
@ -1,23 +1,19 @@
|
|||
/** @file buzz_utility.cpp
|
||||
* @version 1.0
|
||||
* @date 27.09.2016
|
||||
* @brief Buzz Implementation as a node in ROS for Dji M100 Drone.
|
||||
* @author Vivek Shankar Varadharajan
|
||||
* @copyright 2016 MistLab. All rights reserved.
|
||||
*/
|
||||
#define _GNU_SOURCE
|
||||
#include <stdio.h>
|
||||
|
||||
#include "buzz_utility.h"
|
||||
#include "buzzuav_closures.h"
|
||||
#include <buzz/buzzdebug.h>
|
||||
|
||||
#include <stdlib.h>
|
||||
#include <string.h>
|
||||
#include <unistd.h>
|
||||
|
||||
#include <arpa/inet.h>
|
||||
#include <errno.h>
|
||||
#include <netdb.h>
|
||||
#include <sys/types.h>
|
||||
#include <sys/socket.h>
|
||||
#include <pthread.h>
|
||||
#include <vector>
|
||||
#include <iostream>
|
||||
#include <bitset>
|
||||
#include <map>
|
||||
using namespace std;
|
||||
/****************************************/
|
||||
|
|
@ -33,15 +29,13 @@ static int TCP_COMM_STREAM = -1;
|
|||
static uint8_t* STREAM_SEND_BUF = NULL;
|
||||
|
||||
|
||||
|
||||
|
||||
/****************************************/
|
||||
|
||||
/*adds neighbours position*/
|
||||
void neighbour_pos_callback(std::map< int, Pos_struct> neighbours_pos_map){
|
||||
/* Reset neighbor information */
|
||||
buzzneighbors_reset(VM);
|
||||
/* Reset neighbor information */
|
||||
buzzneighbors_reset(VM);
|
||||
/* Get robot id and update neighbor information */
|
||||
map< int, Pos_struct >::iterator it;
|
||||
map< int, Pos_struct >::iterator it;
|
||||
for (it=neighbours_pos_map.begin(); it!=neighbours_pos_map.end(); ++it){
|
||||
buzzneighbors_add(VM,
|
||||
it->first,
|
||||
|
|
@ -49,10 +43,10 @@ map< int, Pos_struct >::iterator it;
|
|||
(it->second).y,
|
||||
(it->second).z);
|
||||
}
|
||||
|
||||
|
||||
}
|
||||
|
||||
/***************************************/
|
||||
/*Reinterprets uint64_t into 4 uint16_t*/
|
||||
/***************************************/
|
||||
uint16_t* u64_cvt_u16(uint64_t u64){
|
||||
uint16_t* out = new uint16_t[4];
|
||||
uint32_t int32_1 = u64 & 0xFFFFFFFF;
|
||||
|
|
@ -63,9 +57,12 @@ uint16_t* u64_cvt_u16(uint64_t u64){
|
|||
out[3] = (int32_2 & (0xFFFF0000) ) >> 16;
|
||||
//cout << " values " <<out[0] <<" "<<out[1] <<" "<<out[2] <<" "<<out[3] <<" ";
|
||||
return out;
|
||||
|
||||
}
|
||||
|
||||
/***************************************************/
|
||||
/*Appends obtained messages to buzz in message Queue*/
|
||||
/***************************************************/
|
||||
|
||||
void in_msg_process(uint64_t* payload){
|
||||
|
||||
/* Go through messages and add them to the FIFO */
|
||||
|
|
@ -74,10 +71,10 @@ void in_msg_process(uint64_t* payload){
|
|||
delete[] data;
|
||||
uint8_t* pl =(uint8_t*)malloc(size);
|
||||
memset(pl, 0,size);
|
||||
/* Copy packet into temporary buffer */
|
||||
/* Copy packet into temporary buffer */
|
||||
memcpy(pl, payload ,size);
|
||||
|
||||
size_t tot = sizeof(uint32_t);
|
||||
size_t tot = sizeof(uint32_t);
|
||||
|
||||
/* Go through the messages until there's nothing else to read */
|
||||
uint16_t unMsgSize;
|
||||
|
|
@ -96,15 +93,18 @@ void in_msg_process(uint64_t* payload){
|
|||
/* Process messages */
|
||||
buzzvm_process_inmsgs(VM);
|
||||
}
|
||||
/***************************************************/
|
||||
/*Obtains messages from buzz out message Queue*/
|
||||
/***************************************************/
|
||||
|
||||
uint64_t* out_msg_process(){
|
||||
buzzvm_process_outmsgs(VM);
|
||||
uint8_t* buff_send =(uint8_t*)malloc(MSG_SIZE);
|
||||
memset(buff_send, 0, MSG_SIZE);
|
||||
ssize_t tot = sizeof(uint16_t);
|
||||
/* Send robot id */
|
||||
/* Send robot id */
|
||||
*(uint16_t*)(buff_send+tot) = (uint16_t) VM->robot;
|
||||
tot += sizeof(uint16_t);
|
||||
tot += sizeof(uint16_t);
|
||||
/* Send messages from FIFO */
|
||||
do {
|
||||
/* Are there more messages? */
|
||||
|
|
@ -135,16 +135,18 @@ uint64_t* out_msg_process(){
|
|||
int total_size =(ceil((float)tot/(float)sizeof(uint64_t)));
|
||||
*(uint16_t*)buff_send = (uint16_t) total_size;
|
||||
|
||||
|
||||
uint64_t* payload_64 = new uint64_t[total_size];
|
||||
|
||||
uint64_t* payload_64 = new uint64_t[total_size];
|
||||
|
||||
memcpy((void*)payload_64, (void*)buff_send, total_size*sizeof(uint64_t));
|
||||
|
||||
memcpy((void*)payload_64, (void*)buff_send, total_size*sizeof(uint64_t));
|
||||
/*for(int i=0;i<total_size;i++){
|
||||
cout<<" payload from out msg "<<*(payload_64+i)<<endl;
|
||||
}*/
|
||||
/* Send message */
|
||||
return payload_64;
|
||||
}
|
||||
/****************************************/
|
||||
|
||||
/*Buzz script not able to load*/
|
||||
/****************************************/
|
||||
|
||||
static const char* buzz_error_info() {
|
||||
|
|
@ -170,6 +172,7 @@ static const char* buzz_error_info() {
|
|||
}
|
||||
|
||||
/****************************************/
|
||||
/*Buzz hooks that can be used inside .bzz file*/
|
||||
/****************************************/
|
||||
|
||||
static int buzz_register_hooks() {
|
||||
|
|
@ -192,7 +195,7 @@ static int buzz_register_hooks() {
|
|||
}
|
||||
|
||||
/****************************************/
|
||||
|
||||
/*Sets the .bzz and .bdbg file*/
|
||||
/****************************************/
|
||||
|
||||
int buzz_script_set(const char* bo_filename,
|
||||
|
|
@ -202,7 +205,7 @@ int buzz_script_set(const char* bo_filename,
|
|||
gethostname(hstnm, 30);
|
||||
/* Make numeric id from hostname */
|
||||
/* NOTE: here we assume that the hostname is in the format Knn */
|
||||
int id =0; // strtol(hstnm + 1, NULL, 10);
|
||||
int id = strtol(hstnm + 1, NULL, 10);
|
||||
cout << " Robot ID" << id<< endl;
|
||||
/* Reset the Buzz VM */
|
||||
if(VM) buzzvm_destroy(&VM);
|
||||
|
|
@ -260,6 +263,7 @@ int buzz_script_set(const char* bo_filename,
|
|||
}
|
||||
|
||||
/****************************************/
|
||||
/*Swarm struct*/
|
||||
/****************************************/
|
||||
|
||||
struct buzzswarm_elem_s {
|
||||
|
|
@ -297,11 +301,9 @@ void check_swarm_members(const void* key, void* data, void* params) {
|
|||
}
|
||||
}
|
||||
}
|
||||
/*Step through the buzz script*/
|
||||
|
||||
void buzz_script_step() {
|
||||
|
||||
|
||||
//buzzvm_process_inmsgs(VM);
|
||||
/*
|
||||
* Update sensors
|
||||
*/
|
||||
|
|
@ -319,7 +321,7 @@ void buzz_script_step() {
|
|||
/* Print swarm */
|
||||
buzzswarm_members_print(stdout, VM->swarmmembers, VM->robot);
|
||||
/* Check swarm state */
|
||||
/* int status = 1;
|
||||
/* int status = 1;
|
||||
buzzdict_foreach(VM->swarmmembers, check_swarm_members, &status);
|
||||
if(status == 1 &&
|
||||
buzzdict_size(VM->swarmmembers) < 9)
|
||||
|
|
@ -330,13 +332,10 @@ void buzz_script_step() {
|
|||
}
|
||||
|
||||
/****************************************/
|
||||
/*Destroy the bvm and other resorces*/
|
||||
/****************************************/
|
||||
|
||||
void buzz_script_destroy() {
|
||||
|
||||
/* Get rid of stream buffer */
|
||||
//free(STREAM_SEND_BUF);
|
||||
/* Get rid of virtual machine */
|
||||
if(VM) {
|
||||
if(VM->state != BUZZVM_STATE_READY) {
|
||||
fprintf(stderr, "%s: execution terminated abnormally: %s\n\n",
|
||||
|
|
@ -353,6 +352,7 @@ void buzz_script_destroy() {
|
|||
}
|
||||
|
||||
/****************************************/
|
||||
/*Execution completed*/
|
||||
/****************************************/
|
||||
|
||||
int buzz_script_done() {
|
||||
|
|
|
|||
|
|
@ -1,6 +1,5 @@
|
|||
#ifndef BUZZ_UTILITY_H
|
||||
#define BUZZ_UTILITY_H
|
||||
#include <vector>
|
||||
#include <stdint.h>
|
||||
#include <map>
|
||||
struct pos_struct
|
||||
|
|
@ -13,19 +12,21 @@ typedef struct pos_struct Pos_struct ;
|
|||
|
||||
uint16_t* u64_cvt_u16(uint64_t u64);
|
||||
|
||||
extern int buzz_listen(const char* type,
|
||||
int buzz_listen(const char* type,
|
||||
int msg_size);
|
||||
|
||||
void neighbour_pos_callback(std::map< int, Pos_struct> neighbours_pos_map);
|
||||
|
||||
void in_msg_process(uint64_t* payload);
|
||||
|
||||
uint64_t* out_msg_process();
|
||||
|
||||
extern int buzz_script_set(const char* bo_filename,
|
||||
int buzz_script_set(const char* bo_filename,
|
||||
const char* bdbg_filename);
|
||||
extern void buzz_script_step();
|
||||
void buzz_script_step();
|
||||
|
||||
extern void buzz_script_destroy();
|
||||
void buzz_script_destroy();
|
||||
|
||||
extern int buzz_script_done();
|
||||
int buzz_script_done();
|
||||
|
||||
#endif
|
||||
|
|
|
|||
|
|
@ -1,4 +1,11 @@
|
|||
#define _GNU_SOURCE
|
||||
/** @file buzzuav_closures.cpp
|
||||
* @version 1.0
|
||||
* @date 27.09.2016
|
||||
* @brief Buzz Implementation as a node in ROS for Dji M100 Drone.
|
||||
* @author Vivek Shankar Varadharajan
|
||||
* @copyright 2016 MistLab. All rights reserved.
|
||||
*/
|
||||
//#define _GNU_SOURCE
|
||||
#include <stdio.h>
|
||||
#include "buzzuav_closures.h"
|
||||
#include "uav_utility.h"
|
||||
|
|
@ -56,16 +63,17 @@ int buzzros_print(buzzvm_t vm) {
|
|||
|
||||
int buzzuav_goto(buzzvm_t vm) {
|
||||
buzzvm_lnum_assert(vm, 3);
|
||||
buzzvm_lload(vm, 1); /* Lattitude */
|
||||
buzzvm_lload(vm, 1); /* Altitude */
|
||||
buzzvm_lload(vm, 2); /* Longitude */
|
||||
buzzvm_lload(vm, 3); /* Altitude */
|
||||
buzzvm_lload(vm, 3); /* Latitude */
|
||||
buzzvm_type_assert(vm, 3, BUZZTYPE_FLOAT);
|
||||
buzzvm_type_assert(vm, 2, BUZZTYPE_FLOAT);
|
||||
buzzvm_type_assert(vm, 1, BUZZTYPE_FLOAT);
|
||||
goto_pos[2]=buzzvm_stack_at(vm, 1)->f.value * 10.0f;
|
||||
goto_pos[1]=buzzvm_stack_at(vm, 2)->f.value * 10.0f;
|
||||
goto_pos[0]=buzzvm_stack_at(vm, 3)->f.value * 10.0f;
|
||||
cur_cmd=mavros_msgs::CommandCode::NAV_WAYPOINT;
|
||||
goto_pos[2]=buzzvm_stack_at(vm, 1)->f.value;
|
||||
goto_pos[1]=buzzvm_stack_at(vm, 2)->f.value;
|
||||
goto_pos[0]=buzzvm_stack_at(vm, 3)->f.value;
|
||||
cur_cmd=mavros_msgs::CommandCode::NAV_WAYPOINT;
|
||||
printf(" Buzz requested Go To, to Latitude: %15f , Longitude: %15f, Altitude: %15f \n",goto_pos[0],goto_pos[1],goto_pos[2]);
|
||||
return buzzvm_ret0(vm);
|
||||
}
|
||||
|
||||
|
|
@ -95,24 +103,27 @@ cur_cmd=rc_cmd;
|
|||
|
||||
int buzzuav_takeoff(buzzvm_t vm) {
|
||||
cur_cmd=mavros_msgs::CommandCode::NAV_TAKEOFF;
|
||||
printf(" Buzz requested Take off !!! \n");
|
||||
return buzzvm_ret0(vm);
|
||||
}
|
||||
|
||||
int buzzuav_land(buzzvm_t vm) {
|
||||
cur_cmd=mavros_msgs::CommandCode::NAV_LAND;
|
||||
printf(" Buzz requested Land !!! \n");
|
||||
return buzzvm_ret0(vm);
|
||||
}
|
||||
|
||||
int buzzuav_gohome(buzzvm_t vm) {
|
||||
cur_cmd=mavros_msgs::CommandCode::NAV_RETURN_TO_LAUNCH;
|
||||
printf(" Buzz requested gohome !!! \n");
|
||||
return buzzvm_ret0(vm);
|
||||
}
|
||||
|
||||
/****************************************/
|
||||
void set_battery(float voltage,float current,float remaining){
|
||||
batt[0]=voltage;
|
||||
batt[1]=current;
|
||||
batt[2]=remaining;
|
||||
batt[0]=voltage;
|
||||
batt[1]=current;
|
||||
batt[2]=remaining;
|
||||
}
|
||||
/****************************************/
|
||||
|
||||
|
|
@ -137,10 +148,11 @@ int buzzuav_update_battery(buzzvm_t vm) {
|
|||
}
|
||||
|
||||
/****************************************/
|
||||
/*To do !!!!!*/
|
||||
/****************************************/
|
||||
|
||||
int buzzuav_update_prox(buzzvm_t vm) {
|
||||
static char PROXIMITY_BUF[256];
|
||||
/* static char PROXIMITY_BUF[256];
|
||||
int i;
|
||||
//kh4_proximity_ir(PROXIMITY_BUF, DSPIC);
|
||||
buzzvm_pushs(vm, buzzvm_string_register(vm, "proximity_ir", 1));
|
||||
|
|
@ -161,7 +173,7 @@ int buzzuav_update_prox(buzzvm_t vm) {
|
|||
buzzvm_tput(vm);
|
||||
}
|
||||
buzzvm_gstore(vm);
|
||||
return vm->state;
|
||||
return vm->state;*/
|
||||
}
|
||||
|
||||
/****************************************/
|
||||
|
|
|
|||
|
|
@ -17,15 +17,13 @@ int buzzros_print(buzzvm_t vm);
|
|||
int buzzuav_goto(buzzvm_t vm);
|
||||
/* Returns the current command from local variable*/
|
||||
int getcmd();
|
||||
|
||||
/*Sets goto position could be used for bypassing*/
|
||||
void set_goto(double pos[]);
|
||||
|
||||
/*sets rc requested command */
|
||||
void rc_call(int rc_cmd);
|
||||
|
||||
/* sets the battery state to the local variable
|
||||
*/
|
||||
/* sets the battery state */
|
||||
void set_battery(float voltage,float current,float remaining);
|
||||
/*retuns the current go to position from local variable*/
|
||||
/*retuns the current go to position */
|
||||
double* getgoto();
|
||||
/*
|
||||
* Commands the UAV to takeoff
|
||||
|
|
@ -47,7 +45,7 @@ int buzzuav_update_battery(buzzvm_t vm);
|
|||
|
||||
/*
|
||||
* Updates IR information in Buzz
|
||||
* Proximity and ground sensors
|
||||
* Proximity and ground sensors to do !!!!
|
||||
*/
|
||||
int buzzuav_update_prox(buzzvm_t vm);
|
||||
|
||||
|
|
|
|||
165
src/out.basm
165
src/out.basm
|
|
@ -1,13 +1,11 @@
|
|||
!23
|
||||
!24
|
||||
'init
|
||||
'i
|
||||
'v
|
||||
'stigmergy
|
||||
'create
|
||||
'step
|
||||
's
|
||||
'swarm
|
||||
'create
|
||||
'join
|
||||
'step
|
||||
'neighbors
|
||||
'foreach
|
||||
'print
|
||||
|
|
@ -20,20 +18,23 @@
|
|||
'azimuth
|
||||
'elevation =
|
||||
'elevation
|
||||
'exec
|
||||
'uav_takeoff
|
||||
'uav_goto
|
||||
'reset
|
||||
'destroy
|
||||
|
||||
pushs 0
|
||||
pushcn @__label_1
|
||||
gstore
|
||||
pushs 5
|
||||
pushs 6
|
||||
pushcn @__label_2
|
||||
gstore
|
||||
pushs 21
|
||||
pushcn @__label_4
|
||||
gstore
|
||||
pushs 22
|
||||
pushcn @__label_5
|
||||
pushcn @__label_8
|
||||
gstore
|
||||
pushs 23
|
||||
pushcn @__label_9
|
||||
gstore
|
||||
nop
|
||||
|
||||
|
|
@ -47,67 +48,103 @@
|
|||
pushi 0 |3,2,/home/vivek/catkin_ws/src/rosbuzz/src/test.bzz
|
||||
gstore |3,3,/home/vivek/catkin_ws/src/rosbuzz/src/test.bzz
|
||||
pushs 2 |4,2,/home/vivek/catkin_ws/src/rosbuzz/src/test.bzz
|
||||
pushs 3 |4,13,/home/vivek/catkin_ws/src/rosbuzz/src/test.bzz
|
||||
gload |4,13,/home/vivek/catkin_ws/src/rosbuzz/src/test.bzz
|
||||
pushs 4 |4,14,/home/vivek/catkin_ws/src/rosbuzz/src/test.bzz
|
||||
tget |4,20,/home/vivek/catkin_ws/src/rosbuzz/src/test.bzz
|
||||
pushi 1 |4,21,/home/vivek/catkin_ws/src/rosbuzz/src/test.bzz
|
||||
pushi 1 |4,23,/home/vivek/catkin_ws/src/rosbuzz/src/test.bzz
|
||||
callc |4,23,/home/vivek/catkin_ws/src/rosbuzz/src/test.bzz
|
||||
gstore |4,23,/home/vivek/catkin_ws/src/rosbuzz/src/test.bzz
|
||||
ret0 |6,1,/home/vivek/catkin_ws/src/rosbuzz/src/test.bzz
|
||||
pushs 3 |4,9,/home/vivek/catkin_ws/src/rosbuzz/src/test.bzz
|
||||
gload |4,9,/home/vivek/catkin_ws/src/rosbuzz/src/test.bzz
|
||||
pushs 4 |4,10,/home/vivek/catkin_ws/src/rosbuzz/src/test.bzz
|
||||
tget |4,16,/home/vivek/catkin_ws/src/rosbuzz/src/test.bzz
|
||||
pushi 10 |4,17,/home/vivek/catkin_ws/src/rosbuzz/src/test.bzz
|
||||
pushi 1 |4,20,/home/vivek/catkin_ws/src/rosbuzz/src/test.bzz
|
||||
callc |4,20,/home/vivek/catkin_ws/src/rosbuzz/src/test.bzz
|
||||
gstore |4,20,/home/vivek/catkin_ws/src/rosbuzz/src/test.bzz
|
||||
pushs 2 |5,1,/home/vivek/catkin_ws/src/rosbuzz/src/test.bzz
|
||||
gload |5,1,/home/vivek/catkin_ws/src/rosbuzz/src/test.bzz
|
||||
pushs 5 |5,2,/home/vivek/catkin_ws/src/rosbuzz/src/test.bzz
|
||||
tget |5,6,/home/vivek/catkin_ws/src/rosbuzz/src/test.bzz
|
||||
pushi 0 |5,8,/home/vivek/catkin_ws/src/rosbuzz/src/test.bzz
|
||||
callc |5,8,/home/vivek/catkin_ws/src/rosbuzz/src/test.bzz
|
||||
ret0 |7,1,/home/vivek/catkin_ws/src/rosbuzz/src/test.bzz
|
||||
|
||||
@__label_2
|
||||
pushs 6 |10,2,/home/vivek/catkin_ws/src/rosbuzz/src/test.bzz
|
||||
pushs 7 |10,9,/home/vivek/catkin_ws/src/rosbuzz/src/test.bzz
|
||||
gload |10,9,/home/vivek/catkin_ws/src/rosbuzz/src/test.bzz
|
||||
pushs 4 |10,10,/home/vivek/catkin_ws/src/rosbuzz/src/test.bzz
|
||||
tget |10,16,/home/vivek/catkin_ws/src/rosbuzz/src/test.bzz
|
||||
pushi 10 |10,17,/home/vivek/catkin_ws/src/rosbuzz/src/test.bzz
|
||||
pushi 1 |10,20,/home/vivek/catkin_ws/src/rosbuzz/src/test.bzz
|
||||
callc |10,20,/home/vivek/catkin_ws/src/rosbuzz/src/test.bzz
|
||||
gstore |10,20,/home/vivek/catkin_ws/src/rosbuzz/src/test.bzz
|
||||
pushs 6 |11,1,/home/vivek/catkin_ws/src/rosbuzz/src/test.bzz
|
||||
gload |11,1,/home/vivek/catkin_ws/src/rosbuzz/src/test.bzz
|
||||
pushs 8 |11,2,/home/vivek/catkin_ws/src/rosbuzz/src/test.bzz
|
||||
tget |11,6,/home/vivek/catkin_ws/src/rosbuzz/src/test.bzz
|
||||
pushi 0 |11,8,/home/vivek/catkin_ws/src/rosbuzz/src/test.bzz
|
||||
callc |11,8,/home/vivek/catkin_ws/src/rosbuzz/src/test.bzz
|
||||
pushs 9 |13,9,/home/vivek/catkin_ws/src/rosbuzz/src/test.bzz
|
||||
gload |13,9,/home/vivek/catkin_ws/src/rosbuzz/src/test.bzz
|
||||
pushs 10 |13,10,/home/vivek/catkin_ws/src/rosbuzz/src/test.bzz
|
||||
tget |13,17,/home/vivek/catkin_ws/src/rosbuzz/src/test.bzz
|
||||
pushl @__label_3 |14,2,/home/vivek/catkin_ws/src/rosbuzz/src/test.bzz
|
||||
pushi 1 |18,42,/home/vivek/catkin_ws/src/rosbuzz/src/test.bzz
|
||||
callc |18,42,/home/vivek/catkin_ws/src/rosbuzz/src/test.bzz
|
||||
ret0 |30,1,/home/vivek/catkin_ws/src/rosbuzz/src/test.bzz
|
||||
pushs 7 |12,9,/home/vivek/catkin_ws/src/rosbuzz/src/test.bzz
|
||||
gload |12,9,/home/vivek/catkin_ws/src/rosbuzz/src/test.bzz
|
||||
pushs 8 |12,10,/home/vivek/catkin_ws/src/rosbuzz/src/test.bzz
|
||||
tget |12,17,/home/vivek/catkin_ws/src/rosbuzz/src/test.bzz
|
||||
pushl @__label_3 |13,2,/home/vivek/catkin_ws/src/rosbuzz/src/test.bzz
|
||||
pushi 1 |17,42,/home/vivek/catkin_ws/src/rosbuzz/src/test.bzz
|
||||
callc |17,42,/home/vivek/catkin_ws/src/rosbuzz/src/test.bzz
|
||||
pushs 1 |18,4,/home/vivek/catkin_ws/src/rosbuzz/src/test.bzz
|
||||
gload |18,4,/home/vivek/catkin_ws/src/rosbuzz/src/test.bzz
|
||||
pushi 1 |18,6,/home/vivek/catkin_ws/src/rosbuzz/src/test.bzz
|
||||
eq |18,7,/home/vivek/catkin_ws/src/rosbuzz/src/test.bzz
|
||||
jumpz @__label_4 |18,8,/home/vivek/catkin_ws/src/rosbuzz/src/test.bzz
|
||||
pushs 2 |19,1,/home/vivek/catkin_ws/src/rosbuzz/src/test.bzz
|
||||
gload |19,1,/home/vivek/catkin_ws/src/rosbuzz/src/test.bzz
|
||||
pushs 19 |19,2,/home/vivek/catkin_ws/src/rosbuzz/src/test.bzz
|
||||
tget |19,6,/home/vivek/catkin_ws/src/rosbuzz/src/test.bzz
|
||||
pushl @__label_6 |19,7,/home/vivek/catkin_ws/src/rosbuzz/src/test.bzz
|
||||
pushi 1 |19,34,/home/vivek/catkin_ws/src/rosbuzz/src/test.bzz
|
||||
callc |19,34,/home/vivek/catkin_ws/src/rosbuzz/src/test.bzz
|
||||
pushs 1 |20,1,/home/vivek/catkin_ws/src/rosbuzz/src/test.bzz
|
||||
pushi 0 |20,2,/home/vivek/catkin_ws/src/rosbuzz/src/test.bzz
|
||||
gstore |20,3,/home/vivek/catkin_ws/src/rosbuzz/src/test.bzz
|
||||
jump @__label_5 |22,4,/home/vivek/catkin_ws/src/rosbuzz/src/test.bzz
|
||||
@__label_4 |22,4,/home/vivek/catkin_ws/src/rosbuzz/src/test.bzz
|
||||
pushs 2 |23,1,/home/vivek/catkin_ws/src/rosbuzz/src/test.bzz
|
||||
gload |23,1,/home/vivek/catkin_ws/src/rosbuzz/src/test.bzz
|
||||
pushs 19 |23,2,/home/vivek/catkin_ws/src/rosbuzz/src/test.bzz
|
||||
tget |23,6,/home/vivek/catkin_ws/src/rosbuzz/src/test.bzz
|
||||
pushl @__label_7 |23,7,/home/vivek/catkin_ws/src/rosbuzz/src/test.bzz
|
||||
pushi 1 |23,51,/home/vivek/catkin_ws/src/rosbuzz/src/test.bzz
|
||||
callc |23,51,/home/vivek/catkin_ws/src/rosbuzz/src/test.bzz
|
||||
pushs 1 |24,1,/home/vivek/catkin_ws/src/rosbuzz/src/test.bzz
|
||||
pushi 1 |24,2,/home/vivek/catkin_ws/src/rosbuzz/src/test.bzz
|
||||
gstore |24,3,/home/vivek/catkin_ws/src/rosbuzz/src/test.bzz
|
||||
@__label_5 |25,1,/home/vivek/catkin_ws/src/rosbuzz/src/test.bzz
|
||||
ret0 |35,1,/home/vivek/catkin_ws/src/rosbuzz/src/test.bzz
|
||||
|
||||
@__label_3
|
||||
pushs 11 |15,9,/home/vivek/catkin_ws/src/rosbuzz/src/test.bzz
|
||||
gload |15,9,/home/vivek/catkin_ws/src/rosbuzz/src/test.bzz
|
||||
pushs 12 |15,10,/home/vivek/catkin_ws/src/rosbuzz/src/test.bzz
|
||||
lload 1 |15,23,/home/vivek/catkin_ws/src/rosbuzz/src/test.bzz
|
||||
pushs 13 |15,25,/home/vivek/catkin_ws/src/rosbuzz/src/test.bzz
|
||||
pushs 14 |16,8,/home/vivek/catkin_ws/src/rosbuzz/src/test.bzz
|
||||
pushs 9 |14,9,/home/vivek/catkin_ws/src/rosbuzz/src/test.bzz
|
||||
gload |14,9,/home/vivek/catkin_ws/src/rosbuzz/src/test.bzz
|
||||
pushs 10 |14,10,/home/vivek/catkin_ws/src/rosbuzz/src/test.bzz
|
||||
lload 1 |14,23,/home/vivek/catkin_ws/src/rosbuzz/src/test.bzz
|
||||
pushs 11 |14,25,/home/vivek/catkin_ws/src/rosbuzz/src/test.bzz
|
||||
pushs 12 |15,8,/home/vivek/catkin_ws/src/rosbuzz/src/test.bzz
|
||||
lload 2 |15,28,/home/vivek/catkin_ws/src/rosbuzz/src/test.bzz
|
||||
pushs 13 |15,29,/home/vivek/catkin_ws/src/rosbuzz/src/test.bzz
|
||||
tget |15,37,/home/vivek/catkin_ws/src/rosbuzz/src/test.bzz
|
||||
pushs 14 |15,39,/home/vivek/catkin_ws/src/rosbuzz/src/test.bzz
|
||||
pushs 15 |16,8,/home/vivek/catkin_ws/src/rosbuzz/src/test.bzz
|
||||
lload 2 |16,28,/home/vivek/catkin_ws/src/rosbuzz/src/test.bzz
|
||||
pushs 15 |16,29,/home/vivek/catkin_ws/src/rosbuzz/src/test.bzz
|
||||
tget |16,37,/home/vivek/catkin_ws/src/rosbuzz/src/test.bzz
|
||||
pushs 16 |16,39,/home/vivek/catkin_ws/src/rosbuzz/src/test.bzz
|
||||
pushs 16 |16,29,/home/vivek/catkin_ws/src/rosbuzz/src/test.bzz
|
||||
tget |16,36,/home/vivek/catkin_ws/src/rosbuzz/src/test.bzz
|
||||
pushs 14 |16,38,/home/vivek/catkin_ws/src/rosbuzz/src/test.bzz
|
||||
pushs 17 |17,8,/home/vivek/catkin_ws/src/rosbuzz/src/test.bzz
|
||||
lload 2 |17,28,/home/vivek/catkin_ws/src/rosbuzz/src/test.bzz
|
||||
pushs 18 |17,29,/home/vivek/catkin_ws/src/rosbuzz/src/test.bzz
|
||||
tget |17,36,/home/vivek/catkin_ws/src/rosbuzz/src/test.bzz
|
||||
pushs 16 |17,38,/home/vivek/catkin_ws/src/rosbuzz/src/test.bzz
|
||||
pushs 19 |18,8,/home/vivek/catkin_ws/src/rosbuzz/src/test.bzz
|
||||
lload 2 |18,28,/home/vivek/catkin_ws/src/rosbuzz/src/test.bzz
|
||||
pushs 20 |18,29,/home/vivek/catkin_ws/src/rosbuzz/src/test.bzz
|
||||
tget |18,38,/home/vivek/catkin_ws/src/rosbuzz/src/test.bzz
|
||||
pushi 11 |18,40,/home/vivek/catkin_ws/src/rosbuzz/src/test.bzz
|
||||
callc |18,40,/home/vivek/catkin_ws/src/rosbuzz/src/test.bzz
|
||||
ret0 |18,41,/home/vivek/catkin_ws/src/rosbuzz/src/test.bzz
|
||||
tget |17,38,/home/vivek/catkin_ws/src/rosbuzz/src/test.bzz
|
||||
pushi 11 |17,40,/home/vivek/catkin_ws/src/rosbuzz/src/test.bzz
|
||||
callc |17,40,/home/vivek/catkin_ws/src/rosbuzz/src/test.bzz
|
||||
ret0 |17,41,/home/vivek/catkin_ws/src/rosbuzz/src/test.bzz
|
||||
|
||||
@__label_4
|
||||
ret0 |34,1,/home/vivek/catkin_ws/src/rosbuzz/src/test.bzz
|
||||
@__label_6
|
||||
pushs 20 |19,30,/home/vivek/catkin_ws/src/rosbuzz/src/test.bzz
|
||||
gload |19,30,/home/vivek/catkin_ws/src/rosbuzz/src/test.bzz
|
||||
pushi 0 |19,32,/home/vivek/catkin_ws/src/rosbuzz/src/test.bzz
|
||||
callc |19,32,/home/vivek/catkin_ws/src/rosbuzz/src/test.bzz
|
||||
ret0 |19,33,/home/vivek/catkin_ws/src/rosbuzz/src/test.bzz
|
||||
|
||||
@__label_5
|
||||
ret0 |38,1,/home/vivek/catkin_ws/src/rosbuzz/src/test.bzz
|
||||
@__label_7
|
||||
pushs 21 |23,27,/home/vivek/catkin_ws/src/rosbuzz/src/test.bzz
|
||||
gload |23,27,/home/vivek/catkin_ws/src/rosbuzz/src/test.bzz
|
||||
pushf 1.1234 |23,28,/home/vivek/catkin_ws/src/rosbuzz/src/test.bzz
|
||||
pushf 2.2345 |23,35,/home/vivek/catkin_ws/src/rosbuzz/src/test.bzz
|
||||
pushf 3.3456 |23,42,/home/vivek/catkin_ws/src/rosbuzz/src/test.bzz
|
||||
pushi 3 |23,49,/home/vivek/catkin_ws/src/rosbuzz/src/test.bzz
|
||||
callc |23,49,/home/vivek/catkin_ws/src/rosbuzz/src/test.bzz
|
||||
ret0 |23,50,/home/vivek/catkin_ws/src/rosbuzz/src/test.bzz
|
||||
|
||||
@__label_8
|
||||
ret0 |39,1,/home/vivek/catkin_ws/src/rosbuzz/src/test.bzz
|
||||
|
||||
@__label_9
|
||||
ret0 |43,1,/home/vivek/catkin_ws/src/rosbuzz/src/test.bzz
|
||||
|
|
|
|||
BIN
src/out.bdbg
BIN
src/out.bdbg
Binary file not shown.
BIN
src/out.bo
BIN
src/out.bo
Binary file not shown.
|
|
@ -1,7 +1,8 @@
|
|||
/** @file rosbuzz.cpp
|
||||
* @version 1
|
||||
* @version 1.0
|
||||
* @date 27.09.2016
|
||||
* @brief Buzz Implementation as a node in ROS.
|
||||
* @brief Buzz Implementation as a node in ROS for Dji M100 Drone.
|
||||
* @author Vivek Shankar Varadharajan
|
||||
* @copyright 2016 MistLab. All rights reserved.
|
||||
*/
|
||||
|
||||
|
|
@ -27,21 +28,28 @@
|
|||
#include <map>
|
||||
using namespace std;
|
||||
|
||||
|
||||
/**
|
||||
* This program implements Buzz node in ros using mavros_msgs, Developed for Dji M100.
|
||||
*/
|
||||
|
||||
|
||||
|
||||
static int done = 0;
|
||||
static double cur_pos[3];
|
||||
static uint64_t payload;
|
||||
static std::map< int, Pos_struct> neighbours_pos_map;
|
||||
static int timer_step=0;
|
||||
/**
|
||||
* This program implements Buzz node in ros.
|
||||
*/
|
||||
|
||||
/*Refresh neighbours Position for every ten step*/
|
||||
void maintain_pos(int tim_step){
|
||||
if(timer_step >=10){
|
||||
neighbours_pos_map.clear();
|
||||
timer_step=0;
|
||||
}
|
||||
|
||||
}
|
||||
|
||||
/*Maintain neighbours position*/
|
||||
void neighbours_pos_maintain(int id, Pos_struct pos_arr ){
|
||||
map< int, Pos_struct >::iterator it = neighbours_pos_map.find(id);
|
||||
if(it!=neighbours_pos_map.end())
|
||||
|
|
@ -49,22 +57,15 @@ neighbours_pos_map.erase(it);
|
|||
neighbours_pos_map.insert(make_pair(id, pos_arr));
|
||||
}
|
||||
|
||||
/*usage*/
|
||||
/*print usage information not needed at the moment*/
|
||||
void usage(const char* path, int status) {
|
||||
fprintf(stderr, "Usage:\n");
|
||||
fprintf(stderr, "\t%s <stream> <msg_size> <file.bo> <file.bdb>\n\n", path);
|
||||
fprintf(stderr, "== Options ==\n\n");
|
||||
fprintf(stderr, " stream The stream type: tcp or bt\n");
|
||||
fprintf(stderr, " msg_size The message size in bytes\n");
|
||||
fprintf(stderr, " file.bo The Buzz bytecode file\n");
|
||||
fprintf(stderr, " file.bdbg The Buzz debug file\n\n");
|
||||
exit(status);
|
||||
|
||||
}
|
||||
|
||||
/*Set the current position of the robot callback*/
|
||||
void set_cur_pos(double latitude,
|
||||
double longitude,
|
||||
double altitude){
|
||||
/* set the current position of the robot*/
|
||||
cur_pos [0] =latitude;
|
||||
cur_pos [1] =longitude;
|
||||
cur_pos [2] =altitude;
|
||||
|
|
@ -73,8 +74,8 @@ cur_pos [2] =altitude;
|
|||
|
||||
/*convert from catresian to spherical coordinate system callback */
|
||||
double* cvt_spherical_coordinates(double neighbours_pos_payload []){
|
||||
|
||||
double latitude,longitude,altitude;
|
||||
//for(int i=0;i<pos_vect.size();i++){
|
||||
latitude=neighbours_pos_payload[0];
|
||||
longitude = neighbours_pos_payload[1];
|
||||
altitude=neighbours_pos_payload[2];
|
||||
|
|
@ -99,39 +100,35 @@ set_cur_pos(msg->latitude,msg->longitude,msg->altitude);
|
|||
/*payload callback callback*/
|
||||
void payload_obt(const mavros_msgs::Mavlink::ConstPtr& msg)
|
||||
{
|
||||
uint64_t message_obt[(msg->payload64.end())-(msg->payload64.begin())];
|
||||
uint64_t message_obt[msg->payload64.size()];
|
||||
int i = 0;
|
||||
|
||||
// print all the remaining numbers
|
||||
/* Go throught the obtained payload*/
|
||||
for(std::vector<long unsigned int>::const_iterator it = msg->payload64.begin(); it != msg->payload64.end(); ++it)
|
||||
{
|
||||
message_obt[i] =(uint64_t) *it;
|
||||
cout<<" obtaind message "<<message_obt[i]<<endl;
|
||||
//cout<<"[Debug:] obtaind message "<<message_obt[i]<<endl;
|
||||
i++;
|
||||
}
|
||||
/* Extract neighbours position from payload*/
|
||||
double neighbours_pos_payload[3];
|
||||
memcpy(neighbours_pos_payload, message_obt, 3*sizeof(uint64_t));
|
||||
//cout<<"neighbours pos recived "<<" lat " << (double)neighbours_pos_payload[0]<<" long "<<(double)neighbours_pos_payload[1] <<" alti "<<(double)neighbours_pos_payload[2]<<endl;
|
||||
|
||||
/*Convert obtained position to relative position*/
|
||||
for(i=0;i<3;i++){
|
||||
neighbours_pos_payload[i]=neighbours_pos_payload[i]-cur_pos[i];
|
||||
}
|
||||
double* cvt_neighbours_pos_payload = cvt_spherical_coordinates(neighbours_pos_payload);
|
||||
//cout<<" neighbours pos cvt " <<" lat " << cvt_neighbours_pos_payload[0]<<" long "<<cvt_neighbours_pos_payload[1] <<" alti "<<cvt_neighbours_pos_payload[2]<<endl;
|
||||
|
||||
uint16_t* out = u64_cvt_u16((uint64_t)*(message_obt+3));
|
||||
//cout << " values " <<out[0] <<" "<<(int)out[1] <<" "<<out[2] <<" "<<out[3] <<" "<<endl;
|
||||
struct pos_struct n_pos;
|
||||
n_pos.x=cvt_neighbours_pos_payload[0];
|
||||
n_pos.y=cvt_neighbours_pos_payload[1];
|
||||
n_pos.z=cvt_neighbours_pos_payload[2];
|
||||
/*Extract robot id of the neighbour*/
|
||||
uint16_t* out = u64_cvt_u16((uint64_t)*(message_obt+3));
|
||||
/*pass neighbour position to local maintaner*/
|
||||
Pos_struct n_pos(cvt_neighbours_pos_payload[0],cvt_neighbours_pos_payload[1],cvt_neighbours_pos_payload[2]);
|
||||
neighbours_pos_maintain((int)out[1],n_pos);
|
||||
delete[] out;
|
||||
in_msg_process((message_obt+3));
|
||||
|
||||
}
|
||||
|
||||
|
||||
/*Dji RC commands service */
|
||||
int oldcmdID=0;
|
||||
int rc_cmd;
|
||||
bool rc_callback(mavros_msgs::CommandInt::Request &req,
|
||||
|
|
@ -181,7 +178,7 @@ if(req.command==oldcmdID)
|
|||
}
|
||||
|
||||
|
||||
/*controlc handler callback*/
|
||||
/*control c handler callback*/
|
||||
static void ctrlc_handler(int sig) {
|
||||
done = 1;
|
||||
}
|
||||
|
|
@ -227,9 +224,9 @@ int main(int argc, char **argv)
|
|||
|
||||
|
||||
/* The bytecode filename */
|
||||
char* bcfname = "../catkin_ws/src/rosbuzz/src/out.bo"; //argv[1];
|
||||
char* bcfname = (char*)"../catkin_ws/src/rosbuzz/src/out.bo"; //argv[1];
|
||||
/* The debugging information file name */
|
||||
char* dbgfname = "../catkin_ws/src/rosbuzz/src/out.bdbg"; //argv[2];
|
||||
char* dbgfname = (char*)"../catkin_ws/src/rosbuzz/src/out.bdbg"; //argv[2];
|
||||
/* Set CTRL-C handler */
|
||||
signal(SIGTERM, ctrlc_handler);
|
||||
signal(SIGINT, ctrlc_handler);
|
||||
|
|
@ -239,8 +236,9 @@ int main(int argc, char **argv)
|
|||
if(buzz_script_set(bcfname, dbgfname)) {
|
||||
fprintf(stdout, "Bytecode file found and set\n");
|
||||
|
||||
|
||||
/*Commands for dji flight controller*/
|
||||
mavros_msgs::CommandInt cmd_srv;
|
||||
|
||||
int count = 0;
|
||||
while (ros::ok() && !done && !buzz_script_done())
|
||||
{
|
||||
|
|
@ -262,19 +260,27 @@ int main(int argc, char **argv)
|
|||
/* diji client call*/
|
||||
if (mav_client.call(cmd_srv)){ ROS_INFO("Reply: %ld", (long int)cmd_srv.response.success); }
|
||||
else{ ROS_ERROR("Failed to call service 'djicmd'"); }
|
||||
/*Prepare Pay load to be sent*/
|
||||
/*obtain Pay load to be sent*/
|
||||
uint64_t* payload_out_ptr= out_msg_process();
|
||||
uint64_t position[3];
|
||||
/*Appened current position to message*/
|
||||
memcpy(position, cur_pos, 3*sizeof(uint64_t));
|
||||
mavros_msgs::Mavlink payload_out;
|
||||
payload_out.payload64.push_back(position[0]);
|
||||
payload_out.payload64.push_back(position[1]);
|
||||
payload_out.payload64.push_back(position[2]);
|
||||
|
||||
/*Append Buzz message*/
|
||||
uint16_t* out = u64_cvt_u16(payload_out_ptr[0]);
|
||||
for(int i=0;i<out[0];i++){
|
||||
payload_out.payload64.push_back(payload_out_ptr[i]);
|
||||
}
|
||||
int i=0;
|
||||
for(std::vector<long unsigned int>::const_iterator it = payload_out.payload64.begin(); it != payload_out.payload64.end(); ++it)
|
||||
{
|
||||
//message_obt[i] =(uint64_t) *it;
|
||||
//cout<<" [Debug:] sent message "<<*it<<endl;
|
||||
i++;
|
||||
}
|
||||
/*publish prepared messages in respective topic*/
|
||||
payload_pub.publish(payload_out);
|
||||
delete[] out;
|
||||
|
|
@ -286,13 +292,7 @@ int main(int argc, char **argv)
|
|||
++count;
|
||||
timer_step+=1;
|
||||
maintain_pos(timer_step);
|
||||
/* cout<<"time step"<<timer_step<<endl;
|
||||
map< int, Pos_struct >::iterator it;
|
||||
for (it=neighbours_pos_map.begin(); it!=neighbours_pos_map.end(); ++it){
|
||||
std::cout << it->first << " => " << (it->second).x <<" " << (it->second).y <<" " << (it->second).z<<endl;
|
||||
}
|
||||
*/
|
||||
|
||||
|
||||
}
|
||||
/* Cleanup */
|
||||
buzz_script_destroy();
|
||||
|
|
|
|||
23
src/test.bzz
23
src/test.bzz
|
|
@ -1,31 +1,36 @@
|
|||
# Executed once at init time.
|
||||
function init() {
|
||||
i=0
|
||||
v = stigmergy.create(1)
|
||||
#v.put("a", 6)
|
||||
s = swarm.create(10)
|
||||
s.join()
|
||||
|
||||
}
|
||||
|
||||
# Executed at each time step.
|
||||
function step() {
|
||||
s = swarm.create(10)
|
||||
s.join()
|
||||
|
||||
# print all the neighbours position
|
||||
neighbors.foreach(
|
||||
function(rid, data) {
|
||||
print("robot ", rid, ": ",
|
||||
"distance = ", data.distance, ", ",
|
||||
"azimuth = ", data.azimuth, ", ",
|
||||
"elevation = ", data.elevation) })
|
||||
#if(i==1){
|
||||
if(i==1){
|
||||
s.exec(function() {uav_takeoff()})
|
||||
i=0
|
||||
}
|
||||
else{
|
||||
s.exec(function() {uav_goto(1.1234,2.2345,3.3456)})
|
||||
i=1
|
||||
}
|
||||
#if(i==5){
|
||||
#s.leave()
|
||||
#i=0
|
||||
#}
|
||||
#else{
|
||||
#i=1
|
||||
#}
|
||||
#s.select(uav_takeoff)
|
||||
#print("Distance to source:", mydist, neighbors.count())
|
||||
|
||||
#i = i + 1
|
||||
|
||||
}
|
||||
|
||||
|
|
|
|||
|
|
@ -4,50 +4,20 @@
|
|||
/****************************************/
|
||||
/****************************************/
|
||||
|
||||
//knet_dev_t* DSPIC;
|
||||
static const int ACC_INC = 3;
|
||||
static const int ACC_DIV = 0;
|
||||
static const int MIN_SPEED_ACC = 20;
|
||||
static const int MIN_SPEED_DEC = 1;
|
||||
static const int MAX_SPEED = 400; /* mm/sec */
|
||||
|
||||
/****************************************/
|
||||
/*To do !*/
|
||||
/****************************************/
|
||||
|
||||
void uav_setup() {
|
||||
/* Set the libkhepera debug level */
|
||||
/*kb_set_debug_level(2);
|
||||
// initiate libkhepera and robot access
|
||||
if(kh4_init(0, NULL) !=0)
|
||||
{
|
||||
printf("\nERROR: could not initialize the khepera!\n\n");
|
||||
return;
|
||||
}
|
||||
/* open robot socket and store the handle in their respective pointers
|
||||
DSPIC = knet_open("Khepera4:dsPic", KNET_BUS_I2C, 0, NULL);
|
||||
/* Set speed profile
|
||||
kh4_SetSpeedProfile(ACC_INC, /* Acceleration increment
|
||||
ACC_DIV, /* Acceleration divider
|
||||
MIN_SPEED_ACC, /* Minimum speed acc
|
||||
MIN_SPEED_DEC, /* Minimum speed dec
|
||||
MAX_SPEED, /* Maximum speed
|
||||
DSPIC
|
||||
);
|
||||
kh4_SetMode(kh4RegSpeedProfile, DSPIC);
|
||||
/* Mute ultrasonic sensor
|
||||
kh4_activate_us(0, DSPIC);*/
|
||||
|
||||
}
|
||||
|
||||
/****************************************/
|
||||
/*To do !*/
|
||||
/****************************************/
|
||||
|
||||
void uav_done() {
|
||||
/* /* Stop wheels
|
||||
kh4_set_speed(0, 0, DSPIC);
|
||||
/* Set motors to idle
|
||||
kh4_SetMode(kh4RegIdle, DSPIC);
|
||||
/* Clear rgb leds because they consume energy
|
||||
kh4_SetRGBLeds(0, 0, 0, 0, 0, 0, 0, 0, 0, DSPIC); */
|
||||
|
||||
fprintf(stdout, "Robot stopped.\n");
|
||||
}
|
||||
|
||||
|
|
|
|||
|
|
@ -1,12 +1,9 @@
|
|||
#ifndef UAV_UTILITY_H
|
||||
#define UAV_UTILITY_H
|
||||
|
||||
//#include <khepera/khepera.h>
|
||||
|
||||
extern void uav_setup();
|
||||
|
||||
extern void uav_done();
|
||||
|
||||
//extern knet_dev_t* DSPIC;
|
||||
|
||||
#endif
|
||||
|
|
|
|||
Loading…
Reference in New Issue