text: /*
   * Copyright (C) 2016-2019 Istituto Italiano di Tecnologia (IIT)
   *
   * This software may be modified and distributed under the terms of the
   * BSD 3-Clause license. See the accompanying LICENSE file for details.
   */
  
  #include <yarp/os/LogStream.h>
  #include <yarp/os/Property.h>
  #include <yarp/dev/Drivers.h>
  #include <yarp/dev/IVisualServoing.h>
  #include <yarp/dev/PolyDriver.h>
  #include <yarp/math/Math.h>
  
  using namespace yarp::os;
  using namespace yarp::sig;
  using namespace yarp::dev;
  using namespace yarp::math;
  
  YARP_DECLARE_PLUGINS(visualservoingplugin);
  
  int main(int argc, char **argv)
  {
      Network yarp;
      if (!yarp.checkNetwork(3.0))
      {
          yError("YARP seems unavailable!");
          return EXIT_FAILURE;
      }
  
      YARP_REGISTER_PLUGINS(visualservoingplugin);
  
      Property prop_server_vs;
      prop_server_vs.put("device",    "visualservoingserver");
      prop_server_vs.put("verbosity", true);
      prop_server_vs.put("simulate",  true);
      prop_server_vs.put("robot",     "icubSim");
      bool use_fwd_kin = false;
  
      PolyDriver drv_server_vs(prop_server_vs);
      if (!drv_server_vs.isValid())
      {
          yError("Could not run VisualServoingServer!");
          return EXIT_FAILURE;
      }
  
      IVisualServoing *visual_servoing;
      drv_server_vs.view(visual_servoing);
      if (visual_servoing == YARP_NULLPTR)
      {
          yError("Could not get interfacate to VisualServoingServer!");
          return EXIT_FAILURE;
      }
  
      /* Set visual servoing control*/
      visual_servoing->setVisualServoControl("decoupled");
  
      /* Stored set-up: t170904 */
      visual_servoing->storedInit("t170904");
  
      visual_servoing->initFacilities(use_fwd_kin);
      visual_servoing->storedGoToGoal("t170904");
  
      visual_servoing->checkVisualServoingController();
      visual_servoing->waitVisualServoingDone();
  
      visual_servoing->stopFacilities();
  
      /* Stored set-up: t170427 */
      visual_servoing->storedInit("t170427");
  
      visual_servoing->initFacilities(use_fwd_kin);
      visual_servoing->storedGoToGoal("t170427");
  
      visual_servoing->checkVisualServoingController();
      visual_servoing->waitVisualServoingDone();
  
      visual_servoing->stopFacilities();
  
      return EXIT_SUCCESS;
  }
  