calibrate_segment.cpp
Go to the documentation of this file.
00001 
00002 #include <ros/ros.h>
00003 #include <vicon_bridge/viconCalibrateSegment.h>
00004 #include <iostream>
00005 
00006 int main(int argc, char** argv)
00007 {
00008 
00009   ros::init(argc, argv, "call_calibrate_segment");
00010   ros::NodeHandle nh;
00011 
00012   if (!(argc == 3 || argc == 4))
00013   {
00014     std::cout << "usage: calibrate <subject name> <segment name> [z offset in m]" << std::endl;
00015     return 0;
00016   }
00017 
00018   ros::ServiceClient client = nh.serviceClient<vicon_bridge::viconCalibrateSegment> ("vicon/calibrate_segment");
00019 
00020   vicon_bridge::viconCalibrateSegment::Request req;
00021   vicon_bridge::viconCalibrateSegment::Response resp;
00022 
00023   req.n_measurements = 100;
00024 
00025   req.subject_name = argv[1];
00026   req.segment_name = argv[2];
00027 
00028   if (argc == 4)
00029   {
00030     //          req.z_offset = atof(argv[3]);
00031     std::istringstream i(argv[3]);
00032     if (!(i >> req.z_offset))
00033     {
00034       ROS_ERROR_STREAM("cannot convert z offset to a number");
00035       return 0;
00036     }
00037   }
00038   else
00039     req.z_offset = 0;
00040 
00041   ROS_INFO("trying to calibrate %s/%s with z offset = %f m", req.subject_name.c_str(), req.segment_name.c_str(), req.z_offset);
00042 
00043   client.call(req, resp);
00044 
00045   if (resp.success)
00046   {
00047     const geometry_msgs::Quaternion & q = resp.pose.pose.orientation;
00048     const geometry_msgs::Point & t = resp.pose.pose.position;
00049     ROS_INFO("calibration successful, q=[ %f %f %f %f ], t=[ %f %f %f ] ", q.w, q.x, q.y, q.x, t.x, t.y, t.z);
00050   }
00051   else
00052   {
00053     ROS_ERROR_STREAM("calibration failed: "<<resp.status);
00054   }
00055 
00056   return 0;
00057 }


vicon_bridge
Author(s):
autogenerated on Mon Sep 14 2015 03:57:50