Skip to content

quick fix #1

New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

Sign up for GitHub

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

Merged
KishanSawant merged 1 commit into from
Jun 1, 2026
+205 −93
Merged

quick fix #1

Show file tree
Hide file tree
Changes from all commits
Commits
Show all changes
1 commit
Select commit
File filter

Filter by extension

Filter by extension
Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
36 changes: 36 additions & 0 deletions Code/robot_controller2/Util/launch_notes
View file
Open in desktop
Original file line number Diff line number Diff line change
@@ -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



16 changes: 8 additions & 8 deletions Code/robot_controller2/robot_controller2/Evaluation.py
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -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)

Expand Down
244 changes: 160 additions & 84 deletions Code/robot_controller2/robot_controller2/GT_Visualizer.py
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -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):
Expand Down Expand Up @@ -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 = []
Expand Down Expand Up @@ -271,74 +293,82 @@ 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


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)



Expand Down Expand Up @@ -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)



Expand All @@ -437,3 +503,13 @@ def main(args=None):

if __name__ == '__main__':
main()










Loading