manual_tf_alignment.cpp
Go to the documentation of this file.
00001 /*********************************************************************
00002  * Software License Agreement (BSD License)
00003  *
00004  *  Copyright (c) 2015, PickNik LLC
00005  *  All rights reserved.
00006  *
00007  *  Redistribution and use in source and binary forms, with or without
00008  *  modification, are permitted provided that the following conditions
00009  *  are met:
00010  *
00011  *   * Redistributions of source code must retain the above copyright
00012  *     notice, this list of conditions and the following disclaimer.
00013  *   * Redistributions in binary form must reproduce the above
00014  *     copyright notice, this list of conditions and the following
00015  *     disclaimer in the documentation and/or other materials provided
00016  *     with the distribution.
00017  *   * Neither the name of PickNik LLC nor the names of its
00018  *     contributors may be used to endorse or promote products derived
00019  *     from this software without specific prior written permission.
00020  *
00021  *  THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
00022  *  "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
00023  *  LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
00024  *  FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
00025  *  COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
00026  *  INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
00027  *  BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
00028  *  LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
00029  *  CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
00030  *  LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
00031  *  ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
00032  *  POSSIBILITY OF SUCH DAMAGE.
00033  *********************************************************************/
00034 
00035 /*
00036  * Author : Andy McEvoy, Dave Coleman
00037  * Desc   : Allows manual control of a TF through the keyboard
00038  */
00039 
00040 #include <tf_keyboard_cal/manual_tf_alignment.h>
00041 
00042 // Parameter loading
00043 #include <rosparam_shortcuts/rosparam_shortcuts.h>
00044 
00045 #include <iostream>
00046 #include <fstream>
00047 #include <string>
00048 
00049 #include <ros/package.h>
00050 
00051 namespace tf_keyboard_cal
00052 {
00053 
00054 ManualTFAlignment::ManualTFAlignment()
00055   : nh_("~")
00056   , name_("manipulation_data") // for namespacing logging messages
00057 {
00058   // set defaults
00059   mode_ = 1;
00060   delta_ = 0.010;
00061 
00062   // initial camera transform
00063   double x, y, z, roll, pitch, yaw;
00064 
00065   // Get settings from rosparam
00066   std::size_t error = 0;
00067   error += !rosparam_shortcuts::get(name_, nh_, "initial_x", x);
00068   error += !rosparam_shortcuts::get(name_, nh_, "initial_y", y);
00069   error += !rosparam_shortcuts::get(name_, nh_, "initial_z", z);
00070   error += !rosparam_shortcuts::get(name_, nh_, "initial_roll", roll);
00071   error += !rosparam_shortcuts::get(name_, nh_, "initial_pitch", pitch);
00072   error += !rosparam_shortcuts::get(name_, nh_, "initial_yaw", yaw);
00073   error += !rosparam_shortcuts::get(name_, nh_, "file_package", file_package_);
00074   error += !rosparam_shortcuts::get(name_, nh_, "file_name", file_name_);
00075   error += !rosparam_shortcuts::get(name_, nh_, "topic_name", topic_name_);
00076   error += !rosparam_shortcuts::get(name_, nh_, "from", from_);
00077   error += !rosparam_shortcuts::get(name_, nh_, "to", to_);
00078   rosparam_shortcuts::shutdownIfError(name_, error);
00079 
00080   setPose(Eigen::Vector3d(x, y, z), Eigen::Vector3d(roll, pitch, yaw));
00081 
00082   // default, save in tf_keyboard_cal/data
00083   std::string package_path = ros::package::getPath(file_package_);
00084   save_path_ = package_path + file_name_;
00085 
00086   // listen to keyboard topic
00087   keyboard_sub_ = nh_.subscribe(topic_name_, 100,
00088                                 &ManualTFAlignment::keyboardCallback, this);
00089 
00090   // Echo info
00091   ROS_INFO_STREAM_NAMED("manualTF","Listening to topic : " << topic_name_);
00092   ROS_INFO_STREAM_NAMED("manualTF","Transform from     : " << from_);
00093   ROS_INFO_STREAM_NAMED("manualTF","Transform to       : " << to_);
00094   ROS_INFO_STREAM_NAMED("manualTF","Config File        : " << save_path_);
00095   ROS_INFO_STREAM_NAMED("manualTF","Initial transform  : " << x << ", " << y << ", " << z << ", " << roll << ", " << pitch << ", " << yaw );
00096 }
00097 
00098 void ManualTFAlignment::keyboardCallback(const keyboard::Key::ConstPtr& msg)
00099 {
00100   int entry = msg->code;
00101   const double fine = 0.001;
00102   const double coarse = 0.01;
00103   const double very_coarse = 0.1;
00104 
00105   //std::cout << "key: " << entry << std::endl;
00106 
00107   switch(entry)
00108   {
00109     case 112: //
00110       writeTFToFile();
00111       break;
00112     case 117: // (very coarse delta)
00113       std::cout << "Delta = very coarse (0.1)" << std::endl;
00114       delta_ = very_coarse;
00115       break;
00116     case 105: // (coarse delta)
00117       std::cout << "Delta = coarse (0.01)" << std::endl;
00118       delta_ = coarse;
00119       break;
00120     case 111: // (fine delta)
00121       std::cout << "Delta = fine (0.001)" << std::endl;
00122       delta_ = fine;
00123       break;
00124 
00125     // X axis
00126     case 113: // up
00127       updateTF(1, delta_);
00128       break;
00129     case 97: // down
00130       updateTF(1, -delta_);
00131       break;
00132 
00133     // y axis
00134     case 119: // up
00135       updateTF(2, delta_);
00136       break;
00137     case 115: // down
00138       updateTF(2, -delta_);
00139       break;
00140 
00141     // z axis
00142     case 101: // up
00143       updateTF(3, delta_);
00144       break;
00145     case 100: // down
00146       updateTF(3, -delta_);
00147       break;
00148 
00149     // roll
00150     case 114: // up
00151       updateTF(4, delta_);
00152       break;
00153     case 102: // down
00154       updateTF(4, -delta_);
00155       break;
00156 
00157     // pitch
00158     case 116: // up
00159       updateTF(5, delta_);
00160       break;
00161     case 103: // down
00162       updateTF(5, -delta_);
00163       break;
00164 
00165     // yaw
00166     case 121: // up
00167       updateTF(6, delta_);
00168       break;
00169     case 104: // down
00170       updateTF(6, -delta_);
00171       break;
00172 
00173     default:
00174       // don't do anything
00175       break;
00176   }
00177 }
00178 
00179 void ManualTFAlignment::printMenu()
00180 {
00181   std::cout << "Manual alignment of camera to world CS:" << std::endl;
00182   std::cout << "=======================================" << std::endl;
00183   std::cout << "MOVE: X  Y  Z  R  P  YAW " << std::endl;
00184   std::cout << "------------------------" << std::endl;
00185   std::cout << "up    q  w  e  r  t  y " << std::endl;
00186   std::cout << "down  a  s  d  f  g  h " << std::endl;
00187   std::cout << std::endl;
00188   std::cout << "Fast: u " << std::endl;
00189   std::cout << "Med:  i " << std::endl;
00190   std::cout << "Slow: o " << std::endl;
00191   std::cout << "Save: p " << std::endl;
00192 }
00193 
00194 void ManualTFAlignment::publishTF()
00195 {
00196   static tf::TransformBroadcaster br;
00197   tf::Transform transform;
00198   tf::Quaternion q;
00199 
00200   // set camera pose translation
00201   transform.setOrigin( tf::Vector3( translation_[0],
00202                                     translation_[1],
00203                                     translation_[2]) );
00204 
00205   // set camera pose rotation
00206   q.setRPY(rotation_[0], rotation_[1], rotation_[2]);
00207   transform.setRotation(q);
00208 
00209   // publish
00210   br.sendTransform(tf::StampedTransform(transform, ros::Time::now(), from_ , to_));
00211 }
00212 
00213 void ManualTFAlignment::setPose(Eigen::Vector3d translation, Eigen::Vector3d rotation)
00214 {
00215   translation_ = translation;
00216   rotation_ = rotation;
00217 }
00218 
00219 void ManualTFAlignment::updateTF(int mode, double delta)
00220 {
00221   ROS_DEBUG_STREAM_NAMED("tf_alignment","mode = " << mode << ", delta = " << delta);
00222 
00223   switch(mode)
00224   {
00225     case 1:
00226       translation_ += Eigen::Vector3d(delta, 0, 0);
00227       break;
00228     case 2:
00229       translation_ += Eigen::Vector3d(0, delta, 0);
00230       break;
00231     case 3:
00232       translation_ += Eigen::Vector3d(0, 0, delta);
00233       break;
00234     case 4:
00235       rotation_ += Eigen::Vector3d(delta, 0, 0);
00236       break;
00237     case 5:
00238       rotation_ += Eigen::Vector3d(0, delta, 0);
00239       break;
00240     case 6:
00241       rotation_ += Eigen::Vector3d(0, 0, delta);
00242       break;
00243     default:
00244       // don't do anything
00245       break;
00246   }
00247 }
00248 
00249 
00250 void ManualTFAlignment::writeTFToFile()
00251 {
00252   std::ofstream file (save_path_.c_str()); //, std::ios::app);
00253   ROS_INFO_STREAM_NAMED("tf_align.write","Writing transformation to file " << save_path_);
00254 
00255   if (!file.is_open())
00256     ROS_ERROR_STREAM_NAMED("tf_align.write","Output file could not be opened: " << save_path_);
00257   else
00258   {
00259     ROS_INFO_STREAM_NAMED("tf_align.write","Initial transform  : " << translation_[0] << ", " << translation_[1]
00260                           << ", " << translation_[2] << ", " << rotation_[0] << ", " << rotation_[1]
00261                           << ", " << rotation_[2] );
00262 
00263     file << "initial_x: " << translation_[0] << std::endl;
00264     file << "initial_y: " << translation_[1] << std::endl;
00265     file << "initial_z: " << translation_[2] << std::endl;
00266     file << "initial_roll: " << rotation_[0] << std::endl;
00267     file << "initial_pitch: " << rotation_[1] << std::endl;
00268     file << "initial_yaw: " << rotation_[2] << std::endl;
00269     file << "from: " << from_ << std::endl;
00270     file << "to: " << to_ << std::endl;
00271     file << "file_package: " << file_package_ << std::endl;
00272     file << "file_name: " << file_name_ << std::endl;
00273     file << "topic_name: " << topic_name_ << std::endl;
00274   }
00275   file.close();
00276 
00277 }
00278 
00279 }


tf_keyboard_cal
Author(s): Dave Coleman , Andy McEvoy
autogenerated on Thu Jun 6 2019 18:20:29