diff --git a/mapviz_plugins/include/mapviz_plugins/point_drawing_plugin.h b/mapviz_plugins/include/mapviz_plugins/point_drawing_plugin.h index 131f73ee..2f354475 100644 --- a/mapviz_plugins/include/mapviz_plugins/point_drawing_plugin.h +++ b/mapviz_plugins/include/mapviz_plugins/point_drawing_plugin.h @@ -81,7 +81,7 @@ namespace mapviz_plugins { } virtual void Transform(); - virtual bool DrawPoints(); + virtual bool DrawPoints(double scale); virtual bool DrawArrows(); virtual bool DrawArrow(const StampedPoint& point); virtual bool DrawLaps(); @@ -94,8 +94,11 @@ namespace mapviz_plugins protected Q_SLOTS: virtual void DrawIcon(); virtual void SetDrawStyle(QString style); + virtual void SetStaticArrowSizes(bool isChecked); + virtual void SetArrowSize(int arrowSize); protected: + int arrow_size_; DrawStyle draw_style_; StampedPoint cur_point_; std::list points_; @@ -106,6 +109,8 @@ namespace mapviz_plugins QColor color_; bool lap_checked_; int buffer_holder_; + double scale_; + bool static_arrow_sizes_; private: std::vector > laps_; diff --git a/mapviz_plugins/src/navsat_config.ui b/mapviz_plugins/src/navsat_config.ui index f4e25529..36215d5c 100644 --- a/mapviz_plugins/src/navsat_config.ui +++ b/mapviz_plugins/src/navsat_config.ui @@ -7,7 +7,7 @@ 0 0 400 - 300 + 197 @@ -23,7 +23,34 @@ 2 - + + + + + + + + + + + + + 1 + + + 500 + + + 25 + + + Qt::Horizontal + + + + + + @@ -36,7 +63,7 @@ - + @@ -113,7 +140,7 @@ - + @@ -126,7 +153,7 @@ - + @@ -164,7 +191,7 @@ - + @@ -177,7 +204,7 @@ - + QAbstractSpinBox::PlusMinus @@ -226,6 +253,32 @@ + + + + + Sans Serif + 8 + + + + Static Arrow Sizes: + + + + + + + Qt::Vertical + + + + 20 + 40 + + + + diff --git a/mapviz_plugins/src/navsat_plugin.cpp b/mapviz_plugins/src/navsat_plugin.cpp index 29afe58c..2e00a3c6 100644 --- a/mapviz_plugins/src/navsat_plugin.cpp +++ b/mapviz_plugins/src/navsat_plugin.cpp @@ -73,6 +73,10 @@ namespace mapviz_plugins SLOT(BufferSizeChanged(int))); QObject::connect(ui_.drawstyle, SIGNAL(activated(QString)), this, SLOT(SetDrawStyle(QString))); + QObject::connect(ui_.static_arrow_sizes, SIGNAL(clicked(bool)), + this, SLOT(SetStaticArrowSizes(bool))); + QObject::connect(ui_.arrow_size, SIGNAL(valueChanged(int)), + this, SLOT(SetArrowSize(int))); connect(ui_.color, SIGNAL(colorEdited(const QColor&)), this, SLOT(DrawIcon())); } @@ -229,7 +233,7 @@ namespace mapviz_plugins void NavSatPlugin::Draw(double x, double y, double scale) { color_ = ui_.color->color(); - if (DrawPoints()) + if (DrawPoints(scale)) { PrintInfo("OK"); } @@ -280,6 +284,18 @@ namespace mapviz_plugins ui_.buffersize->setValue(buffer_size_); } + if (node["static_arrow_sizes"]) + { + bool static_arrow_sizes = node["static_arrow_sizes"].as(); + ui_.static_arrow_sizes->setChecked(static_arrow_sizes); + SetStaticArrowSizes(static_arrow_sizes); + } + + if (node["arrow_size"]) + { + ui_.arrow_size->setValue(node["arrow_size"].as()); + } + TopicEdited(); } @@ -298,5 +314,9 @@ namespace mapviz_plugins YAML::Value << position_tolerance_; emitter << YAML::Key << "buffer_size" << YAML::Value << buffer_size_; + + emitter << YAML::Key << "static_arrow_sizes" << YAML::Value << ui_.static_arrow_sizes->isChecked(); + + emitter << YAML::Key << "arrow_size" << YAML::Value << ui_.arrow_size->value(); } } diff --git a/mapviz_plugins/src/odometry_config.ui b/mapviz_plugins/src/odometry_config.ui index ff3638bb..8748c1d9 100644 --- a/mapviz_plugins/src/odometry_config.ui +++ b/mapviz_plugins/src/odometry_config.ui @@ -7,7 +7,7 @@ 0 0 400 - 300 + 247 @@ -17,21 +17,77 @@ - - 2 - - - 2 - - - 2 - - - 2 - 4 + + 2 + + + + + 6 + + + + + + + + + + + + 1 + + + 500 + + + 25 + + + Qt::Horizontal + + + QSlider::NoTicks + + + + + + + + + + Sans Serif + 8 + + + + Static Arrow Sizes: + + + + + + + + 0 + 0 + + + + QAbstractSpinBox::PlusMinus + + + + + + 1.000000000000000 + + + @@ -93,25 +149,6 @@ - - - - - 0 - 0 - - - - QAbstractSpinBox::PlusMinus - - - - - - 1.000000000000000 - - - @@ -122,7 +159,7 @@ - + @@ -141,7 +178,7 @@ - + @@ -204,7 +241,7 @@ - + @@ -217,14 +254,14 @@ - + - + @@ -237,7 +274,7 @@ - + QAbstractSpinBox::PlusMinus @@ -247,7 +284,7 @@ - + @@ -260,14 +297,14 @@ - + - + @@ -279,6 +316,19 @@ + + + + Qt::Vertical + + + + 20 + 40 + + + + diff --git a/mapviz_plugins/src/odometry_plugin.cpp b/mapviz_plugins/src/odometry_plugin.cpp index f37701ca..25c492b0 100644 --- a/mapviz_plugins/src/odometry_plugin.cpp +++ b/mapviz_plugins/src/odometry_plugin.cpp @@ -83,6 +83,10 @@ namespace mapviz_plugins SLOT(BufferSizeChanged(int))); QObject::connect(ui_.drawstyle, SIGNAL(activated(QString)), this, SLOT(SetDrawStyle(QString))); + QObject::connect(ui_.static_arrow_sizes, SIGNAL(clicked(bool)), + this, SLOT(SetStaticArrowSizes(bool))); + QObject::connect(ui_.arrow_size, SIGNAL(valueChanged(int)), + this, SLOT(SetArrowSize(int))); connect(ui_.color, SIGNAL(colorEdited(const QColor&)), this, SLOT(DrawIcon())); } @@ -290,7 +294,7 @@ namespace mapviz_plugins { DrawCovariance(); } - if (DrawPoints()) + if (DrawPoints(scale)) { PrintInfo("OK"); } @@ -368,19 +372,31 @@ namespace mapviz_plugins ui_.buffersize->setValue(buffer_size_); } - if (swri_yaml_util::FindValue(node, "show_covariance")) + if (node["show_covariance"]) { bool show_covariance = false; node["show_covariance"] >> show_covariance; ui_.show_covariance->setChecked(show_covariance); } - if (swri_yaml_util::FindValue(node, "show_laps")) + if (node["show_laps"]) { bool show_laps = false; node["show_laps"] >> show_laps; ui_.show_laps->setChecked(show_laps); } + if (node["static_arrow_sizes"]) + { + bool static_arrow_sizes = node["static_arrow_sizes"].as(); + ui_.static_arrow_sizes->setChecked(static_arrow_sizes); + SetStaticArrowSizes(static_arrow_sizes); + } + + if (node["arrow_size"]) + { + ui_.arrow_size->setValue(node["arrow_size"].as()); + } + TopicEdited(); } @@ -411,5 +427,9 @@ namespace mapviz_plugins bool show_covariance = ui_.show_covariance->isChecked(); emitter << YAML::Key << "show_covariance" << YAML::Value << show_covariance; + + emitter << YAML::Key << "static_arrow_sizes" << YAML::Value << ui_.static_arrow_sizes->isChecked(); + + emitter << YAML::Key << "arrow_size" << YAML::Value << ui_.arrow_size->value(); } } diff --git a/mapviz_plugins/src/path_config.ui b/mapviz_plugins/src/path_config.ui index 5321af80..5a43a980 100644 --- a/mapviz_plugins/src/path_config.ui +++ b/mapviz_plugins/src/path_config.ui @@ -7,7 +7,7 @@ 0 0 400 - 300 + 79 @@ -23,7 +23,7 @@ 2 - + @@ -48,7 +48,7 @@ - + @@ -125,6 +125,19 @@ + + + + Qt::Vertical + + + + 20 + 40 + + + + diff --git a/mapviz_plugins/src/path_plugin.cpp b/mapviz_plugins/src/path_plugin.cpp index 04f86e97..4cb2085c 100644 --- a/mapviz_plugins/src/path_plugin.cpp +++ b/mapviz_plugins/src/path_plugin.cpp @@ -184,15 +184,15 @@ namespace mapviz_plugins void PathPlugin::Draw(double x, double y, double scale) { - bool lines, points; - lines = points = false; + bool lines; + bool points; color_ = ui_.path_color->color(); draw_style_ = LINES; - lines = DrawPoints(); + lines = DrawPoints(scale); color_ = color_.dark(200); draw_style_ = POINTS; - points = DrawPoints(); - if (lines == true && points == true) + points = DrawPoints(scale); + if (lines && points) { PrintInfo("OK"); } diff --git a/mapviz_plugins/src/point_drawing_plugin.cpp b/mapviz_plugins/src/point_drawing_plugin.cpp index d304d683..3db8e367 100644 --- a/mapviz_plugins/src/point_drawing_plugin.cpp +++ b/mapviz_plugins/src/point_drawing_plugin.cpp @@ -45,16 +45,20 @@ namespace mapviz_plugins { PointDrawingPlugin::PointDrawingPlugin() - : draw_style_(LINES), + : arrow_size_(25), + draw_style_(LINES), position_tolerance_(0.0), buffer_size_(0), covariance_checked_(false), new_lap_(true), lap_checked_(false), buffer_holder_(false), + scale_(1.0), + static_arrow_sizes_(false), got_begin_(false) { } + void PointDrawingPlugin::DrawIcon() { if (icon_) @@ -94,6 +98,12 @@ namespace mapviz_plugins icon_->SetPixmap(icon); } } + + void PointDrawingPlugin::SetArrowSize(int arrowSize) + { + arrow_size_ = arrowSize; + } + void PointDrawingPlugin::SetDrawStyle(QString style) { if (style == "lines") @@ -112,20 +122,26 @@ namespace mapviz_plugins DrawIcon(); } - bool PointDrawingPlugin::DrawPoints() + void PointDrawingPlugin::SetStaticArrowSizes(bool isChecked) { - bool transformed = false; + static_arrow_sizes_ = isChecked; + } + + bool PointDrawingPlugin::DrawPoints(double scale) + { + scale_ = scale; + bool transformed = true; if (lap_checked_) { CollectLaps(); if (draw_style_ == ARROWS) { - transformed = DrawLapsArrows(); + transformed &= DrawLapsArrows(); } else { - transformed = DrawLaps(); + transformed &= DrawLaps(); } } else if (buffer_size_ == INT_MAX) @@ -136,14 +152,16 @@ namespace mapviz_plugins } if (draw_style_ == ARROWS) { - transformed = DrawArrows(); + transformed &= DrawArrows(); } else { - transformed = DrawLines(); + transformed &= DrawLines(); } + return transformed; } + void PointDrawingPlugin::CollectLaps() { if (!got_begin_) @@ -177,7 +195,7 @@ namespace mapviz_plugins bool PointDrawingPlugin::DrawLines() { - bool transformed = false; + bool success = cur_point_.transformed; glColor4f(color_.redF(), color_.greenF(), color_.blueF(), 1.0); if (draw_style_ == LINES) { @@ -193,11 +211,10 @@ namespace mapviz_plugins std::list::iterator it = points_.begin(); for (; it != points_.end(); ++it) { + success &= it->transformed; if (it->transformed) { glVertex2f(it->transformed_point.getX(), it->transformed_point.getY()); - - transformed = true; } } @@ -205,14 +222,13 @@ namespace mapviz_plugins { glVertex2f(cur_point_.transformed_point.getX(), cur_point_.transformed_point.getY()); - - transformed = true; } glEnd(); - return transformed; + return success; } + bool PointDrawingPlugin::DrawArrow(const StampedPoint& it) { if (it.transformed) @@ -239,22 +255,23 @@ namespace mapviz_plugins bool PointDrawingPlugin::DrawArrows() { - bool transformed = false; + bool success = true; glLineWidth(2); glBegin(GL_LINES); glColor4f(color_.redF(), color_.greenF(), color_.blueF(), 0.5); std::list::iterator it = points_.begin(); for (; it != points_.end(); ++it) { - transformed = DrawArrow(*it); + success &= DrawArrow(*it); } - transformed = DrawArrow(cur_point_); + success &= DrawArrow(cur_point_); glEnd(); - return transformed; + return success; } + bool PointDrawingPlugin::TransformPoint(StampedPoint& point) { swri_transform_util::Transform transform; @@ -264,12 +281,25 @@ namespace mapviz_plugins tf::Transform orientation(tf::Transform(transform.GetOrientation()) * point.orientation); + + double size = static_cast(arrow_size_); + if (static_arrow_sizes_) + { + size *= scale_; + } + else + { + size /= 10.0; + } + double arrow_width = size / 5.0; + double head_length = size * 0.75; + point.transformed_arrow_point = - point.transformed_point + orientation * tf::Point(1.0, 0.0, 0.0); + point.transformed_point + orientation * tf::Point(size, 0.0, 0.0); point.transformed_arrow_left = - point.transformed_point + orientation * tf::Point(0.75, -0.2, 0.0); + point.transformed_point + orientation * tf::Point(head_length, -arrow_width, 0.0); point.transformed_arrow_right = - point.transformed_point + orientation * tf::Point(0.75, 0.2, 0.0); + point.transformed_point + orientation * tf::Point(head_length, arrow_width, 0.0); if (covariance_checked_) { @@ -318,7 +348,7 @@ namespace mapviz_plugins bool PointDrawingPlugin::DrawLaps() { - bool transformed = false; + bool transformed = points_.size() != 0; glColor4f(color_.redF(), color_.greenF(), color_.blueF(), 0.5); glLineWidth(3); QColor base_color = color_; @@ -369,11 +399,11 @@ namespace mapviz_plugins std::list::iterator it = points_.begin(); for (; it != points_.end(); ++it) { + transformed &= it->transformed; if (it->transformed) { glVertex2f(it->transformed_point.getX(), it->transformed_point.getY()); - transformed = true; } } } @@ -381,6 +411,7 @@ namespace mapviz_plugins glEnd(); return transformed; } + void PointDrawingPlugin::UpdateColor(QColor base_color, int i) { int hue = color_.hue() + (i + 1) * 10 * M_PI; @@ -397,7 +428,7 @@ namespace mapviz_plugins bool PointDrawingPlugin::DrawLapsArrows() { - bool transformed = false; + bool success = laps_.size() != 0 && points_.size() != 0; glColor4f(color_.redF(), color_.greenF(), color_.blueF(), 0.5); glLineWidth(2); QColor base_color = color_; @@ -410,7 +441,7 @@ namespace mapviz_plugins for (; it != laps_[i].end(); ++it) { glBegin(GL_LINE_STRIP); - transformed = DrawArrow(*it); + success &= DrawArrow(*it); glEnd(); } } @@ -430,11 +461,11 @@ namespace mapviz_plugins for (; it != points_.end(); ++it) { glBegin(GL_LINE_STRIP); - transformed = DrawArrow(*it); + success &= DrawArrow(*it); glEnd(); } } - return transformed; + return success; } } diff --git a/mapviz_plugins/src/tf_frame_config.ui b/mapviz_plugins/src/tf_frame_config.ui index 525263e6..9832eafe 100644 --- a/mapviz_plugins/src/tf_frame_config.ui +++ b/mapviz_plugins/src/tf_frame_config.ui @@ -7,7 +7,7 @@ 0 0 400 - 300 + 197 @@ -23,7 +23,26 @@ 2 - + + + + + 24 + 24 + + + + false + + + + + + + + + + @@ -36,7 +55,20 @@ - + + + + + Sans Serif + 8 + + + + Static Arrow Sizes: + + + + @@ -113,7 +145,7 @@ - + @@ -126,7 +158,7 @@ - + @@ -145,26 +177,7 @@ - - - - - 24 - 24 - - - - false - - - - - - - - - - + @@ -177,7 +190,7 @@ - + QAbstractSpinBox::PlusMinus @@ -231,6 +244,46 @@ + + + + + + + + + + + + + 1 + + + 500 + + + 25 + + + Qt::Horizontal + + + + + + + + + Qt::Vertical + + + + 20 + 40 + + + + diff --git a/mapviz_plugins/src/tf_frame_plugin.cpp b/mapviz_plugins/src/tf_frame_plugin.cpp index 9688030e..c139106e 100644 --- a/mapviz_plugins/src/tf_frame_plugin.cpp +++ b/mapviz_plugins/src/tf_frame_plugin.cpp @@ -78,6 +78,10 @@ namespace mapviz_plugins SLOT(BufferSizeChanged(int))); QObject::connect(ui_.drawstyle, SIGNAL(activated(QString)), this, SLOT(SetDrawStyle(QString))); + QObject::connect(ui_.static_arrow_sizes, SIGNAL(clicked(bool)), + this, SLOT(SetStaticArrowSizes(bool))); + QObject::connect(ui_.arrow_size, SIGNAL(valueChanged(int)), + this, SLOT(SetArrowSize(int))); connect(ui_.color, SIGNAL(colorEdited(const QColor&)), this, SLOT(DrawIcon())); } @@ -215,7 +219,7 @@ namespace mapviz_plugins void TfFramePlugin::Draw(double x, double y, double scale) { color_ = ui_.color->color(); - if (DrawPoints()) + if (DrawPoints(scale)) { PrintInfo("OK"); } @@ -265,6 +269,18 @@ namespace mapviz_plugins ui_.buffersize->setValue(buffer_size_); } + if (node["static_arrow_sizes"]) + { + bool static_arrow_sizes = node["static_arrow_sizes"].as(); + ui_.static_arrow_sizes->setChecked(static_arrow_sizes); + SetStaticArrowSizes(static_arrow_sizes); + } + + if (node["arrow_size"]) + { + ui_.arrow_size->setValue(node["arrow_size"].as()); + } + FrameEdited(); } @@ -283,5 +299,9 @@ namespace mapviz_plugins YAML::Value << position_tolerance_; emitter << YAML::Key << "buffer_size" << YAML::Value << buffer_size_; + + emitter << YAML::Key << "static_arrow_sizes" << YAML::Value << ui_.static_arrow_sizes->isChecked(); + + emitter << YAML::Key << "arrow_size" << YAML::Value << ui_.arrow_size->value(); } }