Program Listing for File marker_plugin.h
↰ Return to documentation for file (/tmp/ws/src/mapviz/mapviz_plugins/include/mapviz_plugins/marker_plugin.h
)
// *****************************************************************************
//
// Copyright (c) 2014-2020, 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 MAPVIZ_PLUGINS__MARKER_PLUGIN_H_
#define MAPVIZ_PLUGINS__MARKER_PLUGIN_H_
#include <mapviz/mapviz_plugin.h>
// QT libraries
#include <QGLWidget>
#include <QListWidgetItem>
// ROS libraries
#include <tf2/transform_datatypes.h>
#include <visualization_msgs/msg/marker.hpp>
#include <visualization_msgs/msg/marker_array.hpp>
#include <boost/functional/hash.hpp>
#include <mapviz/map_canvas.h>
// C++ standard libraries
#include <string>
#include <utility>
#include <unordered_map>
#include <vector>
// QT autogenerated files
#include "ui_marker_config.h"
namespace mapviz_plugins
{
using MarkerId = std::pair<std::string, int>;
struct MarkerIdHash {
std::size_t operator () (const MarkerId &p) const {
std::size_t seed = 0;
boost::hash_combine(seed, p.first);
boost::hash_combine(seed, p.second);
return seed;
}
};
struct MarkerNsHash {
std::size_t operator () (const std::string &p) const {
std::size_t seed = 0;
boost::hash_combine(seed, p);
return seed;
}
};
class MarkerPlugin : public mapviz::MapvizPlugin
{
Q_OBJECT
public:
MarkerPlugin();
~MarkerPlugin() override = default;
bool Initialize(QGLWidget* canvas) override;
void Shutdown() override {}
void Draw(double x, double y, double scale) override;
void Paint(QPainter* painter, double x, double y, double scale) override;
void Transform() override;
void LoadConfig(const YAML::Node& node, const std::string& path) override;
void SaveConfig(YAML::Emitter& emitter, const std::string& path) override;
QWidget* GetConfigWidget(QWidget* parent) override;
bool SupportsPainting() override
{
return true;
}
protected:
void PrintError(const std::string& message) override;
void PrintInfo(const std::string& message) override;
void PrintWarning(const std::string& message) override;
void timerEvent(QTimerEvent *) override;
protected Q_SLOTS:
void SelectTopic();
void TopicEdited();
void ClearHistory() override;
private:
struct Color
{
float r, g, b, a;
};
struct StampedPoint
{
tf2::Vector3 point;
tf2::Quaternion orientation;
tf2::Vector3 transformed_point;
tf2::Vector3 arrow_point;
tf2::Vector3 transformed_arrow_point;
tf2::Vector3 transformed_arrow_left;
tf2::Vector3 transformed_arrow_right;
Color color;
};
struct MarkerData
{
rclcpp::Time stamp;
rclcpp::Time expire_time;
int display_type;
Color color;
std::vector<StampedPoint> points;
std::string text;
float scale_x;
float scale_y;
float scale_z;
std::string source_frame;
swri_transform_util::Transform local_transform;
bool transformed;
};
Ui::marker_config ui_{};
QWidget* config_widget_;
std::string topic_;
rclcpp::Subscription<visualization_msgs::msg::Marker>::SharedPtr marker_sub_;
rclcpp::Subscription<visualization_msgs::msg::MarkerArray>::SharedPtr marker_array_sub_;
bool connected_;
bool has_message_{};
std::unordered_map<MarkerId, MarkerData, MarkerIdHash> markers_;
std::unordered_map<std::string, bool, MarkerNsHash> marker_visible_;
void handleMarker(visualization_msgs::msg::Marker::ConstSharedPtr marker);
void handleMarkerArray(visualization_msgs::msg::MarkerArray::ConstSharedPtr markers);
void processMarker(const visualization_msgs::msg::Marker& marker);
void subscribe();
void transformArrow(MarkerData& markerData,
const swri_transform_util::Transform& transform);
};
} // namespace mapviz_plugins
#endif // MAPVIZ_PLUGINS__MARKER_PLUGIN_H_