diff --git a/launch/rosbuzz.launch b/launch/rosbuzz.launch index d79ce4d..df715b1 100644 --- a/launch/rosbuzz.launch +++ b/launch/rosbuzz.launch @@ -2,10 +2,10 @@ - + - + diff --git a/src/out.basm b/src/out.basm new file mode 100644 index 0000000..267387d --- /dev/null +++ b/src/out.basm @@ -0,0 +1,49 @@ +!8 +'init +'step +'print +'bzz print: voltage: +'battery +'voltage +'reset +'destroy + + pushs 0 + pushcn @__label_1 + gstore + pushs 1 + pushcn @__label_2 + gstore + pushs 6 + pushcn @__label_3 + gstore + pushs 7 + pushcn @__label_4 + gstore + nop + +@__label_0 + +@__exitpoint + done + +@__label_1 + ret0 |4,1,/home/ubuntu/ROS_WS/src/ROSBuzz/src/test.bzz + +@__label_2 + pushs 2 |9,5,/home/ubuntu/ROS_WS/src/ROSBuzz/src/test.bzz + gload |9,5,/home/ubuntu/ROS_WS/src/ROSBuzz/src/test.bzz + pushs 3 |9,6,/home/ubuntu/ROS_WS/src/ROSBuzz/src/test.bzz + pushs 4 |9,37,/home/ubuntu/ROS_WS/src/ROSBuzz/src/test.bzz + gload |9,37,/home/ubuntu/ROS_WS/src/ROSBuzz/src/test.bzz + pushs 5 |9,38,/home/ubuntu/ROS_WS/src/ROSBuzz/src/test.bzz + tget |9,45,/home/ubuntu/ROS_WS/src/ROSBuzz/src/test.bzz + pushi 2 |9,46,/home/ubuntu/ROS_WS/src/ROSBuzz/src/test.bzz + callc |9,46,/home/ubuntu/ROS_WS/src/ROSBuzz/src/test.bzz + ret0 |11,1,/home/ubuntu/ROS_WS/src/ROSBuzz/src/test.bzz + +@__label_3 + ret0 |15,1,/home/ubuntu/ROS_WS/src/ROSBuzz/src/test.bzz + +@__label_4 + ret0 |19,1,/home/ubuntu/ROS_WS/src/ROSBuzz/src/test.bzz diff --git a/src/out.bdbg b/src/out.bdbg new file mode 100644 index 0000000..8e25df4 Binary files /dev/null and b/src/out.bdbg differ diff --git a/src/out.bo b/src/out.bo new file mode 100644 index 0000000..862cdf5 Binary files /dev/null and b/src/out.bo differ diff --git a/src/roscontroller.cpp b/src/roscontroller.cpp index 597c632..8f9554f 100644 --- a/src/roscontroller.cpp +++ b/src/roscontroller.cpp @@ -79,8 +79,8 @@ namespace rosbzz_node{ void roscontroller::Initialize_pub_sub(){ /*subscribers*/ - current_position_sub = n_c.subscribe("current_pos", 1000, &roscontroller::current_pos,this); - battery_sub = n_c.subscribe("battery_state", 1000, &roscontroller::battery,this); + current_position_sub = n_c.subscribe("/dji_sdk/global_position", 1000, &roscontroller::current_pos,this); + battery_sub = n_c.subscribe("/dji_sdk/power_status", 1000, &roscontroller::battery,this); payload_sub = n_c.subscribe("inMavlink", 1000, &roscontroller::payload_obt,this); /*publishers*/ payload_pub = n_c.advertise("outMavlink", 1000); diff --git a/src/test.bzz b/src/test.bzz index c24846e..0c6a535 100644 --- a/src/test.bzz +++ b/src/test.bzz @@ -1,27 +1,12 @@ # Executed once at init time. function init() { -i=1 -s = swarm.create(10) -s.join() } # Executed at each time step. function step() { -# print all the neighbours position -neighbors.foreach( - function(rid, data) { - print("robot ", rid, ": ", - "distance = ", data.distance, ", ", - "azimuth = ", data.azimuth, ", ", - "elevation = ", data.elevation) }) -if(i==1){ -s.exec(function() {uav_gohome()}) -} -else{ -s.exec(function() {uav_goto(1.1234,2.2345,3.3456)}) -} +print("bzz print: voltage: ", battery.voltage) }