diff --git a/resources/objects/plane_colored.urdf b/resources/objects/plane_colored.urdf new file mode 100644 index 000000000..2eef8d27f --- /dev/null +++ b/resources/objects/plane_colored.urdf @@ -0,0 +1,26 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/src/pycram/ros_utils/viz_marker_publisher.py b/src/pycram/ros_utils/viz_marker_publisher.py index 613bfbebb..5a64a71b4 100644 --- a/src/pycram/ros_utils/viz_marker_publisher.py +++ b/src/pycram/ros_utils/viz_marker_publisher.py @@ -8,7 +8,8 @@ from std_msgs.msg import ColorRGBA from visualization_msgs.msg import Marker, MarkerArray -from ..datastructures.dataclasses import BoxVisualShape, CylinderVisualShape, MeshVisualShape, SphereVisualShape +from pycrap import Floor +from ..datastructures.dataclasses import BoxVisualShape, CylinderVisualShape, MeshVisualShape, SphereVisualShape, Color from ..datastructures.pose import Pose, Transform from ..datastructures.world import World from ..designator import ObjectDesignatorDescription @@ -16,6 +17,7 @@ from ..ros.logging import loginfo, logwarn, logerr from ..ros.publisher import create_publisher from ..ros.ros_tools import sleep +from ..world_concepts.world_object import Object class VizMarkerPublisher: @@ -23,7 +25,7 @@ class VizMarkerPublisher: Publishes an Array of visualization marker which represent the situation in the World """ - def __init__(self, topic_name="/pycram/viz_marker", interval=0.1): + def __init__(self, topic_name="/pycram/viz_marker", interval=0.1, spawn_floor = True): """ The Publisher creates an Array of Visualization marker with a Marker for each link of each Object in the World. This Array is published with a rate of interval. @@ -34,6 +36,9 @@ def __init__(self, topic_name="/pycram/viz_marker", interval=0.1): self.topic_name = topic_name self.interval = interval + if spawn_floor: + _ = Object("plane_colored", Floor, f"plane_colored.urdf") + self.pub = create_publisher(self.topic_name, MarkerArray, queue_size=10) self.thread = threading.Thread(target=self._publish)