Add visualizer for PoseArray

This commit is contained in:
Kevin Nickels 2021-01-07 15:24:16 -06:00
parent 9417c9e4b8
commit f600ee256a
6 changed files with 740 additions and 0 deletions

View File

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

View File

@ -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

View File

@ -0,0 +1,104 @@
/**
* 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>
// C++ standard libraries
#include <list>
#include <string>
#include <vector>
// 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_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_

View File

@ -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>

View File

@ -0,0 +1,299 @@
/**
* Copyright 2020 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>
// C++ standard libraries
#include <cstdio>
#include <vector>
// QT libraries
#include <QDialog>
#include <QGLWidget>
#include <QPalette>
#include <opencv2/core/core.hpp>
// ROS libraries
#include <ros/master.h>
#include <swri_image_util/geometry_util.h>
#include <swri_transform_util/transform_util.h>
#include <mapviz/select_topic_dialog.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_.buffersize, SIGNAL(valueChanged(int)), this,
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)));
QObject::connect(ui_.color, SIGNAL(colorEdited(const QColor&)), this,
SLOT(SetColor(const QColor&)));
QObject::connect(ui_.show_laps, SIGNAL(toggled(bool)), this,
SLOT(LapToggled(bool)));
QObject::connect(ui_.buttonResetBuffer, SIGNAL(pressed()), this,
SLOT(ClearPoints()));
}
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;
has_message_ = true;
}
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 == "points")
{
ui_.drawstyle->setCurrentIndex(1);
SetDrawStyle( POINTS );
}
else if (draw_style == "arrows")
{
ui_.drawstyle->setCurrentIndex(2);
SetDrawStyle( ARROWS );
}
}
if (node["position_tolerance"])
{
double position_tolerance;
node["position_tolerance"] >> position_tolerance;
ui_.positiontolerance->setValue(position_tolerance);
PositionToleranceChanged(position_tolerance);
}
if (node["buffer_size"])
{
double buffer_size;
node["buffer_size"] >> buffer_size;
ui_.buffersize->setValue(buffer_size);
BufferSizeChanged(buffer_size);
}
if (node["show_laps"])
{
bool show_laps = false;
node["show_laps"] >> show_laps;
ui_.show_laps->setChecked(show_laps);
LapToggled(show_laps);
}
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();
}
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 << "buffer_size" << YAML::Value << bufferSize();
bool show_laps = ui_.show_laps->isChecked();
emitter << YAML::Key << "show_laps" << YAML::Value << show_laps;
emitter << YAML::Key << "static_arrow_sizes" << YAML::Value << ui_.static_arrow_sizes->isChecked();
emitter << YAML::Key << "arrow_size" << YAML::Value << ui_.arrow_size->value();
}
}

View File

@ -0,0 +1,325 @@
<?xml version="1.0" encoding="UTF-8"?>
<ui version="4.0">
<class>pose_config</class>
<widget class="QWidget" name="pose_array_config">
<property name="geometry">
<rect>
<x>0</x>
<y>0</y>
<width>400</width>
<height>222</height>
</rect>
</property>
<property name="windowTitle">
<string>Form</string>
</property>
<property name="styleSheet">
<string notr="true"/>
</property>
<layout class="QFormLayout" name="formLayout">
<item row="0" column="0">
<widget class="QLabel" name="label">
<property name="font">
<font>
<family>Sans Serif</family>
<pointsize>8</pointsize>
</font>
</property>
<property name="text">
<string>Topic:</string>
</property>
</widget>
</item>
<item row="0" column="1">
<widget class="QLineEdit" name="topic">
<property name="font">
<font>
<family>Sans Serif</family>
<pointsize>8</pointsize>
</font>
</property>
</widget>
</item>
<item row="1" column="1">
<widget class="QPushButton" name="selecttopic">
<property name="maximumSize">
<size>
<width>55</width>
<height>16777215</height>
</size>
</property>
<property name="font">
<font>
<family>Sans Serif</family>
<pointsize>8</pointsize>
</font>
</property>
<property name="styleSheet">
<string notr="true"/>
</property>
<property name="text">
<string>Select</string>
</property>
</widget>
</item>
<item row="2" column="0">
<widget class="QLabel" name="label_3">
<property name="font">
<font>
<family>Sans Serif</family>
<pointsize>8</pointsize>
</font>
</property>
<property name="text">
<string>Color:</string>
</property>
</widget>
</item>
<item row="2" column="1">
<widget class="mapviz::ColorButton" name="color">
<property name="maximumSize">
<size>
<width>24</width>
<height>24</height>
</size>
</property>
<property name="autoFillBackground">
<bool>false</bool>
</property>
<property name="styleSheet">
<string notr="true"/>
</property>
<property name="text">
<string/>
</property>
</widget>
</item>
<item row="3" column="0">
<widget class="QLabel" name="label_7">
<property name="font">
<font>
<family>Sans Serif</family>
<pointsize>8</pointsize>
</font>
</property>
<property name="text">
<string>Draw Style:</string>
</property>
</widget>
</item>
<item row="3" column="1">
<widget class="QComboBox" name="drawstyle">
<property name="maximumSize">
<size>
<width>16777215</width>
<height>25</height>
</size>
</property>
<property name="font">
<font>
<family>Sans Serif</family>
<pointsize>9</pointsize>
</font>
</property>
<item>
<property name="text">
<string>points</string>
</property>
</item>
<item>
<property name="text">
<string>arrows</string>
</property>
</item>
</widget>
</item>
<item row="4" column="0">
<widget class="QLabel" name="label_8">
<property name="font">
<font>
<family>Sans Serif</family>
<pointsize>8</pointsize>
</font>
</property>
<property name="text">
<string>Static Arrow Sizes:</string>
</property>
</widget>
</item>
<item row="4" column="1">
<layout class="QHBoxLayout" name="horizontalLayout">
<item>
<widget class="QCheckBox" name="static_arrow_sizes">
<property name="text">
<string/>
</property>
</widget>
</item>
<item>
<widget class="QSlider" name="arrow_size">
<property name="minimum">
<number>1</number>
</property>
<property name="maximum">
<number>500</number>
</property>
<property name="value">
<number>25</number>
</property>
<property name="orientation">
<enum>Qt::Horizontal</enum>
</property>
</widget>
</item>
</layout>
</item>
<item row="5" column="0">
<widget class="QLabel" name="label_4">
<property name="font">
<font>
<family>Sans Serif</family>
<pointsize>8</pointsize>
</font>
</property>
<property name="text">
<string>Position Tolerance:</string>
</property>
</widget>
</item>
<item row="5" column="1">
<widget class="QDoubleSpinBox" name="positiontolerance">
<property name="sizePolicy">
<sizepolicy hsizetype="Preferred" vsizetype="Fixed">
<horstretch>0</horstretch>
<verstretch>0</verstretch>
</sizepolicy>
</property>
<property name="buttonSymbols">
<enum>QAbstractSpinBox::PlusMinus</enum>
</property>
<property name="suffix">
<string/>
</property>
<property name="value">
<double>1.000000000000000</double>
</property>
</widget>
</item>
<item row="6" column="0">
<widget class="QLabel" name="label_6">
<property name="font">
<font>
<family>Sans Serif</family>
<pointsize>8</pointsize>
</font>
</property>
<property name="text">
<string>Buffer Size:</string>
</property>
</widget>
</item>
<item row="6" column="1">
<layout class="QHBoxLayout" name="horizontalLayout_2">
<item>
<widget class="QSpinBox" name="buffersize">
<property name="buttonSymbols">
<enum>QAbstractSpinBox::PlusMinus</enum>
</property>
<property name="maximum">
<number>99999999</number>
</property>
</widget>
</item>
<item>
<widget class="QPushButton" name="buttonResetBuffer">
<property name="maximumSize">
<size>
<width>55</width>
<height>16777215</height>
</size>
</property>
<property name="text">
<string>Clear</string>
</property>
</widget>
</item>
</layout>
</item>
<item row="7" column="0">
<widget class="QLabel" name="label_5">
<property name="text">
<string>Show Laps</string>
</property>
</widget>
</item>
<item row="7" column="1">
<widget class="QCheckBox" name="show_laps">
<property name="text">
<string/>
</property>
</widget>
</item>
<item row="8" column="0">
<widget class="QLabel" name="label_2">
<property name="font">
<font>
<family>Sans Serif</family>
<pointsize>8</pointsize>
</font>
</property>
<property name="text">
<string>Status:</string>
</property>
</widget>
</item>
<item row="8" column="1">
<widget class="QLabel" name="status">
<property name="font">
<font>
<family>Sans Serif</family>
<pointsize>8</pointsize>
</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="9" column="1">
<spacer name="verticalSpacer">
<property name="orientation">
<enum>Qt::Vertical</enum>
</property>
<property name="sizeHint" stdset="0">
<size>
<width>20</width>
<height>40</height>
</size>
</property>
</spacer>
</item>
</layout>
</widget>
<customwidgets>
<customwidget>
<class>mapviz::ColorButton</class>
<extends>QPushButton</extends>
<header location="global">mapviz/color_button.h</header>
</customwidget>
</customwidgets>
<resources/>
<connections/>
<slots>
<slot>SelectColor()</slot>
<slot>SelectTopic()</slot>
<slot>TopicEdited()</slot>
<slot>PositionToleranceChanged(double)</slot>
<slot>AngleToleranceChanged(double)</slot>
</slots>
</ui>