fast_slam_ros

1#include "fast_slam_ros_visualization/point_array_display.hpp"
2
3#include <OgreSceneNode.h>
4#include <OgreSceneManager.h>
5
6#include "rviz_rendering/objects/shape.hpp"
7
8#include "rviz_common/display_context.hpp"
9#include "rviz_common/msg_conversions.hpp"
10#include "rviz_common/properties/color_property.hpp"
11#include "rviz_common/properties/float_property.hpp"
12#include "rviz_common/frame_manager_iface.hpp"
13#include "rviz_common/validate_floats.hpp"
14#include "rviz_common/logging.hpp"
15
16namespace fast_slam_ros_visualization{
17    
18PointArrayDisplay::PointArrayDisplay(rviz_common::DisplayContext * context){
19    context_ = context;
20    scene_manager_ = context_->getSceneManager();
21    scene_node_ = scene_manager_->getRootSceneNode()->createChildSceneNode();
22    setUpProperties();
23}
24
25PointArrayDisplay::PointArrayDisplay(){
26    setUpProperties();
27}
28
29PointArrayDisplay::~PointArrayDisplay() = default;
30
31
32void PointArrayDisplay::processMessage(fast_slam_ros_msgs::msg::PointArray::ConstSharedPtr msg){
33
34    rclcpp::Time time_stamp(msg->header.stamp, RCL_ROS_TIME);
35    if (!updateFrame(msg->header.frame_id, time_stamp)) {
36        setMissingTransformToFixedFrame(msg->header.frame_id);
37        return;
38    }
39    setTransformOk();
40
41    visuals_.clear();
42
43    for(const auto &point : msg->points){
44        if (!rviz_common::validateFloats(point)) {
45            setStatus(
46            rviz_common::properties::StatusProperty::Error, "Topic",
47            "Message contained invalid floating point values (nans or infs)");
48            return;
49        }
50        createNewSphereVisual(point);
51    }
52}
53
54void PointArrayDisplay::onInitialize(){
55    MFDClass::onInitialize();
56}
57
58void PointArrayDisplay::reset(){
59    MFDClass::reset();
60    visuals_.clear();
61}
62
63void PointArrayDisplay::updateColorAndAlpha(){
64    float alpha = alpha_property_->getFloat();
65    float radius = radius_property_->getFloat();
66    Ogre::ColourValue color = color_property_->getOgreColor();
67
68    for (auto visual : visuals_) {
69        visual->setColor(color.r, color.g, color.b, alpha);
70        visual->setScale(Ogre::Vector3(radius, radius, radius));
71    }
72}
73
74void PointArrayDisplay::setUpProperties(){
75    color_property_ = new rviz_common::properties::ColorProperty(
76        "Color", QColor(204, 41, 204), "Color of a point", this, SLOT(updateColorAndAlpha()));
77
78    alpha_property_ = new rviz_common::properties::FloatProperty(
79        "Alpha", 1.0f, "0 is fully transparent, 1.0 is fully opaque.",
80        this, SLOT(updateColorAndAlpha()));
81
82    radius_property_ = new rviz_common::properties::FloatProperty(
83        "Radius", 0.2f, "Radius of a point", this, SLOT(updateColorAndAlpha()));
84}
85
86void PointArrayDisplay::createNewSphereVisual(const geometry_msgs::msg::Point & msg){
87    std::shared_ptr<rviz_rendering::Shape> visual = std::make_shared<rviz_rendering::Shape>(
88        rviz_rendering::Shape::Sphere, context_->getSceneManager(), scene_node_);
89
90    float alpha = alpha_property_->getFloat();
91    float radius = radius_property_->getFloat();
92    Ogre::ColourValue color = color_property_->getOgreColor();
93    visual->setColor(color.r, color.g, color.b, alpha);
94    visual->setPosition(rviz_common::pointMsgToOgre(msg));
95    visual->setScale(Ogre::Vector3(radius, radius, radius));
96
97    visuals_.push_back(visual);
98}
99
100}
101
102#include <pluginlib/class_list_macros.hpp>  // NOLINT
103PLUGINLIB_EXPORT_CLASS(fast_slam_ros_visualization::PointArrayDisplay, rviz_common::Display)