From 60aa13a4adeaaa6d457e37915dbba74990b915a9 Mon Sep 17 00:00:00 2001
From: vivek-shankar <robovivek@gmail.com>
Date: Fri, 23 Dec 2016 14:21:52 -0500
Subject: [PATCH] topic change for buzz objects

---
 launch/rosbuzz_2_parallel.launch | 25 ++++-----
 src/roscontroller.cpp            |  6 +--
 src/test1.bzz                    | 90 ++++++++++++++++++++++++++++++--
 3 files changed, 101 insertions(+), 20 deletions(-)

diff --git a/launch/rosbuzz_2_parallel.launch b/launch/rosbuzz_2_parallel.launch
index 9f9aaa0..d3e1c0e 100644
--- a/launch/rosbuzz_2_parallel.launch
+++ b/launch/rosbuzz_2_parallel.launch
@@ -2,22 +2,23 @@
 
 <launch>
 		<node name="rosbuzz_node" pkg="rosbuzz" type="rosbuzz_node" respawn="false" output="screen" >	
-		<param name="bzzfile_name" value="/home/vivek/catkin_ws/src/rosbuzz/src/test.bzz" />
-                <param name="rcclient" value="true" /> 
-		<param name="rcservice_name" value="rc_cmd" />
-		<param name="fcclient_name" value="/djicmd" />
-		<param name="in_payload" value="/rosbuzz_node1/outMavlink"/>
-		<param name="out_payload" value="outMavlink"/>
-		<param name="robot_id" value="1"/>
-		</node>
-		<node name="rosbuzz_node1" pkg="rosbuzz" type="rosbuzz_node" respawn="false" output="screen" >	
 		<param name="bzzfile_name" value="/home/vivek/catkin_ws/src/rosbuzz/src/test1.bzz" />
                 <param name="rcclient" value="true" /> 
 		<param name="rcservice_name" value="rc_cmd" />
 		<param name="fcclient_name" value="/djicmd" />
-		<param name="in_payload" value="/rosbuzz_node/outMavlink"/>
-		<param name="out_payload" value="outMavlink"/>
-		<param name="robot_id" value="2"/>
+		<param name="in_payload" value="/inMavlink"/>
+		<!-- param name="in_payload" value="/rosbuzz_node1/outMavlink"-->
+		<param name="out_payload" value="/outMavlink"/>
+		<param name="robot_id" value="1"/>
 		</node>
+		<!-- node name="rosbuzz_node1" pkg="rosbuzz" type="rosbuzz_node" respawn="false" output="screen" -->	
+		<!-- param name="bzzfile_name" value="/home/vivek/catkin_ws/src/rosbuzz/src/test1.bzz" -->
+                <!-- param name="rcclient" value="true" --> 
+		<!-- param name="rcservice_name" value="rc_cmd" -->
+		<!-- param name="fcclient_name" value="/djicmd" -->
+		<!-- param name="in_payload" value="/rosbuzz_node/outMavlink"-->
+		<!-- param name="out_payload" value="outMavlink"-->
+		<!-- param name="robot_id" value="2"-->
+		<!-- node-->
 		
 </launch>
diff --git a/src/roscontroller.cpp b/src/roscontroller.cpp
index 97fb3ee..0ed7bc0 100644
--- a/src/roscontroller.cpp
+++ b/src/roscontroller.cpp
@@ -86,10 +86,10 @@ namespace rosbzz_node{
 
 	void roscontroller::Initialize_pub_sub(ros::NodeHandle n_c){
 		/*subscribers*/
-  		current_position_sub = n_c.subscribe("/mav/global_position", 1000, &roscontroller::current_pos,this);
-  		battery_sub = n_c.subscribe("/mav/power_status", 1000, &roscontroller::battery,this);
+  		current_position_sub = n_c.subscribe("/global_position", 1000, &roscontroller::current_pos,this);
+  		battery_sub = n_c.subscribe("/power_status", 1000, &roscontroller::battery,this);
   		payload_sub = n_c.subscribe(in_payload, 1000, &roscontroller::payload_obt,this);
-		flight_status_sub =n_c.subscribe("/mav/flight_status",100, &roscontroller::flight_status_update,this);
+		flight_status_sub =n_c.subscribe("/flight_status",100, &roscontroller::flight_status_update,this);
   		/*publishers*/
 		payload_pub = n_c.advertise<mavros_msgs::Mavlink>(out_payload, 1000);
 		cout<<out_payload<<endl;
diff --git a/src/test1.bzz b/src/test1.bzz
index a8ffe0b..5cdee45 100644
--- a/src/test1.bzz
+++ b/src/test1.bzz
@@ -1,9 +1,86 @@
+
+########################################
+#
+# BARRIER-RELATED FUNCTIONS
+#
+########################################
+
+#
+# Constants
+#
+BARRIER_VSTIG = 1
+ROBOTS = 2 # number of robots in the swarm
+
+#
+# Sets a barrier
+#
+function barrier_set(threshold, transf) {
+  statef = function() {
+    barrier_wait(threshold, transf);
+  }
+  barrier = stigmergy.create(BARRIER_VSTIG)
+  statestr = "barrier"
+}
+
+#
+# Make yourself ready
+#
+function barrier_ready() {
+  barrier.put(id, 1)
+}
+
+#
+# Executes the barrier
+#
+function barrier_wait(threshold, transf) {
+  barrier.get(id)
+  extradbg = barrier.size()
+  if(barrier.size() >= threshold) {
+    barrier = nil
+    transf()
+  }
+}
+
+# flight status
+
+function idle() {
+}
+
+function takeoff() {
+	if( flight.status == 2) {
+		barrier_set(ROBOTS,transition_to_land)
+		barrier_ready()
+	}
+	else
+		uav_takeoff()
+}
+function land() {
+	if( flight.status == 1) {
+		barrier_set(ROBOTS,idle)
+		barrier_ready()
+	}
+	else
+		uav_land()
+}
+
+
+function transition_to_land() {
+	if(tim>=100){
+		statef=land
+		tim=0
+	}else{
+		tim=tim+1
+	}
+}
+
 # Executed once at init time.
 function init() {
 i = 0
 a = 0
 val = 0
 oldcmd = 0
+tim=0
+statef=idle
 }
  
 # Executed at each time step.
@@ -13,25 +90,28 @@ neighbors.listen("cmd",
    function(vid, value, rid) {
       print("Got (", vid, ",", value, ") from robot #", rid)
 	if(value==22) {
-		uav_takeoff()
+		statef=takeoff
 	} else if(value==21) {
-		uav_land()
+		statef=land
 	}
    }
 )
 
+
+print("Flight status: ",flight.status)
 if(flight.rc_cmd!=oldcmd) {
-	print(flight.rc_cmd)
+	
 	if(flight.rc_cmd==22) {
-		uav_takeoff()		
+		statef=takeoff		
 		neighbors.broadcast("cmd", 22)
 	} else if(flight.rc_cmd==21) {
-		uav_land()
+		statef=land
 		neighbors.broadcast("cmd", 21)
 	}
 	oldcmd=flight.rc_cmd
 }
 
+statef()
 }
  
 # Executed once when the robot (or the simulator) is reset.