def cbSrvSetCameraInfo(self, req):
# TODO: save req.camera_info to yaml file
self.log.info("[cbSrvSetCameraInfo] Callback!")
filename = self.cali_file_folder + self.get_namespace().strip(
"/") + ".yaml"
response = SetCameraInfo.Response()
response.success = self.saveCameraInfo(req.camera_info, filename)
response.status_message = "Write to %s" % filename #TODO file name
return response
def fill_set_camera_info():
cam_info = CameraInfo()
cam_info.header.frame_id = '/CameraTop_frame'
cam_info.header.stamp = rospy.Time.now()
cam_info.P[0] = 640.0
cam_info.P[1] = 0.0
cam_info.P[2] = 320.0
cam_info.P[3] = 0
cam_info.P[4] = 0.0
cam_info.P[5] = 373.31
cam_info.P[6] = 120.0
cam_info.P[7] = 0.0
cam_info.P[8] = 0.0
cam_info.P[9] = 0.0
cam_info.P[10] = 1.0
cam_info.P[11] = 0.0
setCameraInfo = SetCameraInfo()
setCameraInfo.camera_info = cam_info
return setCameraInfo
def fill_set_camera_info():
cam_info = CameraInfo()
cam_info.header.frame_id = "/CameraTop_frame"
cam_info.header.stamp = rospy.Time.now()
cam_info.P[0] = 640.0
cam_info.P[1] = 0.0
cam_info.P[2] = 320.0
cam_info.P[3] = 0
cam_info.P[4] = 0.0
cam_info.P[5] = 373.31
cam_info.P[6] = 120.0
cam_info.P[7] = 0.0
cam_info.P[8] = 0.0
cam_info.P[9] = 0.0
cam_info.P[10] = 1.0
cam_info.P[11] = 0.0
setCameraInfo = SetCameraInfo()
setCameraInfo.camera_info = cam_info
return setCameraInfo
# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
# "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
# LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
# FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
# COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
# INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
# BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
# LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
# CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
# LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
# ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
# POSSIBILITY OF SUCH DAMAGE.
#####################################################################
import roslib; roslib.load_manifest('wge100_camera')
import rospy
from sensor_msgs.srv import SetCameraInfo
from sensor_msgs.msg import CameraInfo
#service = rospy.ServiceProxy("camera/set_camera_info", SetCameraInfo)
service = rospy.ServiceProxy("wge100_camera/set_camera_info", SetCameraInfo)
info = CameraInfo()
info.height = 480
info.width = 640
info.K = [1, 0, 7, 0, 5, 4, 0, 0, 1]
req = SetCameraInfo()
req.camera_info = info
service(info)