Create Coordinate Picker plugin (#593)

* Create Coordinate Picker plugin

Listens for mouse clicks on the map, transforms them to a specified
frame, and prints a list of them in the plugin GUI. The most recent
coordinates are copied to the clipboard.

README.md

@@ -82,6 +82,15 @@ Build the workspace with catkin_make:
 Plug-ins
 --------
 
+### Coordinate Picker
+
+Transforms coordinates of clicked points on the map to a specified frame. The most recent coordinate is placed on the clipboard, and a list of coordinates is displayed in the GUI.
+
+![](doc/images/screenshot_coordinate_picker.png)
+
+**Parameters**
+* Frame: coordinate frame into which to transform the clicked point
+
 ### Disparity
 
 Overlays a [sensor_msgs::DisparityImage](http://docs.ros.org/api/stereo_msgs/html/msg/DisparityImage.html) onto the display using the ''jet'' color map.

mapviz_plugins/CMakeLists.txt

@@ -75,6 +75,7 @@ include_directories(include
 
 set(UI_FILES
     ui/attitude_indicator_config.ui
+    ui/coordinate_picker_config.ui
     ui/disparity_config.ui
     ui/draw_polygon_config.ui
     ui/gps_config.ui
@@ -99,6 +100,7 @@ set(UI_FILES
 set(SRC_FILES
     src/attitude_indicator_plugin.cpp 
     src/canvas_click_filter.cpp
+    src/coordinate_picker_plugin.cpp
     src/disparity_plugin.cpp
     src/draw_polygon_plugin.cpp
     src/gps_plugin.cpp
@@ -125,6 +127,7 @@ set(SRC_FILES
 set(HEADER_FILES
     include/${PROJECT_NAME}/attitude_indicator_plugin.h
     include/${PROJECT_NAME}/canvas_click_filter.h
+    include/${PROJECT_NAME}/coordinate_picker_plugin.h
     include/${PROJECT_NAME}/disparity_plugin.h
     include/${PROJECT_NAME}/draw_polygon_plugin.h
     include/${PROJECT_NAME}/gps_plugin.h

mapviz_plugins/include/mapviz_plugins/coordinate_picker_plugin.h

@@ -0,0 +1,94 @@
+// *****************************************************************************
+//
+// Copyright (c) 2018, Southwest Research Institute® (SwRI®)
+// All rights reserved.
+//
+// Redistribution and use in source and binary forms, with or without
+// modification, are permitted provided that the following conditions are met:
+//     * Redistributions of source code must retain the above copyright
+//       notice, this list of conditions and the following disclaimer.
+//     * Redistributions in binary form must reproduce the above copyright
+//       notice, this list of conditions and the following disclaimer in the
+//       documentation and/or other materials provided with the distribution.
+//     * Neither the name of Southwest Research Institute® (SwRI®) nor the
+//       names of its contributors may be used to endorse or promote products
+//       derived from this software without specific prior written permission.
+//
+// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
+// AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
+// IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
+// ARE DISCLAIMED. IN NO EVENT SHALL <COPYRIGHT HOLDER> BE LIABLE FOR ANY
+// DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES
+// (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+// LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND
+// ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
+// (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
+// SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
+//
+// *****************************************************************************
+
+#ifndef ALPACA_SIMULATOR_COORDINATE_PICKER_PLUGIN_H_
+#define ALPACA_SIMULATOR_COORDINATE_PICKER_PLUGIN_H_
+
+#include <mapviz/mapviz_plugin.h>
+
+// ROS Libraries
+#include <ros/ros.h>
+
+// Mapviz libraries
+#include <mapviz/map_canvas.h>
+
+// QT autogenerated files
+#include "ui_coordinate_picker_config.h"
+
+namespace mapviz_plugins
+{
+  class CoordinatePickerPlugin : public mapviz::MapvizPlugin
+  {
+    Q_OBJECT
+
+    public:
+
+      CoordinatePickerPlugin();
+      virtual ~CoordinatePickerPlugin();
+
+      bool Initialize(QGLWidget* canvas);
+      void Shutdown() { };
+
+      void Draw(double x, double y, double scale);
+      void Transform() { };
+      
+      void LoadConfig(const YAML::Node& node, const std::string& path);
+      void SaveConfig(YAML::Emitter& emitter, const std::string& path);
+
+      QWidget* GetConfigWidget(QWidget* parent);
+
+      void PrintError(const std::string& message);
+      void PrintInfo(const std::string& message);
+      void PrintWarning(const std::string& message);
+
+    protected:
+      bool eventFilter(QObject* object, QEvent* event);
+      bool handleMousePress(QMouseEvent*);
+      bool handleMouseRelease(QMouseEvent*);
+      bool handleMouseMove(QMouseEvent*);
+
+    protected Q_SLOTS:
+      void SelectFrame();
+      void FrameEdited();
+      void ToggleCopyOnClick(int state);
+      void ClearCoordList();
+
+    private:
+      Ui::coordinate_picker_config ui_;
+      QWidget* config_widget_;
+      mapviz::MapCanvas* map_canvas_;
+
+      bool copy_on_click_;
+  };
+
+
+}
+
+#endif // ALPACA_SIMULATOR_COORDINATE_PICKER_PLUGIN_H_
+

mapviz_plugins/mapviz_plugins.xml

@@ -56,5 +56,8 @@
   <class name="mapviz_plugins/move_base" type="mapviz_plugins::MoveBasePlugin" base_class_type="mapviz::MapvizPlugin">
     <description>Interface to the [move_base] action server.</description>
   </class>
+  <class name="mapviz_plugins/coordinate_picker" type="mapviz_plugins::CoordinatePickerPlugin" base_class_type="mapviz::MapvizPlugin">
+    <description>Prints clicked coordinates in specified frame and copies to clipboard.</description>
+  </class>
 </library>
 

mapviz_plugins/src/coordinate_picker_plugin.cpp

@@ -0,0 +1,268 @@
+// *****************************************************************************
+//
+// Copyright (c) 2018, Southwest Research Institute® (SwRI®)
+// All rights reserved.
+//
+// Redistribution and use in source and binary forms, with or without
+// modification, are permitted provided that the following conditions are met:
+//     * Redistributions of source code must retain the above copyright
+//       notice, this list of conditions and the following disclaimer.
+//     * Redistributions in binary form must reproduce the above copyright
+//       notice, this list of conditions and the following disclaimer in the
+//       documentation and/or other materials provided with the distribution.
+//     * Neither the name of Southwest Research Institute® (SwRI®) nor the
+//       names of its contributors may be used to endorse or promote products
+//       derived from this software without specific prior written permission.
+//
+// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
+// AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
+// IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
+// ARE DISCLAIMED. IN NO EVENT SHALL <COPYRIGHT HOLDER> BE LIABLE FOR ANY
+// DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES
+// (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+// LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND
+// ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
+// (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
+// SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
+//
+// *****************************************************************************
+
+#include <mapviz_plugins/coordinate_picker_plugin.h>
+#include <mapviz/mapviz_plugin.h>
+
+#include <QClipboard>
+#include <QMouseEvent>
+#include <QTextStream>
+
+// ROS Libraries
+#include <ros/ros.h>
+
+// Mapviz Libraries
+#include <mapviz/select_frame_dialog.h>
+
+//
+#include <swri_transform_util/transform.h>
+
+#include <pluginlib/class_list_macros.h>
+PLUGINLIB_EXPORT_CLASS(mapviz_plugins::CoordinatePickerPlugin, mapviz::MapvizPlugin)
+
+namespace mapviz_plugins
+{
+
+CoordinatePickerPlugin::CoordinatePickerPlugin()
+  : config_widget_(new QWidget()),
+  map_canvas_(NULL),
+  copy_on_click_(false)
+{
+  ui_.setupUi(config_widget_);
+
+  QObject::connect(ui_.selectframe, SIGNAL(clicked()),
+                   this, SLOT(SelectFrame()));
+  QObject::connect(ui_.frame, SIGNAL(editingFinished()),
+                   this, SLOT(FrameEdited()));
+  QObject::connect(ui_.copyCheckBox, SIGNAL(stateChanged(int)),
+                   this, SLOT(ToggleCopyOnClick(int)));
+  QObject::connect(ui_.clearListButton, SIGNAL(clicked()),
+                   this, SLOT(ClearCoordList()));
+}
+
+CoordinatePickerPlugin::~CoordinatePickerPlugin()
+{
+  if (map_canvas_)
+  {
+    map_canvas_->removeEventFilter(this);
+  }
+}
+
+QWidget* CoordinatePickerPlugin::GetConfigWidget(QWidget* parent)
+{
+  config_widget_->setParent(parent);
+
+  return config_widget_;
+}
+
+bool CoordinatePickerPlugin::Initialize(QGLWidget* canvas)
+{
+  map_canvas_ = static_cast< mapviz::MapCanvas* >(canvas);
+  map_canvas_->installEventFilter(this);
+
+  initialized_ = true;
+  PrintInfo("OK");
+
+  return true;
+}
+
+bool CoordinatePickerPlugin::eventFilter(QObject* object, QEvent* event)
+{
+  switch (event->type())
+  {
+    case QEvent::MouseButtonPress:
+      return handleMousePress(static_cast< QMouseEvent* >(event));
+    case QEvent::MouseButtonRelease:
+      return handleMouseRelease(static_cast< QMouseEvent* >(event));
+    case QEvent::MouseMove:
+      return handleMouseMove(static_cast< QMouseEvent* >(event));
+    default:
+      return false;
+  }
+}
+
+bool CoordinatePickerPlugin::handleMousePress(QMouseEvent* event)
+{
+  QPointF point = event->localPos();
+  ROS_DEBUG("Map point: %f %f", point.x(), point.y());
+
+  swri_transform_util::Transform transform;
+  std::string frame = ui_.frame->text().toStdString();
+  if (frame.empty())
+  {
+    frame = target_frame_;
+  }
+
+  // Frames get confusing. The `target_frame_` member is set by the "Fixed
+  // Frame" combobox in the GUI. When we transform the map coordinate to the
+  // fixed frame, we get it in the `target_frame_` frame.
+  //
+  // Then we translate from that frame into *our* target frame, `frame`.
+  if (tf_manager_.GetTransform(frame, target_frame_, transform))
+  {
+    ROS_DEBUG("Transforming from fixed frame '%s' to (plugin) target frame '%s'",
+              target_frame_.c_str(),
+              frame.c_str());
+    QPointF transformed = map_canvas_->MapGlCoordToFixedFrame(point);
+    ROS_DEBUG("Point in fixed frame: %f %f", transformed.x(), transformed.y());
+    tf::Vector3 position(transformed.x(), transformed.y(), 0.0);
+    position = transform * position;
+    point.setX(position.x());
+    point.setY(position.y());
+
+    PrintInfo("OK");
+  }
+  else
+  {
+    QString warning;
+    QTextStream(&warning) << "No available transform from '" << QString::fromStdString(target_frame_) << "' to '" << QString::fromStdString(frame) << "'";
+    PrintWarning(warning.toStdString());
+    return false;
+  }
+
+  ROS_DEBUG("Transformed point in frame '%s': %f %f", frame.c_str(), point.x(), point.y());
+  QString new_point;
+  QTextStream stream(&new_point);
+  stream.setRealNumberPrecision(9);
+  stream << point.x() << "," << point.y();
+
+  if (copy_on_click_)
+  {
+    QClipboard* clipboard = QGuiApplication::clipboard();
+    clipboard->setText(new_point);
+  }
+
+  stream << " (" << QString::fromStdString(frame) << ")\n";
+
+  ui_.coordTextEdit->setPlainText(ui_.coordTextEdit->toPlainText().prepend(new_point));
+
+  // Let other plugins process this event too
+  return false;
+}
+
+bool CoordinatePickerPlugin::handleMouseRelease(QMouseEvent* event)
+{
+  // Let other plugins process this event too
+  return false;
+}
+
+bool CoordinatePickerPlugin::handleMouseMove(QMouseEvent* event)
+{
+  // Let other plugins process this event too
+  return false;
+}
+
+void CoordinatePickerPlugin::SelectFrame()
+{
+  std::string frame = mapviz::SelectFrameDialog::selectFrame(tf_);
+  if (!frame.empty())
+  {
+    ui_.frame->setText(QString::fromStdString(frame));
+    FrameEdited();
+  }
+}
+
+void CoordinatePickerPlugin::FrameEdited()
+{
+  ROS_INFO("Setting target frame to %s", ui_.frame->text().toStdString().c_str());
+}
+
+void CoordinatePickerPlugin::ToggleCopyOnClick(int state)
+{
+  switch(state)
+  {
+    case Qt::Checked:
+      copy_on_click_ = true;
+      break;
+    case Qt::PartiallyChecked:
+    case Qt::Unchecked:
+    default:
+      copy_on_click_ = false;
+      break;
+  }
+}
+
+void CoordinatePickerPlugin::ClearCoordList()
+{
+  ui_.coordTextEdit->setPlainText(QString());
+}
+
+void CoordinatePickerPlugin::Draw(double x, double y, double scale)
+{
+}
+
+void CoordinatePickerPlugin::LoadConfig(const YAML::Node& node, const std::string& path)
+{
+  if (node["frame"])
+  {
+    std::string frame;
+    node["frame"] >> frame;
+    ui_.frame->setText(QString::fromStdString(frame));
+  }
+
+  if (node["copy"])
+  {
+    bool copy;
+    node["copy"] >> copy;
+    if (copy)
+    {
+      ui_.copyCheckBox->setCheckState(Qt::Checked);
+    }
+    else
+    {
+      ui_.copyCheckBox->setCheckState(Qt::Unchecked);
+    }
+  }
+}
+
+void CoordinatePickerPlugin::SaveConfig(YAML::Emitter& emitter, const std::string& path)
+{
+  std::string frame = ui_.frame->text().toStdString();
+  emitter << YAML::Key << "frame" << YAML::Value << frame;
+
+  bool copy_on_click = ui_.copyCheckBox->isChecked();
+  emitter << YAML::Key << "copy" << YAML::Value << copy_on_click;
+}
+
+void CoordinatePickerPlugin::PrintError(const std::string& message)
+{
+  PrintErrorHelper(ui_.status, message, 1.0);
+}
+
+void CoordinatePickerPlugin::PrintInfo(const std::string& message)
+{
+  PrintInfoHelper(ui_.status, message, 1.0);
+}
+
+void CoordinatePickerPlugin::PrintWarning(const std::string& message)
+{
+  PrintWarningHelper(ui_.status, message, 1.0);
+}
+
+} // namespace mapviz_plugins

mapviz_plugins/ui/coordinate_picker_config.ui

@@ -0,0 +1,122 @@
+<?xml version="1.0" encoding="UTF-8"?>
+<ui version="4.0">
+ <class>coordinate_picker_config</class>
+ <widget class="QWidget" name="coordinate_picker_config">
+  <property name="geometry">
+   <rect>
+    <x>0</x>
+    <y>0</y>
+    <width>404</width>
+    <height>304</height>
+   </rect>
+  </property>
+  <property name="windowTitle">
+   <string>Form</string>
+  </property>
+  <property name="styleSheet">
+   <string notr="true"/>
+  </property>
+  <layout class="QGridLayout" name="gridLayout">
+   <property name="leftMargin">
+    <number>2</number>
+   </property>
+   <property name="topMargin">
+    <number>2</number>
+   </property>
+   <property name="rightMargin">
+    <number>2</number>
+   </property>
+   <property name="bottomMargin">
+    <number>2</number>
+   </property>
+   <property name="verticalSpacing">
+    <number>4</number>
+   </property>
+   <item row="6" column="0">
+    <widget class="QLabel" name="label_2">
+     <property name="font">
+      <font/>
+     </property>
+     <property name="text">
+      <string>Status:</string>
+     </property>
+    </widget>
+   </item>
+   <item row="6" column="2" colspan="2">
+    <widget class="QLabel" name="status">
+     <property name="font">
+      <font/>
+     </property>
+     <property name="styleSheet">
+      <string notr="true"/>
+     </property>
+     <property name="text">
+      <string>No topic</string>
+     </property>
+     <property name="wordWrap">
+      <bool>true</bool>
+     </property>
+    </widget>
+   </item>
+   <item row="2" column="2">
+    <widget class="QPlainTextEdit" name="coordTextEdit">
+     <property name="lineWrapMode">
+      <enum>QPlainTextEdit::NoWrap</enum>
+     </property>
+     <property name="readOnly">
+      <bool>true</bool>
+     </property>
+     <property name="tabStopWidth">
+      <number>4</number>
+     </property>
+     <property name="textInteractionFlags">
+      <set>Qt::TextSelectableByKeyboard|Qt::TextSelectableByMouse</set>
+     </property>
+     <property name="placeholderText">
+      <string>Click on the map; coordinates appear here</string>
+     </property>
+    </widget>
+   </item>
+   <item row="0" column="0">
+    <widget class="QLabel" name="label">
+     <property name="text">
+      <string>Frame:</string>
+     </property>
+    </widget>
+   </item>
+   <item row="0" column="2">
+    <widget class="QLineEdit" name="frame"/>
+   </item>
+   <item row="0" column="3">
+    <widget class="QPushButton" name="selectframe">
+     <property name="text">
+      <string>Select</string>
+     </property>
+    </widget>
+   </item>
+   <item row="1" column="2">
+    <widget class="QCheckBox" name="copyCheckBox">
+     <property name="text">
+      <string>Copy to Clipboard on Click</string>
+     </property>
+    </widget>
+   </item>
+   <item row="2" column="3">
+    <widget class="QPushButton" name="clearListButton">
+     <property name="text">
+      <string>Clear List</string>
+     </property>
+    </widget>
+   </item>
+  </layout>
+ </widget>
+ <resources/>
+ <connections/>
+ <slots>
+  <slot>SelectColor()</slot>
+  <slot>SelectTopic()</slot>
+  <slot>TopicEdited()</slot>
+  <slot>PositionToleranceChanged(double)</slot>
+  <slot>AngleToleranceChanged(double)</slot>
+ </slots>
+</ui>