def get_normal(self):
req = CameraToLidarTransformRequest()
req.header = self.found_shape.header
req.point = Point()
req.point.x = self.found_shape.CenterX
req.point.y = self.found_shape.CenterY
req.tolerance = self.normal_approx_tolerance_proportion*self.found_shape.img_width
self.normal_res = yield self.cameraLidarTransformer(req)
if self.normal_res.success:
transformObj = yield self.navigator.tf_listener.get_transform('/enu', '/'+req.header.frame_id)
self.enunormal = transformObj.transform_vector(navigator_tools.rosmsg_to_numpy(self.normal_res.normal))
self.enupoint = transformObj.transform_point(navigator_tools.rosmsg_to_numpy(self.normal_res.closest))
target_marker = Marker()
target_marker.scale.x = 0.1;
target_marker.scale.y = 0.5;
target_marker.scale.z = 0.5;
target_marker.header.frame_id = "enu"
target_marker.action = Marker.ADD;
target_marker.header.stamp = self.navigator.nh.get_time()
target_marker.points.append(navigator_tools.numpy_to_point(self.enupoint))
target_marker.points.append(navigator_tools.numpy_to_point(self.enupoint+self.enunormal))
target_marker.type = Marker.ARROW
#target_marker.text = "Shooter Target Pose"
target_marker.ns = "detect_deliver"
target_marker.id = 1
target_marker.color.r = 1
target_marker.color.a = 1
self.markers.markers.append(target_marker)
defer.returnValue(self.normal_res.success)
def get_normal(self):
req = CameraToLidarTransformRequest()
req.header = self.found_shape.header
req.point = Point()
req.point.x = self.found_shape.CenterX
req.point.y = self.found_shape.CenterY
rect = self._bounding_rect(self.found_shape.points)
req.tolerance = int(min(rect[0] - rect[3], rect[1] - rect[4]) / 2.0)
self.normal_res = yield self.cameraLidarTransformer(req)
if self.normal_res.success:
transformObj = yield self.navigator.tf_listener.get_transform(
'/enu', '/' + req.header.frame_id)
self.enunormal = transformObj.transform_vector(
navigator_tools.rosmsg_to_numpy(self.normal_res.normal))
self.enupoint = transformObj.transform_point(
navigator_tools.rosmsg_to_numpy(self.normal_res.closest))
target_marker = Marker()
target_marker.scale.x = 0.1
target_marker.scale.y = 0.5
target_marker.scale.z = 0.5
target_marker.header.frame_id = "enu"
target_marker.action = Marker.ADD
target_marker.header.stamp = self.navigator.nh.get_time()
target_marker.points.append(
navigator_tools.numpy_to_point(self.enupoint))
target_marker.points.append(
navigator_tools.numpy_to_point(self.enupoint + self.enunormal))
target_marker.type = Marker.ARROW
#target_marker.text = "Shooter Target Pose"
target_marker.ns = "detect_deliver"
target_marker.id = 1
target_marker.color.r = 1
target_marker.color.a = 1
self.markers.markers.append(target_marker)
defer.returnValue(self.normal_res.success)
def as_MoveGoal(self, move_type='drive', **kwargs):
if 'focus' in kwargs:
if not isinstance(kwargs['focus'], Point):
kwargs['focus'] = navigator_tools.numpy_to_point(
kwargs['focus'])
return MoveGoal(goal=self.as_Pose(), move_type=move_type, **kwargs)
def get_aligned_pos(self):
# ~position, rot = yield self.navigator.tx_pose
self.aligned_position = self.enupoint + self.shoot_distance_meters * self.enunormal # moves x meters away
angle = np.arctan2(-self.enunormal[0], self.enunormal[1])
self.aligned_orientation = trns.quaternion_from_euler(
0, 0, angle) # Align perpindicular
move_marker = Marker()
move_marker.scale.x = 1
move_marker.scale.y = .1
move_marker.scale.z = .1
move_marker.action = Marker.ADD
move_marker.header.frame_id = "enu"
move_marker.header.stamp = self.navigator.nh.get_time()
move_marker.pose.position = navigator_tools.numpy_to_point(
self.aligned_position)
move_marker.pose.orientation = navigator_tools.numpy_to_quaternion(
self.aligned_orientation)
move_marker.type = Marker.ARROW
move_marker.text = "Align goal"
move_marker.ns = "detect_deliver"
move_marker.id = 2
move_marker.color.r = 1
move_marker.color.a = 1
self.markers.markers.append(move_marker)
def as_MoveGoal(self, move_type='drive', **kwargs):
if 'focus' in kwargs:
if not isinstance(kwargs['focus'], Point):
kwargs['focus'] = navigator_tools.numpy_to_point(kwargs['focus'])
return MoveGoal(
goal=self.as_Pose(),
move_type=move_type,
**kwargs
)
def position_to_object(self, position, color, id, name="totem"):
obj = PerceptionObject()
obj.id = int(id)
obj.header = navigator_tools.make_header(frame="enu")
obj.name = name
obj.position = navigator_tools.numpy_to_point(position)
obj.color.r = color[0]
obj.color.g = color[1]
obj.color.b = color[2]
obj.color.a = 1
return obj
def position_to_object(self, position, color, id, name="totem"):
obj = PerceptionObject()
obj.id = int(id)
obj.header = navigator_tools.make_header(frame="enu")
obj.name = name
obj.position = navigator_tools.numpy_to_point(position)
obj.color.r = color[0]
obj.color.g = color[1]
obj.color.b = color[2]
obj.color.a = 1
return obj
def got_request(self, req):
to_frame = "enu" if req.to_frame == '' else req.to_frame
resp = BoundsResponse(enforce=False)
self.bounds = [self.convert(CoordinateConversionRequest(frame="lla", point=lla)) for lla in self.lla_bounds]
bounds = [navigator_tools.numpy_to_point(getattr(response, to_frame)) for response in self.bounds]
resp.enforce = self.enforce
resp.bounds = bounds
return resp
def as_MoveGoal(self, move_type='drive', **kwargs):
if 'focus' in kwargs:
if not isinstance(kwargs['focus'], Point):
kwargs['focus'] = navigator_tools.numpy_to_point(kwargs['focus'])
for key in kwargs.keys():
if not hasattr(MoveGoal, key):
fprint("MoveGoal msg doesn't have a field called '{}' you tried to set via kwargs.".format(key), title="POSE_EDITOR",
msg_color="red")
del kwargs[key]
return MoveGoal(
goal=self.as_Pose(),
move_type=move_type,
**kwargs
)
def draw_sphere(position, color, scaling=(0.11, 0.11, 0.11), m_id=4, frame='/base_link'):
pose = Pose(
position=navigator_tools.numpy_to_point(position),
orientation=navigator_tools.numpy_to_quaternion([0.0, 0.0, 0.0, 1.0])
)
marker = visualization_msgs.Marker(
ns='wamv',
id=m_id,
header=navigator_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(),
)
rviz_pub.publish(marker)
def got_request(nh, req, convert):
to_frame = "enu" if req.to_frame == '' else req.to_frame
resp = BoundsResponse(enforce=False)
if (yield nh.has_param("/bounds/enforce")) and (
yield nh.get_param("/bounds/enforce")):
# We want to enforce the bounds that were set
lla_bounds = yield nh.get_param("/bounds/lla")
bounds = []
for lla_bound in lla_bounds:
bound = yield convert(
CoordinateConversionRequest(frame="lla",
point=np.append(lla_bound, 0)))
bounds.append(
navigator_tools.numpy_to_point(getattr(bound, to_frame)))
resp = BoundsResponse(enforce=True, bounds=bounds)
defer.returnValue(resp)
def draw_sphere(position,
color,
scaling=(0.11, 0.11, 0.11),
m_id=4,
frame='/base_link'):
pose = Pose(position=navigator_tools.numpy_to_point(position),
orientation=navigator_tools.numpy_to_quaternion(
[0.0, 0.0, 0.0, 1.0]))
marker = visualization_msgs.Marker(
ns='wamv',
id=m_id,
header=navigator_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(),
)
rviz_pub.publish(marker)
def as_MoveGoal(self, move_type='drive', **kwargs):
if 'focus' in kwargs:
if not isinstance(kwargs['focus'], Point):
kwargs['focus'] = navigator_tools.numpy_to_point(
kwargs['focus'])
if 'speed_factor' in kwargs and isinstance(kwargs['speed_factor'],
float):
# User wants a uniform speed factor
sf = kwargs['speed_factor']
kwargs['speed_factor'] = [sf, sf, sf]
for key in kwargs.keys():
if not hasattr(MoveGoal, key):
fprint(
"MoveGoal msg doesn't have a field called '{}' you tried to set via kwargs."
.format(key),
title="POSE_EDITOR",
msg_color="red")
del kwargs[key]
return MoveGoal(goal=self.as_Pose(), move_type=move_type, **kwargs)
def get_aligned_pos(self):
# ~position, rot = yield self.navigator.tx_pose
self.aligned_position = self.enupoint + self.shoot_distance_meters * self.enunormal # moves x meters away
angle = np.arctan2(-self.enunormal[0], self.enunormal[1])
self.aligned_orientation = trns.quaternion_from_euler(0, 0, angle) # Align perpindicular
move_marker = Marker()
move_marker.scale.x = 1;
move_marker.scale.y = .1;
move_marker.scale.z = .1;
move_marker.action = Marker.ADD;
move_marker.header.frame_id = "enu"
move_marker.header.stamp = self.navigator.nh.get_time()
move_marker.pose.position = navigator_tools.numpy_to_point(self.aligned_position)
move_marker.pose.orientation = navigator_tools.numpy_to_quaternion(self.aligned_orientation)
move_marker.type = Marker.ARROW
move_marker.text = "Align goal"
move_marker.ns = "detect_deliver"
move_marker.id = 2
move_marker.color.r = 1
move_marker.color.a = 1
self.markers.markers.append(move_marker)
def main(navigator):
result = navigator.fetch_result()
found_buoys = yield navigator.database_query("start_gate")
buoys = np.array(map(Buoy.from_srv, found_buoys.objects))
if len(buoys) == 2:
ogrid, setup, target = yield two_buoys(navigator, buoys, 10)
elif len(buoys) == 4:
ogrid, setup, target = yield four_buoys(navigator, buoys, 10)
else:
raise Exception
# Put the target into the point cloud as well
points = []
points.append(navigator_tools.numpy_to_point(target.position))
pc = PointCloud(header=navigator_tools.make_header(frame='/enu'),
points=np.array(points))
yield navigator._point_cloud_pub.publish(pc)
print "Waiting 3 seconds to move..."
yield navigator.nh.sleep(3)
yield setup.go(move_type='skid')
pose = yield navigator.tx_pose
ogrid.generate_grid(pose)
msg = ogrid.get_message()
print "publishing"
latched = navigator.latching_publisher("/mission_ogrid", OccupancyGrid,
msg)
print "START_GATE: Moving in 5 seconds!"
yield target.go(initial_plan_time=5)
return_with(result)
def main(navigator):
result = navigator.fetch_result()
found_buoys = yield navigator.database_query("start_gate")
buoys = np.array(map(Buoy.from_srv, found_buoys.objects))
if len(buoys) == 2:
ogrid, setup, target = yield two_buoys(navigator, buoys, 10)
elif len(buoys) == 4:
ogrid, setup, target = yield four_buoys(navigator, buoys, 10)
else:
raise Exception
# Put the target into the point cloud as well
points = []
points.append(navigator_tools.numpy_to_point(target.position))
pc = PointCloud(header=navigator_tools.make_header(frame='/enu'),
points=np.array(points))
yield navigator._point_cloud_pub.publish(pc)
print "Waiting 3 seconds to move..."
yield navigator.nh.sleep(3)
yield setup.go(move_type='skid')
pose = yield navigator.tx_pose
ogrid.generate_grid(pose)
msg = ogrid.get_message()
print "publishing"
latched = navigator.latching_publisher("/mission_ogrid", OccupancyGrid, msg)
print "START_GATE: Moving in 5 seconds!"
yield target.go(initial_plan_time=5)
return_with(result)
def main(navigator):
result = navigator.fetch_result()
found_buoys = yield navigator.database_query("start_gate")
if found_buoys.found:
buoys = np.array(map(Buoy.from_srv, found_buoys.objects))
else:
print "START_GATE: Error 4 - No db buoy response..."
result.success = False
result.response = result.DbObjectNotFound
result.message = "Start gates not found in the database."
return_with(result)
if len(buoys) != 4:
print "START_GATE: Error 5 - Invaild number of buoys found: {} (expecting 4)".format(len(buoys))
result.success = False
result.message = "Invaild number of buoys found: {} (expecting 4)".format(len(buoys))
return_with(result)
# buoys = [Buoy.from_srv(left), Buoy.from_srv(right)]
#buoys = np.array(get_buoys())
pose = yield navigator.tx_pose
# Get the ones closest to us and assume those are the front
distances = np.array([b.distance(pose[0]) for b in buoys])
back = buoys[np.argsort(distances)[-2:]]
front = buoys[np.argsort(distances)[:2]]
#print "front", front
#print "back", back
# Made it this far, make sure the red one is on the left and green on the right ================
t = txros.tf.Transform.from_Pose_message(navigator_tools.numpy_quat_pair_to_pose(*pose))
t_mat44 = t.as_matrix()
f_bl_buoys = [t_mat44.dot(np.append(buoy.position, 1)) for buoy in front]
b_bl_buoys = [t_mat44.dot(np.append(buoy.position, 1)) for buoy in back]
# Angles are w.r.t positive x-axis. Positive is CCW around the z-axis.
angle_buoys = np.array([np.arctan2(buoy[1], buoy[0]) for buoy in f_bl_buoys])
#print angle_buoys, front
f_left_buoy, f_right_buoy = front[np.argmax(angle_buoys)], front[np.argmin(angle_buoys)]
angle_buoys = np.array([np.arctan2(buoy[1], buoy[0]) for buoy in b_bl_buoys])
b_left_buoy, b_right_buoy = back[np.argmax(angle_buoys)], back[np.argmin(angle_buoys)]
points = [navigator_tools.numpy_to_point(b.position) for b in [f_left_buoy]]
# Lets plot a course, yarrr
f_between_vector, f_direction_vector = get_path(f_left_buoy, f_right_buoy)
f_mid_point = f_left_buoy.position + f_between_vector / 2
b_between_vector, _ = get_path(b_left_buoy, b_right_buoy)
b_mid_point = b_left_buoy.position + b_between_vector / 2
through_vector = b_mid_point - f_mid_point
through_vector = through_vector / np.linalg.norm(through_vector)
#print mid_point
setup_dist = 20 # Line up with the start gate this many meters infront of the gate.
setup = f_mid_point - f_direction_vector * setup_dist
target = b_mid_point + through_vector * setup_dist
# Put the target into the point cloud as well
#points.append(navigator_tools.numpy_to_point(b_left_buoy))
points.append(navigator_tools.numpy_to_point(target))
pc = PointCloud(header=navigator_tools.make_header(frame='/enu'),
points=np.array(points))
yield navigator._point_cloud_pub.publish(pc)
print "Waiting 3 seconds to move..."
yield navigator.nh.sleep(3)
yield navigator.move.set_position(setup).look_at(f_mid_point).go(move_type="skid")
pose = yield navigator.tx_pose
ogrid = OgridFactory(f_left_buoy.position, f_right_buoy.position, pose[0],
target, left_b_pos=b_left_buoy.position, right_b_pos=b_right_buoy.position)
msg = ogrid.get_message()
print "publishing"
latched = navigator.latching_publisher("/mission_ogrid", OccupancyGrid, msg)
yield navigator.nh.sleep(5)
print "START_GATE: Moving in 5 seconds!"
yield navigator.move.set_position(target).go(initial_plan_time=5)
return_with(result)
def do_buoys(srv, left, right, red_seg, green_seg, tf_listener):
'''
FYI:
`left`: the left camera ImageGetter object
`right`: the right camera ImageGetter object
'''
left_point = None
right_point = None
while not rospy.is_shutdown():
# Get all the required TF links
try:
# Working copy of the current frame obtained at the same time as the tf link
tf_listener.waitForTransform("enu", "front_left_cam", rospy.Time(),
rospy.Duration(4.0))
left_image, right_image = left.frame, right.frame
cam_tf = tf_listener.lookupTransform("front_left_cam",
"front_right_cam",
left.sub.last_image_time)
cam_p, cam_q = tf_listener.lookupTransform(
"enu", "front_left_cam", left.sub.last_image_time)
cam_p = np.array([cam_p])
cam_r = tf.transformations.quaternion_matrix(cam_q)[:3, :3]
break
except (tf.LookupException, tf.ConnectivityException,
tf.ExtrapolationException, TypeError) as e:
print e
rospy.logwarn("TF link not found.")
time.sleep(.5)
continue
red_left_pt, rl_area = red_seg.segment(left_image)
red_right_pt, rr_area = red_seg.segment(right_image)
green_left_pt, gl_area = green_seg.segment(left_image)
green_right_pt, gr_area = green_seg.segment(right_image)
area_tol = 50
if np.linalg.norm(rl_area - rr_area) > area_tol or np.linalg.norm(
gl_area - gr_area) > area_tol:
rospy.logwarn("Unsafe segmentation")
StartGateResponse(success=False)
red_point_np = do_the_magic(red_left_pt, red_right_pt, cam_tf)
red_point_np = np.array(cam_r.dot(red_point_np) + cam_p)[0]
green_point_np = do_the_magic(green_left_pt, green_right_pt, cam_tf)
green_point_np = np.array(cam_r.dot(green_point_np) + cam_p)[0]
# Just for visualization
for i in range(5):
# Publish it 5 times so we can see it in rviz
navigator_tools.draw_ray_3d(red_left_pt,
left_cam, [1, 0, 0, 1],
m_id=0,
frame="front_left_cam")
navigator_tools.draw_ray_3d(red_right_pt,
right_cam, [1, 0, 0, 1],
m_id=1,
frame="front_right_cam")
navigator_tools.draw_ray_3d(green_left_pt,
left_cam, [0, 1, 0, 1],
m_id=2,
frame="front_left_cam")
navigator_tools.draw_ray_3d(green_right_pt,
right_cam, [0, 1, 0, 1],
m_id=3,
frame="front_right_cam")
red_point = PointStamped()
red_point.header = navigator_tools.make_header(frame="enu")
red_point.point = navigator_tools.numpy_to_point(red_point_np)
red_pub.publish(red_point)
green_point = PointStamped()
green_point.header = navigator_tools.make_header(frame="enu")
green_point.point = navigator_tools.numpy_to_point(green_point_np)
green_pub.publish(green_point)
time.sleep(1)
# Now we have two points, find their midpoint and calculate a target angle
midpoint = (red_point_np + green_point_np) / 2
between_vector = green_point_np - red_point_np # Red is on the left when we go out.
yaw_theta = np.arctan2(between_vector[1], between_vector[0])
# Rotate that theta by 90 deg to get the angle through the buoys
yaw_theta += np.pi / 2.0
p = midpoint
q = tf.transformations.quaternion_from_euler(0, 0, yaw_theta)
target_pose = PoseStamped()
target_pose.header = navigator_tools.make_header(frame="enu")
target_pose.pose = navigator_tools.numpy_quat_pair_to_pose(p, q)
return StartGateResponse(target=target_pose, success=True)
def main(navigator):
result = navigator.fetch_result()
found_buoys = yield navigator.database_query("start_gate")
if found_buoys.found:
buoys = np.array(map(Buoy.from_srv, found_buoys.objects))
else:
print "START_GATE: Error 4 - No db buoy response..."
result.success = False
result.response = result.DbObjectNotFound
result.message = "Start gates not found in the database."
return_with(result)
if len(buoys) != 4:
print "START_GATE: Error 5 - Invaild number of buoys found: {} (expecting 4)".format(
len(buoys))
result.success = False
result.message = "Invaild number of buoys found: {} (expecting 4)".format(
len(buoys))
return_with(result)
# buoys = [Buoy.from_srv(left), Buoy.from_srv(right)]
#buoys = np.array(get_buoys())
pose = yield navigator.tx_pose
# Get the ones closest to us and assume those are the front
distances = np.array([b.distance(pose[0]) for b in buoys])
back = buoys[np.argsort(distances)[-2:]]
front = buoys[np.argsort(distances)[:2]]
#print "front", front
#print "back", back
# Made it this far, make sure the red one is on the left and green on the right ================
t = txros.tf.Transform.from_Pose_message(
navigator_tools.numpy_quat_pair_to_pose(*pose))
t_mat44 = t.as_matrix()
f_bl_buoys = [t_mat44.dot(np.append(buoy.position, 1)) for buoy in front]
b_bl_buoys = [t_mat44.dot(np.append(buoy.position, 1)) for buoy in back]
# Angles are w.r.t positive x-axis. Positive is CCW around the z-axis.
angle_buoys = np.array(
[np.arctan2(buoy[1], buoy[0]) for buoy in f_bl_buoys])
#print angle_buoys, front
f_left_buoy, f_right_buoy = front[np.argmax(angle_buoys)], front[np.argmin(
angle_buoys)]
angle_buoys = np.array(
[np.arctan2(buoy[1], buoy[0]) for buoy in b_bl_buoys])
b_left_buoy, b_right_buoy = back[np.argmax(angle_buoys)], back[np.argmin(
angle_buoys)]
points = [
navigator_tools.numpy_to_point(b.position) for b in [f_left_buoy]
]
# Lets plot a course, yarrr
f_between_vector, f_direction_vector = get_path(f_left_buoy, f_right_buoy)
f_mid_point = f_left_buoy.position + f_between_vector / 2
b_between_vector, _ = get_path(b_left_buoy, b_right_buoy)
b_mid_point = b_left_buoy.position + b_between_vector / 2
through_vector = b_mid_point - f_mid_point
through_vector = through_vector / np.linalg.norm(through_vector)
#print mid_point
setup_dist = 20 # Line up with the start gate this many meters infront of the gate.
setup = f_mid_point - f_direction_vector * setup_dist
target = b_mid_point + through_vector * setup_dist
# Put the target into the point cloud as well
#points.append(navigator_tools.numpy_to_point(b_left_buoy))
points.append(navigator_tools.numpy_to_point(target))
pc = PointCloud(header=navigator_tools.make_header(frame='/enu'),
points=np.array(points))
yield navigator._point_cloud_pub.publish(pc)
print "Waiting 3 seconds to move..."
yield navigator.nh.sleep(3)
yield navigator.move.set_position(setup).look_at(f_mid_point).go(
move_type="skid")
pose = yield navigator.tx_pose
ogrid = OgridFactory(f_left_buoy.position,
f_right_buoy.position,
pose[0],
target,
left_b_pos=b_left_buoy.position,
right_b_pos=b_right_buoy.position)
msg = ogrid.get_message()
print "publishing"
latched = navigator.latching_publisher("/mission_ogrid", OccupancyGrid,
msg)
yield navigator.nh.sleep(5)
print "START_GATE: Moving in 5 seconds!"
yield navigator.move.set_position(target).go(initial_plan_time=5)
return_with(result)
def do_buoys(srv, left, right, red_seg, green_seg, tf_listener):
'''
FYI:
`left`: the left camera ImageGetter object
`right`: the right camera ImageGetter object
'''
left_point = None
right_point = None
while not rospy.is_shutdown():
# Get all the required TF links
try:
# Working copy of the current frame obtained at the same time as the tf link
tf_listener.waitForTransform("enu", "stereo_left_cam", rospy.Time(), rospy.Duration(4.0))
left_image, right_image = left.frame, right.frame
cam_tf = tf_listener.lookupTransform("stereo_left_cam", "stereo_right_cam", left.sub.last_image_time)
cam_p, cam_q = tf_listener.lookupTransform("enu", "stereo_left_cam", left.sub.last_image_time)
cam_p = np.array([cam_p])
cam_r = tf.transformations.quaternion_matrix(cam_q)[:3, :3]
break
except (tf.LookupException, tf.ConnectivityException, tf.ExtrapolationException, TypeError) as e:
print e
rospy.logwarn("TF link not found.")
time.sleep(.5)
continue
red_left_pt, rl_area = red_seg.segment(left_image)
red_right_pt, rr_area = red_seg.segment(right_image)
green_left_pt, gl_area = green_seg.segment(left_image)
green_right_pt, gr_area = green_seg.segment(right_image)
area_tol = 50
if np.linalg.norm(rl_area - rr_area) > area_tol or np.linalg.norm(gl_area - gr_area) > area_tol:
rospy.logwarn("Unsafe segmentation")
StartGateResponse(success=False)
red_point_np = do_the_magic(red_left_pt, red_right_pt, cam_tf)
red_point_np = np.array(cam_r.dot(red_point_np) + cam_p)[0]
green_point_np = do_the_magic(green_left_pt, green_right_pt, cam_tf)
green_point_np = np.array(cam_r.dot(green_point_np) + cam_p)[0]
# Just for visualization
for i in range(5):
# Publish it 5 times so we can see it in rviz
navigator_tools.draw_ray_3d(red_left_pt, left_cam, [1, 0, 0, 1], m_id=0, frame="stereo_left_cam")
navigator_tools.draw_ray_3d(red_right_pt, right_cam, [1, 0, 0, 1], m_id=1, frame="stereo_right_cam")
navigator_tools.draw_ray_3d(green_left_pt, left_cam, [0, 1, 0, 1], m_id=2, frame="stereo_left_cam")
navigator_tools.draw_ray_3d(green_right_pt, right_cam, [0, 1, 0, 1], m_id=3, frame="stereo_right_cam")
red_point = PointStamped()
red_point.header = navigator_tools.make_header(frame="enu")
red_point.point = navigator_tools.numpy_to_point(red_point_np)
red_pub.publish(red_point)
green_point = PointStamped()
green_point.header = navigator_tools.make_header(frame="enu")
green_point.point = navigator_tools.numpy_to_point(green_point_np)
green_pub.publish(green_point)
time.sleep(1)
# Now we have two points, find their midpoint and calculate a target angle
midpoint = (red_point_np + green_point_np) / 2
between_vector = green_point_np - red_point_np # Red is on the left when we go out.
yaw_theta = np.arctan2(between_vector[1], between_vector[0])
# Rotate that theta by 90 deg to get the angle through the buoys
yaw_theta += np.pi / 2.0
p = midpoint
q = tf.transformations.quaternion_from_euler(0, 0, yaw_theta)
target_pose = PoseStamped()
target_pose.header = navigator_tools.make_header(frame="enu")
target_pose.pose = navigator_tools.numpy_quat_pair_to_pose(p, q)
return StartGateResponse(target=target_pose, success=True)
