Spiri
/
ROSBuzz_MISTLab
12
0
Fork 0
Code Issues Pull Requests Packages Projects Releases Wiki Activity

Class Implementaion and code clean commit

This commit is contained in:
Vivek Shankar Varadharajan 2016-10-10 20:22:17 -04:00
parent 6bb28d7ae6
commit 9b25ad53bd
15 changed files with 875 additions and 630 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

13
CMakeLists.txt
View File

@ -1,5 +1,10 @@
cmake_minimum_required(VERSION 2.8.3)
project(rosbuzz)
if(UNIX)
SET(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -Wall -std=gnu++11")
endif()
## Find catkin macros and libraries
find_package(catkin REQUIRED COMPONENTS
roscpp
@ -13,18 +18,24 @@ find_package(catkin REQUIRED COMPONENTS
###################################
catkin_package(
#CATKIN_DEPENDS message_runtime
INCLUDE_DIRS include
# LIBRARIES xbee_ros_node
CATKIN_DEPENDS roscpp std_msgs mavros_msgs sensor_msgs
# DEPENDS system_lib
)
###########
## Build ##
###########
include_directories(
include ${rosbuzz_INCLUDE_DIRS}
${catkin_INCLUDE_DIRS}
)
# Executables
add_executable(rosbuzz_node src/rosbuzz.cpp
src/roscontroller.cpp
src/buzz_utility.cpp
src/buzzuav_closures.cpp
src/uav_utility.cpp)

44
include/buzz_utility.h Normal file
View File

@ -0,0 +1,44 @@
#pragma once
#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 <iostream>
#include <stdint.h>
#include <map>
using namespace std;
namespace buzz_utility{
struct pos_struct
{
double x,y,z;
pos_struct(double x,double y,double z):x(x),y(y),z(z){};
pos_struct(){}
};
typedef struct pos_struct Pos_struct ;
uint16_t* u64_cvt_u16(uint64_t u64);
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();
int buzz_script_set(const char* bo_filename,
const char* bdbg_filename, int robot_id);
void buzz_script_step();
void buzz_script_destroy();
int buzz_script_done();
}

58
include/buzzuav_closures.h Normal file
View File

@ -0,0 +1,58 @@
#pragma once
//#ifndef BUZZUAV_CLOSURES_H
//#define BUZZUAV_CLOSURES_H
#include <buzz/buzzvm.h>
#include <stdio.h>
#include "uav_utility.h"
#include "mavros_msgs/CommandCode.h"
#include "ros/ros.h"
namespace buzzuav_closures{
/*
* prextern int() function in Buzz
* This function is used to print data from buzz
* The command to use in Buzz is buzzros_print takes any available datatype in Buzz
*/
int buzzros_print(buzzvm_t vm);
/*
* buzzuav_goto(latitude,longitude,altitude) function in Buzz
* commands the UAV to go to a position supplied
*/
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 */
void set_battery(float voltage,float current,float remaining);
/*retuns the current go to position */
double* getgoto();
/*
* Commands the UAV to takeoff
*/
int buzzuav_takeoff(buzzvm_t vm);
/* Commands the UAV to land
*/
int buzzuav_land(buzzvm_t vm);
/* Command the UAV to go to home location
*/
int buzzuav_gohome(buzzvm_t vm);
/*
* Updates battery information in Buzz
*/
int buzzuav_update_battery(buzzvm_t vm);
/*
* Updates IR information in Buzz
* Proximity and ground sensors to do !!!!
*/
int buzzuav_update_prox(buzzvm_t vm);
//#endif
}

103
include/roscontroller.h Normal file
View File

@ -0,0 +1,103 @@
#pragma once
#include "ros/ros.h"
#include "sensor_msgs/NavSatFix.h"
#include "mavros_msgs/GlobalPositionTarget.h"
#include "mavros_msgs/CommandCode.h"
#include "mavros_msgs/CommandInt.h"
#include "mavros_msgs/State.h"
#include "mavros_msgs/BatteryStatus.h"
#include "mavros_msgs/Mavlink.h"
#include "sensor_msgs/NavSatStatus.h"
#include <sstream>
#include <buzz/buzzasm.h>
#include "buzzuav_closures.h"
#include "buzz_utility.h"
#include "uav_utility.h"
#include <stdlib.h>
#include <string.h>
#include <math.h>
#include <signal.h>
#include <ostream>
#include <map>
using namespace std;
namespace rosbzz_node{
class roscontroller{
public:
roscontroller();
~roscontroller();
//void RosControllerInit();
void RosControllerRun();
private:
double cur_pos[3];
uint64_t payload;
std::map< int, buzz_utility::Pos_struct> neighbours_pos_map;
int timer_step=0;
int robot_id=0;
int oldcmdID=0;
int rc_cmd;
std::string bzzfile_name, fcclient_name, rcservice_name; //, rcclient;
bool rcclient;
ros::ServiceClient mav_client;
ros::Publisher payload_pub;
ros::ServiceServer service;
ros::Subscriber current_position_sub;
ros::Subscriber battery_sub;
ros::Subscriber payload_sub;
/*Create node Handler*/
ros::NodeHandle n_c;
/*Commands for flight controller*/
mavros_msgs::CommandInt cmd_srv;
/* The bytecode filename */
char* bcfname = (char*)"../catkin_ws/src/rosbuzz/src/out.bo"; //argv[1];
/* The debugging information file name */
char* dbgfname = (char*)"../catkin_ws/src/rosbuzz/src/out.bdbg"; //argv[2];
void Initialize_pub_sub();
/*Obtain data from ros parameter server*/
void Rosparameters_get();
void Compile_bzz();
/*Prepare messages and publish*/
inline void prepare_msg_and_publish();
/*Refresh neighbours Position for every ten step*/
inline void maintain_pos(int tim_step);
/*Maintain neighbours position*/
inline void neighbours_pos_maintain(int id, buzz_utility::Pos_struct pos_arr );
/*Set the current position of the robot callback*/
inline void set_cur_pos(double latitude,
double longitude,
double altitude);
/*convert from catresian to spherical coordinate system callback */
inline double* cvt_spherical_coordinates(double neighbours_pos_payload []);
/*battery status callback*/
inline void battery(const mavros_msgs::BatteryStatus::ConstPtr& msg);
/*current position callback*/
inline void current_pos(const sensor_msgs::NavSatFix::ConstPtr& msg);
/*payload callback callback*/
inline void payload_obt(const mavros_msgs::Mavlink::ConstPtr& msg);
/* RC commands service */
inline bool rc_callback(mavros_msgs::CommandInt::Request &req,
mavros_msgs::CommandInt::Response &res);
};
}

0
src/uav_utility.h → include/uav_utility.h
View File

8
package.xml
View File

@ -40,6 +40,14 @@
<!-- Use test_depend for packages you need only for testing: -->
<!-- <test_depend>gtest</test_depend> -->
<buildtool_depend>catkin</buildtool_depend>
<build_depend>roscpp</build_depend>
<run_depend>roscpp</run_depend>
<build_depend>std_msgs</build_depend>
<run_depend>std_msgs</run_depend>
<build_depend>mavros_msgs</build_depend>
<run_depend>mavros_msgs</run_depend>
<build_depend>sensor_msgs</build_depend>
<run_depend>sensor_msgs</run_depend>
<!-- <build_depend>message_generation</build_depend> -->
<!-- <run_depend>message_runtime</run_depend> -->

475
src/buzz_utility.cpp
View File

@ -5,266 +5,257 @@
* @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 <iostream>
#include <map>
using namespace std;
/****************************************/
/****************************************/
static buzzvm_t VM = 0;
static char* BO_FNAME = 0;
static uint8_t* BO_BUF = 0;
static buzzdebug_t DBG_INFO = 0;
static int MSG_SIZE = 32;
static int TCP_LIST_STREAM = -1;
static int TCP_COMM_STREAM = -1;
static uint8_t* STREAM_SEND_BUF = NULL;
namespace buzz_utility{
/****************************************/
/****************************************/
/****************************************/
/*adds neighbours position*/
void neighbour_pos_callback(std::map< int, Pos_struct> neighbours_pos_map){
/* Reset neighbor information */
buzzneighbors_reset(VM);
/* Get robot id and update neighbor information */
map< int, Pos_struct >::iterator it;
for (it=neighbours_pos_map.begin(); it!=neighbours_pos_map.end(); ++it){
buzzneighbors_add(VM,
it->first,
(it->second).x,
(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;
uint32_t int32_2 = (u64 & 0xFFFFFFFF00000000 ) >> 32;
out[0] = int32_1 & 0xFFFF;
out[1] = (int32_1 & (0xFFFF0000) ) >> 16;
out[2] = int32_2 & 0xFFFF;
out[3] = (int32_2 & (0xFFFF0000) ) >> 16;
//cout << " values " <<out[0] <<" "<<out[1] <<" "<<out[2] <<" "<<out[3] <<" ";
return out;
}
static buzzvm_t VM = 0;
static char* BO_FNAME = 0;
static uint8_t* BO_BUF = 0;
static buzzdebug_t DBG_INFO = 0;
static unsigned int MSG_SIZE = 32;
/****************************************/
/*adds neighbours position*/
void neighbour_pos_callback(std::map< int, Pos_struct> neighbours_pos_map){
/* Reset neighbor information */
buzzneighbors_reset(VM);
/* Get robot id and update neighbor information */
map< int, Pos_struct >::iterator it;
for (it=neighbours_pos_map.begin(); it!=neighbours_pos_map.end(); ++it){
buzzneighbors_add(VM,
it->first,
(it->second).x,
(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;
uint32_t int32_2 = (u64 & 0xFFFFFFFF00000000 ) >> 32;
out[0] = int32_1 & 0xFFFF;
out[1] = (int32_1 & (0xFFFF0000) ) >> 16;
out[2] = int32_2 & 0xFFFF;
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*/
/***************************************************/
/***************************************************/
/*Appends obtained messages to buzz in message Queue*/
/***************************************************/
void in_msg_process(uint64_t* payload){
void in_msg_process(uint64_t* payload){
/* Go through messages and add them to the FIFO */
uint16_t* data= u64_cvt_u16((uint64_t)payload[0]);
uint16_t size=data[0]*sizeof(uint64_t);
delete[] data;
uint8_t* pl =(uint8_t*)malloc(size);
memset(pl, 0,size);
/* Copy packet into temporary buffer */
memcpy(pl, payload ,size);
/* Go through messages and add them to the FIFO */
uint16_t* data= u64_cvt_u16((uint64_t)payload[0]);
uint16_t size=data[0]*sizeof(uint64_t);
delete[] data;
uint8_t* pl =(uint8_t*)malloc(size);
memset(pl, 0,size);
/* 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;
do {
/* Get payload size */
unMsgSize = *(uint16_t*)(pl + tot);
tot += sizeof(uint16_t);
/* Append message to the Buzz input message queue */
if(unMsgSize > 0 && unMsgSize <= size*sizeof(uint64_t) - tot) {
buzzinmsg_queue_append(VM->inmsgs,
buzzmsg_payload_frombuffer(pl +tot, unMsgSize));
tot += unMsgSize;
}
}while(size - tot > sizeof(uint16_t) && unMsgSize > 0);
/* Go through the messages until there's nothing else to read */
uint16_t unMsgSize;
do {
/* Get payload size */
unMsgSize = *(uint16_t*)(pl + tot);
tot += sizeof(uint16_t);
/* Append message to the Buzz input message queue */
if(unMsgSize > 0 && unMsgSize <= size*sizeof(uint64_t) - tot) {
buzzinmsg_queue_append(VM->inmsgs,
buzzmsg_payload_frombuffer(pl +tot, unMsgSize));
tot += unMsgSize;
}
}while(size - tot > sizeof(uint16_t) && unMsgSize > 0);
/* Process messages */
buzzvm_process_inmsgs(VM);
}
/***************************************************/
/*Obtains messages from buzz out message Queue*/
/***************************************************/
/* 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 */
*(uint16_t*)(buff_send+tot) = (uint16_t) VM->robot;
tot += sizeof(uint16_t);
/* Send messages from FIFO */
do {
/* Are there more messages? */
if(buzzoutmsg_queue_isempty(VM->outmsgs)) break;
/* Get first message */
buzzmsg_payload_t m = buzzoutmsg_queue_first(VM->outmsgs);
/* Make sure the next message fits the data buffer */
if(tot + buzzmsg_payload_size(m) + sizeof(uint16_t)
>
MSG_SIZE) {
buzzmsg_payload_destroy(&m);
break;
}
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 */
*(uint16_t*)(buff_send+tot) = (uint16_t) VM->robot;
tot += sizeof(uint16_t);
/* Send messages from FIFO */
do {
/* Are there more messages? */
if(buzzoutmsg_queue_isempty(VM->outmsgs)) break;
/* Get first message */
buzzmsg_payload_t m = buzzoutmsg_queue_first(VM->outmsgs);
/* Make sure the next message fits the data buffer */
if(tot + buzzmsg_payload_size(m) + sizeof(uint16_t)
>
MSG_SIZE) {
buzzmsg_payload_destroy(&m);
break;
}
/* Add message length to data buffer */
*(uint16_t*)(buff_send + tot) = (uint16_t)buzzmsg_payload_size(m);
tot += sizeof(uint16_t);
/* Add message length to data buffer */
*(uint16_t*)(buff_send + tot) = (uint16_t)buzzmsg_payload_size(m);
tot += sizeof(uint16_t);
/* Add payload to data buffer */
memcpy(buff_send + tot, m->data, buzzmsg_payload_size(m));
tot += buzzmsg_payload_size(m);
/* Add payload to data buffer */
memcpy(buff_send + tot, m->data, buzzmsg_payload_size(m));
tot += buzzmsg_payload_size(m);
/* Get rid of message */
buzzoutmsg_queue_next(VM->outmsgs);
buzzmsg_payload_destroy(&m);
} while(1);
/* Get rid of message */
buzzoutmsg_queue_next(VM->outmsgs);
buzzmsg_payload_destroy(&m);
} while(1);
int total_size =(ceil((float)tot/(float)sizeof(uint64_t)));
*(uint16_t*)buff_send = (uint16_t) total_size;
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));
/*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*/
/****************************************/
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() {
buzzdebug_entry_t dbg = *buzzdebug_info_get_fromoffset(DBG_INFO, &VM->pc);
char* msg;
if(dbg != NULL) {
asprintf(&msg,
"%s: execution terminated abnormally at %s:%" PRIu64 ":%" PRIu64 " : %s\n\n",
BO_FNAME,
dbg->fname,
dbg->line,
dbg->col,
VM->errormsg);
}
else {
asprintf(&msg,
"%s: execution terminated abnormally at bytecode offset %d: %s\n\n",
BO_FNAME,
VM->pc,
VM->errormsg);
}
return msg;
}
static const char* buzz_error_info() {
buzzdebug_entry_t dbg = *buzzdebug_info_get_fromoffset(DBG_INFO, &VM->pc);
char* msg;
if(dbg != NULL) {
asprintf(&msg,
"%s: execution terminated abnormally at %s:%" PRIu64 ":%" PRIu64 " : %s\n\n",
BO_FNAME,
dbg->fname,
dbg->line,
dbg->col,
VM->errormsg);
}
else {
asprintf(&msg,
"%s: execution terminated abnormally at bytecode offset %d: %s\n\n",
BO_FNAME,
VM->pc,
VM->errormsg);
}
return msg;
}
/****************************************/
/*Buzz hooks that can be used inside .bzz file*/
/****************************************/
/****************************************/
/*Buzz hooks that can be used inside .bzz file*/
/****************************************/
static int buzz_register_hooks() {
buzzvm_pushs(VM, buzzvm_string_register(VM, "print", 1));
buzzvm_pushcc(VM, buzzvm_function_register(VM, buzzros_print));
buzzvm_gstore(VM);
buzzvm_pushs(VM, buzzvm_string_register(VM, "uav_goto", 1));
buzzvm_pushcc(VM, buzzvm_function_register(VM, buzzuav_goto));
buzzvm_gstore(VM);
buzzvm_pushs(VM, buzzvm_string_register(VM, "uav_takeoff", 1));
buzzvm_pushcc(VM, buzzvm_function_register(VM, buzzuav_takeoff));
buzzvm_gstore(VM);
buzzvm_pushs(VM, buzzvm_string_register(VM, "uav_gohome", 1));
buzzvm_pushcc(VM, buzzvm_function_register(VM, buzzuav_gohome));
buzzvm_gstore(VM);
buzzvm_pushs(VM, buzzvm_string_register(VM, "uav_land", 1));
buzzvm_pushcc(VM, buzzvm_function_register(VM, buzzuav_land));
buzzvm_gstore(VM);
return BUZZVM_STATE_READY;
}
static int buzz_register_hooks() {
buzzvm_pushs(VM, buzzvm_string_register(VM, "print", 1));
buzzvm_pushcc(VM, buzzvm_function_register(VM, buzzuav_closures::buzzros_print));
buzzvm_gstore(VM);
buzzvm_pushs(VM, buzzvm_string_register(VM, "uav_goto", 1));
buzzvm_pushcc(VM, buzzvm_function_register(VM, buzzuav_closures::buzzuav_goto));
buzzvm_gstore(VM);
buzzvm_pushs(VM, buzzvm_string_register(VM, "uav_takeoff", 1));
buzzvm_pushcc(VM, buzzvm_function_register(VM, buzzuav_closures::buzzuav_takeoff));
buzzvm_gstore(VM);
buzzvm_pushs(VM, buzzvm_string_register(VM, "uav_gohome", 1));
buzzvm_pushcc(VM, buzzvm_function_register(VM, buzzuav_closures::buzzuav_gohome));
buzzvm_gstore(VM);
buzzvm_pushs(VM, buzzvm_string_register(VM, "uav_land", 1));
buzzvm_pushcc(VM, buzzvm_function_register(VM, buzzuav_closures::buzzuav_land));
buzzvm_gstore(VM);
return BUZZVM_STATE_READY;
}
/****************************************/
/*Sets the .bzz and .bdbg file*/
/****************************************/
/****************************************/
/*Sets the .bzz and .bdbg file*/
/****************************************/
int buzz_script_set(const char* bo_filename,
const char* bdbg_filename, int robot_id) {
/* Get hostname */
char hstnm[30];
gethostname(hstnm, 30);
/* Make numeric id from hostname */
/* NOTE: here we assume that the hostname is in the format Knn */
int id = robot_id; //strtol(hstnm + 1, NULL, 10);
cout << " Robot ID" << id<< endl;
/* Reset the Buzz VM */
if(VM) buzzvm_destroy(&VM);
VM = buzzvm_new(id);
/* Get rid of debug info */
if(DBG_INFO) buzzdebug_destroy(&DBG_INFO);
DBG_INFO = buzzdebug_new();
/* Read bytecode and fill in data structure */
FILE* fd = fopen(bo_filename, "rb");
if(!fd) {
perror(bo_filename);
return 0;
}
fseek(fd, 0, SEEK_END);
size_t bcode_size = ftell(fd);
rewind(fd);
BO_BUF = (uint8_t*)malloc(bcode_size);
if(fread(BO_BUF, 1, bcode_size, fd) < bcode_size) {
perror(bo_filename);
buzzvm_destroy(&VM);
buzzdebug_destroy(&DBG_INFO);
fclose(fd);
return 0;
}
fclose(fd);
/* Read debug information */
if(!buzzdebug_fromfile(DBG_INFO, bdbg_filename)) {
buzzvm_destroy(&VM);
buzzdebug_destroy(&DBG_INFO);
perror(bdbg_filename);
return 0;
}
/* Set byte code */
if(buzzvm_set_bcode(VM, BO_BUF, bcode_size) != BUZZVM_STATE_READY) {
buzzvm_destroy(&VM);
buzzdebug_destroy(&DBG_INFO);
fprintf(stdout, "%s: Error loading Buzz script\n\n", bo_filename);
return 0;
}
/* Register hook functions */
if(buzz_register_hooks() != BUZZVM_STATE_READY) {
buzzvm_destroy(&VM);
buzzdebug_destroy(&DBG_INFO);
fprintf(stdout, "%s: Error registering hooks\n\n", bo_filename);
return 0;
}
/* Save bytecode file name */
BO_FNAME = strdup(bo_filename);
/* Execute the global part of the script */
buzzvm_execute_script(VM);
/* Call the Init() function */
buzzvm_function_call(VM, "init", 0);
/* All OK */
return 1;
}
int buzz_script_set(const char* bo_filename,
const char* bdbg_filename, int robot_id) {
/* Get hostname */
char hstnm[30];
gethostname(hstnm, 30);
/* Make numeric id from hostname */
/* NOTE: here we assume that the hostname is in the format Knn */
int id = robot_id; //strtol(hstnm + 1, NULL, 10);
cout << " Robot ID" << id<< endl;
/* Reset the Buzz VM */
if(VM) buzzvm_destroy(&VM);
VM = buzzvm_new(id);
/* Get rid of debug info */
if(DBG_INFO) buzzdebug_destroy(&DBG_INFO);
DBG_INFO = buzzdebug_new();
/* Read bytecode and fill in data structure */
FILE* fd = fopen(bo_filename, "rb");
if(!fd) {
perror(bo_filename);
return 0;
}
fseek(fd, 0, SEEK_END);
size_t bcode_size = ftell(fd);
rewind(fd);
BO_BUF = (uint8_t*)malloc(bcode_size);
if(fread(BO_BUF, 1, bcode_size, fd) < bcode_size) {
perror(bo_filename);
buzzvm_destroy(&VM);
buzzdebug_destroy(&DBG_INFO);
fclose(fd);
return 0;
}
fclose(fd);
/* Read debug information */
if(!buzzdebug_fromfile(DBG_INFO, bdbg_filename)) {
buzzvm_destroy(&VM);
buzzdebug_destroy(&DBG_INFO);
perror(bdbg_filename);
return 0;
}
/* Set byte code */
if(buzzvm_set_bcode(VM, BO_BUF, bcode_size) != BUZZVM_STATE_READY) {
buzzvm_destroy(&VM);
buzzdebug_destroy(&DBG_INFO);
fprintf(stdout, "%s: Error loading Buzz script\n\n", bo_filename);
return 0;
}
/* Register hook functions */
if(buzz_register_hooks() != BUZZVM_STATE_READY) {
buzzvm_destroy(&VM);
buzzdebug_destroy(&DBG_INFO);
fprintf(stdout, "%s: Error registering hooks\n\n", bo_filename);
return 0;
}
/* Save bytecode file name */
BO_FNAME = strdup(bo_filename);
/* Execute the global part of the script */
buzzvm_execute_script(VM);
/* Call the Init() function */
buzzvm_function_call(VM, "init", 0);
/* All OK */
return 1;
}
/****************************************/
/*Swarm struct*/
/****************************************/
/****************************************/
/*Swarm struct*/
/****************************************/
struct buzzswarm_elem_s {
buzzdarray_t swarms;
@ -307,8 +298,8 @@ void buzz_script_step() {
/*
* Update sensors
*/
buzzuav_update_battery(VM);
buzzuav_update_prox(VM);
buzzuav_closures::buzzuav_update_battery(VM);
buzzuav_closures::buzzuav_update_prox(VM);
/*
* Call Buzz step() function
*/
@ -351,6 +342,10 @@ void buzz_script_destroy() {
fprintf(stdout, "Script execution stopped.\n");
}
/****************************************/
/****************************************/
/****************************************/
/*Execution completed*/
/****************************************/
@ -359,7 +354,7 @@ int buzz_script_done() {
return VM->state != BUZZVM_STATE_READY;
}
/****************************************/
/****************************************/
}

18
src/buzz_utility.h
View File

@ -1,13 +1,25 @@
#ifndef BUZZ_UTILITY_H
#define BUZZ_UTILITY_H
#pragma once
#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 <iostream>
#include <stdint.h>
#include <map>
using namespace std;
namespace buzz_utility{
struct pos_struct
{
double x,y,z;
pos_struct(double x,double y,double z):x(x),y(y),z(z){};
pos_struct(){}
};
typedef struct pos_struct Pos_struct ;
uint16_t* u64_cvt_u16(uint64_t u64);
@ -29,4 +41,4 @@ void buzz_script_destroy();
int buzz_script_done();
#endif
}

56
src/buzzasm.h
View File

@ -1,56 +0,0 @@
#ifndef BUZZASM_H
#define BUZZASM_H
#include <buzz/buzzvm.h>
#include <buzz/buzzdebug.h>
#ifdef __cplusplus
extern "C" {
#endif
/*
* Compiles an assembly file into bytecode.
* @param fname The file name where the code is located.
* @param buf The buffer in which the bytecode will be stored. Created internally.
* @param size The size of the bytecode buffer.
* @param dbg The debug data structure to fill into. Created internally.
* @return 0 if no error occurred, 1 for I/O error, 2 for compilation error.
*/
extern int buzz_asm(const char* fname,
uint8_t** buf,
uint32_t* size,
buzzdebug_t* dbg);
/*
* Decompiles bytecode into an assembly file.
* @param buf The buffer in which the bytecode is stored.
* @param size The size of the bytecode buffer.
* @param dbg The debug data structure.
* @param fname The file name where the assembly will be written.
* @return 0 if no error occurred, 1 for I/O error, 2 for decompilation error.
*/
extern int buzz_deasm(const uint8_t* buf,
uint32_t size,
buzzdebug_t dbg,
const char* fname);
/*
* Decompiles the bytecode of a single instruction.
* This function writes the decompiled instruction into a new string pointed to by
* *buf. When you're done with it, you must free it.
* NOTE: no sanity check is performed to make sure that this function does not
* read beyond the bcode buffer size.
* @param bcode The buffer in which the bytecode is stored.
* @param off The offset at which decompilation must occur.
* @param buf The bytecode where the
* @return 1 if no error occurred, 0 for decompilation error.
*/
extern int buzz_instruction_deasm(const uint8_t* bcode,
uint32_t off,
char** buf);
#ifdef __cplusplus
}
#endif
#endif

30
src/buzzuav_closures.cpp
View File

@ -6,15 +6,12 @@
* @copyright 2016 MistLab. All rights reserved.
*/
//#define _GNU_SOURCE
#include <stdio.h>
#include "buzzuav_closures.h"
#include "uav_utility.h"
#include "mavros_msgs/CommandCode.h"
#include "ros/ros.h"
double goto_pos[3];
float batt[3];
int cur_cmd;
namespace buzzuav_closures{
static double goto_pos[3];
static float batt[3];
static int cur_cmd;
static int rc_cmd=0;
/****************************************/
/****************************************/
@ -84,7 +81,12 @@ return goto_pos;
}
/******************************/
int getcmd(){
if(rc_cmd==0) return cur_cmd;
else {
cur_cmd=rc_cmd;
rc_cmd=0;
return cur_cmd;
}
}
void set_goto(double pos[]){
@ -94,8 +96,8 @@ goto_pos[2]=pos[2];
}
void rc_call(int rc_cmd){
cur_cmd=rc_cmd;
void rc_call(int rc_cmd_in){
rc_cmd=rc_cmd_in;
}
/****************************************/
@ -128,7 +130,7 @@ void set_battery(float voltage,float current,float remaining){
/****************************************/
int buzzuav_update_battery(buzzvm_t vm) {
static char BATTERY_BUF[256];
//static char BATTERY_BUF[256];
buzzvm_pushs(vm, buzzvm_string_register(vm, "battery", 1));
buzzvm_pusht(vm);
buzzvm_dup(vm);
@ -172,9 +174,11 @@ int buzzuav_update_prox(buzzvm_t vm) {
buzzvm_pushi(vm, (PROXIMITY_BUF[i*2] | PROXIMITY_BUF[i*2+1] << 8));
buzzvm_tput(vm);
}
buzzvm_gstore(vm);
return vm->state;*/
buzzvm_gstore(vm);*/
return vm->state;
}
/****************************************/
/****************************************/
}

15
src/buzzuav_closures.h
View File

@ -1,7 +1,13 @@
#ifndef BUZZUAV_CLOSURES_H
#define BUZZUAV_CLOSURES_H
#pragma once
//#ifndef BUZZUAV_CLOSURES_H
//#define BUZZUAV_CLOSURES_H
#include <buzz/buzzvm.h>
#include <stdio.h>
#include "uav_utility.h"
#include "mavros_msgs/CommandCode.h"
#include "ros/ros.h"
namespace buzzuav_closures{
/*
* prextern int() function in Buzz
@ -49,4 +55,5 @@ int buzzuav_update_battery(buzzvm_t vm);
*/
int buzzuav_update_prox(buzzvm_t vm);
#endif
//#endif
}

318
src/rosbuzz.cpp
View File

@ -6,325 +6,19 @@
* @copyright 2016 MistLab. All rights reserved.
*/
#include "ros/ros.h"
#include "sensor_msgs/NavSatFix.h"
#include "mavros_msgs/GlobalPositionTarget.h"
#include "mavros_msgs/CommandCode.h"
#include "mavros_msgs/CommandInt.h"
#include "mavros_msgs/State.h"
#include "mavros_msgs/BatteryStatus.h"
#include "mavros_msgs/Mavlink.h"
#include "sensor_msgs/NavSatStatus.h"
#include <sstream>
#include <buzz/buzzasm.h>
#include "buzzuav_closures.h"
#include "buzz_utility.h"
#include "uav_utility.h"
#include <stdlib.h>
#include <string.h>
#include <math.h>
#include <signal.h>
#include <ostream>
#include <map>
using namespace std;
#include "roscontroller.h"
/**
* This program implements Buzz node in ros using mavros_msgs.
*/
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;
static int robot_id=0;
/*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())
neighbours_pos_map.erase(it);
neighbours_pos_map.insert(make_pair(id, pos_arr));
}
/*print usage information not needed at the moment*/
void usage(const char* path, int status) {
}
/*Set the current position of the robot callback*/
void set_cur_pos(double latitude,
double longitude,
double altitude){
cur_pos [0] =latitude;
cur_pos [1] =longitude;
cur_pos [2] =altitude;
}
/*convert from catresian to spherical coordinate system callback */
double* cvt_spherical_coordinates(double neighbours_pos_payload []){
double latitude,longitude,altitude;
latitude=neighbours_pos_payload[0];
longitude = neighbours_pos_payload[1];
altitude=neighbours_pos_payload[2];
neighbours_pos_payload[0]=sqrt(pow(latitude,2.0)+pow(longitude,2.0)+pow(altitude,2.0));
neighbours_pos_payload[1]=atan(longitude/latitude);
neighbours_pos_payload[2]=atan((sqrt(pow(latitude,2.0)+pow(longitude,2.0)))/altitude);
return neighbours_pos_payload;
}
/*battery status callback*/
void battery(const mavros_msgs::BatteryStatus::ConstPtr& msg)
{
set_battery(msg->voltage,msg->current,msg->remaining);
}
/*current position callback*/
void current_pos(const sensor_msgs::NavSatFix::ConstPtr& msg)
{
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.size()];
int i = 0;
/* 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<<"[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));
/*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);
/*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,
mavros_msgs::CommandInt::Response &res){
if(req.command==oldcmdID)
return true;
else
oldcmdID=req.command;
switch(req.command)
{
case mavros_msgs::CommandCode::NAV_TAKEOFF:
ROS_INFO("RC_call: TAKE OFF!!!!");
rc_cmd=mavros_msgs::CommandCode::NAV_TAKEOFF;
rc_call(rc_cmd);
res.success = true;
break;
case mavros_msgs::CommandCode::NAV_LAND:
ROS_INFO("RC_Call: LAND!!!!");
rc_cmd=mavros_msgs::CommandCode::NAV_LAND;
rc_call(rc_cmd);
res.success = true;
break;
case mavros_msgs::CommandCode::NAV_RETURN_TO_LAUNCH:
ROS_INFO("RC_Call: GO HOME!!!!");
rc_cmd=mavros_msgs::CommandCode::NAV_RETURN_TO_LAUNCH;
rc_call(rc_cmd);
res.success = true;
break;
case mavros_msgs::CommandCode::NAV_WAYPOINT:
ROS_INFO("RC_Call: GO TO!!!! x = %f , y = %f , Z = %f",req.param1,req.param2,req.param3);
double rc_goto[3];
rc_goto[0]=req.param1;
rc_goto[1]=req.param2;
rc_goto[2]=req.param3;
set_goto(rc_goto);
rc_cmd=mavros_msgs::CommandCode::NAV_WAYPOINT;
rc_call(rc_cmd);
res.success = true;
break;
default:
res.success = false;
break;
}
return true;
}
/*control c handler callback*/
static void ctrlc_handler(int sig) {
done = 1;
}
int main(int argc, char **argv)
{
ros::ServiceServer service;
/*initiate rosBuzz*/
/*Initialize rosBuzz node*/
ros::init(argc, argv, "rosBuzz");
ROS_INFO("Buzz_node");
/*Create node Handler*/
ros::NodeHandle n_c;
std::string bzzfile_name, fcclient_name, rcservice_name; //, rcclient;
bool rcclient;
/*Obtain .bzz file name from parameter server*/
if(ros::param::get("/rosbuzz_node/bzzfile_name", bzzfile_name));
else {ROS_ERROR("Provide a .bzz file to run in Launch file"); system("rosnode kill rosbuzz_node");}
/*Obtain rc service option from parameter server*/
if(ros::param::get("/rosbuzz_node/rcclient", rcclient)){
if(rcclient==true){
/*Service*/
if(ros::param::get("/rosbuzz_node/rcservice_name", rcservice_name)){
service = n_c.advertiseService(rcservice_name, rc_callback);
ROS_INFO("Ready to receive Mav Commands from RC client");
}
else{ROS_ERROR("Provide a name topic name for rc service in Launch file"); system("rosnode kill rosbuzz_node");}
}
else if(rcclient==false){ROS_INFO("RC service is disabled");}
}
else{ROS_ERROR("Provide a rc client option: yes or no in Launch file"); system("rosnode kill rosbuzz_node");}
/*Obtain fc client name from parameter server*/
if(ros::param::get("/rosbuzz_node/fcclient_name", fcclient_name));
else {ROS_ERROR("Provide a fc client name in Launch file"); system("rosnode kill rosbuzz_node");}
/*Obtain robot_id from parameter server*/
ros::param::get("/rosbuzz_node/robot_id", robot_id);
/*subscribers*/
ros::Subscriber current_position_sub = n_c.subscribe("current_pos", 1000, current_pos);
ros::Subscriber battery_sub = n_c.subscribe("battery_state", 1000, battery);
ros::Subscriber payload_sub = n_c.subscribe("inMavlink", 1000, payload_obt);
/*publishers*/
ros::Publisher payload_pub = n_c.advertise<mavros_msgs::Mavlink>("outMavlink", 1000);
/* Clients*/
ros::ServiceClient mav_client = n_c.serviceClient<mavros_msgs::CommandInt>(fcclient_name);
cout<< " rc client name"<<rcservice_name;
/*loop rate of ros*/
ros::Rate loop_rate(1);
/*Compile the buzz code .bzz to .bo*/
stringstream bzzfile_in_compile;
bzzfile_in_compile << "bzzparse "<<bzzfile_name<<" ../catkin_ws/src/rosbuzz/src/out.basm";
//system("rm ../catkin_ws/src/rosbuzz/src/out.basm ../catkin_ws/src/rosbuzz/src/out.bo ../catkin_ws/src/rosbuzz/src/out.bdbg");
system(bzzfile_in_compile.str().c_str());
system("bzzasm ../catkin_ws/src/rosbuzz/src/out.basm ../catkin_ws/src/rosbuzz/src/out.bo ../catkin_ws/src/rosbuzz/src/out.bdbg");
/* The bytecode filename */
char* bcfname = (char*)"../catkin_ws/src/rosbuzz/src/out.bo"; //argv[1];
/* The debugging information file name */
char* dbgfname = (char*)"../catkin_ws/src/rosbuzz/src/out.bdbg"; //argv[2];
/* Set CTRL-C handler */
signal(SIGTERM, ctrlc_handler);
signal(SIGINT, ctrlc_handler);
/* Set the Buzz bytecode */
if(buzz_script_set(bcfname, dbgfname,robot_id)) {
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())
{
/*Update neighbors position inside Buzz*/
neighbour_pos_callback(neighbours_pos_map);
/* Main loop */
buzz_script_step();
/*prepare the goto publish message */
double* goto_pos = getgoto();
cmd_srv.request.param1 = goto_pos[0];
cmd_srv.request.param2 = goto_pos[1];
cmd_srv.request.param3 = goto_pos[2];
cmd_srv.request.command = getcmd();
/* 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'"); }
/*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;
delete[] payload_out_ptr;
/*run once*/
ros::spinOnce();
/*sleep for the mentioned loop rate*/
loop_rate.sleep();
++count;
timer_step+=1;
maintain_pos(timer_step);
}
/* Cleanup */
buzz_script_destroy();
/* Stop the robot */
uav_done();
}
/* All done */
return 0;
rosbzz_node::roscontroller RosController;
RosController.RosControllerRun();
return 0;
}

264
src/roscontroller.cpp Normal file
View File

@ -0,0 +1,264 @@
#include "roscontroller.h"
namespace rosbzz_node{
/***Constructor***/
roscontroller::roscontroller()
{
ROS_INFO("Buzz_node");
/*Obtain parameters from ros parameter server*/
Rosparameters_get();
/*Initialize publishers, subscribers and client*/
Initialize_pub_sub();
/*Compile the .bzz file to .basm, .bo and .bdbg*/
Compile_bzz();
}
/***Destructor***/
roscontroller::~roscontroller()
{
/* All done */
/* Cleanup */
buzz_utility::buzz_script_destroy();
/* Stop the robot */
uav_done();
}
/*rosbuzz_node run*/
void roscontroller::RosControllerRun(){
/* Set the Buzz bytecode */
if(buzz_utility::buzz_script_set(bcfname, dbgfname,robot_id)) {
fprintf(stdout, "Bytecode file found and set\n");
while (ros::ok() && !buzz_utility::buzz_script_done())
{
/*Update neighbors position inside Buzz*/
buzz_utility::neighbour_pos_callback(neighbours_pos_map);
/*Step buzz script */
buzz_utility::buzz_script_step();
/*Prepare messages and publish them in respective topic*/
prepare_msg_and_publish();
/*run once*/
ros::spinOnce();
/*loop rate of ros*/
ros::Rate loop_rate(1);
/*sleep for the mentioned loop rate*/
loop_rate.sleep();
timer_step+=1;
maintain_pos(timer_step);
}
}
}
void roscontroller::Rosparameters_get(){
/*Obtain .bzz file name from parameter server*/
if(ros::param::get("/rosbuzz_node/bzzfile_name", bzzfile_name));
else {ROS_ERROR("Provide a .bzz file to run in Launch file"); system("rosnode kill rosbuzz_node");}
/*Obtain rc service option from parameter server*/
if(ros::param::get("/rosbuzz_node/rcclient", rcclient)){
if(rcclient==true){
/*Service*/
if(ros::param::get("/rosbuzz_node/rcservice_name", rcservice_name)){
service = n_c.advertiseService(rcservice_name, &roscontroller::rc_callback,this);
ROS_INFO("Ready to receive Mav Commands from RC client");
}
else{ROS_ERROR("Provide a name topic name for rc service in Launch file"); system("rosnode kill rosbuzz_node");}
}
else if(rcclient==false){ROS_INFO("RC service is disabled");}
}
else{ROS_ERROR("Provide a rc client option: yes or no in Launch file"); system("rosnode kill rosbuzz_node");}
/*Obtain fc client name from parameter server*/
if(ros::param::get("/rosbuzz_node/fcclient_name", fcclient_name));
else {ROS_ERROR("Provide a fc client name in Launch file"); system("rosnode kill rosbuzz_node");}
/*Obtain robot_id from parameter server*/
ros::param::get("/rosbuzz_node/robot_id", robot_id);
}
void roscontroller::Initialize_pub_sub(){
/*subscribers*/
current_position_sub = n_c.subscribe("current_pos", 1000, &roscontroller::current_pos,this);
battery_sub = n_c.subscribe("battery_state", 1000, &roscontroller::battery,this);
payload_sub = n_c.subscribe("inMavlink", 1000, &roscontroller::payload_obt,this);
/*publishers*/
payload_pub = n_c.advertise<mavros_msgs::Mavlink>("outMavlink", 1000);
/* Clients*/
mav_client = n_c.serviceClient<mavros_msgs::CommandInt>(fcclient_name);
}
void roscontroller::Compile_bzz(){
/*Compile the buzz code .bzz to .bo*/
stringstream bzzfile_in_compile;
bzzfile_in_compile << "bzzparse "<<bzzfile_name<<" ../catkin_ws/src/rosbuzz/src/out.basm";
//system("rm ../catkin_ws/src/rosbuzz/src/out.basm ../catkin_ws/src/rosbuzz/src/out.bo ../catkin_ws/src/rosbuzz/src/out.bdbg");
system(bzzfile_in_compile.str().c_str());
system("bzzasm ../catkin_ws/src/rosbuzz/src/out.basm ../catkin_ws/src/rosbuzz/src/out.bo ../catkin_ws/src/rosbuzz/src/out.bdbg");
}
inline void roscontroller::prepare_msg_and_publish(){
/*prepare the goto publish message */
double* goto_pos = buzzuav_closures::getgoto();
cmd_srv.request.param1 = goto_pos[0];
cmd_srv.request.param2 = goto_pos[1];
cmd_srv.request.param3 = goto_pos[2];
cmd_srv.request.command = buzzuav_closures::getcmd();
/* flight controller 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'"); }
/*obtain Pay load to be sent*/
uint64_t* payload_out_ptr= buzz_utility::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 = buzz_utility::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;
uint64_t message_obt[payload_out.payload64.size()];
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;
delete[] payload_out_ptr;
}
/*Refresh neighbours Position for every ten step*/
inline void roscontroller::maintain_pos(int tim_step){
if(timer_step >=10){
neighbours_pos_map.clear();
timer_step=0;
}
}
/*Maintain neighbours position*/
inline void roscontroller::neighbours_pos_maintain(int id, buzz_utility::Pos_struct pos_arr ){
map< int, buzz_utility::Pos_struct >::iterator it = neighbours_pos_map.find(id);
if(it!=neighbours_pos_map.end())
neighbours_pos_map.erase(it);
neighbours_pos_map.insert(make_pair(id, pos_arr));
}
/*Set the current position of the robot callback*/
inline void roscontroller::set_cur_pos(double latitude,
double longitude,
double altitude){
cur_pos [0] =latitude;
cur_pos [1] =longitude;
cur_pos [2] =altitude;
}
/*convert from catresian to spherical coordinate system callback */
inline double* roscontroller::cvt_spherical_coordinates(double neighbours_pos_payload []){
double latitude,longitude,altitude;
latitude=neighbours_pos_payload[0];
longitude = neighbours_pos_payload[1];
altitude=neighbours_pos_payload[2];
neighbours_pos_payload[0]=sqrt(pow(latitude,2.0)+pow(longitude,2.0)+pow(altitude,2.0));
neighbours_pos_payload[1]=atan(longitude/latitude);
neighbours_pos_payload[2]=atan((sqrt(pow(latitude,2.0)+pow(longitude,2.0)))/altitude);
return neighbours_pos_payload;
}
/*battery status callback*/
inline void roscontroller::battery(const mavros_msgs::BatteryStatus::ConstPtr& msg){
buzzuav_closures::set_battery(msg->voltage,msg->current,msg->remaining);
}
/*current position callback*/
inline void roscontroller::current_pos(const sensor_msgs::NavSatFix::ConstPtr& msg){
set_cur_pos(msg->latitude,msg->longitude,msg->altitude);
}
/*payload callback callback*/
inline void roscontroller::payload_obt(const mavros_msgs::Mavlink::ConstPtr& msg){
uint64_t message_obt[msg->payload64.size()];
int i = 0;
/* 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<<"[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));
/*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);
/*Extract robot id of the neighbour*/
uint16_t* out = buzz_utility::u64_cvt_u16((uint64_t)*(message_obt+3));
/*pass neighbour position to local maintaner*/
buzz_utility::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;
buzz_utility::in_msg_process((message_obt+3));
}
/* RC command service */
inline bool roscontroller::rc_callback(mavros_msgs::CommandInt::Request &req,
mavros_msgs::CommandInt::Response &res){
int rc_cmd;
if(req.command==oldcmdID)
return true;
else oldcmdID=req.command;
switch(req.command){
case mavros_msgs::CommandCode::NAV_TAKEOFF:
ROS_INFO("RC_call: TAKE OFF!!!!");
rc_cmd=mavros_msgs::CommandCode::NAV_TAKEOFF;
buzzuav_closures::rc_call(rc_cmd);
res.success = true;
break;
case mavros_msgs::CommandCode::NAV_LAND:
ROS_INFO("RC_Call: LAND!!!!");
rc_cmd=mavros_msgs::CommandCode::NAV_LAND;
buzzuav_closures::rc_call(rc_cmd);
res.success = true;
break;
case mavros_msgs::CommandCode::NAV_RETURN_TO_LAUNCH:
ROS_INFO("RC_Call: GO HOME!!!!");
rc_cmd=mavros_msgs::CommandCode::NAV_RETURN_TO_LAUNCH;
buzzuav_closures::rc_call(rc_cmd);
res.success = true;
break;
case mavros_msgs::CommandCode::NAV_WAYPOINT:
ROS_INFO("RC_Call: GO TO!!!! x = %f , y = %f , Z = %f",req.param1,req.param2,req.param3);
double rc_goto[3];
rc_goto[0]=req.param1;
rc_goto[1]=req.param2;
rc_goto[2]=req.param3;
buzzuav_closures::set_goto(rc_goto);
rc_cmd=mavros_msgs::CommandCode::NAV_WAYPOINT;
buzzuav_closures::rc_call(rc_cmd);
res.success = true;
break;
default:
res.success = false;
break;
}
return true;
}
}

101
src/roscontroller.h Normal file
View File

@ -0,0 +1,101 @@
#pragma once
#include "ros/ros.h"
#include "sensor_msgs/NavSatFix.h"
#include "mavros_msgs/GlobalPositionTarget.h"
#include "mavros_msgs/CommandCode.h"
#include "mavros_msgs/CommandInt.h"
#include "mavros_msgs/State.h"
#include "mavros_msgs/BatteryStatus.h"
#include "mavros_msgs/Mavlink.h"
#include "sensor_msgs/NavSatStatus.h"
#include <sstream>
#include <buzz/buzzasm.h>
#include "buzzuav_closures.h"
#include "buzz_utility.h"
#include "uav_utility.h"
#include <stdlib.h>
#include <string.h>
#include <math.h>
#include <signal.h>
#include <ostream>
#include <map>
using namespace std;
namespace rosbzz_node{
class roscontroller{
public:
roscontroller();
~roscontroller();
void RosControllerRun();
private:
double cur_pos[3];
uint64_t payload;
std::map< int, buzz_utility::Pos_struct> neighbours_pos_map;
int timer_step=0;
int robot_id=0;
int oldcmdID=0;
std::string bzzfile_name, fcclient_name, rcservice_name; //, rcclient;
bool rcclient;
ros::ServiceClient mav_client;
ros::Publisher payload_pub;
ros::ServiceServer service;
ros::Subscriber current_position_sub;
ros::Subscriber battery_sub;
ros::Subscriber payload_sub;
/*Create node Handler*/
ros::NodeHandle n_c;
/*Commands for flight controller*/
mavros_msgs::CommandInt cmd_srv;
/* The bytecode filename */
char* bcfname = (char*)"../catkin_ws/src/rosbuzz/src/out.bo"; //argv[1];
/* The debugging information file name */
char* dbgfname = (char*)"../catkin_ws/src/rosbuzz/src/out.bdbg"; //argv[2];
void Initialize_pub_sub();
/*Obtain data from ros parameter server*/
void Rosparameters_get();
void Compile_bzz();
/*Prepare messages and publish*/
inline void prepare_msg_and_publish();
/*Refresh neighbours Position for every ten step*/
inline void maintain_pos(int tim_step);
/*Maintain neighbours position*/
inline void neighbours_pos_maintain(int id, buzz_utility::Pos_struct pos_arr );
/*Set the current position of the robot callback*/
inline void set_cur_pos(double latitude,
double longitude,
double altitude);
/*convert from catresian to spherical coordinate system callback */
inline double* cvt_spherical_coordinates(double neighbours_pos_payload []);
/*battery status callback*/
inline void battery(const mavros_msgs::BatteryStatus::ConstPtr& msg);
/*current position callback*/
inline void current_pos(const sensor_msgs::NavSatFix::ConstPtr& msg);
/*payload callback callback*/
inline void payload_obt(const mavros_msgs::Mavlink::ConstPtr& msg);
/* RC commands service */
inline bool rc_callback(mavros_msgs::CommandInt::Request &req,
mavros_msgs::CommandInt::Response &res);
};
}

2
src/test.bzz
View File

@ -17,7 +17,7 @@ neighbors.foreach(
"azimuth = ", data.azimuth, ", ",
"elevation = ", data.elevation) })
if(i==1){
s.exec(function() {uav_takeoff()})
s.exec(function() {uav_gohome()})
}
else{
s.exec(function() {uav_goto(1.1234,2.2345,3.3456)})