generalized some parts of RRT and complete export occ_grid

CMakeLists.txt

@@ -16,6 +16,7 @@ find_package(catkin REQUIRED COMPONENTS
   std_msgs
   mavros_msgs
   sensor_msgs
+  nav_msgs
   message_generation
 )
 

buzz_scripts/include/rrtstar.bzz

@@ -6,54 +6,41 @@
 include "mapmatrix.bzz"
 
 RRT_TIMEOUT = 500
+GSCALE = {.x=1, .y=1}
 map = nil
 cur_cell = {}
 nei_cell = {}
 
-function UAVinit_map(m_navigation) {
+# create a map with its length to fit the goal (rel. pos.) and the current position as the center
+function dyn_init_map(m_navigation) {
     # create a map big enough for the goal
-    init_map(2*math.round(math.vec2.length(m_navigation))+10)
+    init_map(math.round(2*math.vec2.length(m_navigation))+10)
     # center the robot on the grid
     cur_cell = math.vec2.new(math.round(size(map)/2.0),math.round(size(map[1])/2.0))
 }
 
-function UAVpathPlanner(m_navigation, m_pos) {
+# start the RRT* to reach the goal (rel. pos.)
+function pathPlanner(m_goal, m_pos, kh4) {
     # place the robot on the grid
-    cur_cell = getcell(m_pos,0)
+    cur_cell = getcell(m_pos)
     # create the goal in the map grid
-    mapgoal = math.vec2.add(m_navigation,cur_cell)
+    mapgoal = getcell(math.vec2.add(m_goal, m_pos))
-    mapgoal = math.vec2.new(math.round(mapgoal.x),math.round(mapgoal.y))
+
+    #print_map(map)
+    #export_map(map)
 
     # search for a path
-    return RRTSTAR(mapgoal,math.vec2.new(5,5))  #size(map[1])/20.0,size(map[1])/20.0))
+    return RRTSTAR(mapgoal,math.vec2.new(5 * GSCALE.x, 5  * GSCALE.y))  #size(map[1])/20.0,size(map[1])/20.0))
 }
 
-function Kh4pathPlanner(m_goal, m_pos) {
+function getcell(m_curpos) {
-    # move 0,0 to a corner instead of the center
-    m_goal = math.vec2.sub(m_goal,math.vec2.new(GRIDCM.xmin, GRIDCM.ymin))
-    m_goal = math.vec2.new(math.round(m_goal.x*CM2KH4.x), math.round(m_goal.y*CM2KH4.y))
-
-    # place the robot on the grid
-    cur_cell = getcell(m_pos,1)
-    log("Starting from cell: ", cur_cell.x, " ", cur_cell.y)
-    log("Going to cell: ", m_goal.x, " ", m_goal.y)
-
-    # search for a path
-    print_map(map)
-    # print_map_argos(map)
-    return RRTSTAR(m_goal, math.vec2.new(10.0 * CM2KH4.x, 10.0 * CM2KH4.y))
-}
-
-function getcell(m_curpos, kh4) {
   var cell = {}
-  if(kh4) { # for khepera playground
+  # relative to center (start position)
-    cell = math.vec2.sub(m_curpos, math.vec2.new(GRIDCM.xmin, GRIDCM.ymin))
+  cell = math.vec2.new(math.round(size(map)/2.0),math.round(size(map[1])/2.0))
-    cell = math.vec2.new(math.round(cell.x*CM2KH4.x), math.round(cell.y*CM2KH4.y))
+  var offset = math.vec2.new(m_curpos.x*GSCALE.x, m_curpos.y*GSCALE.y)
-  } else { # for uav map relative to home
+  cell = math.vec2.add(cell, offset)
-    cell = math.vec2.new(math.round(size(map)/2.0),math.round(size(map[1])/2.0))
+  cell = math.vec2.new(math.round(cell.x), math.round(cell.y))
-    cell = math.vec2.add(cell, m_curpos)
+  
-    cell = math.vec2.new(math.round(cell.x), math.round(cell.y))
-  }
   if(cell.x > size(map))
     cell.x = size(map)
   if(cell.y > size(map[1]))
@@ -72,7 +59,7 @@ function updateMap(m_pos) {
 }
 
 function UAVgetneiobs (m_curpos) {
-    cur_cell = getcell(m_curpos,0)
+    cur_cell = getcell(m_curpos)
     # add all neighbors as obstacles in the grid
     neighbors.foreach(function(rid, data) {
             add_obstacle(math.vec2.add(math.vec2.newp(data.distance,data.azimuth),cur_cell), 0, 1.0)
@@ -81,7 +68,7 @@ function UAVgetneiobs (m_curpos) {
 
 function getneiobs (m_curpos) {
   var foundobstacle = 0
-  cur_cell = getcell(m_curpos,1)
+  cur_cell = getcell(m_curpos)
   var old_nei = table_copy(nei_cell)
   #log("NeiObs debug: ", nei_cell[9], " ", nei_cell[3])
 
@@ -127,7 +114,7 @@ function getneiobs (m_curpos) {
 
 function getproxobs (m_curpos) {
   foundobstacle = 0
-  cur_cell = getcell(m_curpos,1)
+  cur_cell = getcell(m_curpos)
 
   reduce(proximity,
     function(key, value, acc) {
@@ -170,13 +157,13 @@ function getproxobs (m_curpos) {
 function check_path(m_path, goal_l, m_curpos, kh4) {
   var pathisblocked = 0
   var nb = goal_l
-  cur_cell = getcell(m_curpos,kh4)
+  cur_cell = getcell(m_curpos)
   var Cvec = cur_cell
   while(nb <= size(m_path) and nb <= goal_l+1) {
     var Nvec = getvec(m_path, nb)
     if(kh4 == 0) {
         Nvec = vec_from_gps(Nvec.x,Nvec.y,1)
-        Nvec = getcell(Nvec,kh4)
+        Nvec = getcell(Nvec)
     }
     if(doesItIntersect(Cvec, Nvec)) {
       log("Path blocked ! (", Cvec.x, "/", Cvec.y, "->", Nvec.x, "/", Nvec.y, ")")
@@ -225,7 +212,7 @@ function RRTSTAR(GOAL,TOL) {
         minCounter = 0;
 
         if(size(pointList) != 0) {
-            log("Found ", size(pointList), " close to point: ", pointList[size(pointList)][3]) 
+            #log("Found ", size(pointList), " close to point: ", pointList[size(pointList)][3]) 
             ipt=1
             while(ipt <= size(pointList)) {
                 pointNumber = {}
@@ -329,8 +316,7 @@ function RRTSTAR(GOAL,TOL) {
     }
     if(goalReached){
         log("Goal found(",numberOfPoints,")!")
-        Path = getPathGPS(Q,numberOfPoints)
+        Path = getPath(Q,numberOfPoints, 1)
-        print_pos(Path)
     } else {
       log("FAILED TO FIND A PATH!!!")
       Path = nil
@@ -424,62 +410,40 @@ function doesItIntersect(point,vector) {
     return 0
 }
 
-function getPathGPS(Q,nb){
+# create a table with only the path's points in order
+function getPath(Q,nb,gps){
     #log("-----------CONVERTING PATH!!!")
-    path={}
+    var pathL={}
     var npt=0
-    var isrecursive = 0
     # get the path from the point list
-    while(nb > 1 and isrecursive!=1){
+    while(nb > 1) {
         npt=npt+1
-        path[npt] = {}
+        pathL[npt] = {}
-        path[npt][1]=Q[nb][1]
+        pathL[npt][1]=Q[nb][1]
-        path[npt][2]=Q[nb][2]
+        pathL[npt][2]=Q[nb][2]
         if(nb != Q[Q[nb][3]][3])
             nb = Q[nb][3];
         else {
-            isrecursive = 1
-            path={}
             log("ERROR - Recursive path !!!")
+            return nil
         }
     }
 
-    # re-order the list and make the path points absolute
-    pathR={}
-    var totpt = npt + 1
-    while(npt > 0){
-        tmpgoal = gps_from_vec(math.vec2.sub(getvec(path,npt),cur_cell))
-        pathR[totpt-npt] = {}
-        pathR[totpt-npt][1]=tmpgoal.latitude
-        pathR[totpt-npt][2]=tmpgoal.longitude
-        npt = npt - 1
-    }
-    return pathR
-}
-
-# create a table with only the path's points in order
-function getPath(Q,nb){
-    path={}
-    var npt=0
-
-    # log("get the path from the point list")
-    while(nb > 1){
-        npt=npt+1
-        path[npt] = {}
-        path[npt][1]=Q[nb][1]
-        path[npt][2]=Q[nb][2]
-        nb = Q[nb][3];
-    }
-
     # log("re-order the list")
     # table_print(path.mat)
-    pathR={}
+    var pathR={}
     var totpt = npt + 1
     while(npt > 0){
-        tmpgoal = getvec(path,npt)
         pathR[totpt-npt] = {}
-        pathR[totpt-npt][1]=tmpgoal.x
+        if(gps) {
-        pathR[totpt-npt][2]=tmpgoal.y
+            tmpgoal = gps_from_vec(math.vec2.sub(getvec(pathL,npt),cur_cell))
+            pathR[totpt-npt][1]=tmpgoal.latitude
+            pathR[totpt-npt][2]=tmpgoal.longitude
+        } else {
+            tmpgoal = getvec(path,npt)
+            pathR[totpt-npt][1]=tmpgoal.x
+            pathR[totpt-npt][2]=tmpgoal.y
+        }
         npt = npt - 1
     }
     #log("Double-check path: ", check_path(pathR, 1, cur_cell, 1))

buzz_scripts/include/uavstates.bzz

@@ -104,15 +104,16 @@ function gotoWPRRT(transf) {
 		if(Path==nil){
 			# create the map
 			if(map==nil) {
-				UAVinit_map(rc_goal)
+				dyn_init_map(rc_goal)
 				homegps.lat = position.latitude
 				homegps.long = position.longitude
 			}
 			# add proximity sensor and neighbor obstacles to the map
 			while(Path==nil) {
 				updateMap(m_pos)
-				Path = UAVpathPlanner(rc_goal, m_pos)
+				Path = pathPlanner(rc_goal, m_pos)
 			}
+        	print_pos(Path)
 			cur_goal_l = 1
 		} else if(cur_goal_l <= size(Path)) {
 			var cur_goal_gps = getvec(Path, cur_goal_l)	#x=latitude, y=longitude
@@ -126,8 +127,9 @@ function gotoWPRRT(transf) {
 					rc_goal = vec_from_gps(rc_goto.latitude, rc_goto.longitude,0)
 					while(Path == nil) {
 						updateMap(m_pos)
-						Path = UAVpathPlanner(rc_goal, m_pos)
+						Path = pathPlanner(rc_goal, m_pos)
 					}
+        			print_pos(Path)
 					cur_goal_l = 1
 				} else {
 					cur_goal_pt = math.vec2.scale(cur_goal_pt, GOTO_MAXVEL/math.vec2.length(cur_goal_pt))

include/buzzuav_closures.h

@@ -94,6 +94,10 @@ void set_currentpos(double latitude, double longitude, double altitude);
  * returns the current go to position
  */
 double* getgoto();
+/*
+ * returns the current grid
+ */
+std::map<int, std::map<int,int>> getgrid();
 /*
  * returns the current Buzz state
  */

include/roscontroller.h

@@ -1,6 +1,7 @@
 #pragma once
 #include <ros/ros.h>
 #include <sensor_msgs/NavSatFix.h>
+#include <nav_msgs/OccupancyGrid.h>
 #include <std_msgs/UInt8.h>
 #include "mavros_msgs/GlobalPositionTarget.h"
 #include "mavros_msgs/CommandCode.h"
@@ -124,6 +125,7 @@ private:
   ros::Publisher MPpayload_pub;
   ros::Publisher neigh_pos_pub;
   ros::Publisher uavstate_pub;
+  ros::Publisher grid_pub;
   ros::Publisher localsetpoint_nonraw_pub;
   ros::ServiceServer service;
   ros::Subscriber current_position_sub;
@@ -179,6 +181,9 @@ private:
   /*UAVState publisher*/
   void uavstate_publisher();
 
+  /*Grid publisher*/
+  void grid_publisher();
+
   /*BVM message payload publisher*/
   void send_MPpayload();
 

package.xml

@@ -48,6 +48,8 @@
   <run_depend>mavros_msgs</run_depend>
   <build_depend>sensor_msgs</build_depend>
   <run_depend>sensor_msgs</run_depend>
+  <build_depend>nav_msgs</build_depend>
+  <run_depend>nav_msgs</run_depend>
   <build_depend>message_generation</build_depend> 
   <run_depend>message_runtime</run_depend> 
 

src/buzzuav_closures.cpp

@@ -36,6 +36,7 @@ std::map<int, buzz_utility::RB_struct> targets_map;
 std::map<int, buzz_utility::RB_struct> wplist_map;
 std::map<int, buzz_utility::Pos_struct> neighbors_map;
 std::map<int, buzz_utility::neighbors_status> neighbors_status_map;
+std::map<int, std::map<int,int>> grid;
 
 /****************************************/
 /****************************************/
@@ -178,26 +179,24 @@ int buzz_exportmap(buzzvm_t vm)
 / Buzz closure to export a 2D map
 /----------------------------------------*/
 {
-  /* Make sure one parameter has been passed */
   buzzvm_lnum_assert(vm, 1);
-  /* Get the parameter */
+  // Get the parameter
   buzzvm_lload(vm, 1);
-  buzzvm_type_assert(vm, 1, BUZZTYPE_TABLE);  // matrix
+  buzzvm_type_assert(vm, 1, BUZZTYPE_TABLE);    // dictionary
-  /* Get the table */
   buzzobj_t t = buzzvm_stack_at(vm, 1);
-  /* Copy the values into a vector */
+  for(int32_t i = 1; i < buzzdict_size(t->t.value); ++i) {
-  std::vector<float> mat;
-  for (int32_t i = 0; i < buzzdict_size(t->t.value); ++i)
-  {
-    /* Duplicate the table */
     buzzvm_dup(vm);
-    /* Push the index */
     buzzvm_pushi(vm, i);
-    /* Get the value */
     buzzvm_tget(vm);
-    /* Store it in the vector (assume all values are float, no mistake...) */
+    std::map<int, int> row;
-    mat.push_back((float)buzzvm_stack_at(vm, 1)->f.value);
+    for(int32_t j = 1; j < buzzdict_size(buzzvm_stack_at(vm, 1)->t.value); ++j) {
-    /* Get rid of the value, now useless */
+      buzzvm_dup(vm);
+      buzzvm_pushi(vm, j);
+      buzzvm_tget(vm);
+      row.insert(std::pair<int,int>(j,round(buzzvm_stack_at(vm, 1)->f.value*100.0)));
+      buzzvm_pop(vm);
+    }
+    grid.insert(std::pair<int,std::map<int, int>>(i,row));
     buzzvm_pop(vm);
   }
   return buzzvm_ret0(vm);
@@ -457,6 +456,14 @@ double* getgoto()
   return goto_pos;
 }
 
+std::map<int, std::map<int,int>> getgrid()
+/*
+/ return the grid
+/-------------------------------------------------------------*/
+{
+  return grid;
+}
+
 float* getgimbal()
 /*
 / return current gimbal commands

src/roscontroller.cpp

@@ -114,8 +114,10 @@ void roscontroller::RosControllerRun()
     // ROS_WARN("[%i] -----------------------STARTING MAIN LOOP!", robot_id);
     while (ros::ok() && !buzz_utility::buzz_script_done())
     {
-      //  Neighbours of the robot published with id in respective topic
+      //  Publish topics
       neighbours_pos_publisher();
+      uavstate_publisher();
+      grid_publisher();
       send_MPpayload();
       //  Check updater state and step code
       update_routine();
@@ -337,6 +339,7 @@ void roscontroller::Initialize_pub_sub(ros::NodeHandle& n_c)
   MPpayload_pub = n_c.advertise<mavros_msgs::Mavlink>("fleet_status", 5);
   neigh_pos_pub = n_c.advertise<rosbuzz::neigh_pos>("neighbours_pos", 5);
   uavstate_pub = n_c.advertise<std_msgs::String>("uavstate", 5);
+  grid_pub = n_c.advertise<nav_msgs::OccupancyGrid>("grid", 5);
   localsetpoint_nonraw_pub = n_c.advertise<geometry_msgs::PoseStamped>(setpoint_name, 5);
 
   // Service Clients
@@ -447,6 +450,45 @@ void roscontroller::uavstate_publisher()
   uavstate_pub.publish(uavstate_msg);
 }
 
+void roscontroller::grid_publisher()
+/*
+/ Publish current Grid from Buzz script
+/----------------------------------------------------*/
+{
+  std::map<int, std::map<int,int>> grid = buzzuav_closures::getgrid();
+  std::map<int, std::map<int,int>>::iterator itr = grid.begin();
+  int g_w = itr->second.size();
+  int g_h = grid.size();
+
+  if(g_w!=0 && g_h!=0) {
+    auto current_time = ros::Time::now();
+    nav_msgs::OccupancyGrid grid_msg;
+    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 = gridMap.getResolution();
+    grid_msg.info.width = g_w;
+    grid_msg.info.height = g_h;
+    grid_msg.info.origin.position.x = 0.0;
+    grid_msg.info.origin.position.y = 0.0;
+    grid_msg.info.origin.position.z = 0.0;
+    grid_msg.info.origin.orientation.x = 0.0;
+    grid_msg.info.origin.orientation.y = 0.0;
+    grid_msg.info.origin.orientation.z = 0.0;
+    grid_msg.info.origin.orientation.w = 1.0;
+    grid_msg.data.resize(g_w*g_h);
+
+    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) {
+        grid_msg.data[itr->first*g_w+itc->first] = round(itc->second*100.0);
+      }
+    }
+    grid_pub.publish(grid_msg);
+  }
+}
+
+
 void roscontroller::Arm()
 /*
 / Functions handling the local MAV ROS flight controller