Implement reset time service (#1109)

ros2 · Dec 26, 2023 · a4f4de9 · a4f4de9
1 parent bb2ab95
commit a4f4de9
Show file tree

Hide file tree

Showing 4 changed files with 44 additions and 5 deletions.
diff --git a/rviz_common/CMakeLists.txt b/rviz_common/CMakeLists.txt
@@ -40,6 +40,7 @@ find_package(resource_retriever REQUIRED)
 find_package(rviz_rendering REQUIRED)
 find_package(sensor_msgs REQUIRED)
 find_package(std_msgs REQUIRED)
+find_package(std_srvs REQUIRED)
 find_package(tinyxml2_vendor REQUIRED)
 find_package(tf2 REQUIRED)
 find_package(tf2_ros REQUIRED)
@@ -243,6 +244,7 @@ target_link_libraries(rviz_common PUBLIC
   rviz_rendering::rviz_rendering
   ${sensor_msgs_TARGETS}
   ${std_msgs_TARGETS}
+  ${std_srvs_TARGETS}
   tf2::tf2
   tf2_ros::tf2_ros
   yaml-cpp
@@ -267,6 +269,7 @@ ament_export_dependencies(
   rviz_rendering
   sensor_msgs
   std_msgs
+  std_srvs
   tf2
   tf2_ros
   yaml_cpp_vendor

diff --git a/rviz_common/include/rviz_common/view_controller.hpp b/rviz_common/include/rviz_common/view_controller.hpp
@@ -32,6 +32,7 @@
 #define RVIZ_COMMON__VIEW_CONTROLLER_HPP_
 
 #include <string>
+#include <memory>
 
 #include <OgreVector.h>
 
@@ -40,6 +41,8 @@
 #include <QString>  // NOLINT: cpplint is unable to handle the include order here
 #include <Qt>  // NOLINT: cpplint is unable to handle the include order here
 #include <QVariant>  // NOLINT: cpplint is unable to handle the include order here
+#include <rclcpp/service.hpp>
+#include <std_srvs/srv/empty.hpp>
 
 #include "rviz_common/properties/property.hpp"
 #include "rviz_common/visibility_control.hpp"
@@ -211,6 +214,8 @@ class RVIZ_COMMON_PUBLIC ViewController : public properties::Property
 
   virtual FocalPointStatus getFocalPointStatus();
 
+  void resetTime();
+
 Q_SIGNALS:
   void configChanged();
 
@@ -279,6 +284,13 @@ private Q_SLOTS:
 
   // Default cursors for the most common actions
   QMap<CursorType, QCursor> standard_cursors_;
+
+  rclcpp::Service<std_srvs::srv::Empty>::SharedPtr reset_time_srv_;
+
+  void resetService(
+    const std::shared_ptr<rmw_request_id_t>,
+    const std::shared_ptr<std_srvs::srv::Empty::Request>,
+    const std::shared_ptr<std_srvs::srv::Empty::Response>);
 };
 
 }  // namespace rviz_common

diff --git a/rviz_common/package.xml b/rviz_common/package.xml
@@ -45,6 +45,7 @@
   <depend>rviz_rendering</depend>
   <depend>sensor_msgs</depend>
   <depend>std_msgs</depend>
+  <depend>std_srvs</depend>
   <depend>tf2</depend>
   <depend>tf2_ros</depend>
   <depend>message_filters</depend>

diff --git a/rviz_common/src/rviz_common/view_controller.cpp b/rviz_common/src/rviz_common/view_controller.cpp
@@ -126,6 +126,16 @@ void ViewController::initialize(DisplayContext * context)
   stereo_enable_->setBool(false);
   stereo_enable_->hide();
   // }
+
+  auto ros_node_abstraction = context_->getRosNodeAbstraction().lock();
+  if (ros_node_abstraction) {
+    auto node = ros_node_abstraction->get_raw_node();
+    reset_time_srv_ = node->create_service<std_srvs::srv::Empty>(
+      ros_node_abstraction->get_node_name() + "/reset_time",
+      std::bind(
+        &ViewController::resetService, this,
+        std::placeholders::_1, std::placeholders::_2, std::placeholders::_3));
+  }
 }
 
 ViewController::~ViewController()
@@ -235,12 +245,25 @@ void ViewController::handleKeyEvent(QKeyEvent * event, RenderPanel * panel)
   }
 
   if (event->key() == Qt::Key_R) {
-    rviz_common::VisualizationManager * vis_manager =
-      dynamic_cast<rviz_common::VisualizationManager *>(context_);
+    resetTime();
+  }
+}
 
-    if (vis_manager != nullptr) {
-      vis_manager->resetTime();
-    }
+void ViewController::resetService(
+  const std::shared_ptr<rmw_request_id_t>,
+  const std::shared_ptr<std_srvs::srv::Empty::Request>,
+  const std::shared_ptr<std_srvs::srv::Empty::Response>)
+{
+  resetTime();
+}
+
+void ViewController::resetTime()
+{
+  rviz_common::VisualizationManager * vis_manager =
+    dynamic_cast<rviz_common::VisualizationManager *>(context_);
+
+  if (vis_manager != nullptr) {
+    vis_manager->resetTime();
   }
 }