apply review comments

Author: EricCousineau-TRI

drake_ros_core/CMakeLists.txt

@@ -26,22 +26,22 @@ if(NOT Eigen3_FOUND)
 endif()
 
 add_library(drake_ros_core
-  src/geometry_conversions.cc
   src/drake_ros.cc
+  src/geometry_conversions.cc
   src/publisher.cc
   src/ros_interface_system.cc
   src/ros_publisher_system.cc
   src/ros_subscriber_system.cc
   src/subscription.cc
 )
 
-target_link_libraries(drake_ros_core PUBLIC
-    drake::drake
-    rclcpp::rclcpp
-    rosidl_runtime_c::rosidl_runtime_c
-    rosidl_typesupport_cpp::rosidl_typesupport_cpp
-    Eigen3::Eigen
-    ${geometry_msgs_TARGETS}
+ament_target_dependencies(drake_ros_core
+  Eigen3
+  drake
+  geometry_msgs
+  rclcpp
+  rosidl_runtime_c
+  rosidl_typesupport_cpp
 )
 
 target_include_directories(drake_ros_core
@@ -56,10 +56,10 @@ ament_export_libraries(drake_ros_core)
 ament_export_targets(${PROJECT_NAME})
 
 ament_export_dependencies(drake)
+ament_export_dependencies(geometry_msgs)
 ament_export_dependencies(rclcpp)
 ament_export_dependencies(rosidl_runtime_c)
 ament_export_dependencies(rosidl_typesupport_cpp)
-ament_export_dependencies(geometry_msgs)
 
 install(TARGETS ${PROJECT_NAME} EXPORT ${PROJECT_NAME}
   ARCHIVE DESTINATION lib
@@ -116,6 +116,8 @@ if(BUILD_TESTING)
     # We do not expose `rmw_isoliation` via CMake.
     _TEST_DISABLE_RMW_ISOLATION
   )
+  # N.B. We cannot use `ament_target_dependencies` because we cannot access this
+  # package's exported ament target.
   target_link_libraries(test_pub_sub
     drake::drake
     drake_ros_core
@@ -136,7 +138,7 @@ if(BUILD_TESTING)
   )
 
   ament_add_gtest(test_geometry_conversions test/test_geometry_conversions.cc)
-  taRget_link_libraries(test_geometry_conversions drake_ros_core)
+  target_link_libraries(test_geometry_conversions drake_ros_core)
 endif()
 
 ament_package()

drake_ros_core/include/drake_ros_core/geometry_conversions.h

@@ -12,6 +12,35 @@
 // See the License for the specific language governing permissions and
 // limitations under the License.
 
+/**
+@file
+
+Conversions for ROS's geometry_msgs types for Drake and Eigen types that Drake
+uses.
+
+### Type Suffixes
+
+Note that quantities like `Eigen::Vector3d`, `Eigen::Isometry3d`,
+`drake::math::RigidTransformd`, etc. all have the `d` suffix to denote
+templating on `double`.
+
+For simplicity, we strip these suffixes from the names in the conversion
+functions.
+
+### Spatial Vectors
+
+All 6d spatial vectors are ordered by [rotational; translational], per
+ttps://drake.mit.edu/doxygen_cxx/group__multibody__spatial__vectors.html
+
+### Isometry3 Elements
+
+All Isometry3<> conversion methods actually use RigidTransform as an
+intermediate. This means that we are restricted to SE(3), or more specifically,
+rotations are constrained to be in SO(3), not just O(3), so it is not to
+express mirroring in Isometry3<> results (even though the type is capable of
+expressing it).
+*/
+
 #pragma once
 
 #include <Eigen/Geometry>
@@ -28,66 +57,90 @@
 
 namespace drake_ros_core {
 
-Eigen::Vector3d RosPointToVector3d(const geometry_msgs::msg::Point& point);
+// Vector / Translation.
+
+Eigen::Vector3d RosPointToVector3(const geometry_msgs::msg::Point& point);
+
+geometry_msgs::msg::Point Vector3ToRosPoint(const Eigen::Vector3d& point);
+
+Eigen::Vector3d RosVector3ToVector3(const geometry_msgs::msg::Vector3& point);
 
-geometry_msgs::msg::Point Vector3dToRosPoint(const Eigen::Vector3d& point);
+geometry_msgs::msg::Vector3 Vector3ToRosVector3(const Eigen::Vector3d& point);
 
-Eigen::Quaternion<double> RosQuatToQuat(
+// Orientation.
+
+Eigen::Quaternion<double> RosQuaternionToQuaternion(
     const geometry_msgs::msg::Quaternion& quat);
 
-geometry_msgs::msg::Quaternion QuatToRosQuat(
+geometry_msgs::msg::Quaternion QuaternionToRosQuaternion(
     const Eigen::Quaternion<double>& quat);
 
-Eigen::Isometry3d RosPoseToIsometry3d(const geometry_msgs::msg::Pose& pose);
+drake::math::RotationMatrixd RosQuaternionToRotationMatrix(
+    const geometry_msgs::msg::Quaternion& quat);
+
+geometry_msgs::msg::Quaternion RotationMatrixToRosQuaternion(
+    const drake::math::RotationMatrixd& rotation);
 
-geometry_msgs::msg::Pose Isometry3dToRosPose(const Eigen::Isometry3d& isometry);
+// Pose.
 
 drake::math::RigidTransformd RosPoseToRigidTransform(
     const geometry_msgs::msg::Pose& pose);
 
 geometry_msgs::msg::Pose RigidTransformToRosPose(
     const drake::math::RigidTransformd& transform);
 
-Eigen::Isometry3d RosTransformToIsometry3d(
-    const geometry_msgs::msg::Transform& transform);
-
-geometry_msgs::msg::Transform Isometry3dToRosTransform(
-    const Eigen::Isometry3d& isometry);
-
 drake::math::RigidTransformd RosTransformToRigidTransform(
     const geometry_msgs::msg::Transform& transform);
 
 geometry_msgs::msg::Transform RigidTransformToRosTransform(
     const drake::math::RigidTransformd& transform);
 
-drake::Vector6d RosTwistToVector6d(const geometry_msgs::msg::Twist& twist);
+Eigen::Isometry3d RosPoseToIsometry3(const geometry_msgs::msg::Pose& pose);
+
+geometry_msgs::msg::Pose Isometry3ToRosPose(const Eigen::Isometry3d& isometry);
+
+Eigen::Isometry3d RosTransformToIsometry3(
+    const geometry_msgs::msg::Transform& transform);
+
+geometry_msgs::msg::Transform Isometry3ToRosTransform(
+    const Eigen::Isometry3d& isometry);
+
+// Spatial Velocity.
+
+drake::Vector6d RosTwistToVector6(const geometry_msgs::msg::Twist& twist);
 
-geometry_msgs::msg::Twist Vector6dToRosTwist(const drake::Vector6d& vector);
+geometry_msgs::msg::Twist Vector6ToRosTwist(const drake::Vector6d& vector);
 
-drake::multibody::SpatialVelocity<double> RosTwistToVelocity(
+drake::multibody::SpatialVelocity<double> RosTwistToSpatialVelocity(
     const geometry_msgs::msg::Twist& twist);
 
-geometry_msgs::msg::Twist VelocityToRosTwist(
+geometry_msgs::msg::Twist SpatialVelocityToRosTwist(
     const drake::multibody::SpatialVelocity<double>& velocity);
 
-drake::Vector6d RosAccelToVector6d(const geometry_msgs::msg::Accel& accel);
+// Spatial Acceleration.
 
-geometry_msgs::msg::Accel Vector6dToRosAccel(const drake::Vector6d& vector);
+drake::Vector6d RosAccelerationToVector6(
+    const geometry_msgs::msg::Accel& accel);
+
+geometry_msgs::msg::Accel Vector6ToRosAcceleration(
+    const drake::Vector6d& vector);
 
-drake::multibody::SpatialAcceleration<double> RosAccelToAccel(
+drake::multibody::SpatialAcceleration<double> RosAccelerationToSpatialAcceleration(
     const geometry_msgs::msg::Accel& accel);
 
-geometry_msgs::msg::Accel AccelToRosAccel(
+geometry_msgs::msg::Accel SpatialAccelerationToRosAcceleration(
     const drake::multibody::SpatialAcceleration<double>& accel);
 
-drake::Vector6d RosWrenchToVector6d(const geometry_msgs::msg::Wrench& wrench);
+// Spatial Force.
+
+drake::Vector6d RosWrenchToVector6(const geometry_msgs::msg::Wrench& wrench);
 
-geometry_msgs::msg::Wrench Vector6dToRosWrench(const drake::Vector6d& vector);
+geometry_msgs::msg::Wrench Vector6ToRosWrench(const drake::Vector6d& vector);
 
-drake::multibody::SpatialForce<double> RosWrenchToForce(
+drake::multibody::SpatialForce<double> RosWrenchToSpatialForce(
     const geometry_msgs::msg::Wrench& wrench);
 
-geometry_msgs::msg::Wrench ForceToRosWrench(
+geometry_msgs::msg::Wrench SpatialForceToRosWrench(
     const drake::multibody::SpatialForce<double>& force);
 
 }  // namespace drake_ros_core