Fixed some bugs in file handeling
This commit is contained in:
vivek
parent
021d12b451
commit
b950a1e11a
|
|
@ -0,0 +1,52 @@
|
|||
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
|
||||
std_msgs
|
||||
#mavros_msgs
|
||||
sensor_msgs
|
||||
)
|
||||
|
||||
###################################
|
||||
## catkin specific configuration ##
|
||||
###################################
|
||||
|
||||
catkin_package(
|
||||
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)
|
||||
target_link_libraries(rosbuzz_node ${catkin_LIBRARIES} /usr/local/lib/libbuzz.so /usr/local/lib/libbuzzdbg.so -lpthread)
|
||||
|
||||
# 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}
|
||||
)
|
||||
|
||||
find_package(catkin REQUIRED COMPONENTS roslaunch)
|
||||
roslaunch_add_file_check(launch)
|
||||
|
|
@ -32,7 +32,7 @@ public:
|
|||
~roscontroller();
|
||||
//void RosControllerInit();
|
||||
void RosControllerRun();
|
||||
|
||||
|
||||
private:
|
||||
|
||||
double cur_pos[3];
|
||||
|
|
@ -42,7 +42,7 @@ private:
|
|||
int robot_id=0;
|
||||
int oldcmdID=0;
|
||||
int rc_cmd;
|
||||
std::string bzzfile_name, fcclient_name, rcservice_name; //, rcclient;
|
||||
std::string bzzfile_name, fcclient_name, rcservice_name,bcfname,dbgfname; //, rcclient;
|
||||
bool rcclient;
|
||||
ros::ServiceClient mav_client;
|
||||
ros::Publisher payload_pub;
|
||||
|
|
@ -54,10 +54,7 @@ private:
|
|||
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();
|
||||
|
||||
|
|
|
|||
|
|
@ -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();
|
||||
/* The bytecode filename */
|
||||
char* bcfname; // = (char*)"/src/rosbuzz/src/out.bo"; //argv[1];
|
||||
/* The debugging information file name */
|
||||
char* dbgfname; //= (char*)"/src/rosbuzz/src/out.bdbg"; //argv[2];
|
||||
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;
|
||||
|
||||
|
||||
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,0 +1,11 @@
|
|||
<!-- Launch file for ROSBuzz -->
|
||||
|
||||
<launch>
|
||||
<node name="rosbuzz_node" pkg="rosbuzz" type="rosbuzz_node" respawn="false" output="screen" >
|
||||
<param name="bzzfile_name" value="~/catkin_ws/src/rosbuzz/src/test.bzz" />
|
||||
<param name="rcclient" value="true" />
|
||||
<param name="rcservice_name" value="rc_cmd" />
|
||||
<param name="fcclient_name" value="fc_cmd" />
|
||||
</node>
|
||||
<param name="robot_id" value="1"/>
|
||||
</launch>
|
||||
|
|
@ -191,6 +191,7 @@ namespace buzz_utility{
|
|||
|
||||
int buzz_script_set(const char* bo_filename,
|
||||
const char* bdbg_filename, int robot_id) {
|
||||
//cout<<"bo file name"<<bo_filename;
|
||||
/* Get hostname */
|
||||
char hstnm[30];
|
||||
gethostname(hstnm, 30);
|
||||
|
|
|
|||
|
|
@ -0,0 +1,361 @@
|
|||
/** @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.
|
||||
*/
|
||||
|
||||
#include <buzz_utility.h>
|
||||
|
||||
namespace buzz_utility{
|
||||
|
||||
/****************************************/
|
||||
/****************************************/
|
||||
|
||||
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*/
|
||||
/***************************************************/
|
||||
|
||||
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);
|
||||
|
||||
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);
|
||||
|
||||
/* 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;
|
||||
}
|
||||
|
||||
/* 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);
|
||||
|
||||
/* 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;
|
||||
|
||||
|
||||
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*/
|
||||
/****************************************/
|
||||
|
||||
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*/
|
||||
/****************************************/
|
||||
|
||||
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*/
|
||||
/****************************************/
|
||||
|
||||
int buzz_script_set(const char* bo_filename,
|
||||
const char* bdbg_filename, int robot_id) {
|
||||
cout<<"bo file name"<<bo_filename;
|
||||
/* 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*/
|
||||
/****************************************/
|
||||
|
||||
struct buzzswarm_elem_s {
|
||||
buzzdarray_t swarms;
|
||||
uint16_t age;
|
||||
};
|
||||
typedef struct buzzswarm_elem_s* buzzswarm_elem_t;
|
||||
|
||||
void check_swarm_members(const void* key, void* data, void* params) {
|
||||
buzzswarm_elem_t e = *(buzzswarm_elem_t*)data;
|
||||
int* status = (int*)params;
|
||||
if(*status == 3) return;
|
||||
if(buzzdarray_size(e->swarms) != 1) {
|
||||
fprintf(stderr, "Swarm list size is not 1\n");
|
||||
*status = 3;
|
||||
}
|
||||
else {
|
||||
int sid = 1;
|
||||
if(*buzzdict_get(VM->swarms, &sid, uint8_t) &&
|
||||
buzzdarray_get(e->swarms, 0, uint16_t) != sid) {
|
||||
fprintf(stderr, "I am in swarm #%d and neighbor is in %d\n",
|
||||
sid,
|
||||
buzzdarray_get(e->swarms, 0, uint16_t));
|
||||
*status = 3;
|
||||
return;
|
||||
}
|
||||
sid = 2;
|
||||
if(*buzzdict_get(VM->swarms, &sid, uint8_t) &&
|
||||
buzzdarray_get(e->swarms, 0, uint16_t) != sid) {
|
||||
fprintf(stderr, "I am in swarm #%d and neighbor is in %d\n",
|
||||
sid,
|
||||
buzzdarray_get(e->swarms, 0, uint16_t));
|
||||
*status = 3;
|
||||
return;
|
||||
}
|
||||
}
|
||||
}
|
||||
/*Step through the buzz script*/
|
||||
|
||||
void buzz_script_step() {
|
||||
/*
|
||||
* Update sensors
|
||||
*/
|
||||
buzzuav_closures::buzzuav_update_battery(VM);
|
||||
buzzuav_closures::buzzuav_update_prox(VM);
|
||||
/*
|
||||
* Call Buzz step() function
|
||||
*/
|
||||
if(buzzvm_function_call(VM, "step", 0) != BUZZVM_STATE_READY) {
|
||||
fprintf(stderr, "%s: execution terminated abnormally: %s\n\n",
|
||||
BO_FNAME,
|
||||
buzz_error_info());
|
||||
buzzvm_dump(VM);
|
||||
}
|
||||
/* Print swarm */
|
||||
buzzswarm_members_print(stdout, VM->swarmmembers, VM->robot);
|
||||
/* Check swarm state */
|
||||
/* int status = 1;
|
||||
buzzdict_foreach(VM->swarmmembers, check_swarm_members, &status);
|
||||
if(status == 1 &&
|
||||
buzzdict_size(VM->swarmmembers) < 9)
|
||||
status = 2;
|
||||
buzzvm_pushs(VM, buzzvm_string_register(VM, "swarm_status", 1));
|
||||
buzzvm_pushi(VM, status);
|
||||
buzzvm_gstore(VM);*/
|
||||
}
|
||||
|
||||
/****************************************/
|
||||
/*Destroy the bvm and other resorces*/
|
||||
/****************************************/
|
||||
|
||||
void buzz_script_destroy() {
|
||||
if(VM) {
|
||||
if(VM->state != BUZZVM_STATE_READY) {
|
||||
fprintf(stderr, "%s: execution terminated abnormally: %s\n\n",
|
||||
BO_FNAME,
|
||||
buzz_error_info());
|
||||
buzzvm_dump(VM);
|
||||
}
|
||||
buzzvm_function_call(VM, "destroy", 0);
|
||||
buzzvm_destroy(&VM);
|
||||
free(BO_FNAME);
|
||||
buzzdebug_destroy(&DBG_INFO);
|
||||
}
|
||||
fprintf(stdout, "Script execution stopped.\n");
|
||||
}
|
||||
|
||||
|
||||
/****************************************/
|
||||
/****************************************/
|
||||
|
||||
/****************************************/
|
||||
/*Execution completed*/
|
||||
/****************************************/
|
||||
|
||||
int buzz_script_done() {
|
||||
return VM->state != BUZZVM_STATE_READY;
|
||||
}
|
||||
|
||||
}
|
||||
|
||||
|
||||
|
||||
|
|
@ -29,7 +29,7 @@ namespace rosbzz_node{
|
|||
/*rosbuzz_node run*/
|
||||
void roscontroller::RosControllerRun(){
|
||||
/* Set the Buzz bytecode */
|
||||
if(buzz_utility::buzz_script_set(bcfname, dbgfname,robot_id)) {
|
||||
if(buzz_utility::buzz_script_set(bcfname.c_str(), dbgfname.c_str(),robot_id)) {
|
||||
fprintf(stdout, "Bytecode file found and set\n");
|
||||
while (ros::ok() && !buzz_utility::buzz_script_done())
|
||||
{
|
||||
|
|
@ -92,10 +92,20 @@ namespace rosbzz_node{
|
|||
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";
|
||||
std::string path = bzzfile_name.substr(0, bzzfile_name.find_last_of("\\/"));
|
||||
bzzfile_in_compile << "bzzparse "<<bzzfile_name<<" "<<path<< "/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");
|
||||
bzzfile_in_compile.str("");
|
||||
bzzfile_in_compile <<"bzzasm "<<path<<"/out.basm "<<path<<"/out.bo "<<path<<"/out.bdbg";
|
||||
system(bzzfile_in_compile.str().c_str());
|
||||
bzzfile_in_compile.str("");
|
||||
bzzfile_in_compile <<path<<"/out.bo";
|
||||
bcfname = bzzfile_in_compile.str();
|
||||
bzzfile_in_compile.str("");
|
||||
bzzfile_in_compile <<path<<"/out.bdbg";
|
||||
dbgfname = bzzfile_in_compile.str();
|
||||
|
||||
}
|
||||
|
||||
void roscontroller::prepare_msg_and_publish(){
|
||||
|
|
@ -105,7 +115,7 @@ namespace rosbzz_node{
|
|||
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*/
|
||||
/* 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*/
|
||||
|
|
@ -192,10 +202,10 @@ namespace rosbzz_node{
|
|||
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;
|
||||
for(i=0;i < (int)msg->payload64.size();i++){
|
||||
message_obt[i] =(uint64_t)msg->payload64[i];
|
||||
//cout<<"[Debug:] obtaind message "<<message_obt[i]<<endl;
|
||||
i++;
|
||||
//i++;
|
||||
}
|
||||
/* Extract neighbours position from payload*/
|
||||
double neighbours_pos_payload[3];
|
||||
|
|
|
|||
|
|
@ -0,0 +1,275 @@
|
|||
|
||||
#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.c_str(), dbgfname.c_str(),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;
|
||||
std::string path = bzzfile_name.substr(0, bzzfile_name.find_last_of("\\/"));
|
||||
bzzfile_in_compile << "bzzparse "<<bzzfile_name<<" "<<path<< "/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());
|
||||
bzzfile_in_compile.str("");
|
||||
cout << "Found PAth name is"<< bzzfile_name.substr(0, bzzfile_name.find_last_of("\\/")) <<"string stream: "<<bzzfile_in_compile.str() << endl;
|
||||
bzzfile_in_compile <<"bzzasm "<<path<<"/out.basm "<<path<<"/out.bo "<<path<<"/out.bdbg";
|
||||
system(bzzfile_in_compile.str().c_str());
|
||||
bzzfile_in_compile.str("");
|
||||
bzzfile_in_compile <<path<<"/out.bo";
|
||||
bcfname = bzzfile_in_compile.str();
|
||||
bzzfile_in_compile.str("");
|
||||
bzzfile_in_compile <<path<<"/out.bdbg";
|
||||
dbgfname = bzzfile_in_compile.str();
|
||||
|
||||
}
|
||||
|
||||
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*/
|
||||
void roscontroller::maintain_pos(int tim_step){
|
||||
if(timer_step >=10){
|
||||
neighbours_pos_map.clear();
|
||||
timer_step=0;
|
||||
}
|
||||
}
|
||||
|
||||
/*Maintain neighbours position*/
|
||||
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*/
|
||||
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 */
|
||||
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*/
|
||||
void roscontroller::battery(const mavros_msgs::BatteryStatus::ConstPtr& msg){
|
||||
buzzuav_closures::set_battery(msg->voltage,msg->current,msg->remaining);
|
||||
}
|
||||
|
||||
/*current position callback*/
|
||||
void roscontroller::current_pos(const sensor_msgs::NavSatFix::ConstPtr& msg){
|
||||
set_cur_pos(msg->latitude,msg->longitude,msg->altitude);
|
||||
}
|
||||
|
||||
/*payload callback callback*/
|
||||
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(i=0;i < (int)msg->payload64.size();i++){
|
||||
message_obt[i] =(uint64_t)msg->payload64[i];
|
||||
//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 */
|
||||
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;
|
||||
}
|
||||
|
||||
}
|
||||
|
||||
|
||||
Loading…
Reference in New Issue