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