From 99d5463e80c4500327d2fc3ce4dd8cb62256a70a Mon Sep 17 00:00:00 2001 From: Felix Date: Mon, 11 May 2026 18:55:59 +0200 Subject: [PATCH 1/2] quick fix --- Code/robot_controller2/Util/launch_notes | 36 ++++++++++ .../robot_controller2/Evaluation.py | 16 ++--- .../robot_controller2/GT_Visualizer.py | 50 ++++++++------ Code/robot_controller2/setup.cfg | 2 +- core_algorithm/.idea/core_algorithm.iml | 14 ++++ .../inspectionProfiles/profiles_settings.xml | 6 ++ core_algorithm/.idea/misc.xml | 4 ++ core_algorithm/.idea/modules.xml | 8 +++ core_algorithm/.idea/vcs.xml | 6 ++ core_algorithm/.idea/workspace.xml | 66 +++++++++++++++++++ core_algorithm/README.md | 0 .../inspectionProfiles/profiles_settings.xml | 6 ++ simulate_algorithm/.idea/modules.xml | 8 +++ .../.idea/simulate_algorithm.iml | 8 +++ simulate_algorithm/.idea/vcs.xml | 6 ++ simulate_algorithm/.idea/workspace.xml | 66 +++++++++++++++++++ 16 files changed, 272 insertions(+), 30 deletions(-) create mode 100644 Code/robot_controller2/Util/launch_notes create mode 100644 core_algorithm/.idea/core_algorithm.iml create mode 100644 core_algorithm/.idea/inspectionProfiles/profiles_settings.xml create mode 100644 core_algorithm/.idea/misc.xml create mode 100644 core_algorithm/.idea/modules.xml create mode 100644 core_algorithm/.idea/vcs.xml create mode 100644 core_algorithm/.idea/workspace.xml create mode 100644 core_algorithm/README.md create mode 100644 simulate_algorithm/.idea/inspectionProfiles/profiles_settings.xml create mode 100644 simulate_algorithm/.idea/modules.xml create mode 100644 simulate_algorithm/.idea/simulate_algorithm.iml create mode 100644 simulate_algorithm/.idea/vcs.xml create mode 100644 simulate_algorithm/.idea/workspace.xml diff --git a/Code/robot_controller2/Util/launch_notes b/Code/robot_controller2/Util/launch_notes new file mode 100644 index 0000000..48b4fdb --- /dev/null +++ b/Code/robot_controller2/Util/launch_notes @@ -0,0 +1,36 @@ +launch options: + +LAUNCH FOR PLAN + +cd ~/Master-thesis/Project-Code/ros2_ws +colcon build (bws) +source install/local_setup.bash (sws) change in rosrc +rviz2 -d /home/linux/Master-thesis/Project-Code/ros2_ws/src/interactive_perception/Code/robot_controller2/robot_controller2/resources/config2.rviz +ros2 run motion_specification_action motion_specification_action_server +ros2 run robot_controller2 Visualizer +ros2 run robot_controller2 GT_Visualizer +ros2 run robot_controller2 Evaluation +ros2 run robot_controller2 Reasoner + + +LAUNCH FOR MARKER DETECTION +cd ~/python_venvs/ +source ~/python_venvs/aruco_venv/bin/activate +cd ~/Master-thesis/Project-Code/ros2_ws +bws +source install/setup.bash +ros2 launch aruco_pose_estimation aruco_pose_estimation.launch.py +cd ~/Master-thesis/Project-Code/ros2_ws +source install/setup.bash +ros2 launch kinova_vision kinova_vision.launch.py device:=192.168.1.12 +ros2 run robot_controller2 GT_Visualizer + + +ros2 bag record -o "e4_r1" /corner_position /marker_position /plane_position /tf /tf_static /visualization_marker /ground_truth_corners /ground_truth_centroid /ee_pose /motion_specification/goal /motion_specification/result + + +ros2 bag record -o "e4_r1" \ + /corner_position /marker_position /plane_position /tf /tf_static /visualization_marker /ground_truth_corners /ground_truth_centroid /ee_pose + + + diff --git a/Code/robot_controller2/robot_controller2/Evaluation.py b/Code/robot_controller2/robot_controller2/Evaluation.py index d270e3c..9e7846d 100644 --- a/Code/robot_controller2/robot_controller2/Evaluation.py +++ b/Code/robot_controller2/robot_controller2/Evaluation.py @@ -145,50 +145,50 @@ def __init__(self): self.subscription = self.create_subscription( PoseStamped, '/corner_position', - self.corner_callback, + self.estimated_corners_callback, 10 ) self.subscription = self.create_subscription( PoseStamped, '/plane_position', - self.plane_callback, + self.estimated_pose_callback, 10 ) self.subscription = self.create_subscription( Point, '/ground_truth_corners', - self.point_callback, + self.ground_truth_corners_callback, 10 ) self.subscription = self.create_subscription( PoseStamped, '/ground_truth_centroid', - self.centroid_callback, + self.ground_truth_pose_callback, 10 ) - def corner_callback(self, msg): + def estimated_corners_callback(self, msg): position = msg.pose.position orientation = msg.pose.orientation frame_id = msg.header.frame_id estimated_corners.append([position, frame_id]) - def plane_callback(self, msg): + def estimated_pose_callback(self, msg): position = msg.pose.position orientation = msg.pose.orientation frame_id = msg.header.frame_id estimated_pose.append([position, orientation, frame_id]) - def centroid_callback(self, msg): + def ground_truth_pose_callback(self, msg): position = msg.pose.position orientation = msg.pose.orientation frame_id = msg.header.frame_id ground_truth_pose.append([position, orientation, frame_id]) - def point_callback(self, msg): + def ground_truth_corners_callback(self, msg): position = msg ground_truth_corners.append(position) diff --git a/Code/robot_controller2/robot_controller2/GT_Visualizer.py b/Code/robot_controller2/robot_controller2/GT_Visualizer.py index 6323d68..2ef4a86 100644 --- a/Code/robot_controller2/robot_controller2/GT_Visualizer.py +++ b/Code/robot_controller2/robot_controller2/GT_Visualizer.py @@ -242,6 +242,9 @@ def publish_gt(self): if not ground_truth: return + if len(ground_truth) < 4: + return + #Compute fixed transform only once if not self.ground_truth_computed: all_poses = [] @@ -303,33 +306,38 @@ def publish_gt(self): fixed_marker.action = Marker.ADD fixed_marker.pose.orientation.w = 1.0 - # Hardcoded object dimensions - x_len = 0.6 #0.31 #0.34 - y_len = 0.34 #0.98 #0.6 + # # Hardcoded object dimensions + # x_len = 0.6 #0.31 #0.34 + # y_len = 0.34 #0.98 #0.6 + # + # # Define rectangle corners + # p0 = Point(x=-x_len / 2, y=-y_len / 2, z=0.0) + # p1 = Point(x=x_len / 2, y=-y_len / 2, z=0.0) + # p2 = Point(x=x_len / 2, y=y_len / 2, z=0.0) + # p3 = Point(x=-x_len / 2, y=y_len / 2, z=0.0) + # + # poses = [p0, p1, p2, p3] - # Define rectangle corners - p0 = Point(x=-x_len / 2, y=-y_len / 2, z=0.0) - p1 = Point(x=x_len / 2, y=-y_len / 2, z=0.0) - p2 = Point(x=x_len / 2, y=y_len / 2, z=0.0) - p3 = Point(x=-x_len / 2, y=y_len / 2, z=0.0) + poses = [] - poses = [p0, p1, p2, p3] + for pose in all_poses: + poses.append(Point(x=pose[0], y=pose[1], z=pose[2])) for pose in poses: self.publisher_point.publish(pose) - # Add rectangle as two triangles - fixed_marker.points.extend([p0, p1, p2]) - fixed_marker.points.extend([p0, p2, p3]) - - fixed_marker.scale.x = 1.0 - fixed_marker.scale.y = 1.0 - fixed_marker.scale.z = 1.0 - - fixed_marker.color.r = 1.0 - fixed_marker.color.g = 0.0 - fixed_marker.color.b = 0.5 - fixed_marker.color.a = 0.4 + # # Add rectangle as two triangles + # fixed_marker.points.extend([p0, p1, p2]) + # fixed_marker.points.extend([p0, p2, p3]) + # + # fixed_marker.scale.x = 1.0 + # fixed_marker.scale.y = 1.0 + # fixed_marker.scale.z = 1.0 + # + # fixed_marker.color.r = 1.0 + # fixed_marker.color.g = 0.0 + # fixed_marker.color.b = 0.5 + # fixed_marker.color.a = 0.4 self.ground_truth_computed = True diff --git a/Code/robot_controller2/setup.cfg b/Code/robot_controller2/setup.cfg index 16cf776..e92ada3 100644 --- a/Code/robot_controller2/setup.cfg +++ b/Code/robot_controller2/setup.cfg @@ -1,5 +1,5 @@ [build_scripts] -executable = /usr/bin/env /home/melody/python_venvs/felix_thesis/bin/python3 +executable = /usr/bin/env python3 [develop] script_dir=$base/lib/robot_controller2 [install] diff --git a/core_algorithm/.idea/core_algorithm.iml b/core_algorithm/.idea/core_algorithm.iml new file mode 100644 index 0000000..3258e20 --- /dev/null +++ b/core_algorithm/.idea/core_algorithm.iml @@ -0,0 +1,14 @@ + + + + + + + + + + + + \ No newline at end of file diff --git a/core_algorithm/.idea/inspectionProfiles/profiles_settings.xml b/core_algorithm/.idea/inspectionProfiles/profiles_settings.xml new file mode 100644 index 0000000..105ce2d --- /dev/null +++ b/core_algorithm/.idea/inspectionProfiles/profiles_settings.xml @@ -0,0 +1,6 @@ + + + + \ No newline at end of file diff --git a/core_algorithm/.idea/misc.xml b/core_algorithm/.idea/misc.xml new file mode 100644 index 0000000..ddb403f --- /dev/null +++ b/core_algorithm/.idea/misc.xml @@ -0,0 +1,4 @@ + + + + \ No newline at end of file diff --git a/core_algorithm/.idea/modules.xml b/core_algorithm/.idea/modules.xml new file mode 100644 index 0000000..962a39b --- /dev/null +++ b/core_algorithm/.idea/modules.xml @@ -0,0 +1,8 @@ + + + + + + + + \ No newline at end of file diff --git a/core_algorithm/.idea/vcs.xml b/core_algorithm/.idea/vcs.xml new file mode 100644 index 0000000..6c0b863 --- /dev/null +++ b/core_algorithm/.idea/vcs.xml @@ -0,0 +1,6 @@ + + + + + + \ No newline at end of file diff --git a/core_algorithm/.idea/workspace.xml b/core_algorithm/.idea/workspace.xml new file mode 100644 index 0000000..b1e2322 --- /dev/null +++ b/core_algorithm/.idea/workspace.xml @@ -0,0 +1,66 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + 1778513432647 + + + + + + \ No newline at end of file diff --git a/core_algorithm/README.md b/core_algorithm/README.md new file mode 100644 index 0000000..e69de29 diff --git a/simulate_algorithm/.idea/inspectionProfiles/profiles_settings.xml b/simulate_algorithm/.idea/inspectionProfiles/profiles_settings.xml new file mode 100644 index 0000000..105ce2d --- /dev/null +++ b/simulate_algorithm/.idea/inspectionProfiles/profiles_settings.xml @@ -0,0 +1,6 @@ + + + + \ No newline at end of file diff --git a/simulate_algorithm/.idea/modules.xml b/simulate_algorithm/.idea/modules.xml new file mode 100644 index 0000000..aa47c44 --- /dev/null +++ b/simulate_algorithm/.idea/modules.xml @@ -0,0 +1,8 @@ + + + + + + + + \ No newline at end of file diff --git a/simulate_algorithm/.idea/simulate_algorithm.iml b/simulate_algorithm/.idea/simulate_algorithm.iml new file mode 100644 index 0000000..d0876a7 --- /dev/null +++ b/simulate_algorithm/.idea/simulate_algorithm.iml @@ -0,0 +1,8 @@ + + + + + + + + \ No newline at end of file diff --git a/simulate_algorithm/.idea/vcs.xml b/simulate_algorithm/.idea/vcs.xml new file mode 100644 index 0000000..6c0b863 --- /dev/null +++ b/simulate_algorithm/.idea/vcs.xml @@ -0,0 +1,6 @@ + + + + + + \ No newline at end of file diff --git a/simulate_algorithm/.idea/workspace.xml b/simulate_algorithm/.idea/workspace.xml new file mode 100644 index 0000000..6b35e4f --- /dev/null +++ b/simulate_algorithm/.idea/workspace.xml @@ -0,0 +1,66 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + 1778513621012 + + + + + + \ No newline at end of file From 67c5214d9bfbe9f360dd356896e4f18b7dabb436 Mon Sep 17 00:00:00 2001 From: Felix Date: Tue, 19 May 2026 14:28:05 +0200 Subject: [PATCH 2/2] Working vision --- Code/robot_controller2/.idea/.gitignore | 5 + .../inspectionProfiles/profiles_settings.xml | 6 + Code/robot_controller2/.idea/misc.xml | 7 + Code/robot_controller2/.idea/modules.xml | 8 + .../.idea/robot_controller2.iml | 17 ++ Code/robot_controller2/.idea/vcs.xml | 6 + .../robot_controller2.egg-info/PKG-INFO | 20 ++ .../robot_controller2.egg-info/SOURCES.txt | 28 +++ .../dependency_links.txt | 1 + .../entry_points.txt | 8 + .../robot_controller2.egg-info/requires.txt | 6 + .../robot_controller2.egg-info/top_level.txt | 2 + .../robot_controller2.egg-info/zip-safe | 1 + .../robot_controller2/GT_Visualizer.py | 200 +++++++++++------- 14 files changed, 244 insertions(+), 71 deletions(-) create mode 100644 Code/robot_controller2/.idea/.gitignore create mode 100644 Code/robot_controller2/.idea/inspectionProfiles/profiles_settings.xml create mode 100644 Code/robot_controller2/.idea/misc.xml create mode 100644 Code/robot_controller2/.idea/modules.xml create mode 100644 Code/robot_controller2/.idea/robot_controller2.iml create mode 100644 Code/robot_controller2/.idea/vcs.xml create mode 100644 Code/robot_controller2/robot_controller2.egg-info/PKG-INFO create mode 100644 Code/robot_controller2/robot_controller2.egg-info/SOURCES.txt create mode 100644 Code/robot_controller2/robot_controller2.egg-info/dependency_links.txt create mode 100644 Code/robot_controller2/robot_controller2.egg-info/entry_points.txt create mode 100644 Code/robot_controller2/robot_controller2.egg-info/requires.txt create mode 100644 Code/robot_controller2/robot_controller2.egg-info/top_level.txt create mode 100644 Code/robot_controller2/robot_controller2.egg-info/zip-safe diff --git a/Code/robot_controller2/.idea/.gitignore b/Code/robot_controller2/.idea/.gitignore new file mode 100644 index 0000000..b58b603 --- /dev/null +++ b/Code/robot_controller2/.idea/.gitignore @@ -0,0 +1,5 @@ +# Default ignored files +/shelf/ +/workspace.xml +# Editor-based HTTP Client requests +/httpRequests/ diff --git a/Code/robot_controller2/.idea/inspectionProfiles/profiles_settings.xml b/Code/robot_controller2/.idea/inspectionProfiles/profiles_settings.xml new file mode 100644 index 0000000..105ce2d --- /dev/null +++ b/Code/robot_controller2/.idea/inspectionProfiles/profiles_settings.xml @@ -0,0 +1,6 @@ + + + + \ No newline at end of file diff --git a/Code/robot_controller2/.idea/misc.xml b/Code/robot_controller2/.idea/misc.xml new file mode 100644 index 0000000..222e17d --- /dev/null +++ b/Code/robot_controller2/.idea/misc.xml @@ -0,0 +1,7 @@ + + + + + + \ No newline at end of file diff --git a/Code/robot_controller2/.idea/modules.xml b/Code/robot_controller2/.idea/modules.xml new file mode 100644 index 0000000..9bd0c85 --- /dev/null +++ b/Code/robot_controller2/.idea/modules.xml @@ -0,0 +1,8 @@ + + + + + + + + \ No newline at end of file diff --git a/Code/robot_controller2/.idea/robot_controller2.iml b/Code/robot_controller2/.idea/robot_controller2.iml new file mode 100644 index 0000000..ec66280 --- /dev/null +++ b/Code/robot_controller2/.idea/robot_controller2.iml @@ -0,0 +1,17 @@ + + + + + + + + + + + + + + \ No newline at end of file diff --git a/Code/robot_controller2/.idea/vcs.xml b/Code/robot_controller2/.idea/vcs.xml new file mode 100644 index 0000000..b2bdec2 --- /dev/null +++ b/Code/robot_controller2/.idea/vcs.xml @@ -0,0 +1,6 @@ + + + + + + \ No newline at end of file diff --git a/Code/robot_controller2/robot_controller2.egg-info/PKG-INFO b/Code/robot_controller2/robot_controller2.egg-info/PKG-INFO new file mode 100644 index 0000000..c7a73e3 --- /dev/null +++ b/Code/robot_controller2/robot_controller2.egg-info/PKG-INFO @@ -0,0 +1,20 @@ +Metadata-Version: 2.4 +Name: robot_controller2 +Version: 0.0.0 +Summary: TODO: Package description +Maintainer: linux +Maintainer-email: linux@todo.todo +License: Apache-2.0 +License-File: LICENSE +Requires-Dist: setuptools +Requires-Dist: rclpy +Requires-Dist: networkx +Requires-Dist: mapbox-earcut +Requires-Dist: core-algorithm +Requires-Dist: PyYAML +Dynamic: license +Dynamic: license-file +Dynamic: maintainer +Dynamic: maintainer-email +Dynamic: requires-dist +Dynamic: summary diff --git a/Code/robot_controller2/robot_controller2.egg-info/SOURCES.txt b/Code/robot_controller2/robot_controller2.egg-info/SOURCES.txt new file mode 100644 index 0000000..d6ba5b2 --- /dev/null +++ b/Code/robot_controller2/robot_controller2.egg-info/SOURCES.txt @@ -0,0 +1,28 @@ +LICENSE +package.xml +setup.cfg +setup.py +action/MyAction.action +action/__init__.py +robot_controller2/Evaluation.py +robot_controller2/GT_Visualizer.py +robot_controller2/Graph.py +robot_controller2/Object_knowledge.py +robot_controller2/Reasoner.py +robot_controller2/Reasoner_refactored.py +robot_controller2/Templates.py +robot_controller2/Util.py +robot_controller2/Visualizer.py +robot_controller2/__init__.py +robot_controller2.egg-info/PKG-INFO +robot_controller2.egg-info/SOURCES.txt +robot_controller2.egg-info/dependency_links.txt +robot_controller2.egg-info/entry_points.txt +robot_controller2.egg-info/requires.txt +robot_controller2.egg-info/top_level.txt +robot_controller2.egg-info/zip-safe +robot_controller2/config/reasoner_config.yaml +robot_controller2/resources/config2.rviz +test/test_copyright.py +test/test_flake8.py +test/test_pep257.py \ No newline at end of file diff --git a/Code/robot_controller2/robot_controller2.egg-info/dependency_links.txt b/Code/robot_controller2/robot_controller2.egg-info/dependency_links.txt new file mode 100644 index 0000000..8b13789 --- /dev/null +++ b/Code/robot_controller2/robot_controller2.egg-info/dependency_links.txt @@ -0,0 +1 @@ + diff --git a/Code/robot_controller2/robot_controller2.egg-info/entry_points.txt b/Code/robot_controller2/robot_controller2.egg-info/entry_points.txt new file mode 100644 index 0000000..431fe85 --- /dev/null +++ b/Code/robot_controller2/robot_controller2.egg-info/entry_points.txt @@ -0,0 +1,8 @@ +[console_scripts] +Evaluation = robot_controller2.Evaluation:main +GT_Visualizer = robot_controller2.GT_Visualizer:main +Reasoner_refactored = robot_controller2.Reasoner_refactored:main +Templates = robot_controller2.Templates:main +Util = robot_controller2.Util:main +Visualizer = robot_controller2.Visualizer:main +test_ms = robot_controller2.test_motion_spec:main diff --git a/Code/robot_controller2/robot_controller2.egg-info/requires.txt b/Code/robot_controller2/robot_controller2.egg-info/requires.txt new file mode 100644 index 0000000..4f561a2 --- /dev/null +++ b/Code/robot_controller2/robot_controller2.egg-info/requires.txt @@ -0,0 +1,6 @@ +setuptools +rclpy +networkx +mapbox-earcut +core-algorithm +PyYAML diff --git a/Code/robot_controller2/robot_controller2.egg-info/top_level.txt b/Code/robot_controller2/robot_controller2.egg-info/top_level.txt new file mode 100644 index 0000000..f758a97 --- /dev/null +++ b/Code/robot_controller2/robot_controller2.egg-info/top_level.txt @@ -0,0 +1,2 @@ +action +robot_controller2 diff --git a/Code/robot_controller2/robot_controller2.egg-info/zip-safe b/Code/robot_controller2/robot_controller2.egg-info/zip-safe new file mode 100644 index 0000000..8b13789 --- /dev/null +++ b/Code/robot_controller2/robot_controller2.egg-info/zip-safe @@ -0,0 +1 @@ + diff --git a/Code/robot_controller2/robot_controller2/GT_Visualizer.py b/Code/robot_controller2/robot_controller2/GT_Visualizer.py index 2ef4a86..f600121 100644 --- a/Code/robot_controller2/robot_controller2/GT_Visualizer.py +++ b/Code/robot_controller2/robot_controller2/GT_Visualizer.py @@ -179,12 +179,31 @@ def ground_truth_callback(self, msg): for marker_id, pose in zip(msg.marker_ids, msg.poses): if marker_id not in ground_truth: - poses.append(pose.position) + pose_in = PoseStamped() + pose_in.header.stamp = Time(seconds=0).to_msg() + pose_in.header.frame_id = 'camera_color_frame' + + pose_in.pose.position.x = pose.position.x + pose_in.pose.position.y = pose.position.y + pose_in.pose.position.z = pose.position.z + pose_in.pose.orientation = pose.orientation + print(pose_in.pose) + try: + pose_out = self.tf_buffer.transform( + pose_in, + 'eddie_base_link', + timeout=Duration(seconds=0.5) + ) + except TransformException as ex: + self.get_logger().warn(f'Could not transform Pose: {ex}') + return ground_truth[marker_id] = { - "position": pose.position, - "orientation": pose.orientation, - "frame_id": frame_id + "position": pose_out.pose.position, + "orientation": pose_out.pose.orientation, + "frame_id": pose_out.header.frame_id } + poses.append(pose_out.pose.position) + print(pose_out) def plane_callback(self, msg): @@ -274,37 +293,37 @@ def publish_gt(self): pose_in.pose.position.y = pos.y pose_in.pose.position.z = pos.z pose_in.pose.orientation = ori - - try: - pose_out = self.tf_buffer.transform( - pose_in, - 'eddie_base_link', - timeout=Duration(seconds=0.5) - ) - except TransformException as ex: - self.get_logger().warn(f'Could not transform Pose: {ex}') - return - - self.publisher_centroid.publish(pose_out) + # + # try: + # pose_out = self.tf_buffer.transform( + # pose_in, + # 'eddie_base_link', + # timeout=Duration(seconds=0.5) + # ) + # except TransformException as ex: + # self.get_logger().warn(f'Could not transform Pose: {ex}') + # return + + self.publisher_centroid.publish(pose_in) # Save the fixed transform self.fixed_tf = TransformStamped() self.fixed_tf.header.frame_id = 'eddie_base_link' self.fixed_tf.child_frame_id = 'ground_truth_object' - self.fixed_tf.transform.translation.x = pose_out.pose.position.x - self.fixed_tf.transform.translation.y = pose_out.pose.position.y - self.fixed_tf.transform.translation.z = pose_out.pose.position.z - self.fixed_tf.transform.rotation = pose_out.pose.orientation + self.fixed_tf.transform.translation.x = pose_in.pose.position.x + self.fixed_tf.transform.translation.y = pose_in.pose.position.y + self.fixed_tf.transform.translation.z = pose_in.pose.position.z + self.fixed_tf.transform.rotation = pose_in.pose.orientation # Publish Plane Marker - fixed_marker = Marker() - fixed_marker.header.frame_id = "ground_truth_object" - fixed_marker.header.stamp = self.get_clock().now().to_msg() - fixed_marker.ns = "surface" - fixed_marker.id = 0 - fixed_marker.type = Marker.TRIANGLE_LIST - fixed_marker.action = Marker.ADD - fixed_marker.pose.orientation.w = 1.0 + # fixed_marker = Marker() + # fixed_marker.header.frame_id = "ground_truth_object" + # fixed_marker.header.stamp = self.get_clock().now().to_msg() + # fixed_marker.ns = "ground_truth_surface" + # fixed_marker.id = 0 + # fixed_marker.type = Marker.TRIANGLE_LIST + # fixed_marker.action = Marker.ADD + # fixed_marker.pose.orientation.w = 1.0 # # Hardcoded object dimensions # x_len = 0.6 #0.31 #0.34 @@ -318,17 +337,20 @@ def publish_gt(self): # # poses = [p0, p1, p2, p3] - poses = [] - - for pose in all_poses: - poses.append(Point(x=pose[0], y=pose[1], z=pose[2])) + # poses = [] + # + # for pose in all_poses: + # poses.append(Point(x=pose[0], y=pose[1], z=pose[2])) - for pose in poses: - self.publisher_point.publish(pose) + # for pose in poses: + # self.publisher_point.publish(pose) - # # Add rectangle as two triangles + # Add rectangle as two triangles # fixed_marker.points.extend([p0, p1, p2]) # fixed_marker.points.extend([p0, p2, p3]) + + # fixed_marker.points.extend([poses[0], poses[1], poses[2]]) + # fixed_marker.points.extend([poses[0], poses[2], poses[3]]) # # fixed_marker.scale.x = 1.0 # fixed_marker.scale.y = 1.0 @@ -345,8 +367,8 @@ def publish_gt(self): if self.fixed_tf: self.fixed_tf.header.stamp = self.get_clock().now().to_msg() self.br.sendTransform(self.fixed_tf) - fixed_marker.header.stamp = self.get_clock().now().to_msg() - self.publisher.publish(fixed_marker) + # fixed_marker.header.stamp = self.get_clock().now().to_msg() + # self.publisher.publish(fixed_marker) @@ -390,41 +412,77 @@ def send_transform(self): self.br.sendTransform(t) + if self.fixed_tf and len(poses) >= 4: - out = [] - - for i, point_msg in enumerate(poses): - point_in = PointStamped() - point_in.header.stamp = Time(seconds=0).to_msg() - point_in.header.frame_id = 'ground_truth_object' - - point_in.point.x = point_msg.x - point_in.point.y = point_msg.y - point_in.point.z = point_msg.z - - try: - point_out = self.tf_buffer.transform( - point_in, - 'eddie_base_link', - timeout=Duration(seconds=0.1) - ) - out.append(point_out) - - except TransformException as ex: - self.get_logger().warn( - f'Could not transform Point' - ) - break - - list_out = [] - for stamped_point in out: - point = stamped_point.point - x = float(point.x) # - y = float(point.y) - z = float(point.z) - list_out.append([x, y, z]) - - tf_poses = list_out + # out = [] + # + # for i, point_msg in enumerate(poses): + # point_in = PointStamped() + # point_in.header.stamp = Time(seconds=0).to_msg() + # point_in.header.frame_id = 'camera_color_frame' + # + # point_in.point.x = point_msg.x + # point_in.point.y = point_msg.y + # point_in.point.z = point_msg.z + # + # try: + # point_out = self.tf_buffer.transform( + # point_in, + # 'eddie_base_link', + # timeout=Duration(seconds=0.1) + # ) + # out.append(point_out) + # + # except TransformException as ex: + # self.get_logger().warn( + # f'Could not transform Point' + # ) + # break + # + # list_out = [] + # for stamped_point in out: + # point = stamped_point.point + # x = float(point.x) # + # y = float(point.y) + # z = float(point.z) + # list_out.append([x, y, z]) + # + # tf_poses = list_out + # print(tf_poses) + + + # Publish Plane Marker + fixed_marker = Marker() + fixed_marker.header.frame_id = "eddie_base_link" + fixed_marker.header.stamp = self.get_clock().now().to_msg() + fixed_marker.ns = "ground_truth_surface" + fixed_marker.id = 0 + fixed_marker.type = Marker.TRIANGLE_LIST + fixed_marker.action = Marker.ADD + fixed_marker.pose.orientation.w = 1.0 + + publish_poses = [] + + for pose in poses: + publish_poses.append(Point(x=pose.x, y=pose.y, z=pose.z)) + self.publisher_point.publish(Point(x=pose.x, y=pose.y, z=pose.z)) + + + fixed_marker.points.extend([publish_poses[0], publish_poses[1], publish_poses[2]]) + fixed_marker.points.extend([publish_poses[0], publish_poses[2], publish_poses[3]]) + + fixed_marker.scale.x = 1.0 + fixed_marker.scale.y = 1.0 + fixed_marker.scale.z = 1.0 + + fixed_marker.color.r = 1.0 + fixed_marker.color.g = 0.0 + fixed_marker.color.b = 0.5 + fixed_marker.color.a = 0.4 + + + fixed_marker.header.stamp = self.get_clock().now().to_msg() + self.publisher.publish(fixed_marker)