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: | 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 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 @@ - + - + diff --git a/src/pycram/datastructures/enums.py b/src/pycram/datastructures/enums.py index b353b5319..d2dfb68b2 100644 --- a/src/pycram/datastructures/enums.py +++ b/src/pycram/datastructures/enums.py @@ -89,3 +89,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..6e18fedbe 100644 --- a/src/pycram/ros/viz_marker_publisher.py +++ b/src/pycram/ros/viz_marker_publisher.py @@ -1,16 +1,17 @@ import atexit import threading import time +from typing import List, Optional, Tuple +import rospy from geometry_msgs.msg import Vector3 from std_msgs.msg import ColorRGBA +from visualization_msgs.msg import Marker, MarkerArray -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 ..datastructures.dataclasses import BoxVisualShape, CylinderVisualShape, MeshVisualShape, SphereVisualShape +from ..datastructures.pose import Pose, Transform +from ..designator import ObjectDesignatorDescription +from ..world import World class VizMarkerPublisher: @@ -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: 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. + + :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.interval = interval + self.log_message = 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 + + :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.start_time = time.time() + thread = threading.Thread(target=self._publish, args=(pose, bw_object, name, color)) + thread.start() + rospy.loginfo(self.log_message) + thread.join() + + def _publish(self, pose: Pose, bw_object: Optional[ObjectDesignatorDescription] = None, name: Optional[str] = None, + color: Optional[List] = None): + """ + 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 bw_object is None: + self._publish_pose(name=name, pose=pose, color=color) + else: + self._publish_object(name=name, pose=pose, bw_object=bw_object) + + rospy.sleep(self.interval) + + def _publish_pose(self, name: str, pose: Pose, color: Optional[List] = 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._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) + self.log_message = f"Pose '{name}' published" + + 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: ObjectDesignatorDescription 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.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: 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 + + :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 = marker_pose.pose + 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: 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 + for marker in self.marker_array.markers: + if marker.id == marker_id: + # Update successful + marker.pose = new_pose + self.log_message = f"Marker '{marker.ns}' 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: Optional[ObjectDesignatorDescription] = None, name: Optional[str] = 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) + + rospy.loginfo(f"Removed Marker '{name}'") + + 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) + + rospy.loginfo('Removed all markers')