Add visualizer for PoseArray

docs/_plugins/pose_array.md

@@ -0,0 +1,6 @@
+---
+title: "Pose Array"
+description: "Displays an Array of Poses"
+image: ""
+parameters:
+---

mapviz_plugins/CMakeLists.txt

@@ -95,6 +95,7 @@ set(UI_FILES
     ui/plan_route_config.ui
     ui/point_click_publisher_config.ui
     ui/pointcloud2_config.ui
+    ui/pose_array_config.ui
     ui/pose_config.ui
     ui/robot_image_config.ui
     ui/route_config.ui
@@ -128,6 +129,7 @@ set(SRC_FILES
     src/point_click_publisher_plugin.cpp
     src/pointcloud2_plugin.cpp
     src/point_drawing_plugin.cpp
+    src/pose_array_plugin.cpp
     src/pose_plugin.cpp
     src/robot_image_plugin.cpp
     src/route_plugin.cpp
@@ -161,6 +163,7 @@ set(HEADER_FILES
     include/${PROJECT_NAME}/point_click_publisher_plugin.h
     include/${PROJECT_NAME}/pointcloud2_plugin.h
     include/${PROJECT_NAME}/point_drawing_plugin.h
+    include/${PROJECT_NAME}/pose_array_plugin.h
     include/${PROJECT_NAME}/pose_plugin.h
     include/${PROJECT_NAME}/robot_image_plugin.h
     include/${PROJECT_NAME}/route_plugin.h

mapviz_plugins/include/mapviz_plugins/pose_array_plugin.h

@@ -0,0 +1,100 @@
+/**
+ * Copyright 2021 Trinity University 
+ *
+ * Redistribution and use in source and binary forms, with or without 
+ * modification, are permitted provided that the following conditions are met:
+ *
+ * 1. Redistributions of source code must retain the above copyright notice, 
+ *    this list of conditions and the following disclaimer.
+ *
+ * 2. 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.
+ *
+ * 3. Neither the name of the copyright holder 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 THE COPYRIGHT HOLDER OR CONTRIBUTORS 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 MAPVIZ_PLUGINS_POSE_ARRAY_PLUGIN_H_
+#define MAPVIZ_PLUGINS_POSE_ARRAY_PLUGIN_H_
+
+// Include mapviz_plugin.h first to ensure GL deps are included in the right order
+#include <mapviz/mapviz_plugin.h>
+#include <mapviz/map_canvas.h>
+#include <mapviz_plugins/point_drawing_plugin.h>
+#include <mapviz/select_topic_dialog.h>
+
+// QT libraries
+#include <QGLWidget>
+#include <QObject>
+#include <QWidget>
+
+// ROS libraries
+#include <ros/ros.h>
+#include <tf/transform_datatypes.h>
+#include <geometry_msgs/PoseArray.h>
+#include <geometry_msgs/PoseStamped.h>
+#include <swri_transform_util/local_xy_util.h>
+
+
+// QT autogenerated files
+#include "ui_pose_array_config.h"
+
+namespace mapviz_plugins
+{
+  class PoseArrayPlugin : public mapviz_plugins::PointDrawingPlugin
+  {
+    Q_OBJECT
+
+   public:
+    PoseArrayPlugin();
+    virtual ~PoseArrayPlugin();
+
+    bool Initialize(QGLWidget* canvas);
+    void Shutdown()
+    {
+    }
+
+    void Draw(double x, double y, double scale);
+
+    void LoadConfig(const YAML::Node& node, const std::string& path);
+    void SaveConfig(YAML::Emitter& emitter, const std::string& path);
+
+    QWidget* GetConfigWidget(QWidget* parent);
+
+   protected:
+    void PrintError(const std::string& message);
+    void PrintInfo(const std::string& message);
+    void PrintWarning(const std::string& message);
+
+   protected Q_SLOTS:
+    void SelectTopic();
+    void TopicEdited();
+
+   private:
+    Ui::pose_array_config ui_;
+    QWidget* config_widget_;
+
+    std::string topic_;
+
+    ros::Subscriber pose_sub_;
+    bool has_message_;
+
+    void PoseArrayCallback(const geometry_msgs::PoseArrayConstPtr& msg);
+  };
+}
+
+#endif  // MAPVIZ_PLUGINS_POSE_ARRAY_PLUGIN_H_

mapviz_plugins/mapviz_plugins.xml

@@ -19,6 +19,9 @@
   <class name="mapviz_plugins/pose" type="mapviz_plugins::PosePlugin" base_class_type="mapviz::MapvizPlugin">
     <description>Pose mapviz plugin.</description>
   </class>
+  <class name="mapviz_plugins/pose_array" type="mapviz_plugins::PoseArrayPlugin" base_class_type="mapviz::MapvizPlugin">
+    <description>Pose Array mapviz plugin.</description>
+  </class>
   <class name="mapviz_plugins/marker" type="mapviz_plugins::MarkerPlugin" base_class_type="mapviz::MapvizPlugin">
     <description>Marker mapviz plugin.</description>
   </class>

mapviz_plugins/src/pose_array_plugin.cpp

@@ -0,0 +1,256 @@
+/**
+ * Copyright 2021 Trinity University
+ *
+ * Redistribution and use in source and binary forms, with or without 
+ * modification, are permitted provided that the following conditions are met:
+ *
+ * 1. Redistributions of source code must retain the above copyright notice, 
+ *    this list of conditions and the following disclaimer.
+ *
+ * 2. 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.
+ *
+ * 3. Neither the name of the copyright holder 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 THE COPYRIGHT HOLDER OR CONTRIBUTORS 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/pose_array_plugin.h>
+
+// Declare plugin
+#include <pluginlib/class_list_macros.h>
+PLUGINLIB_EXPORT_CLASS(mapviz_plugins::PoseArrayPlugin, mapviz::MapvizPlugin)
+
+namespace mapviz_plugins
+{
+  PoseArrayPlugin::PoseArrayPlugin() : config_widget_(new QWidget())
+  {
+    ui_.setupUi(config_widget_);
+
+    ui_.color->setColor(Qt::green);
+
+    // Set background white
+    QPalette p(config_widget_->palette());
+    p.setColor(QPalette::Background, Qt::white);
+    config_widget_->setPalette(p);
+
+    // Set status text red
+    QPalette p3(ui_.status->palette());
+    p3.setColor(QPalette::Text, Qt::red);
+    ui_.status->setPalette(p3);
+
+    QObject::connect(ui_.selecttopic, SIGNAL(clicked()), this,
+                     SLOT(SelectTopic()));
+    QObject::connect(ui_.topic, SIGNAL(editingFinished()), this,
+                     SLOT(TopicEdited()));
+    QObject::connect(ui_.positiontolerance, SIGNAL(valueChanged(double)), this,
+                     SLOT(PositionToleranceChanged(double)));
+    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)));
+    QObject::connect(ui_.color, SIGNAL(colorEdited(const QColor&)), this,
+            SLOT(SetColor(const QColor&)));
+  }
+
+  PoseArrayPlugin::~PoseArrayPlugin()
+  {
+  }
+
+  void PoseArrayPlugin::SelectTopic()
+  {
+    ros::master::TopicInfo topic =
+        mapviz::SelectTopicDialog::selectTopic("geometry_msgs/PoseArray");
+
+    if (!topic.name.empty())
+    {
+      ui_.topic->setText(QString::fromStdString(topic.name));
+      TopicEdited();
+    }
+  }
+
+  void PoseArrayPlugin::TopicEdited()
+  {
+    std::string topic = ui_.topic->text().trimmed().toStdString();
+    if (topic != topic_)
+    {
+      initialized_ = false;
+      ClearPoints();
+      has_message_ = false;
+      PrintWarning("No messages received.");
+
+      pose_sub_.shutdown();
+
+      topic_ = topic;
+      if (!topic.empty())
+      {
+        pose_sub_ = node_.subscribe(topic_, 10, &PoseArrayPlugin::PoseArrayCallback, this);
+
+        ROS_INFO("Subscribing to %s", topic_.c_str());
+      }
+    }
+  }
+
+  void PoseArrayPlugin::PoseArrayCallback(const geometry_msgs::PoseArrayConstPtr& msg)
+  {  
+    if (!has_message_)
+    {
+      initialized_ = true; // callback won't draw till this is true
+      has_message_ = true;
+    }
+
+    ClearPoints();
+
+    StampedPoint stamped_point;
+    for (unsigned int i=0 ; i < msg->poses.size(); i++)
+    {
+        stamped_point.stamp = msg->header.stamp;
+        stamped_point.source_frame = msg->header.frame_id;
+        geometry_msgs::Pose pose = msg->poses[i];
+
+        stamped_point.point = tf::Point(pose.position.x,
+                                        pose.position.y,
+                                        pose.position.z);
+
+        stamped_point.orientation = tf::Quaternion(
+                pose.orientation.x,
+                pose.orientation.y,
+                pose.orientation.z,
+                pose.orientation.w);
+
+        pushPoint( std::move( stamped_point) );
+    }
+  }
+
+  void PoseArrayPlugin::PrintError(const std::string& message)
+  {
+    PrintErrorHelper(ui_.status, message);
+  }
+
+  void PoseArrayPlugin::PrintInfo(const std::string& message)
+  {
+    PrintInfoHelper(ui_.status, message);
+  }
+
+  void PoseArrayPlugin::PrintWarning(const std::string& message)
+  {
+    PrintWarningHelper(ui_.status, message);
+  }
+
+  QWidget* PoseArrayPlugin::GetConfigWidget(QWidget* parent)
+  {
+    config_widget_->setParent(parent);
+
+    return config_widget_;
+  }
+
+  bool PoseArrayPlugin::Initialize(QGLWidget* canvas)
+  {
+    canvas_ = canvas;
+    SetColor(ui_.color->color());
+
+    return true;
+  }
+
+  void PoseArrayPlugin::Draw(double x, double y, double scale)
+  {
+    if (DrawPoints(scale))
+    {
+      PrintInfo("OK");
+    }
+  }
+
+  void PoseArrayPlugin::LoadConfig(const YAML::Node& node, const std::string& path)
+  {
+    if (node["topic"])
+    {
+      std::string topic;
+      node["topic"] >> topic;
+      ui_.topic->setText(topic.c_str());
+    }
+
+    if (node["color"])
+    {
+      std::string color;
+      node["color"] >> color;
+      QColor qcolor(color.c_str());
+      SetColor(qcolor);
+      ui_.color->setColor(qcolor);
+    }
+
+    if (node["draw_style"])
+    {
+      std::string draw_style;
+      node["draw_style"] >> draw_style;
+
+      if (draw_style == "arrows")
+      {
+        ui_.drawstyle->setCurrentIndex(0);
+        SetDrawStyle( ARROWS );
+      }
+      else if (draw_style == "points")
+      {
+        ui_.drawstyle->setCurrentIndex(1);
+        SetDrawStyle( POINTS );
+      }
+    }
+
+    if (node["position_tolerance"])
+    {
+      double position_tolerance;
+      node["position_tolerance"] >> position_tolerance;
+      ui_.positiontolerance->setValue(position_tolerance);
+      PositionToleranceChanged(position_tolerance);
+    }
+
+    if (node["static_arrow_sizes"])
+    {
+      bool static_arrow_sizes = node["static_arrow_sizes"].as<bool>();
+      ui_.static_arrow_sizes->setChecked(static_arrow_sizes);
+      SetStaticArrowSizes(static_arrow_sizes);
+    }
+
+    if (node["arrow_size"])
+    {
+      int arrow_size = node["arrow_size"].as<int>();
+      ui_.arrow_size->setValue(arrow_size);
+      SetArrowSize(arrow_size);
+    }
+
+    TopicEdited(); // forces redraw
+  }
+
+  void PoseArrayPlugin::SaveConfig(YAML::Emitter& emitter, const std::string& path)
+  {
+    std::string topic = ui_.topic->text().toStdString();
+    emitter << YAML::Key << "topic" << YAML::Value << topic;
+
+    emitter << YAML::Key << "color" << YAML::Value
+            << ui_.color->color().name().toStdString();
+
+    std::string draw_style = ui_.drawstyle->currentText().toStdString();
+    emitter << YAML::Key << "draw_style" << YAML::Value << draw_style;
+
+    emitter << YAML::Key << "position_tolerance" <<
+               YAML::Value << positionTolerance();
+
+    emitter << YAML::Key << "static_arrow_sizes" << YAML::Value << ui_.static_arrow_sizes->isChecked();
+
+    emitter << YAML::Key << "arrow_size" << YAML::Value << ui_.arrow_size->value();
+  }
+}