client.cpp
Go to the documentation of this file.
00001 /****************************************************************************
00002  *
00003  * $Id: file.h 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  * Client node
00038  *
00039  * Authors:
00040  * Filip Novotny
00041  *
00042  *
00043  *****************************************************************************/
00044 
00050 #include "client.h"
00051 #include <geometry_msgs/Transform.h>
00052 #include "visp_hand2eye_calibration/TransformArray.h"
00053 #include <visp_bridge/3dpose.h>
00054 #include "names.h"
00055 
00056 #include <visp/vpCalibration.h>
00057 #include <visp/vpExponentialMap.h>
00058 
00059 namespace visp_hand2eye_calibration
00060 {
00061 Client::Client()
00062 {
00063   camera_object_publisher_
00064       = n_.advertise<geometry_msgs::Transform> (visp_hand2eye_calibration::camera_object_topic, 1000);
00065   world_effector_publisher_
00066       = n_.advertise<geometry_msgs::Transform> (visp_hand2eye_calibration::world_effector_topic, 1000);
00067 
00068   reset_service_
00069       = n_.serviceClient<visp_hand2eye_calibration::reset> (visp_hand2eye_calibration::reset_service);
00070   compute_effector_camera_service_
00071       = n_.serviceClient<visp_hand2eye_calibration::compute_effector_camera> (
00072                                                                                       visp_hand2eye_calibration::compute_effector_camera_service);
00073   compute_effector_camera_quick_service_
00074       = n_.serviceClient<visp_hand2eye_calibration::compute_effector_camera_quick> (
00075                                                                                             visp_hand2eye_calibration::compute_effector_camera_quick_service);
00076 }
00077 
00078 void Client::initAndSimulate()
00079 {
00080   ROS_INFO("Waiting for topics...");
00081   ros::Duration(1.).sleep();
00082   while(!reset_service_.call(reset_comm)){
00083     if(!ros::ok()) return;
00084     ros::Duration(1).sleep();
00085   }
00086 
00087 
00088   // We want to calibrate the hand to eye extrinsic camera parameters from 6 couple of poses: cMo and wMe
00089   const int N = 6;
00090   // Input: six couple of poses used as input in the calibration proces
00091   vpHomogeneousMatrix cMo; // eye (camera) to object transformation. The object frame is attached to the calibrartion grid
00092   vpHomogeneousMatrix wMe; // world to hand (end-effector) transformation
00093   vpHomogeneousMatrix eMc; // hand (end-effector) to eye (camera) transformation
00094 
00095   // Initialize an eMc transformation used to produce the simulated input transformations cMo and wMe
00096   vpTranslationVector etc(0.1, 0.2, 0.3);
00097   vpThetaUVector erc;
00098   erc[0] = vpMath::rad(10); // 10 deg
00099   erc[1] = vpMath::rad(-10); // -10 deg
00100   erc[2] = vpMath::rad(25); // 25 deg
00101 
00102   eMc.buildFrom(etc, erc);
00103   ROS_INFO("1) GROUND TRUTH:");
00104 
00105   ROS_INFO_STREAM("hand to eye transformation: " <<std::endl<<visp_bridge::toGeometryMsgsTransform(eMc)<<std::endl);
00106 
00107   vpColVector v_c(6); // camera velocity used to produce 6 simulated poses
00108   for (int i = 0; i < N; i++)
00109   {
00110     v_c = 0;
00111     if (i == 0)
00112     {
00113       // Initialize first poses
00114       cMo.buildFrom(0, 0, 0.5, 0, 0, 0); // z=0.5 m
00115       wMe.buildFrom(0, 0, 0, 0, 0, 0); // Id
00116     }
00117     else if (i == 1)
00118       v_c[3] = M_PI / 8;
00119     else if (i == 2)
00120       v_c[4] = M_PI / 8;
00121     else if (i == 3)
00122       v_c[5] = M_PI / 10;
00123     else if (i == 4)
00124       v_c[0] = 0.5;
00125     else if (i == 5)
00126       v_c[1] = 0.8;
00127 
00128     vpHomogeneousMatrix cMc; // camera displacement
00129     cMc = vpExponentialMap::direct(v_c); // Compute the camera displacement due to the velocity applied to the camera
00130     if (i > 0)
00131     {
00132       // From the camera displacement cMc, compute the wMe and cMo matrixes
00133       cMo = cMc.inverse() * cMo;
00134       wMe = wMe * eMc * cMc * eMc.inverse();
00135 
00136     }
00137 
00138     geometry_msgs::Transform pose_c_o;
00139     pose_c_o = visp_bridge::toGeometryMsgsTransform(cMo);
00140     geometry_msgs::Transform pose_w_e;
00141     pose_w_e = visp_bridge::toGeometryMsgsTransform(wMe);
00142     camera_object_publisher_.publish(pose_c_o);
00143     world_effector_publisher_.publish(pose_w_e);
00144     emc_quick_comm.request.camera_object.transforms.push_back(pose_c_o);
00145     emc_quick_comm.request.world_effector.transforms.push_back(pose_w_e);
00146 
00147   }
00148   ros::Duration(1.).sleep();
00149 
00150 }
00151 
00152 void Client::computeUsingQuickService()
00153 {
00154   vpHomogeneousMatrix eMc;
00155   vpThetaUVector erc;
00156   ROS_INFO("2) QUICK SERVICE:");
00157   if (compute_effector_camera_quick_service_.call(emc_quick_comm))
00158   {
00159     ROS_INFO_STREAM("hand_camera: "<< std::endl << emc_quick_comm.response.effector_camera);
00160   }
00161   else
00162   {
00163     ROS_ERROR("Failed to call service");
00164   }
00165 }
00166 
00167 void Client::computeFromTopicStream()
00168 {
00169   vpHomogeneousMatrix eMc;
00170   vpThetaUVector erc;
00171   ROS_INFO("3) TOPIC STREAM:");
00172   if (compute_effector_camera_service_.call(emc_comm))
00173   {
00174     ROS_INFO_STREAM("hand_camera: " << std::endl << emc_comm.response.effector_camera);
00175   }
00176   else
00177   {
00178     ROS_ERROR("Failed to call service");
00179   }
00180 
00181 }
00182 }
00183 
00184 /*
00185  * Local variables:
00186  * c-basic-offset: 2
00187  * End:
00188  */


visp_hand2eye_calibration
Author(s): Filip Novotny
autogenerated on Mon Oct 6 2014 08:40:31