From 149abb9d532f8b2a7a49051d133609d2cf3b14cf Mon Sep 17 00:00:00 2001
From: David St-Onge <david.st-onge@polymtl.ca>
Date: Wed, 25 Jan 2017 21:10:07 -0500
Subject: [PATCH] fix ROS std msgs

---
 src/roscontroller.cpp |  4 +++-
 src/test1.bzz         | 12 +++++++-----
 2 files changed, 10 insertions(+), 6 deletions(-)

diff --git a/src/roscontroller.cpp b/src/roscontroller.cpp
index 4d97bec..029a835 100644
--- a/src/roscontroller.cpp
+++ b/src/roscontroller.cpp
@@ -37,12 +37,14 @@ namespace rosbzz_node{
   			{
       				/*Update neighbors position inside Buzz*/
      				buzz_utility::neighbour_pos_callback(neighbours_pos_map);
-				
+				auto current_time = ros::Time::now();
 				map< int, buzz_utility::Pos_struct >::iterator it;
 				rosbuzz::neigh_pos neigh_pos_array; //neigh_pos_array.clear(); 
 				for (it=raw_neighbours_pos_map.begin(); it!=raw_neighbours_pos_map.end(); ++it){
 					sensor_msgs::NavSatFix neigh_tmp;
 					//cout<<"iterator it val: "<< it-> first << " After convertion: " <<(uint8_t) buzz_utility::get_rid_uint8compac(it->first)<<endl;				
+					neigh_tmp.header.stamp = current_time;
+					neigh_tmp.header.frame_id = "/world";
 					neigh_tmp.position_covariance_type=it->first; //custom robot id storage
                         		neigh_tmp.longitude=(it->second).x;
                         		neigh_tmp.latitude=(it->second).y;
diff --git a/src/test1.bzz b/src/test1.bzz
index 6cb8501..edbbc3f 100644
--- a/src/test1.bzz
+++ b/src/test1.bzz
@@ -1,10 +1,12 @@
 # We need this for 2D vectors
 # Make sure you pass the correct include path to "bzzc -I <path1:path2> ..."
 include "/home/ubuntu/buzz/src/include/vec2.bzz"
+
+TARGET_ALTITUDE = 10.0
  
 # Lennard-Jones parameters
-TARGET     = 0.000000100001
-EPSILON    = 0.01
+TARGET     = 0.000000100005
+EPSILON    = 0.1
  
 # Lennard-Jones interaction magnitude
 function lj_magnitude(dist, target, epsilon) {
@@ -29,7 +31,7 @@ function hexagon() {
   if(neighbors.count() > 0)
     math.vec2.scale(accum, 1.0 / neighbors.count())
   # Move according to vector
-  uav_goto(accum.x+position.latitude, accum.y+position.longitude, position.altitude)
+  uav_goto(accum.x+position.latitude, accum.y+position.longitude, TARGET_ALTITUDE)
 }
  
 ########################################
@@ -89,12 +91,12 @@ neighbors.listen("cmd",
 }
 
 function takeoff() {
-	if( flight.status == 2 and position.altitude >= 10.0) {
+	if( flight.status == 2 and position.altitude >= TARGET_ALTITUDE-TARGET_ALTITUDE/50.0) {
 		barrier_set(ROBOTS,hexagon)
 		barrier_ready()
 	}
 	else if( flight.status !=3)
-		uav_takeoff(10.1)
+		uav_takeoff(TARGET_ALTITUDE)
 }
 function land() {
 	if( flight.status == 1) {