Add actuator_msgs to humble bridge.

Signed-off-by: Benjamin Perseghetti <bperseghetti@rudislabs.com>

ros_gz_bridge/CMakeLists.txt

@@ -60,6 +60,7 @@ set(GZ_MSGS_VERSION_FULL ${GZ_MSGS_VERSION_MAJOR}.${GZ_MSGS_VERSION_MINOR}.${GZ_
 
 set(BRIDGE_MESSAGE_TYPES
   builtin_interfaces
+  actuator_msgs
   geometry_msgs
   nav_msgs
   rcl_interfaces

ros_gz_bridge/include/ros_gz_bridge/convert.hpp

@@ -15,6 +15,7 @@
 #ifndef ROS_GZ_BRIDGE__CONVERT_HPP_
 #define ROS_GZ_BRIDGE__CONVERT_HPP_
 
+#include <ros_gz_bridge/convert/actuator_msgs.hpp>
 #include <ros_gz_bridge/convert/geometry_msgs.hpp>
 #include <ros_gz_bridge/convert/nav_msgs.hpp>
 #include <ros_gz_bridge/convert/ros_gz_interfaces.hpp>

ros_gz_bridge/include/ros_gz_bridge/convert/actuator_msgs.hpp

@@ -0,0 +1,43 @@
+// Copyright 2023 Rudis Laboratories LLC
+//
+// 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 ROS_GZ_BRIDGE__CONVERT__ACTUATOR_MSGS_HPP_
+#define ROS_GZ_BRIDGE__CONVERT__ACTUATOR_MSGS_HPP_
+
+// Gazebo Msgs
+#include <ignition/msgs/actuators.pb.h>
+
+// ROS 2 messages
+#include <actuator_msgs/msg/actuators.hpp>
+
+#include <ros_gz_bridge/convert_decl.hpp>
+
+namespace ros_gz_bridge
+{
+// actuator_msgs
+template<>
+void
+convert_ros_to_gz(
+  const actuator_msgs::msg::Actuators & ros_msg,
+  ignition::msgs::Actuators & gz_msg);
+
+template<>
+void
+convert_gz_to_ros(
+  const ignition::msgs::Actuators & gz_msg,
+  actuator_msgs::msg::Actuators & ros_msg);
+
+}  // namespace ros_gz_bridge
+
+#endif  // ROS_GZ_BRIDGE__CONVERT__ACTUATOR_MSGS_HPP_

ros_gz_bridge/package.xml

@@ -13,6 +13,7 @@
   <buildtool_depend>ament_cmake</buildtool_depend>
   <buildtool_depend>pkg-config</buildtool_depend>
 
+  <depend>actuator_msgs</depend>
   <depend>geometry_msgs</depend>
   <depend>nav_msgs</depend>
   <depend>rclcpp</depend>

ros_gz_bridge/ros_gz_bridge/mappings.py

@@ -27,6 +27,9 @@
     'builtin_interfaces': [
         Mapping('Time', 'Time'),
     ],
+    'actuator_msgs': [
+        Mapping('Actuators', 'Actuators'),
+    ],
     'geometry_msgs': [
         Mapping('Point', 'Vector3d'),
         Mapping('Pose', 'Pose'),

ros_gz_bridge/src/convert/actuator_msgs.cpp

@@ -0,0 +1,62 @@
+// Copyright 2023 Rudis Laboratories LLC
+//
+// 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 "convert/utils.hpp"
+#include "ros_gz_bridge/convert/actuator_msgs.hpp"
+
+namespace ros_gz_bridge
+{
+
+template<>
+void
+convert_ros_to_gz(
+  const actuator_msgs::msg::Actuators & ros_msg,
+  ignition::msgs::Actuators & gz_msg)
+{
+  convert_ros_to_gz(ros_msg.header, (*gz_msg.mutable_header()));
+
+  for (auto i = 0u; i < ros_msg.position.size(); ++i) {
+    gz_msg.add_position(ros_msg.position[i]);
+  }
+
+  for (auto i = 0u; i < ros_msg.velocity.size(); ++i) {
+    gz_msg.add_velocity(ros_msg.velocity[i]);
+  }
+  for (auto i = 0u; i < ros_msg.normalized.size(); ++i) {
+    gz_msg.add_normalized(ros_msg.normalized[i]);
+  }
+}
+
+template<>
+void
+convert_gz_to_ros(
+  const ignition::msgs::Actuators & gz_msg,
+  actuator_msgs::msg::Actuators & ros_msg)
+{
+  convert_gz_to_ros(gz_msg.header(), ros_msg.header);
+
+  for (auto i = 0; i < gz_msg.position_size(); ++i) {
+    ros_msg.position.push_back(gz_msg.position(i));
+  }
+
+  for (auto i = 0; i < gz_msg.velocity_size(); ++i) {
+    ros_msg.velocity.push_back(gz_msg.velocity(i));
+  }
+
+  for (auto i = 0; i < gz_msg.normalized_size(); ++i) {
+    ros_msg.normalized.push_back(gz_msg.normalized(i));
+  }
+}
+
+}  // namespace ros_gz_bridge

ros_gz_bridge/test/utils/gz_test_msg.cpp

@@ -955,10 +955,10 @@ void createTestMsg(ignition::msgs::Actuators & _msg)
   createTestMsg(header_msg);
   _msg.mutable_header()->CopyFrom(header_msg);
 
-  for (int i = 0u; i < 5; ++i) {
-    _msg.add_position(i);
-    _msg.add_velocity(i);
-    _msg.add_normalized(i);
+  for (int i = 0u; i < 3; ++i) {
+    _msg.add_position(0.5);
+    _msg.add_velocity(1.0);
+    _msg.add_normalized(0.2);
   }
 }
 
@@ -970,9 +970,15 @@ void compareTestMsg(const std::shared_ptr<ignition::msgs::Actuators> & _msg)
   compareTestMsg(std::make_shared<ignition::msgs::Header>(_msg->header()));
 
   for (int i = 0; i < expected_msg.position_size(); ++i) {
-    EXPECT_EQ(expected_msg.position(i), _msg->position(i));
-    EXPECT_EQ(expected_msg.velocity(i), _msg->velocity(i));
-    EXPECT_EQ(expected_msg.normalized(i), _msg->normalized(i));
+    EXPECT_FLOAT_EQ(expected_msg.position(i), _msg->position(i));
+  }
+
+  for (int i = 0; i < expected_msg.velocity_size(); ++i) {
+    EXPECT_FLOAT_EQ(expected_msg.velocity(i), _msg->velocity(i));
+  }
+
+  for (int i = 0; i < expected_msg.normalized_size(); ++i) {
+    EXPECT_FLOAT_EQ(expected_msg.normalized(i), _msg->normalized(i));
   }
 }
 

ros_gz_bridge/test/utils/ros_test_msg.cpp

@@ -51,6 +51,37 @@ void compareTestMsg(const std::shared_ptr<std_msgs::msg::Bool> & _msg)
   EXPECT_EQ(expected_msg.data, _msg->data);
 }
 
+void createTestMsg(actuator_msgs::msg::Actuators & _msg)
+{
+  std_msgs::msg::Header header_msg;
+  createTestMsg(header_msg);
+
+  _msg.header = header_msg;
+  _msg.position = {0.5, 0.5, 0.5};
+  _msg.velocity = {1.0, 1.0, 1.0};
+  _msg.normalized = {0.2, 0.2, 0.2};
+}
+
+void compareTestMsg(const std::shared_ptr<actuator_msgs::msg::Actuators> & _msg)
+{
+  actuator_msgs::msg::Actuators expected_msg;
+  createTestMsg(expected_msg);
+
+  compareTestMsg(_msg->header);
+
+  for (auto i = 0u; i < _msg->position.size(); ++i) {
+    EXPECT_FLOAT_EQ(expected_msg.position[i], _msg->position[i]);
+  }
+
+  for (auto i = 0u; i < _msg->velocity.size(); ++i) {
+    EXPECT_FLOAT_EQ(expected_msg.velocity[i], _msg->velocity[i]);
+  }
+
+  for (auto i = 0u; i < _msg->normalized.size(); ++i) {
+    EXPECT_FLOAT_EQ(expected_msg.normalized[i], _msg->normalized[i]);
+  }
+}
+
 void createTestMsg(std_msgs::msg::ColorRGBA & _msg)
 {
   _msg.r = 0.2;

ros_gz_bridge/test/utils/ros_test_msg.hpp

@@ -29,6 +29,7 @@
 #include <std_msgs/msg/u_int32.hpp>
 #include <std_msgs/msg/header.hpp>
 #include <std_msgs/msg/string.hpp>
+#include <actuator_msgs/msg/actuators.hpp>
 #include <geometry_msgs/msg/point.hpp>
 #include <geometry_msgs/msg/pose.hpp>
 #include <geometry_msgs/msg/pose_array.hpp>
@@ -181,6 +182,16 @@ void createTestMsg(rosgraph_msgs::msg::Clock & _msg);
 /// \param[in] _msg The message to compare.
 void compareTestMsg(const std::shared_ptr<rosgraph_msgs::msg::Clock> & _msg);
 
+/// actuator_msgs
+
+/// \brief Create a message used for testing.
+/// \param[out] _msg The message populated.
+void createTestMsg(actuator_msgs::msg::Actuators & _msg);
+
+/// \brief Compare a message with the populated for testing.
+/// \param[in] _msg The message to compare.
+void compareTestMsg(const std::shared_ptr<actuator_msgs::msg::Actuators> & _msg);
+
 /// geometry_msgs
 
 /// \brief Create a message used for testing.