Add drake_ros_examples package

drake_ros_core/README.md

@@ -16,7 +16,7 @@ To build it:
 1. [Download Drake binary](https://drake.mit.edu/from_binary.html), nightly or any stable releases after 14 Jan 2022.
 1. Extract the Drake binary installation, install it's prerequisites, and [use this Python virutalenv trick](https://drake.mit.edu/from_binary.html).
 1. Activate the drake virtual environment.
-1. Build it using Colcon, or using CMake directly.
+1. Build it using Colcon.
 
     **Colcon**
     1. Make a workspace `mkdir -p ./ws/src`

drake_ros_core/package.xml

@@ -10,6 +10,7 @@
 
   <buildtool_depend>ament_cmake_ros</buildtool_depend>
 
+  <depend>pybind11-dev</depend>
   <depend>rclcpp</depend>
   <depend>rclpy</depend>
   <depend>rosidl_runtime_c</depend>

drake_ros_examples/.clang-format

@@ -0,0 +1,45 @@
+# -*- yaml -*-
+
+# Taken from RobotLocomotion/drake @ 73925c7eb71361e2e97601af4182ba54072d192a
+# This file determines clang-format's style settings; for details, refer to
+# http://clang.llvm.org/docs/ClangFormatStyleOptions.html
+
+BasedOnStyle:  Google
+
+Language: Cpp
+
+# Force pointers to the type for C++.
+DerivePointerAlignment: false
+PointerAlignment: Left
+
+# Specify the #include statement order.  This implements the order mandated by
+# the Google C++ Style Guide: related header, C headers, C++ headers, library
+# headers, and finally the project headers.
+#
+# To obtain updated lists of system headers used in the below expressions, see:
+# http://stackoverflow.com/questions/2027991/list-of-standard-header-files-in-c-and-c/2029106#2029106.
+IncludeCategories:
+  # Spacers used by drake/tools/formatter.py.
+  - Regex:    '^<clang-format-priority-15>$'
+    Priority: 15
+  - Regex:    '^<clang-format-priority-25>$'
+    Priority: 25
+  - Regex:    '^<clang-format-priority-35>$'
+    Priority: 35
+  - Regex:    '^<clang-format-priority-45>$'
+    Priority: 45
+  # C system headers.
+  - Regex:    '^[<"](aio|arpa/inet|assert|complex|cpio|ctype|curses|dirent|dlfcn|errno|fcntl|fenv|float|fmtmsg|fnmatch|ftw|glob|grp|iconv|inttypes|iso646|langinfo|libgen|limits|locale|math|monetary|mqueue|ndbm|netdb|net/if|netinet/in|netinet/tcp|nl_types|poll|pthread|pwd|regex|sched|search|semaphore|setjmp|signal|spawn|stdalign|stdarg|stdatomic|stdbool|stddef|stdint|stdio|stdlib|stdnoreturn|string|strings|stropts|sys/ipc|syslog|sys/mman|sys/msg|sys/resource|sys/select|sys/sem|sys/shm|sys/socket|sys/stat|sys/statvfs|sys/time|sys/times|sys/types|sys/uio|sys/un|sys/utsname|sys/wait|tar|term|termios|tgmath|threads|time|trace|uchar|ulimit|uncntrl|unistd|utime|utmpx|wchar|wctype|wordexp)\.h[">]$'
+    Priority: 20
+  # C++ system headers (as of C++17).
+  - Regex:    '^[<"](algorithm|any|array|atomic|bitset|cassert|ccomplex|cctype|cerrno|cfenv|cfloat|charconv|chrono|cinttypes|ciso646|climits|clocale|cmath|codecvt|complex|condition_variable|csetjmp|csignal|cstdalign|cstdarg|cstdbool|cstddef|cstdint|cstdio|cstdlib|cstring|ctgmath|ctime|cuchar|cwchar|cwctype|deque|exception|execution|filesystem|forward_list|fstream|functional|future|initializer_list|iomanip|ios|iosfwd|iostream|istream|iterator|limits|list|locale|map|memory|memory_resource|mutex|new|numeric|optional|ostream|queue|random|ratio|regex|scoped_allocator|set|shared_mutex|sstream|stack|stdexcept|streambuf|string|string_view|strstream|system_error|thread|tuple|type_traits|typeindex|typeinfo|unordered_map|unordered_set|utility|valarray|variant|vector)[">]$'
+    Priority: 30
+  # Other libraries' h files (with angles).
+  - Regex:    '^<'
+    Priority: 40
+  # Your project's h files.
+  - Regex:    '^"drake_ros_examples'
+    Priority: 50
+  # Other libraries' h files (with quotes).
+  - Regex:    '^"'
+    Priority: 40

drake_ros_examples/CMakeLists.txt

@@ -0,0 +1,28 @@
+cmake_minimum_required(VERSION 3.10)
+project(drake_ros_examples)
+
+# Default to C++17
+if(NOT CMAKE_CXX_STANDARD)
+  set(CMAKE_CXX_STANDARD 17)
+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_core REQUIRED)
+find_package(drake_ros_viz REQUIRED)
+
+add_subdirectory(examples)
+
+if(BUILD_TESTING)
+  find_package(ament_cmake_clang_format REQUIRED)
+  find_package(ament_cmake_cpplint REQUIRED)
+
+  ament_clang_format(CONFIG_FILE .clang-format)
+  ament_cpplint()
+endif()
+
+ament_package()

drake_ros_examples/CPPLINT.cfg

@@ -0,0 +1,64 @@
+# Taken from RobotLocomotion/drake @ 73925c7eb71361e2e97601af4182ba54072d192a
+#
+# Copyright 2016 Robot Locomotion Group @ CSAIL. All rights reserved.
+#
+# Redistribution and use in source and binary forms, with or without modification,
+# are permitted provided that the following conditions are met:
+#
+# 1. Redistributions of source code must retain the above copyright notice,
+#    this list of conditions and the following disclaimer.
+# 2. 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.
+# 3. Neither the name of the copyright holder 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 HOLDER 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.
+
+# Stop searching for additional config files.
+set noparent
+
+# Disable a warning about C++ features that were not in the original
+# C++11 specification (and so might not be well-supported).  In the
+# case of Drake, our supported minimum platforms are new enough that
+# this warning is irrelevant.
+filter=-build/c++11
+
+# Drake uses `#pragma once`, not the `#ifndef FOO_H` guard.
+# https://drake.mit.edu/styleguide/cppguide.html#The__pragma_once_Guard
+filter=-build/header_guard
+filter=+build/pragma_once
+
+# Disable cpplint's include order.  We have our own via //tools:drakelint.
+filter=-build/include_order
+
+# TODO(hidmic): uncomment when ament_cpplint updates its vendored cpplint
+# version to support the following filters. Also remove corresponding NOLINT
+# codetags in sources.
+## Allow private, sibling include files.
+#filter=-build/include_subdir
+
+# We do not care about the whitespace details of a TODO comment.  It is not
+# relevant for easy grepping, and the GSG does not specify any particular
+# whitespace style.  (We *do* care what the "TODO(username)" itself looks like
+# because GSG forces a particular style there, but that formatting is covered
+# by the readability/todo rule, which we leave enabled.)
+filter=-whitespace/todo
+
+# TODO(#1805) Fix this.
+filter=-legal/copyright
+
+# Ignore code that isn't ours.
+exclude_files=third_party
+
+# It's not worth lint-gardening the documentation.
+exclude_files=doc

drake_ros_examples/README.md

@@ -0,0 +1,40 @@
+# Drake ROS Examples
+
+This is a collection of examples built around `drake_ros` libraries' C++ and Python APIs.
+
+## Building
+
+This package has been built and tested on Ubuntu Focal with ROS Rolling, using a Drake nightly or any stable releases after 14 Jan 2022.
+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 Drake binary](https://drake.mit.edu/from_binary.html), nightly or any stable releases after 14 Jan 2022.
+1. Extract the Drake binary installation, install it's prerequisites, and [use this Python virutalenv trick](https://drake.mit.edu/from_binary.html).
+1. Activate the drake virtual environment.
+1. Build it using Colcon.
+
+    **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 `colcon build --packages-up-to drake_ros_examples`
+
+## 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>`.
+
+## 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,17 @@
+add_executable(iiwa_manipulator iiwa_manipulator.cpp)
+target_link_libraries(iiwa_manipulator
+  drake::drake
+  drake_ros_core::drake_ros_core
+  drake_ros_viz::drake_ros_viz
+)
+
+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,19 @@
+# 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,114 @@
+// 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 <cmath>
+#include <memory>
+#include <utility>
+
+#include <drake/common/eigen_types.h>
+#include <drake/examples/manipulation_station/manipulation_station.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_ros_core/drake_ros.h>
+#include <drake_ros_core/ros_interface_system.h>
+#include <drake_ros_viz/rviz_visualizer.h>
+
+using drake_ros_core::DrakeRos;
+using drake_ros_core::RosInterfaceSystem;
+using drake_ros_viz::RvizVisualizer;
+
+using drake::examples::manipulation_station::ManipulationStation;
+using drake::systems::Adder;
+using drake::systems::ConstantVectorSource;
+using drake::systems::Simulator;
+using drake::systems::Sine;
+
+int main() {
+  drake::systems::DiagramBuilder<double> builder;
+
+  drake_ros_core::init();
+  auto ros_interface_system = builder.AddSystem<RosInterfaceSystem>(
+      std::make_unique<DrakeRos>("iiwa_manipulator_node"));
+
+  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());
+
+  rviz_visualizer->RegisterMultibodyPlant(
+      &manipulation_station->get_multibody_plant());
+
+  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;
+}