launch file commit
This commit is contained in:
parent
d664adc06b
commit
a128625619
|
@ -37,4 +37,5 @@ install(TARGETS rosbuzz_node
|
||||||
RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION}
|
RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION}
|
||||||
)
|
)
|
||||||
|
|
||||||
|
find_package(catkin REQUIRED COMPONENTS roslaunch)
|
||||||
|
roslaunch_add_file_check(launch)
|
||||||
|
|
|
@ -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>
|
|
@ -40,8 +40,8 @@
|
||||||
<!-- Use test_depend for packages you need only for testing: -->
|
<!-- Use test_depend for packages you need only for testing: -->
|
||||||
<!-- <test_depend>gtest</test_depend> -->
|
<!-- <test_depend>gtest</test_depend> -->
|
||||||
<buildtool_depend>catkin</buildtool_depend>
|
<buildtool_depend>catkin</buildtool_depend>
|
||||||
<build_depend>message_generation</build_depend>
|
<!-- <build_depend>message_generation</build_depend> -->
|
||||||
<run_depend>message_runtime</run_depend>
|
<!-- <run_depend>message_runtime</run_depend> -->
|
||||||
|
|
||||||
<!-- The export tag contains other, unspecified, tags -->
|
<!-- The export tag contains other, unspecified, tags -->
|
||||||
<export>
|
<export>
|
||||||
|
|
|
@ -199,13 +199,13 @@ static int buzz_register_hooks() {
|
||||||
/****************************************/
|
/****************************************/
|
||||||
|
|
||||||
int buzz_script_set(const char* bo_filename,
|
int buzz_script_set(const char* bo_filename,
|
||||||
const char* bdbg_filename) {
|
const char* bdbg_filename, int robot_id) {
|
||||||
/* Get hostname */
|
/* Get hostname */
|
||||||
char hstnm[30];
|
char hstnm[30];
|
||||||
gethostname(hstnm, 30);
|
gethostname(hstnm, 30);
|
||||||
/* Make numeric id from hostname */
|
/* Make numeric id from hostname */
|
||||||
/* NOTE: here we assume that the hostname is in the format Knn */
|
/* NOTE: here we assume that the hostname is in the format Knn */
|
||||||
int id = strtol(hstnm + 1, NULL, 10);
|
int id = robot_id; //strtol(hstnm + 1, NULL, 10);
|
||||||
cout << " Robot ID" << id<< endl;
|
cout << " Robot ID" << id<< endl;
|
||||||
/* Reset the Buzz VM */
|
/* Reset the Buzz VM */
|
||||||
if(VM) buzzvm_destroy(&VM);
|
if(VM) buzzvm_destroy(&VM);
|
||||||
|
|
|
@ -22,7 +22,7 @@ void in_msg_process(uint64_t* payload);
|
||||||
uint64_t* out_msg_process();
|
uint64_t* out_msg_process();
|
||||||
|
|
||||||
int buzz_script_set(const char* bo_filename,
|
int buzz_script_set(const char* bo_filename,
|
||||||
const char* bdbg_filename);
|
const char* bdbg_filename, int robot_id);
|
||||||
void buzz_script_step();
|
void buzz_script_step();
|
||||||
|
|
||||||
void buzz_script_destroy();
|
void buzz_script_destroy();
|
||||||
|
|
150
src/out.basm
150
src/out.basm
|
@ -1,150 +0,0 @@
|
||||||
!24
|
|
||||||
'init
|
|
||||||
'i
|
|
||||||
's
|
|
||||||
'swarm
|
|
||||||
'create
|
|
||||||
'join
|
|
||||||
'step
|
|
||||||
'neighbors
|
|
||||||
'foreach
|
|
||||||
'print
|
|
||||||
'robot
|
|
||||||
':
|
|
||||||
'distance =
|
|
||||||
'distance
|
|
||||||
',
|
|
||||||
'azimuth =
|
|
||||||
'azimuth
|
|
||||||
'elevation =
|
|
||||||
'elevation
|
|
||||||
'exec
|
|
||||||
'uav_takeoff
|
|
||||||
'uav_goto
|
|
||||||
'reset
|
|
||||||
'destroy
|
|
||||||
|
|
||||||
pushs 0
|
|
||||||
pushcn @__label_1
|
|
||||||
gstore
|
|
||||||
pushs 6
|
|
||||||
pushcn @__label_2
|
|
||||||
gstore
|
|
||||||
pushs 22
|
|
||||||
pushcn @__label_8
|
|
||||||
gstore
|
|
||||||
pushs 23
|
|
||||||
pushcn @__label_9
|
|
||||||
gstore
|
|
||||||
nop
|
|
||||||
|
|
||||||
@__label_0
|
|
||||||
|
|
||||||
@__exitpoint
|
|
||||||
done
|
|
||||||
|
|
||||||
@__label_1
|
|
||||||
pushs 1 |3,1,/home/vivek/catkin_ws/src/rosbuzz/src/test.bzz
|
|
||||||
pushi 0 |3,2,/home/vivek/catkin_ws/src/rosbuzz/src/test.bzz
|
|
||||||
gstore |3,3,/home/vivek/catkin_ws/src/rosbuzz/src/test.bzz
|
|
||||||
pushs 2 |4,2,/home/vivek/catkin_ws/src/rosbuzz/src/test.bzz
|
|
||||||
pushs 3 |4,9,/home/vivek/catkin_ws/src/rosbuzz/src/test.bzz
|
|
||||||
gload |4,9,/home/vivek/catkin_ws/src/rosbuzz/src/test.bzz
|
|
||||||
pushs 4 |4,10,/home/vivek/catkin_ws/src/rosbuzz/src/test.bzz
|
|
||||||
tget |4,16,/home/vivek/catkin_ws/src/rosbuzz/src/test.bzz
|
|
||||||
pushi 10 |4,17,/home/vivek/catkin_ws/src/rosbuzz/src/test.bzz
|
|
||||||
pushi 1 |4,20,/home/vivek/catkin_ws/src/rosbuzz/src/test.bzz
|
|
||||||
callc |4,20,/home/vivek/catkin_ws/src/rosbuzz/src/test.bzz
|
|
||||||
gstore |4,20,/home/vivek/catkin_ws/src/rosbuzz/src/test.bzz
|
|
||||||
pushs 2 |5,1,/home/vivek/catkin_ws/src/rosbuzz/src/test.bzz
|
|
||||||
gload |5,1,/home/vivek/catkin_ws/src/rosbuzz/src/test.bzz
|
|
||||||
pushs 5 |5,2,/home/vivek/catkin_ws/src/rosbuzz/src/test.bzz
|
|
||||||
tget |5,6,/home/vivek/catkin_ws/src/rosbuzz/src/test.bzz
|
|
||||||
pushi 0 |5,8,/home/vivek/catkin_ws/src/rosbuzz/src/test.bzz
|
|
||||||
callc |5,8,/home/vivek/catkin_ws/src/rosbuzz/src/test.bzz
|
|
||||||
ret0 |7,1,/home/vivek/catkin_ws/src/rosbuzz/src/test.bzz
|
|
||||||
|
|
||||||
@__label_2
|
|
||||||
pushs 7 |12,9,/home/vivek/catkin_ws/src/rosbuzz/src/test.bzz
|
|
||||||
gload |12,9,/home/vivek/catkin_ws/src/rosbuzz/src/test.bzz
|
|
||||||
pushs 8 |12,10,/home/vivek/catkin_ws/src/rosbuzz/src/test.bzz
|
|
||||||
tget |12,17,/home/vivek/catkin_ws/src/rosbuzz/src/test.bzz
|
|
||||||
pushl @__label_3 |13,2,/home/vivek/catkin_ws/src/rosbuzz/src/test.bzz
|
|
||||||
pushi 1 |17,42,/home/vivek/catkin_ws/src/rosbuzz/src/test.bzz
|
|
||||||
callc |17,42,/home/vivek/catkin_ws/src/rosbuzz/src/test.bzz
|
|
||||||
pushs 1 |18,4,/home/vivek/catkin_ws/src/rosbuzz/src/test.bzz
|
|
||||||
gload |18,4,/home/vivek/catkin_ws/src/rosbuzz/src/test.bzz
|
|
||||||
pushi 1 |18,6,/home/vivek/catkin_ws/src/rosbuzz/src/test.bzz
|
|
||||||
eq |18,7,/home/vivek/catkin_ws/src/rosbuzz/src/test.bzz
|
|
||||||
jumpz @__label_4 |18,8,/home/vivek/catkin_ws/src/rosbuzz/src/test.bzz
|
|
||||||
pushs 2 |19,1,/home/vivek/catkin_ws/src/rosbuzz/src/test.bzz
|
|
||||||
gload |19,1,/home/vivek/catkin_ws/src/rosbuzz/src/test.bzz
|
|
||||||
pushs 19 |19,2,/home/vivek/catkin_ws/src/rosbuzz/src/test.bzz
|
|
||||||
tget |19,6,/home/vivek/catkin_ws/src/rosbuzz/src/test.bzz
|
|
||||||
pushl @__label_6 |19,7,/home/vivek/catkin_ws/src/rosbuzz/src/test.bzz
|
|
||||||
pushi 1 |19,34,/home/vivek/catkin_ws/src/rosbuzz/src/test.bzz
|
|
||||||
callc |19,34,/home/vivek/catkin_ws/src/rosbuzz/src/test.bzz
|
|
||||||
pushs 1 |20,1,/home/vivek/catkin_ws/src/rosbuzz/src/test.bzz
|
|
||||||
pushi 0 |20,2,/home/vivek/catkin_ws/src/rosbuzz/src/test.bzz
|
|
||||||
gstore |20,3,/home/vivek/catkin_ws/src/rosbuzz/src/test.bzz
|
|
||||||
jump @__label_5 |22,4,/home/vivek/catkin_ws/src/rosbuzz/src/test.bzz
|
|
||||||
@__label_4 |22,4,/home/vivek/catkin_ws/src/rosbuzz/src/test.bzz
|
|
||||||
pushs 2 |23,1,/home/vivek/catkin_ws/src/rosbuzz/src/test.bzz
|
|
||||||
gload |23,1,/home/vivek/catkin_ws/src/rosbuzz/src/test.bzz
|
|
||||||
pushs 19 |23,2,/home/vivek/catkin_ws/src/rosbuzz/src/test.bzz
|
|
||||||
tget |23,6,/home/vivek/catkin_ws/src/rosbuzz/src/test.bzz
|
|
||||||
pushl @__label_7 |23,7,/home/vivek/catkin_ws/src/rosbuzz/src/test.bzz
|
|
||||||
pushi 1 |23,51,/home/vivek/catkin_ws/src/rosbuzz/src/test.bzz
|
|
||||||
callc |23,51,/home/vivek/catkin_ws/src/rosbuzz/src/test.bzz
|
|
||||||
pushs 1 |24,1,/home/vivek/catkin_ws/src/rosbuzz/src/test.bzz
|
|
||||||
pushi 1 |24,2,/home/vivek/catkin_ws/src/rosbuzz/src/test.bzz
|
|
||||||
gstore |24,3,/home/vivek/catkin_ws/src/rosbuzz/src/test.bzz
|
|
||||||
@__label_5 |25,1,/home/vivek/catkin_ws/src/rosbuzz/src/test.bzz
|
|
||||||
ret0 |35,1,/home/vivek/catkin_ws/src/rosbuzz/src/test.bzz
|
|
||||||
|
|
||||||
@__label_3
|
|
||||||
pushs 9 |14,9,/home/vivek/catkin_ws/src/rosbuzz/src/test.bzz
|
|
||||||
gload |14,9,/home/vivek/catkin_ws/src/rosbuzz/src/test.bzz
|
|
||||||
pushs 10 |14,10,/home/vivek/catkin_ws/src/rosbuzz/src/test.bzz
|
|
||||||
lload 1 |14,23,/home/vivek/catkin_ws/src/rosbuzz/src/test.bzz
|
|
||||||
pushs 11 |14,25,/home/vivek/catkin_ws/src/rosbuzz/src/test.bzz
|
|
||||||
pushs 12 |15,8,/home/vivek/catkin_ws/src/rosbuzz/src/test.bzz
|
|
||||||
lload 2 |15,28,/home/vivek/catkin_ws/src/rosbuzz/src/test.bzz
|
|
||||||
pushs 13 |15,29,/home/vivek/catkin_ws/src/rosbuzz/src/test.bzz
|
|
||||||
tget |15,37,/home/vivek/catkin_ws/src/rosbuzz/src/test.bzz
|
|
||||||
pushs 14 |15,39,/home/vivek/catkin_ws/src/rosbuzz/src/test.bzz
|
|
||||||
pushs 15 |16,8,/home/vivek/catkin_ws/src/rosbuzz/src/test.bzz
|
|
||||||
lload 2 |16,28,/home/vivek/catkin_ws/src/rosbuzz/src/test.bzz
|
|
||||||
pushs 16 |16,29,/home/vivek/catkin_ws/src/rosbuzz/src/test.bzz
|
|
||||||
tget |16,36,/home/vivek/catkin_ws/src/rosbuzz/src/test.bzz
|
|
||||||
pushs 14 |16,38,/home/vivek/catkin_ws/src/rosbuzz/src/test.bzz
|
|
||||||
pushs 17 |17,8,/home/vivek/catkin_ws/src/rosbuzz/src/test.bzz
|
|
||||||
lload 2 |17,28,/home/vivek/catkin_ws/src/rosbuzz/src/test.bzz
|
|
||||||
pushs 18 |17,29,/home/vivek/catkin_ws/src/rosbuzz/src/test.bzz
|
|
||||||
tget |17,38,/home/vivek/catkin_ws/src/rosbuzz/src/test.bzz
|
|
||||||
pushi 11 |17,40,/home/vivek/catkin_ws/src/rosbuzz/src/test.bzz
|
|
||||||
callc |17,40,/home/vivek/catkin_ws/src/rosbuzz/src/test.bzz
|
|
||||||
ret0 |17,41,/home/vivek/catkin_ws/src/rosbuzz/src/test.bzz
|
|
||||||
|
|
||||||
@__label_6
|
|
||||||
pushs 20 |19,30,/home/vivek/catkin_ws/src/rosbuzz/src/test.bzz
|
|
||||||
gload |19,30,/home/vivek/catkin_ws/src/rosbuzz/src/test.bzz
|
|
||||||
pushi 0 |19,32,/home/vivek/catkin_ws/src/rosbuzz/src/test.bzz
|
|
||||||
callc |19,32,/home/vivek/catkin_ws/src/rosbuzz/src/test.bzz
|
|
||||||
ret0 |19,33,/home/vivek/catkin_ws/src/rosbuzz/src/test.bzz
|
|
||||||
|
|
||||||
@__label_7
|
|
||||||
pushs 21 |23,27,/home/vivek/catkin_ws/src/rosbuzz/src/test.bzz
|
|
||||||
gload |23,27,/home/vivek/catkin_ws/src/rosbuzz/src/test.bzz
|
|
||||||
pushf 1.1234 |23,28,/home/vivek/catkin_ws/src/rosbuzz/src/test.bzz
|
|
||||||
pushf 2.2345 |23,35,/home/vivek/catkin_ws/src/rosbuzz/src/test.bzz
|
|
||||||
pushf 3.3456 |23,42,/home/vivek/catkin_ws/src/rosbuzz/src/test.bzz
|
|
||||||
pushi 3 |23,49,/home/vivek/catkin_ws/src/rosbuzz/src/test.bzz
|
|
||||||
callc |23,49,/home/vivek/catkin_ws/src/rosbuzz/src/test.bzz
|
|
||||||
ret0 |23,50,/home/vivek/catkin_ws/src/rosbuzz/src/test.bzz
|
|
||||||
|
|
||||||
@__label_8
|
|
||||||
ret0 |39,1,/home/vivek/catkin_ws/src/rosbuzz/src/test.bzz
|
|
||||||
|
|
||||||
@__label_9
|
|
||||||
ret0 |43,1,/home/vivek/catkin_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.
|
@ -30,16 +30,15 @@ using namespace std;
|
||||||
|
|
||||||
|
|
||||||
/**
|
/**
|
||||||
* This program implements Buzz node in ros using mavros_msgs, Developed for Dji M100.
|
* This program implements Buzz node in ros using mavros_msgs.
|
||||||
*/
|
*/
|
||||||
|
|
||||||
|
|
||||||
|
|
||||||
static int done = 0;
|
static int done = 0;
|
||||||
static double cur_pos[3];
|
static double cur_pos[3];
|
||||||
static uint64_t payload;
|
static uint64_t payload;
|
||||||
static std::map< int, Pos_struct> neighbours_pos_map;
|
static std::map< int, Pos_struct> neighbours_pos_map;
|
||||||
static int timer_step=0;
|
static int timer_step=0;
|
||||||
|
static int robot_id=0;
|
||||||
|
|
||||||
/*Refresh neighbours Position for every ten step*/
|
/*Refresh neighbours Position for every ten step*/
|
||||||
void maintain_pos(int tim_step){
|
void maintain_pos(int tim_step){
|
||||||
|
@ -188,19 +187,38 @@ static void ctrlc_handler(int sig) {
|
||||||
int main(int argc, char **argv)
|
int main(int argc, char **argv)
|
||||||
{
|
{
|
||||||
|
|
||||||
/*Compile the buzz code .bzz to .bo*/
|
ros::ServiceServer service;
|
||||||
system("rm ../catkin_ws/src/rosbuzz/src/out.basm ../catkin_ws/src/rosbuzz/src/out.bo ../catkin_ws/src/rosbuzz/src/out.bdbg");
|
|
||||||
system("bzzparse ../catkin_ws/src/rosbuzz/src/test.bzz ../catkin_ws/src/rosbuzz/src/out.basm");
|
|
||||||
system("bzzasm ../catkin_ws/src/rosbuzz/src/out.basm ../catkin_ws/src/rosbuzz/src/out.bo ../catkin_ws/src/rosbuzz/src/out.bdbg");
|
|
||||||
|
|
||||||
/*initiate rosBuzz*/
|
/*initiate rosBuzz*/
|
||||||
ros::init(argc, argv, "rosBuzz");
|
ros::init(argc, argv, "rosBuzz");
|
||||||
ROS_INFO("Buzz_node");
|
ROS_INFO("Buzz_node");
|
||||||
|
|
||||||
/*Create node Handler*/
|
/*Create node Handler*/
|
||||||
ros::NodeHandle n_c;
|
ros::NodeHandle n_c;
|
||||||
|
std::string bzzfile_name, fcclient_name, rcservice_name; //, rcclient;
|
||||||
|
bool rcclient;
|
||||||
|
/*Obtain .bzz file name from parameter server*/
|
||||||
|
if(ros::param::get("/rosbuzz_node/bzzfile_name", bzzfile_name));
|
||||||
|
else {ROS_ERROR("Provide a .bzz file to run in Launch file"); system("rosnode kill rosbuzz_node");}
|
||||||
|
|
||||||
|
/*Obtain rc service option from parameter server*/
|
||||||
|
if(ros::param::get("/rosbuzz_node/rcclient", rcclient)){
|
||||||
|
if(rcclient==true){
|
||||||
|
/*Service*/
|
||||||
|
if(ros::param::get("/rosbuzz_node/rcservice_name", rcservice_name)){
|
||||||
|
service = n_c.advertiseService(rcservice_name, rc_callback);
|
||||||
|
ROS_INFO("Ready to receive Mav Commands from RC client");
|
||||||
|
}
|
||||||
|
else{ROS_ERROR("Provide a name topic name for rc service in Launch file"); system("rosnode kill rosbuzz_node");}
|
||||||
|
}
|
||||||
|
else if(rcclient==false){ROS_INFO("RC service is disabled");}
|
||||||
|
}
|
||||||
|
else{ROS_ERROR("Provide a rc client option: yes or no in Launch file"); system("rosnode kill rosbuzz_node");}
|
||||||
|
/*Obtain fc client name from parameter server*/
|
||||||
|
if(ros::param::get("/rosbuzz_node/fcclient_name", fcclient_name));
|
||||||
|
else {ROS_ERROR("Provide a fc client name in Launch file"); system("rosnode kill rosbuzz_node");}
|
||||||
|
/*Obtain robot_id from parameter server*/
|
||||||
|
ros::param::get("/rosbuzz_node/robot_id", robot_id);
|
||||||
|
|
||||||
|
|
||||||
/*subscribers*/
|
/*subscribers*/
|
||||||
ros::Subscriber current_position_sub = n_c.subscribe("current_pos", 1000, current_pos);
|
ros::Subscriber current_position_sub = n_c.subscribe("current_pos", 1000, current_pos);
|
||||||
|
|
||||||
|
@ -213,16 +231,20 @@ int main(int argc, char **argv)
|
||||||
ros::Publisher payload_pub = n_c.advertise<mavros_msgs::Mavlink>("outMavlink", 1000);
|
ros::Publisher payload_pub = n_c.advertise<mavros_msgs::Mavlink>("outMavlink", 1000);
|
||||||
|
|
||||||
/* Clients*/
|
/* Clients*/
|
||||||
ros::ServiceClient mav_client = n_c.serviceClient<mavros_msgs::CommandInt>("djicmd");
|
ros::ServiceClient mav_client = n_c.serviceClient<mavros_msgs::CommandInt>(fcclient_name);
|
||||||
|
cout<< " rc client name"<<rcservice_name;
|
||||||
|
|
||||||
/*Services*/
|
|
||||||
ros::ServiceServer service = n_c.advertiseService("djicmd_rc", rc_callback);
|
|
||||||
ROS_INFO("Ready to receive Mav Commands from dji RC client");
|
|
||||||
|
|
||||||
/*loop rate of ros*/
|
/*loop rate of ros*/
|
||||||
ros::Rate loop_rate(1);
|
ros::Rate loop_rate(1);
|
||||||
|
|
||||||
|
/*Compile the buzz code .bzz to .bo*/
|
||||||
|
stringstream bzzfile_in_compile;
|
||||||
|
bzzfile_in_compile << "bzzparse "<<bzzfile_name<<" ../catkin_ws/src/rosbuzz/src/out.basm";
|
||||||
|
//system("rm ../catkin_ws/src/rosbuzz/src/out.basm ../catkin_ws/src/rosbuzz/src/out.bo ../catkin_ws/src/rosbuzz/src/out.bdbg");
|
||||||
|
system(bzzfile_in_compile.str().c_str());
|
||||||
|
system("bzzasm ../catkin_ws/src/rosbuzz/src/out.basm ../catkin_ws/src/rosbuzz/src/out.bo ../catkin_ws/src/rosbuzz/src/out.bdbg");
|
||||||
|
|
||||||
/* The bytecode filename */
|
/* The bytecode filename */
|
||||||
char* bcfname = (char*)"../catkin_ws/src/rosbuzz/src/out.bo"; //argv[1];
|
char* bcfname = (char*)"../catkin_ws/src/rosbuzz/src/out.bo"; //argv[1];
|
||||||
/* The debugging information file name */
|
/* The debugging information file name */
|
||||||
|
@ -233,12 +255,12 @@ int main(int argc, char **argv)
|
||||||
|
|
||||||
|
|
||||||
/* Set the Buzz bytecode */
|
/* Set the Buzz bytecode */
|
||||||
if(buzz_script_set(bcfname, dbgfname)) {
|
if(buzz_script_set(bcfname, dbgfname,robot_id)) {
|
||||||
fprintf(stdout, "Bytecode file found and set\n");
|
fprintf(stdout, "Bytecode file found and set\n");
|
||||||
|
|
||||||
/*Commands for dji flight controller*/
|
/*Commands for dji flight controller*/
|
||||||
mavros_msgs::CommandInt cmd_srv;
|
mavros_msgs::CommandInt cmd_srv;
|
||||||
|
|
||||||
int count = 0;
|
int count = 0;
|
||||||
while (ros::ok() && !done && !buzz_script_done())
|
while (ros::ok() && !done && !buzz_script_done())
|
||||||
{
|
{
|
||||||
|
@ -255,7 +277,6 @@ int main(int argc, char **argv)
|
||||||
cmd_srv.request.param1 = goto_pos[0];
|
cmd_srv.request.param1 = goto_pos[0];
|
||||||
cmd_srv.request.param2 = goto_pos[1];
|
cmd_srv.request.param2 = goto_pos[1];
|
||||||
cmd_srv.request.param3 = goto_pos[2];
|
cmd_srv.request.param3 = goto_pos[2];
|
||||||
|
|
||||||
cmd_srv.request.command = getcmd();
|
cmd_srv.request.command = getcmd();
|
||||||
/* diji client call*/
|
/* diji 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); }
|
||||||
|
|
13
src/test.bzz
13
src/test.bzz
|
@ -1,6 +1,6 @@
|
||||||
# Executed once at init time.
|
# Executed once at init time.
|
||||||
function init() {
|
function init() {
|
||||||
i=0
|
i=1
|
||||||
s = swarm.create(10)
|
s = swarm.create(10)
|
||||||
s.join()
|
s.join()
|
||||||
|
|
||||||
|
@ -8,6 +8,7 @@ s.join()
|
||||||
|
|
||||||
# Executed at each time step.
|
# Executed at each time step.
|
||||||
function step() {
|
function step() {
|
||||||
|
|
||||||
# print all the neighbours position
|
# print all the neighbours position
|
||||||
neighbors.foreach(
|
neighbors.foreach(
|
||||||
function(rid, data) {
|
function(rid, data) {
|
||||||
|
@ -17,20 +18,10 @@ neighbors.foreach(
|
||||||
"elevation = ", data.elevation) })
|
"elevation = ", data.elevation) })
|
||||||
if(i==1){
|
if(i==1){
|
||||||
s.exec(function() {uav_takeoff()})
|
s.exec(function() {uav_takeoff()})
|
||||||
i=0
|
|
||||||
}
|
}
|
||||||
else{
|
else{
|
||||||
s.exec(function() {uav_goto(1.1234,2.2345,3.3456)})
|
s.exec(function() {uav_goto(1.1234,2.2345,3.3456)})
|
||||||
i=1
|
|
||||||
}
|
}
|
||||||
#if(i==5){
|
|
||||||
#s.leave()
|
|
||||||
#i=0
|
|
||||||
#}
|
|
||||||
#else{
|
|
||||||
#i=1
|
|
||||||
#}
|
|
||||||
#i = i + 1
|
|
||||||
|
|
||||||
}
|
}
|
||||||
|
|
||||||
|
|
Loading…
Reference in New Issue