def get_second_image(self, image_topic):
# Set exposure
print "Changing the exposure of camera to 0.2"
self.client.update_configuration({'exposure': 0.2})
rospy.sleep(0.1)
image1 = TopicUtils.get_next_message(image_topic, Image)
print "Changing the exposure of camera to 0.4"
self.client.update_configuration({'exposure': 0.4})
rospy.sleep(0.1)
image2 = TopicUtils.get_next_message(image_topic, Image)
return (image1, image2)
def get_second_image(self,image_topic):
# Set exposure
print "Changing the exposure of camera to 0.2"
self.client.update_configuration({'exposure':0.2})
rospy.sleep(0.1)
image1 = TopicUtils.get_next_message(image_topic,Image)
print "Changing the exposure of camera to 0.4"
self.client.update_configuration({'exposure':0.4})
rospy.sleep(0.1)
image2 = TopicUtils.get_next_message(image_topic,Image)
return (image1,image2)
def process_mono(self,req):
image_topic = "/%s/image_rect_color"%req.camera
info_topic = "/%s/camera_info"%req.camera
if self.ignore_first_picture:
image = None
else:
image = TopicUtils.get_next_message(image_topic,Image)
info = TopicUtils.get_next_message(info_topic,CameraInfo)
(click_points,params,param_names,image_annotated) = self.unpack_and_process(image,info)
#Publish annotated image stream
self.anno_pub.publish(image_annotated)
#Compute 3d points
pts3d = []
try:
convert_to_3d = rospy.ServiceProxy(self.convert_mono_service,ConvertPoint)
pts3d = [convert_to_3d(pt).pt3d for pt in click_points]
except rospy.ServiceException,e:
rospy.loginfo("Service Call Failed: %s"%e)
def process_stereo(self,req):
image_topic_left = "/%s/left/image_rect_color"%req.camera
info_topic_left = "%s/left/camera_info"%req.camera
image_topic_right = "/%s/right/image_rect_color"%req.camera
info_topic_right = "%s/right/camera_info"%req.camera
image_left = TopicUtils.get_next_message(image_topic_left,Image)
info_left = TopicUtils.get_next_message(info_topic_left,CameraInfo)
image_right = TopicUtils.get_next_message(image_topic_right,Image)
info_right = TopicUtils.get_next_message(info_topic_right,CameraInfo)
(click_points_left,params,param_names,image_annotated_left) = self.unpack_and_process(image_left,info_left)
(click_points_right,params,param_names,image_annotated_right) = self.unpack_and_process(image_right,info_right)
#Publish annotated image stream
self.left_anno_pub.publish(image_annotated_left)
self.right_anno_pub.publish(image_annotated_right)
#Compute 3d points
try:
convert_to_3d = rospy.ServiceProxy(self.convert_stereo_service,ConvertPoints)
pts3d = [convert_to_3d(click_points_left[i],click_points_right[i]).pt3d for i in range(len(click_points_left))]
except rospy.ServiceException,e:
rospy.loginfo("Service Call Failed: %s"%e)
def get_info(self):
info = TopicUtils.get_next_message(self.cameraInfoTopic,CameraInfo)
return info
def get_image(self):
image = TopicUtils.get_next_message(self.cameraTopic,Image)
return image
