indigo-devel backports from kinetic-devel (1.12.9) (#1110)

* Add fullscreen option. (#1017)

* urdfdom compatibility (#1064)

* Use urdf::*ShredPtr instead of boost::shared_ptr (#1044)

urdfdom_headers uses C++ std::shared_ptr. As it exports it as custom
*SharedPtr type, we can use the to sty compatible.

This also adds a proper dependency for urdfdom-headers

* adaptions to build against both urdfdom 0.3 and 0.4

... relying on the compatibility layer of urdf package

* Update display if empty pointcloud2 is published (#1073)

Do not show last point cloud any more, if published point cloud message does not contain any points

* Correctly scale the render panel on high resolution displays (#1078)

* support multiple material for one link (#1079)

* Fixed duplicate property name for Path colors (#1089)

See issue #1087.

* fix type error in newer versions of urdf (#1098)

* Use unique material names for robot links. (#1102)

* avoid C++11 feature for back port to indigo

CMakeLists.txt

@@ -17,6 +17,8 @@ find_package(Boost REQUIRED
   thread
 )
 
+find_package(urdfdom_headers REQUIRED)
+
 find_package(PkgConfig REQUIRED)
 
 find_package(ASSIMP QUIET)
@@ -129,6 +131,7 @@ find_package(catkin REQUIRED
   tf
   urdf
   visualization_msgs
+  urdfdom_headers
 )
 
 if(${tf_VERSION} VERSION_LESS "1.11.3")
@@ -210,6 +213,7 @@ include_directories(SYSTEM
   ${OGRE_OV_INCLUDE_DIRS}
   ${OPENGL_INCLUDE_DIR}
   ${PYTHON_INCLUDE_PATH}
+  ${urdfdom_headers_INCLUDE_DIRS}
 )
 include_directories(src ${catkin_INCLUDE_DIRS})
 

package.xml

@@ -49,6 +49,7 @@
   <build_depend>visualization_msgs</build_depend>
   <build_depend>yaml-cpp</build_depend>
   <build_depend>opengl</build_depend>
+  <build_depend>liburdfdom-headers-dev</build_depend>
 
   <run_depend>assimp</run_depend>
   <run_depend>eigen</run_depend>
@@ -81,6 +82,7 @@
   <run_depend>visualization_msgs</run_depend>
   <run_depend>yaml-cpp</run_depend>
   <run_depend>opengl</run_depend>
+  <run_depend>liburdfdom-headers-dev</run_depend>
 
   <export>
     <rviz plugin="${prefix}/plugin_description.xml"/>

src/rviz/default_plugin/effort_display.cpp

@@ -208,11 +208,11 @@ namespace rviz
 	    return;
 	}
         setStatus(rviz::StatusProperty::Ok, "URDF", "Robot model parserd Ok");
-	for (std::map<std::string, boost::shared_ptr<urdf::Joint> >::iterator it = robot_model_->joints_.begin(); it != robot_model_->joints_.end(); it ++ ) {
-            boost::shared_ptr<urdf::Joint> joint = it->second;
+    for (std::map<std::string, urdf::JointSharedPtr >::iterator it = robot_model_->joints_.begin(); it != robot_model_->joints_.end(); it ++ ) {
+        urdf::JointSharedPtr joint = it->second;
 	    if ( joint->type == urdf::Joint::REVOLUTE ) {
                 std::string joint_name = it->first;
-		boost::shared_ptr<urdf::JointLimits> limit = joint->limits;
+                urdf::JointLimitsSharedPtr limit = joint->limits;
                 joints_[joint_name] = createJoint(joint_name);
                 //joints_[joint_name]->max_effort_property_->setFloat(limit->effort);
                 //joints_[joint_name]->max_effort_property_->setReadOnly( true );

src/rviz/default_plugin/effort_visual.cpp

@@ -31,7 +31,7 @@ namespace rviz
 
 	// We create the arrow object within the frame node so that we can
 	// set its position and direction relative to its header frame.
-	for (std::map<std::string, boost::shared_ptr<urdf::Joint> >::iterator it = urdf_model_->joints_.begin(); it != urdf_model_->joints_.end(); it ++ ) {
+	for (std::map<std::string, urdf::JointSharedPtr >::iterator it = urdf_model_->joints_.begin(); it != urdf_model_->joints_.end(); it ++ ) {
 	    if ( it->second->type == urdf::Joint::REVOLUTE ) {
                 std::string joint_name = it->first;
                 effort_enabled_[joint_name] = true;
@@ -103,7 +103,7 @@ namespace rviz
                 if ( ! effort_enabled_[joint_name] ) continue;
 
 		//tf::Transform offset = poseFromJoint(joint);
-		boost::shared_ptr<urdf::JointLimits> limit = joint->limits;
+		urdf::JointLimitsSharedPtr limit = joint->limits;
 		double max_effort = limit->effort, effort_value = 0.05;
 
 		if ( max_effort != 0.0 )

src/rviz/default_plugin/path_display.cpp

@@ -96,7 +96,7 @@ PathDisplay::PathDisplay()
                                                         "Radius of the axes.",
                                                         this, SLOT(updatePoseAxisGeometry()) );
 
-  pose_arrow_color_property_ = new ColorProperty( "Color",
+  pose_arrow_color_property_ = new ColorProperty( "Pose Color",
                                                   QColor( 255, 85, 255 ),
                                                   "Color to draw the poses.",
                                                   this, SLOT(updatePoseArrowColor()));

src/rviz/default_plugin/point_cloud2_display.cpp

@@ -106,50 +106,51 @@ void PointCloud2Display::processMessage( const sensor_msgs::PointCloud2ConstPtr&
   }
 
   filtered->data.resize(cloud->data.size());
+  uint32_t output_count;
   if (point_count == 0)
   {
-    return;
-  }
-
-  uint8_t* output_ptr = &filtered->data.front();
-  const uint8_t* ptr = &cloud->data.front(), *ptr_end = &cloud->data.back(), *ptr_init;
-  size_t points_to_copy = 0;
-  for (; ptr < ptr_end; ptr += point_step)
-  {
-    float x = *reinterpret_cast<const float*>(ptr + xoff);
-    float y = *reinterpret_cast<const float*>(ptr + yoff);
-    float z = *reinterpret_cast<const float*>(ptr + zoff);
-    if (validateFloats(x) && validateFloats(y) && validateFloats(z))
+    output_count = 0;
+  } else {
+    uint8_t* output_ptr = &filtered->data.front();
+    const uint8_t* ptr = &cloud->data.front(), *ptr_end = &cloud->data.back(), *ptr_init;
+    size_t points_to_copy = 0;
+    for (; ptr < ptr_end; ptr += point_step)
     {
-      if (points_to_copy == 0)
+      float x = *reinterpret_cast<const float*>(ptr + xoff);
+      float y = *reinterpret_cast<const float*>(ptr + yoff);
+      float z = *reinterpret_cast<const float*>(ptr + zoff);
+      if (validateFloats(x) && validateFloats(y) && validateFloats(z))
       {
-        // Only memorize where to start copying from
-        ptr_init = ptr;
-        points_to_copy = 1;
+        if (points_to_copy == 0)
+        {
+          // Only memorize where to start copying from
+          ptr_init = ptr;
+          points_to_copy = 1;
+        }
+        else
+        {
+          ++points_to_copy;
+        }
       }
       else
       {
-        ++points_to_copy;
+        if (points_to_copy)
+        {
+          // Copy all the points that need to be copied
+          memcpy(output_ptr, ptr_init, point_step*points_to_copy);
+          output_ptr += point_step*points_to_copy;
+          points_to_copy = 0;
+        }
       }
     }
-    else
+    // Don't forget to flush what needs to be copied
+    if (points_to_copy)
     {
-      if (points_to_copy)
-      {
-        // Copy all the points that need to be copied
-        memcpy(output_ptr, ptr_init, point_step*points_to_copy);
-        output_ptr += point_step*points_to_copy;
-        points_to_copy = 0;
-      }
+      memcpy(output_ptr, ptr_init, point_step*points_to_copy);
+      output_ptr += point_step*points_to_copy;
     }
+    output_count = (output_ptr - &filtered->data.front()) / point_step;
   }
-  // Don't forget to flush what needs to be copied
-  if (points_to_copy)
-  {
-    memcpy(output_ptr, ptr_init, point_step*points_to_copy);
-    output_ptr += point_step*points_to_copy;
-  }
-  uint32_t output_count = (output_ptr - &filtered->data.front()) / point_step;
 
   filtered->header = cloud->header;
   filtered->fields = cloud->fields;

src/rviz/ogre_helpers/render_system.cpp

@@ -27,6 +27,8 @@
  * POSSIBILITY OF SUCH DAMAGE.
  */
 
+#include <sstream>
+
 #include <QMetaType>
 
 // This is required for QT_MAC_USE_COCOA to be set
@@ -342,7 +344,7 @@ int checkBadDrawable( Display* display, XErrorEvent* error )
 }
 #endif // Q_WS_X11
 
-Ogre::RenderWindow* RenderSystem::makeRenderWindow( intptr_t window_id, unsigned int width, unsigned int height )
+Ogre::RenderWindow* RenderSystem::makeRenderWindow( intptr_t window_id, unsigned int width, unsigned int height, double pixel_ratio )
 {
   static int windowCounter = 0; // Every RenderWindow needs a unique name, oy.
 
@@ -372,6 +374,11 @@ Ogre::RenderWindow* RenderSystem::makeRenderWindow( intptr_t window_id, unsigned
 #else
   params["macAPI"] = "carbon";
 #endif
+  {
+    std::stringstream ss;
+    ss << pixel_ratio;
+    params["contentScalingFactor"] = ss.str();
+  }
 
   std::ostringstream stream;
   stream << "OgreWindow(" << windowCounter++ << ")";

src/rviz/ogre_helpers/render_system.h

@@ -46,7 +46,7 @@ class RenderSystem
 public:
   static RenderSystem* get();
 
-  Ogre::RenderWindow* makeRenderWindow( intptr_t window_id, unsigned int width, unsigned int height );
+  Ogre::RenderWindow* makeRenderWindow( intptr_t window_id, unsigned int width, unsigned int height, double pixel_ratio = 1.0 );
 
   Ogre::Root* root() { return ogre_root_; }
 

src/rviz/ogre_helpers/render_widget.cpp

@@ -38,6 +38,9 @@
 #include <QPaintEvent>
 #include <QShowEvent>
 #include <QVBoxLayout>
+#if QT_VERSION >= QT_VERSION_CHECK(5, 0, 0)
+#include <QWindow>
+#endif
 
 namespace rviz
 {
@@ -83,8 +86,12 @@ RenderWidget::RenderWidget( RenderSystem* render_system, QWidget *parent )
 
 #if QT_VERSION < QT_VERSION_CHECK(5, 0, 0)
   QApplication::syncX();
+  double pixel_ratio = 1.0;
+#else
+  QWindow* window = windowHandle();
+  double pixel_ratio = window ? window->devicePixelRatio() : 1.0;
 #endif
-  render_window_ = render_system_->makeRenderWindow( win_id, width(), height() );
+  render_window_ = render_system_->makeRenderWindow( win_id, width(), height(), pixel_ratio );
 }
 
 RenderWidget::~RenderWidget()

src/rviz/panel_dock_widget.cpp

@@ -100,13 +100,13 @@ void PanelDockWidget::setCollapsed( bool collapse )
   {
     if ( isVisible() )
     {
-      QDockWidget::setVisible( false );
+      PanelDockWidget::setVisible( false );
       collapsed_ = collapse;
     }
   }
   else
   {
-    QDockWidget::setVisible( true );
+    PanelDockWidget::setVisible( true );
     collapsed_ = collapse;
   }
 }
@@ -144,4 +144,16 @@ void PanelDockWidget::load( Config config )
   config.mapGetBool( "collapsed", &collapsed_ );
 }
 
+void PanelDockWidget::setVisible( bool visible )
+{
+  requested_visibility_ = visible;
+  QDockWidget::setVisible(requested_visibility_ && !forced_hidden_);
+}
+
+void PanelDockWidget::overrideVisibility( bool hidden )
+{
+  forced_hidden_ = hidden;
+  setVisible(requested_visibility_);
+}
+
 } // end namespace rviz

src/rviz/panel_dock_widget.h

@@ -58,6 +58,9 @@ Q_OBJECT
   virtual void save( Config config );
   virtual void load( Config config );
 
+  /** @brief Override setVisible to respect the visibility override, */
+  virtual void setVisible( bool visible );
+
 protected:
 
   virtual void closeEvent ( QCloseEvent * event );
@@ -66,6 +69,9 @@ public Q_SLOTS:
 
   void setWindowTitle( QString title );
 
+  /** @ Override the visibility of the widget. **/
+  virtual void overrideVisibility( bool hide );
+
 private Q_SLOTS:
   void onChildDestroyed( QObject* );
 
@@ -76,6 +82,8 @@ private Q_SLOTS:
 private:
   // set to true if this panel was collapsed
   bool collapsed_;
+  bool requested_visibility_;
+  bool forced_hidden_;
   QLabel *icon_label_;
   QLabel *title_label_;
 };

src/rviz/robot/robot.cpp

@@ -236,7 +236,7 @@ void Robot::clear()
 
 RobotLink* Robot::LinkFactory::createLink(
     Robot* robot,
-    const boost::shared_ptr<const urdf::Link>& link,
+    const urdf::LinkConstSharedPtr& link,
     const std::string& parent_joint_name,
     bool visual,
     bool collision)
@@ -246,7 +246,7 @@ RobotLink* Robot::LinkFactory::createLink(
 
 RobotJoint* Robot::LinkFactory::createJoint(
     Robot* robot,
-    const boost::shared_ptr<const urdf::Joint>& joint)
+    const urdf::JointConstSharedPtr& joint)
 {
   return new RobotJoint(robot, joint);
 }
@@ -265,12 +265,12 @@ void Robot::load( const urdf::ModelInterface &urdf, bool visual, bool collision
   // Create properties for each link.
   // Properties are not added to display until changedLinkTreeStyle() is called (below).
   {
-    typedef std::map<std::string, boost::shared_ptr<urdf::Link> > M_NameToUrdfLink;
+    typedef std::map<std::string, urdf::LinkSharedPtr > M_NameToUrdfLink;
     M_NameToUrdfLink::const_iterator link_it = urdf.links_.begin();
     M_NameToUrdfLink::const_iterator link_end = urdf.links_.end();
     for( ; link_it != link_end; ++link_it )
     {
-      const boost::shared_ptr<const urdf::Link>& urdf_link = link_it->second;
+      const urdf::LinkConstSharedPtr& urdf_link = link_it->second;
       std::string parent_joint_name;
 
       if (urdf_link != urdf.getRoot() && urdf_link->parent_joint)
@@ -298,12 +298,12 @@ void Robot::load( const urdf::ModelInterface &urdf, bool visual, bool collision
   // Create properties for each joint.
   // Properties are not added to display until changedLinkTreeStyle() is called (below).
   {
-    typedef std::map<std::string, boost::shared_ptr<urdf::Joint> > M_NameToUrdfJoint;
+    typedef std::map<std::string, urdf::JointSharedPtr > M_NameToUrdfJoint;
     M_NameToUrdfJoint::const_iterator joint_it = urdf.joints_.begin();
     M_NameToUrdfJoint::const_iterator joint_end = urdf.joints_.end();
     for( ; joint_it != joint_end; ++joint_it )
     {
-      const boost::shared_ptr<const urdf::Joint>& urdf_joint = joint_it->second;
+      const urdf::JointConstSharedPtr& urdf_joint = joint_it->second;
       RobotJoint* joint = link_factory_->createJoint( this, urdf_joint );
 
       joints_[urdf_joint->name] = joint;

src/rviz/robot/robot.h

@@ -39,6 +39,8 @@
 #include <OgreQuaternion.h>
 #include <OgreAny.h>
 
+#include <urdf/model.h> // can be replaced later by urdf_model/types.h
+
 namespace Ogre
 {
 class SceneManager;
@@ -62,13 +64,6 @@ namespace tf
 class TransformListener;
 }
 
-namespace urdf
-{
-class ModelInterface;
-class Link;
-class Joint;
-}
-
 namespace rviz
 {
 
@@ -173,12 +168,12 @@ Q_OBJECT
   {
   public:
     virtual RobotLink* createLink( Robot* robot,
-                                   const boost::shared_ptr<const urdf::Link>& link,
+                                   const urdf::LinkConstSharedPtr& link,
                                    const std::string& parent_joint_name,
                                    bool visual,
                                    bool collision);
     virtual RobotJoint* createJoint( Robot* robot,
-                                     const boost::shared_ptr<const urdf::Joint>& joint);
+                                     const urdf::JointConstSharedPtr& joint);
   };
 
   /** Call this before load() to subclass the RobotLink or RobotJoint class used in the link property.

src/rviz/robot/robot_joint.cpp

@@ -38,15 +38,10 @@
 #include "rviz/ogre_helpers/axes.h"
 #include "rviz/load_resource.h"
 
-#include <urdf_model/model.h>
-#include <urdf_model/link.h>
-#include <urdf_model/joint.h>
-
-
 namespace rviz
 {
 
-RobotJoint::RobotJoint( Robot* robot, const boost::shared_ptr<const urdf::Joint>& joint )
+RobotJoint::RobotJoint( Robot* robot, const urdf::JointConstSharedPtr& joint )
   : robot_( robot )
   , name_( joint->name )
   , child_link_name_( joint->child_link_name )