Skip to content

Commit

Permalink
cleaned up, implemented outreach
Browse files Browse the repository at this point in the history
  • Loading branch information
@albus-fang
albus-fang committed May 10, 2024
1 parent 55cd504 commit 28bd11c
Show file tree
Hide file tree
Showing 2 changed files with 119 additions and 23 deletions.
100 changes: 100 additions & 0 deletions stream_transform.py
View file
Original file line number Diff line number Diff line change
@@ -0,0 +1,100 @@
import pyrealsense2 as rs
import numpy as np
import cv2
from geometry_msgs.msg import Quaternion
from tf2_ros.transform_listener import TransformListener
from numpy.linalg import inv
from scipy.spatial.transform import Rotation as R
from calibrate import calibrate
from std_msgs.msg import String

#extrinsics = calibrate()

extrinsics = np.array([[ 0.99926912, -0.03471002, 0.01601377, -0.34649297],
[-0.03404074, -0.9986065 , -0.04032727, 0.14374124],
[ 0.01739122, 0.03975268, -0.99905819, 2.76456235],
[ 0. , 0. , 0. , 1. ]])

def rot_to_hom(rot_matrix, trans):

'''
Construct Homogeneous matrix from R and T
rot_matrix: (3x3) np.ndarray
trans: (3x1) np.ndarray
'''
hom_mat = np.eye(4)
hom_mat[:3,:3] = rot_matrix
hom_mat[:3,-1] = trans
return hom_mat


def robot_to_world(hom_mat, coord):

return np.mat_mul(hom_mat, coord)
def camera_to_world(hom_mat, camera_coord):
hom_mat = np.eye(4)
hom_mat[1,1] = -1
hom_mat[2,2] = -1
z = 2.8
x = 0.3117
y = 0.191
hom_mat[0,3] = x
hom_mat[1,3] = y
hom_mat[2,3] = z
world_coordinates = hom_mat @ np.array([camera_coord[0], camera_coord[1],camera_coord[2],1])
return world_coordinates[0:3]

def world_to_camera(world_coordinates):
world_coordinates = np.append(world_coordinates,1)
hom_mat = extrinsics
camera_coord = hom_mat @ world_coordinates
camera_coord = camera_coord[0:3]
return camera_coord
def camera_to_world_calibrate(camera_coordinates):
camera_coordinates = np.append(camera_coordinates,1)
hom_mat = extrinsics
hom_mat = np.linalg.inv(hom_mat)
world_coordinates = hom_mat @ camera_coordinates
return world_coordinates[0:3]

def world_to_drone(hom_mat, coord):
'''
hom_mat: (4x4) np.ndarray
coord : (4x1) np.ndarray
'''
############## for testing ################
robot_height = 0.3
z = 2.8
x = 0.3117
y = 0.191
hom_mat2 = np.eye(4)
hom_mat2[0,3] = -x
hom_mat2[1,3] = -y
hom_mat2[2,3] = -z

###########################################
# hom_mat2 = inv(hom_mat)
drone_coord = (hom_mat2) @ coord
return drone_coord

def drone_to_camera(drone_coord):
'''
drone -> cam
H = [R t]
[0 1]
'''
coord = drone_coord[0:3]
mat = np.array([[1,0,0],[0,-1,0],[0,0,-1]])
camera_coord = np.matmul(mat,coord)
return camera_coord

r = R.from_quat([0,0,1,0.5])
rotate = r.as_matrix()


42 changes: 19 additions & 23 deletions xavier_node_outreach.py
View file
Original file line number Diff line number Diff line change
Expand Up @@ -14,15 +14,13 @@
import time
from stream_transform import rot_to_hom, world_to_drone,drone_to_camera, camera_to_world_calibrate
import apriltag
from rover_node import MinimalPublisher
import ast
import threading
class MinimalSubscriber(Node):

def __init__(self):
## initializing the camera
super().__init__('minimal_subscriber')
#self.output = cv2.VideoWriter('output.avi', cv2.VideoWriter_fourcc(*'MPEG'), 30, (640,480))
###################################################################################################
####################### Declare and Initialize Preliminary Parameters ######################
###################################################################################################
Expand All @@ -44,7 +42,7 @@ def __init__(self):
self.pos_list = [() for _ in range(0, self.num_robot)]
self.trails = [[] for _ in range(0, self.num_robot)]
self.is_published = False

self.reset = False
###################################################################################################
##################### Setup server ######################
###################################################################################################
Expand All @@ -64,8 +62,7 @@ def __init__(self):
self.cfg = rs.config()
self.cfg.enable_stream(rs.stream.color, 640, 480, rs.format.bgr8,30)
self.cfg.enable_stream(rs.stream.depth, 640, 480, rs.format.z16,30)
#self.cfg.enable_stream(rs.stream.color, 1280, 720, rs.format.bgr8,30)
#self.cfg.enable_stream(rs.stream.depth, 1280, 720, rs.format.z16,30)

self.c = self.pipe.start(self.cfg)
self.profile = self.c.get_stream(rs.stream.color)
self.depth_image = self.c.get_stream(rs.stream.depth)
Expand All @@ -77,7 +74,6 @@ def __init__(self):
########### Subscribers ###########
self.robot_subscription = self.create_subscription(
TransformStamped,
#'/vicon/kepler/kepler',
'/vicon/px4_1/px4_1',
self.listener_callback_robot,
10)
Expand Down Expand Up @@ -111,9 +107,7 @@ def listener_callback_robot(self, msg):
def listener_callback_camera(self, msg):
self.quat = msg.transform.rotation
self.trans = msg.transform.translation
#print("camera_info received")
#self.get_logger().info('I heard: "%s"' % msg.data)
# def create_px4_msg(self, world_coordinates):


###################################################################################################
###################### Set up Helpers ######################
Expand All @@ -136,7 +130,7 @@ def euclidean_distance( point1, point2):
x1, y1 = point1
x2, y2 = point2
return np.sqrt((x2 - x1) ** 2 + (y2 - y1) ** 2)
if euclidean_distance(target_pixel_coordinates,actual_pixel_coordinates) < 10:
if euclidean_distance(target_pixel_coordinates,actual_pixel_coordinates) < 20:
return True
else:
return False
Expand Down Expand Up @@ -210,18 +204,17 @@ def draw_dotted_curve(self, img, coordinates, dot_spacing=8,curve_color=(255, 0,
dashed = dashed + 1
else:
dashed = 0
#img = draw_traj(img, coordinates)
return img
def timer_callback(self):
#pdb.set_trace()

def timer_callback(self):
frame = self.pipe.wait_for_frames()
depth_frame = frame.get_depth_frame()
color_frame = frame.get_color_frame()

robot_trans = self.robot_trans
robot_quat = self.robot_quat

if self.reset == True:
self.trails = [[] for _ in range(0, self.num_robot)]
self.reset = False
if self.is_target_set and self.target is not None and self.is_published is False:
self.publish_msg()
self.is_published = True
Expand Down Expand Up @@ -368,16 +361,19 @@ def coordinate_reception(xavier_node):
while True:
coord_data = coord_client_socket.recv(1024).decode('utf-8')
if not coord_data:
break
coord_data = ast.literal_eval(coord_data)
xavier_node.target = coord_data
xavier_node.is_target_set = True
xavier_node.is_published = False
print(f"Received coordinates: {coord_data}")
break
if coord_data == 'reset':
xavier_node.reset = True
else:
coord_data = ast.literal_eval(coord_data)
xavier_node.target = coord_data
xavier_node.is_target_set = True
xavier_node.is_published = False
print(f"Received coordinates: {coord_data}")
# Process the received coordinates here
finally:
coord_client_socket.close()
coord_server_socket.close()
coord_client_socket.close()
coord_server_socket.close()



Expand Down

0 comments on commit 28bd11c

Please sign in to comment.