Migrate LaserScanDisplay

rviz_default_plugins/CMakeLists.txt

@@ -25,6 +25,7 @@ find_package(rviz_rendering REQUIRED)
 find_package(Qt5 REQUIRED COMPONENTS Widgets)
 
 find_package(geometry_msgs REQUIRED)
+find_package(laser_geometry REQUIRED)
 find_package(nav_msgs REQUIRED)
 find_package(rclcpp REQUIRED)
 # find_package(tf2 REQUIRED)
@@ -38,6 +39,7 @@ set(rviz_default_plugins_headers_to_moc
   src/rviz_default_plugins/displays/camera/camera_display.hpp
   src/rviz_default_plugins/displays/grid/grid_display.hpp
   src/rviz_default_plugins/displays/image/image_display.hpp
+  src/rviz_default_plugins/displays/laser_scan/laser_scan_display.hpp
   src/rviz_default_plugins/displays/marker/marker_display.hpp
   src/rviz_default_plugins/displays/path/path_display.hpp
   src/rviz_default_plugins/displays/pointcloud/point_cloud_common.hpp
@@ -62,6 +64,7 @@ set(rviz_default_plugins_source_files
   src/rviz_default_plugins/displays/grid/grid_display.cpp
   src/rviz_default_plugins/displays/image/image_display.cpp
   src/rviz_default_plugins/displays/image/ros_image_texture.cpp
+  src/rviz_default_plugins/displays/laser_scan/laser_scan_display.cpp
   src/rviz_default_plugins/displays/marker/markers/arrow_marker.cpp
   src/rviz_default_plugins/displays/marker/markers/line_list_marker.cpp
   src/rviz_default_plugins/displays/marker/markers/line_marker_base.cpp
@@ -114,12 +117,14 @@ target_include_directories(rviz_default_plugins
     $<BUILD_INTERFACE:${CMAKE_CURRENT_SOURCE_DIR}/include>
 )
 target_link_libraries(rviz_default_plugins
-  rviz_rendering::rviz_rendering
-  rviz_common::rviz_common
+  laser_geometry::laser_geometry
   resource_retriever::resource_retriever
+  rviz_common::rviz_common
+  rviz_rendering::rviz_rendering
 )
 ament_target_dependencies(rviz_default_plugins
   geometry_msgs
+  laser_geometry
   nav_msgs
   rclcpp
   resource_retriever

rviz_default_plugins/package.xml

@@ -25,6 +25,7 @@
   <exec_depend>libqt5-widgets</exec_depend>
 
   <depend>geometry_msgs</depend>
+  <depend>laser_geometry</depend>
   <depend>nav_msgs</depend>
   <depend>pluginlib</depend>
   <depend>rclcpp</depend>

rviz_default_plugins/plugins_description.xml

@@ -35,6 +35,17 @@
     <message_type>sensor_msgs/Image</message_type>
   </class>
 
+  <class 
+    name="rviz/LaserScan"
+    type="rviz_default_plugins::displays::LaserScanDisplay"
+    base_class_type="rviz_common::Display"
+  >
+    <description>
+      Displays the data from a sensor_msgs::LaserScan message as points in the world, drawn as points, billboards, or cubes.  &lt;a href="http://www.ros.org/wiki/rviz/DisplayTypes/LaserScan"&gt;More Information&lt;/a&gt;.
+    </description>
+    <message_type>sensor_msgs/LaserScan</message_type>
+  </class>
+
   <class
     name="rviz/PointCloud"
     type="rviz_default_plugins::displays::PointCloudDisplay"

rviz_default_plugins/src/rviz_default_plugins/displays/laser_scan/laser_scan_display.cpp

@@ -0,0 +1,105 @@
+/*
+ * Copyright (c) 2008, Willow Garage, Inc.
+ * All rights reserved.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions are met:
+ *
+ *     * Redistributions of source code must retain the above copyright
+ *       notice, this list of conditions and the following disclaimer.
+ *     * Redistributions in binary form must reproduce the above copyright
+ *       notice, this list of conditions and the following disclaimer in the
+ *       documentation and/or other materials provided with the distribution.
+ *     * Neither the name of the Willow Garage, Inc. nor the names of its
+ *       contributors may be used to endorse or promote products derived from
+ *       this software without specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
+ * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
+ * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
+ * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE
+ * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
+ * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
+ * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
+ * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
+ * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
+ * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.
+ */
+
+#include "laser_scan_display.hpp"
+
+#include <memory>
+#include <string>
+
+#include "laser_geometry/laser_geometry.hpp"
+#include "tf2_ros/buffer.h"
+
+#include "src/rviz_default_plugins/displays/pointcloud/point_cloud_common.hpp"
+#include "rviz_common/properties/int_property.hpp"
+#include "rviz_common/properties/queue_size_property.hpp"
+#include "rviz_common/validate_floats.hpp"
+
+namespace rviz_default_plugins
+{
+namespace displays
+{
+
+LaserScanDisplay::LaserScanDisplay()
+: point_cloud_common_(std::make_unique<rviz_default_plugins::PointCloudCommon>(this)),
+  queue_size_property_(std::make_unique<rviz_common::QueueSizeProperty>(this, 10)),
+  projector_(std::make_unique<laser_geometry::LaserProjection>())
+{}
+
+void LaserScanDisplay::onInitialize()
+{
+  RTDClass::onInitialize();
+  point_cloud_common_->initialize(context_, scene_node_);
+}
+
+void LaserScanDisplay::processMessage(sensor_msgs::msg::LaserScan::ConstSharedPtr scan)
+{
+  // TODO(Martin-Idel-SI): Reenable once tf_filter is ported or delete if necessary
+//  Compute tolerance necessary for this scan
+//  ros::Duration tolerance(scan->time_increment * scan->ranges.size());
+//  if (tolerance > filter_tolerance_)
+//  {
+//    filter_tolerance_ = tolerance;
+//    tf_filter_->setTolerance(filter_tolerance_);
+//  }
+
+  auto cloud = std::make_shared<sensor_msgs::msg::PointCloud2>();
+
+  try {
+    auto buffer = context_->getFrameManager()->getTFBufferPtr();
+    projector_->transformLaserScanToPointCloud(
+      fixed_frame_.toStdString(),
+      *scan,
+      *cloud,
+      *buffer,
+      laser_geometry::channel_option::Intensity);
+  } catch (tf2::TransformException & e) {
+    RVIZ_COMMON_LOG_DEBUG_STREAM(
+      "LaserScan [" << qPrintable(getName()) << "]: failed to transform scan:" << e.what() << ".");
+    return;
+  }
+
+  point_cloud_common_->addMessage(cloud);
+}
+
+void LaserScanDisplay::update(float wall_dt, float ros_dt)
+{
+  point_cloud_common_->update(wall_dt, ros_dt);
+}
+
+void LaserScanDisplay::reset()
+{
+  RTDClass::reset();
+  point_cloud_common_->reset();
+}
+
+}  // namespace displays
+}  // namespace rviz_default_plugins
+
+#include <pluginlib/class_list_macros.hpp>  // NOLINT
+PLUGINLIB_EXPORT_CLASS(rviz_default_plugins::displays::LaserScanDisplay, rviz_common::Display)

rviz/src/rviz/default_plugin/laser_scan_display.h → rviz_default_plugins/src/rviz_default_plugins/displays/laser_scan/laser_scan_display.hpp

@@ -27,54 +27,59 @@
  * POSSIBILITY OF SUCH DAMAGE.
  */
 
-#ifndef RVIZ_LASER_SCAN_DISPLAY_H
-#define RVIZ_LASER_SCAN_DISPLAY_H
+#ifndef RVIZ_DEFAULT_PLUGINS__DISPLAYS__LASER_SCAN__LASER_SCAN_DISPLAY_HPP_
+#define RVIZ_DEFAULT_PLUGINS__DISPLAYS__LASER_SCAN__LASER_SCAN_DISPLAY_HPP_
 
-#include <sensor_msgs/LaserScan.h>
+#include <memory>
 
-#include "rviz/message_filter_display.h"
+#include "sensor_msgs/msg/laser_scan.hpp"
+
+#include "rviz_common/ros_topic_display.hpp"
 
 namespace laser_geometry
 {
 class LaserProjection;
-}
+}  // namespace laser_geometry
 
-namespace rviz
+namespace rviz_common
+{
+class QueueSizeProperty;
+namespace properties
 {
-
 class IntProperty;
+}  // namespace properties
+}  // namespace rviz_common
+namespace rviz_default_plugins
+{
 class PointCloudCommon;
+namespace displays
+{
 
 /** @brief Visualizes a laser scan, received as a sensor_msgs::LaserScan. */
-class LaserScanDisplay: public MessageFilterDisplay<sensor_msgs::LaserScan>
+// TODO(botteroa-si): This display originally extended the MessageFilterDisplay. Revisit when
+// available
+class LaserScanDisplay : public rviz_common::RosTopicDisplay<sensor_msgs::msg::LaserScan>
 {
-Q_OBJECT
 public:
   LaserScanDisplay();
-  ~LaserScanDisplay();
+  ~LaserScanDisplay() override = default;
 
-  virtual void reset();
-
-  virtual void update( float wall_dt, float ros_dt );
-
-private Q_SLOTS:
-  void updateQueueSize();
+  void reset() override;
+  void update(float wall_dt, float ros_dt) override;
 
 protected:
   /** @brief Do initialization. Overridden from MessageFilterDisplay. */
-  virtual void onInitialize();
+  void onInitialize() override;
 
   /** @brief Process a single message.  Overridden from MessageFilterDisplay. */
-  virtual void processMessage( const sensor_msgs::LaserScanConstPtr& scan );
-
-  IntProperty* queue_size_property_;
-
-  PointCloudCommon* point_cloud_common_;
+  void processMessage(sensor_msgs::msg::LaserScan::ConstSharedPtr scan) override;
 
-  laser_geometry::LaserProjection* projector_;
-  ros::Duration filter_tolerance_;
+  std::unique_ptr<PointCloudCommon> point_cloud_common_;
+  std::unique_ptr<rviz_common::QueueSizeProperty> queue_size_property_;
+  std::unique_ptr<laser_geometry::LaserProjection> projector_;
 };
 
-} // namespace rviz
+}  // namespace displays
+}  // namespace rviz_default_plugins
 
-#endif
+#endif  // RVIZ_DEFAULT_PLUGINS__DISPLAYS__LASER_SCAN__LASER_SCAN_DISPLAY_HPP_