Merge branch 'melodic-devel' into noetic-devel

.clang-format

@@ -58,6 +58,7 @@ BreakBeforeBraces: Custom
 
 # Control of individual brace wrapping cases
 BraceWrapping:
+    AfterCaseLabel: true
     AfterClass: true
     AfterCaseLabel: true
     AfterControlStatement: true

src/rviz/default_plugin/covariance_property.cpp

@@ -47,8 +47,7 @@ CovarianceProperty::CovarianceProperty(const QString& name,
                                        Property* parent,
                                        const char* changed_slot,
                                        QObject* receiver)
-  // NOTE: changed_slot and receiver aren't passed to BoolProperty here, but initialized at the end of
-  // this constructor
+  // NOTE: changed() signal will only be initialized at the end of this constructor
   : BoolProperty(name, default_value, description, parent)
 {
   position_property_ =
@@ -104,14 +103,14 @@ CovarianceProperty::CovarianceProperty(const QString& name,
   orientation_offset_property_ = new FloatProperty(
       "Offset", 1.0f,
       "For 3D poses is the distance where to position the ellipses representing orientation covariance. "
-      "For 2D poses is the height of the triangle representing the variance on yaw",
+      "For 2D poses is the height of the triangle representing the variance on yaw.",
       orientation_property_, SLOT(updateColorAndAlphaAndScaleAndOffset()), this);
   orientation_offset_property_->setMin(0);
 
   orientation_scale_property_ =
       new FloatProperty("Scale", 1.0f,
                         "Scale factor to be applied to orientation covariance shapes. "
-                        "Corresponds to the number of standard deviations to display",
+                        "Corresponds to the number of standard deviations to display.",
                         orientation_property_, SLOT(updateColorAndAlphaAndScaleAndOffset()), this);
   orientation_scale_property_->setMin(0);
 

src/rviz/default_plugin/covariance_visual.cpp

@@ -53,20 +53,18 @@ double deg2rad(double degrees)
 void makeRightHanded(Eigen::Matrix3d& eigenvectors, Eigen::Vector3d& eigenvalues)
 {
   // Note that sorting of eigenvalues may end up with left-hand coordinate system.
-  // So here we correctly sort it so that it does end up being righ-handed and normalised.
-  Eigen::Vector3d c0 = eigenvectors.block<3, 1>(0, 0);
+  // So here we correctly sort it so that it does end up being right-handed and normalised.
+  Eigen::Vector3d c0 = eigenvectors.col(0);
   c0.normalize();
-  Eigen::Vector3d c1 = eigenvectors.block<3, 1>(0, 1);
+  Eigen::Vector3d c1 = eigenvectors.col(1);
   c1.normalize();
-  Eigen::Vector3d c2 = eigenvectors.block<3, 1>(0, 2);
+  Eigen::Vector3d c2 = eigenvectors.col(2);
   c2.normalize();
   Eigen::Vector3d cc = c0.cross(c1);
   if (cc.dot(c2) < 0)
   {
     eigenvectors << c1, c0, c2;
-    double e = eigenvalues[0];
-    eigenvalues[0] = eigenvalues[1];
-    eigenvalues[1] = e;
+    std::swap(eigenvalues[0], eigenvalues[1]);
   }
   else
   {
@@ -91,9 +89,7 @@ void makeRightHanded(Eigen::Matrix2d& eigenvectors, Eigen::Vector2d& eigenvalues
   if (cc[2] < 0)
   {
     eigenvectors << c1.head<2>(), c0.head<2>();
-    double e = eigenvalues[0];
-    eigenvalues[0] = eigenvalues[1];
-    eigenvalues[1] = e;
+    std::swap(eigenvalues[0], eigenvalues[1]);
   }
   else
   {
@@ -129,9 +125,10 @@ void computeShapeScaleAndOrientation3D(const Eigen::Matrix3d& covariance,
   makeRightHanded(eigenvectors, eigenvalues);
 
   // Define the rotation
-  orientation.FromRotationMatrix(Ogre::Matrix3(
-      eigenvectors(0, 0), eigenvectors(0, 1), eigenvectors(0, 2), eigenvectors(1, 0), eigenvectors(1, 1),
-      eigenvectors(1, 2), eigenvectors(2, 0), eigenvectors(2, 1), eigenvectors(2, 2)));
+  orientation.FromRotationMatrix(Ogre::Matrix3( // clang-format off
+      eigenvectors(0, 0), eigenvectors(0, 1), eigenvectors(0, 2),
+      eigenvectors(1, 0), eigenvectors(1, 1), eigenvectors(1, 2),
+      eigenvectors(2, 0), eigenvectors(2, 1), eigenvectors(2, 2))); // clang-format on
 
   // Define the scale. eigenvalues are the variances, so we take the sqrt to draw the standard deviation
   scale.x = 2 * std::sqrt(eigenvalues[0]);
@@ -179,26 +176,31 @@ void computeShapeScaleAndOrientation2D(const Eigen::Matrix2d& covariance,
   // deviation. The scale of the missing dimension is set to zero.
   if (plane == YZ_PLANE)
   {
-    orientation.FromRotationMatrix(Ogre::Matrix3(1, 0, 0, 0, eigenvectors(0, 0), eigenvectors(0, 1), 0,
-                                                 eigenvectors(1, 0), eigenvectors(1, 1)));
+    orientation.FromRotationMatrix(Ogre::Matrix3(1, 0, 0, // clang-format off
+                                                 0, eigenvectors(0, 0), eigenvectors(0, 1),
+                                                 0, eigenvectors(1, 0), eigenvectors(1, 1))); // clang-format on
 
     scale.x = 0;
     scale.y = 2 * std::sqrt(eigenvalues[0]);
     scale.z = 2 * std::sqrt(eigenvalues[1]);
   }
   else if (plane == XZ_PLANE)
   {
-    orientation.FromRotationMatrix(Ogre::Matrix3(eigenvectors(0, 0), 0, eigenvectors(0, 1), 0, 1, 0,
-                                                 eigenvectors(1, 0), 0, eigenvectors(1, 1)));
+    orientation.FromRotationMatrix(
+        Ogre::Matrix3(eigenvectors(0, 0), 0, eigenvectors(0, 1), // clang-format off
+                      0, 1, 0,
+                      eigenvectors(1, 0), 0, eigenvectors(1, 1))); // clang-format on
 
     scale.x = 2 * std::sqrt(eigenvalues[0]);
     scale.y = 0;
     scale.z = 2 * std::sqrt(eigenvalues[1]);
   }
   else // plane == XY_PLANE
   {
-    orientation.FromRotationMatrix(Ogre::Matrix3(eigenvectors(0, 0), eigenvectors(0, 1), 0,
-                                                 eigenvectors(1, 0), eigenvectors(1, 1), 0, 0, 0, 1));
+    orientation.FromRotationMatrix(
+        Ogre::Matrix3(eigenvectors(0, 0), eigenvectors(0, 1), 0, // clang-format off
+                      eigenvectors(1, 0), eigenvectors(1, 1), 0,
+                      0, 0, 1)); // clang-format on
 
     scale.x = 2 * std::sqrt(eigenvalues[0]);
     scale.y = 2 * std::sqrt(eigenvalues[1]);
@@ -428,8 +430,7 @@ void CovarianceVisual::updateOrientation(const Eigen::Matrix6d& covariance, Shap
       covarianceAxis = covariance.block<2, 2>(3, 3);
     }
 
-    // NOTE: The cylinder mesh is oriented along its y axis, thus we want to flat it out into the XZ
-    // plane
+    // NOTE: The cylinder mesh is oriented along its y axis, we want to flat it out into the XZ plane
     computeShapeScaleAndOrientation2D(covarianceAxis, shape_scale, shape_orientation, XZ_PLANE);
     // Give a minimal height for the cylinder for better visualization
     shape_scale.y = 0.001;

src/rviz/default_plugin/effort_display.cpp

@@ -1,5 +1,6 @@
 #include <OgreSceneNode.h>
 #include <OgreSceneManager.h>
+#include <QTimer>
 
 #include <rviz/visualization_manager.h>
 #include <rviz/properties/color_property.h>
@@ -177,22 +178,33 @@ void EffortDisplay::load()
 {
   // get robot_description
   std::string content;
-  if (!update_nh_.getParam(robot_description_property_->getStdString(), content))
+  try
   {
-    std::string loc;
-    if (update_nh_.searchParam(robot_description_property_->getStdString(), loc))
+    if (!update_nh_.getParam(robot_description_property_->getStdString(), content))
     {
-      update_nh_.getParam(loc, content);
-    }
-    else
-    {
-      clear();
-      setStatus(rviz::StatusProperty::Error, "URDF",
-                "Parameter [" + robot_description_property_->getString() +
-                    "] does not exist, and was not found by searchParam()");
-      return;
+      std::string loc;
+      if (update_nh_.searchParam(robot_description_property_->getStdString(), loc))
+        update_nh_.getParam(loc, content);
+      else
+      {
+        clear();
+        setStatus(StatusProperty::Error, "URDF",
+                  QString("Parameter [%1] does not exist, and was not found by searchParam()")
+                      .arg(robot_description_property_->getString()));
+        // try again in a second
+        QTimer::singleShot(1000, this, SLOT(updateRobotDescription()));
+        return;
+      }
     }
   }
+  catch (const ros::InvalidNameException& e)
+  {
+    clear();
+    setStatus(StatusProperty::Error, "URDF",
+              QString("Invalid parameter name: %1.\n%2")
+                  .arg(robot_description_property_->getString(), e.what()));
+    return;
+  }
 
   if (content.empty())
   {

src/rviz/default_plugin/robot_model_display.cpp

@@ -140,24 +140,33 @@ void RobotModelDisplay::load()
   context_->queueRender();
 
   std::string content;
-  if (!update_nh_.getParam(robot_description_property_->getStdString(), content))
+  try
   {
-    std::string loc;
-    if (update_nh_.searchParam(robot_description_property_->getStdString(), loc))
+    if (!update_nh_.getParam(robot_description_property_->getStdString(), content))
     {
-      update_nh_.getParam(loc, content);
-    }
-    else
-    {
-      clear();
-      setStatus(StatusProperty::Error, "URDF",
-                "Parameter [" + robot_description_property_->getString() +
-                    "] does not exist, and was not found by searchParam()");
-      // try again in a second
-      QTimer::singleShot(1000, this, SLOT(updateRobotDescription()));
-      return;
+      std::string loc;
+      if (update_nh_.searchParam(robot_description_property_->getStdString(), loc))
+        update_nh_.getParam(loc, content);
+      else
+      {
+        clear();
+        setStatus(StatusProperty::Error, "URDF",
+                  QString("Parameter [%1] does not exist, and was not found by searchParam()")
+                      .arg(robot_description_property_->getString()));
+        // try again in a second
+        QTimer::singleShot(1000, this, SLOT(updateRobotDescription()));
+        return;
+      }
     }
   }
+  catch (const ros::InvalidNameException& e)
+  {
+    clear();
+    setStatus(StatusProperty::Error, "URDF",
+              QString("Invalid parameter name: %1.\n%2")
+                  .arg(robot_description_property_->getString(), e.what()));
+    return;
+  }
 
   if (content.empty())
   {

src/rviz/default_plugin/tf_display.cpp

@@ -179,11 +179,10 @@ TFDisplay::TFDisplay() : Display(), update_timer_(0.0f), changing_single_frame_e
   alpha_property_->setMin(0);
   alpha_property_->setMax(1);
 
-  update_rate_property_ =
-      new FloatProperty("Update Interval", 0,
-                        "The interval, in seconds, at which to update the frame transforms. "
-                        "0 means to do so every update cycle.",
-                        this);
+  update_rate_property_ = new FloatProperty("Update Interval", 0,
+                                            "The interval, in seconds, at which to update the frame "
+                                            "transforms. 0 means to do so every update cycle.",
+                                            this);
   update_rate_property_->setMin(0);
 
   frame_timeout_property_ = new FloatProperty(

src/rviz/ogre_helpers/arrow.h

@@ -37,7 +37,7 @@
 namespace Ogre
 {
 class Any;
-}
+} // namespace Ogre
 
 
 namespace rviz

src/rviz/ogre_helpers/axes.h

@@ -44,7 +44,7 @@
 namespace Ogre
 {
 class Any;
-}
+} // namespace Ogre
 
 namespace rviz
 {

src/rviz/ogre_helpers/line.h

@@ -40,7 +40,7 @@
 namespace Ogre
 {
 class Any;
-}
+} // namespace Ogre
 
 namespace rviz
 {

src/rviz/ogre_helpers/object.h

@@ -36,7 +36,7 @@
 namespace Ogre
 {
 class Any;
-}
+} // namespace Ogre
 
 namespace rviz
 {

src/rviz/ogre_helpers/point_cloud.cpp

@@ -86,8 +86,7 @@ static float g_box_vertices[6 * 6 * 3] = {
 
     // bottom
     -0.5, -0.5, -0.5, -0.5, -0.5, 0.5, 0.5, -0.5, -0.5, 0.5, -0.5, -0.5, -0.5, -0.5, 0.5, 0.5, -0.5, 0.5,
-    // clang-format on
-};
+}; // clang-format on
 
 Ogre::String PointCloud::sm_Type = "PointCloud";
 

src/rviz/properties/splitter_handle.cpp

@@ -32,6 +32,7 @@
 #include <QPaintEvent>
 #include <QPainter>
 #include <QTreeView>
+#include <QHeaderView>
 
 #include <rviz/properties/splitter_handle.h>
 
@@ -42,12 +43,14 @@ SplitterHandle::SplitterHandle(QTreeView* parent)
 {
   setCursor(Qt::SplitHCursor);
   updateGeometry();
+  parent_->header()->setStretchLastSection(false);
   parent_->installEventFilter(this);
 }
 
 bool SplitterHandle::eventFilter(QObject* event_target, QEvent* event)
 {
-  if (event_target == parent_ && (event->type() == QEvent::Resize || event->type() == QEvent::Paint))
+  if (event_target == parent_ &&
+      (event->type() == QEvent::Resize || event->type() == QEvent::LayoutRequest))
   {
     updateGeometry();
   }
@@ -57,13 +60,10 @@ bool SplitterHandle::eventFilter(QObject* event_target, QEvent* event)
 void SplitterHandle::updateGeometry()
 {
   int w = 7;
-  int content_width = parent_->contentsRect().width();
-  int new_column_width = int(first_column_size_ratio_ * content_width);
-  if (new_column_width != parent_->columnWidth(0))
-  {
-    parent_->setColumnWidth(0, new_column_width);
-    parent_->setColumnWidth(1, content_width - new_column_width);
-  }
+  int new_column_width = int(first_column_size_ratio_ * parent_->contentsRect().width());
+  parent_->setColumnWidth(0, new_column_width);
+  parent_->setColumnWidth(1, parent_->viewport()->contentsRect().width() - new_column_width);
+
   int new_x = new_column_width - w / 2 + parent_->columnViewportPosition(0);
   if (new_x != x() || parent_->height() != height())
     setGeometry(new_x, 0, w, parent_->height());

src/rviz/selection/selection_manager.cpp

@@ -589,14 +589,10 @@ void SelectionManager::unpackColors(Ogre::PixelBox& box, V_CollObject& pixels)
   {
     for (int x = 0; x < w; x++)
     {
-      if (size == 4) // In case of a 4-byte color format, we can directly process the 32-bit values
-      {
-        uint32_t pos = (x + y * w) * 4;
-        uint32_t pix_val = *(uint32_t*)((uint8_t*)box.data + pos);
-        pixels.push_back(colorToHandle(box.format, pix_val));
-      }
-      else // otherwise perform "official" transformation into float-based Ogre::ColourValue and back
-        pixels.push_back(colorToHandle(box.getColourAt(x, y, 1)));
+      uint32_t pos = (x + y * w) * size;
+      uint32_t pix_val = 0;
+      memcpy((uint8_t*)&pix_val, (uint8_t*)box.data + pos, size);
+      pixels.push_back(colorToHandle(box.format, pix_val));
     }
   }
 }

src/rviz/visualization_manager.h

@@ -405,9 +405,8 @@ protected Q_SLOTS:
   Ogre::Root* ogre_root_;             ///< Ogre Root
   Ogre::SceneManager* scene_manager_; ///< Ogre scene manager associated with this panel
 
-  QTimer* update_timer_; ///< Update timer.  Display::update is called on each display whenever this
-                         /// timer fires
-  ros::Time last_update_ros_time_; ///< Update stopwatch.  Stores how long it's been since the last update
+  QTimer* update_timer_; ///< Display::update is called on each display whenever this timer fires
+  ros::Time last_update_ros_time_; ///< Stores how long it's been since the last update
   ros::WallTime last_update_wall_time_;
 
   volatile bool shutting_down_;