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 #include <multires_image/multires_image_plugin.h>
00031
00032
00033 #include <cstdio>
00034
00035
00036 #include <QFileDialog>
00037 #include <QGLWidget>
00038 #include <QPalette>
00039
00040
00041 #include <ros/ros.h>
00042 #include <tf/transform_datatypes.h>
00043
00044
00045 #include <pluginlib/class_list_macros.h>
00046 PLUGINLIB_EXPORT_CLASS(mapviz_plugins::MultiresImagePlugin, mapviz::MapvizPlugin)
00047
00048 namespace mapviz_plugins
00049 {
00050 MultiresImagePlugin::MultiresImagePlugin() :
00051 loaded_(false),
00052 tile_set_(NULL),
00053 tile_view_(NULL),
00054 config_widget_(new QWidget()),
00055 transformed_(false),
00056 offset_x_(0.0),
00057 offset_y_(0.0)
00058 {
00059 ui_.setupUi(config_widget_);
00060
00061 QPalette p(config_widget_->palette());
00062 p.setColor(QPalette::Background, Qt::white);
00063 config_widget_->setPalette(p);
00064
00065 QPalette p2(ui_.status->palette());
00066 p2.setColor(QPalette::Text, Qt::red);
00067 ui_.status->setPalette(p2);
00068
00069 QObject::connect(ui_.browse, SIGNAL(clicked()), this, SLOT(SelectFile()));
00070 QObject::connect(ui_.path, SIGNAL(editingFinished()), this, SLOT(AcceptConfiguration()));
00071 QObject::connect(ui_.x_offset_spin_box, SIGNAL(valueChanged(double)), this, SLOT(SetXOffset(double)));
00072 QObject::connect(ui_.y_offset_spin_box, SIGNAL(valueChanged(double)), this, SLOT(SetYOffset(double)));
00073
00074 source_frame_ = "/";
00075 }
00076
00077 MultiresImagePlugin::~MultiresImagePlugin()
00078 {
00079 delete tile_view_;
00080 delete tile_set_;
00081 }
00082
00083 void MultiresImagePlugin::PrintError(const std::string& message)
00084 {
00085 if (message == ui_.status->text().toStdString())
00086 return;
00087
00088 ROS_ERROR("Error: %s", message.c_str());
00089 QPalette p(ui_.status->palette());
00090 p.setColor(QPalette::Text, Qt::red);
00091 ui_.status->setPalette(p);
00092 ui_.status->setText(message.c_str());
00093 }
00094
00095 void MultiresImagePlugin::PrintInfo(const std::string& message)
00096 {
00097 if (message == ui_.status->text().toStdString())
00098 return;
00099
00100 ROS_INFO("%s", message.c_str());
00101 QPalette p(ui_.status->palette());
00102 p.setColor(QPalette::Text, Qt::green);
00103 ui_.status->setPalette(p);
00104 ui_.status->setText(message.c_str());
00105 }
00106
00107 void MultiresImagePlugin::PrintWarning(const std::string& message)
00108 {
00109 if (message == ui_.status->text().toStdString())
00110 return;
00111
00112 ROS_WARN("%s", message.c_str());
00113 QPalette p(ui_.status->palette());
00114 p.setColor(QPalette::Text, Qt::darkYellow);
00115 ui_.status->setPalette(p);
00116 ui_.status->setText(message.c_str());
00117 }
00118
00119 void MultiresImagePlugin::AcceptConfiguration()
00120 {
00121 ROS_INFO("Accept multires image configuration.");
00122 if (tile_set_ != NULL && tile_set_->GeoReference().GeoPath() == ui_.path->text().toStdString())
00123 {
00124
00125 }
00126 else
00127 {
00128 loaded_ = false;
00129 delete tile_set_;
00130 delete tile_view_;
00131 tile_set_ = new multires_image::TileSet(ui_.path->text().toStdString());
00132
00133 if (tile_set_->Load())
00134 {
00135 loaded_ = true;
00136
00137 source_frame_ = tile_set_->GeoReference().Projection();
00138 if (source_frame_.empty() || source_frame_[0] != '/')
00139 {
00140 source_frame_ = std::string("/") + source_frame_;
00141 }
00142
00143 QPalette p(ui_.status->palette());
00144 p.setColor(QPalette::Text, Qt::green);
00145 ui_.status->setPalette(p);
00146 ui_.status->setText("OK");
00147
00148 initialized_ = true;
00149
00150 MultiresView* view = new MultiresView(tile_set_, canvas_);
00151 tile_view_ = view;
00152 }
00153 else
00154 {
00155 PrintError("Failed to load image.");
00156 delete tile_set_;
00157 tile_set_ = 0;
00158 tile_view_ = 0;
00159 }
00160 }
00161 }
00162
00163 void MultiresImagePlugin::SelectFile()
00164 {
00165 QFileDialog dialog(config_widget_, "Select Multires Image");
00166 dialog.setFileMode(QFileDialog::ExistingFile);
00167 dialog.setNameFilter(tr("Geo Files (*.geo)"));
00168
00169 dialog.exec();
00170
00171 if (dialog.result() == QDialog::Accepted && dialog.selectedFiles().count() == 1)
00172 {
00173 ui_.path->setText(dialog.selectedFiles().first());
00174 AcceptConfiguration();
00175 }
00176 }
00177
00178
00179 void MultiresImagePlugin::SetXOffset(double offset_x)
00180 {
00181 offset_x_ = offset_x;
00182 }
00183
00184 void MultiresImagePlugin::SetYOffset(double offset_y)
00185 {
00186 offset_y_ = offset_y;
00187 }
00188
00189 QWidget* MultiresImagePlugin::GetConfigWidget(QWidget* parent)
00190 {
00191 config_widget_->setParent(parent);
00192
00193 return config_widget_;
00194 }
00195
00196 bool MultiresImagePlugin::Initialize(QGLWidget* canvas)
00197 {
00198 canvas_ = canvas;
00199
00200 return true;
00201 }
00202
00203 void MultiresImagePlugin::GetCenterPoint(double x, double y)
00204 {
00205 tf::Point point(x, y, 0);
00206 tf::Point center = inverse_transform_ * point;
00207 center_x_ = center.getX();
00208 center_y_ = center.getY();
00209 }
00210
00211 void MultiresImagePlugin::Draw(double x, double y, double scale)
00212 {
00213 if (transformed_ && tile_set_ != NULL && tile_view_ != NULL)
00214 {
00215 GetCenterPoint(x, y);
00216 tile_view_->SetView(center_x_, center_y_, 1, scale);
00217
00218 tile_view_->Draw();
00219
00220 PrintInfo("OK");
00221 }
00222 }
00223
00224 void MultiresImagePlugin::Transform()
00225 {
00226 transformed_ = false;
00227
00228 if (!loaded_)
00229 return;
00230
00231 if (!tf_manager_.GetTransform(target_frame_, source_frame_, transform_))
00232 {
00233 PrintError("Failed transform from " + source_frame_ + " to " + target_frame_);
00234 return;
00235 }
00236
00237 if (!tf_manager_.GetTransform(source_frame_, target_frame_, inverse_transform_))
00238 {
00239 PrintError("Failed inverse transform from " + target_frame_ + " to " + source_frame_);
00240 return;
00241 }
00242
00243
00244 swri_transform_util::Transform offset(
00245 tf::Transform(
00246 tf::createIdentityQuaternion(),
00247 tf::Vector3(offset_x_, offset_y_, 0.0)));
00248
00249
00250 for (int i = 0; i < tile_set_->LayerCount(); i++)
00251 {
00252 multires_image::TileSetLayer* layer = tile_set_->GetLayer(i);
00253 for (int r = 0; r < layer->RowCount(); r++)
00254 {
00255 for (int c = 0; c < layer->ColumnCount(); c++)
00256 {
00257 multires_image::Tile* tile = layer->GetTile(c, r);
00258
00259 tile->Transform(transform_, offset);
00260 }
00261 }
00262 }
00263
00264 transformed_ = true;
00265 }
00266
00267 boost::filesystem::path MultiresImagePlugin::MakePathRelative(boost::filesystem::path path, boost::filesystem::path base)
00268 {
00269
00270 if (path.has_root_path())
00271 {
00272 if (path.root_path() != base.root_path())
00273 {
00274 return path;
00275 }
00276 else
00277 {
00278 return MakePathRelative(path.relative_path(), base.relative_path());
00279 }
00280 }
00281 else
00282 {
00283 if (base.has_root_path())
00284 {
00285 ROS_WARN("Cannot uncomplete a path relative path from a rooted base.");
00286 return path;
00287 }
00288 else
00289 {
00290 typedef boost::filesystem::path::const_iterator path_iterator;
00291 path_iterator path_it = path.begin();
00292 path_iterator base_it = base.begin();
00293 while (path_it != path.end() && base_it != base.end())
00294 {
00295 if (*path_it != *base_it)
00296 break;
00297 ++path_it;
00298 ++base_it;
00299 }
00300 boost::filesystem::path result;
00301 for (; base_it != base.end(); ++base_it)
00302 {
00303 result /= "..";
00304 }
00305 for (; path_it != path.end(); ++path_it)
00306 {
00307 result /= *path_it;
00308 }
00309 return result;
00310 }
00311 }
00312 }
00313
00314 void MultiresImagePlugin::LoadConfig(const YAML::Node& node, const std::string& path)
00315 {
00316 if (node["path"])
00317 {
00318 std::string path_string;
00319 node["path"] >> path_string;
00320
00321 boost::filesystem::path image_path(path_string);
00322 if (image_path.is_complete() == false)
00323 {
00324 boost::filesystem::path base_path(path);
00325 path_string =
00326 (path / image_path.relative_path()).normalize().string();
00327 }
00328
00329 ui_.path->setText(path_string.c_str());
00330
00331 AcceptConfiguration();
00332 }
00333
00334 if (node["offset_x"])
00335 {
00336 node["offset_x"] >> offset_x_;
00337 ui_.x_offset_spin_box->setValue(offset_x_);
00338 }
00339 if (node["offset_y"])
00340 {
00341 node["offset_y"] >> offset_y_;
00342 ui_.y_offset_spin_box->setValue(offset_y_);
00343 }
00344 }
00345
00346 void MultiresImagePlugin::SaveConfig(YAML::Emitter& emitter, const std::string& path)
00347 {
00348 boost::filesystem::path abs_path(ui_.path->text().toStdString());
00349 boost::filesystem::path base_path(path);
00350 boost::filesystem::path rel_path = MakePathRelative(abs_path, base_path);
00351
00352 emitter << YAML::Key << "path" << YAML::Value << rel_path.string();
00353 emitter << YAML::Key << "offset_x" << YAML::Value << offset_x_;
00354 emitter << YAML::Key << "offset_y" << YAML::Value << offset_y_;
00355 }
00356 }
00357