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

modified rosbuzz to be compatible for parallel run of multiple instances

This commit is contained in:
vivek 2016-11-28 20:35:33 -05:00
parent 27bdabeda4
commit 5b288b0d37
14 changed files with 146 additions and 259 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

2
CMakeLists.txt
View File

@ -9,7 +9,7 @@ endif()
find_package(catkin REQUIRED COMPONENTS find_package(catkin REQUIRED COMPONENTS
roscpp roscpp
std_msgs std_msgs
mavros_msgs # mavros_msgs
sensor_msgs sensor_msgs
) )

13
include/roscontroller.h
View File

@ -24,12 +24,13 @@
using namespace std; using namespace std;
namespace rosbzz_node{ namespace rosbzz_node{
/*Create node Handler*/
//ros::NodeHandle n_c;
class roscontroller{ class roscontroller{
public: public:
roscontroller(); roscontroller(ros::NodeHandle n_c);
~roscontroller(); ~roscontroller();
//void RosControllerInit(); //void RosControllerInit();
void RosControllerRun(); void RosControllerRun();
@ -43,7 +44,7 @@ private:
int robot_id=0; int robot_id=0;
int oldcmdID=0; int oldcmdID=0;
int rc_cmd; int rc_cmd;
std::string bzzfile_name, fcclient_name, rcservice_name,bcfname,dbgfname; //, rcclient; std::string bzzfile_name, fcclient_name, rcservice_name,bcfname,dbgfname,out_payload,in_payload; //, rcclient;
bool rcclient; bool rcclient;
ros::ServiceClient mav_client; ros::ServiceClient mav_client;
ros::Publisher payload_pub; ros::Publisher payload_pub;
@ -52,16 +53,14 @@ private:
ros::Subscriber battery_sub; ros::Subscriber battery_sub;
ros::Subscriber payload_sub; ros::Subscriber payload_sub;
ros::Subscriber flight_status_sub; ros::Subscriber flight_status_sub;
/*Create node Handler*/
ros::NodeHandle n_c;
/*Commands for flight controller*/ /*Commands for flight controller*/
mavros_msgs::CommandInt cmd_srv; mavros_msgs::CommandInt cmd_srv;
void Initialize_pub_sub(); void Initialize_pub_sub(ros::NodeHandle n_c);
/*Obtain data from ros parameter server*/ /*Obtain data from ros parameter server*/
void Rosparameters_get(); void Rosparameters_get(ros::NodeHandle n_c);
/*compiles buzz script from the specified .bzz file*/ /*compiles buzz script from the specified .bzz file*/
void Compile_bzz(); void Compile_bzz();

100
include/roscontroller.h.save
View File

@ -1,100 +0,0 @@
#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,bcfname,dbgfname; //, 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);
};
}

10
launch/rosbuzz.launch
View File

@ -2,10 +2,14 @@
<launch> <launch>
<node name="rosbuzz_node" pkg="rosbuzz" type="rosbuzz_node" respawn="false" output="screen" > <node name="rosbuzz_node" pkg="rosbuzz" type="rosbuzz_node" respawn="false" output="screen" >
<param name="bzzfile_name" value="../ROS_WS/src/ROSBuzz/src/test.bzz" /> <param name="bzzfile_name" value="/home/vivek/catkin_ws/src/rosbuzz/src/test.bzz" />
<param name="rcclient" value="true" /> <param name="rcclient" value="true" />
<param name="rcservice_name" value="rc_cmd" /> <param name="rcservice_name" value="rc_cmd" />
<param name="fcclient_name" value="djicmd" /> <param name="fcclient_name" value="/djicmd" />
</node> <param name="in_payload" value="/rosbuzz_node1/outMavlink"/>
<param name="out_payload" value="outMavlink"/>
<param name="robot_id" value="1"/> <param name="robot_id" value="1"/>
</node>
</launch> </launch>

23
launch/rosbuzz_2_parallel.launch Normal file
View File

@ -0,0 +1,23 @@
<!-- Launch file for ROSBuzz -->
<launch>
<node name="rosbuzz_node" pkg="rosbuzz" type="rosbuzz_node" respawn="false" output="screen" >
<param name="bzzfile_name" value="/home/vivek/catkin_ws/src/rosbuzz/src/test.bzz" />
<param name="rcclient" value="true" />
<param name="rcservice_name" value="rc_cmd" />
<param name="fcclient_name" value="/djicmd" />
<param name="in_payload" value="/rosbuzz_node1/outMavlink"/>
<param name="out_payload" value="outMavlink"/>
<param name="robot_id" value="1"/>
</node>
<node name="rosbuzz_node1" pkg="rosbuzz" type="rosbuzz_node" respawn="false" output="screen" >
<param name="bzzfile_name" value="/home/vivek/catkin_ws/src/rosbuzz/src/test1.bzz" />
<param name="rcclient" value="true" />
<param name="rcservice_name" value="rc_cmd" />
<param name="fcclient_name" value="/djicmd" />
<param name="in_payload" value="/rosbuzz_node/outMavlink"/>
<param name="out_payload" value="outMavlink"/>
<param name="robot_id" value="2"/>
</node>
</launch>

0
launch/rosbuzz.launch.save → src/.Rhistory
View File

9
src/buzz_utility.cpp
View File

@ -66,7 +66,12 @@ namespace buzz_utility{
memcpy(pl, payload ,size); memcpy(pl, payload ,size);
size_t tot = sizeof(uint32_t); size_t tot = sizeof(uint32_t);
/*for(int i=0;i<data[0];i++){
uint16_t* out = u64_cvt_u16(payload[i]);
for(int k=0;k<4;k++){
cout<<" [Debug inside buzz util: obt msg:] "<<out[k]<<endl;
}
}*/
/* Go through the messages until there's nothing else to read */ /* Go through the messages until there's nothing else to read */
uint16_t unMsgSize; uint16_t unMsgSize;
do { do {
@ -74,7 +79,7 @@ namespace buzz_utility{
unMsgSize = *(uint16_t*)(pl + tot); unMsgSize = *(uint16_t*)(pl + tot);
tot += sizeof(uint16_t); tot += sizeof(uint16_t);
/* Append message to the Buzz input message queue */ /* Append message to the Buzz input message queue */
if(unMsgSize > 0 && unMsgSize <= size*sizeof(uint64_t) - tot) { if(unMsgSize > 0 && unMsgSize <= size - tot ) {
buzzinmsg_queue_append(VM->inmsgs, buzzinmsg_queue_append(VM->inmsgs,
buzzmsg_payload_frombuffer(pl +tot, unMsgSize)); buzzmsg_payload_frombuffer(pl +tot, unMsgSize));
tot += unMsgSize; tot += unMsgSize;

113
src/out.basm
View File

@ -1,113 +0,0 @@
!21
'init
'i
'uav_takeoff
'step
'print
'bzz print: remaining:
'battery
'capacity
'latitude :
'position
'latitude
' longitude:
'longitude
' altitude :
'altitude
'flight status:
'flight
'status
'uav_land
'reset
'destroy
pushs 0
pushcn @__label_1
gstore
pushs 3
pushcn @__label_2
gstore
pushs 19
pushcn @__label_5
gstore
pushs 20
pushcn @__label_6
gstore
nop
@__label_0
@__exitpoint
done
@__label_1
pushs 1 |3,1,/home/ubuntu/ROS_WS/src/ROSBuzz/src/test.bzz
pushi 0 |3,2,/home/ubuntu/ROS_WS/src/ROSBuzz/src/test.bzz
gstore |3,3,/home/ubuntu/ROS_WS/src/ROSBuzz/src/test.bzz
pushs 2 |4,11,/home/ubuntu/ROS_WS/src/ROSBuzz/src/test.bzz
gload |4,11,/home/ubuntu/ROS_WS/src/ROSBuzz/src/test.bzz
pushi 0 |4,13,/home/ubuntu/ROS_WS/src/ROSBuzz/src/test.bzz
callc |4,13,/home/ubuntu/ROS_WS/src/ROSBuzz/src/test.bzz
ret0 |5,1,/home/ubuntu/ROS_WS/src/ROSBuzz/src/test.bzz
@__label_2
pushs 4 |10,5,/home/ubuntu/ROS_WS/src/ROSBuzz/src/test.bzz
gload |10,5,/home/ubuntu/ROS_WS/src/ROSBuzz/src/test.bzz
pushs 5 |10,6,/home/ubuntu/ROS_WS/src/ROSBuzz/src/test.bzz
pushs 6 |10,39,/home/ubuntu/ROS_WS/src/ROSBuzz/src/test.bzz
gload |10,39,/home/ubuntu/ROS_WS/src/ROSBuzz/src/test.bzz
pushs 7 |10,40,/home/ubuntu/ROS_WS/src/ROSBuzz/src/test.bzz
tget |10,48,/home/ubuntu/ROS_WS/src/ROSBuzz/src/test.bzz
pushi 2 |10,49,/home/ubuntu/ROS_WS/src/ROSBuzz/src/test.bzz
callc |10,49,/home/ubuntu/ROS_WS/src/ROSBuzz/src/test.bzz
pushs 4 |11,5,/home/ubuntu/ROS_WS/src/ROSBuzz/src/test.bzz
gload |11,5,/home/ubuntu/ROS_WS/src/ROSBuzz/src/test.bzz
pushs 8 |11,6,/home/ubuntu/ROS_WS/src/ROSBuzz/src/test.bzz
pushs 9 |11,28,/home/ubuntu/ROS_WS/src/ROSBuzz/src/test.bzz
gload |11,28,/home/ubuntu/ROS_WS/src/ROSBuzz/src/test.bzz
pushs 10 |11,29,/home/ubuntu/ROS_WS/src/ROSBuzz/src/test.bzz
tget |11,37,/home/ubuntu/ROS_WS/src/ROSBuzz/src/test.bzz
pushs 11 |11,38,/home/ubuntu/ROS_WS/src/ROSBuzz/src/test.bzz
pushs 9 |11,61,/home/ubuntu/ROS_WS/src/ROSBuzz/src/test.bzz
gload |11,61,/home/ubuntu/ROS_WS/src/ROSBuzz/src/test.bzz
pushs 12 |11,62,/home/ubuntu/ROS_WS/src/ROSBuzz/src/test.bzz
tget |11,71,/home/ubuntu/ROS_WS/src/ROSBuzz/src/test.bzz
pushs 13 |11,72,/home/ubuntu/ROS_WS/src/ROSBuzz/src/test.bzz
pushs 9 |11,96,/home/ubuntu/ROS_WS/src/ROSBuzz/src/test.bzz
gload |11,96,/home/ubuntu/ROS_WS/src/ROSBuzz/src/test.bzz
pushs 14 |11,97,/home/ubuntu/ROS_WS/src/ROSBuzz/src/test.bzz
tget |11,105,/home/ubuntu/ROS_WS/src/ROSBuzz/src/test.bzz
pushi 6 |11,106,/home/ubuntu/ROS_WS/src/ROSBuzz/src/test.bzz
callc |11,106,/home/ubuntu/ROS_WS/src/ROSBuzz/src/test.bzz
pushs 4 |12,5,/home/ubuntu/ROS_WS/src/ROSBuzz/src/test.bzz
gload |12,5,/home/ubuntu/ROS_WS/src/ROSBuzz/src/test.bzz
pushs 15 |12,6,/home/ubuntu/ROS_WS/src/ROSBuzz/src/test.bzz
pushs 16 |12,30,/home/ubuntu/ROS_WS/src/ROSBuzz/src/test.bzz
gload |12,30,/home/ubuntu/ROS_WS/src/ROSBuzz/src/test.bzz
pushs 17 |12,31,/home/ubuntu/ROS_WS/src/ROSBuzz/src/test.bzz
tget |12,37,/home/ubuntu/ROS_WS/src/ROSBuzz/src/test.bzz
pushi 2 |12,38,/home/ubuntu/ROS_WS/src/ROSBuzz/src/test.bzz
callc |12,38,/home/ubuntu/ROS_WS/src/ROSBuzz/src/test.bzz
pushs 1 |13,4,/home/ubuntu/ROS_WS/src/ROSBuzz/src/test.bzz
gload |13,4,/home/ubuntu/ROS_WS/src/ROSBuzz/src/test.bzz
pushi 10 |13,6,/home/ubuntu/ROS_WS/src/ROSBuzz/src/test.bzz
eq |13,8,/home/ubuntu/ROS_WS/src/ROSBuzz/src/test.bzz
jumpz @__label_3 |13,9,/home/ubuntu/ROS_WS/src/ROSBuzz/src/test.bzz
pushs 18 |13,18,/home/ubuntu/ROS_WS/src/ROSBuzz/src/test.bzz
gload |13,18,/home/ubuntu/ROS_WS/src/ROSBuzz/src/test.bzz
pushi 0 |13,20,/home/ubuntu/ROS_WS/src/ROSBuzz/src/test.bzz
callc |13,20,/home/ubuntu/ROS_WS/src/ROSBuzz/src/test.bzz
@__label_3 |14,0,/home/ubuntu/ROS_WS/src/ROSBuzz/src/test.bzz
pushs 1 |14,1,/home/ubuntu/ROS_WS/src/ROSBuzz/src/test.bzz
pushs 1 |14,3,/home/ubuntu/ROS_WS/src/ROSBuzz/src/test.bzz
gload |14,3,/home/ubuntu/ROS_WS/src/ROSBuzz/src/test.bzz
pushi 1 |14,4,/home/ubuntu/ROS_WS/src/ROSBuzz/src/test.bzz
add |14,5,/home/ubuntu/ROS_WS/src/ROSBuzz/src/test.bzz
gstore |14,5,/home/ubuntu/ROS_WS/src/ROSBuzz/src/test.bzz
ret0 |16,1,/home/ubuntu/ROS_WS/src/ROSBuzz/src/test.bzz
@__label_5
ret0 |20,1,/home/ubuntu/ROS_WS/src/ROSBuzz/src/test.bzz
@__label_6
ret0 |24,1,/home/ubuntu/ROS_WS/src/ROSBuzz/src/test.bzz

BIN
src/out.bdbg
View File

Binary file not shown.

BIN
src/out.bo
View File

Binary file not shown.

3
src/rosbuzz.cpp
View File

@ -16,7 +16,8 @@ int main(int argc, char **argv)
{ {
/*Initialize rosBuzz node*/ /*Initialize rosBuzz node*/
ros::init(argc, argv, "rosBuzz"); ros::init(argc, argv, "rosBuzz");
rosbzz_node::roscontroller RosController; ros::NodeHandle n_c("~");
rosbzz_node::roscontroller RosController(n_c);
RosController.RosControllerRun(); RosController.RosControllerRun();
return 0; return 0;
} }

60
src/roscontroller.cpp
View File

@ -3,15 +3,18 @@
namespace rosbzz_node{ namespace rosbzz_node{
/*Create node Handler*/
/***Constructor***/ /***Constructor***/
roscontroller::roscontroller() roscontroller::roscontroller(ros::NodeHandle n_c)
{ {
/*Create node Handler*/
ROS_INFO("Buzz_node"); ROS_INFO("Buzz_node");
/*Obtain parameters from ros parameter server*/ /*Obtain parameters from ros parameter server*/
Rosparameters_get(); Rosparameters_get(n_c);
/*Initialize publishers, subscribers and client*/ /*Initialize publishers, subscribers and client*/
Initialize_pub_sub(); Initialize_pub_sub(n_c);
/*Compile the .bzz file to .basm, .bo and .bdbg*/ /*Compile the .bzz file to .basm, .bo and .bdbg*/
Compile_bzz(); Compile_bzz();
} }
@ -52,15 +55,16 @@ namespace rosbzz_node{
} }
} }
void roscontroller::Rosparameters_get(){ void roscontroller::Rosparameters_get(ros::NodeHandle n_c){
/*Obtain .bzz file name from parameter server*/ /*Obtain .bzz file name from parameter server*/
if(ros::param::get("/rosbuzz_node/bzzfile_name", bzzfile_name)); if(n_c.getParam("bzzfile_name", bzzfile_name));
else {ROS_ERROR("Provide a .bzz file to run in Launch file"); system("rosnode kill rosbuzz_node");} else {ROS_ERROR("Provide a .bzz file to run in Launch file"); system("rosnode kill rosbuzz_node");}
/*Obtain rc service option from parameter server*/ /*Obtain rc service option from parameter server*/
if(ros::param::get("/rosbuzz_node/rcclient", rcclient)){ if(n_c.getParam("rcclient", rcclient)){
if(rcclient==true){ if(rcclient==true){
/*Service*/ /*Service*/
if(ros::param::get("/rosbuzz_node/rcservice_name", rcservice_name)){ if(n_c.getParam("rcservice_name", rcservice_name)){
service = n_c.advertiseService(rcservice_name, &roscontroller::rc_callback,this); service = n_c.advertiseService(rcservice_name, &roscontroller::rc_callback,this);
ROS_INFO("Ready to receive Mav Commands from RC client"); ROS_INFO("Ready to receive Mav Commands from RC client");
} }
@ -70,21 +74,28 @@ namespace rosbzz_node{
} }
else{ROS_ERROR("Provide a rc client option: yes or no in Launch file"); system("rosnode kill rosbuzz_node");} 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*/ /*Obtain fc client name from parameter server*/
if(ros::param::get("/rosbuzz_node/fcclient_name", fcclient_name)); if(n_c.getParam("fcclient_name", fcclient_name));
else {ROS_ERROR("Provide a fc client name in Launch file"); system("rosnode kill rosbuzz_node");} else {ROS_ERROR("Provide a fc client name in Launch file"); system("rosnode kill rosbuzz_node");}
/*Obtain robot_id from parameter server*/ /*Obtain robot_id from parameter server*/
ros::param::get("/rosbuzz_node/robot_id", robot_id); n_c.getParam("robot_id", robot_id);
/*Obtain out payload name*/
n_c.getParam("out_payload", out_payload);
/*Obtain in payload name*/
n_c.getParam("in_payload", in_payload);
} }
void roscontroller::Initialize_pub_sub(){ void roscontroller::Initialize_pub_sub(ros::NodeHandle n_c){
/*subscribers*/ /*subscribers*/
current_position_sub = n_c.subscribe("/mav/global_position", 1000, &roscontroller::current_pos,this); current_position_sub = n_c.subscribe("/mav/global_position", 1000, &roscontroller::current_pos,this);
battery_sub = n_c.subscribe("/mav/power_status", 1000, &roscontroller::battery,this); battery_sub = n_c.subscribe("/mav/power_status", 1000, &roscontroller::battery,this);
payload_sub = n_c.subscribe("inMavlink", 1000, &roscontroller::payload_obt,this); payload_sub = n_c.subscribe(in_payload, 1000, &roscontroller::payload_obt,this);
flight_status_sub =n_c.subscribe("/mav/flight_status",100, &roscontroller::flight_status_update,this); flight_status_sub =n_c.subscribe("/mav/flight_status",100, &roscontroller::flight_status_update,this);
/*publishers*/ /*publishers*/
payload_pub = n_c.advertise<mavros_msgs::Mavlink>("outMavlink", 1000); payload_pub = n_c.advertise<mavros_msgs::Mavlink>(out_payload, 1000);
/* Clients*/ /* Clients*/
mav_client = n_c.serviceClient<mavros_msgs::CommandInt>(fcclient_name); mav_client = n_c.serviceClient<mavros_msgs::CommandInt>(fcclient_name);
@ -94,17 +105,22 @@ namespace rosbzz_node{
/*Compile the buzz code .bzz to .bo*/ /*Compile the buzz code .bzz to .bo*/
stringstream bzzfile_in_compile; stringstream bzzfile_in_compile;
std::string path = bzzfile_name.substr(0, bzzfile_name.find_last_of("\\/")); std::string path = bzzfile_name.substr(0, bzzfile_name.find_last_of("\\/"));
bzzfile_in_compile << "bzzparse "<<bzzfile_name<<" "<<path<< "/out.basm"; bzzfile_in_compile<<path<<"/";
path = bzzfile_in_compile.str();
bzzfile_in_compile.str("");
std::string name = bzzfile_name.substr(bzzfile_name.find_last_of("/\\") + 1);
name = name.substr(0,name.find_last_of("."));
bzzfile_in_compile << "bzzparse "<<bzzfile_name<<" "<<path<< name<<".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("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(bzzfile_in_compile.str().c_str());
bzzfile_in_compile.str(""); bzzfile_in_compile.str("");
bzzfile_in_compile <<"bzzasm "<<path<<"/out.basm "<<path<<"/out.bo "<<path<<"/out.bdbg"; bzzfile_in_compile <<"bzzasm "<<path<<name<<".basm "<<path<<name<<".bo "<<path<<name<<".bdbg";
system(bzzfile_in_compile.str().c_str()); system(bzzfile_in_compile.str().c_str());
bzzfile_in_compile.str(""); bzzfile_in_compile.str("");
bzzfile_in_compile <<path<<"/out.bo"; bzzfile_in_compile <<path<<name<<".bo";
bcfname = bzzfile_in_compile.str(); bcfname = bzzfile_in_compile.str();
bzzfile_in_compile.str(""); bzzfile_in_compile.str("");
bzzfile_in_compile <<path<<"/out.bdbg"; bzzfile_in_compile <<path<<name<<".bdbg";
dbgfname = bzzfile_in_compile.str(); dbgfname = bzzfile_in_compile.str();
} }
@ -117,8 +133,8 @@ namespace rosbzz_node{
cmd_srv.request.param3 = goto_pos[2]; cmd_srv.request.param3 = goto_pos[2];
cmd_srv.request.command = buzzuav_closures::getcmd(); 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); } if (mav_client.call(cmd_srv));//{ROS_INFO("Reply: %ld", (long int)cmd_srv.response.success); }
else{ ROS_ERROR("Failed to call service 'djicmd'"); } else ROS_ERROR("Failed to call service from flight controller");
/*obtain Pay load to be sent*/ /*obtain Pay load to be sent*/
uint64_t* payload_out_ptr= buzz_utility::out_msg_process(); uint64_t* payload_out_ptr= buzz_utility::out_msg_process();
uint64_t position[3]; uint64_t position[3];
@ -138,8 +154,14 @@ namespace rosbzz_node{
for(std::vector<long unsigned int>::const_iterator it = payload_out.payload64.begin(); for(std::vector<long unsigned int>::const_iterator it = payload_out.payload64.begin();
it != payload_out.payload64.end(); ++it){ it != payload_out.payload64.end(); ++it){
message_obt[i] =(uint64_t) *it; message_obt[i] =(uint64_t) *it;
cout<<" [Debug:] sent message "<<*it<<endl;
i++; i++;
}
for(i=0;i<payload_out.payload64.size();i++){
out = buzz_utility::u64_cvt_u16(message_obt[i]);
for(int k=0;k<4;k++){
cout<<" [Debug:] sent message "<<out[k]<<endl;
}
}*/ }*/
/*publish prepared messages in respective topic*/ /*publish prepared messages in respective topic*/
payload_pub.publish(payload_out); payload_pub.publish(payload_out);

24
src/test.bzz
View File

@ -1,17 +1,27 @@
# Executed once at init time. # Executed once at init time.
function init() { function init() {
i=0 i = 1
uav_takeoff()
} }
# Executed at each time step. # Executed at each time step.
function step() { function step() {
print("bzz print: remaining: ", battery.capacity) if (i == 0) {
print("latitude : ",position.latitude," longitude: ",position.longitude," altitude : ", position.altitude) neighbors.listen("Take",
print("flight status: ",flight.status) function(vid, value, rid) {
if(i==10){uav_land()} print("Got (", vid, ",", value, ") from robot #", rid)
i=i+1 }
)
neighbors.listen("key",
function(vid, value, rid) {
print("Got (", vid, ",", value, ") from robot #", rid)
}
)
}
else{
neighbors.broadcast("key", "yes")
neighbors.broadcast("Take", "no")
}
} }

36
src/test1.bzz Normal file
View File

@ -0,0 +1,36 @@
# Executed once at init time.
function init() {
i = 0
}
# Executed at each time step.
function step() {
if (i == 0) {
neighbors.listen("Take",
function(vid, value, rid) {
print("Got (", vid, ",", value, ") from robot #", rid)
}
)
neighbors.listen("key",
function(vid, value, rid) {
print("Got (", vid, ",", value, ") from robot #", rid)
}
)
}
else{
neighbors.broadcast("key", "yes")
neighbors.broadcast("Take", "no")
}
}
# Executed once when the robot (or the simulator) is reset.
function reset() {
}
# Executed once at the end of experiment.
function destroy() {
}