/
/
/
1#include "fast_slam_ros_visualization/line_landmark_display.hpp"
2
3
4#include <OgreManualObject.h>
5#include <OgreMaterialManager.h>
6#include <OgreTechnique.h>
7#include <OgreSceneManager.h>
8
9#include "rviz_common/logging.hpp"
10#include "rviz_common/msg_conversions.hpp"
11#include "rviz_common/properties/enum_property.hpp"
12#include "rviz_common/properties/color_property.hpp"
13#include "rviz_common/properties/float_property.hpp"
14#include "rviz_common/validate_floats.hpp"
15
16#include "rviz_rendering/material_manager.hpp"
17
18
19
20namespace fast_slam_ros_visualization{
21
22FlatArrow::FlatArrow(Ogre::SceneManager * scene_manager):
23 scene_manager_(scene_manager),
24 manual_object_(nullptr) {}
25
26FlatArrow::~FlatArrow(){
27 if (manual_object_) {
28 scene_manager_->destroyManualObject(manual_object_);
29 }
30}
31
32void FlatArrow::createAndAttachManualObject(Ogre::SceneNode * scene_node){
33 manual_object_ = scene_manager_->createManualObject();
34 manual_object_->setDynamic(true);
35 scene_node->attachObject(manual_object_);
36}
37
38void FlatArrow::updateManualObject(Ogre::ColourValue color, float alpha, float length, const OgrePose & pose){
39 clear();
40
41 color.a = alpha;
42 rviz_rendering::MaterialManager::enableAlphaBlending(material_, alpha);
43
44 manual_object_->begin(material_->getName(), Ogre::RenderOperation::OT_LINE_LIST, "rviz_rendering");
45 setManualObjectVertices(color, length, pose);
46 manual_object_->end();
47}
48void FlatArrow::clear(){
49 if (manual_object_) {
50 manual_object_->clear();
51 }
52}
53
54void FlatArrow::setManualObjectMaterial(Ogre::MaterialPtr material){
55 material_ = material;
56}
57
58void FlatArrow::setManualObjectVertices(const Ogre::ColourValue & color, float length, const OgrePose & pose){
59 manual_object_->estimateVertexCount(6);
60
61 Ogre::Vector3 vertices[6];
62 vertices[0] = Ogre::Vector3(0); // back of arrow
63 vertices[1] = pose.orientation * Ogre::Vector3(pose.distance, 0, 0); // tip of arrow
64 vertices[2] = vertices[1];
65 vertices[3] = pose.orientation * Ogre::Vector3(pose.distance - length, length, 0);
66 vertices[4] = vertices[1];
67 vertices[5] = pose.orientation * Ogre::Vector3(pose.distance - length, -length, 0);
68
69 for (const auto & vertex : vertices) {
70 manual_object_->position(vertex);
71 manual_object_->colour(color);
72 }
73}
74
75
76
77
78
79
80
81
82
83
84
85
86
87Line::Line(Ogre::SceneManager * scene_manager):
88 scene_manager_(scene_manager),
89 manual_object_(nullptr) {}
90
91Line::~Line(){
92 if (manual_object_) {
93 scene_manager_->destroyManualObject(manual_object_);
94 }
95}
96
97void Line::createAndAttachManualObject(Ogre::SceneNode * scene_node){
98 manual_object_ = scene_manager_->createManualObject();
99 manual_object_->setDynamic(true);
100 scene_node->attachObject(manual_object_);
101}
102
103void Line::updateManualObject(Ogre::ColourValue color, float alpha, float length, const OgrePose & pose){
104 clear();
105
106 color.a = alpha;
107 rviz_rendering::MaterialManager::enableAlphaBlending(material_, alpha);
108
109 manual_object_->begin(material_->getName(), Ogre::RenderOperation::OT_LINE_LIST, "rviz_rendering");
110 setManualObjectVertices(color, length, pose);
111 manual_object_->end();
112}
113
114void Line::clear(){
115 if (manual_object_) {
116 manual_object_->clear();
117 }
118}
119
120void Line::setManualObjectMaterial(Ogre::MaterialPtr material){
121 material_ = material;
122}
123
124void Line::setManualObjectVertices(const Ogre::ColourValue & color, float length, const OgrePose & pose){
125 manual_object_->estimateVertexCount(4);
126
127 Ogre::Vector3 vertices[4];
128 vertices[0] = pose.orientation * Ogre::Vector3(pose.distance, 0, 0); // tip of arrow
129 vertices[1] = pose.orientation * Ogre::Vector3(pose.distance, length, 0);
130 vertices[2] = vertices[0];
131 vertices[3] = pose.orientation * Ogre::Vector3(pose.distance, -length, 0);
132
133 for (const auto & vertex : vertices) {
134 manual_object_->position(vertex);
135 manual_object_->colour(color);
136 }
137}
138
139
140
141
142
143
144
145
146
147
148
149
150
151
152
153LineLandmarkDisplay::LineLandmarkDisplay(rviz_common::DisplayContext * display_context, Ogre::SceneNode * scene_node):
154 LineLandmarkDisplay(){
155 context_ = display_context;
156 scene_node_ = scene_node;
157 scene_manager_ = context_->getSceneManager();
158}
159
160LineLandmarkDisplay::LineLandmarkDisplay(){
161 initializeProperties();
162
163 arrow_alpha_property_->setMin(0);
164 arrow_alpha_property_->setMax(1);
165
166 static int material_count = 0;
167 std::string material_name = "Material" + std::to_string(material_count++);
168 material_ = rviz_rendering::MaterialManager::createMaterialWithNoLighting(material_name);
169}
170
171LineLandmarkDisplay::~LineLandmarkDisplay() = default;
172
173void LineLandmarkDisplay::processMessage(fast_slam_ros_msgs::msg::LineLandmarkArray::ConstSharedPtr msg){
174
175 rclcpp::Time time_stamp(msg->header.stamp, RCL_ROS_TIME);
176 if (!updateFrame(msg->header.frame_id, time_stamp)) {
177 setMissingTransformToFixedFrame(msg->header.frame_id);
178 return;
179 }
180 setTransformOk();
181
182 poses_.resize(msg->line_landmarks.size());
183
184 for (std::size_t i = 0; i < msg->line_landmarks.size(); ++i){
185 if (!rviz_common::validateFloats(msg->line_landmarks[i].heading) || !rviz_common::validateFloats(msg->line_landmarks[i].distance)) {
186 setStatus(
187 rviz_common::properties::StatusProperty::Error,
188 "Topic",
189 "Message contained invalid floating point values (nans or infs)");
190 return;
191 }
192
193 poses_[i].distance = msg->line_landmarks[i].distance;
194 poses_[i].orientation = Ogre::Quaternion(Ogre::Radian(msg->line_landmarks[i].heading), Ogre::Vector3(0,0,1));
195 }
196
197 updateArrows2d();
198
199 context_->queueRender();
200}
201
202
203void LineLandmarkDisplay::onInitialize(){
204 MFDClass::onInitialize();
205}
206
207void LineLandmarkDisplay::reset(){
208 MFDClass::reset();
209}
210
211void LineLandmarkDisplay::updateArrowColor(){
212 updateArrows2d();
213 context_->queueRender();
214}
215
216void LineLandmarkDisplay::initializeProperties(){
217 arrow_color_property_ = new rviz_common::properties::ColorProperty(
218 "Color", QColor(255, 25, 0), "Color to draw the arrows.", this, SLOT(updateArrowColor()));
219
220 arrow_alpha_property_ = new rviz_common::properties::FloatProperty(
221 "Alpha",
222 1.0f,
223 "Amount of transparency to apply to the displayed poses.",
224 this,
225 SLOT(updateArrowColor()));
226
227 line_length_property_ = new rviz_common::properties::FloatProperty(
228 "Arrow Length", 2.0f, "Length of the arrows.", this, SLOT(updateArrow2dGeometry()));
229
230 arrow_tip_length_property_ = new rviz_common::properties::FloatProperty(
231 "Arrow tip length", 0.2f, "Length of the arrows.", this, SLOT(updateArrow2dGeometry()));
232
233}
234void LineLandmarkDisplay::updateArrow2dGeometry(){
235 updateArrows2d();
236 context_->queueRender();
237}
238
239void LineLandmarkDisplay::updateArrows2d(){
240 while (arrows_.size() < poses_.size()) {
241 std::unique_ptr<FlatArrow> new_arrow = std::make_unique<FlatArrow>(scene_manager_);
242 new_arrow->createAndAttachManualObject(scene_node_);
243
244 arrows_.push_back(std::move(new_arrow));
245 }
246 while (arrows_.size() > poses_.size()) {
247 arrows_.pop_back();
248 }
249
250 while (lines_.size() < poses_.size()) {
251 std::unique_ptr<Line> new_line = std::make_unique<Line>(scene_manager_);
252 new_line->createAndAttachManualObject(scene_node_);
253
254 lines_.push_back(std::move(new_line));
255 }
256 while (lines_.size() > poses_.size()) {
257 lines_.pop_back();
258 }
259
260 for(size_t i = 0; i < poses_.size(); ++i){
261 arrows_[i]->setManualObjectMaterial(material_);
262 arrows_[i]->updateManualObject(
263 arrow_color_property_->getOgreColor(),
264 arrow_alpha_property_->getFloat(),
265 arrow_tip_length_property_->getFloat(),
266 poses_[i]);
267
268 lines_[i]->setManualObjectMaterial(material_);
269 lines_[i]->updateManualObject(
270 arrow_color_property_->getOgreColor(),
271 arrow_alpha_property_->getFloat(),
272 line_length_property_->getFloat(),
273 poses_[i]);
274 }
275}
276
277}
278#include <pluginlib/class_list_macros.hpp> // NOLINT
279PLUGINLIB_EXPORT_CLASS(fast_slam_ros_visualization::LineLandmarkDisplay, rviz_common::Display)