Add Drake C++ systems to drake_ros_viz package.

Signed-off-by: Michel Hidalgo <[email protected]>

drake_ros_viz/CMakeLists.txt

@@ -12,15 +12,23 @@ endif()
 find_package(ament_cmake_ros REQUIRED)
 find_package(geometry_msgs REQUIRED)
 find_package(drake REQUIRED)
+find_package(drake_ros_core REQUIRED)
+find_package(drake_ros_tf2 REQUIRED)
+find_package(rclcpp REQUIRED)
 find_package(visualization_msgs REQUIRED)
 
 add_library(drake_ros_viz
+  src/rviz_visualizer.cpp
+  src/scene_markers_system.cpp
   src/utilities/name_conventions.cpp
   src/utilities/type_conversion.cpp
 )
 
 target_link_libraries(drake_ros_viz PUBLIC
     drake::drake
+    drake_ros_core::drake_ros_core
+    drake_ros_tf2::drake_ros_tf2
+    rclcpp::rclcpp
     ${geometry_msgs_TARGETS}
     ${visualization_msgs_TARGETS}
 )
@@ -37,7 +45,10 @@ ament_export_libraries(drake_ros_viz)
 ament_export_targets(${PROJECT_NAME})
 
 ament_export_dependencies(drake)
+ament_export_dependencies(drake_ros_core)
+ament_export_dependencies(drake_ros_tf2)
 ament_export_dependencies(geometry_msgs)
+ament_export_dependencies(rclcpp)
 ament_export_dependencies(visualization_msgs)
 
 install(TARGETS ${PROJECT_NAME} EXPORT ${PROJECT_NAME}
@@ -59,6 +70,14 @@ if(BUILD_TESTING)
   ament_cpplint()
 
   find_package(ament_cmake_gtest REQUIRED)
+  find_package(test_msgs REQUIRED)
+
+  ament_add_gtest(test_scene_markers test/test_scene_markers.cpp)
+  target_link_libraries(test_scene_markers
+    drake::drake
+    drake_ros_viz
+    ${visualization_msgs_TARGETS}
+  )
 
   ament_add_gtest(test_type_conversion test/test_type_conversion.cpp)
   target_link_libraries(test_type_conversion

drake_ros_viz/include/drake_ros_viz/rviz_visualizer.hpp

@@ -0,0 +1,74 @@
+// Copyright 2021 Open Source Robotics Foundation, Inc.
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+//     http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+// See the License for the specific language governing permissions and
+// limitations under the License.
+#ifndef DRAKE_ROS_VIZ__RVIZ_VISUALIZER_HPP_
+#define DRAKE_ROS_VIZ__RVIZ_VISUALIZER_HPP_
+
+#include <memory>
+#include <unordered_set>
+
+#include <drake/multibody/plant/multibody_plant.h>
+#include <drake/systems/framework/diagram.h>
+#include <drake_ros_core/drake_ros_interface.hpp>
+
+namespace drake_ros_viz {
+
+/// Set of parameters that configure an RvizVisualizer.
+struct RvizVisualizerParams {
+  /// Publish triggers for scene markers and tf broadcasting.
+  std::unordered_set<drake::systems::TriggerType> publish_triggers{
+      drake::systems::TriggerType::kForced,
+      drake::systems::TriggerType::kPeriodic};
+
+  /// Period for periodic scene markers and tf broadcasting.
+  double publish_period{0.05};
+
+  /// Whether to perform tf broadcasting or not.
+  bool publish_tf{true};
+};
+
+/// System for SceneGraph visualization in RViz.
+///
+/// This system is a subdiagram aggregating a SceneMarkersSystem, a
+/// RosPublisherSystem, and optionally a TfBroadcasterSystem to enable
+/// SceneGraph visualization in RViz. All scene geometries are published
+/// as a visualization_msgs/msg/MarkerArray message to a `/scene_markers`
+/// ROS topic. If `publish_tf` is `true`, all SceneGraph frames are
+/// broadcasted as tf2 transforms.
+///
+/// It exports one input port:
+/// - *graph_query* (abstract): expects a QueryObject from the SceneGraph.
+class RvizVisualizer : public drake::systems::Diagram<double> {
+ public:
+  explicit RvizVisualizer(
+      std::shared_ptr<drake_ros_core::DrakeRosInterface> ros_interface,
+      RvizVisualizerParams params = {});
+
+  ~RvizVisualizer() override;
+
+  // Forwarded to SceneTfSystem::RegisterMultibodyPlant
+  // and SceneTfBroadcasterSystem::RegisterMultibodyPlant
+  void RegisterMultibodyPlant(
+      const drake::multibody::MultibodyPlant<double>* plant);
+
+  const drake::systems::InputPort<double>& get_graph_query_port() const;
+
+ private:
+  class RvizVisualizerPrivate;
+
+  std::unique_ptr<RvizVisualizerPrivate> impl_;
+};
+
+}  // namespace drake_ros_viz
+
+#endif  // DRAKE_ROS_VIZ__RVIZ_VISUALIZER_HPP_

drake_ros_viz/include/drake_ros_viz/scene_markers_system.hpp

@@ -0,0 +1,118 @@
+// Copyright 2021 Open Source Robotics Foundation, Inc.
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+//     http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+// See the License for the specific language governing permissions and
+// limitations under the License.
+#ifndef DRAKE_ROS_VIZ__SCENE_MARKERS_SYSTEM_HPP_
+#define DRAKE_ROS_VIZ__SCENE_MARKERS_SYSTEM_HPP_
+
+#include <memory>
+#include <optional>  // NOLINT(build/include_order)
+#include <string>
+
+#include <drake/geometry/geometry_roles.h>
+#include <drake/geometry/rgba.h>
+#include <drake/multibody/plant/multibody_plant.h>
+#include <drake/systems/framework/leaf_system.h>
+#include <visualization_msgs/msg/marker_array.hpp>
+
+namespace drake_ros_viz {
+
+/// Set of parameters that configure a SceneMarkersSystem.
+struct SceneMarkersParams {
+  /// Configure SceneMarkersSystem to depict illustration geometries.
+  static SceneMarkersParams Illustration() { return {}; }
+
+  /// Configure SceneMarkersSystem to depict proximity geometries.
+  static SceneMarkersParams Proximity() {
+    SceneMarkersParams params;
+    params.role = drake::geometry::Role::kProximity;
+    params.default_color.set(0.5, 0.5, 0.5, 1.0);
+    return params;
+  }
+
+  /// Optional namespace for all markers, defaults
+  /// to utilities::GetMarkerNamespace() outcome.
+  std::optional<std::string> marker_namespace{std::nullopt};
+
+  /// Role of the geometries to depict.
+  drake::geometry::Role role{drake::geometry::Role::kIllustration};
+
+  /// Default marker color if no ("phong", "diffuse") property is found.
+  drake::geometry::Rgba default_color{0.9, 0.9, 0.9, 1.0};
+};
+
+/// System for SceneGraph depiction as a ROS markers array.
+///
+/// This system outputs a `visualization_msgs/msg/MarkerArray` populated with
+/// all geometries found in a SceneGraph, using Context time to timestamp
+/// each `visualization_msgs/msg/Marker` message.
+///
+/// It has one input port:
+/// - *graph_query* (abstract): expects a QueryObject from the SceneGraph.
+///
+/// It has one output port:
+/// - *scene_markers* (abstract): all scene geometries, as a
+///   visualization_msg::msg::MarkerArray message.
+///
+/// This system provides the same base functionality in terms of SceneGraph
+/// geometries lookup and message conversion for ROS-based applications as
+/// the DrakeVisualizer system does for LCM-based applications.
+/// Thus, discussions in DrakeVisualizer's documentation about geometry
+/// versioning, geometry roles, and so on are equally applicable here.
+class SceneMarkersSystem : public drake::systems::LeafSystem<double> {
+ public:
+  explicit SceneMarkersSystem(SceneMarkersParams params = {});
+  virtual ~SceneMarkersSystem();
+
+  /// Register a MultibodyPlant present in the scene
+  /**
+   * This provides the system with additional information to generate
+   * semantically meaningful frame string IDs and marker namespaces.
+   */
+  void RegisterMultibodyPlant(
+      const drake::multibody::MultibodyPlant<double>* plant);
+
+  const SceneMarkersParams& params() const;
+
+  const drake::systems::InputPort<double>& get_graph_query_port() const;
+
+  const drake::systems::OutputPort<double>& get_markers_output_port() const;
+
+ private:
+  // Outputs visualization_msgs::msg::MarkerArray message,
+  // timestamping the most up-to-date version.
+  void PopulateSceneMarkersMessage(
+      const drake::systems::Context<double>& context,
+      visualization_msgs::msg::MarkerArray* output_value) const;
+
+  // Returns cached visualization_msgs::msg::MarkerArray message,
+  // which is invalidated (and thus recomputed) upon a SceneGraph
+  // geometry version change.
+  const visualization_msgs::msg::MarkerArray& EvalSceneMarkers(
+      const drake::systems::Context<double>& context,
+      bool* cached = nullptr) const;
+
+  // Inspects the SceneGraph and carries out the conversion
+  // to visualization_msgs::msg::MarkerArray message unconditionally.
+  void CalcSceneMarkers(
+      const drake::systems::Context<double>& context,
+      visualization_msgs::msg::MarkerArray* output_value) const;
+
+  // PIMPL forward declaration
+  class SceneMarkersSystemPrivate;
+
+  std::unique_ptr<SceneMarkersSystemPrivate> impl_;
+};
+
+}  // namespace drake_ros_viz
+
+#endif  // DRAKE_ROS_VIZ__SCENE_MARKERS_SYSTEM_HPP_

drake_ros_viz/package.xml

@@ -10,12 +10,16 @@
 
   <buildtool_depend>ament_cmake_ros</buildtool_depend>
 
+  <depend>drake_ros_core</depend>
+  <depend>drake_ros_tf2</depend>
   <depend>geometry_msgs</depend>
+  <depend>rclcpp</depend>
   <depend>visualization_msgs</depend>
 
   <test_depend>ament_cmake_clang_format</test_depend>
   <test_depend>ament_cmake_cpplint</test_depend>
   <test_depend>ament_cmake_gtest</test_depend>
+  <test_depend>test_msgs</test_depend>
 
   <export>
     <build_type>ament_cmake</build_type>

drake_ros_viz/src/rviz_visualizer.cpp

@@ -0,0 +1,101 @@
+// Copyright 2021 Open Source Robotics Foundation, Inc.
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+//     http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+// See the License for the specific language governing permissions and
+// limitations under the License.
+
+#include "drake_ros_viz/rviz_visualizer.hpp"
+
+#include <memory>
+#include <unordered_set>
+#include <utility>
+
+#include "drake_ros_viz/scene_markers_system.hpp"
+#include <drake/systems/framework/diagram_builder.h>
+#include <drake_ros_core/ros_publisher_system.hpp>
+#include <drake_ros_tf2/scene_tf_broadcaster_system.hpp>
+#include <rclcpp/qos.hpp>
+#include <visualization_msgs/msg/marker_array.hpp>
+
+namespace drake_ros_viz {
+
+class RvizVisualizer::RvizVisualizerPrivate {
+ public:
+  SceneMarkersSystem* scene_visual_markers;
+  SceneMarkersSystem* scene_collision_markers;
+  drake_ros_tf2::SceneTfBroadcasterSystem* scene_tf_broadcaster{nullptr};
+};
+
+RvizVisualizer::RvizVisualizer(
+    std::shared_ptr<drake_ros_core::DrakeRosInterface> ros_interface,
+    RvizVisualizerParams params)
+    : impl_(new RvizVisualizerPrivate()) {
+  drake::systems::DiagramBuilder<double> builder;
+
+  using drake_ros_core::RosPublisherSystem;
+  auto scene_visual_markers_publisher = builder.AddSystem(
+      RosPublisherSystem::Make<visualization_msgs::msg::MarkerArray>(
+          "/scene_markers/visual", rclcpp::QoS(1), ros_interface,
+          params.publish_triggers, params.publish_period));
+
+  impl_->scene_visual_markers =
+      builder.AddSystem<SceneMarkersSystem>(SceneMarkersParams::Illustration());
+
+  builder.Connect(impl_->scene_visual_markers->get_markers_output_port(),
+                  scene_visual_markers_publisher->get_input_port());
+
+  builder.ExportInput(impl_->scene_visual_markers->get_graph_query_port(),
+                      "graph_query");
+
+  auto scene_collision_markers_publisher = builder.AddSystem(
+      RosPublisherSystem::Make<visualization_msgs::msg::MarkerArray>(
+          "/scene_markers/collision", rclcpp::QoS(1), ros_interface,
+          params.publish_triggers, params.publish_period));
+
+  impl_->scene_collision_markers =
+      builder.AddSystem<SceneMarkersSystem>(SceneMarkersParams::Proximity());
+
+  builder.Connect(impl_->scene_collision_markers->get_markers_output_port(),
+                  scene_collision_markers_publisher->get_input_port());
+
+  builder.ConnectInput("graph_query",
+                       impl_->scene_collision_markers->get_graph_query_port());
+
+  if (params.publish_tf) {
+    impl_->scene_tf_broadcaster =
+        builder.AddSystem<drake_ros_tf2::SceneTfBroadcasterSystem>(
+            ros_interface, drake_ros_tf2::SceneTfBroadcasterParams{
+                               params.publish_triggers, params.publish_period});
+
+    builder.ConnectInput("graph_query",
+                         impl_->scene_tf_broadcaster->get_graph_query_port());
+  }
+
+  builder.BuildInto(this);
+}
+
+RvizVisualizer::~RvizVisualizer() {}
+
+void RvizVisualizer::RegisterMultibodyPlant(
+    const drake::multibody::MultibodyPlant<double>* plant) {
+  impl_->scene_visual_markers->RegisterMultibodyPlant(plant);
+  impl_->scene_collision_markers->RegisterMultibodyPlant(plant);
+  if (impl_->scene_tf_broadcaster) {
+    impl_->scene_tf_broadcaster->RegisterMultibodyPlant(plant);
+  }
+}
+
+const drake::systems::InputPort<double>& RvizVisualizer::get_graph_query_port()
+    const {
+  return get_input_port();
+}
+
+}  // namespace drake_ros_viz