From 31e64eccaa99f53c13398322927437d2b9beb32f Mon Sep 17 00:00:00 2001 From: Vanessa Hassouna Date: Tue, 16 Apr 2024 16:24:48 +0200 Subject: [PATCH 1/6] [viz_marker_pub] added new manual marker publisher to just publish one pose or object --- src/pycram/datastructures/enums.py | 5 + src/pycram/ros/viz_marker_publisher.py | 214 ++++++++++++++++++++++++- 2 files changed, 218 insertions(+), 1 deletion(-) diff --git a/src/pycram/datastructures/enums.py b/src/pycram/datastructures/enums.py index a6867246f..f8f6ae7aa 100644 --- a/src/pycram/datastructures/enums.py +++ b/src/pycram/datastructures/enums.py @@ -86,3 +86,8 @@ class WorldMode(Enum): """ GUI = "GUI" DIRECT = "DIRECT" + +class AxisIdentifier(Enum): + X = (1, 0, 0) + Y = (0, 1, 0) + Z = (0, 0, 1) \ No newline at end of file diff --git a/src/pycram/ros/viz_marker_publisher.py b/src/pycram/ros/viz_marker_publisher.py index fc33f5f6e..148c77116 100644 --- a/src/pycram/ros/viz_marker_publisher.py +++ b/src/pycram/ros/viz_marker_publisher.py @@ -2,9 +2,10 @@ import threading import time -from geometry_msgs.msg import Vector3 +from geometry_msgs.msg import Vector3, Pose, Quaternion, Point from std_msgs.msg import ColorRGBA +from pycram.datastructures.enums import AxisIdentifier from pycram.world import World from visualization_msgs.msg import MarkerArray, Marker import rospy @@ -107,3 +108,214 @@ def _stop_publishing(self) -> None: """ self.kill_event.set() self.thread.join() + + +class ManualMarkerPublisher: + """ + Class to manually add and remove marker of objects and poses. + """ + + def __init__(self, topic_name='/pycram/manual_marker', interval=0.1): + """ + The Publisher creates an Array of Visualization marker with a marker for a pose or object. + This Array is published with a rate of interval. + + :param topic_name: Name of the marker topic + :param interval: Interval at which the marker should be published + """ + self.start_time = None + self.marker_array_pub = rospy.Publisher(topic_name, MarkerArray, queue_size=10) + + self.marker_array = MarkerArray() + self.marker_overview = {} + self.current_id = 0 + + self.bw_object = None + self.color = None + self.pose = None + self.name = None + self.interval = interval + + def publish(self, pose: Pose, color=None, bw_object=None, name=None): + """ + Publish a pose or an object into the MarkerArray. + Priorities to add an object if possible + + :param pose: Pose of the marker + :param color: Color of the marker if no object is given + :param bw_object: Object to add as a marker + :param name: Name of the marker + """ + + if color is None: + color = [1, 0, 1, 1] + self.color = color + self.name = name + self.pose = pose + self.bw_object = bw_object + + self.start_time = time.time() + thread = threading.Thread(target=self._publish) + thread.start() + thread.join() + + def _publish(self): + """ + Publish the marker into the MarkerArray + """ + stop_thread = False + duration = 2 + + while not stop_thread: + if time.time() - self.start_time > duration: + stop_thread = True + if self.bw_object is None: + self._publish_pose(name=self.name, pose=self.pose, color=self.color) + else: + self._publish_object(name=self.name, pose=self.pose, bw_object=self.bw_object) + + rospy.sleep(self.interval) + + def _publish_pose(self, name, pose, color=None): + """ + Publish a Pose as a marker + + :param name: Name of the marker + :param pose: Pose of the marker + :param color: Color of the marker + """ + + if name is None: + name = 'pose_marker' + + if name in self.marker_overview.keys(): + self._update_marker(self.marker_overview[name], new_pose=pose) + return + + color_rgba = ColorRGBA(*color) + self.create_marker(name=name, marker_type=Marker.ARROW, marker_pose=pose, + marker_scales=(0.05, 0.05, 0.05), color_rgba=color_rgba) + self.marker_array_pub.publish(self.marker_array) + rospy.logwarn("Marker array published") + + def _publish_object(self, name, pose, bw_object): + """ + Publish an Object as a marker + + :param name: Name of the marker + :param pose: Pose of the marker + :param bw_object: Bulletworld object for the marker + """ + + bw_real = bw_object.resolve() + + if name is None: + name = bw_real.name + + if name in self.marker_overview.keys(): + self._update_marker(self.marker_overview[name], new_pose=pose) + return + + path = bw_real.bullet_world_object.urdf_object.links[0].visual.geometry.filename + + self._make_marker_array(name=name, marker_type=Marker.MESH_RESOURCE, marker_pose=pose, + path_to_resource=path) + + self.marker_array_pub.publish(self.marker_array) + + def _make_marker_array(self, name, marker_type, marker_pose: Pose, marker_scales=(1.0, 1.0, 1.0), + color_rgba=ColorRGBA(*[1.0, 1.0, 1.0, 1.0]), path_to_resource=None): + """ + Create a Marker and add it to the MarkerArray + + :param name: Name of the Marker + :param marker_type: Type of the marker to create + :param marker_pose: Pose of the marker + :param marker_scales: individual scaling of the markers axes + :param color_rgba: Color of the marker as RGBA + :param path_to_resource: Path to the resource of a Bulletworld object + """ + + frame_id = marker_pose.header.frame_id + new_marker = Marker() + new_marker.id = self.current_id + new_marker.header.frame_id = frame_id + new_marker.ns = name + new_marker.header.stamp = rospy.Time.now() + new_marker.type = marker_type + new_marker.action = Marker.ADD + new_marker.pose.position.x = marker_pose.pose.position.x + new_marker.pose.position.y = marker_pose.pose.position.y + new_marker.pose.position.z = marker_pose.pose.position.z + new_marker.pose.orientation = Quaternion(0.0, 0.0, 0.0, 1.0) + new_marker.scale.x = marker_scales[0] + new_marker.scale.y = marker_scales[1] + new_marker.scale.z = marker_scales[2] + new_marker.color.a = color_rgba.a + new_marker.color.r = color_rgba.r + new_marker.color.g = color_rgba.g + new_marker.color.b = color_rgba.b + + if path_to_resource is not None: + new_marker.mesh_resource = 'file://' + path_to_resource + + self.marker_array.markers.append(new_marker) + self.marker_overview[name] = new_marker.id + self.current_id += 1 + + def _update_marker(self, marker_id, new_pose): + """ + Update an existing marker to a new pose + + :param marker_id: id of the marker that should be updated + :param new_pose: Pose where the updated marker is set + """ + + # Find the marker with the specified ID + for marker in self.marker_array.markers: + if marker.id == marker_id: + # Update successful + marker.pose = new_pose + rospy.logdebug(f"Marker {marker_id} updated") + self.marker_array_pub.publish(self.marker_array) + return True + + # Update was not successful + rospy.logwarn(f"Marker {marker_id} not found for update") + return False + + def remove_marker(self, bw_object=None, name=None): + """ + Remove a marker by object or name + + :param bw_object: Object which marker should be removed + :param name: Name of object that should be removed + """ + + if bw_object is not None: + bw_real = bw_object.resolve() + name = bw_real.name + + if name is None: + rospy.logerr('No name for object given, cannot remove marker') + return + + marker_id = self.marker_overview.pop(name) + + for marker in self.marker_array.markers: + if marker.id == marker_id: + marker.action = Marker.DELETE + + self.marker_array_pub.publish(self.marker_array) + self.marker_array.markers.pop(marker_id) + self.marker_array_pub.publish(self.marker_array) + + def clear_all_marker(self): + """ + Clear all existing markers + """ + for marker in self.marker_array.markers: + marker.action = Marker.DELETE + + self.marker_overview = {} + self.marker_array_pub.publish(self.marker_array) From 0e2f6930c9f079c5e259823b833fb27f02939051 Mon Sep 17 00:00:00 2001 From: kecks Date: Fri, 26 Apr 2024 13:30:15 +0200 Subject: [PATCH 2/6] [manual_viz_marker] applied requested changes --- src/pycram/ros/viz_marker_publisher.py | 81 +++++++++++++------------- 1 file changed, 40 insertions(+), 41 deletions(-) diff --git a/src/pycram/ros/viz_marker_publisher.py b/src/pycram/ros/viz_marker_publisher.py index 148c77116..6b34d0202 100644 --- a/src/pycram/ros/viz_marker_publisher.py +++ b/src/pycram/ros/viz_marker_publisher.py @@ -1,17 +1,16 @@ import atexit import threading import time +from typing import List, Optional, Tuple -from geometry_msgs.msg import Vector3, Pose, Quaternion, Point -from std_msgs.msg import ColorRGBA - -from pycram.datastructures.enums import AxisIdentifier -from pycram.world import World -from visualization_msgs.msg import MarkerArray, Marker import rospy - -from pycram.datastructures.pose import Transform -from pycram.datastructures.dataclasses import MeshVisualShape, CylinderVisualShape, BoxVisualShape, SphereVisualShape +from geometry_msgs.msg import Vector3 +from pycram.datastructures.dataclasses import BoxVisualShape, CylinderVisualShape, MeshVisualShape, SphereVisualShape +from pycram.datastructures.pose import Pose, Transform +from pycram.designator import ObjectDesignatorDescription +from pycram.world import World +from std_msgs.msg import ColorRGBA +from visualization_msgs.msg import Marker, MarkerArray class VizMarkerPublisher: @@ -115,7 +114,7 @@ class ManualMarkerPublisher: Class to manually add and remove marker of objects and poses. """ - def __init__(self, topic_name='/pycram/manual_marker', interval=0.1): + def __init__(self, topic_name: str = '/pycram/manual_marker', interval: float = 0.1): """ The Publisher creates an Array of Visualization marker with a marker for a pose or object. This Array is published with a rate of interval. @@ -130,13 +129,11 @@ def __init__(self, topic_name='/pycram/manual_marker', interval=0.1): self.marker_overview = {} self.current_id = 0 - self.bw_object = None - self.color = None - self.pose = None - self.name = None self.interval = interval + self.log_message = None - def publish(self, pose: Pose, color=None, bw_object=None, name=None): + def publish(self, pose: Pose, color: Optional[List] = None, bw_object: Optional[ObjectDesignatorDescription] = None, + name: Optional[str] = None): """ Publish a pose or an object into the MarkerArray. Priorities to add an object if possible @@ -149,17 +146,15 @@ def publish(self, pose: Pose, color=None, bw_object=None, name=None): if color is None: color = [1, 0, 1, 1] - self.color = color - self.name = name - self.pose = pose - self.bw_object = bw_object self.start_time = time.time() - thread = threading.Thread(target=self._publish) + thread = threading.Thread(target=self._publish, args=(pose, bw_object, name, color)) thread.start() + rospy.loginfo(self.log_message) thread.join() - def _publish(self): + def _publish(self, pose: Pose, bw_object: Optional[ObjectDesignatorDescription] = None, name: Optional[str] = None, + color: Optional[List] = None): """ Publish the marker into the MarkerArray """ @@ -169,14 +164,14 @@ def _publish(self): while not stop_thread: if time.time() - self.start_time > duration: stop_thread = True - if self.bw_object is None: - self._publish_pose(name=self.name, pose=self.pose, color=self.color) + if bw_object is None: + self._publish_pose(name=name, pose=pose, color=color) else: - self._publish_object(name=self.name, pose=self.pose, bw_object=self.bw_object) + self._publish_object(name=name, pose=pose, bw_object=bw_object) rospy.sleep(self.interval) - def _publish_pose(self, name, pose, color=None): + def _publish_pose(self, name: str, pose: Pose, color: Optional[List] = None): """ Publish a Pose as a marker @@ -193,18 +188,18 @@ def _publish_pose(self, name, pose, color=None): return color_rgba = ColorRGBA(*color) - self.create_marker(name=name, marker_type=Marker.ARROW, marker_pose=pose, - marker_scales=(0.05, 0.05, 0.05), color_rgba=color_rgba) + self._make_marker_array(name=name, marker_type=Marker.ARROW, marker_pose=pose, + marker_scales=(0.05, 0.05, 0.05), color_rgba=color_rgba) self.marker_array_pub.publish(self.marker_array) - rospy.logwarn("Marker array published") + self.log_message = f"Pose '{name}' published" - def _publish_object(self, name, pose, bw_object): + def _publish_object(self, name: Optional[str], pose: Pose, bw_object: ObjectDesignatorDescription): """ Publish an Object as a marker :param name: Name of the marker :param pose: Pose of the marker - :param bw_object: Bulletworld object for the marker + :param bw_object: ObjectDesignatorDescription for the marker """ bw_real = bw_object.resolve() @@ -216,15 +211,17 @@ def _publish_object(self, name, pose, bw_object): self._update_marker(self.marker_overview[name], new_pose=pose) return - path = bw_real.bullet_world_object.urdf_object.links[0].visual.geometry.filename + path = bw_real.world_object.root_link.geometry.file_name self._make_marker_array(name=name, marker_type=Marker.MESH_RESOURCE, marker_pose=pose, path_to_resource=path) self.marker_array_pub.publish(self.marker_array) + self.log_message = f"Object '{name}' published" - def _make_marker_array(self, name, marker_type, marker_pose: Pose, marker_scales=(1.0, 1.0, 1.0), - color_rgba=ColorRGBA(*[1.0, 1.0, 1.0, 1.0]), path_to_resource=None): + def _make_marker_array(self, name, marker_type: int, marker_pose: Pose, marker_scales: Tuple = (1.0, 1.0, 1.0), + color_rgba: ColorRGBA = ColorRGBA(*[1.0, 1.0, 1.0, 1.0]), + path_to_resource: Optional[str] = None): """ Create a Marker and add it to the MarkerArray @@ -244,10 +241,7 @@ def _make_marker_array(self, name, marker_type, marker_pose: Pose, marker_scales new_marker.header.stamp = rospy.Time.now() new_marker.type = marker_type new_marker.action = Marker.ADD - new_marker.pose.position.x = marker_pose.pose.position.x - new_marker.pose.position.y = marker_pose.pose.position.y - new_marker.pose.position.z = marker_pose.pose.position.z - new_marker.pose.orientation = Quaternion(0.0, 0.0, 0.0, 1.0) + new_marker.pose = marker_pose.pose new_marker.scale.x = marker_scales[0] new_marker.scale.y = marker_scales[1] new_marker.scale.z = marker_scales[2] @@ -263,12 +257,14 @@ def _make_marker_array(self, name, marker_type, marker_pose: Pose, marker_scales self.marker_overview[name] = new_marker.id self.current_id += 1 - def _update_marker(self, marker_id, new_pose): + def _update_marker(self, marker_id: int, new_pose: Pose) -> bool: """ Update an existing marker to a new pose :param marker_id: id of the marker that should be updated :param new_pose: Pose where the updated marker is set + + :return: True if update was successful, False otherwise """ # Find the marker with the specified ID @@ -276,7 +272,7 @@ def _update_marker(self, marker_id, new_pose): if marker.id == marker_id: # Update successful marker.pose = new_pose - rospy.logdebug(f"Marker {marker_id} updated") + self.log_message = f"Marker '{marker.ns}' updated" self.marker_array_pub.publish(self.marker_array) return True @@ -284,7 +280,7 @@ def _update_marker(self, marker_id, new_pose): rospy.logwarn(f"Marker {marker_id} not found for update") return False - def remove_marker(self, bw_object=None, name=None): + def remove_marker(self, bw_object: Optional[ObjectDesignatorDescription] = None, name: Optional[str] = None): """ Remove a marker by object or name @@ -308,7 +304,8 @@ def remove_marker(self, bw_object=None, name=None): self.marker_array_pub.publish(self.marker_array) self.marker_array.markers.pop(marker_id) - self.marker_array_pub.publish(self.marker_array) + + rospy.loginfo(f"Removed Marker '{name}'") def clear_all_marker(self): """ @@ -319,3 +316,5 @@ def clear_all_marker(self): self.marker_overview = {} self.marker_array_pub.publish(self.marker_array) + + rospy.loginfo('Removed all markers') From f56c2ea5d2a738fff8f24298ace781de4100721c Mon Sep 17 00:00:00 2001 From: "dependabot[bot]" <49699333+dependabot[bot]@users.noreply.github.com> Date: Mon, 6 May 2024 20:45:02 +0000 Subject: [PATCH 3/6] Bump jinja2 from 3.1.3 to 3.1.4 in /doc Bumps [jinja2](https://github.com/pallets/jinja) from 3.1.3 to 3.1.4. - [Release notes](https://github.com/pallets/jinja/releases) - [Changelog](https://github.com/pallets/jinja/blob/main/CHANGES.rst) - [Commits](https://github.com/pallets/jinja/compare/3.1.3...3.1.4) --- updated-dependencies: - dependency-name: jinja2 dependency-type: direct:production ... Signed-off-by: dependabot[bot] --- doc/requirements.txt | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/doc/requirements.txt b/doc/requirements.txt index 6e99a79a1..1d73d4587 100644 --- a/doc/requirements.txt +++ b/doc/requirements.txt @@ -6,4 +6,4 @@ sphinx-numfig sphinx-autoapi sphinx-rtd-theme nbsphinx -jinja2==3.1.3 +jinja2==3.1.4 From 102236f60aa24b1c9a2b0d36e79fd7aff5cabd67 Mon Sep 17 00:00:00 2001 From: kecks Date: Mon, 13 May 2024 13:49:32 +0200 Subject: [PATCH 4/6] [pr2_urdf] adjusted values for torso_lift_joint --- resources/pr2.urdf | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/resources/pr2.urdf b/resources/pr2.urdf index 040ef1e45..671407ef6 100644 --- a/resources/pr2.urdf +++ b/resources/pr2.urdf @@ -668,11 +668,11 @@ - + - + From c483509f4e5902130d6d0fa89006b521a8e4950c Mon Sep 17 00:00:00 2001 From: kecks Date: Mon, 13 May 2024 13:58:35 +0200 Subject: [PATCH 5/6] [manual_viz_marker] pycram imports are now relative --- src/pycram/ros/viz_marker_publisher.py | 9 +++++---- 1 file changed, 5 insertions(+), 4 deletions(-) diff --git a/src/pycram/ros/viz_marker_publisher.py b/src/pycram/ros/viz_marker_publisher.py index 6b34d0202..6e18fedbe 100644 --- a/src/pycram/ros/viz_marker_publisher.py +++ b/src/pycram/ros/viz_marker_publisher.py @@ -5,13 +5,14 @@ import rospy from geometry_msgs.msg import Vector3 -from pycram.datastructures.dataclasses import BoxVisualShape, CylinderVisualShape, MeshVisualShape, SphereVisualShape -from pycram.datastructures.pose import Pose, Transform -from pycram.designator import ObjectDesignatorDescription -from pycram.world import World from std_msgs.msg import ColorRGBA from visualization_msgs.msg import Marker, MarkerArray +from ..datastructures.dataclasses import BoxVisualShape, CylinderVisualShape, MeshVisualShape, SphereVisualShape +from ..datastructures.pose import Pose, Transform +from ..designator import ObjectDesignatorDescription +from ..world import World + class VizMarkerPublisher: """ From a4052de08bf0e7823741aa804dcc61417782da04 Mon Sep 17 00:00:00 2001 From: Jonas Dech Date: Tue, 21 May 2024 17:07:19 +0200 Subject: [PATCH 6/6] [ci] Change mv to cp to fix inter-device communictaion --- .github/workflows/new-pycram-ci.yml | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/.github/workflows/new-pycram-ci.yml b/.github/workflows/new-pycram-ci.yml index 1fa0f8839..7ad8b1486 100644 --- a/.github/workflows/new-pycram-ci.yml +++ b/.github/workflows/new-pycram-ci.yml @@ -44,7 +44,7 @@ jobs: rm -rf /opt/ros/overlay_ws/src/pycram/* cd /opt/ros/overlay_ws/src/pycram rm -rf .git .github .gitignore .gitmodules .readthedocs.yaml - mv /__w/${{ github.event.repository.name }}/${{ github.event.repository.name }}/ros/src/pycram /opt/ros/overlay_ws/src + cp -r /__w/${{ github.event.repository.name }}/${{ github.event.repository.name }}/ros/src/pycram /opt/ros/overlay_ws/src - name: Remake workspace & start roscore run: |