Fix the flakey rviz_rendering tests (#1026)

* Fix memory leak in effort_visual.cpp.

When shutting down, we were forgetting to free the
effort_arrow and effort_circle memory.  Switch to
using unique pointers there, which will take care of
this for us.

* Make sure to initialize ScrewVisual member variables.

Otherwise you can run into situations where the user
forgets to call one of the 'set' methods, and we now
access uninitialized memory.

* Make sure to set the frame orientation and position in effort visual.

As the API currently stands, it is very easy to access
uninitialized memory by forgetting to call setFramePosition()
and setFrameOrientation() before calling setEffort().
Fix at least this bit in the tests, and clean things
up a little so this is easier to read.

* Change EffortVisual constructor to take width and scale.

That way we ensure that these will always be set.

* Revamp adding things to the EffortVisual class during setEffort.

This is in an attempt to make sure we don't access
uninitialized memory.  It is a little complicated because
it is possible for the user to have set some of the information
before the setEffort() call, and we don't want to forget that.
So when setEffort() is called, we use the already set values
if they are available.  If they aren't, we use the identity.

On disable, we only erase the effort_arrow and effort_circle,
again so we don't forget information the user may have otherwise
provided us.

With all of this in place, it isn't possible to call setEffort
anymore and access uninitialized memory.

Signed-off-by: Chris Lalancette <[email protected]>

rviz_default_plugins/src/rviz_default_plugins/displays/effort/effort_display.cpp

@@ -291,12 +291,13 @@ void EffortDisplay::processMessage(sensor_msgs::msg::JointState::ConstSharedPtr
   std::shared_ptr<rviz_rendering::EffortVisual> visual;
   if (visuals_.size() == static_cast<size_t>(history_length_property_->getInt())) {
     visual = visuals_.front();
+    visual->setWidth(width_property_->getFloat());
+    visual->setScale(scale_property_->getFloat());
   } else {
     visual = std::make_shared<rviz_rendering::EffortVisual>(
-      context_->getSceneManager(), scene_node_);
+      context_->getSceneManager(), scene_node_,
+      width_property_->getFloat(), scale_property_->getFloat());
   }
-  visual->setWidth(width_property_->getFloat());
-  visual->setScale(scale_property_->getFloat());
 
   if (visuals_.size() >= static_cast<size_t>(history_length_property_->getInt())) {
     visuals_.pop_front();

rviz_rendering/include/rviz_rendering/objects/effort_visual.hpp

@@ -31,6 +31,7 @@
 #define RVIZ_RENDERING__OBJECTS__EFFORT_VISUAL_HPP_
 
 #include <map>
+#include <memory>
 #include <string>
 
 #include <OgreSceneNode.h>
@@ -47,7 +48,8 @@ class EffortVisual
 {
 public:
   RVIZ_RENDERING_PUBLIC
-  EffortVisual(Ogre::SceneManager * scene_manager, Ogre::SceneNode * parent_node);
+  EffortVisual(
+    Ogre::SceneManager * scene_manager, Ogre::SceneNode * parent_node, float width, float scale);
 
   // set rainbow color
   RVIZ_RENDERING_PUBLIC
@@ -72,8 +74,8 @@ class EffortVisual
 
 private:
   // The object implementing the effort circle
-  std::map<std::string, rviz_rendering::BillboardLine *> effort_circle_;
-  std::map<std::string, rviz_rendering::Arrow *> effort_arrow_;
+  std::map<std::string, std::unique_ptr<rviz_rendering::BillboardLine>> effort_circle_;
+  std::map<std::string, std::unique_ptr<rviz_rendering::Arrow>> effort_arrow_;
   std::map<std::string, bool> effort_enabled_;
 
   Ogre::SceneManager * scene_manager_;

rviz_rendering/src/rviz_rendering/objects/effort_visual.cpp

@@ -27,16 +27,17 @@
  * POSSIBILITY OF SUCH DAMAGE.
  */
 
- #define _USE_MATH_DEFINES
+#define _USE_MATH_DEFINES
 #include "rviz_rendering/objects/effort_visual.hpp"
 
 #include <algorithm>
 #include <cmath>
 
 namespace rviz_rendering
 {
-EffortVisual::EffortVisual(Ogre::SceneManager * scene_manager, Ogre::SceneNode * parent_node)
-: scene_manager_(scene_manager), parent_node_(parent_node)
+EffortVisual::EffortVisual(
+  Ogre::SceneManager * scene_manager, Ogre::SceneNode * parent_node, float width, float scale)
+: scene_manager_(scene_manager), parent_node_(parent_node), width_(width), scale_(scale)
 {
 }
 
@@ -71,15 +72,31 @@ void EffortVisual::setEffort(const std::string & joint_name, double effort, doub
   bool enabled = effort_enabled_.insert(std::make_pair(joint_name, true)).first->second;
 
   // enable or disable draw
-  if (effort_circle_.find(joint_name) != effort_circle_.end() && !enabled) {  // enable->disable
-    delete (effort_circle_[joint_name]);
-    delete (effort_arrow_[joint_name]);
-    effort_circle_.erase(joint_name);
-    effort_arrow_.erase(joint_name);
-  }
-  if (effort_circle_.find(joint_name) == effort_circle_.end() && enabled) {  // disable -> enable
-    effort_circle_[joint_name] = new rviz_rendering::BillboardLine(scene_manager_, parent_node_);
-    effort_arrow_[joint_name] = new rviz_rendering::Arrow(scene_manager_, parent_node_);
+  if (enabled) {
+    if (effort_circle_.count(joint_name) == 0) {
+      effort_circle_[joint_name] =
+        std::make_unique<rviz_rendering::BillboardLine>(scene_manager_, parent_node_);
+    }
+    if (effort_arrow_.count(joint_name) == 0) {
+      effort_arrow_[joint_name] =
+        std::make_unique<rviz_rendering::Arrow>(scene_manager_, parent_node_);
+    }
+    if (position_.count(joint_name) == 0) {
+      position_[joint_name] = Ogre::Vector3(0.0f, 0.0f, 0.0f);
+    }
+    if (orientation_.count(joint_name) == 0) {
+      orientation_[joint_name] = Ogre::Quaternion();
+    }
+  } else {
+    if (effort_circle_.count(joint_name) != 0) {
+      effort_circle_.erase(joint_name);
+    }
+    if (effort_arrow_.count(joint_name) != 0) {
+      effort_arrow_.erase(joint_name);
+    }
+    // Note that we specifically do not erase the position_ and orientation_ here, as the user
+    // may have set them via setFrame{Position,Orientation} below and we don't want to
+    // forget that information.
   }
 
   if (!enabled) {

rviz_rendering/src/rviz_rendering/objects/screw_visual.cpp

@@ -42,17 +42,16 @@
 namespace rviz_rendering
 {
 ScrewVisual::ScrewVisual(Ogre::SceneManager * scene_manager, Ogre::SceneNode * parent_node)
+: linear_scale_(0.0f), angular_scale_(0.0f), width_(0.0f), hide_small_values_(true),
+  scene_manager_(scene_manager)
 {
-  scene_manager_ = scene_manager;
-
   // Ogre::SceneNode s form a tree, with each node storing the transform (position and orientation)
   // of itself relative to its parent. Ogre does the math of combining those transforms
   // for rendering. Here we create a node to store the pose of the screw's header
   // frame relative to the RViz fixed frame.
   frame_node_ = parent_node->createChildSceneNode();
   linear_node_ = frame_node_->createChildSceneNode();
   angular_node_ = frame_node_->createChildSceneNode();
-  hide_small_values_ = true;
 
   // We create the arrow object within the frame node so that we can
   // set its position and direction relative to its header frame.

rviz_rendering/test/rviz_rendering/objects/effort_visual_test.cpp

@@ -73,24 +73,25 @@ class EffortVisualTestFixture : public ::testing::Test
   std::shared_ptr<rviz_rendering::OgreTestingEnvironment> testing_environment_;
 };
 
-Ogre::SceneNode * findForceArrow(Ogre::SceneNode * scene_node)
+static Ogre::SceneNode * findForceArrow(Ogre::SceneNode * scene_node)
 {
-  auto arrows = rviz_rendering::findAllArrows(scene_node);
-  auto billboard_line = rviz_rendering::findOneBillboardChain(scene_node);
-  for (const auto & arrow : arrows) {
+  std::vector<Ogre::SceneNode *> arrows = rviz_rendering::findAllArrows(scene_node);
+  Ogre::BillboardChain * billboard_line = rviz_rendering::findOneBillboardChain(scene_node);
+  for (Ogre::SceneNode * arrow : arrows) {
     if (billboard_line->getParentSceneNode()->getParent() == arrow->getParent()) {
       return arrow;
     }
   }
   return nullptr;
 }
 
-
 TEST_F(EffortVisualTestFixture, setEffort_sets_force_arrow_correctly) {
   auto scene_manager = Ogre::Root::getSingletonPtr()->createSceneManager();
   auto root_node = scene_manager->getRootSceneNode();
 
-  auto effort_visual = std::make_shared<rviz_rendering::EffortVisual>(scene_manager, root_node);
+  auto effort_visual = std::make_shared<rviz_rendering::EffortVisual>(
+    scene_manager, root_node, 0.0f, 0.0f);
+  ASSERT_NE(nullptr, effort_visual);
 
   Ogre::ColourValue color;
   effort_visual->getRainbowColor(0, color);
@@ -99,10 +100,11 @@ TEST_F(EffortVisualTestFixture, setEffort_sets_force_arrow_correctly) {
   effort_visual->getRainbowColor(1, color);
   EXPECT_THAT(color, ColorEq(Ogre::ColourValue(1, 0, 0, 1)));
 
+  effort_visual->setFramePosition("joint1", Ogre::Vector3(0, 0, 0));
+  effort_visual->setFrameOrientation("joint1", Ogre::Quaternion());
   effort_visual->setEffort("joint1", 1, 10);
 
-  effort_visual->setFramePosition("joint1", Ogre::Vector3());
-  auto arrows = rviz_rendering::findAllArrows(root_node);
+  std::vector<Ogre::SceneNode *> arrows = rviz_rendering::findAllArrows(root_node);
   EXPECT_THAT(arrows, SizeIs(1u));
   EXPECT_THAT(
     arrows[0]->convertWorldToLocalPosition(Ogre::Vector3(0, 0, 0)),
@@ -124,14 +126,14 @@ TEST_F(EffortVisualTestFixture, setEffort_hides_force_arrow_for_larger_width_tha
   auto scene_manager = Ogre::Root::getSingletonPtr()->createSceneManager();
   auto root_node = scene_manager->getRootSceneNode();
 
-  auto effort_visual = std::make_shared<rviz_rendering::EffortVisual>(scene_manager, root_node);
+  auto effort_visual = std::make_shared<rviz_rendering::EffortVisual>(
+    scene_manager, root_node, 5.0f, 0.7f);
+  ASSERT_NE(nullptr, effort_visual);
 
-  Ogre::Vector3 pos1(1, 2, 3);
+  effort_visual->setFramePosition("joint1", Ogre::Vector3(0, 0, 0));
+  effort_visual->setFrameOrientation("joint1", Ogre::Quaternion());
   effort_visual->setEffort("joint1", 1, 10);
 
-  effort_visual->setScale(0.7f);
-  effort_visual->setWidth(5);
-
   auto arrows = rviz_rendering::findAllArrows(root_node);
   EXPECT_THAT(arrows, SizeIs(1u));
   auto force_arrow = findForceArrow(root_node);

rviz_rendering/test/rviz_rendering/objects/screw_visual_test.cpp

@@ -100,9 +100,9 @@ TEST_F(ScrewVisualTestFixture, setScrew_sets_linear_arrow_correctly) {
   auto root_node = scene_manager->getRootSceneNode();
 
   auto screw_visual = std::make_shared<rviz_rendering::ScrewVisual>(scene_manager, root_node);
-  EXPECT_NE(nullptr, screw_visual);
+  ASSERT_NE(nullptr, screw_visual);
 
-  auto arrows = rviz_rendering::findAllArrows(root_node);
+  std::vector<Ogre::SceneNode *> arrows = rviz_rendering::findAllArrows(root_node);
   EXPECT_THAT(arrows, SizeIs(3u));
   auto linear_arrow = findLinearArrow(root_node);
   auto angular_arrow = findAngularArrow(root_node);