From 1f2fae047a36d3c561a3a4085c8436693a6f772b Mon Sep 17 00:00:00 2001
From: dave <david.st-onge@polymtl.ca>
Date: Wed, 20 Dec 2017 16:10:30 -0500
Subject: [PATCH] fix proxmity topic

---
 buzz_scripts/testRRT.bzz         | 14 ++++++++++----
 launch/launch_config/topics.yaml |  1 +
 src/roscontroller.cpp            |  9 +++++----
 3 files changed, 16 insertions(+), 8 deletions(-)

diff --git a/buzz_scripts/testRRT.bzz b/buzz_scripts/testRRT.bzz
index 7478529..755bc4f 100644
--- a/buzz_scripts/testRRT.bzz
+++ b/buzz_scripts/testRRT.bzz
@@ -13,10 +13,10 @@ function action() {
 function init() {
   	uav_initstig()
 	uav_initswarm()
-	#statef=turnedoff
-	#UAVSTATE = "TURNEDOFF"
-	statef = takeoff
-	BVMSTATE = "TAKEOFF"
+	statef=turnedoff
+	BVMSTATE = "TURNEDOFF"
+	#statef = takeoff
+	#BVMSTATE = "TAKEOFF"
 }
 
 # Executed at each time step.
@@ -26,6 +26,12 @@ function step() {
 	statef()
 
 	log("Current state: ", BVMSTATE)
+	log("Obstacles: ")
+	reduce(proximity,
+    function(key, value, acc) {
+		log(key, " - ", value.angle, value.value)
+		return acc
+	}, math.vec2.new(0, 0))
 }
  
 # Executed once when the robot (or the simulator) is reset.
diff --git a/launch/launch_config/topics.yaml b/launch/launch_config/topics.yaml
index e7358cd..9e33d37 100644
--- a/launch/launch_config/topics.yaml
+++ b/launch/launch_config/topics.yaml
@@ -10,3 +10,4 @@ topics:
   localpos: local_position/pose
   stream:  set_stream_rate
   altitude: global_position/rel_alt
+  obstacles: obstacles
diff --git a/src/roscontroller.cpp b/src/roscontroller.cpp
index 2bf5cab..11d5913 100644
--- a/src/roscontroller.cpp
+++ b/src/roscontroller.cpp
@@ -318,7 +318,7 @@ void roscontroller::GetSubscriptionParameters(ros::NodeHandle& node_handle)
     system("rosnode kill rosbuzz_node");
   }
 
-  node_handle.getParam("obstacles", obstacles_topic);
+  node_handle.getParam("topics/obstacles", obstacles_topic);
 }
 
 void roscontroller::Initialize_pub_sub(ros::NodeHandle& n_c)
@@ -468,7 +468,7 @@ void roscontroller::grid_publisher()
     grid_msg.header.frame_id = "/world";
     grid_msg.header.stamp = current_time;
     grid_msg.info.map_load_time = current_time;  // Same as header stamp as we do not load the map.
-    grid_msg.info.resolution = 0.1;//gridMap.getResolution();
+    grid_msg.info.resolution = 0.01;//gridMap.getResolution();
     grid_msg.info.width = g_w;
     grid_msg.info.height = g_h;
     grid_msg.info.origin.position.x = 0.0;
@@ -483,8 +483,9 @@ void roscontroller::grid_publisher()
     for (itr=grid.begin(); itr!=grid.end(); ++itr) {
       std::map<int,int>::iterator itc = itr->second.begin();
       for (itc=itr->second.begin(); itc!=itr->second.end(); ++itc) {
-        //ROS_INFO("--------------> index: %i (%i,%i): %i", (itr->first-1)*g_w+itc->first, itr->first, itc->first, round(itc->second*100.0));
-        grid_msg.data[(itr->first-1)*g_w+itc->first] = round(itc->second*100.0);
+        grid_msg.data[(itr->first-1)*g_w+itc->first] = itc->second;
+        // DEBUG
+        //ROS_INFO("--------------> index: %i (%i,%i): %i", (itr->first-1)*g_w+itc->first, itr->first, itc->first, grid_msg.data[(itr->first-1)*g_w+itc->first]);
       }
     }
     grid_pub.publish(grid_msg);