From 326d8654d326db1a3172f201ec72d09ddc4d6a22 Mon Sep 17 00:00:00 2001 From: daviddorf2023 Date: Sun, 5 Nov 2023 15:30:22 -0600 Subject: [PATCH] Bugfixes to sphere marker publishing in 3D. Separate RViz config files for 2D and 3D. --- config/rrt2Dconfig.rviz | 227 ++++++++++++++++++++ config/{rrtconfig.rviz => rrt3Dconfig.rviz} | 23 +- launch/rrt2Dlaunch.xml | 4 +- launch/rrt3Dlaunch.xml | 6 +- ros2rrt/obstacle_pub_3D.py | 37 ++++ ros2rrt/rrt2D.py | 2 +- ros2rrt/rrt3D.py | 4 +- setup.py | 1 + 8 files changed, 288 insertions(+), 16 deletions(-) create mode 100644 config/rrt2Dconfig.rviz rename config/{rrtconfig.rviz => rrt3Dconfig.rviz} (93%) create mode 100644 ros2rrt/obstacle_pub_3D.py diff --git a/config/rrt2Dconfig.rviz b/config/rrt2Dconfig.rviz new file mode 100644 index 0000000..ae6374b --- /dev/null +++ b/config/rrt2Dconfig.rviz @@ -0,0 +1,227 @@ +Panels: + - Class: rviz_common/Displays + Help Height: 78 + Name: Displays + Property Tree Widget: + Expanded: + - /Path1/Topic1 + Splitter Ratio: 0.5 + Tree Height: 549 + - Class: rviz_common/Selection + Name: Selection + - Class: rviz_common/Tool Properties + Expanded: + - /2D Goal Pose1 + - /Publish Point1 + Name: Tool Properties + Splitter Ratio: 0.5886790156364441 + - Class: rviz_common/Views + Expanded: + - /Current View1 + Name: Views + Splitter Ratio: 0.5 + - Class: rviz_common/Time + Experimental: false + Name: Time + SyncMode: 0 + SyncSource: "" +Visualization Manager: + Class: "" + Displays: + - Alpha: 0.5 + Cell Size: 1 + Class: rviz_default_plugins/Grid + Color: 160; 160; 164 + Enabled: true + Line Style: + Line Width: 0.029999999329447746 + Value: Lines + Name: Grid + Normal Cell Count: 0 + Offset: + X: 0 + Y: 0 + Z: 0 + Plane: XY + Plane Cell Count: 10 + Reference Frame: + Value: true + - Class: rviz_default_plugins/MarkerArray + Enabled: true + Name: MarkerArray + Namespaces: + rrt_markers: true + Topic: + Depth: 5 + Durability Policy: Volatile + History Policy: Keep Last + Reliability Policy: Reliable + Value: /rrt_markers + Value: true + - Alpha: 1 + Buffer Length: 1 + Class: rviz_default_plugins/Path + Color: 224; 27; 36 + Enabled: true + Head Diameter: 0.30000001192092896 + Head Length: 0.20000000298023224 + Length: 0.30000001192092896 + Line Style: Lines + Line Width: 0.029999999329447746 + Name: Path + Offset: + X: 0 + Y: 0 + Z: 0 + Pose Color: 255; 85; 255 + Pose Style: None + Radius: 0.029999999329447746 + Shaft Diameter: 0.10000000149011612 + Shaft Length: 0.10000000149011612 + Topic: + Depth: 5 + Durability Policy: Volatile + Filter size: 10 + History Policy: Keep Last + Reliability Policy: Reliable + Value: /rrt_path + Value: true + - Class: rviz_default_plugins/MarkerArray + Enabled: true + Name: MarkerArray + Namespaces: + rrt_markers: true + Topic: + Depth: 5 + Durability Policy: Volatile + History Policy: Keep Last + Reliability Policy: Reliable + Value: /obstacle_markers_2D + Value: true + - Alpha: 0.699999988079071 + Binary representation: false + Binary threshold: 100 + Class: rviz_default_plugins/Map + Color Scheme: map + Draw Behind: false + Enabled: true + Name: Map + Topic: + Depth: 5 + Durability Policy: Volatile + Filter size: 10 + History Policy: Keep Last + Reliability Policy: Reliable + Value: /occupancy_grid + Update Topic: + Depth: 5 + Durability Policy: Volatile + History Policy: Keep Last + Reliability Policy: Reliable + Value: /occupancy_grid_updates + Use Timestamp: false + Value: true + Enabled: true + Global Options: + Background Color: 48; 48; 48 + Fixed Frame: map + Frame Rate: 30 + Name: root + Tools: + - Class: rviz_default_plugins/Interact + Hide Inactive Objects: true + - Class: rviz_default_plugins/MoveCamera + - Class: rviz_default_plugins/Select + - Class: rviz_default_plugins/FocusCamera + - Class: rviz_default_plugins/Measure + Line color: 128; 128; 0 + - Class: rviz_default_plugins/SetInitialPose + Covariance x: 0.25 + Covariance y: 0.25 + Covariance yaw: 0.06853891909122467 + Topic: + Depth: 5 + Durability Policy: Volatile + History Policy: Keep Last + Reliability Policy: Reliable + Value: /initialpose + - Class: rviz_default_plugins/SetGoal + Topic: + Depth: 5 + Durability Policy: Volatile + History Policy: Keep Last + Reliability Policy: Reliable + Value: /goal_pose + - Class: rviz_default_plugins/PublishPoint + Single click: true + Topic: + Depth: 5 + Durability Policy: Volatile + History Policy: Keep Last + Reliability Policy: Reliable + Value: /clicked_point + Transformation: + Current: + Class: rviz_default_plugins/TF + Value: true + Views: + Current: + Class: rviz_default_plugins/Orbit + Distance: 8.339649200439453 + Enable Stereo Rendering: + Stereo Eye Separation: 0.05999999865889549 + Stereo Focal Distance: 1 + Swap Stereo Eyes: false + Value: false + Focal Point: + X: 0 + Y: 0 + Z: 0 + Focal Shape Fixed Size: true + Focal Shape Size: 0.05000000074505806 + Invert Z Axis: false + Name: Current View + Near Clip Distance: 0.009999999776482582 + Pitch: 0.9703981876373291 + Target Frame: + Value: Orbit (rviz) + Yaw: 4.0604071617126465 + Saved: + - Class: rviz_default_plugins/Orbit + Distance: 10 + Enable Stereo Rendering: + Stereo Eye Separation: 0.05999999865889549 + Stereo Focal Distance: 1 + Swap Stereo Eyes: false + Value: false + Focal Point: + X: 0 + Y: 0 + Z: 0 + Focal Shape Fixed Size: true + Focal Shape Size: 0.05000000074505806 + Invert Z Axis: false + Name: Orbit + Near Clip Distance: 0.009999999776482582 + Pitch: 0.785398006439209 + Target Frame: + Value: Orbit (rviz) + Yaw: 0.785398006439209 +Window Geometry: + Displays: + collapsed: false + Height: 846 + Hide Left Dock: false + Hide Right Dock: false + QMainWindow State: 000000ff00000000fd000000040000000000000156000002b0fc0200000008fb0000001200530065006c0065006300740069006f006e00000001e10000009b0000005c00fffffffb0000001e0054006f006f006c002000500072006f007000650072007400690065007302000001ed000001df00000185000000a3fb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb000000100044006900730070006c006100790073010000003d000002b0000000c900fffffffb0000002000730065006c0065006300740069006f006e00200062007500660066006500720200000138000000aa0000023a00000294fb00000014005700690064006500530074006500720065006f02000000e6000000d2000003ee0000030bfb0000000c004b0069006e0065006300740200000186000001060000030c00000261000000010000010f000002b0fc0200000003fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000000a00560069006500770073010000003d000002b0000000a400fffffffb0000001200530065006c0065006300740069006f006e010000025a000000b200000000000000000000000200000490000000a9fc0100000001fb0000000a00560069006500770073030000004e00000080000002e10000019700000003000004b00000003efc0100000002fb0000000800540069006d00650100000000000004b0000002fb00fffffffb0000000800540069006d006501000000000000045000000000000000000000023f000002b000000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000 + Selection: + collapsed: false + Time: + collapsed: false + Tool Properties: + collapsed: false + Views: + collapsed: false + Width: 1200 + X: 697 + Y: 27 diff --git a/config/rrtconfig.rviz b/config/rrt3Dconfig.rviz similarity index 93% rename from config/rrtconfig.rviz rename to config/rrt3Dconfig.rviz index 4df77ec..60a18d4 100644 --- a/config/rrtconfig.rviz +++ b/config/rrt3Dconfig.rviz @@ -4,11 +4,6 @@ Panels: Name: Displays Property Tree Widget: Expanded: - - /Global Options1 - - /Status1 - - /MarkerArray1 - - /Path1 - - /Path1/Status1 - /Path1/Topic1 Splitter Ratio: 0.5 Tree Height: 549 @@ -63,10 +58,22 @@ Visualization Manager: Reliability Policy: Reliable Value: /rrt_markers Value: true + - Class: rviz_default_plugins/MarkerArray + Enabled: true + Name: MarkerArray + Namespaces: + rrt_markers: true + Topic: + Depth: 5 + Durability Policy: Volatile + History Policy: Keep Last + Reliability Policy: Reliable + Value: /obstacle_markers_3D + Value: true - Alpha: 1 Buffer Length: 1 Class: rviz_default_plugins/Path - Color: 25; 255; 0 + Color: 224; 27; 36 Enabled: true Head Diameter: 0.30000001192092896 Head Length: 0.20000000298023224 @@ -152,10 +159,10 @@ Visualization Manager: Invert Z Axis: false Name: Current View Near Clip Distance: 0.009999999776482582 - Pitch: 0.44539856910705566 + Pitch: 0.4953986406326294 Target Frame: Value: Orbit (rviz) - Yaw: 3.6854054927825928 + Yaw: 4.685409069061279 Saved: - Class: rviz_default_plugins/Orbit Distance: 10 diff --git a/launch/rrt2Dlaunch.xml b/launch/rrt2Dlaunch.xml index b96959a..14d153b 100644 --- a/launch/rrt2Dlaunch.xml +++ b/launch/rrt2Dlaunch.xml @@ -7,9 +7,9 @@ - + - + diff --git a/launch/rrt3Dlaunch.xml b/launch/rrt3Dlaunch.xml index e5e0e37..0fff2ef 100644 --- a/launch/rrt3Dlaunch.xml +++ b/launch/rrt3Dlaunch.xml @@ -8,11 +8,11 @@ - + - + - + diff --git a/ros2rrt/obstacle_pub_3D.py b/ros2rrt/obstacle_pub_3D.py new file mode 100644 index 0000000..993cb1a --- /dev/null +++ b/ros2rrt/obstacle_pub_3D.py @@ -0,0 +1,37 @@ +import rclpy +from rclpy.node import Node +from visualization_msgs.msg import Marker, MarkerArray +from .submodules.Marker import Sphere, Box, create_marker + + +class ObstaclePublisher2D(Node): + + def __init__(self): + super().__init__('obstacle_publisher_3D') + self.publisher = self.create_publisher( + MarkerArray, 'obstacle_markers_3D', 10) + self.timer = self.create_timer( + 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] + + def publish_obstacle(self): + marker_array = MarkerArray() + for obstacle in self.obstacle_list: + if isinstance(obstacle, Sphere): + marker = create_marker(Marker.SPHERE, self.obstacle_list.index( + obstacle), [1.0, 0.0, 0.0, 1.0], [obstacle.radius * 2, obstacle.radius * 2, obstacle.radius * 2], [obstacle.x, obstacle.y, obstacle.z]) + marker_array.markers.append(marker) + elif isinstance(obstacle, Box): + marker = create_marker(Marker.CUBE, self.obstacle_list.index( + obstacle), [1.0, 0.0, 0.0, 1.0], [obstacle.width, obstacle.height, obstacle.depth], [obstacle.x, obstacle.y, obstacle.z]) + marker_array.markers.append(marker) + self.publisher.publish(marker_array) + + +def main(args=None): + rclpy.init(args=args) + obstacle_publisher = ObstaclePublisher2D() + rclpy.spin(obstacle_publisher) + rclpy.shutdown() diff --git a/ros2rrt/rrt2D.py b/ros2rrt/rrt2D.py index 063f9e8..f57bdbe 100644 --- a/ros2rrt/rrt2D.py +++ b/ros2rrt/rrt2D.py @@ -21,7 +21,7 @@ def __init__(self): ('obstacle_sub_mode', True), ('step_size', 0.05), ('node_limit', 5000), - ('goal_tolerance', 0.5), + ('goal_tolerance', 0.2), ('wall_confidence', 50) ] self.declare_parameters(namespace='', parameters=parameters) diff --git a/ros2rrt/rrt3D.py b/ros2rrt/rrt3D.py index fb56cef..c09b9de 100644 --- a/ros2rrt/rrt3D.py +++ b/ros2rrt/rrt3D.py @@ -39,7 +39,7 @@ def __init__(self): if self.obstacle_sub_mode: self.obstacle_list = [] self.obstacle_subscription = self.create_subscription( - MarkerArray, 'obstacle_markers', self.obstacle_callback, 10) + MarkerArray, 'obstacle_markers_3D', self.obstacle_callback, 10) self.start_node = TreeNode(self.start_position, None) self.node_list = [self.start_node] self.timer = self.create_timer(1.0, self.timer_callback) @@ -129,7 +129,7 @@ def obstacle_callback(self, msg): """ self.obstacle_list = [] for marker in msg.markers: - if marker.type == Marker.CYLINDER: + if marker.type == Marker.SPHERE: self.obstacle_list.append(Sphere( marker.pose.position.x, marker.pose.position.y, marker.pose.position.z, marker.scale.x/2)) elif marker.type == Marker.CUBE: diff --git a/setup.py b/setup.py index 4ff2e3a..11c6588 100644 --- a/setup.py +++ b/setup.py @@ -29,6 +29,7 @@ 'map_frame_pub = ros2rrt.map_frame_pub:main', 'occupancy_pub = ros2rrt.occupancy_pub:main', 'obstacle_pub_2D = ros2rrt.obstacle_pub_2D:main', + 'obstacle_pub_3D = ros2rrt.obstacle_pub_3D:main', ], }, )