From 830929b20e8da070d2e4feb6da2a9fb66c8afa6f Mon Sep 17 00:00:00 2001 From: daviddorf2023 Date: Sat, 10 Feb 2024 13:56:03 -0500 Subject: [PATCH] Pass colcon test --- multidim_rrt_planner/rrt2D.py | 7 ++++--- multidim_rrt_planner/rrt3D.py | 10 ++++++---- 2 files changed, 10 insertions(+), 7 deletions(-) diff --git a/multidim_rrt_planner/rrt2D.py b/multidim_rrt_planner/rrt2D.py index 46916bf..0013c4c 100644 --- a/multidim_rrt_planner/rrt2D.py +++ b/multidim_rrt_planner/rrt2D.py @@ -129,7 +129,6 @@ def __init__(self): def rrt_callback(self, request, response): """Service callback for running the RRT.""" - # Clear the markers and path self.marker_array = MarkerArray() clear_marker = Marker() @@ -142,9 +141,11 @@ def rrt_callback(self, request, response): # Publish the start and goal markers self.marker_array.markers.append(create_marker(Marker.SPHERE, 0, [ - 1.0, 0.0, 0.0, 1.0], [0.2, 0.2, 0.2], [self.start_position[0], self.start_position[1], 0.0])) + 1.0, 0.0, 0.0, 1.0], [0.2, 0.2, 0.2], [self.start_position[0], + self.start_position[1], 0.0])) self.marker_array.markers.append(create_marker(Marker.SPHERE, 1, [ - 0.0, 0.0, 1.0, 1.0], [0.2, 0.2, 0.2], [self.goal_position[0], self.goal_position[1], 0.0])) + 0.0, 0.0, 1.0, 1.0], [0.2, 0.2, 0.2], [self.goal_position[0], + self.goal_position[1], 0.0])) # Run the RRT self.node_list = [self.start_node] diff --git a/multidim_rrt_planner/rrt3D.py b/multidim_rrt_planner/rrt3D.py index 1029aa2..c53e34f 100644 --- a/multidim_rrt_planner/rrt3D.py +++ b/multidim_rrt_planner/rrt3D.py @@ -113,7 +113,6 @@ def __init__(self): def rrt_callback(self, request, response): """Service callback for running the RRT.""" - # Clear the markers and path self.marker_array = MarkerArray() clear_marker = Marker() @@ -126,9 +125,11 @@ def rrt_callback(self, request, response): # Publish the start and goal markers self.marker_array.markers.append(create_marker(Marker.SPHERE, 0, [ - 1.0, 0.0, 0.0, 1.0], [0.2, 0.2, 0.2], [self.start_position[0], self.start_position[1], self.start_position[2]])) + 1.0, 0.0, 0.0, 1.0], [0.2, 0.2, 0.2], [self.start_position[0], self.start_position[1], + self.start_position[2]])) self.marker_array.markers.append(create_marker(Marker.SPHERE, 1, [ - 0.0, 0.0, 1.0, 1.0], [0.2, 0.2, 0.2], [self.goal_position[0], self.goal_position[1], self.goal_position[2]])) + 0.0, 0.0, 1.0, 1.0], [0.2, 0.2, 0.2], [self.goal_position[0], self.goal_position[1], + self.goal_position[2]])) # Run the RRT self.node_list = [self.start_node] @@ -201,7 +202,8 @@ def run_rrt_3D(self): min_node.add_child(new_node) self.node_list.append(new_node) marker = create_marker(Marker.SPHERE, self.node_list.index(new_node) + 2, [ - 0.0, 1.0, 0.0, 1.0], [0.1, 0.1, 0.1], [new_node.val[0], new_node.val[1], new_node.val[2]]) + 0.0, 1.0, 0.0, 1.0], [0.1, 0.1, 0.1], [new_node.val[0], new_node.val[1], + new_node.val[2]]) self.marker_array.markers.append(marker) self.marker_publisher.publish(self.marker_array) self.get_logger().info('Path not found')