[DO NOT MERGE] Add generic systems for Drake ROS integration

drake_ros_examples/CMakeLists.txt

@@ -0,0 +1,24 @@
+cmake_minimum_required(VERSION 3.10)
+project(drake_ros_examples)
+
+# Default to C++14
+if(NOT CMAKE_CXX_STANDARD)
+  set(CMAKE_CXX_STANDARD 14)
+endif()
+
+if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang")
+  add_compile_options(-Wall -Wextra -Wpedantic -Werror)
+endif()
+
+find_package(ament_cmake_ros REQUIRED)
+find_package(drake REQUIRED)
+find_package(drake_ros_systems REQUIRED)
+
+add_subdirectory(examples)
+
+if(BUILD_TESTING)
+  find_package(ament_lint_auto REQUIRED)
+  ament_lint_auto_find_test_dependencies()
+endif()
+
+ament_package()

drake_ros_examples/README.md

@@ -0,0 +1,65 @@
+# Drake ROS Examples
+
+This is a collection of examples built around `drake_ros_systems` C++ and Python APIs.
+
+## Building
+
+This package has been built and tested on Ubuntu Focal with ROS Rolling, using a Drake nightly from April 2021.
+It may work on other versions of ROS and Drake, but it hasn't been tested.
+
+To build it:
+
+1. [Install ROS Rolling](https://index.ros.org/doc/ros2/Installation/Rolling/)
+1. Source your ROS installation `. /opt/ros/rolling/setup.bash`
+1. [Download April-ish 2021 Drake binary](https://drake.mit.edu/from_binary.html)
+1. Extract the Drake binary installation, install it's prerequisites, and [use this Python virutalenv trick](https://drake.mit.edu/python_bindings.html#inside-virtualenv).
+1. Activate the drake virtual environment
+1. Build it using Colcon, or using CMake directly
+
+    **Colcon**
+    1. Make a workspace `mkdir -p ./ws/src`
+    1. `cd ./ws/src`
+    1. Get this code `git clone https://github.com/RobotLocomotion/drake-ros.git`
+    1. `cd ..`
+    1. Build this package and its dependencies `colcon build --packages-up-to drake_ros_systems`
+
+    **CMake**
+    1. Get this code `git clone https://github.com/RobotLocomotion/drake-ros.git`
+    1. Build the [`drake_ros_systems`](../drake_ros_systems/README.md#building) package using CMake first.
+    1. Manually set `CMAKE_PREFIX_PATH`: `export CMAKE_PREFIX_PATH=$CMAKE_PREFIX_PATH:$(pwd)/drake-ros/drake_ros_systems/install`
+    1. `cd drake-ros/drake_ros_examples`
+    1. Make a build and install folder to avoid installing to the whole system `mkdir build install`
+    1. `cd build`
+    1. Configure the project `cmake -DCMAKE_INSTALL_PREFIX=$(pwd)/../install ..`
+    1. Build the project `make && make install`
+
+## Running
+
+* If you built with `colcon`, then source your workspace.
+
+```
+. ./ws/install/setup.bash
+# Also make sure to activate drake virtual environment
+```
+
+  Now you can run C++ and Python examples using `ros2 run drake_ros_examples <example-executable-or-script>`.
+
+
+* If you built with plain CMake, then source the ROS workspace and set these variables.
+
+```
+. /opt/ros/rolling/setup.bash
+# Also make sure to activate drake virtual environment
+# CD to repository root
+export LD_LIBRARY_PATH=$(pwd)/drake_ros_systems/install/lib:$LD_LIBRARY_PATH
+export LD_LIBRARY_PATH=$(pwd)/drake_ros_examples/install/lib:$LD_LIBRARY_PATH
+export PYTHONPATH=$(pwd)/drake_ros_systems/install/lib/$(python -c 'import sys; print(f"python{sys.version_info[0]}.{sys.version_info[1]}")')/site-packages:$PYTHONPATH
+export PYTHONPATH=$(pwd)/drake_ros_examples/install/lib/$(python -c 'import sys; print(f"python{sys.version_info[0]}.{sys.version_info[1]}")')/site-packages:$PYTHONPATH
+```
+
+  Now you can run C++ and Python examples from the install folder using `./drake-ros/drake_ros_examples/install/lib/drake_ros_examples/<example-executable-or-script>`.
+
+## List of examples
+
+- [RS flip flop](./examples/rs_flip_flop): a latch with a ROS 2 topic interface.
+- [IIWA manipulator](./examples/iiwa_manipulator): an RViz visualization of a static IIWA arm.

drake_ros_examples/examples/CMakeLists.txt

@@ -0,0 +1,2 @@
+add_subdirectory(iiwa_manipulator)
+add_subdirectory(rs_flip_flop)

drake_ros_examples/examples/iiwa_manipulator/CMakeLists.txt

@@ -0,0 +1,16 @@
+add_executable(iiwa_manipulator iiwa_manipulator.cpp)
+target_link_libraries(iiwa_manipulator
+  drake::drake
+  drake_ros_systems::drake_ros_systems
+)
+
+install(
+  PROGRAMS iiwa_manipulator.py
+  DESTINATION lib/${PROJECT_NAME}
+)
+
+install(
+  TARGETS
+    iiwa_manipulator
+  DESTINATION lib/${PROJECT_NAME}
+)

drake_ros_examples/examples/iiwa_manipulator/README.md

@@ -0,0 +1,21 @@
+# IIWA Manipulator
+
+## Overview
+
+Both `iiwa_manipulator` and `iiwa_manipulator.py` enable RViz visualization of a static [`ManipulationStation`](https://github.com/RobotLocomotion/drake/tree/master/examples/manipulation_station) example.
+They publish the following topics:
+
+* `/tf` (all scene frames)
+* `/scene_markers` (all scene geometries, including the robot model)
+
+## How To
+
+Run either the C++ `iiwa_manipulator` executable or the Python `iiwa_manipulator.py` script as explained [here](../../README.md#running).
+
+Run RViz in a different terminal with your ROS installation sourced to visualize the station:
+
+```
+ros2 run rviz2 rviz2 -d iiwa_manipulator.rviz
+```
+
+

drake_ros_examples/examples/iiwa_manipulator/iiwa_manipulator.cpp

@@ -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.
+
+#include <drake/common/eigen_types.h>
+#include <drake/systems/analysis/simulator.h>
+#include <drake/systems/framework/diagram_builder.h>
+#include <drake/systems/primitives/adder.h>
+#include <drake/systems/primitives/constant_vector_source.h>
+#include <drake/systems/primitives/sine.h>
+
+
+#include <drake/examples/manipulation_station/manipulation_station.h>
+
+#include <drake_ros_systems/drake_ros.hpp>
+#include <drake_ros_systems/ros_interface_system.hpp>
+#include <drake_ros_systems/rviz_visualizer.hpp>
+
+#include <cmath>
+#include <memory>
+#include <utility>
+
+using drake_ros_systems::DrakeRos;
+using drake_ros_systems::RosInterfaceSystem;
+using drake_ros_systems::RvizVisualizer;
+
+using drake::systems::Adder;
+using drake::systems::ConstantVectorSource;
+using drake::systems::Sine;
+using drake::systems::Simulator;
+using drake::examples::manipulation_station::ManipulationStation;
+
+
+int main()
+{
+  drake::systems::DiagramBuilder<double> builder;
+
+  auto ros_interface_system =
+    builder.AddSystem<RosInterfaceSystem>(std::make_unique<DrakeRos>());
+
+  auto manipulation_station = builder.AddSystem<ManipulationStation>();
+  manipulation_station->SetupClutterClearingStation();
+  manipulation_station->Finalize();
+
+  // Make the base joint swing sinusoidally.
+  auto constant_term = builder.AddSystem<ConstantVectorSource>(
+    drake::VectorX<double>::Zero(manipulation_station->num_iiwa_joints()));
+
+  drake::VectorX<double> amplitudes =
+    drake::VectorX<double>::Zero(manipulation_station->num_iiwa_joints());
+  amplitudes[0] = M_PI / 4.;  // == 45 degrees
+  const drake::VectorX<double> frequencies =
+    drake::VectorX<double>::Constant(manipulation_station->num_iiwa_joints(), 1.);  // Hz
+  const drake::VectorX<double> phases =
+    drake::VectorX<double>::Zero(manipulation_station->num_iiwa_joints());
+  auto variable_term = builder.AddSystem<Sine>(amplitudes, frequencies, phases);
+
+  auto joint_trajectory_generator =
+    builder.AddSystem<Adder>(2, manipulation_station->num_iiwa_joints());
+
+  builder.Connect(
+    constant_term->get_output_port(),
+    joint_trajectory_generator->get_input_port(0));
+  builder.Connect(
+    variable_term->get_output_port(0),
+    joint_trajectory_generator->get_input_port(1));
+  builder.Connect(
+    joint_trajectory_generator->get_output_port(),
+    manipulation_station->GetInputPort("iiwa_position"));
+
+  auto rviz_visualizer = builder.AddSystem<RvizVisualizer>(
+    ros_interface_system->get_ros_interface());
+
+  builder.Connect(
+    manipulation_station->GetOutputPort("query_object"),
+    rviz_visualizer->get_graph_query_port());
+
+  auto diagram = builder.Build();
+  auto context = diagram->CreateDefaultContext();
+
+  auto simulator = std::make_unique<Simulator<double>>(*diagram, std::move(context));
+  simulator->set_target_realtime_rate(1.0);
+  simulator->Initialize();
+
+  auto & simulator_context = simulator->get_mutable_context();
+
+  auto & manipulation_station_context =
+    diagram->GetMutableSubsystemContext(*manipulation_station, &simulator_context);
+
+  auto & constant_term_context =
+    diagram->GetMutableSubsystemContext(*constant_term, &simulator_context);
+
+  // Fix gripper joints' position.
+  manipulation_station->GetInputPort("wsg_position")
+  .FixValue(&manipulation_station_context, 0.);
+
+  // Use default positions for every joint but the base joint.
+  drake::systems::BasicVector<double> & constants =
+    constant_term->get_mutable_source_value(&constant_term_context);
+  constants.set_value(
+    manipulation_station->GetIiwaPosition(manipulation_station_context));
+  constants.get_mutable_value()[0] = -M_PI / 4.;
+
+  while (true) {
+    simulator->AdvanceTo(simulator_context.get_time() + 0.1);
+  }
+  return 0;
+}

drake_ros_examples/examples/iiwa_manipulator/iiwa_manipulator.py

@@ -0,0 +1,96 @@
+#!/usr/bin/env python3
+# 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.
+
+import numpy as np
+
+from drake_ros_systems import RosInterfaceSystem
+from drake_ros_systems import RvizVisualizer
+
+from pydrake.examples.manipulation_station import ManipulationStation
+from pydrake.systems.analysis import Simulator
+from pydrake.systems.framework import DiagramBuilder
+from pydrake.systems.primitives import Adder
+from pydrake.systems.primitives import ConstantVectorSource
+from pydrake.systems.primitives import Sine
+
+
+def main():
+    builder = DiagramBuilder()
+
+    ros_interface_system = builder.AddSystem(RosInterfaceSystem())
+
+    manipulation_station = builder.AddSystem(ManipulationStation())
+    manipulation_station.SetupClutterClearingStation()
+    manipulation_station.Finalize()
+
+    # Make the base joint swing sinusoidally.
+    constant_term = builder.AddSystem(ConstantVectorSource(
+        np.zeros(manipulation_station.num_iiwa_joints())))
+
+    amplitudes = np.zeros(manipulation_station.num_iiwa_joints())
+    amplitudes[0] = np.pi / 4.  # == 45 degrees
+    frequencies = np.ones(manipulation_station.num_iiwa_joints())
+    phases = np.zeros(manipulation_station.num_iiwa_joints())
+    variable_term = builder.AddSystem(Sine(amplitudes, frequencies, phases))
+
+    joint_trajectory_generator = builder.AddSystem(
+        Adder(2, manipulation_station.num_iiwa_joints()))
+
+    builder.Connect(constant_term.get_output_port(),
+                    joint_trajectory_generator.get_input_port(0))
+    builder.Connect(variable_term.get_output_port(0),
+                    joint_trajectory_generator.get_input_port(1))
+    builder.Connect(joint_trajectory_generator.get_output_port(),
+                    manipulation_station.GetInputPort('iiwa_position'))
+
+    rviz_visualizer = builder.AddSystem(
+        RvizVisualizer(ros_interface_system.get_ros_interface()))
+
+    builder.Connect(
+        manipulation_station.GetOutputPort('query_object'),
+        rviz_visualizer.get_graph_query_port()
+    )
+
+    diagram = builder.Build()
+
+    simulator = Simulator(diagram)
+    simulator.Initialize()
+    simulator.set_target_realtime_rate(1.0)
+    context = simulator.get_mutable_context()
+
+    manipulation_station_context = \
+        diagram.GetMutableSubsystemContext(manipulation_station, context)
+    constant_term_context = \
+        diagram.GetMutableSubsystemContext(constant_term, context)
+
+    # Fix gripper joints' position.
+    manipulation_station.GetInputPort('wsg_position').FixValue(
+        manipulation_station_context, np.zeros(1))
+
+    # Use default positions for every joint but the base joint.
+    constants = constant_term.get_mutable_source_value(constant_term_context)
+    constants.set_value(
+        manipulation_station.GetIiwaPosition(manipulation_station_context))
+    constants.get_mutable_value()[0] = -np.pi / 4.
+
+    try:
+        while True:
+            simulator.AdvanceTo(context.get_time() + 0.1)
+    except KeyboardInterrupt:
+        pass
+
+
+if __name__ == '__main__':
+    main()