Go to the documentation of this file.00001
00002
00003
00004
00005
00006
00007
00008
00009
00010
00011
00012
00013
00014
00015
00016
00017
00018
00019
00020 #include <fstream>
00021 #include <ctime>
00022
00023 #include <ros/ros.h>
00024 #include <rosbag/bag.h>
00025 #include <rosbag/view.h>
00026 #include <rosbag/query.h>
00027
00028 #include <std_msgs/String.h>
00029 #include <robot_calibration_msgs/CalibrationData.h>
00030 #include <robot_calibration_msgs/CaptureConfig.h>
00031
00032 #include <robot_calibration/capture/chain_manager.h>
00033 #include <robot_calibration/capture/feature_finder.h>
00034
00035 #include <camera_calibration_parsers/parse.h>
00036 #include <robot_calibration/ceres/optimizer.h>
00037 #include <robot_calibration/camera_info.h>
00038
00039 #include <boost/foreach.hpp>
00040
00065
00066
00067
00068
00069
00070
00071 int main(int argc, char** argv)
00072 {
00073 ros::init(argc, argv,"robot_calibration");
00074 ros::NodeHandle nh("~");
00075
00076
00077 bool verbose;
00078 nh.param<bool>("verbose", verbose, false);
00079
00080
00081 std_msgs::String description_msg;
00082 std::vector<robot_calibration_msgs::CalibrationData> data;
00083
00084
00085 std::string pose_bag_name("calibration_poses.bag");
00086 if (argc > 1)
00087 pose_bag_name = argv[1];
00088
00089 if (pose_bag_name.compare("--from-bag") != 0)
00090 {
00091
00092 robot_calibration::ChainManager chain_manager_(nh);
00093 robot_calibration::FeatureFinderMap finders_;
00094 if (!robot_calibration::loadFeatureFinders(nh, finders_))
00095 {
00096 ROS_FATAL("Unable to load feature finders!");
00097 return -1;
00098 }
00099
00100 ros::Publisher pub = nh.advertise<robot_calibration_msgs::CalibrationData>("/calibration_data", 10);
00101 ros::Publisher urdf_pub = nh.advertise<std_msgs::String>("/robot_description", 1, true);
00102
00103
00104 if (!nh.getParam("/robot_description", description_msg.data))
00105 {
00106 ROS_FATAL("robot_description not set!");
00107 return -1;
00108 }
00109 urdf_pub.publish(description_msg);
00110
00111
00112 std::vector<robot_calibration_msgs::CaptureConfig> poses;
00113 if (pose_bag_name.compare("--manual") != 0)
00114 {
00115 ROS_INFO_STREAM("Opening " << pose_bag_name);
00116 rosbag::Bag bag;
00117 try
00118 {
00119 bag.open(pose_bag_name, rosbag::bagmode::Read);
00120 }
00121 catch (rosbag::BagException)
00122 {
00123 ROS_FATAL_STREAM("Cannot open " << pose_bag_name);
00124 return -1;
00125 }
00126 rosbag::View data_view(bag, rosbag::TopicQuery("calibration_joint_states"));
00127
00128 BOOST_FOREACH (rosbag::MessageInstance const m, data_view)
00129 {
00130 robot_calibration_msgs::CaptureConfig::ConstPtr msg = m.instantiate<robot_calibration_msgs::CaptureConfig>();
00131 if (msg == NULL)
00132 {
00133
00134 sensor_msgs::JointState::ConstPtr js_msg = m.instantiate<sensor_msgs::JointState>();
00135 if (js_msg != NULL)
00136 {
00137 robot_calibration_msgs::CaptureConfig config;
00138 config.joint_states = *js_msg;
00139
00140 for (robot_calibration::FeatureFinderMap::iterator it = finders_.begin();
00141 it != finders_.end();
00142 it++)
00143 {
00144 config.features.push_back(it->first);
00145 }
00146 poses.push_back(config);
00147 }
00148 }
00149 else
00150 {
00151 poses.push_back(*msg);
00152 }
00153 }
00154 }
00155 else
00156 {
00157 ROS_INFO("Using manual calibration mode...");
00158 }
00159
00160
00161 for (unsigned pose_idx = 0;
00162 (pose_idx < poses.size() || poses.size() == 0) && ros::ok();
00163 ++pose_idx)
00164 {
00165 robot_calibration_msgs::CalibrationData msg;
00166
00167 if (poses.size() == 0)
00168 {
00169
00170 ROS_INFO("Press key when arm is ready... (type 'done' to finish capture)");
00171 std::string throwaway;
00172 std::getline(std::cin, throwaway);
00173 if (throwaway.compare("done") == 0)
00174 break;
00175 if (!ros::ok())
00176 break;
00177 }
00178 else
00179 {
00180
00181 if (!chain_manager_.moveToState(poses[pose_idx].joint_states))
00182 {
00183 ROS_WARN("Unable to move to desired state for sample %u.", pose_idx);
00184 continue;
00185 }
00186 }
00187
00188
00189 chain_manager_.waitToSettle();
00190
00191
00192 ros::Duration(0.1).sleep();
00193
00194
00195 bool found_all_features = true;
00196 if (poses.size() == 0)
00197 {
00198
00199 for (robot_calibration::FeatureFinderMap::iterator it = finders_.begin();
00200 it != finders_.end();
00201 it++)
00202 {
00203 if (!it->second->find(&msg))
00204 {
00205 ROS_WARN("%s failed to capture features.", it->first.c_str());
00206 found_all_features = false;
00207 break;
00208 }
00209 }
00210 }
00211 else
00212 {
00213
00214 for (size_t i = 0; i < poses[pose_idx].features.size(); i++)
00215 {
00216 std::string feature = poses[pose_idx].features[i];
00217 if (!finders_[feature]->find(&msg))
00218 {
00219 ROS_WARN("%s failed to capture features.", feature.c_str());
00220 found_all_features = false;
00221 break;
00222 }
00223 }
00224 }
00225
00226
00227 if (found_all_features)
00228 {
00229 ROS_INFO("Captured pose %u", pose_idx);
00230 }
00231 else
00232 {
00233 ROS_WARN("Failed to capture sample %u.", pose_idx);
00234 continue;
00235 }
00236
00237
00238 chain_manager_.getState(&msg.joint_states);
00239
00240
00241 pub.publish(msg);
00242
00243
00244 data.push_back(msg);
00245 }
00246
00247 ROS_INFO("Done capturing samples");
00248 }
00249 else
00250 {
00251
00252 std::string data_bag_name("/tmp/calibration_data.bag");
00253 if (argc > 2)
00254 data_bag_name = argv[2];
00255 ROS_INFO_STREAM("Loading calibration data from " << data_bag_name);
00256
00257 rosbag::Bag bag_;
00258 try
00259 {
00260 bag_.open(data_bag_name, rosbag::bagmode::Read);
00261 }
00262 catch (rosbag::BagException)
00263 {
00264 ROS_FATAL_STREAM("Cannot open " << data_bag_name);
00265 return -1;
00266 }
00267
00268
00269 rosbag::View model_view_(bag_, rosbag::TopicQuery("robot_description"));
00270 if (model_view_.size() < 1)
00271 {
00272 std::cerr << "robot_description topic not found in bag file." << std::endl;
00273 return -1;
00274 }
00275 std_msgs::String::ConstPtr description_ = model_view_.begin()->instantiate<std_msgs::String>();
00276 description_msg = *description_;
00277
00278
00279 rosbag::View data_view_(bag_, rosbag::TopicQuery("calibration_data"));
00280 BOOST_FOREACH (rosbag::MessageInstance const m, data_view_)
00281 {
00282 robot_calibration_msgs::CalibrationData::ConstPtr msg = m.instantiate<robot_calibration_msgs::CalibrationData>();
00283 data.push_back(*msg);
00284 }
00285 }
00286
00287
00288 robot_calibration::OptimizationParams params;
00289 params.LoadFromROS(nh);
00290 robot_calibration::Optimizer opt(description_msg.data);
00291 opt.optimize(params, data, verbose);
00292 if (verbose)
00293 {
00294 std::cout << "Parameter Offsets:" << std::endl;
00295 std::cout << opt.getOffsets()->getOffsetYAML() << std::endl;
00296 }
00297
00298
00299 std::string s = opt.getOffsets()->updateURDF(description_msg.data);
00300
00301
00302 char datecode[80];
00303 {
00304 std::time_t t = std::time(NULL);
00305 std::strftime(datecode, 80, "%Y_%m_%d_%H_%M_%S", std::localtime(&t));
00306 }
00307
00308
00309 {
00310 std::stringstream urdf_name;
00311 urdf_name << "/tmp/calibrated_" << datecode << ".urdf";
00312 std::ofstream file;
00313 file.open(urdf_name.str().c_str());
00314 file << s;
00315 file.close();
00316 }
00317
00318
00319 {
00320 std::stringstream depth_name;
00321 depth_name << "/tmp/depth_" << datecode << ".yaml";
00322 camera_calibration_parsers::writeCalibration(depth_name.str(), "",
00323 robot_calibration::updateCameraInfo(
00324 opt.getOffsets()->get("camera_fx"),
00325 opt.getOffsets()->get("camera_fy"),
00326 opt.getOffsets()->get("camera_cx"),
00327 opt.getOffsets()->get("camera_cy"),
00328 data[0].observations[0].ext_camera_info.camera_info));
00329
00330 std::stringstream rgb_name;
00331 rgb_name << "/tmp/rgb_" << datecode << ".yaml";
00332 camera_calibration_parsers::writeCalibration(rgb_name.str(), "",
00333 robot_calibration::updateCameraInfo(
00334 opt.getOffsets()->get("camera_fx"),
00335 opt.getOffsets()->get("camera_fy"),
00336 opt.getOffsets()->get("camera_cx"),
00337 opt.getOffsets()->get("camera_cy"),
00338 data[0].observations[0].ext_camera_info.camera_info));
00339 }
00340
00341
00342 {
00343 std::stringstream yaml_name;
00344 yaml_name << "/tmp/calibration_" << datecode << ".yaml";
00345 std::ofstream file;
00346 file.open(yaml_name.str().c_str());
00347 file << opt.getOffsets()->getOffsetYAML();
00348 file << "depth_info: depth_" << datecode << ".yaml" << std::endl;
00349 file << "rgb_info: rgb_" << datecode << ".yaml" << std::endl;
00350 file << "urdf: calibrated_" << datecode << ".urdf" << std::endl;
00351 file.close();
00352 }
00353
00354 ROS_INFO("Done calibrating");
00355
00356 return 0;
00357 }