From bdb6fdeadaa57b3cb94aa8557087933348414d78 Mon Sep 17 00:00:00 2001 From: OrganomagnesiumHalide <99297401+OrganomagnesiumHalide@users.noreply.github.com> Date: Sun, 4 Dec 2022 21:55:30 -0800 Subject: [PATCH 1/3] Fixed bug 690 --- mil_common/mil_missions/mil_missions_core/exceptions.py | 5 ++++- 1 file changed, 4 insertions(+), 1 deletion(-) diff --git a/mil_common/mil_missions/mil_missions_core/exceptions.py b/mil_common/mil_missions/mil_missions_core/exceptions.py index 108793738..bde0b2a9b 100644 --- a/mil_common/mil_missions/mil_missions_core/exceptions.py +++ b/mil_common/mil_missions/mil_missions_core/exceptions.py @@ -11,7 +11,10 @@ class MissionException(Exception): parameters (Dict[Any, Any]): ??? """ - def __init__(self, message, parameters={}): + def __init__(self, message, parameters=None): + if parameters is None: + parameters = {} + self.message = message self.parameters = parameters super(Exception, self).__init__(message) From a3ba71aa3d1b4da71915f4fa4c00ddfd68c98633 Mon Sep 17 00:00:00 2001 From: OrganomagnesiumHalide <99297401+OrganomagnesiumHalide@users.noreply.github.com> Date: Sun, 4 Dec 2022 22:07:32 -0800 Subject: [PATCH 2/3] Fixed issues #639 and #690 --- .../sub8_perception/nodes/buoy_finder.py | 7 +- .../sub8_perception/sub8_vision_tools/rviz.py | 87 ------------------- .../mil_missions_core/exceptions.py | 2 +- 3 files changed, 4 insertions(+), 92 deletions(-) delete mode 100644 SubjuGator/perception/sub8_perception/sub8_vision_tools/rviz.py diff --git a/SubjuGator/perception/sub8_perception/nodes/buoy_finder.py b/SubjuGator/perception/sub8_perception/nodes/buoy_finder.py index 034b53955..e32bf5eaf 100755 --- a/SubjuGator/perception/sub8_perception/nodes/buoy_finder.py +++ b/SubjuGator/perception/sub8_perception/nodes/buoy_finder.py @@ -20,8 +20,8 @@ VisionRequest2DResponse, VisionRequestResponse, ) -from sub8_vision_tools import MultiObservation, rviz - +from sub8_vision_tools import MultiObservation +from rviz_helpers import (Image_Publisher, Image_Publisher) class Buoy: """ @@ -156,7 +156,6 @@ def __init__(self): self.image_sub = Image_Subscriber(camera, self.image_cb) if self.debug_ros: - self.rviz = rviz.RvizVisualizer(topic="~markers") self.mask_pub = Image_Publisher("~mask_image") rospy.Timer(rospy.Duration(1), self.print_status) @@ -389,7 +388,7 @@ def find_single_buoy(self, buoy_type): buoy.est = self.multi_obs.lst_sqr_intersection(observations, pose_pairs) buoy.status = "Pose found" if self.debug_ros: - self.rviz.draw_sphere( + draw_sphere( buoy.est, color=buoy.draw_colors, scaling=(0.2286, 0.2286, 0.2286), diff --git a/SubjuGator/perception/sub8_perception/sub8_vision_tools/rviz.py b/SubjuGator/perception/sub8_perception/sub8_vision_tools/rviz.py deleted file mode 100644 index cc3f4c49e..000000000 --- a/SubjuGator/perception/sub8_perception/sub8_vision_tools/rviz.py +++ /dev/null @@ -1,87 +0,0 @@ -#!/usr/bin/env python3 - -import mil_ros_tools -import numpy as np -import rospy -import visualization_msgs.msg as visualization_msgs -from geometry_msgs.msg import Point, Pose, Vector3 -from std_msgs.msg import ColorRGBA - - -class RvizVisualizer: - - """Cute tool for drawing both depth and height-from-bottom in RVIZ""" - - def __init__(self, topic="visualization/markers"): - self.rviz_pub = rospy.Publisher(topic, visualization_msgs.Marker, queue_size=3) - - def draw_sphere( - self, position, color, scaling=(0.11, 0.11, 0.11), _id=4, frame="/front_stereo" - ): - pose = Pose( - position=mil_ros_tools.numpy_to_point(position), - orientation=mil_ros_tools.numpy_to_quaternion([0.0, 0.0, 0.0, 1.0]), - ) - - marker = visualization_msgs.Marker( - ns="sub", - id=_id, - header=mil_ros_tools.make_header(frame=frame), - type=visualization_msgs.Marker.SPHERE, - action=visualization_msgs.Marker.ADD, - pose=pose, - color=ColorRGBA(*color), - scale=Vector3(*scaling), - lifetime=rospy.Duration(), - ) - self.rviz_pub.publish(marker) - - def draw_ray_3d( - self, - pix_coords, - camera_model, - color, - frame="/front_stereo", - _id=100, - length=35, - timestamp=None, - ): - """Handle range data grabbed from dvl""" - # future: should be /base_link/dvl, no? - marker = self.make_ray( - base=np.array([0.0, 0.0, 0.0]), - direction=np.array(camera_model.projectPixelTo3dRay(pix_coords)), - length=length, - color=color, - frame=frame, - timestamp=timestamp, - _id=_id, - ) - - self.rviz_pub.publish(marker) - - def make_ray( - self, - base, - direction, - length, - color, - frame="/base_link", - _id=100, - timestamp=None, - **kwargs - ): - """Handle the frustration that Rviz cylinders are designated by their center, not base""" - marker = visualization_msgs.Marker( - ns="sub", - id=_id, - header=mil_ros_tools.make_header(frame=frame, stamp=timestamp), - type=visualization_msgs.Marker.LINE_STRIP, - action=visualization_msgs.Marker.ADD, - color=ColorRGBA(*color), - scale=Vector3(0.05, 0.05, 0.05), - points=map(lambda o: Point(*o), [base, direction * length]), - lifetime=rospy.Duration(), - **kwargs - ) - return marker diff --git a/mil_common/mil_missions/mil_missions_core/exceptions.py b/mil_common/mil_missions/mil_missions_core/exceptions.py index bde0b2a9b..1121ae41d 100644 --- a/mil_common/mil_missions/mil_missions_core/exceptions.py +++ b/mil_common/mil_missions/mil_missions_core/exceptions.py @@ -14,7 +14,7 @@ class MissionException(Exception): def __init__(self, message, parameters=None): if parameters is None: parameters = {} - + self.message = message self.parameters = parameters super(Exception, self).__init__(message) From 0571ca5a12af7bd7211ae45ac412b544bd567975 Mon Sep 17 00:00:00 2001 From: Cameron Brown Date: Tue, 5 Mar 2024 19:51:45 -0500 Subject: [PATCH 3/3] Remove unrelated changes --- .../nodes/buoy_finder.py | 21 +++++++++---------- 1 file changed, 10 insertions(+), 11 deletions(-) diff --git a/SubjuGator/perception/subjugator_perception/nodes/buoy_finder.py b/SubjuGator/perception/subjugator_perception/nodes/buoy_finder.py index 054b59f7b..21802b82a 100755 --- a/SubjuGator/perception/subjugator_perception/nodes/buoy_finder.py +++ b/SubjuGator/perception/subjugator_perception/nodes/buoy_finder.py @@ -9,10 +9,9 @@ import tf from geometry_msgs.msg import Point, Pose, Pose2D, PoseStamped from image_geometry import PinholeCameraModel -from mil_ros_tools import Image_Subscriber, rosmsg_to_numpy +from mil_ros_tools import Image_Publisher, Image_Subscriber, rosmsg_to_numpy from mil_vision_tools import CircleFinder, Threshold from nav_msgs.msg import Odometry -from rviz_helpers import Image_Publisher from std_msgs.msg import Header from std_srvs.srv import SetBool, SetBoolResponse from subjugator_msgs.srv import ( @@ -21,7 +20,7 @@ VisionRequest2DResponse, VisionRequestResponse, ) -from subjugator_vision_tools import MultiObservation +from subjugator_vision_tools import MultiObservation, rviz class Buoy: @@ -158,6 +157,7 @@ def __init__(self): self.image_sub = Image_Subscriber(camera, self.image_cb) if self.debug_ros: + self.rviz = rviz.RvizVisualizer(topic="~markers") self.mask_pub = Image_Publisher("~mask_image") rospy.Timer(rospy.Duration(1), self.print_status) @@ -398,14 +398,13 @@ def find_single_buoy(self, buoy_type): buoy.est = self.multi_obs.lst_sqr_intersection(observations, pose_pairs) buoy.status = "Pose found" if self.debug_ros: - # draw_sphere( - # buoy.est, - # color=buoy.draw_colors, - # scaling=(0.2286, 0.2286, 0.2286), - # frame="map", - # _id=buoy.visual_id, - # ) - pass + self.rviz.draw_sphere( + buoy.est, + color=buoy.draw_colors, + scaling=(0.2286, 0.2286, 0.2286), + frame="map", + _id=buoy.visual_id, + ) else: buoy.status = f"{len(observations)} observations" return center, radius