00001
00002
00003
00004
00005
00006
00007
00008
00009
00010
00011
00012
00013
00014
00015
00016
00017
00018
00019
00020
00021
00022
00023
00024
00025
00026
00027
00028
00029
00030
00031
00032
00033
00034
00035
00036
00037
00038
00039
00040 #include <tf_keyboard_cal/manual_tf_alignment.h>
00041
00042
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")
00057 {
00058
00059 mode_ = 1;
00060 delta_ = 0.010;
00061
00062
00063 double x, y, z, roll, pitch, yaw;
00064
00065
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
00083 std::string package_path = ros::package::getPath(file_package_);
00084 save_path_ = package_path + file_name_;
00085
00086
00087 keyboard_sub_ = nh_.subscribe(topic_name_, 100,
00088 &ManualTFAlignment::keyboardCallback, this);
00089
00090
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
00106
00107 switch(entry)
00108 {
00109 case 112:
00110 writeTFToFile();
00111 break;
00112 case 117:
00113 std::cout << "Delta = very coarse (0.1)" << std::endl;
00114 delta_ = very_coarse;
00115 break;
00116 case 105:
00117 std::cout << "Delta = coarse (0.01)" << std::endl;
00118 delta_ = coarse;
00119 break;
00120 case 111:
00121 std::cout << "Delta = fine (0.001)" << std::endl;
00122 delta_ = fine;
00123 break;
00124
00125
00126 case 113:
00127 updateTF(1, delta_);
00128 break;
00129 case 97:
00130 updateTF(1, -delta_);
00131 break;
00132
00133
00134 case 119:
00135 updateTF(2, delta_);
00136 break;
00137 case 115:
00138 updateTF(2, -delta_);
00139 break;
00140
00141
00142 case 101:
00143 updateTF(3, delta_);
00144 break;
00145 case 100:
00146 updateTF(3, -delta_);
00147 break;
00148
00149
00150 case 114:
00151 updateTF(4, delta_);
00152 break;
00153 case 102:
00154 updateTF(4, -delta_);
00155 break;
00156
00157
00158 case 116:
00159 updateTF(5, delta_);
00160 break;
00161 case 103:
00162 updateTF(5, -delta_);
00163 break;
00164
00165
00166 case 121:
00167 updateTF(6, delta_);
00168 break;
00169 case 104:
00170 updateTF(6, -delta_);
00171 break;
00172
00173 default:
00174
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
00201 transform.setOrigin( tf::Vector3( translation_[0],
00202 translation_[1],
00203 translation_[2]) );
00204
00205
00206 q.setRPY(rotation_[0], rotation_[1], rotation_[2]);
00207 transform.setRotation(q);
00208
00209
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
00245 break;
00246 }
00247 }
00248
00249
00250 void ManualTFAlignment::writeTFToFile()
00251 {
00252 std::ofstream file (save_path_.c_str());
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 }