Add argument for examples to run and add additional obstacles for RRT.

launch/rrt2Dlaunch.xml

@@ -3,8 +3,10 @@
   <arg name="start_y" default="0.0"/>
   <arg name="goal_x" default="1.0"/>
   <arg name="goal_y" default="-1.0"/>
-  <arg name="map_sub_mode" default="True"/>
-  <arg name="obstacle_sub_mode" default="True"/>
+  <arg name="map_bounds_x" default="100"/>
+  <arg name="map_bounds_y" default="100"/>
+  <arg name="map_sub_mode" default="$(eval '\'$(var example_publishers)\' == \'True\'')"/>
+  <arg name="obstacle_sub_mode" default="$(eval '\'$(var example_publishers)\' == \'True\'')"/>
   <arg name="step_size" default="0.05"/>
   <arg name="node_limit" default="5000"/>
   <arg name="goal_tolerance" default="0.2"/>

launch/rrt3Dlaunch.xml

@@ -5,14 +5,17 @@
   <arg name="goal_x" default="1.0"/>
   <arg name="goal_y" default="-1.0"/>
   <arg name="goal_z" default="0.0"/>
-  <arg name="obstacle_sub_mode" default="True"/>
+  <arg name="map_bounds_x" default="100"/>
+  <arg name="map_bounds_y" default="100"/>
+  <arg name="map_bounds_z" default="100"/>
   <arg name="step_size" default="0.05"/>
   <arg name="node_limit" default="5000"/>
-  <arg name="goal_tolerance" default="0.5"/>
-  <arg name="wall_confidence" default="50"/>
+  <arg name="goal_tolerance" default="0.35"/>
+  <arg name="example_publishers" default="False"/>
+  <arg name="obstacle_sub_mode" default="$(eval '\'$(var example_publishers)\' == \'True\'')"/>
   <node name="rviz2" pkg="rviz2" exec="rviz2" args="-d $(find-pkg-share multidim_rrt_planner)/config/rrt3Dconfig.rviz"/>
-  <node pkg="multidim_rrt_planner" exec="map_frame_pub"/>
-  <node pkg="multidim_rrt_planner" exec="obstacle_pub_3D"/>
+  <node pkg="multidim_rrt_planner" exec="map_frame_pub" if="$(eval '\'$(var example_publishers)\' == \'True\'')"/>
+  <node pkg="multidim_rrt_planner" exec="obstacle_pub_3D" if="$(eval '\'$(var example_publishers)\' == \'True\'')"/>
   <node pkg="multidim_rrt_planner" exec="rrt3D">
     <param name="start_x" value="$(var start_x)"/>
     <param name="start_y" value="$(var start_y)"/>
@@ -22,7 +25,6 @@
     <param name="step_size" value="$(var step_size)"/>
     <param name="node_limit" value="$(var node_limit)"/>
     <param name="goal_tolerance" value="$(var goal_tolerance)"/>
-    <param name="wall_confidence" value="$(var wall_confidence)"/>
   </node>
 </launch>
 

multidim_rrt_planner/Marker.py

@@ -72,21 +72,3 @@ def create_marker(marker_type: int, marker_id: int, color: list,
     marker.pose.position.y = position[1]
     marker.pose.position.z = position[2]
     return marker
-
-
-def delete_marker(marker_id: int) -> Marker:
-    """
-    Delete a marker for visualization.
-
-    Arguments:
-    ---------
-    marker_id : int
-        The ID of the marker
-
-    """
-    marker = Marker()
-    marker.header.frame_id = "map"
-    marker.ns = "rrt_markers"
-    marker.id = marker_id
-    marker.action = Marker.DELETE
-    return marker

multidim_rrt_planner/obstacle_pub_2D.py

@@ -39,7 +39,9 @@ def __init__(self):
             1.0, self.publish_obstacle)
         self.obstacle_1 = Circle(1.0, 1.0, 1.0)
         self.obstacle_2 = Rectangle(-1.0, -1.0, 1.0, 1.0, 0.0)
-        self.obstacle_list = [self.obstacle_1, self.obstacle_2]
+        self.obstacle_3 = Circle(0.6, -0.6, 0.3)
+        self.obstacle_list = [self.obstacle_1,
+                              self.obstacle_2, self.obstacle_3]
 
     def publish_obstacle(self):
         """Publish obstacles in 2D."""

multidim_rrt_planner/obstacle_pub_3D.py

@@ -39,7 +39,9 @@ def __init__(self):
             1.0, self.publish_obstacle)
         self.obstacle_1 = Sphere(1.0, 1.0, 1.0, 1.0)
         self.obstacle_2 = Box(-1.0, -1.0, 1.0, 1.0, 1.0, 1.0, 0.0)
-        self.obstacle_list = [self.obstacle_1, self.obstacle_2]
+        self.obstacle_3 = Sphere(0.6, -0.6, 0.6, 0.3)
+        self.obstacle_list = [self.obstacle_1,
+                              self.obstacle_2, self.obstacle_3]
 
     def publish_obstacle(self):
         """Publish obstacles in 3D."""

multidim_rrt_planner/rrt2D.py

@@ -6,8 +6,7 @@
 from std_srvs.srv import Empty
 import numpy as np
 from .TreeNode import TreeNode
-from .Marker import Circle, Rectangle, create_marker, delete_marker
-import time
+from .Marker import Circle, Rectangle, create_marker
 
 
 class RRT2DNode(Node):
@@ -26,6 +25,10 @@ class RRT2DNode(Node):
         The y coordinate of the goal position.
     map_sub_mode : bool
         Whether or not to subscribe to the occupancy grid.
+    map_bounds_x : int
+        The width of the map that the rrt will be generated in.
+    map_bounds_y : int
+        The height of the map that the rrt will be generated in.
     obstacle_sub_mode : bool
         Whether or not to subscribe to the obstacle markers.
     step_size : float
@@ -63,17 +66,14 @@ class RRT2DNode(Node):
         The start node.
     node_list : list
         The list of nodes.
-    timer : Timer
-        The ROS timer for publishing markers and the path.
     marker_publisher : Publisher
         The ROS publisher for the markers.
+    path_publisher : Publisher
 
     Methods
     -------
     run_rrt_2D()
         Generates the RRT.
-    timer_callback()
-        Timer callback for publishing the final markers and path until the node is destroyed.
     occupancy_grid_callback(msg)
         Callback for the OccupancyGrid subscriber.
     obstacle_callback(msg)
@@ -94,8 +94,10 @@ def __init__(self):
             ('start_y', 0.0),
             ('goal_x', 1.0),
             ('goal_y', -1.0),
-            ('map_sub_mode', True),
-            ('obstacle_sub_mode', True),
+            ('map_sub_mode', False),
+            ('map_bounds_x', 100),
+            ('map_bounds_y', 100),
+            ('obstacle_sub_mode', False),
             ('step_size', 0.05),
             ('node_limit', 5000),
             ('goal_tolerance', 0.2),
@@ -114,8 +116,7 @@ def __init__(self):
             self.occupancy_grid_subscription = self.create_subscription(
                 OccupancyGrid, 'occupancy_grid', self.occupancy_grid_callback, 10)
         else:
-            self.map_size = np.array([100, 100])
-            self.map_resolution = 1.0
+            self.map_size = np.array([self.map_bounds_x, self.map_bounds_y])
         if self.obstacle_sub_mode:
             self.obstacle_list = []
             self.obstacle_subscription = self.create_subscription(
@@ -191,25 +192,27 @@ def run_rrt_2D(self):
                 new_node_unit_vec = min_node_vec / min_distance
                 new_node_val = min_node.val + new_node_unit_vec * self.step_size
                 new_node = TreeNode(new_node_val, min_node)
-                obstacle_collision = False  # Check if new_node collides with any obstacles
-                if self.obstacle_list:
-                    for obstacle in self.obstacle_list:
-                        if isinstance(obstacle, Circle):
-                            obstacle_vec = new_node.val - \
-                                np.array([obstacle.x, obstacle.y])
-                            obstacle_distance = np.linalg.norm(obstacle_vec)
-                            if obstacle_distance < obstacle.radius:
-                                obstacle_collision = True
-                                break
-                        elif isinstance(obstacle, Rectangle):
-                            if (obstacle.x - obstacle.width/2 < new_node.val[0]
-                                < obstacle.x + obstacle.width/2) and \
-                                (obstacle.y - obstacle.height/2 < new_node.val[1]
-                                 < obstacle.y + obstacle.height/2):
-                                obstacle_collision = True
-                                break
-                    if obstacle_collision:
-                        continue
+                if self.obstacle_sub_mode:
+                    obstacle_collision = False  # Check if new_node collides with any obstacles
+                    if self.obstacle_list:
+                        for obstacle in self.obstacle_list:
+                            if isinstance(obstacle, Circle):
+                                obstacle_vec = new_node.val - \
+                                    np.array([obstacle.x, obstacle.y])
+                                obstacle_distance = np.linalg.norm(
+                                    obstacle_vec)
+                                if obstacle_distance < obstacle.radius:
+                                    obstacle_collision = True
+                                    break
+                            elif isinstance(obstacle, Rectangle):
+                                if (obstacle.x - obstacle.width/2 < new_node.val[0]
+                                    < obstacle.x + obstacle.width/2) and \
+                                    (obstacle.y - obstacle.height/2 < new_node.val[1]
+                                     < obstacle.y + obstacle.height/2):
+                                    obstacle_collision = True
+                                    break
+                        if obstacle_collision:
+                            continue
                 wall_collision = False  # Check if new_node is closer than a step size to the wall
                 if self.map_sub_mode:
                     for pixel_center in self.wall_centers:
@@ -260,7 +263,7 @@ def obstacle_callback(self, msg):
         Arguments:
         ---------
         msg : MarkerArray
-            The received MarkerArray message.
+            The obstacle marker data.
 
         """
         self.obstacle_list = []