def cbSrvSetCameraInfo(self, req):
rospy.loginfo("[cbSrvSetCameraInfo] Callback!")
filename = self.cali_file_folder + rospy.get_namespace().strip("/") + ".yaml"
response = SetCameraInfoResponse()
response.success = self.saveCameraInfo(req.camera_info, filename)
response.status_message = "Write to %s" % filename
return response
def cbSrvSetCameraInfo(self, req):
# TODO: save req.camera_info to yaml file
rospy.loginfo("[cbSrvSetCameraInfo] Callback!")
filename = self.cali_file_folder + "intrinsic" + ".yaml"
response = SetCameraInfoResponse()
response.success = self.saveCameraInfo(req.camera_info, filename)
response.status_message = "Write to %s" % filename #TODO file name
return response
def cbSrvSetCameraInfo(self,req):
# TODO: save req.camera_info to yaml file
rospy.loginfo("[cbSrvSetCameraInfo] Callback!")
filename = self.cali_file_folder + rospy.get_namespace().strip("/") + ".yaml"
response = SetCameraInfoResponse()
response.success = self.saveCameraInfo(req.camera_info,filename)
response.status_message = "Write to %s" %filename #TODO file name
return response
def setCameraInfo(self, req):
rospy.logdebug('SetCameraInfo received for ' + self.cname)
self.camera_info = req.camera_info
res = SetCameraInfoResponse()
res.success = saveCalibrationFile(req.camera_info, self.file, self.cname)
if not res.success:
res.status_message = "Error storing camera calibration."
return res
def set_camera_info_right(self, srv): srv.camera_info.header.frame_id = "camr" self.cam_right_calibration = srv.camera_info with open(self.right_cal_file,'w') as f: f.write(yaml.dump(image_processing.camera_info_to_yaml(srv.camera_info))) response = SetCameraInfoResponse() response.success = True response.status_message = "" return response
def srvSetCameraInfo(self, req):
# TODO: save req.camera_info to yaml file
rospy.loginfo("[cbSrvSetCameraInfo] Callback!")
# filename = self.cali_file_folder + "pi_cam_%dx%d" % (480,360) + ".yaml"
filename = self.cali_file_folder + \
time.strftime('%Y-%m-%d_%H-%M-%S') + ".yaml"
response = SetCameraInfoResponse()
response.success = self.saveCameraInfo(req.camera_info, filename)
response.status_message = "Write to %s" % filename # TODO file name
return response
def setCameraInfo(self, req):
""" Callback for SetCameraInfo request.
:param req: SetCameraInfo request message.
:returns: SetCameraInfo response message, success is True if
message handled.
:post: camera_info updated, can be used immediately without
reloading.
"""
rospy.logdebug('SetCameraInfo received for ' + self.cname)
self.camera_info = req.camera_info
rsp = SetCameraInfoResponse()
rsp.success = saveCalibration(req.camera_info, self.url, self.cname)
if not rsp.success:
rsp.status_message = "Error storing camera calibration."
return rsp
def setCameraInfo(self, req):
""" Callback for SetCameraInfo request.
:param req: SetCameraInfo request message.
:returns: SetCameraInfo response message, success is True if
message handled.
:post: camera_info updated, can be used immediately without
reloading.
"""
rospy.logdebug('SetCameraInfo received for ' + self.cname)
self.camera_info = req.camera_info
rsp = SetCameraInfoResponse()
rsp.success = saveCalibration(req.camera_info,
self.url, self.cname)
if not rsp.success:
rsp.status_message = "Error storing camera calibration."
return rsp
def on_save_camera_info(self, req):
# Save camera info (calibration) to a yaml file.
cam_info = req.camera_info
calib = {
'image_width': cam_info.width,
'image_height': cam_info.height,
'camera_name': rospy.get_name().strip("/"),
'distortion_model': cam_info.distortion_model,
'distortion_coefficients': {
'data': cam_info.D,
'rows': 1,
'cols': 5
},
'camera_matrix': {
'data': cam_info.K,
'rows': 3,
'cols': 3
},
'rectification_matrix': {
'data': cam_info.R,
'rows': 3,
'cols': 3
},
'projection_matrix': {
'data': cam_info.P,
'rows': 3,
'cols': 4
}
}
# Prepare response
file_name = self.get_config_path(self.veh_name)
response = SetCameraInfoResponse()
response.success = False
response.status_message = "Failed to save calibration to %s" % file_name
# Write to file
try:
out_file = open(file_name, 'w')
yaml.safe_dump(calib, out_file)
except IOError:
return response
rospy.loginfo("[%s] Saved calibration to %s." %
(self.node_name, file_name))
response.success = True
response.status_message = "Saved calibration to %s" % file_name
return response
#!/usr/bin/env python from sensor_msgs.srv import SetCameraInfoRequest, SetCameraInfoResponse from myproject.srv import BatResponse, BatRequest, BazRequest, BazResponse cam_req = SetCameraInfoRequest() cam_rep = SetCameraInfoResponse() bat_req = BatRequest() bat_rep = BatResponse() baz_req = BazRequest() baz_rep = BazResponse()
