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

rebase

This commit is contained in:
isvogor 2017-02-23 19:39:59 -05:00
parent 87b2861503 4ac9d89f7c
commit 12ca11b275
8 changed files with 714 additions and 485 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

1
CMakeLists.txt
View File

@ -15,7 +15,6 @@ find_package(catkin REQUIRED COMPONENTS
)
##############################
#Generate messages#
##############################
add_message_files(

13
include/roscontroller.h
View File

@ -11,6 +11,8 @@
#include "mavros_msgs/State.h"
#include "mavros_msgs/BatteryStatus.h"
#include "mavros_msgs/Mavlink.h"
#include "mavros_msgs/WaypointPush.h"
#include "mavros_msgs/Waypoint.h"
#include "sensor_msgs/NavSatStatus.h"
#include <sensor_msgs/LaserScan.h>
#include <rosbuzz/neigh_pos.h>
@ -82,6 +84,8 @@ private:
mavros_msgs::SetMode m_cmdSetMode;
ros::ServiceClient mode_client;
ros::ServiceClient mission_client;
void Initialize_pub_sub(ros::NodeHandle n_c);
/*Obtain data from ros parameter server*/
@ -149,9 +153,16 @@ private:
void Arm();
void SetMode();
void SetMode(std::string mode, int delay_miliseconds);
void SetModeAsync(std::string mode, int delay_miliseconds);
//void SetModeAsync(std::string mode, int delay);
void Subscribe(ros::NodeHandle n_c);
void WaypointMissionSetup(float lat, float lng, float alg);
};
}

47
launch/rosbuzz-testing-sitl.launch Normal file
View File

@ -0,0 +1,47 @@
<launch>
<!-- RUN mavros -->
<arg name="fcu_url" default="tcp://127.0.0.1:5760" />
<arg name="gcs_url" default="" />
<arg name="tgt_system" default="1" />
<arg name="tgt_component" default="1" />
<arg name="log_output" default="screen" />
<include file="$(find mavros)/launch/node.launch">
<arg name="pluginlists_yaml" value="$(find mavros)/launch/apm_pluginlists.yaml" />
<arg name="config_yaml" value="$(find mavros)/launch/apm_config.yaml" />
<arg name="fcu_url" value="$(arg fcu_url)" />
<arg name="gcs_url" value="$(arg gcs_url)" />
<arg name="tgt_system" value="$(arg tgt_system)" />
<arg name="tgt_component" value="$(arg tgt_component)" />
<arg name="log_output" value="$(arg log_output)" />
</include>
<!-- run xbee -->
<node pkg="xbee_ros_node" type="xbee_mav" name="xbee_mav" args="slave swarm" output="screen" />
<param name="Xbee_In_From_Buzz" type="str" value="outMavlink" />
<param name="Xbee_Out_To_Buzz" type="str" value="inMavlink" />
<param name="Xbee_In_From_Controller" type="str" value="xbee_cmd" />
<param name="No_of_dev" type="int" value="3" />
<!-- run rosbuzz -->
<node name="rosbuzz_node" pkg="rosbuzz" type="rosbuzz_node" respawn="false" output="screen" >
<rosparam file="/home/ivan/catkin_ws/src/rosbuzz/launch/launch_config/solo.yaml"/>
<param name="bzzfile_name" value="/home/ivan/catkin_ws/src/rosbuzz/src/testalone.bzz" />
<param name="rcclient" value="true" />
<param name="rcservice_name" value="/buzzcmd" />
<param name="in_payload" value="/inMavlink"/>
<param name="out_payload" value="/outMavlink"/>
<param name="robot_id" value="3"/>
<param name="No_of_Robots" value="3"/>
<param name="stand_by" value="/home/ivan/catkin_ws/src/rosbuzz/src/stand_by.bo"/>
</node>
<node pkg="rosloader" type="rosloader" name="rosloader" output="screen"/>
<!-- set streaming rate
/mavros/cmd/arming
<node pkg="rosservice" type="rosservice" name="stream_rate" args="call /mavros/set_stream_rate 0 10 1" />
-->
</launch>

44
launch/rosbuzz-testing.launch Normal file
View File

@ -0,0 +1,44 @@
<launch>
<!-- RUN mavros -->
<arg name="fcu_url" default="/dev/ttyAMA0:115200" />
<arg name="gcs_url" default="" />
<arg name="tgt_system" default="1" />
<arg name="tgt_component" default="1" />
<arg name="log_output" default="screen" />
<include file="$(find mavros)/launch/node.launch">
<arg name="pluginlists_yaml" value="$(find mavros)/launch/apm_pluginlists.yaml" />
<arg name="config_yaml" value="$(find mavros)/launch/apm_config.yaml" />
<arg name="fcu_url" value="$(arg fcu_url)" />
<arg name="gcs_url" value="$(arg gcs_url)" />
<arg name="tgt_system" value="$(arg tgt_system)" />
<arg name="tgt_component" value="$(arg tgt_component)" />
<arg name="log_output" value="$(arg log_output)" />
</include>
<!-- set streaming rate -->
<node pkg="rosservice" type="rosservice" name="freq" args="call /mavros/set_stream_rate 0 10 1" output="screen" />
<!-- run xbee -->
<node pkg="xbee_ros_node" type="xbee_mav" name="xbee_mav" args="slave swarm" output="screen" />
<param name="Xbee_In_From_Buzz" type="str" value="outMavlink" />
<param name="Xbee_Out_To_Buzz" type="str" value="inMavlink" />
<param name="Xbee_In_From_Controller" type="str" value="xbee_cmd" />
<param name="No_of_dev" type="int" value="3" />
<param name="Xbee_Out_To_Controller" type="str" value="mav_dji_cmd" />
<!-- run rosbuzz -->
<node name="rosbuzz_node" pkg="rosbuzz" type="rosbuzz_node" respawn="false" output="screen" >
<rosparam file="/home/pi/ros_catkin_ws/src/ROSBuzz/launch/launch_config/solo.yaml"/>
<param name="bzzfile_name" value="/home/pi/ros_catkin_ws/src/ROSBuzz/src/testalone.bzz" />
<param name="rcclient" value="true" />
<param name="rcservice_name" value="/buzzcmd" />
<param name="in_payload" value="/inMavlink"/>
<param name="out_payload" value="/outMavlink"/>
<param name="robot_id" value="3"/>
<param name="No_of_Robots" value="3"/>
<param name="stand_by" value="/home/pi/ros_catkin_ws/src/ROSBuzz/src/stand_by.bo"/>
</node>
</launch>

7
launch/rosbuzz.launch
View File

@ -2,15 +2,14 @@
<launch>
<node name="rosbuzz_node" pkg="rosbuzz" type="rosbuzz_node" respawn="false" output="screen" >
<rosparam file="$(env ROS_WS)/src/ROSBuzz/launch/launch_config/$(env ROBOT).yaml"/>
<param name="bzzfile_name" value="$(env ROS_WS)/src/ROSBuzz/src/test1.bzz" />
<rosparam file="/home/ivan/catkin_ws/src/rosbuzz/launch/launch_config/solo.yaml"/>
<param name="bzzfile_name" value="/home/ivan/catkin_ws/src/rosbuzz/src/testalone.bzz" />
<param name="rcclient" value="true" />
<param name="rcservice_name" value="/buzzcmd" />
<param name="in_payload" value="/inMavlink"/>
<param name="out_payload" value="/outMavlink"/>
<param name="robot_id" value="3"/>
<param name="No_of_Robots" value="3"/>
<param name="stand_by" value="$(env ROS_WS)/src/ROSBuzz/src/stand_by.bo"/>
<param name="stand_by" value="/home/ivan/catkin_ws/src/rosbuzz/src/stand_by.bo"/>
</node>
</launch>

20
misc/cmdlinectr.sh Normal file
View File

@ -0,0 +1,20 @@
#! /bin/bash
function takeoff {
rosservice call /buzzcmd 0 22 0 0 0 0 0 0 0 0
}
function land {
rosservice call /buzzcmd 0 21 0 0 0 0 0 0 0 0
}
function record {
rosbag record /flight_status /global_position /dji_sdk/local_position /neighbours_pos /power_status /guidance/obstacle_distance /guidance/front_depth_image/compressedDepth /guidance/right_depth_image/compressedDepth /guidance/left_depth_image/compressedDepth /guidance/bottom_depth_image/compressedDepth /guidance/back_depth_image/compressedDepth
}
function clean {
sudo rm /var/log/upstart/robot*
sudo rm /var/log/upstart/x3s*
}
function startrobot {
sudo service robot start
}
function stoprobot {
sudo service robot stop
}

622
src/buzz_utility.cpp
View File

@ -8,72 +8,69 @@
#include <buzz_utility.h>
namespace buzz_utility{
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 uint8_t MSG_SIZE = 50; // Only 100 bytes of Buzz messages every step
static int MAX_MSG_SIZE = 10000; // Maximum Msg size for sending update packets
static buzzvm_t VM = 0;
static char* BO_FNAME = 0;
static uint8_t* BO_BUF = 0;
static buzzdebug_t DBG_INFO = 0;
static uint8_t MSG_SIZE = 50; // Only 100 bytes of Buzz messages every step
static int MAX_MSG_SIZE = 10000; // Maximum Msg size for sending update packets
/****************************************/
/*adds neighbours position*/
void neighbour_pos_callback(std::map< int, Pos_struct> neighbours_pos_map){
/****************************************/
/*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,
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);
}
}
/**************************************************************************/
/*Deserializes uint64_t into 4 uint16_t, freeing out is left to the user */
/**************************************************************************/
uint16_t* u64_cvt_u16(uint64_t u64){
}
/**************************************************************************/
/*Deserializes uint64_t into 4 uint16_t, freeing out is left to the user */
/**************************************************************************/
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;
uint32_t int32_2 = (u64 & 0xFFFFFFFF00000000) >> 32;
out[0] = int32_1 & 0xFFFF;
out[1] = (int32_1 & (0xFFFF0000) ) >> 16;
out[1] = (int32_1 & (0xFFFF0000)) >> 16;
out[2] = int32_2 & 0xFFFF;
out[3] = (int32_2 & (0xFFFF0000) ) >> 16;
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*/
/***************************************************/
/*******************************************************************************************************************/
/* Message format of payload (Each slot is uint64_t) */
/* _______________________________________________________________________________________________________________ */
/*| | |*/
/*|Size in Uint64_t(but size is Uint16_t)|robot_id|Update msg size|Update msg|Update msgs+Buzz_msgs with size.....|*/
/*|__________________________________________________________________________|____________________________________|*/
/*******************************************************************************************************************/
/*******************************************************************************************************************/
/* Message format of payload (Each slot is uint64_t) */
/* _______________________________________________________________________________________________________________ */
/*| | |*/
/*|Size in Uint64_t(but size is Uint16_t)|robot_id|Update msg size|Update msg|Update msgs+Buzz_msgs with size.....|*/
/*|__________________________________________________________________________|____________________________________|*/
/*******************************************************************************************************************/
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* data = u64_cvt_u16((uint64_t) payload[0]);
/*Size is at first 2 bytes*/
uint16_t size=data[0]*sizeof(uint64_t);
uint16_t size = data[0] * sizeof(uint64_t);
delete[] data;
uint8_t* pl =(uint8_t*)malloc(size);
memset(pl, 0,size);
uint8_t* pl = (uint8_t*) malloc(size);
memset(pl, 0, size);
/* Copy packet into temporary buffer */
memcpy(pl, payload ,size);
memcpy(pl, payload, size);
/*size and robot id read*/
size_t tot = sizeof(uint32_t);
/*for(int i=0;i<data[0];i++){
@ -84,7 +81,7 @@ namespace buzz_utility{
}*/
/* Go through the messages until there's nothing else to read */
//fprintf(stdout,"Total data size : utils : %d\n",(int)size);
uint16_t unMsgSize=0;
uint16_t unMsgSize = 0;
//uint8_t is_msg_pres=*(uint8_t*)(pl + tot);
//tot+=sizeof(uint8_t);
//fprintf(stdout,"is_updater msg present : %i\n",(int)is_msg_pres);
@ -105,31 +102,31 @@ namespace buzz_utility{
/*Obtain Buzz messages only when they are present*/
do {
/* Get payload size */
unMsgSize = *(uint16_t*)(pl + tot);
unMsgSize = *(uint16_t*) (pl + tot);
tot += sizeof(uint16_t);
/* Append message to the Buzz input message queue */
if(unMsgSize > 0 && unMsgSize <= size - tot ) {
if (unMsgSize > 0 && unMsgSize <= size - tot) {
buzzinmsg_queue_append(VM,
buzzmsg_payload_frombuffer(pl +tot, unMsgSize));
buzzmsg_payload_frombuffer(pl + tot, unMsgSize));
tot += unMsgSize;
}
}while(size - tot > sizeof(uint16_t) && unMsgSize > 0);
} while (size - tot > sizeof(uint16_t) && unMsgSize > 0);
delete[] pl;
/* Process messages */
buzzvm_process_inmsgs(VM);
}
/***************************************************/
/*Obtains messages from buzz out message Queue*/
/***************************************************/
}
/***************************************************/
/*Obtains messages from buzz out message Queue*/
/***************************************************/
uint64_t* out_msg_process(){
uint64_t* out_msg_process() {
buzzvm_process_outmsgs(VM);
uint8_t* buff_send =(uint8_t*)malloc(MAX_MSG_SIZE);
uint8_t* buff_send = (uint8_t*) malloc(MAX_MSG_SIZE);
memset(buff_send, 0, MAX_MSG_SIZE);
/*Taking into consideration the sizes included at the end*/
ssize_t tot = sizeof(uint16_t);
/* Send robot id */
*(uint16_t*)(buff_send+tot) = (uint16_t) VM->robot;
*(uint16_t*) (buff_send + tot) = (uint16_t) VM->robot;
tot += sizeof(uint16_t);
//uint8_t updater_msg_pre = 0;
//uint16_t updater_msgSize= 0;
@ -158,70 +155,17 @@ namespace buzz_utility{
/* Send messages from FIFO */
do {
/* Are there more messages? */
if(buzzoutmsg_queue_isempty(VM)) break;
if (buzzoutmsg_queue_isempty(VM))
break;
/* Get first message */
buzzmsg_payload_t m = buzzoutmsg_queue_first(VM);
/* Make sure the next message makes the data buffer with buzz messages to be less than 100 Bytes */
if(tot + buzzmsg_payload_size(m) + sizeof(uint16_t)
>
MSG_SIZE) {
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);
buzzmsg_payload_destroy(&m);
} while(1);
uint16_t 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));
delete[] buff_send;
/*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;
}
<<<<<<< HEAD
/****************************************/
/*Buzz hooks that can be used inside .bzz file*/
/****************************************/
@ -285,192 +229,303 @@ namespace buzz_utility{
buzzvm_gstore(VM);
return BUZZVM_STATE_READY;
}
=======
/* 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);
>>>>>>> 4ac9d89f7c5e82fe99a48f616c587efd01d50ddd
/****************************************/
/*Sets the .bzz and .bdbg file*/
/****************************************/
/* Get rid of message */
buzzoutmsg_queue_next(VM);
buzzmsg_payload_destroy(&m);
} while (1);
int buzz_script_set(const char* bo_filename,
uint16_t 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));
delete[] buff_send;
/*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, "log", 1));
buzzvm_pushcc(VM, buzzvm_function_register(VM, buzzuav_closures::buzzros_print));
buzzvm_gstore(VM);
buzzvm_pushs(VM, buzzvm_string_register(VM, "uav_moveto", 1));
buzzvm_pushcc(VM, buzzvm_function_register(VM, buzzuav_closures::buzzuav_moveto));
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;
}
/**************************************************/
/*Register dummy Buzz hooks for test during update*/
/**************************************************/
static int testing_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, "log", 1));
buzzvm_pushcc(VM, buzzvm_function_register(VM, buzzuav_closures::buzzros_print));
buzzvm_gstore(VM);
buzzvm_pushs(VM, buzzvm_string_register(VM, "uav_moveto", 1));
buzzvm_pushcc(VM, buzzvm_function_register(VM, buzzuav_closures::dummy_closure));
buzzvm_gstore(VM);
buzzvm_pushs(VM, buzzvm_string_register(VM, "uav_goto", 1));
buzzvm_pushcc(VM, buzzvm_function_register(VM, buzzuav_closures::dummy_closure));
buzzvm_gstore(VM);
buzzvm_pushs(VM, buzzvm_string_register(VM, "uav_takeoff", 1));
buzzvm_pushcc(VM, buzzvm_function_register(VM, buzzuav_closures::dummy_closure));
buzzvm_gstore(VM);
buzzvm_pushs(VM, buzzvm_string_register(VM, "uav_gohome", 1));
buzzvm_pushcc(VM, buzzvm_function_register(VM, buzzuav_closures::dummy_closure));
buzzvm_gstore(VM);
buzzvm_pushs(VM, buzzvm_string_register(VM, "uav_land", 1));
buzzvm_pushcc(VM, buzzvm_function_register(VM, buzzuav_closures::dummy_closure));
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];
//char* hstnm ="M1003";
gethostname(hstnm, 30);
/* Make numeric id from hostname */
/* NOTE: here we assume that the hostname is in the format Knn */
int id = strtol(hstnm+4, NULL, 10);
cout << " Robot ID: " <<id<< endl;
/* Reset the Buzz VM */
if(VM) buzzvm_destroy(&VM);
VM = buzzvm_new((int)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) {
//cout<<"bo file name"<<bo_filename;
/* Get hostname */
char hstnm[30];
//char* hstnm ="M1003";
gethostname(hstnm, 30);
/* Make numeric id from hostname */
/* NOTE: here we assume that the hostname is in the format Knn */
int id = strtol(hstnm+4, NULL, 10);
cout << " Robot ID: " <<id<< endl;
/* Reset the Buzz VM */
if(VM) buzzvm_destroy(&VM);
VM = buzzvm_new((int)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) {
}
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) {
}
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 buzz_update_set(BO_BUF, bdbg_filename, bcode_size);
}
/* 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 buzz_update_set(BO_BUF, bdbg_filename, bcode_size);
}
/****************************************/
/*Sets a new update */
/****************************************/
int buzz_update_set(uint8_t* UP_BO_BUF, const char* bdbg_filename,size_t bcode_size){
/* 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 = strtol(hstnm + 4, NULL, 10);
/****************************************/
/*Sets a new update */
/****************************************/
int buzz_update_set(uint8_t* UP_BO_BUF, const char* bdbg_filename,size_t bcode_size) {
/* 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 = strtol(hstnm + 4, NULL, 10);
/* 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();
/* 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 debug information */
if(!buzzdebug_fromfile(DBG_INFO, bdbg_filename)) {
/* 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, UP_BO_BUF, bcode_size) != BUZZVM_STATE_READY) {
}
/* Set byte code */
if(buzzvm_set_bcode(VM, UP_BO_BUF, bcode_size) != BUZZVM_STATE_READY) {
buzzvm_destroy(&VM);
buzzdebug_destroy(&DBG_INFO);
fprintf(stdout, "%s: Error loading Buzz script\n\n", BO_FNAME);
return 0;
}
/* Register hook functions */
if(buzz_register_hooks() != BUZZVM_STATE_READY) {
}
/* 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_FNAME);
return 0;
}
}
/* 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;
}
/* 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;
}
/****************************************/
/*Performs a initialization test */
/****************************************/
int buzz_update_init_test(uint8_t* UP_BO_BUF, const char* bdbg_filename,size_t bcode_size){
/* 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 = strtol(hstnm + 4, NULL, 10);
/* 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();
/****************************************/
/*Performs a initialization test */
/****************************************/
int buzz_update_init_test(uint8_t* UP_BO_BUF, const char* bdbg_filename,size_t bcode_size) {
/* 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 = strtol(hstnm + 4, NULL, 10);
/* 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 debug information */
if(!buzzdebug_fromfile(DBG_INFO, bdbg_filename)) {
/* 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, UP_BO_BUF, bcode_size) != BUZZVM_STATE_READY) {
}
/* Set byte code */
if(buzzvm_set_bcode(VM, UP_BO_BUF, bcode_size) != BUZZVM_STATE_READY) {
buzzvm_destroy(&VM);
buzzdebug_destroy(&DBG_INFO);
fprintf(stdout, "%s: Error loading Buzz script\n\n", BO_FNAME);
return 0;
}
/* Register hook functions */
if(testing_buzz_register_hooks() != BUZZVM_STATE_READY) {
}
/* Register hook functions */
if(testing_buzz_register_hooks() != BUZZVM_STATE_READY) {
buzzvm_destroy(&VM);
buzzdebug_destroy(&DBG_INFO);
fprintf(stdout, "%s: Error registering hooks\n\n", BO_FNAME);
return 0;
}
}
/* 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;
}
/* 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;
uint16_t age;
};
typedef struct buzzswarm_elem_s* buzzswarm_elem_t;
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) {
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 {
}
else {
int sid = 1;
if(*buzzdict_get(VM->swarms, &sid, uint8_t) &&
buzzdarray_get(e->swarms, 0, uint16_t) != sid) {
@ -489,35 +544,35 @@ namespace buzz_utility{
*status = 3;
return;
}
}
}
/*Step through the buzz script*/
}
}
/*Step through the buzz script*/
void buzz_script_step() {
/*
void buzz_script_step() {
/*
* Update sensors
*/
buzzuav_closures::buzzuav_update_battery(VM);
buzzuav_closures::buzzuav_update_prox(VM);
buzzuav_closures::buzzuav_update_currentpos(VM);
buzzuav_closures::buzzuav_update_flight_status(VM);
buzzuav_closures::buzzuav_update_obstacle(VM);
/*
buzzuav_closures::buzzuav_update_battery(VM);
buzzuav_closures::buzzuav_update_prox(VM);
buzzuav_closures::buzzuav_update_currentpos(VM);
buzzuav_closures::buzzuav_update_flight_status(VM);
buzzuav_closures::buzzuav_update_obstacle(VM);
/*
* Call Buzz step() function
*/
if(buzzvm_function_call(VM, "step", 0) != BUZZVM_STATE_READY) {
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);
}
/*!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!*/
/* look into this currently we don't have swarm feature at all */
/*!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!*/
/*Print swarm*/
//buzzswarm_members_print(stdout, VM->swarmmembers, VM->robot);
/* Check swarm state */
/* int status = 1;
}
/*!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!*/
/* look into this currently we don't have swarm feature at all */
/*!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!*/
/*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)
@ -525,14 +580,14 @@ namespace buzz_utility{
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) {
if(VM->state != BUZZVM_STATE_READY) {
fprintf(stderr, "%s: execution terminated abnormally: %s\n\n",
BO_FNAME,
@ -543,10 +598,9 @@ void buzz_script_destroy() {
buzzvm_destroy(&VM);
free(BO_FNAME);
buzzdebug_destroy(&DBG_INFO);
}
fprintf(stdout, "Script execution stopped.\n");
}
fprintf(stdout, "Script execution stopped.\n");
}
/****************************************/
/****************************************/
@ -556,10 +610,10 @@ void buzz_script_destroy() {
/****************************************/
int buzz_script_done() {
return VM->state != BUZZVM_STATE_READY;
return VM->state != BUZZVM_STATE_READY;
}
int update_step_test(){
int update_step_test() {
buzzuav_closures::buzzuav_update_battery(VM);
buzzuav_closures::buzzuav_update_prox(VM);
@ -568,29 +622,27 @@ int update_step_test(){
buzzuav_closures::buzzuav_update_obstacle(VM);
int a = buzzvm_function_call(VM, "step", 0);
if(a != BUZZVM_STATE_READY){
if(a != BUZZVM_STATE_READY) {
fprintf(stdout, "step test VM state %i\n",a);
fprintf(stdout, " execution terminated abnormally\n\n");
}
}
return a == BUZZVM_STATE_READY;
}
uint16_t get_robotid(){
uint16_t get_robotid() {
/* 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 = strtol(hstnm + 4, NULL, 10);
//fprintf(stdout, "Robot id from get rid buzz util: %i\n",id);
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 = strtol(hstnm + 4, NULL, 10);
//fprintf(stdout, "Robot id from get rid buzz util: %i\n",id);
return (uint16_t)id;
}
buzzvm_t get_vm(){
buzzvm_t get_vm() {
return VM;
}
}

63
src/roscontroller.cpp
View File

@ -1,4 +1,5 @@
#include "roscontroller.h"
#include <thread>
namespace rosbzz_node{
@ -161,6 +162,7 @@ namespace rosbzz_node{
neigh_pos_pub = n_c.advertise<rosbuzz::neigh_pos>("/neighbours_pos",1000);
/* Service Clients*/
arm_client = n_c.serviceClient<mavros_msgs::CommandBool>(armclient);
mission_client = n_c.serviceClient<mavros_msgs::WaypointPush>("/mavros/mission/push");
mode_client = n_c.serviceClient<mavros_msgs::SetMode>(modeclient);
mav_client = n_c.serviceClient<mavros_msgs::CommandLong>(fcclient_name);
@ -224,10 +226,37 @@ namespace rosbzz_node{
}
}
void roscontroller::SetMode(){
void roscontroller::WaypointMissionSetup(float lat, float lng, float alt){
mavros_msgs::WaypointPush srv;
mavros_msgs::Waypoint waypoint;
// prepare waypoint mission package
waypoint.frame = mavros_msgs::Waypoint::FRAME_GLOBAL;
waypoint.command = mavros_msgs::CommandCode::NAV_WAYPOINT;
waypoint.is_current = 2; // IMPORTANT: goto is no longer used, so instead, use magic number 2 for is_current parameter
waypoint.autocontinue = 0;
waypoint.x_lat = lat;
waypoint.y_long = lng;
waypoint.z_alt = alt;
srv.request.waypoints.push_back(waypoint);
if(mission_client.call(srv)){
ROS_INFO("Mission service called!");
} else {
ROS_INFO("Mission service failed!");
}
}
void roscontroller::SetMode(std::string mode, int delay_miliseconds){
// wait if necessary
if (delay_miliseconds > 0){
std::this_thread::sleep_for( std::chrono::milliseconds ( delay_miliseconds ) );
}
// set mode
mavros_msgs::SetMode set_mode_message;
set_mode_message.request.base_mode = 0;
set_mode_message.request.custom_mode = "GUIDED"; // shit!
set_mode_message.request.custom_mode = mode;
if(mode_client.call(set_mode_message)) {
ROS_INFO("Set Mode Service call successful!");
} else {
@ -235,6 +264,12 @@ namespace rosbzz_node{
}
}
//todo: this
void roscontroller::SetModeAsync(std::string mode, int delay_miliseconds){
std::thread([&](){SetMode(mode, delay_miliseconds);}).detach();
cout << "Async call called... " <<endl;
}
/*Prepare messages for each step and publish*/
/*******************************************************************************************************/
@ -566,7 +601,7 @@ namespace rosbzz_node{
buzzuav_closures::flight_status_update(1);
else if (msg->mode == "LAND")
buzzuav_closures::flight_status_update(4);
else // ground standby = LOITER?
else
buzzuav_closures::flight_status_update(1);//?
}
@ -685,11 +720,19 @@ namespace rosbzz_node{
ROS_INFO("RC_call: TAKE OFF!!!!");
rc_cmd=mavros_msgs::CommandCode::NAV_TAKEOFF;
/* arming */
<<<<<<< HEAD
SetMode();
if(!armstate) {
armstate =1;
Arm();
}
=======
SetMode("LOITER", 0);
//SetMode("GUIDED", 0);
cout << "this..." << endl;
SetModeAsync("GUIDED", 2000);
Arm();
>>>>>>> 4ac9d89f7c5e82fe99a48f616c587efd01d50ddd
buzzuav_closures::rc_call(rc_cmd);
res.success = true;
break;
@ -717,11 +760,25 @@ namespace rosbzz_node{
break;
case mavros_msgs::CommandCode::NAV_WAYPOINT:
ROS_INFO("RC_Call: GO TO!!!! ");
//WaypointMissionSetup();
double rc_goto[3];
rc_goto[0] = req.param5;
rc_goto[1] = req.param6;
rc_goto[2] = req.param7;
WaypointMissionSetup(req.param5, req.param6, req.param7);
/*
WaypointMissionSetup(45.505293f, -73.614722f, 2.0f);
std::this_thread::sleep_for( std::chrono::milliseconds ( 100 ) );
WaypointMissionSetup(45.505324f, -73.614502f, 2.0f);
std::this_thread::sleep_for( std::chrono::milliseconds ( 100 ) );
WaypointMissionSetup(45.505452f, -73.614655f, 2.0f);
std::this_thread::sleep_for( std::chrono::milliseconds ( 100 ) );
WaypointMissionSetup(45.505463f, -73.614389f, 2.0f);
std::this_thread::sleep_for( std::chrono::milliseconds ( 100 ) );
*/
buzzuav_closures::rc_set_goto(rc_goto);
rc_cmd=mavros_msgs::CommandCode::NAV_WAYPOINT;
buzzuav_closures::rc_call(rc_cmd);