├── CMakeLists.txt
├── doc
    └── pose_with_covariance_array.png
├── msg
    └── PoseWithCovarianceArray.msg
├── package.xml
├── readme.md
├── rviz_plugin_description.xml
└── src
    ├── pose_with_covariance_array_display.cpp
    ├── pose_with_covariance_array_display.h
    └── pose_with_covariance_array_test_node.cpp


/CMakeLists.txt:
--------------------------------------------------------------------------------
 1 | cmake_minimum_required(VERSION 3.0.2)
 2 | project(pose_with_covariance_array)
 3 | 
 4 | add_compile_options(-std=c++11 -Wall)
 5 | 
 6 | find_package(catkin REQUIRED COMPONENTS
 7 |   roscpp
 8 |   std_msgs
 9 |   geometry_msgs
10 |   message_generation
11 |   rviz
12 | )
13 | 
14 | find_package(Boost REQUIRED)
15 | 
16 | ## This setting causes Qt's "MOC" generation to happen automatically.
17 | set(CMAKE_AUTOMOC ON)
18 | 
19 | ## This plugin includes Qt widgets, so we must include Qt.
20 | ## We'll use the version that rviz used so they are compatible.
21 | if(rviz_QT_VERSION VERSION_LESS "5")
22 |   message(STATUS "Using Qt4 based on the rviz_QT_VERSION: ${rviz_QT_VERSION}")
23 |   find_package(Qt4 ${rviz_QT_VERSION} EXACT REQUIRED QtCore QtGui)
24 |   ## pull in all required include dirs, define QT_LIBRARIES, etc.
25 |   include(${QT_USE_FILE})
26 | else()
27 |   message(STATUS "Using Qt5 based on the rviz_QT_VERSION: ${rviz_QT_VERSION}")
28 |   find_package(Qt5 ${rviz_QT_VERSION} EXACT REQUIRED Core Widgets)
29 |   ## make target_link_libraries(${QT_LIBRARIES}) pull in all required dependencies
30 |   set(QT_LIBRARIES Qt5::Widgets)
31 | endif()
32 | 
33 | ## I prefer the Qt signals and slots to avoid defining "emit", "slots",
34 | ## etc because they can conflict with boost signals, so define QT_NO_KEYWORDS here.
35 | add_definitions(-DQT_NO_KEYWORDS)
36 | 
37 | 
38 | add_message_files(
39 |   FILES
40 |   PoseWithCovarianceArray.msg
41 | )
42 | generate_messages(
43 |   DEPENDENCIES
44 |   std_msgs
45 |   geometry_msgs
46 | )
47 | 
48 | catkin_package(
49 | #  INCLUDE_DIRS include
50 | #  LIBRARIES pose_with_covariance_array
51 |   CATKIN_DEPENDS roscpp std_msgs geometry_msgs message_runtime rviz
52 |   DEPENDS Boost
53 | )
54 | 
55 | include_directories(
56 |  ${catkin_INCLUDE_DIRS}
57 | )
58 | 
59 | add_library(${PROJECT_NAME}
60 |   src/pose_with_covariance_array_display.cpp
61 | )
62 | add_dependencies(${PROJECT_NAME}
63 |   ${${PROJECT_NAME}_EXPORTED_TARGETS}
64 |   ${catkin_EXPORTED_TARGETS}
65 | )
66 | target_link_libraries(${PROJECT_NAME}
67 |   ${QT_LIBRARIES}
68 |   ${catkin_LIBRARIES}
69 | )
70 | 
71 | add_executable(${PROJECT_NAME}_test_node
72 |   src/pose_with_covariance_array_test_node.cpp
73 | )
74 | add_dependencies(${PROJECT_NAME}_test_node
75 |   ${${PROJECT_NAME}_EXPORTED_TARGETS}
76 |   ${catkin_EXPORTED_TARGETS}
77 | )
78 | target_link_libraries(${PROJECT_NAME}_test_node
79 |   ${catkin_LIBRARIES}
80 | )
81 | 


--------------------------------------------------------------------------------
/doc/pose_with_covariance_array.png:
--------------------------------------------------------------------------------
https://raw.githubusercontent.com/ke-sun/pose_with_covariance_array/55e5f358f03483ba36fdeb8894f408d917d77b88/doc/pose_with_covariance_array.png


--------------------------------------------------------------------------------
/msg/PoseWithCovarianceArray.msg:
--------------------------------------------------------------------------------
1 | std_msgs/Header header
2 | geometry_msgs/PoseWithCovariance[] poses
3 | 


--------------------------------------------------------------------------------
/package.xml:
--------------------------------------------------------------------------------
 1 | <?xml version="1.0"?>
 2 | <package format="2">
 3 |   <name>pose_with_covariance_array</name>
 4 |   <version>0.0.0</version>
 5 |   <description>
 6 |     Defines the msg and rviz plugin for pose with covariance array
 7 |   </description>
 8 | 
 9 |   <maintainer email="sunke.polyu@gmail.com">Ke Sun</maintainer>
10 |   <license>BSD 3-Clause</license>
11 | 
12 |   <author>Dave Hershberger</author>
13 |   <author>David Gossow</author>
14 |   <author>Josh Faust</author>
15 |   <author>Ke Sun</author>
16 | 
17 |   <buildtool_depend>catkin</buildtool_depend>
18 | 
19 |   <depend>roscpp</depend>
20 |   <depend>std_msgs</depend>
21 |   <depend>geometry_msgs</depend>
22 | 
23 |   <depend>message_generation</depend>
24 |   <depend>message_runtime</depend>
25 |   <depend>rviz</depend>
26 | 
27 |   <export>
28 |     <rviz plugin="${prefix}/rviz_plugin_description.xml"/>
29 |   </export>
30 | </package>
31 | 


--------------------------------------------------------------------------------
/readme.md:
--------------------------------------------------------------------------------
 1 | # Pose With Covariance Array RViz Plugin
 2 | 
 3 | This package defines the ROS message `PoseWithCovarianceArray` and its corresponding RViz plugin. The implementation of the RViz plugin is adapted from `PoseArray` from [RViz repository](https://github.com/ros-visualization/rviz).
 4 | 
 5 | ## Compilation
 6 | 
 7 | The package is tested with ROS Melodic. Once ROS is installed, the package can be simply compiled with
 8 | ```
 9 | catkin build -DCMAKE_BUILD_TYPE=Release
10 | ```
11 | To Test the implementation, use the provided ROS node,
12 | ```
13 | rosrun pose_with_covariance_array pose_with_covariance_array_test_node
14 | ```
15 | Start RViz (with the workspace containing this package sourced), and add the topic `\pose_array`, the visualization should look like the following.
16 | 
17 | ![example](doc/pose_with_covariance_array.png)
18 | 


--------------------------------------------------------------------------------
/rviz_plugin_description.xml:
--------------------------------------------------------------------------------
 1 | <library path="lib/libpose_with_covariance_array">
 2 |   <class name="pose_with_covariance_array/PoseWithCovarianceArray"
 3 |          type="rviz::PoseWithCovarianceArrayDisplay"
 4 |          base_class_type="rviz::Display">
 5 |     <description>
 6 |       Display an array of poses with covariance.
 7 |     </description>
 8 |     <message_type>pose_with_covariance_array/PoseWithCovarianceArray</message_type>
 9 |   </class>
10 | </library>
11 | 


--------------------------------------------------------------------------------
/src/pose_with_covariance_array_display.cpp:
--------------------------------------------------------------------------------
  1 | /*
  2 |  * Copyright (c) 2012, Willow Garage, Inc.
  3 |  * All rights reserved.
  4 |  *
  5 |  * Redistribution and use in source and binary forms, with or without
  6 |  * modification, are permitted provided that the following conditions are met:
  7 |  *
  8 |  *     * Redistributions of source code must retain the above copyright
  9 |  *       notice, this list of conditions and the following disclaimer.
 10 |  *     * Redistributions in binary form must reproduce the above copyright
 11 |  *       notice, this list of conditions and the following disclaimer in the
 12 |  *       documentation and/or other materials provided with the distribution.
 13 |  *     * Neither the name of the Willow Garage, Inc. nor the names of its
 14 |  *       contributors may be used to endorse or promote products derived from
 15 |  *       this software without specific prior written permission.
 16 |  *
 17 |  * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
 18 |  * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
 19 |  * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
 20 |  * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE
 21 |  * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
 22 |  * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
 23 |  * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
 24 |  * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
 25 |  * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
 26 |  * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
 27 |  * POSSIBILITY OF SUCH DAMAGE.
 28 |  */
 29 | 
 30 | #include <OgreManualObject.h>
 31 | #include <OgreSceneManager.h>
 32 | #include <OgreSceneNode.h>
 33 | 
 34 | #include <rviz/display_context.h>
 35 | #include <rviz/frame_manager.h>
 36 | #include <rviz/properties/enum_property.h>
 37 | #include <rviz/properties/color_property.h>
 38 | #include <rviz/properties/float_property.h>
 39 | #include <rviz/validate_floats.h>
 40 | #include <rviz/validate_quaternions.h>
 41 | #include <rviz/ogre_helpers/arrow.h>
 42 | #include <rviz/ogre_helpers/axes.h>
 43 | #include <rviz/default_plugin/covariance_property.h>
 44 | #include <rviz/default_plugin/covariance_visual.h>
 45 | 
 46 | #include "pose_with_covariance_array_display.h"
 47 | 
 48 | namespace rviz
 49 | {
 50 | namespace
 51 | {
 52 | struct ShapeType
 53 | {
 54 |   enum
 55 |   {
 56 |     Arrow2d,
 57 |     Arrow3d,
 58 |     Axes,
 59 |   };
 60 | };
 61 | 
 62 | Ogre::Vector3 vectorRosToOgre(geometry_msgs::Point const& point)
 63 | {
 64 |   return Ogre::Vector3(point.x, point.y, point.z);
 65 | }
 66 | 
 67 | Ogre::Quaternion quaternionRosToOgre(geometry_msgs::Quaternion const& quaternion)
 68 | {
 69 |   Ogre::Quaternion q;
 70 |   normalizeQuaternion(quaternion, q);
 71 |   return q;
 72 | }
 73 | } // namespace
 74 | 
 75 | PoseWithCovarianceArrayDisplay::PoseWithCovarianceArrayDisplay() : manual_object_(nullptr)
 76 | {
 77 |   shape_property_ = new EnumProperty("Shape", "Axes", "Shape to display the pose as.", this,
 78 |                                      SLOT(updateShapeChoice()));
 79 | 
 80 |   arrow_color_property_ = new ColorProperty("Color", QColor(255, 25, 0), "Color to draw the arrows.",
 81 |                                             this, SLOT(updateArrowColor()));
 82 | 
 83 |   arrow_alpha_property_ =
 84 |       new FloatProperty("Alpha", 1, "Amount of transparency to apply to the displayed poses.", this,
 85 |                         SLOT(updateArrowColor()));
 86 | 
 87 |   arrow2d_length_property_ = new FloatProperty("Arrow Length", 0.3, "Length of the arrows.", this,
 88 |                                                SLOT(updateArrow2dGeometry()));
 89 | 
 90 |   arrow3d_head_radius_property_ =
 91 |       new FloatProperty("Head Radius", 0.03, "Radius of the arrow's head, in meters.", this,
 92 |                         SLOT(updateArrow3dGeometry()));
 93 | 
 94 |   arrow3d_head_length_property_ =
 95 |       new FloatProperty("Head Length", 0.07, "Length of the arrow's head, in meters.", this,
 96 |                         SLOT(updateArrow3dGeometry()));
 97 | 
 98 |   arrow3d_shaft_radius_property_ =
 99 |       new FloatProperty("Shaft Radius", 0.01, "Radius of the arrow's shaft, in meters.", this,
100 |                         SLOT(updateArrow3dGeometry()));
101 | 
102 |   arrow3d_shaft_length_property_ =
103 |       new FloatProperty("Shaft Length", 0.23, "Length of the arrow's shaft, in meters.", this,
104 |                         SLOT(updateArrow3dGeometry()));
105 | 
106 |   axes_length_property_ = new FloatProperty("Axes Length", 0.3, "Length of each axis, in meters.", this,
107 |                                             SLOT(updateAxesGeometry()));
108 | 
109 |   axes_radius_property_ = new FloatProperty("Axes Radius", 0.01, "Radius of each axis, in meters.", this,
110 |                                             SLOT(updateAxesGeometry()));
111 | 
112 |   shape_property_->addOption("Arrow (Flat)", ShapeType::Arrow2d);
113 |   shape_property_->addOption("Arrow (3D)", ShapeType::Arrow3d);
114 |   shape_property_->addOption("Axes", ShapeType::Axes);
115 |   arrow_alpha_property_->setMin(0);
116 |   arrow_alpha_property_->setMax(1);
117 | 
118 |   covariance_property_ =
119 |     new CovarianceProperty("Covariance", true,
120 |                            "Whether or not the covariances of the messages should be shown.", this,
121 |                            SLOT(queueRender()));
122 | }
123 | 
124 | PoseWithCovarianceArrayDisplay::~PoseWithCovarianceArrayDisplay()
125 | {
126 |   if (initialized())
127 |   {
128 |     scene_manager_->destroyManualObject(manual_object_);
129 |   }
130 |   covariance_property_->clearVisual();
131 | }
132 | 
133 | void PoseWithCovarianceArrayDisplay::onInitialize()
134 | {
135 |   MFDClass::onInitialize();
136 |   manual_object_ = scene_manager_->createManualObject();
137 |   manual_object_->setDynamic(true);
138 |   scene_node_->attachObject(manual_object_);
139 |   arrow_node_ = scene_node_->createChildSceneNode();
140 |   axes_node_ = scene_node_->createChildSceneNode();
141 |   updateShapeChoice();
142 | }
143 | 
144 | bool validateFloats(const pose_with_covariance_array::PoseWithCovarianceArray& msg) {
145 |   bool valid = true;
146 |   for (size_t i = 0; i < msg.poses.size(); ++i) {
147 |     valid &= rviz::validateFloats(msg.poses[i].pose);
148 |     valid &= rviz::validateFloats(msg.poses[i].covariance);
149 |   }
150 |   return valid;
151 | }
152 | 
153 | bool validateQuaternions(const pose_with_covariance_array::PoseWithCovarianceArray& msg) {
154 |   bool valid = true;
155 |   for (size_t i = 0; i < msg.poses.size(); ++i)
156 |     valid &= rviz::validateQuaternions(msg.poses[i].pose);
157 |   return valid;
158 | }
159 | 
160 | void PoseWithCovarianceArrayDisplay::processMessage(
161 |     const pose_with_covariance_array::PoseWithCovarianceArray::ConstPtr& msg)
162 | {
163 |   if (!validateFloats(*msg))
164 |   {
165 |     setStatus(StatusProperty::Error, "Topic",
166 |               "Message contained invalid floating point values (nans or infs)");
167 |     return;
168 |   }
169 | 
170 |   if (!validateQuaternions(*msg))
171 |   {
172 |     ROS_WARN_ONCE_NAMED("quaternions",
173 |                         "PoseArray msg received on topic '%s' contains unnormalized quaternions. "
174 |                         "This warning will only be output once but may be true for others; "
175 |                         "enable DEBUG messages for ros.rviz.quaternions to see more details.",
176 |                         topic_property_->getTopicStd().c_str());
177 |     ROS_DEBUG_NAMED("quaternions",
178 |                     "PoseArray msg received on topic '%s' contains unnormalized quaternions.",
179 |                     topic_property_->getTopicStd().c_str());
180 |   }
181 | 
182 |   if (!setTransform(msg->header))
183 |   {
184 |     setStatus(StatusProperty::Error, "Topic", "Failed to look up transform");
185 |     return;
186 |   }
187 | 
188 |   poses_.resize(msg->poses.size());
189 |   for (size_t i = 0; i < msg->poses.size(); ++i)
190 |   {
191 |     poses_[i].position = vectorRosToOgre(msg->poses[i].pose.position);
192 |     poses_[i].orientation = quaternionRosToOgre(msg->poses[i].pose.orientation);
193 |     poses_[i].pose = msg->poses[i];
194 |   }
195 | 
196 |   updateDisplay();
197 |   context_->queueRender();
198 | }
199 | 
200 | bool PoseWithCovarianceArrayDisplay::setTransform(std_msgs::Header const& header)
201 | {
202 |   Ogre::Vector3 position;
203 |   Ogre::Quaternion orientation;
204 |   if (!context_->getFrameManager()->getTransform(header, position, orientation))
205 |   {
206 |     ROS_ERROR("Error transforming pose '%s' from frame '%s' to frame '%s'", qPrintable(getName()),
207 |               header.frame_id.c_str(), qPrintable(fixed_frame_));
208 |     return false;
209 |   }
210 |   scene_node_->setPosition(position);
211 |   scene_node_->setOrientation(orientation);
212 |   return true;
213 | }
214 | 
215 | void PoseWithCovarianceArrayDisplay::updateArrows2d()
216 | {
217 |   manual_object_->clear();
218 | 
219 |   typedef CovarianceProperty::CovarianceVisualPtr CovarianceVisualPtr;
220 |   covariance_property_->clearVisual();
221 | 
222 |   Ogre::ColourValue color = arrow_color_property_->getOgreColor();
223 |   color.a = arrow_alpha_property_->getFloat();
224 |   float length = arrow2d_length_property_->getFloat();
225 |   size_t num_poses = poses_.size();
226 |   manual_object_->estimateVertexCount(num_poses * 6);
227 |   manual_object_->begin("BaseWhiteNoLighting", Ogre::RenderOperation::OT_LINE_LIST);
228 |   for (size_t i = 0; i < num_poses; ++i)
229 |   {
230 |     const Ogre::Vector3& pos = poses_[i].position;
231 |     const Ogre::Quaternion& orient = poses_[i].orientation;
232 |     Ogre::Vector3 vertices[6];
233 |     vertices[0] = pos;                                        // back of arrow
234 |     vertices[1] = pos + orient * Ogre::Vector3(length, 0, 0); // tip of arrow
235 |     vertices[2] = vertices[1];
236 |     vertices[3] = pos + orient * Ogre::Vector3(0.75 * length, 0.2 * length, 0);
237 |     vertices[4] = vertices[1];
238 |     vertices[5] = pos + orient * Ogre::Vector3(0.75 * length, -0.2 * length, 0);
239 | 
240 |     for (int i = 0; i < 6; ++i)
241 |     {
242 |       manual_object_->position(vertices[i]);
243 |       manual_object_->colour(color);
244 |     }
245 | 
246 |     CovarianceVisualPtr cov = covariance_property_->createAndPushBackVisual(scene_manager_, arrow_node_);
247 |     cov->setPosition(poses_[i].position);
248 |     cov->setOrientation(poses_[i].orientation);
249 |     cov->setCovariance(poses_[i].pose);
250 |   }
251 |   manual_object_->end();
252 | }
253 | 
254 | void PoseWithCovarianceArrayDisplay::updateDisplay()
255 | {
256 |   int shape = shape_property_->getOptionInt();
257 |   switch (shape)
258 |   {
259 |   case ShapeType::Arrow2d:
260 |     updateArrows2d();
261 |     arrows3d_.clear();
262 |     axes_.clear();
263 |     break;
264 |   case ShapeType::Arrow3d:
265 |     updateArrows3d();
266 |     manual_object_->clear();
267 |     axes_.clear();
268 |     break;
269 |   case ShapeType::Axes:
270 |     updateAxes();
271 |     manual_object_->clear();
272 |     arrows3d_.clear();
273 |     break;
274 |   }
275 | }
276 | 
277 | void PoseWithCovarianceArrayDisplay::updateArrows3d()
278 | {
279 |   while (arrows3d_.size() < poses_.size())
280 |     arrows3d_.push_back(makeArrow3d());
281 |   while (arrows3d_.size() > poses_.size())
282 |     arrows3d_.pop_back();
283 | 
284 |   typedef CovarianceProperty::CovarianceVisualPtr CovarianceVisualPtr;
285 |   covariance_property_->clearVisual();
286 | 
287 |   Ogre::Quaternion adjust_orientation(Ogre::Degree(-90), Ogre::Vector3::UNIT_Y);
288 |   for (std::size_t i = 0; i < poses_.size(); ++i)
289 |   {
290 |     arrows3d_[i].setPosition(poses_[i].position);
291 |     arrows3d_[i].setOrientation(poses_[i].orientation * adjust_orientation);
292 | 
293 |     CovarianceVisualPtr cov = covariance_property_->createAndPushBackVisual(scene_manager_, arrow_node_);
294 |     cov->setPosition(poses_[i].position);
295 |     cov->setOrientation(poses_[i].orientation);
296 |     cov->setCovariance(poses_[i].pose);
297 |   }
298 | }
299 | 
300 | void PoseWithCovarianceArrayDisplay::updateAxes()
301 | {
302 |   while (axes_.size() < poses_.size())
303 |     axes_.push_back(makeAxes());
304 |   while (axes_.size() > poses_.size())
305 |     axes_.pop_back();
306 | 
307 |   typedef CovarianceProperty::CovarianceVisualPtr CovarianceVisualPtr;
308 |   covariance_property_->clearVisual();
309 | 
310 |   for (std::size_t i = 0; i < poses_.size(); ++i)
311 |   {
312 |     axes_[i].setPosition(poses_[i].position);
313 |     axes_[i].setOrientation(poses_[i].orientation);
314 | 
315 |     CovarianceVisualPtr cov = covariance_property_->createAndPushBackVisual(scene_manager_, axes_node_);
316 |     cov->setPosition(poses_[i].position);
317 |     cov->setOrientation(poses_[i].orientation);
318 |     cov->setCovariance(poses_[i].pose);
319 |   }
320 | }
321 | 
322 | Arrow* PoseWithCovarianceArrayDisplay::makeArrow3d()
323 | {
324 |   Ogre::ColourValue color = arrow_color_property_->getOgreColor();
325 |   color.a = arrow_alpha_property_->getFloat();
326 | 
327 |   Arrow* arrow =
328 |       new Arrow(scene_manager_, arrow_node_, arrow3d_shaft_length_property_->getFloat(),
329 |                 arrow3d_shaft_radius_property_->getFloat(), arrow3d_head_length_property_->getFloat(),
330 |                 arrow3d_head_radius_property_->getFloat());
331 | 
332 |   arrow->setColor(color);
333 |   return arrow;
334 | }
335 | 
336 | Axes* PoseWithCovarianceArrayDisplay::makeAxes()
337 | {
338 |   return new Axes(scene_manager_, axes_node_, axes_length_property_->getFloat(),
339 |                   axes_radius_property_->getFloat());
340 | }
341 | 
342 | void PoseWithCovarianceArrayDisplay::reset()
343 | {
344 |   MFDClass::reset();
345 |   if (manual_object_)
346 |   {
347 |     manual_object_->clear();
348 |   }
349 |   arrows3d_.clear();
350 |   axes_.clear();
351 |   covariance_property_->clearVisual();
352 | }
353 | 
354 | void PoseWithCovarianceArrayDisplay::updateShapeChoice()
355 | {
356 |   int shape = shape_property_->getOptionInt();
357 |   bool use_arrow2d = shape == ShapeType::Arrow2d;
358 |   bool use_arrow3d = shape == ShapeType::Arrow3d;
359 |   bool use_arrow = use_arrow2d || use_arrow3d;
360 |   bool use_axes = shape == ShapeType::Axes;
361 | 
362 |   arrow_color_property_->setHidden(!use_arrow);
363 |   arrow_alpha_property_->setHidden(!use_arrow);
364 | 
365 |   arrow2d_length_property_->setHidden(!use_arrow2d);
366 | 
367 |   arrow3d_shaft_length_property_->setHidden(!use_arrow3d);
368 |   arrow3d_shaft_radius_property_->setHidden(!use_arrow3d);
369 |   arrow3d_head_length_property_->setHidden(!use_arrow3d);
370 |   arrow3d_head_radius_property_->setHidden(!use_arrow3d);
371 | 
372 |   axes_length_property_->setHidden(!use_axes);
373 |   axes_radius_property_->setHidden(!use_axes);
374 | 
375 |   if (initialized())
376 |     updateDisplay();
377 | }
378 | 
379 | void PoseWithCovarianceArrayDisplay::updateArrowColor()
380 | {
381 |   int shape = shape_property_->getOptionInt();
382 |   Ogre::ColourValue color = arrow_color_property_->getOgreColor();
383 |   color.a = arrow_alpha_property_->getFloat();
384 | 
385 |   if (shape == ShapeType::Arrow2d)
386 |   {
387 |     updateArrows2d();
388 |   }
389 |   else if (shape == ShapeType::Arrow3d)
390 |   {
391 |     for (std::size_t i = 0; i < arrows3d_.size(); ++i)
392 |     {
393 |       arrows3d_[i].setColor(color);
394 |     }
395 |   }
396 |   context_->queueRender();
397 | }
398 | 
399 | void PoseWithCovarianceArrayDisplay::updateArrow2dGeometry()
400 | {
401 |   updateArrows2d();
402 |   context_->queueRender();
403 | }
404 | 
405 | void PoseWithCovarianceArrayDisplay::updateArrow3dGeometry()
406 | {
407 |   for (std::size_t i = 0; i < poses_.size(); ++i)
408 |   {
409 |     arrows3d_[i].set(arrow3d_shaft_length_property_->getFloat(),
410 |                      arrow3d_shaft_radius_property_->getFloat(),
411 |                      arrow3d_head_length_property_->getFloat(),
412 |                      arrow3d_head_radius_property_->getFloat());
413 |   }
414 |   context_->queueRender();
415 | }
416 | 
417 | void PoseWithCovarianceArrayDisplay::updateAxesGeometry()
418 | {
419 |   for (std::size_t i = 0; i < poses_.size(); ++i)
420 |   {
421 |     axes_[i].set(axes_length_property_->getFloat(), axes_radius_property_->getFloat());
422 |   }
423 |   context_->queueRender();
424 | }
425 | 
426 | } // namespace rviz
427 | 
428 | #include <pluginlib/class_list_macros.hpp>
429 | PLUGINLIB_EXPORT_CLASS(rviz::PoseWithCovarianceArrayDisplay, rviz::Display)
430 | 


--------------------------------------------------------------------------------
/src/pose_with_covariance_array_display.h:
--------------------------------------------------------------------------------
  1 | /*
  2 |  * Copyright (c) 2012, Willow Garage, Inc.
  3 |  * All rights reserved.
  4 |  *
  5 |  * Redistribution and use in source and binary forms, with or without
  6 |  * modification, are permitted provided that the following conditions are met:
  7 |  *
  8 |  *     * Redistributions of source code must retain the above copyright
  9 |  *       notice, this list of conditions and the following disclaimer.
 10 |  *     * Redistributions in binary form must reproduce the above copyright
 11 |  *       notice, this list of conditions and the following disclaimer in the
 12 |  *       documentation and/or other materials provided with the distribution.
 13 |  *     * Neither the name of the Willow Garage, Inc. nor the names of its
 14 |  *       contributors may be used to endorse or promote products derived from
 15 |  *       this software without specific prior written permission.
 16 |  *
 17 |  * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
 18 |  * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
 19 |  * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
 20 |  * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE
 21 |  * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
 22 |  * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
 23 |  * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
 24 |  * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
 25 |  * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
 26 |  * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
 27 |  * POSSIBILITY OF SUCH DAMAGE.
 28 |  */
 29 | 
 30 | #pragma once
 31 | 
 32 | #include <boost/shared_ptr.hpp>
 33 | #include <boost/ptr_container/ptr_vector.hpp>
 34 | 
 35 | #include <rviz/message_filter_display.h>
 36 | #include <pose_with_covariance_array/PoseWithCovarianceArray.h>
 37 | 
 38 | namespace Ogre
 39 | {
 40 | class ManualObject;
 41 | }
 42 | 
 43 | namespace rviz
 44 | {
 45 | class EnumProperty;
 46 | class ColorProperty;
 47 | class FloatProperty;
 48 | class Arrow;
 49 | class Axes;
 50 | class CovarianceProperty;
 51 | 
 52 | class PoseWithCovarianceArrayDisplay :
 53 |   public MessageFilterDisplay<pose_with_covariance_array::PoseWithCovarianceArray>
 54 | {
 55 |   Q_OBJECT
 56 | public:
 57 |   PoseWithCovarianceArrayDisplay();
 58 |   ~PoseWithCovarianceArrayDisplay() override;
 59 | 
 60 | protected:
 61 |   void onInitialize() override;
 62 |   void reset() override;
 63 |   void processMessage(const pose_with_covariance_array::PoseWithCovarianceArray::ConstPtr& msg) override;
 64 | 
 65 | private:
 66 |   bool setTransform(std_msgs::Header const& header);
 67 |   void updateArrows2d();
 68 |   void updateArrows3d();
 69 |   void updateAxes();
 70 |   void updateDisplay();
 71 |   Axes* makeAxes();
 72 |   Arrow* makeArrow3d();
 73 | 
 74 |   struct OgrePose
 75 |   {
 76 |     Ogre::Vector3 position;
 77 |     Ogre::Quaternion orientation;
 78 |     geometry_msgs::PoseWithCovariance pose;
 79 |   };
 80 | 
 81 |   std::vector<OgrePose> poses_;
 82 |   boost::ptr_vector<Arrow> arrows3d_;
 83 |   boost::ptr_vector<Axes> axes_;
 84 | 
 85 |   Ogre::SceneNode* arrow_node_;
 86 |   Ogre::SceneNode* axes_node_;
 87 |   Ogre::ManualObject* manual_object_;
 88 | 
 89 |   EnumProperty* shape_property_;
 90 |   ColorProperty* arrow_color_property_;
 91 |   FloatProperty* arrow_alpha_property_;
 92 | 
 93 |   FloatProperty* arrow2d_length_property_;
 94 | 
 95 |   FloatProperty* arrow3d_head_radius_property_;
 96 |   FloatProperty* arrow3d_head_length_property_;
 97 |   FloatProperty* arrow3d_shaft_radius_property_;
 98 |   FloatProperty* arrow3d_shaft_length_property_;
 99 | 
100 |   FloatProperty* axes_length_property_;
101 |   FloatProperty* axes_radius_property_;
102 | 
103 |   CovarianceProperty* covariance_property_;
104 | 
105 | private Q_SLOTS:
106 |   /// Update the interface and visible shapes based on the selected shape type.
107 |   void updateShapeChoice();
108 | 
109 |   /// Update the arrow color.
110 |   void updateArrowColor();
111 | 
112 |   /// Update the flat arrow geometry.
113 |   void updateArrow2dGeometry();
114 | 
115 |   /// Update the 3D arrow geometry.
116 |   void updateArrow3dGeometry();
117 | 
118 |   /// Update the axes geometry.
119 |   void updateAxesGeometry();
120 | };
121 | 
122 | } // namespace rviz
123 | 


--------------------------------------------------------------------------------
/src/pose_with_covariance_array_test_node.cpp:
--------------------------------------------------------------------------------
 1 | #include <ros/ros.h>
 2 | #include <pose_with_covariance_array/PoseWithCovarianceArray.h>
 3 | 
 4 | using namespace std;
 5 | using namespace ros;
 6 | using namespace pose_with_covariance_array;
 7 | 
 8 | int main(int argc, char** argv) {
 9 | 
10 |   ros::init(argc, argv, "pose_with_covariance_array_test_node");
11 |   ros::NodeHandle nh;
12 |   ros::Publisher pub = nh.advertise<PoseWithCovarianceArray>("pose_array", 1, true);
13 | 
14 |   PoseWithCovarianceArray msg;
15 |   msg.header.stamp = ros::Time::now();
16 |   msg.header.frame_id = "map";
17 |   msg.poses.resize(3);
18 | 
19 |   msg.poses[0].pose.position.x = 0.0;
20 |   msg.poses[0].pose.position.y = 0.0;
21 |   msg.poses[0].pose.position.z = 0.0;
22 |   msg.poses[0].pose.orientation.x = 0.0;
23 |   msg.poses[0].pose.orientation.y = 0.0;
24 |   msg.poses[0].pose.orientation.z = 0.0;
25 |   msg.poses[0].pose.orientation.w = 1.0;
26 |   for (size_t i=0; i<36; ++i) msg.poses[0].covariance[i] = 0.0;
27 |   msg.poses[0].covariance[0] = 1.0;
28 |   msg.poses[0].covariance[7] = 1.0;
29 |   msg.poses[0].covariance[14] = 0.0;
30 | 
31 |   msg.poses[1].pose.position.x = 1.5;
32 |   msg.poses[1].pose.position.y = 1.5;
33 |   msg.poses[1].pose.position.z = 0.0;
34 |   msg.poses[1].pose.orientation.x = 0.0;
35 |   msg.poses[1].pose.orientation.y = 0.0;
36 |   msg.poses[1].pose.orientation.z = 0.0;
37 |   msg.poses[1].pose.orientation.w = 1.0;
38 |   for (size_t i=0; i<36; ++i) msg.poses[1].covariance[i] = 0.0;
39 |   msg.poses[1].covariance[0] = 1.5;
40 |   msg.poses[1].covariance[7] = 1.5;
41 |   msg.poses[1].covariance[14] = 0.0;
42 | 
43 |   msg.poses[2].pose.position.x = 3.0;
44 |   msg.poses[2].pose.position.y = 3.0;
45 |   msg.poses[2].pose.position.z = 0.0;
46 |   msg.poses[2].pose.orientation.x = 0.0;
47 |   msg.poses[2].pose.orientation.y = 0.0;
48 |   msg.poses[2].pose.orientation.z = 0.0;
49 |   msg.poses[2].pose.orientation.w = 1.0;
50 |   for (size_t i=0; i<36; ++i) msg.poses[2].covariance[i] = 0.0;
51 |   msg.poses[2].covariance[0] = 2.0;
52 |   msg.poses[2].covariance[7] = 2.0;
53 |   msg.poses[2].covariance[14] = 0.0;
54 | 
55 |   pub.publish(msg);
56 | 
57 |   ros::spin();
58 |   return 0;
59 | }
60 | 


--------------------------------------------------------------------------------