geometry_msgs/Twist to ignition::msgs::Twist (#14)

* Adding geometry_msgs/Twist to ignition::msgs::Twist conversion. * Fix test.
gazebosim · May 20, 2019 · 0665453 · 0665453
1 parent 586b44c
commit 0665453
Show file tree

Hide file tree

Showing 12 changed files with 174 additions and 0 deletions.
diff --git a/README.md b/README.md
@@ -20,6 +20,7 @@ service calls.Its support is limited to only the following message types:
 | geometry_msgs/PoseStamped      | ignition::msgs::Pose         |
 | geometry_msgs/Transform        | ignition::msgs::Pose         |
 | geometry_msgs/TransformStamped | ignition::msgs::Pose         |
+| geometry_msgs/Twist            | ignition::msgs::Twist        |
 | mav_msgs/Actuators             | ignition::msgs::Actuators    |
 | rosgraph_msgs/Clock            | ignition::msgs::Clock        |
 | sensor_msgs/Imu                | ignition::msgs::IMU          |

diff --git a/include/ros1_ign_bridge/builtin_interfaces_factories.hpp b/include/ros1_ign_bridge/builtin_interfaces_factories.hpp
@@ -22,6 +22,7 @@
 #include <geometry_msgs/PoseStamped.h>
 #include <geometry_msgs/Transform.h>
 #include <geometry_msgs/TransformStamped.h>
+#include <geometry_msgs/Twist.h>
 #include <geometry_msgs/Vector3.h>
 #include <mav_msgs/Actuators.h>
 #include <rosgraph_msgs/Clock.h>
@@ -259,6 +260,24 @@ Factory<
   const ignition::msgs::Pose & ign_msg,
   geometry_msgs::TransformStamped & ros1_msg);
 
+template<>
+void
+Factory<
+  geometry_msgs::Twist,
+  ignition::msgs::Twist
+>::convert_1_to_ign(
+  const geometry_msgs::Twist & ros1_msg,
+  ignition::msgs::Twist & ign_msg);
+
+template<>
+void
+Factory<
+  geometry_msgs::Twist,
+  ignition::msgs::Twist
+>::convert_ign_to_1(
+  const ignition::msgs::Twist & ign_msg,
+  geometry_msgs::Twist & ros1_msg);
+
 // mav_msgs
 template<>
 void

diff --git a/include/ros1_ign_bridge/convert_builtin_interfaces.hpp b/include/ros1_ign_bridge/convert_builtin_interfaces.hpp
@@ -24,6 +24,7 @@
 #include <rosgraph_msgs/Clock.h>
 #include <geometry_msgs/Transform.h>
 #include <geometry_msgs/TransformStamped.h>
+#include <geometry_msgs/Twist.h>
 #include <mav_msgs/Actuators.h>
 #include <sensor_msgs/FluidPressure.h>
 #include <sensor_msgs/Image.h>
@@ -179,6 +180,18 @@ convert_ign_to_1(
   const ignition::msgs::Pose & ign_msg,
   geometry_msgs::TransformStamped & ros1_msg);
 
+template<>
+void
+convert_1_to_ign(
+  const geometry_msgs::Twist & ros1_msg,
+  ignition::msgs::Twist & ign_msg);
+
+template<>
+void
+convert_ign_to_1(
+  const ignition::msgs::Twist & ign_msg,
+  geometry_msgs::Twist & ros1_msg);
+
 // mav_msgs
 template<>
 void

diff --git a/src/builtin_interfaces_factories.cpp b/src/builtin_interfaces_factories.cpp
@@ -149,6 +149,17 @@ get_factory_builtin_interfaces(
       >
     >("geometry_msgs/TransformStamped", ign_type_name);
   }
+  if (
+    (ros1_type_name == "geometry_msgs/Twist" || ros1_type_name == "") &&
+     ign_type_name == "ignition.msgs.Twist")
+  {
+    return std::make_shared<
+      Factory<
+        geometry_msgs::Twist,
+        ignition::msgs::Twist
+      >
+    >("geometry_msgs/Twist", ign_type_name);
+  }
   if (
     (ros1_type_name == "mav_msgs/Actuators" || ros1_type_name == "") &&
      ign_type_name == "ignition.msgs.Actuators")
@@ -521,6 +532,30 @@ Factory<
   ros1_ign_bridge::convert_ign_to_1(ign_msg, ros1_msg);
 }
 
+template<>
+void
+Factory<
+  geometry_msgs::Twist,
+  ignition::msgs::Twist
+>::convert_1_to_ign(
+  const geometry_msgs::Twist & ros1_msg,
+  ignition::msgs::Twist & ign_msg)
+{
+  ros1_ign_bridge::convert_1_to_ign(ros1_msg, ign_msg);
+}
+
+template<>
+void
+Factory<
+  geometry_msgs::Twist,
+  ignition::msgs::Twist
+>::convert_ign_to_1(
+  const ignition::msgs::Twist & ign_msg,
+  geometry_msgs::Twist & ros1_msg)
+{
+  ros1_ign_bridge::convert_ign_to_1(ign_msg, ros1_msg);
+}
+
 // mav_msgs
 template<>
 void

diff --git a/src/convert_builtin_interfaces.cpp b/src/convert_builtin_interfaces.cpp
@@ -320,6 +320,26 @@ convert_ign_to_1(
   }
 }
 
+template<>
+void
+convert_1_to_ign(
+  const geometry_msgs::Twist & ros1_msg,
+  ignition::msgs::Twist & ign_msg)
+{
+  convert_1_to_ign(ros1_msg.linear,  (*ign_msg.mutable_linear()));
+  convert_1_to_ign(ros1_msg.angular, (*ign_msg.mutable_angular()));
+}
+
+template<>
+void
+convert_ign_to_1(
+  const ignition::msgs::Twist & ign_msg,
+  geometry_msgs::Twist & ros1_msg)
+{
+  convert_ign_to_1(ign_msg.linear(), ros1_msg.linear);
+  convert_ign_to_1(ign_msg.angular(), ros1_msg.angular);
+}
+
 template<>
 void
 convert_1_to_ign(

diff --git a/test/launch/test_ign_subscriber.launch b/test/launch/test_ign_subscriber.launch
@@ -14,6 +14,7 @@
               /pose_stamped@geometry_msgs/PoseStamped@ignition.msgs.Pose
               /transform@geometry_msgs/Transform@ignition.msgs.Pose
               /transform_stamped@geometry_msgs/TransformStamped@ignition.msgs.Pose
+              /twist@geometry_msgs/Twist@ignition.msgs.Twist
               /image@sensor_msgs/Image@ignition.msgs.Image
               /imu@sensor_msgs/Imu@ignition.msgs.IMU
               /laserscan@sensor_msgs/LaserScan@ignition.msgs.LaserScan

diff --git a/test/launch/test_ros1_subscriber.launch b/test/launch/test_ros1_subscriber.launch
@@ -14,6 +14,7 @@
               /pose_stamped@geometry_msgs/PoseStamped@ignition.msgs.Pose
               /transform@geometry_msgs/Transform@ignition.msgs.Pose
               /transform_stamped@geometry_msgs/TransformStamped@ignition.msgs.Pose
+              /twist@geometry_msgs/Twist@ignition.msgs.Twist
               /image@sensor_msgs/Image@ignition.msgs.Image
               /imu@sensor_msgs/Imu@ignition.msgs.IMU
               /laserscan@sensor_msgs/LaserScan@ignition.msgs.LaserScan

diff --git a/test/publishers/ign_publisher.cpp b/test/publishers/ign_publisher.cpp
@@ -136,6 +136,11 @@ int main(int /*argc*/, char **/*argv*/)
   ignition::msgs::Model joint_states_msg;
   ros1_ign_bridge::testing::createTestMsg(joint_states_msg);
 
+  // ignition::msgs::Twist.
+  auto twist_pub = node.Advertise<ignition::msgs::Twist>("twist");
+  ignition::msgs::Twist twist_msg;
+  ros1_ign_bridge::testing::createTestMsg(twist_msg);
+
   // Publish messages at 1Hz.
   while (!g_terminatePub)
   {
@@ -156,6 +161,7 @@ int main(int /*argc*/, char **/*argv*/)
     magnetic_pub.Publish(magnetometer_msg);
     actuators_pub.Publish(actuators_msg);
     joint_states_pub.Publish(joint_states_msg);
+    twist_pub.Publish(twist_msg);
 
     std::this_thread::sleep_for(std::chrono::milliseconds(100));
   }

diff --git a/test/publishers/ros1_publisher.cpp b/test/publishers/ros1_publisher.cpp
@@ -25,6 +25,7 @@
 #include <geometry_msgs/PoseStamped.h>
 #include <geometry_msgs/Transform.h>
 #include <geometry_msgs/TransformStamped.h>
+#include <geometry_msgs/Twist.h>
 #include <mav_msgs/Actuators.h>
 #include <sensor_msgs/Image.h>
 #include <sensor_msgs/Imu.h>
@@ -103,6 +104,12 @@ int main(int argc, char ** argv)
   geometry_msgs::TransformStamped transform_stamped_msg;
   ros1_ign_bridge::testing::createTestMsg(transform_stamped_msg);
 
+  // geometry_msgs::Twist.
+  ros::Publisher twist_pub =
+    n.advertise<geometry_msgs::Twist>("twist", 1000);
+  geometry_msgs::Twist twist_msg;
+  ros1_ign_bridge::testing::createTestMsg(twist_msg);
+
   // mav_msgs::Actuators.
   ros::Publisher actuators_pub =
     n.advertise<mav_msgs::Actuators>("actuators", 1000);
@@ -153,6 +160,7 @@ int main(int argc, char ** argv)
     pose_stamped_pub.publish(pose_stamped_msg);
     transform_pub.publish(transform_msg);
     transform_stamped_pub.publish(transform_stamped_msg);
+    twist_pub.publish(twist_msg);
     actuators_pub.publish(actuators_msg);
     image_pub.publish(image_msg);
     imu_pub.publish(imu_msg);

diff --git a/test/subscribers/ign_subscriber.cpp b/test/subscribers/ign_subscriber.cpp
@@ -179,6 +179,18 @@ TEST(IgnSubscriberTest, TransformStamped)
   EXPECT_TRUE(client.callbackExecuted);
 }
 
+/////////////////////////////////////////////////
+TEST(IgnSubscriberTest, Twist)
+{
+  MyTestClass<ignition::msgs::Twist> client("twist");
+
+  using namespace std::chrono_literals;
+  ros1_ign_bridge::testing::waitUntilBoolVar(
+    client.callbackExecuted, 10ms, 200);
+
+  EXPECT_TRUE(client.callbackExecuted);
+}
+
 /////////////////////////////////////////////////
 TEST(IgnSubscriberTest, Image)
 {

diff --git a/test/subscribers/ros1_subscriber.cpp b/test/subscribers/ros1_subscriber.cpp
@@ -25,6 +25,7 @@
 #include <geometry_msgs/PoseStamped.h>
 #include <geometry_msgs/Transform.h>
 #include <geometry_msgs/TransformStamped.h>
+#include <geometry_msgs/Twist.h>
 #include <geometry_msgs/Quaternion.h>
 #include <geometry_msgs/Vector3.h>
 #include <mav_msgs/Actuators.h>
@@ -197,6 +198,18 @@ TEST(ROS1SubscriberTest, TransformStamped)
   EXPECT_TRUE(client.callbackExecuted);
 }
 
+/////////////////////////////////////////////////
+TEST(ROS1SubscriberTest, Twist)
+{
+  MyTestClass<geometry_msgs::Twist> client("twist");
+
+  using namespace std::chrono_literals;
+  ros1_ign_bridge::testing::waitUntilBoolVarAndSpin(
+    client.callbackExecuted, 10ms, 200);
+
+  EXPECT_TRUE(client.callbackExecuted);
+}
+
 /////////////////////////////////////////////////
 TEST(ROS1SubscriberTest, Image)
 {

diff --git a/test/test_utils.h b/test/test_utils.h
@@ -28,6 +28,7 @@
 #include <geometry_msgs/PoseStamped.h>
 #include <geometry_msgs/Transform.h>
 #include <geometry_msgs/TransformStamped.h>
+#include <geometry_msgs/Twist.h>
 #include <geometry_msgs/Quaternion.h>
 #include <geometry_msgs/Vector3.h>
 #include <mav_msgs/Actuators.h>
@@ -307,6 +308,25 @@ namespace testing
     EXPECT_EQ(expected_msg.child_frame_id, _msg.child_frame_id);
   }
 
+  /// \brief Create a message used for testing.
+  /// \param[out] _msg The message populated.
+  void createTestMsg(geometry_msgs::Twist &_msg)
+  {
+    createTestMsg(_msg.linear);
+    createTestMsg(_msg.angular);
+  }
+
+  /// \brief Compare a message with the populated for testing.
+  /// \param[in] _msg The message to compare.
+  void compareTestMsg(const geometry_msgs::Twist &_msg)
+  {
+    geometry_msgs::Twist expected_msg;
+    createTestMsg(expected_msg);
+
+    compareTestMsg(_msg.linear);
+    compareTestMsg(_msg.angular);
+  }
+
   /// \brief Create a message used for testing.
   /// \param[out] _msg The message populated.
   void createTestMsg(mav_msgs::Actuators &_msg)
@@ -945,6 +965,31 @@ namespace testing
       EXPECT_EQ(expected_msg.normalized(i), _msg.normalized(i));
     }
   }
+
+  /// \brief Create a message used for testing.
+  /// \param[out] _msg The message populated.
+  void createTestMsg(ignition::msgs::Twist &_msg)
+  {
+    ignition::msgs::Header header_msg;
+    ignition::msgs::Vector3d linear_msg;
+    ignition::msgs::Vector3d angular_msg;
+
+    createTestMsg(header_msg);
+    createTestMsg(linear_msg);
+    createTestMsg(angular_msg);
+
+    _msg.mutable_header()->CopyFrom(header_msg);
+    _msg.mutable_linear()->CopyFrom(linear_msg);
+    _msg.mutable_angular()->CopyFrom(angular_msg);
+  }
+
+  /// \brief Compare a message with the populated for testing.
+  /// \param[in] _msg The message to compare.
+  void compareTestMsg(const ignition::msgs::Twist &_msg)
+  {
+    compareTestMsg(_msg.linear());
+    compareTestMsg(_msg.angular());
+  }
 }
 }