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

flight test fixes

This commit is contained in:
David St-Onge 2016-12-06 12:02:39 -05:00
parent ce120426b3
commit 36b496d6d8
8 changed files with 217 additions and 33 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

12
launch/rosbuzzm100.launch
View File

@ -2,21 +2,13 @@
<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" >
<<<<<<< HEAD
<param name="bzzfile_name" value="../ROS_WS/src/ROSBuzz/src/test1.bzz" /> <param name="bzzfile_name" value="../ROS_WS/src/ROSBuzz/src/test1.bzz" />
=======
<param name="bzzfile_name" value="/home/vivek/catkin_ws/src/rosbuzz/src/test.bzz" />
>>>>>>> 9e9eb964b10f7fa49342b6eb629fc7a30e47c355
<param name="rcclient" value="true" /> <param name="rcclient" value="true" />
<param name="rcservice_name" value="rc_cmd" /> <param name="rcservice_name" value="/buzzcmd" />
<param name="fcclient_name" value="/djicmd" /> <param name="fcclient_name" value="/dji_mavcmd" />
<param name="in_payload" value="/inMavlink"/> <param name="in_payload" value="/inMavlink"/>
<param name="out_payload" value="/outMavlink"/> <param name="out_payload" value="/outMavlink"/>
<<<<<<< HEAD
<param name="robot_id" value="1"/> <param name="robot_id" value="1"/>
=======
<param name="robot_id" value="2"/>
>>>>>>> 9e9eb964b10f7fa49342b6eb629fc7a30e47c355
</node> </node>

23
launch/rosbuzzm100.launch.save Normal file
View File

@ -0,0 +1,23 @@
<node name="rosbuzz_node" pkg="rosbuzz" type="rosbuzz_node" respawn="false" output="screen" >
<param name="bzzfile_name" value="../ROS_WS/src/ROSBuzz/src/test1.bzz" />
<param name="rcclient" value="true" />
<param name="rcservice_name" value="/buzzcmd" />
<param name="fcclient_name" value="/dji_mavcmd" />
<param name="in_payload" value="/inMavlink"/>
<param name="out_payload" value="/outMavlink"/>
<param name="robot_id" value="1"/>
</node><!-- 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/test1.bzz" />
<param name="rcclient" value="true" />
<param name="rcservice_name" value="/buzzcmd" />
<param name="fcclient_name" value="/dji_mavcmd" />
<param name="in_payload" value="/inMavlink"/>
<param name="out_payload" value="/outMavlink"/>
<param name="robot_id" value="1"/>
</node>
</launch>

2
src/buzz_utility.cpp
View File

@ -318,7 +318,7 @@ void buzz_script_step() {
buzzvm_dump(VM); buzzvm_dump(VM);
} }
/* Print swarm */ /* Print swarm */
buzzswarm_members_print(stdout, VM->swarmmembers, VM->robot); //buzzswarm_members_print(stdout, VM->swarmmembers, VM->robot);
/* Check swarm state */ /* Check swarm state */
/* int status = 1; /* int status = 1;
buzzdict_foreach(VM->swarmmembers, check_swarm_members, &status); buzzdict_foreach(VM->swarmmembers, check_swarm_members, &status);

4
src/roscontroller.cpp
View File

@ -258,9 +258,9 @@ namespace rosbzz_node{
bool roscontroller::rc_callback(mavros_msgs::CommandInt::Request &req, bool roscontroller::rc_callback(mavros_msgs::CommandInt::Request &req,
mavros_msgs::CommandInt::Response &res){ mavros_msgs::CommandInt::Response &res){
int rc_cmd; int rc_cmd;
if(req.command==oldcmdID) /* if(req.command==oldcmdID)
return true; return true;
else oldcmdID=req.command; else oldcmdID=req.command;*/
switch(req.command){ switch(req.command){
case mavros_msgs::CommandCode::NAV_TAKEOFF: case mavros_msgs::CommandCode::NAV_TAKEOFF:
ROS_INFO("RC_call: TAKE OFF!!!!"); ROS_INFO("RC_call: TAKE OFF!!!!");

170
src/test1.basm Normal file
View File

@ -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

BIN
src/test1.bdbg Normal file
View File

Binary file not shown.

BIN
src/test1.bo Normal file
View File

Binary file not shown.

35
src/test1.bzz
View File

@ -3,34 +3,33 @@ function init() {
i = 0 i = 0
a = 0 a = 0
val = 0 val = 0
oldcmd = 0
} }
# Executed at each time step. # Executed at each time step.
function step() { function step() {
if (i == 0) { neighbors.listen("cmd",
neighbors.listen("Take",
function(vid, value, rid) { function(vid, value, rid) {
print("Got (", vid, ",", value, ") from robot #", rid) print("Got (", vid, ",", value, ") from robot #", rid)
} if(value==22) {
)
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() uav_takeoff()
a=1 } 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
} }
} }