Add base drake_ros_viz package.

drake_ros_core/README.md

@@ -6,30 +6,21 @@ It is similar to this ROS1 prototype [`gizatt/drake_ros_systems`](https://github
 
 ## Building
 
-This package has been built and tested on Ubuntu Focal with ROS2 Rolling, using a Drake nightly from April 2021.
+This package has been built and tested on Ubuntu Focal with ROS2 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 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
-    
+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.
+
     **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-select drake_ros_core`
-
-    **CMake**
-    1. Manually set `CMAKE_PREFIX_PATH`: `export CMAKE_PREFIX_PATH=$CMAKE_PREFIX_PATH:$AMENT_PREFIX_PATH`
-    1. Get this code `git clone https://github.com/RobotLocomotion/drake-ros.git`
-    1. `cd drake-ros/drake_ros_core`
-    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`

drake_ros_core/package.xml

@@ -11,11 +11,10 @@
   <buildtool_depend>ament_cmake_ros</buildtool_depend>
 
   <depend>rclcpp</depend>
+  <depend>rclpy</depend>
   <depend>rosidl_runtime_c</depend>
   <depend>rosidl_typesupport_cpp</depend>
 
-  <exec_depend>rclpy</exec_depend>
-
   <test_depend>ament_cmake_clang_format</test_depend>
   <test_depend>ament_cmake_cpplint</test_depend>
   <test_depend>ament_cmake_gtest</test_depend>

drake_ros_tf2/README.md

@@ -4,15 +4,15 @@ This package provides abstractions to simplify the use of tf2 ROS functionality
 
 ## Building
 
-This package has been built and tested on Ubuntu Focal with ROS Rolling, using a Drake nightly from April 2021.
+This package has been built and tested on Ubuntu Focal with ROS Rolling, using a Drake nightly or any stable releses 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. [Use this Python virtualenv trick](https://drake.mit.edu/from_binary.html), under "Use as a Python Library", to extract the Drake binary installation, install its prerequisites.
+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.
 
@@ -22,13 +22,3 @@ To build it:
     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_tf2`
-
-    **CMake**
-    1. Get this code `git clone https://github.com/RobotLocomotion/drake-ros.git`
-    1. Build the [`drake_ros_core`](../drake_ros_core/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_core/install`
-    1. `cd drake-ros/drake_ros_tf2`
-    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`

drake_ros_tf2/include/drake_ros_tf2/utilities/name_conventions.h

@@ -20,7 +20,6 @@
 #include <drake/multibody/plant/multibody_plant.h>
 
 namespace drake_ros_tf2 {
-namespace utilities {
 
 /** Retrieve conventional tf frame name for a given scene frame.
 
@@ -57,5 +56,4 @@ std::string GetTfFrameName(
         plants,
     const drake::geometry::GeometryId& geometry_id);
 
-}  // namespace utilities
 }  // namespace drake_ros_tf2

drake_ros_tf2/include/drake_ros_tf2/utilities/type_conversion.h

@@ -17,11 +17,9 @@
 #include <geometry_msgs/msg/transform.hpp>
 
 namespace drake_ros_tf2 {
-namespace utilities {
 
-/** Convert `X_AB` into its ROS message equivalent. */
+/** Convert `X_AB` into a Transform ROS message. */
 geometry_msgs::msg::Transform ToTransformMsg(
     const drake::math::RigidTransform<double> X_AB);
 
-}  // namespace utilities
 }  // namespace drake_ros_tf2

drake_ros_tf2/src/scene_tf_system.cc

@@ -82,12 +82,12 @@ void SceneTfSystem::CalcSceneTf(const drake::systems::Context<double>& context,
         continue;
       }
 
-      transform.header.frame_id = utilities::GetTfFrameName(
+      transform.header.frame_id = GetTfFrameName(
           inspector, impl_->plants, inspector.GetParentFrame(frame_id));
       transform.child_frame_id =
-          utilities::GetTfFrameName(inspector, impl_->plants, frame_id);
+          GetTfFrameName(inspector, impl_->plants, frame_id);
       transform.transform =
-          utilities::ToTransformMsg(query_object.GetPoseInParent(frame_id));
+          ToTransformMsg(query_object.GetPoseInParent(frame_id));
       output_value->transforms.push_back(transform);
     }
   }

drake_ros_tf2/src/utilities/internal_name_conventions.h

@@ -17,7 +17,6 @@
 #include <string>
 
 namespace drake_ros_tf2 {
-namespace utilities {
 namespace internal {
 
 std::string ReplaceAllOccurrences(std::string string, const std::string& target,
@@ -30,12 +29,13 @@ std::string ReplaceAllOccurrences(std::string string, const std::string& target,
   return string;
 }
 
-/** Formulate tf frame name given the model instance name, body name, body
-  index and frame ID.
+/* Formulate tf frame name given the model instance name, body name, body
+  index and frame ID value.
   @param[in] model_instance_name name of a given model instance.
   @param[in] body_name name of a given body.
   @param[in] body_index index of a given body.
-  @param[in] frame_id target frame ID.
+  @param[in] frame_id_value value of the given frame ID.
+  @returns formulated tf frame name.
  */
 template <typename ElementIndexType>
 std::string CalcTfFrameName(const std::string& model_instance_name,
@@ -55,9 +55,10 @@ std::string CalcTfFrameName(const std::string& model_instance_name,
   return ss.str();
 }
 
-/** Formulate tf frame name given the frame name and frame ID.
+/* Formulate tf frame name given the frame name and frame ID value.
   @param[in] frame_name name of the given frame.
-  @param[in] frame_id index of the given frame.
+  @param[in] frame_id_value value of the given frame ID.
+  @returns formulated tf frame name.
  */
 std::string CalcTfFrameName(const std::string& frame_name,
                             int64_t frame_id_value) {
@@ -72,5 +73,4 @@ std::string CalcTfFrameName(const std::string& frame_name,
 }
 
 }  // namespace internal
-}  // namespace utilities
 }  // namespace drake_ros_tf2

drake_ros_tf2/src/utilities/name_conventions.cc

@@ -21,7 +21,6 @@
 #include "utilities/internal_name_conventions.h"
 
 namespace drake_ros_tf2 {
-namespace utilities {
 
 std::string GetTfFrameName(
     const drake::geometry::SceneGraphInspector<double>& inspector,
@@ -53,5 +52,4 @@ std::string GetTfFrameName(
   return GetTfFrameName(inspector, plants, inspector.GetFrameId(geometry_id));
 }
 
-}  // namespace utilities
 }  // namespace drake_ros_tf2

drake_ros_tf2/src/utilities/type_conversion.cc

@@ -19,7 +19,6 @@
 #include <geometry_msgs/msg/pose.hpp>
 
 namespace drake_ros_tf2 {
-namespace utilities {
 
 geometry_msgs::msg::Transform ToTransformMsg(
     const drake::math::RigidTransform<double> X_AB) {
@@ -38,5 +37,4 @@ geometry_msgs::msg::Transform ToTransformMsg(
   return msg;
 }
 
-}  // namespace utilities
 }  // namespace drake_ros_tf2

drake_ros_tf2/test/test_name_conventions.cc

@@ -20,54 +20,54 @@
 TEST(NameConventions, CalcTfFrameNameWithBody) {
   // All names are present
   EXPECT_EQ("model_name/body_name/456",
-            drake_ros_tf2::utilities::internal::CalcTfFrameName<int>(
+            drake_ros_tf2::internal::CalcTfFrameName<int>(
                 "model_name", "body_name", 123, 456));
 
   // Scoped model name and body name
   EXPECT_EQ("model_scope/model_name/body_scope/body_name/456",
-            drake_ros_tf2::utilities::internal::CalcTfFrameName(
+            drake_ros_tf2::internal::CalcTfFrameName(
                 "model_scope::model_name", "body_scope::body_name", 123, 456));
 
   // Model name with empty body name
-  EXPECT_EQ("model_name/unnamed_body_123/456",
-            drake_ros_tf2::utilities::internal::CalcTfFrameName("model_name",
-                                                                "", 123, 456));
+  EXPECT_EQ(
+      "model_name/unnamed_body_123/456",
+      drake_ros_tf2::internal::CalcTfFrameName("model_name", "", 123, 456));
 
   // Scoped model name with empty body name
   EXPECT_EQ("model_scope/model_name/unnamed_body_123/456",
-            drake_ros_tf2::utilities::internal::CalcTfFrameName(
-                "model_scope::model_name", "", 123, 456));
+            drake_ros_tf2::internal::CalcTfFrameName("model_scope::model_name",
+                                                     "", 123, 456));
 }
 
 TEST(NameConventions, CalcTfFrameNameWithoutBody) {
   // Frame name is not empty
-  EXPECT_EQ("frame_name", drake_ros_tf2::utilities::internal::CalcTfFrameName(
-                              "frame_name", 123));
+  EXPECT_EQ("frame_name",
+            drake_ros_tf2::internal::CalcTfFrameName("frame_name", 123));
 
   // Frame name is empty
   EXPECT_EQ("unnamed_frame_123",
-            drake_ros_tf2::utilities::internal::CalcTfFrameName("", 123));
+            drake_ros_tf2::internal::CalcTfFrameName("", 123));
 
   // Frame name with / delimiters
   EXPECT_EQ("scope_one/scope_two/frame_name",
-            drake_ros_tf2::utilities::internal::CalcTfFrameName(
+            drake_ros_tf2::internal::CalcTfFrameName(
                 "scope_one/scope_two/frame_name", 123));
 
   // Frame name with :: delimiters
   EXPECT_EQ("scope_one/scope_two/frame_name",
-            drake_ros_tf2::utilities::internal::CalcTfFrameName(
+            drake_ros_tf2::internal::CalcTfFrameName(
                 "scope_one::scope_two::frame_name", 123));
 
   // Frame name with mixed delimiters
   EXPECT_EQ("scope_one/scope_two/frame_name",
-            drake_ros_tf2::utilities::internal::CalcTfFrameName(
+            drake_ros_tf2::internal::CalcTfFrameName(
                 "scope_one/scope_two::frame_name", 123));
 
   // Frame name with only :: delimiter
   EXPECT_EQ("unnamed_frame_123",
-            drake_ros_tf2::utilities::internal::CalcTfFrameName("::", 123));
+            drake_ros_tf2::internal::CalcTfFrameName("::", 123));
 
   // Frame name with only / delimiter
   EXPECT_EQ("unnamed_frame_123",
-            drake_ros_tf2::utilities::internal::CalcTfFrameName("/", 123));
+            drake_ros_tf2::internal::CalcTfFrameName("/", 123));
 }

drake_ros_tf2/test/test_type_conversion.cc

@@ -24,7 +24,7 @@ TEST(TypeConversion, ToTransformMsg) {
                                            cos(-M_PI / 4.)};
   const drake::math::RigidTransform<double> transform{rotation, translation};
   const geometry_msgs::msg::Transform message =
-      drake_ros_tf2::utilities::ToTransformMsg(transform);
+      drake_ros_tf2::ToTransformMsg(transform);
   EXPECT_DOUBLE_EQ(message.translation.x, translation.x());
   EXPECT_DOUBLE_EQ(message.translation.y, translation.y());
   EXPECT_DOUBLE_EQ(message.translation.z, translation.z());

drake_ros_viz/.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_viz'
+    Priority: 50
+  # Other libraries' h files (with quotes).
+  - Regex:    '^"'
+    Priority: 40

drake_ros_viz/CMakeLists.txt

@@ -0,0 +1,76 @@
+cmake_minimum_required(VERSION 3.10)
+project(drake_ros_viz)
+
+# 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 REQUIRED)
+find_package(geometry_msgs REQUIRED)
+find_package(drake REQUIRED)
+
+add_library(drake_ros_viz
+  src/utilities/name_conventions.cc
+  src/utilities/type_conversion.cc
+)
+
+target_link_libraries(drake_ros_viz PUBLIC
+    drake::drake
+    ${geometry_msgs_TARGETS}
+)
+
+target_include_directories(drake_ros_viz
+  PUBLIC
+    "$<BUILD_INTERFACE:${CMAKE_CURRENT_SOURCE_DIR}/include>"
+    "$<BUILD_INTERFACE:${CMAKE_CURRENT_SOURCE_DIR}/src>"
+    "$<INSTALL_INTERFACE:include>"
+)
+
+ament_export_include_directories(include)
+ament_export_libraries(drake_ros_viz)
+ament_export_targets(${PROJECT_NAME})
+
+ament_export_dependencies(drake)
+ament_export_dependencies(geometry_msgs)
+
+install(
+  TARGETS ${PROJECT_NAME}
+  EXPORT ${PROJECT_NAME}
+  ARCHIVE DESTINATION lib
+  LIBRARY DESTINATION lib
+  RUNTIME DESTINATION bin
+)
+
+install(
+  DIRECTORY include/
+  DESTINATION include
+)
+
+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()
+
+  find_package(ament_cmake_gtest REQUIRED)
+
+  ament_add_gtest(test_type_conversion test/test_type_conversion.cc)
+  target_link_libraries(test_type_conversion
+    drake::drake
+    drake_ros_viz
+  )
+
+  ament_add_gtest(test_name_conventions test/test_name_conventions.cc)
+  target_link_libraries(test_name_conventions
+    drake::drake
+    drake_ros_viz
+  )
+endif()
+
+ament_package()