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 0000000..8a6176d Binary files /dev/null and b/src/test1.bdbg differ diff --git a/src/test1.bo b/src/test1.bo new file mode 100644 index 0000000..9ca6f3e Binary files /dev/null and b/src/test1.bo differ 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 } }