visp_hand2eye_calibration: visp_hand2eye_calibration: calibrator.cpp Source File

00001 /****************************************************************************
00002  *
00003  * $Id: file.cpp 3496 2011-11-22 15:14:32Z fnovotny $
00004  *
00005  * This file is part of the ViSP software.
00006  * Copyright (C) 2005 - 2012 by INRIA. All rights reserved.
00007  *
00008  * This software is free software; you can redistribute it and/or
00009  * modify it under the terms of the GNU General Public License
00010  * ("GPL") version 2 as published by the Free Software Foundation.
00011  * See the file LICENSE.txt at the root directory of this source
00012  * distribution for additional information about the GNU GPL.
00013  *
00014  * For using ViSP with software that can not be combined with the GNU
00015  * GPL, please contact INRIA about acquiring a ViSP Professional
00016  * Edition License.
00017  *
00018  * See http://www.irisa.fr/lagadic/visp/visp.html for more information.
00019  *
00020  * This software was developed at:
00021  * INRIA Rennes - Bretagne Atlantique
00022  * Campus Universitaire de Beaulieu
00023  * 35042 Rennes Cedex
00024  * France
00025  * http://www.irisa.fr/lagadic
00026  *
00027  * If you have questions regarding the use of this file, please contact
00028  * INRIA at visp@inria.fr
00029  *
00030  * This file is provided AS IS with NO WARRANTY OF ANY KIND, INCLUDING THE
00031  * WARRANTY OF DESIGN, MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE.
00032  *
00033  * Contact visp@irisa.fr if any conditions of this licensing are
00034  * not clear to you.
00035  *
00036  * Description:
00037  * Calibrator node
00038  *
00039  * Authors:
00040  * Filip Novotny
00041  *
00042  *
00043  *****************************************************************************/
00044 
00050 #include "calibrator.h"
00051 #include "conversions/3dpose.h"
00052 #include "names.h"
00053 #include "visp/vpCalibration.h"
00054 #include "visp/vpHomogeneousMatrix.h"
00055 
00056 
00057 namespace visp_hand2eye_calibration
00058 {
00059   void Calibrator::cameraObjectCallback(const geometry_msgs::Transform::ConstPtr& trans)
00060   {
00061     ROS_DEBUG("new cMo: [%f,%f,%f -- %f,%f,%f,%f]", trans->translation.x,trans->translation.y,trans->translation.z,trans->rotation.x,trans->rotation.y,trans->rotation.z,trans->rotation.w);
00062     vpHomogeneousMatrix cMo;
00063     cMo = visp_bridge::toVispHomogeneousMatrix(*trans);
00064     cMo_vec_.push_back(cMo);
00065   }
00066 
00067   void Calibrator::worldEffectorCallback(const geometry_msgs::Transform::ConstPtr& trans)
00068   {
00069     ROS_DEBUG("new wMe: [%f,%f,%f -- %f,%f,%f,%f]", trans->translation.x,trans->translation.y,trans->translation.z,trans->rotation.x,trans->rotation.y,trans->rotation.z,trans->rotation.w);
00070     vpHomogeneousMatrix wMe;
00071     wMe = visp_bridge::toVispHomogeneousMatrix(*trans);
00072     wMe_vec_.push_back(wMe);
00073   }
00074 
00075   bool Calibrator::computeEffectorCameraCallback(
00076                                                     visp_hand2eye_calibration::compute_effector_camera::Request &req,
00077                                                     visp_hand2eye_calibration::compute_effector_camera::Response &res)
00078   {
00079     if (cMo_vec_.size() != wMe_vec_.size() || wMe_vec_.size() < 2)
00080     {
00081       ROS_ERROR("transformation vectors have different sizes or contain too few elements");
00082       return false;
00083     }
00084 
00085     ROS_INFO("computing %d values...",(int)wMe_vec_.size());
00086     vpHomogeneousMatrix eMc;
00087     #if VISP_VERSION_INT > (2<<16 | 6<<8 | 1)
00088     vpCalibration::calibrationTsai(cMo_vec_, wMe_vec_, eMc);
00089     #else
00090     vpCalibration::calibrationTsai(cMo_vec_.size(),&(cMo_vec_[0]),&(wMe_vec_[0]),eMc);
00091     #endif
00092     geometry_msgs::Transform trans;
00093     trans = visp_bridge::toGeometryMsgsTransform(eMc);
00094 
00095     res.effector_camera = trans;
00096     return true;
00097   }
00098 
00099   bool Calibrator::computeEffectorCameraQuickCallback(
00100                                                           visp_hand2eye_calibration::compute_effector_camera_quick::Request &req,
00101                                                           visp_hand2eye_calibration::compute_effector_camera_quick::Response &res)
00102   {
00103     visp_hand2eye_calibration::TransformArray camera_object = req.camera_object;
00104     visp_hand2eye_calibration::TransformArray world_effector = req.world_effector;
00105     std::vector<vpHomogeneousMatrix> cMo_vec;
00106     std::vector<vpHomogeneousMatrix> wMe_vec;
00107     for(unsigned int i=0;i<camera_object.transforms.size();i++){
00108       cMo_vec.push_back(visp_bridge::toVispHomogeneousMatrix(camera_object.transforms[i]));
00109       wMe_vec.push_back(visp_bridge::toVispHomogeneousMatrix(world_effector.transforms[i]));
00110     }
00111     if (camera_object.transforms.size() != world_effector.transforms.size() || world_effector.transforms.size() < 2)
00112     {
00113       ROS_ERROR("transformation vectors have different sizes or contain too few elements");
00114       return false;
00115     }
00116 
00117     ROS_INFO("computing...");
00118     vpHomogeneousMatrix eMc;
00119     #if VISP_VERSION_INT > (2<<16 | 6<<8 | 1)
00120     vpCalibration::calibrationTsai(cMo_vec, wMe_vec, eMc);
00121     #else
00122     vpCalibration::calibrationTsai(cMo_vec.size(),&(cMo_vec[0]),&(wMe_vec[0]),eMc);
00123     #endif
00124     geometry_msgs::Transform trans;
00125     trans = visp_bridge::toGeometryMsgsTransform(eMc);
00126     res.effector_camera = trans;
00127     return true;
00128   }
00129 
00130   bool Calibrator::resetCallback(visp_hand2eye_calibration::reset::Request &req,
00131                                   visp_hand2eye_calibration::reset::Response &res)
00132   {
00133     ROS_INFO("reseting...");
00134     cMo_vec_.clear();
00135     wMe_vec_.clear();
00136     return true;
00137   }
00138 
00139   Calibrator::Calibrator() :
00140     check_inputs_(ros::NodeHandle(), ros::this_node::getName()),
00141     queue_size_(1000)
00142   {
00143 
00144     //prepare function objects
00145     camera_object_subscriber_callback_t camera_object_callback = boost::bind(&Calibrator::cameraObjectCallback, this, _1);
00146     world_effector_subscriber_t world_effector_callback = boost::bind(&Calibrator::worldEffectorCallback, this, _1);
00147 
00148     compute_effector_camera_service_callback_t compute_effector_camera_callback =
00149         boost::bind(&Calibrator::computeEffectorCameraCallback, this, _1, _2);
00150     compute_effector_camera_quick_service_callback_t compute_effector_camera_quick_callback =
00151         boost::bind(&Calibrator::computeEffectorCameraQuickCallback, this, _1, _2);
00152     reset_service_callback_t reset_callback = boost::bind(&Calibrator::resetCallback, this, _1, _2);
00153 
00154     //define services
00155     compute_effector_camera_service_
00156         = n_.advertiseService(visp_hand2eye_calibration::compute_effector_camera_service,
00157                               compute_effector_camera_callback);
00158     compute_effector_camera_quick_service_
00159         = n_.advertiseService(visp_hand2eye_calibration::compute_effector_camera_quick_service,
00160                               compute_effector_camera_quick_callback);
00161     reset_service_ = n_.advertiseService(visp_hand2eye_calibration::reset_service, reset_callback);
00162 
00163     ros::V_string topics;
00164     topics.push_back(visp_hand2eye_calibration::camera_object_topic);
00165     topics.push_back(visp_hand2eye_calibration::world_effector_topic);
00166 
00167 
00168     check_inputs_.start(topics, 60.0);
00169     if (!ros::ok())
00170       return;
00171     //define subscribers
00172     camera_object_subscriber_ = n_.subscribe(visp_hand2eye_calibration::camera_object_topic, queue_size_,
00173                                              camera_object_callback);
00174     world_effector_subscriber_ = n_.subscribe(visp_hand2eye_calibration::world_effector_topic, queue_size_,
00175                                               world_effector_callback);
00176 
00177 
00178   }
00179 
00180   void Calibrator::spin()
00181   {
00182     ros::spin();
00183   }
00184   Calibrator::~Calibrator()
00185   {
00186 
00187   }
00188 }