minor cleanup

Signed-off-by: stevedan <[email protected]>

tools/testing_environment/100by100_20.yaml

@@ -0,0 +1,6 @@
+image: 100by100_20.pgm
+resolution: 0.05
+origin: [0.0, 0.000000, 0.000000]
+negate: 1
+occupied_thresh: 0.65
+free_thresh: 0.196

tools/testing_environment/debug_script.py

@@ -8,14 +8,71 @@
 from transforms3d.euler import euler2quat
 import os
 import pickle
+from random import randint, seed, uniform
+import glob
+import time
+import math
 
-import numpy as np
 
+import numpy as np
+def getRandomStart(costmap, max_cost, side_buffer, time_stamp, res):
+    start = PoseStamped()
+    start.header.frame_id = 'map'
+    start.header.stamp = time_stamp
+    while True:
+        row = randint(side_buffer, costmap.shape[0] - side_buffer)
+        col = randint(side_buffer, costmap.shape[1] - side_buffer)
+
+        if costmap[row, col] < max_cost:
+            start.pose.position.x = col * res
+            start.pose.position.y = row * res
+
+            yaw = uniform(0, 1) * 2 * math.pi
+            quad = euler2quat(0.0, 0.0, yaw)
+            start.pose.orientation.w = quad[0]
+            start.pose.orientation.x = quad[1]
+            start.pose.orientation.y = quad[2]
+            start.pose.orientation.z = quad[3]
+            break
+    return start
+
+
+def getRandomGoal(costmap, start, max_cost, side_buffer, time_stamp, res):
+    goal = PoseStamped()
+    goal.header.frame_id = 'map'
+    goal.header.stamp = time_stamp
+    while True:
+        row = randint(side_buffer, costmap.shape[0] - side_buffer)
+        col = randint(side_buffer, costmap.shape[1] - side_buffer)
+
+        start_x = start.pose.position.x
+        start_y = start.pose.position.y
+        goal_x = col * res
+        goal_y = row * res
+        x_diff = goal_x - start_x
+        y_diff = goal_y - start_y
+        dist = math.sqrt(x_diff ** 2 + y_diff ** 2)
+
+        if costmap[row, col] < max_cost and dist > 3.0:
+            goal.pose.position.x = goal_x
+            goal.pose.position.y = goal_y
+
+            yaw = uniform(0, 1) * 2 * math.pi
+            quad = euler2quat(0.0, 0.0, yaw)
+            goal.pose.orientation.w = quad[0]
+            goal.pose.orientation.x = quad[1]
+            goal.pose.orientation.y = quad[2]
+            goal.pose.orientation.z = quad[3]
+            break
+    return goal
 
 def main():
     rclpy.init()
 
     navigator = BasicNavigator()
+    map_path = os.getcwd() + '/' + glob.glob('**/100by100_20.yaml', recursive=True)[0]
+    navigator.changeMap(map_path)
+    time.sleep(2)
 
     planner_param_cli = navigator.create_client(SetParameters, '/planner_server/set_parameters')
     print("created client")
@@ -60,36 +117,53 @@ def main():
     print("Goal orientation: ", goal.pose.orientation.x, ", ", goal.pose.orientation.y, ", ", goal.pose.orientation.z, ", ", goal.pose.orientation.w)
     result = []
     results = []
+    random_pairs = 50
+    res = costmap_msg.metadata.resolution
+    max_cost = 210
+    side_buffer = 100
+    time_stamp = navigator.get_clock().now().to_msg()
+    seed(33)
+    i = 0
+    allow_failure = False
     # generate path
     goal_headings = ["DEFAULT" , "BIDIRECTIONAL", "ALL_DIRECTION"]
     planners = ['SmacHybrid', 'Smac2d', 'SmacLattice']
     planners_goal_headings = ["DEFAULT_SmacHybrid" , "DEFAULT_Smac2d", "DEFAULT_SmacLattice", "BIDIRECTIONAL_SmacHybrid", "BIDIRECTIONAL_Smac2d", "BIDIRECTIONAL_SmacLattice", "ALL_DIRECTION_SmacHybrid", "ALL_DIRECTION_Smac2d", "ALL_DIRECTION_SmacLattice"]
-    for goal_heading in goal_headings:
-        print("Goal heading: ", goal_heading)      
-        for planner in planners:
-            print("Planner: ", planner)
-            if(planner != 'Smac2d'):
-                print("Setting goal heading for ", planner)
-                # set parameters planner
-                req = SetParameters.Request()
-                req.parameters = [ 
-                    Parameter(planner + '.goal_heading', Parameter.Type.STRING, goal_heading).to_parameter_msg()
-                ]
-                future = planner_param_cli.call_async(req)
-                rclpy.spin_until_future_complete(navigator, future)
-                # if future.results() is None:
-                #     print('Service call failed %r' % (future.exception(),))
-                #     return
-                # print('Service call result %r' % (future.results().result,))
-            print("Getting path from ", planner)
-            path = navigator._getPathImpl(start, goal, planner, use_start=True)
-            if path is not None and path.error_code == 0:
-                print("Path found by ", planner)
-                result.append(path)
-            else:
-                print(planner, 'planner failed to produce the path')
-        print("\n")
-        # sleep(5)
+    while len(results) != random_pairs:
+        print('Cycle: ', i, 'out of: ', random_pairs)
+        result = []
+        start = getRandomStart(costmap, max_cost, side_buffer, time_stamp, res)
+        goal = getRandomGoal(costmap, start, max_cost, side_buffer, time_stamp, res)
+        for goal_heading in goal_headings:
+            print("Goal heading: ", goal_heading)      
+            for planner in planners:
+                print("Planner: ", planner)
+                if(planner != 'Smac2d'):
+                    print("Setting goal heading for ", planner)
+                    # set parameters planner
+                    req = SetParameters.Request()
+                    req.parameters = [ 
+                        Parameter(planner + '.goal_heading', Parameter.Type.STRING, goal_heading).to_parameter_msg()
+                    ]
+                    future = planner_param_cli.call_async(req)
+                    rclpy.spin_until_future_complete(navigator, future)
+                    # if future.results() is None:
+                    #     print('Service call failed %r' % (future.exception(),))
+                    #     return
+                    # print('Service call result %r' % (future.results().result,))
+                print("Getting path from ", planner)
+                path = navigator._getPathImpl(start, goal, planner, use_start=True)
+                if path is not None and path.error_code == 0:
+                    print("Path found by ", planner)
+                    result.append(path)
+                else:
+                    print("\033[91m", end="")
+                    print(planner, 'planner failed to produce the path')
+                    print("\033[0m", end="")                   
+            print("\n")
+        if len(result) == len(planners_goal_headings):
+            results.append(result)
+            i = i + 1
     print("Done")
     print('Write Results...')
     results.append(result)