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
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 }