[viz_marker_pub] added new manual marker publisher to just publish on… #144
Conversation
| from std_msgs.msg import ColorRGBA | ||
|
|
||
| from pycram.datastructures.enums import AxisIdentifier | ||
| from pycram.world import World |
There was a problem hiding this comment.
Make these relative imports, its cleaner and in line with all other imports.
| self.name = None | ||
| self.interval = interval | ||
|
|
||
| def publish(self, pose: Pose, color=None, bw_object=None, name=None): |
There was a problem hiding this comment.
Typing for optional parameter
| duration = 2 | ||
|
|
||
| while not stop_thread: | ||
| if time.time() - self.start_time > duration: |
There was a problem hiding this comment.
If I understand this correctly, this only publishes for 2 seconds?
|
|
||
| rospy.sleep(self.interval) | ||
|
|
||
| def _publish_pose(self, name, pose, color=None): |
| 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") |
There was a problem hiding this comment.
Is this really a warning, shouldn't this be rospy.loginfo?
| self._update_marker(self.marker_overview[name], new_pose=pose) | ||
| return | ||
|
|
||
| path = bw_real.bullet_world_object.urdf_object.links[0].visual.geometry.filename |
There was a problem hiding this comment.
bullet_world_object was changed to world_object in the AbstractWorld merge.
This will also just work for Objects that are spawned from meshes, meaning Objects that have more than one link will not be displayed correctly.
| 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) |
There was a problem hiding this comment.
Why not use the orientation of the marker_pose?
| self.marker_overview[name] = new_marker.id | ||
| self.current_id += 1 | ||
|
|
||
| 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): |
|
|
||
| self.marker_array_pub.publish(self.marker_array) | ||
| self.marker_array.markers.pop(marker_id) | ||
| self.marker_array_pub.publish(self.marker_array) |
There was a problem hiding this comment.
Why is this published a second time? The marker should already be deleted.
|
Just a general thought: If this should be used in multiple places of the code (for example to show the pose while pick_up) it could make sense to make this a Singelton. |
[manual_viz_marker] applied requested changes
[manual_viz_marker] pycram imports are now relative
[viz_marker_pub] added new manual marker publisher to just publish on…
[viz_marker_pub] added new manual marker publisher to just publish on…
…e pose or object
This class, ManualMarkerPublisher, serves as a tool for manually adding and removing markers for objects and poses.