modified rosbuzz to be compatible for parallel run of multiple instances
This commit is contained in:
vivek
parent
27bdabeda4
commit
5b288b0d37
|
|
@ -9,7 +9,7 @@ endif()
|
|||
find_package(catkin REQUIRED COMPONENTS
|
||||
roscpp
|
||||
std_msgs
|
||||
mavros_msgs
|
||||
# mavros_msgs
|
||||
sensor_msgs
|
||||
)
|
||||
|
||||
|
|
|
|||
|
|
@ -24,12 +24,13 @@
|
|||
using namespace std;
|
||||
|
||||
namespace rosbzz_node{
|
||||
|
||||
|
||||
/*Create node Handler*/
|
||||
//ros::NodeHandle n_c;
|
||||
|
||||
class roscontroller{
|
||||
|
||||
public:
|
||||
roscontroller();
|
||||
roscontroller(ros::NodeHandle n_c);
|
||||
~roscontroller();
|
||||
//void RosControllerInit();
|
||||
void RosControllerRun();
|
||||
|
|
@ -43,7 +44,7 @@ private:
|
|||
int robot_id=0;
|
||||
int oldcmdID=0;
|
||||
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;
|
||||
ros::ServiceClient mav_client;
|
||||
ros::Publisher payload_pub;
|
||||
|
|
@ -52,16 +53,14 @@ private:
|
|||
ros::Subscriber battery_sub;
|
||||
ros::Subscriber payload_sub;
|
||||
ros::Subscriber flight_status_sub;
|
||||
/*Create node Handler*/
|
||||
ros::NodeHandle n_c;
|
||||
/*Commands for flight controller*/
|
||||
mavros_msgs::CommandInt cmd_srv;
|
||||
|
||||
|
||||
void Initialize_pub_sub();
|
||||
void Initialize_pub_sub(ros::NodeHandle n_c);
|
||||
|
||||
/*Obtain data from ros parameter server*/
|
||||
void Rosparameters_get();
|
||||
void Rosparameters_get(ros::NodeHandle n_c);
|
||||
|
||||
/*compiles buzz script from the specified .bzz file*/
|
||||
void Compile_bzz();
|
||||
|
|
|
|||
|
|
@ -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);
|
||||
|
||||
|
||||
|
||||
};
|
||||
|
||||
}
|
||||
|
|
@ -1,11 +1,15 @@
|
|||
<!-- Launch file for ROSBuzz -->
|
||||
|
||||
<launch>
|
||||
<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" />
|
||||
<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" />
|
||||
</node>
|
||||
<param name="robot_id" value="1"/>
|
||||
<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>
|
||||
|
||||
|
||||
</launch>
|
||||
|
|
|
|||
|
|
@ -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>
|
||||
|
|
@ -66,7 +66,12 @@ namespace buzz_utility{
|
|||
memcpy(pl, payload ,size);
|
||||
|
||||
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 */
|
||||
uint16_t unMsgSize;
|
||||
do {
|
||||
|
|
@ -74,13 +79,13 @@ namespace buzz_utility{
|
|||
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) {
|
||||
if(unMsgSize > 0 && unMsgSize <= size - 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);
|
||||
}
|
||||
|
|
|
|||
113
src/out.basm
113
src/out.basm
|
|
@ -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
BIN
src/out.bdbg
Binary file not shown.
BIN
src/out.bo
BIN
src/out.bo
Binary file not shown.
|
|
@ -16,7 +16,8 @@ int main(int argc, char **argv)
|
|||
{
|
||||
/*Initialize rosBuzz node*/
|
||||
ros::init(argc, argv, "rosBuzz");
|
||||
rosbzz_node::roscontroller RosController;
|
||||
ros::NodeHandle n_c("~");
|
||||
rosbzz_node::roscontroller RosController(n_c);
|
||||
RosController.RosControllerRun();
|
||||
return 0;
|
||||
}
|
||||
|
|
|
|||
|
|
@ -3,15 +3,18 @@
|
|||
|
||||
|
||||
namespace rosbzz_node{
|
||||
/*Create node Handler*/
|
||||
|
||||
/***Constructor***/
|
||||
roscontroller::roscontroller()
|
||||
roscontroller::roscontroller(ros::NodeHandle n_c)
|
||||
{
|
||||
/*Create node Handler*/
|
||||
|
||||
ROS_INFO("Buzz_node");
|
||||
/*Obtain parameters from ros parameter server*/
|
||||
Rosparameters_get();
|
||||
Rosparameters_get(n_c);
|
||||
/*Initialize publishers, subscribers and client*/
|
||||
Initialize_pub_sub();
|
||||
Initialize_pub_sub(n_c);
|
||||
/*Compile the .bzz file to .basm, .bo and .bdbg*/
|
||||
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*/
|
||||
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");}
|
||||
/*Obtain rc service option from parameter server*/
|
||||
if(ros::param::get("/rosbuzz_node/rcclient", rcclient)){
|
||||
if(n_c.getParam("rcclient", rcclient)){
|
||||
if(rcclient==true){
|
||||
/*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);
|
||||
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");}
|
||||
/*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");}
|
||||
/*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*/
|
||||
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);
|
||||
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);
|
||||
/*publishers*/
|
||||
payload_pub = n_c.advertise<mavros_msgs::Mavlink>("outMavlink", 1000);
|
||||
payload_pub = n_c.advertise<mavros_msgs::Mavlink>(out_payload, 1000);
|
||||
|
||||
/* Clients*/
|
||||
mav_client = n_c.serviceClient<mavros_msgs::CommandInt>(fcclient_name);
|
||||
|
||||
|
|
@ -94,19 +105,24 @@ namespace rosbzz_node{
|
|||
/*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";
|
||||
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(bzzfile_in_compile.str().c_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());
|
||||
bzzfile_in_compile.str("");
|
||||
bzzfile_in_compile <<path<<"/out.bo";
|
||||
bzzfile_in_compile <<path<<name<<".bo";
|
||||
bcfname = 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();
|
||||
|
||||
|
||||
}
|
||||
|
||||
void roscontroller::prepare_msg_and_publish(){
|
||||
|
|
@ -117,8 +133,8 @@ namespace rosbzz_node{
|
|||
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'"); }
|
||||
if (mav_client.call(cmd_srv));//{ROS_INFO("Reply: %ld", (long int)cmd_srv.response.success); }
|
||||
else ROS_ERROR("Failed to call service from flight controller");
|
||||
/*obtain Pay load to be sent*/
|
||||
uint64_t* payload_out_ptr= buzz_utility::out_msg_process();
|
||||
uint64_t position[3];
|
||||
|
|
@ -138,9 +154,15 @@ namespace rosbzz_node{
|
|||
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++;
|
||||
}*/
|
||||
}
|
||||
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*/
|
||||
payload_pub.publish(payload_out);
|
||||
delete[] out;
|
||||
|
|
|
|||
24
src/test.bzz
24
src/test.bzz
|
|
@ -1,17 +1,27 @@
|
|||
# Executed once at init time.
|
||||
function init() {
|
||||
i=0
|
||||
uav_takeoff()
|
||||
i = 1
|
||||
}
|
||||
|
||||
# Executed at each time step.
|
||||
function step() {
|
||||
|
||||
print("bzz print: remaining: ", battery.capacity)
|
||||
print("latitude : ",position.latitude," longitude: ",position.longitude," altitude : ", position.altitude)
|
||||
print("flight status: ",flight.status)
|
||||
if(i==10){uav_land()}
|
||||
i=i+1
|
||||
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")
|
||||
}
|
||||
|
||||
}
|
||||
|
||||
|
|
|
|||
|
|
@ -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() {
|
||||
}
|
||||
|
||||
Loading…
Reference in New Issue