def request_buoy(self, srv):
print 'requesting', srv
response = self.find_single_buoy(np.copy(self.last_image), srv.target_name)
if response is False:
print 'did not find'
resp = VisionRequest2DResponse(
header=sub8_ros_tools.make_header(frame='/stereo_front/right'),
found=False
)
else:
# Fill in
center, radius = response
resp = VisionRequest2DResponse(
header=Header(stamp=self.last_image_time, frame_id='/stereo_front/right'),
pose=Pose2D(
x=center[0],
y=center[1],
),
max_x=self.last_image.shape[0],
max_y=self.last_image.shape[1],
camera_info=self.image_sub.camera_info,
found=True
)
return resp
def request_pipe(self, data):
if self.last_image is None:
return False # Fail if we have no images cached
timestamp = self.last_image_timestamp
position, orientation, _ = self.find_pipe_new(self.last_image)
found = (position is not None)
if not found:
resp = VisionRequest2DResponse(
header=Header(
stamp=timestamp,
frame_id='/down_camera'),
found=found,
camera_info=self.image_sub.camera_info,
)
else:
resp = VisionRequest2DResponse(
pose=Pose2D(
x=position[0],
y=position[1],
theta=orientation
),
max_x=self.last_image.shape[0],
max_y=self.last_image.shape[1],
camera_info=self.image_sub.camera_info,
header=Header(
stamp=timestamp,
frame_id='/down_camera'),
found=found
)
return resp
def request_bin(self, srv):
self.bin_type = srv.target_name
if (self.last_image != None):
print 'requesting', srv
response = self.find_single_bin(np.copy(self.last_image),
srv.target_name)
if response is False or response is None:
print 'did not find'
resp = VisionRequest2DResponse(
header=mil_ros_tools.make_header(frame='/down'),
found=False)
else:
# Fill in
center, radius = response
resp = VisionRequest2DResponse(
header=Header(stamp=self.last_image_time,
frame_id='/down'),
pose=Pose2D(x=center[0], y=center[1], theta=radius),
max_x=self.last_image.shape[0],
max_y=self.last_image.shape[1],
camera_info=self.image_sub.camera_info,
found=True)
return resp
def get_ideal_response(self, pts):
pts = np.array(pts)
res = VisionRequest2DResponse()
middle = pts[0]+(pts[1]-pts[0])/2
theta = np.arctan( (float(pts[1][1])-pts[0][1]) / (pts[1][0] - pts[0][0]) )
res.pose.x = middle[0]
res.pose.y = middle[1]
res.pose.theta = theta
return res
def get_ideal_response(self, pts):
pts = np.array(pts)
res = VisionRequest2DResponse()
middle = pts[0] + (pts[1] - pts[0]) / 2.0
vector = pts[1] - pts[0]
if vector[0] < 0.0:
vector[0] = -vector[0]
theta = np.arctan2(vector[1], vector[0])
res.pose.x = middle[0]
res.pose.y = middle[1]
res.pose.theta = theta
return res
def cb_2d(self, req):
res = VisionRequest2DResponse()
if (self.last3d == None or not self.enabled):
res.found = False
else:
res.header.frame_id = self.cam.tfFrame()
res.header.stamp = self.last_found_time
res.pose.x = self.last2d[0][0]
res.pose.y = self.last2d[0][1]
res.pose.theta = self.last2d[1]
res.found = True
return res
def request_buoy(self, srv):
'''
Callback for 2D vision request. Returns centroid
of buoy found with color specified in target_name
if found.
'''
if not self.enabled or srv.target_name not in self.buoys:
return VisionRequest2DResponse(found=False)
response = self.find_single_buoy(srv.target_name)
if response is False or response is None:
return VisionRequest2DResponse(found=False)
center, radius = response
return VisionRequest2DResponse(header=Header(
stamp=self.last_image_time, frame_id=self.frame_id),
pose=Pose2D(
x=center[0],
y=center[1],
),
max_x=self.last_image.shape[0],
max_y=self.last_image.shape[1],
camera_info=self.image_sub.camera_info,
found=True)
def _vision_cb_2D(self, req):
res = VisionRequest2DResponse()
if (self.last2d is None or not self.enabled):
res.found = False
else:
res.header.frame_id = self.cam.tfFrame()
res.header.stamp = self.last_found_time_2D
res.pose.x = self.last2d[0][0]
res.pose.y = self.last2d[0][1]
res.camera_info = self.camera_info
res.max_x = self.camera_info.width
res.max_y = self.camera_info.height
if self.last2d[1][0] < 0:
self.last2d[1][0] = -self.last2d[1][0]
self.last2d[1][1] = -self.last2d[1][1]
angle = np.arctan2(self.last2d[1][1], self.last2d[1][0])
res.pose.theta = angle
res.found = True
return res