From 99bfe0a6bbc201263e40bddd30262d0c7a025df0 Mon Sep 17 00:00:00 2001 From: Felix Date: Fri, 29 May 2026 13:05:21 +0200 Subject: [PATCH] File fix --- Code/robot_controller2/Util/launch_notes | 36 +++ .../robot_controller2/Evaluation.py | 16 +- .../robot_controller2/GT_Visualizer.py | 244 ++++++++++++------ Code/robot_controller2/setup.cfg | 2 +- 4 files changed, 205 insertions(+), 93 deletions(-) create mode 100644 Code/robot_controller2/Util/launch_notes 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 e834fee..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): @@ -242,6 +261,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 = [] @@ -271,65 +293,73 @@ 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 - - # 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] - - for pose in poses: - self.publisher_point.publish(pose) + # 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 + # 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] + + # 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) # 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 + # 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 + # 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 @@ -337,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) @@ -382,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 = '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 - out = [] + publish_poses = [] - 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' + 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)) - 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) + 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]]) - except TransformException as ex: - self.get_logger().warn( - f'Could not transform Point' - ) - break + fixed_marker.scale.x = 1.0 + fixed_marker.scale.y = 1.0 + fixed_marker.scale.z = 1.0 - 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]) + fixed_marker.color.r = 1.0 + fixed_marker.color.g = 0.0 + fixed_marker.color.b = 0.5 + fixed_marker.color.a = 0.4 - tf_poses = list_out + + fixed_marker.header.stamp = self.get_clock().now().to_msg() + self.publisher.publish(fixed_marker) @@ -437,3 +503,13 @@ def main(args=None): if __name__ == '__main__': main() + + + + + + + + + + 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]