From 36b496d6d8eaea2c23df759854d7d1e579237bd7 Mon Sep 17 00:00:00 2001 From: David St-Onge Date: Tue, 6 Dec 2016 12:02:39 -0500 Subject: [PATCH] flight test fixes --- launch/rosbuzzm100.launch | 12 +-- launch/rosbuzzm100.launch.save | 23 +++++ src/buzz_utility.cpp | 2 +- src/roscontroller.cpp | 4 +- src/test1.basm | 170 +++++++++++++++++++++++++++++++++ src/test1.bdbg | Bin 0 -> 7571 bytes src/test1.bo | Bin 0 -> 559 bytes src/test1.bzz | 39 ++++---- 8 files changed, 217 insertions(+), 33 deletions(-) create mode 100644 launch/rosbuzzm100.launch.save create mode 100644 src/test1.basm create mode 100644 src/test1.bdbg create mode 100644 src/test1.bo diff --git a/launch/rosbuzzm100.launch b/launch/rosbuzzm100.launch index 134631c..a5b5f4a 100644 --- a/launch/rosbuzzm100.launch +++ b/launch/rosbuzzm100.launch @@ -2,21 +2,13 @@ -<<<<<<< HEAD -======= - ->>>>>>> 9e9eb964b10f7fa49342b6eb629fc7a30e47c355 - - + + -<<<<<<< HEAD -======= - ->>>>>>> 9e9eb964b10f7fa49342b6eb629fc7a30e47c355 diff --git a/launch/rosbuzzm100.launch.save b/launch/rosbuzzm100.launch.save new file mode 100644 index 0000000..8ec7f8d --- /dev/null +++ b/launch/rosbuzzm100.launch.save @@ -0,0 +1,23 @@ + + + + + + + + + + + + + + + + + + + + + + + diff --git a/src/buzz_utility.cpp b/src/buzz_utility.cpp index 57220d9..9b906bb 100644 --- a/src/buzz_utility.cpp +++ b/src/buzz_utility.cpp @@ -318,7 +318,7 @@ void buzz_script_step() { buzzvm_dump(VM); } /* Print swarm */ - buzzswarm_members_print(stdout, VM->swarmmembers, VM->robot); + //buzzswarm_members_print(stdout, VM->swarmmembers, VM->robot); /* Check swarm state */ /* int status = 1; buzzdict_foreach(VM->swarmmembers, check_swarm_members, &status); diff --git a/src/roscontroller.cpp b/src/roscontroller.cpp index 015fed1..b911019 100644 --- a/src/roscontroller.cpp +++ b/src/roscontroller.cpp @@ -258,9 +258,9 @@ namespace rosbzz_node{ bool roscontroller::rc_callback(mavros_msgs::CommandInt::Request &req, mavros_msgs::CommandInt::Response &res){ int rc_cmd; - if(req.command==oldcmdID) +/* if(req.command==oldcmdID) return true; - else oldcmdID=req.command; + else oldcmdID=req.command;*/ switch(req.command){ case mavros_msgs::CommandCode::NAV_TAKEOFF: ROS_INFO("RC_call: TAKE OFF!!!!"); diff --git a/src/test1.basm b/src/test1.basm new file mode 100644 index 0000000..d9f876e --- /dev/null +++ b/src/test1.basm @@ -0,0 +1,170 @@ +!20 +'init +'i +'a +'val +'oldcmd +'step +'neighbors +'listen +'cmd +'print +'Got ( +', +') from robot # +'uav_takeoff +'uav_land +'flight +'rc_cmd +'broadcast +'reset +'destroy + + pushs 0 + pushcn @__label_1 + gstore + pushs 5 + pushcn @__label_2 + gstore + pushs 18 + pushcn @__label_14 + gstore + pushs 19 + pushcn @__label_15 + gstore + nop + +@__label_0 + +@__exitpoint + done + +@__label_1 + pushs 1 |3,2,/home/ubuntu/ROS_WS/src/ROSBuzz/src/test1.bzz + pushi 0 |3,4,/home/ubuntu/ROS_WS/src/ROSBuzz/src/test1.bzz + gstore |3,5,/home/ubuntu/ROS_WS/src/ROSBuzz/src/test1.bzz + pushs 2 |4,2,/home/ubuntu/ROS_WS/src/ROSBuzz/src/test1.bzz + pushi 0 |4,4,/home/ubuntu/ROS_WS/src/ROSBuzz/src/test1.bzz + gstore |4,5,/home/ubuntu/ROS_WS/src/ROSBuzz/src/test1.bzz + pushs 3 |5,4,/home/ubuntu/ROS_WS/src/ROSBuzz/src/test1.bzz + pushi 0 |5,6,/home/ubuntu/ROS_WS/src/ROSBuzz/src/test1.bzz + gstore |5,7,/home/ubuntu/ROS_WS/src/ROSBuzz/src/test1.bzz + pushs 4 |6,7,/home/ubuntu/ROS_WS/src/ROSBuzz/src/test1.bzz + pushi 0 |6,9,/home/ubuntu/ROS_WS/src/ROSBuzz/src/test1.bzz + gstore |6,10,/home/ubuntu/ROS_WS/src/ROSBuzz/src/test1.bzz + ret0 |7,1,/home/ubuntu/ROS_WS/src/ROSBuzz/src/test1.bzz + +@__label_2 + pushs 6 |12,9,/home/ubuntu/ROS_WS/src/ROSBuzz/src/test1.bzz + gload |12,9,/home/ubuntu/ROS_WS/src/ROSBuzz/src/test1.bzz + pushs 7 |12,10,/home/ubuntu/ROS_WS/src/ROSBuzz/src/test1.bzz + tget |12,16,/home/ubuntu/ROS_WS/src/ROSBuzz/src/test1.bzz + pushs 8 |12,17,/home/ubuntu/ROS_WS/src/ROSBuzz/src/test1.bzz + pushl @__label_3 |13,3,/home/ubuntu/ROS_WS/src/ROSBuzz/src/test1.bzz + pushi 2 |21,1,/home/ubuntu/ROS_WS/src/ROSBuzz/src/test1.bzz + callc |21,1,/home/ubuntu/ROS_WS/src/ROSBuzz/src/test1.bzz + pushs 15 |23,9,/home/ubuntu/ROS_WS/src/ROSBuzz/src/test1.bzz + gload |23,9,/home/ubuntu/ROS_WS/src/ROSBuzz/src/test1.bzz + pushs 16 |23,10,/home/ubuntu/ROS_WS/src/ROSBuzz/src/test1.bzz + tget |23,16,/home/ubuntu/ROS_WS/src/ROSBuzz/src/test1.bzz + pushs 4 |23,24,/home/ubuntu/ROS_WS/src/ROSBuzz/src/test1.bzz + gload |23,24,/home/ubuntu/ROS_WS/src/ROSBuzz/src/test1.bzz + neq |23,24,/home/ubuntu/ROS_WS/src/ROSBuzz/src/test1.bzz + jumpz @__label_8 |23,26,/home/ubuntu/ROS_WS/src/ROSBuzz/src/test1.bzz + pushs 9 |24,6,/home/ubuntu/ROS_WS/src/ROSBuzz/src/test1.bzz + gload |24,6,/home/ubuntu/ROS_WS/src/ROSBuzz/src/test1.bzz + pushs 15 |24,13,/home/ubuntu/ROS_WS/src/ROSBuzz/src/test1.bzz + gload |24,13,/home/ubuntu/ROS_WS/src/ROSBuzz/src/test1.bzz + pushs 16 |24,14,/home/ubuntu/ROS_WS/src/ROSBuzz/src/test1.bzz + tget |24,20,/home/ubuntu/ROS_WS/src/ROSBuzz/src/test1.bzz + pushi 1 |24,21,/home/ubuntu/ROS_WS/src/ROSBuzz/src/test1.bzz + callc |24,21,/home/ubuntu/ROS_WS/src/ROSBuzz/src/test1.bzz + pushs 15 |25,10,/home/ubuntu/ROS_WS/src/ROSBuzz/src/test1.bzz + gload |25,10,/home/ubuntu/ROS_WS/src/ROSBuzz/src/test1.bzz + pushs 16 |25,11,/home/ubuntu/ROS_WS/src/ROSBuzz/src/test1.bzz + tget |25,17,/home/ubuntu/ROS_WS/src/ROSBuzz/src/test1.bzz + pushi 22 |25,19,/home/ubuntu/ROS_WS/src/ROSBuzz/src/test1.bzz + eq |25,21,/home/ubuntu/ROS_WS/src/ROSBuzz/src/test1.bzz + jumpz @__label_10 |25,23,/home/ubuntu/ROS_WS/src/ROSBuzz/src/test1.bzz + pushs 13 |26,13,/home/ubuntu/ROS_WS/src/ROSBuzz/src/test1.bzz + gload |26,13,/home/ubuntu/ROS_WS/src/ROSBuzz/src/test1.bzz + pushi 0 |26,17,/home/ubuntu/ROS_WS/src/ROSBuzz/src/test1.bzz + callc |26,17,/home/ubuntu/ROS_WS/src/ROSBuzz/src/test1.bzz + pushs 6 |27,11,/home/ubuntu/ROS_WS/src/ROSBuzz/src/test1.bzz + gload |27,11,/home/ubuntu/ROS_WS/src/ROSBuzz/src/test1.bzz + pushs 17 |27,12,/home/ubuntu/ROS_WS/src/ROSBuzz/src/test1.bzz + tget |27,21,/home/ubuntu/ROS_WS/src/ROSBuzz/src/test1.bzz + pushs 8 |27,22,/home/ubuntu/ROS_WS/src/ROSBuzz/src/test1.bzz + pushi 22 |27,29,/home/ubuntu/ROS_WS/src/ROSBuzz/src/test1.bzz + pushi 2 |27,32,/home/ubuntu/ROS_WS/src/ROSBuzz/src/test1.bzz + callc |27,32,/home/ubuntu/ROS_WS/src/ROSBuzz/src/test1.bzz + jump @__label_11 |28,8,/home/ubuntu/ROS_WS/src/ROSBuzz/src/test1.bzz +@__label_10 |28,8,/home/ubuntu/ROS_WS/src/ROSBuzz/src/test1.bzz + pushs 15 |28,17,/home/ubuntu/ROS_WS/src/ROSBuzz/src/test1.bzz + gload |28,17,/home/ubuntu/ROS_WS/src/ROSBuzz/src/test1.bzz + pushs 16 |28,18,/home/ubuntu/ROS_WS/src/ROSBuzz/src/test1.bzz + tget |28,24,/home/ubuntu/ROS_WS/src/ROSBuzz/src/test1.bzz + pushi 21 |28,26,/home/ubuntu/ROS_WS/src/ROSBuzz/src/test1.bzz + eq |28,28,/home/ubuntu/ROS_WS/src/ROSBuzz/src/test1.bzz + jumpz @__label_12 |28,30,/home/ubuntu/ROS_WS/src/ROSBuzz/src/test1.bzz + pushs 14 |29,10,/home/ubuntu/ROS_WS/src/ROSBuzz/src/test1.bzz + gload |29,10,/home/ubuntu/ROS_WS/src/ROSBuzz/src/test1.bzz + pushi 0 |29,12,/home/ubuntu/ROS_WS/src/ROSBuzz/src/test1.bzz + callc |29,12,/home/ubuntu/ROS_WS/src/ROSBuzz/src/test1.bzz + pushs 6 |30,11,/home/ubuntu/ROS_WS/src/ROSBuzz/src/test1.bzz + gload |30,11,/home/ubuntu/ROS_WS/src/ROSBuzz/src/test1.bzz + pushs 17 |30,12,/home/ubuntu/ROS_WS/src/ROSBuzz/src/test1.bzz + tget |30,21,/home/ubuntu/ROS_WS/src/ROSBuzz/src/test1.bzz + pushs 8 |30,22,/home/ubuntu/ROS_WS/src/ROSBuzz/src/test1.bzz + pushi 21 |30,29,/home/ubuntu/ROS_WS/src/ROSBuzz/src/test1.bzz + pushi 2 |30,32,/home/ubuntu/ROS_WS/src/ROSBuzz/src/test1.bzz + callc |30,32,/home/ubuntu/ROS_WS/src/ROSBuzz/src/test1.bzz +@__label_12 |32,1,/home/ubuntu/ROS_WS/src/ROSBuzz/src/test1.bzz +@__label_11 |32,1,/home/ubuntu/ROS_WS/src/ROSBuzz/src/test1.bzz + pushs 4 |32,7,/home/ubuntu/ROS_WS/src/ROSBuzz/src/test1.bzz + pushs 15 |32,14,/home/ubuntu/ROS_WS/src/ROSBuzz/src/test1.bzz + gload |32,14,/home/ubuntu/ROS_WS/src/ROSBuzz/src/test1.bzz + pushs 16 |32,15,/home/ubuntu/ROS_WS/src/ROSBuzz/src/test1.bzz + tget |32,21,/home/ubuntu/ROS_WS/src/ROSBuzz/src/test1.bzz + gstore |32,21,/home/ubuntu/ROS_WS/src/ROSBuzz/src/test1.bzz +@__label_8 |35,0,/home/ubuntu/ROS_WS/src/ROSBuzz/src/test1.bzz + ret0 |35,1,/home/ubuntu/ROS_WS/src/ROSBuzz/src/test1.bzz + +@__label_3 + pushs 9 |14,11,/home/ubuntu/ROS_WS/src/ROSBuzz/src/test1.bzz + gload |14,11,/home/ubuntu/ROS_WS/src/ROSBuzz/src/test1.bzz + pushs 10 |14,12,/home/ubuntu/ROS_WS/src/ROSBuzz/src/test1.bzz + lload 1 |14,24,/home/ubuntu/ROS_WS/src/ROSBuzz/src/test1.bzz + pushs 11 |14,26,/home/ubuntu/ROS_WS/src/ROSBuzz/src/test1.bzz + lload 2 |14,36,/home/ubuntu/ROS_WS/src/ROSBuzz/src/test1.bzz + pushs 12 |14,38,/home/ubuntu/ROS_WS/src/ROSBuzz/src/test1.bzz + lload 3 |14,59,/home/ubuntu/ROS_WS/src/ROSBuzz/src/test1.bzz + pushi 6 |14,60,/home/ubuntu/ROS_WS/src/ROSBuzz/src/test1.bzz + callc |14,60,/home/ubuntu/ROS_WS/src/ROSBuzz/src/test1.bzz + lload 2 |15,9,/home/ubuntu/ROS_WS/src/ROSBuzz/src/test1.bzz + pushi 22 |15,11,/home/ubuntu/ROS_WS/src/ROSBuzz/src/test1.bzz + eq |15,13,/home/ubuntu/ROS_WS/src/ROSBuzz/src/test1.bzz + jumpz @__label_4 |15,15,/home/ubuntu/ROS_WS/src/ROSBuzz/src/test1.bzz + pushs 13 |16,13,/home/ubuntu/ROS_WS/src/ROSBuzz/src/test1.bzz + gload |16,13,/home/ubuntu/ROS_WS/src/ROSBuzz/src/test1.bzz + pushi 0 |16,15,/home/ubuntu/ROS_WS/src/ROSBuzz/src/test1.bzz + callc |16,15,/home/ubuntu/ROS_WS/src/ROSBuzz/src/test1.bzz + jump @__label_5 |17,8,/home/ubuntu/ROS_WS/src/ROSBuzz/src/test1.bzz +@__label_4 |17,8,/home/ubuntu/ROS_WS/src/ROSBuzz/src/test1.bzz + lload 2 |17,16,/home/ubuntu/ROS_WS/src/ROSBuzz/src/test1.bzz + pushi 21 |17,18,/home/ubuntu/ROS_WS/src/ROSBuzz/src/test1.bzz + eq |17,20,/home/ubuntu/ROS_WS/src/ROSBuzz/src/test1.bzz + jumpz @__label_6 |17,22,/home/ubuntu/ROS_WS/src/ROSBuzz/src/test1.bzz + pushs 14 |18,10,/home/ubuntu/ROS_WS/src/ROSBuzz/src/test1.bzz + gload |18,10,/home/ubuntu/ROS_WS/src/ROSBuzz/src/test1.bzz + pushi 0 |18,12,/home/ubuntu/ROS_WS/src/ROSBuzz/src/test1.bzz + callc |18,12,/home/ubuntu/ROS_WS/src/ROSBuzz/src/test1.bzz +@__label_6 |20,3,/home/ubuntu/ROS_WS/src/ROSBuzz/src/test1.bzz +@__label_5 |20,3,/home/ubuntu/ROS_WS/src/ROSBuzz/src/test1.bzz + ret0 |20,4,/home/ubuntu/ROS_WS/src/ROSBuzz/src/test1.bzz + +@__label_14 + ret0 |39,1,/home/ubuntu/ROS_WS/src/ROSBuzz/src/test1.bzz + +@__label_15 + ret0 |43,1,/home/ubuntu/ROS_WS/src/ROSBuzz/src/test1.bzz diff --git a/src/test1.bdbg b/src/test1.bdbg new file mode 100644 index 0000000000000000000000000000000000000000..8a6176d00a0af3c12200fba12e222a19a4a788fd GIT binary patch literal 7571 zcmbtZJ#5Tz7(VoT-1)ecD*cd)Qlxqp35yyqSV%-W)KZc*O}exacb9Mj3yXz?g@wgp zVPHVwdr$)d5|N0HL6>&vQiFP3e^2tP@ALkbyXAfU&&T_F|L_0q+LBDBE3|5 z^7xsFv2tysHd(Ee2aXS(9vUoH&W`^7dbn1vH@;WLD%Cxuk$Qd3Al9YQyarJ+h)t=q zRfCwavZhp;)ga2oVsk3ZX%O$M40i_`r=8d1iOf?XYcwG@3}Q5N#TZT-?sPWG9}j&cT;A7C0L# zH6h3OQnR?Wt6pT0gBE(@Jcudz45qv2hA1P$C$^PKcv%FMh|p zI7TecVoo8`#6oPX`|(6tDUlW})Cb0*m`c&;PA^Wx?VKSy#nu`y79B~5xT3U^of{IA z(~DUWBDU5eN@T6ZqJxBpPqU%8opVMW18aXY&S0&~MD%{GMr$Jy&VE=U5xr-pM&O0IAP$Edi331g}1QYoO4`zk& literal 0 HcmV?d00001 diff --git a/src/test1.bo b/src/test1.bo new file mode 100644 index 0000000000000000000000000000000000000000..9ca6f3ebd80c74b5a4e14f142470c63eacf0a8ec GIT binary patch literal 559 zcmaJ-TTa6;5ZnUh{b|J#+7XtFRF5)%J;RXxumgrqd z#2B9SdJ*-LaYZ4x)Kv~eNn;}TY{C7hcRJ5>Ai@L_AvYm<_W}s?UA<8mqGI?du+Q*z zgV|GtcMPF}9l9iRh$rS7ZvMipAE&!9hhCl&XE zZ>b5d{Nqn9i0RD&+mF~Tfkw@`9x?Sg@zv|5uWW8lJ(n3mAOAOleNAMJ*`6^}P1=1D nCL9wF81WC{AtQMgeua^cP4F%m+QFxDB5FgOitpG;rIh*ten~@D literal 0 HcmV?d00001 diff --git a/src/test1.bzz b/src/test1.bzz index 476c852..a8ffe0b 100644 --- a/src/test1.bzz +++ b/src/test1.bzz @@ -3,34 +3,33 @@ function init() { i = 0 a = 0 val = 0 +oldcmd = 0 } # Executed at each time step. function step() { -if (i == 0) { -neighbors.listen("Take", +neighbors.listen("cmd", 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) - val = value - } -) -print(val) -if ((val==23) and (a == 0)) { - uav_takeoff() - a=1 + if(value==22) { + uav_takeoff() + } else if(value==21) { + uav_land() } - if (a == 10) uav_land() - if (a != 0) a = a+1 -} -else{ -neighbors.broadcast("key", 23) -neighbors.broadcast("Take", "no") + } +) + +if(flight.rc_cmd!=oldcmd) { + print(flight.rc_cmd) + if(flight.rc_cmd==22) { + uav_takeoff() + neighbors.broadcast("cmd", 22) + } else if(flight.rc_cmd==21) { + uav_land() + neighbors.broadcast("cmd", 21) + } + oldcmd=flight.rc_cmd } }