Conversion between nav_msgs/Odometry and ignition::msgs::Odometry (#22)

* Conversion between nav_msgs/Odometry and ignition::msgs::Odometry. * Update documentation. * More time to run tests * Cleaning test_utils. * Remove explicit ROS dependencies for Travis. * diff drive demo with cmd_vel and odom * process child frame id
gazebosim · Jun 28, 2019 · 4b07992 · 4b07992
1 parent 98db845
commit 4b07992
Show file tree

Hide file tree

Showing 20 changed files with 409 additions and 44 deletions.
diff --git a/.travis/build b/.travis/build
@@ -15,11 +15,8 @@ apt-get install -qq -y libignition-msgs4-dev \
                        libignition-transport7-dev \
                        libignition-gazebo2-dev \
                        python-catkin-tools \
-                       python-rosdep \
-                       ros-melodic-geometry-msgs \
-                       ros-melodic-mav-msgs \
-                       ros-melodic-rostest \
-                       ros-melodic-sensor-msgs
+                       python-rosdep
+
 rosdep init
 rosdep update
 rosdep install --from-paths ./ -i -y --rosdistro melodic \

diff --git a/ros1_ign_bridge/README.md b/ros1_ign_bridge/README.md
@@ -20,6 +20,7 @@ service calls.Its support is limited to only the following message types:
 | geometry_msgs/TransformStamped | ignition::msgs::Pose         |
 | geometry_msgs/Twist            | ignition::msgs::Twist        |
 | mav_msgs/Actuators             | ignition::msgs::Actuators    |
+| nav_msgs/Odometry              | ignition::msgs::Odometry     |
 | rosgraph_msgs/Clock            | ignition::msgs::Clock        |
 | sensor_msgs/CameraInfo         | ignition::msgs::CameraInfo   |
 | sensor_msgs/Imu                | ignition::msgs::IMU          |
@@ -41,6 +42,7 @@ The following ROS 1 packages are required to build and use the bridge:
 * `catkin`
 * `geometry_msgs`
 * `mav_msgs`
+* `nav_msgs`
 * `roscpp`
 * `roslaunch` (for `roscore` executable)
 * `rosmsg`

diff --git a/ros1_ign_bridge/include/ros1_ign_bridge/builtin_interfaces_factories.hpp b/ros1_ign_bridge/include/ros1_ign_bridge/builtin_interfaces_factories.hpp
@@ -25,6 +25,7 @@
 #include <geometry_msgs/Twist.h>
 #include <geometry_msgs/Vector3.h>
 #include <mav_msgs/Actuators.h>
+#include <nav_msgs/Odometry.h>
 #include <rosgraph_msgs/Clock.h>
 #include <sensor_msgs/CameraInfo.h>
 #include <sensor_msgs/FluidPressure.h>
@@ -298,6 +299,25 @@ Factory<
   const ignition::msgs::Actuators & ign_msg,
   mav_msgs::Actuators & ros1_msg);
 
+// nav_msgs
+template<>
+void
+Factory<
+  nav_msgs::Odometry,
+  ignition::msgs::Odometry
+>::convert_1_to_ign(
+  const nav_msgs::Odometry & ros1_msg,
+  ignition::msgs::Odometry & ign_msg);
+
+template<>
+void
+Factory<
+  nav_msgs::Odometry,
+  ignition::msgs::Odometry
+>::convert_ign_to_1(
+  const ignition::msgs::Odometry & ign_msg,
+  nav_msgs::Odometry & ros1_msg);
+
 // sensor_msgs
 template<>
 void

diff --git a/ros1_ign_bridge/include/ros1_ign_bridge/convert_builtin_interfaces.hpp b/ros1_ign_bridge/include/ros1_ign_bridge/convert_builtin_interfaces.hpp
@@ -26,6 +26,7 @@
 #include <geometry_msgs/TransformStamped.h>
 #include <geometry_msgs/Twist.h>
 #include <mav_msgs/Actuators.h>
+#include <nav_msgs/Odometry.h>
 #include <sensor_msgs/CameraInfo.h>
 #include <sensor_msgs/FluidPressure.h>
 #include <sensor_msgs/Image.h>
@@ -206,6 +207,19 @@ convert_ign_to_1(
   const ignition::msgs::Actuators & ign_msg,
   mav_msgs::Actuators & ros1_msg);
 
+// nav_msgs
+template<>
+void
+convert_1_to_ign(
+  const nav_msgs::Odometry & ros1_msg,
+  ignition::msgs::Odometry & ign_msg);
+
+template<>
+void
+convert_ign_to_1(
+  const ignition::msgs::Odometry & ign_msg,
+  nav_msgs::Odometry & ros1_msg);
+
 // sensor_msgs
 template<>
 void

diff --git a/ros1_ign_bridge/package.xml b/ros1_ign_bridge/package.xml
@@ -11,6 +11,7 @@
   <depend>ignition-msgs4</depend>
   <depend>ignition-transport7</depend>
   <depend>mav_msgs</depend>
+  <depend>nav_msgs</depend>
   <depend>rosgraph_msgs</depend>
   <depend>roscpp</depend>
   <depend>sensor_msgs</depend>

diff --git a/ros1_ign_bridge/src/builtin_interfaces_factories.cpp b/ros1_ign_bridge/src/builtin_interfaces_factories.cpp
@@ -171,6 +171,17 @@ get_factory_builtin_interfaces(
       >
     >("mav_msgs/Actuators", ign_type_name);
   }
+  if (
+    (ros1_type_name == "nav_msgs/Odometry" || ros1_type_name == "") &&
+     ign_type_name == "ignition.msgs.Odometry")
+  {
+    return std::make_shared<
+      Factory<
+        nav_msgs::Odometry,
+        ignition::msgs::Odometry
+      >
+    >("nav_msgs/Odometry", ign_type_name);
+  }
   if (
     (ros1_type_name == "sensor_msgs/FluidPressure" || ros1_type_name == "") &&
      ign_type_name == "ignition.msgs.FluidPressure")
@@ -592,6 +603,31 @@ Factory<
   ros1_ign_bridge::convert_ign_to_1(ign_msg, ros1_msg);
 }
 
+// nav_msgs
+template<>
+void
+Factory<
+  nav_msgs::Odometry,
+  ignition::msgs::Odometry
+>::convert_1_to_ign(
+  const nav_msgs::Odometry & ros1_msg,
+  ignition::msgs::Odometry & ign_msg)
+{
+  ros1_ign_bridge::convert_1_to_ign(ros1_msg, ign_msg);
+}
+
+template<>
+void
+Factory<
+  nav_msgs::Odometry,
+  ignition::msgs::Odometry
+>::convert_ign_to_1(
+  const ignition::msgs::Odometry & ign_msg,
+  nav_msgs::Odometry & ros1_msg)
+{
+  ros1_ign_bridge::convert_ign_to_1(ign_msg, ros1_msg);
+}
+
 // sensor_msgs
 template<>
 void

diff --git a/ros1_ign_bridge/src/convert_builtin_interfaces.cpp b/ros1_ign_bridge/src/convert_builtin_interfaces.cpp
@@ -372,6 +372,42 @@ convert_ign_to_1(
     ros1_msg.normalized.push_back(ign_msg.normalized(i));
 }
 
+template<>
+void
+convert_1_to_ign(
+  const nav_msgs::Odometry & ros1_msg,
+  ignition::msgs::Odometry & ign_msg)
+{
+  convert_1_to_ign(ros1_msg.header, (*ign_msg.mutable_header()));
+  convert_1_to_ign(ros1_msg.pose.pose, (*ign_msg.mutable_pose()));
+  convert_1_to_ign(ros1_msg.twist.twist, (*ign_msg.mutable_twist()));
+
+  auto childFrame = ign_msg.mutable_header()->add_data();
+  childFrame->set_key("child_frame_id");
+  childFrame->add_value(ros1_msg.child_frame_id);
+}
+
+template<>
+void
+convert_ign_to_1(
+  const ignition::msgs::Odometry & ign_msg,
+  nav_msgs::Odometry & ros1_msg)
+{
+  convert_ign_to_1(ign_msg.header(), ros1_msg.header);
+  convert_ign_to_1(ign_msg.pose(), ros1_msg.pose.pose);
+  convert_ign_to_1(ign_msg.twist(), ros1_msg.twist.twist);
+
+  for (auto i = 0; i < ign_msg.header().data_size(); ++i)
+  {
+    auto aPair = ign_msg.header().data(i);
+    if (aPair.key() == "child_frame_id" && aPair.value_size() > 0)
+    {
+      ros1_msg.child_frame_id = frame_id_ign_to_1(aPair.value(0));
+      break;
+    }
+  }
+}
+
 template<>
 void
 convert_1_to_ign(

diff --git a/ros1_ign_bridge/test/ign_subscriber.test b/ros1_ign_bridge/test/ign_subscriber.test
@@ -4,6 +4,6 @@
   <include file="$(find ros1_ign_bridge)/test/launch/test_ign_subscriber.launch">
   </include>
 
-  <test test-name="ros1_ign" pkg="ros1_ign_bridge" type="test_ign_subscriber" time-limit="20.0" />
+  <test test-name="ros1_ign" pkg="ros1_ign_bridge" type="test_ign_subscriber" time-limit="60.0" />
 
 </launch>
diff --git a/ros1_ign_bridge/test/launch/test_ign_subscriber.launch b/ros1_ign_bridge/test/launch/test_ign_subscriber.launch
@@ -22,6 +22,7 @@
               /laserscan@sensor_msgs/[email protected]
               /magnetic@sensor_msgs/[email protected]
               /actuators@mav_msgs/[email protected]
+              /odometry@nav_msgs/[email protected]
               /joint_states@sensor_msgs/[email protected]"
   />
 

diff --git a/ros1_ign_bridge/test/launch/test_ros1_subscriber.launch b/ros1_ign_bridge/test/launch/test_ros1_subscriber.launch
@@ -22,6 +22,7 @@
               /laserscan@sensor_msgs/[email protected]
               /magnetic@sensor_msgs/[email protected]
               /actuators@mav_msgs/[email protected]
+              /odometry@nav_msgs/[email protected]
               /joint_states@sensor_msgs/[email protected]"
   />
 

diff --git a/ros1_ign_bridge/test/publishers/ign_publisher.cpp b/ros1_ign_bridge/test/publishers/ign_publisher.cpp
@@ -141,6 +141,11 @@ int main(int /*argc*/, char **/*argv*/)
   ignition::msgs::Actuators actuators_msg;
   ros1_ign_bridge::testing::createTestMsg(actuators_msg);
 
+  // ignition::msgs::Odometry.
+  auto odometry_pub = node.Advertise<ignition::msgs::Odometry>("odometry");
+  ignition::msgs::Odometry odometry_msg;
+  ros1_ign_bridge::testing::createTestMsg(odometry_msg);
+
   // ignition::msgs::Model.
   auto joint_states_pub = node.Advertise<ignition::msgs::Model>("joint_states");
   ignition::msgs::Model joint_states_msg;
@@ -172,6 +177,7 @@ int main(int /*argc*/, char **/*argv*/)
     laserscan_pub.Publish(laserscan_msg);
     magnetic_pub.Publish(magnetometer_msg);
     actuators_pub.Publish(actuators_msg);
+    odometry_pub.Publish(odometry_msg);
     joint_states_pub.Publish(joint_states_msg);
     twist_pub.Publish(twist_msg);
 

diff --git a/ros1_ign_bridge/test/publishers/ros1_publisher.cpp b/ros1_ign_bridge/test/publishers/ros1_publisher.cpp
@@ -27,6 +27,7 @@
 #include <geometry_msgs/TransformStamped.h>
 #include <geometry_msgs/Twist.h>
 #include <mav_msgs/Actuators.h>
+#include <nav_msgs/Odometry.h>
 #include <sensor_msgs/CameraInfo.h>
 #include <sensor_msgs/FluidPressure.h>
 #include <sensor_msgs/Image.h>
@@ -118,6 +119,12 @@ int main(int argc, char ** argv)
   mav_msgs::Actuators actuators_msg;
   ros1_ign_bridge::testing::createTestMsg(actuators_msg);
 
+  // nav_msgs::Odometry.
+  ros::Publisher odometry_pub =
+    n.advertise<nav_msgs::Odometry>("odometry", 1000);
+  nav_msgs::Odometry odometry_msg;
+  ros1_ign_bridge::testing::createTestMsg(odometry_msg);
+
   // sensor_msgs::Image.
   ros::Publisher image_pub =
     n.advertise<sensor_msgs::Image>("image", 1000);
@@ -176,6 +183,7 @@ int main(int argc, char ** argv)
     transform_stamped_pub.publish(transform_stamped_msg);
     twist_pub.publish(twist_msg);
     actuators_pub.publish(actuators_msg);
+    odometry_pub.publish(odometry_msg);
     image_pub.publish(image_msg);
     camera_info_pub.publish(camera_info_msg);
     fluid_pressure_pub.publish(fluid_pressure_msg);

diff --git a/ros1_ign_bridge/test/ros1_subscriber.test b/ros1_ign_bridge/test/ros1_subscriber.test
@@ -4,6 +4,6 @@
   <include file="$(find ros1_ign_bridge)/test/launch/test_ros1_subscriber.launch">
   </include>
 
-  <test test-name="ign_ros1" pkg="ros1_ign_bridge" type="test_ros1_subscriber" time-limit="10.0" />
+  <test test-name="ign_ros1" pkg="ros1_ign_bridge" type="test_ros1_subscriber" time-limit="60.0" />
 
 </launch>
diff --git a/ros1_ign_bridge/test/subscribers/ign_subscriber.cpp b/ros1_ign_bridge/test/subscribers/ign_subscriber.cpp
@@ -275,6 +275,18 @@ TEST(IgnSubscriberTest, Actuators)
   EXPECT_TRUE(client.callbackExecuted);
 }
 
+/////////////////////////////////////////////////
+TEST(IgnSubscriberTest, Odometry)
+{
+  MyTestClass<ignition::msgs::Odometry> client("odometry");
+
+  using namespace std::chrono_literals;
+  ros1_ign_bridge::testing::waitUntilBoolVar(
+    client.callbackExecuted, 10ms, 200);
+
+  EXPECT_TRUE(client.callbackExecuted);
+}
+
 /////////////////////////////////////////////////
 TEST(IgnSubscriberTest, JointStates)
 {

diff --git a/ros1_ign_bridge/test/subscribers/ros1_subscriber.cpp b/ros1_ign_bridge/test/subscribers/ros1_subscriber.cpp
@@ -29,6 +29,7 @@
 #include <geometry_msgs/Quaternion.h>
 #include <geometry_msgs/Vector3.h>
 #include <mav_msgs/Actuators.h>
+#include <nav_msgs/Odometry.h>
 #include <rosgraph_msgs/Clock.h>
 #include <sensor_msgs/CameraInfo.h>
 #include <sensor_msgs/Image.h>
@@ -307,6 +308,18 @@ TEST(ROS1SubscriberTest, Actuators)
   EXPECT_TRUE(client.callbackExecuted);
 }
 
+/////////////////////////////////////////////////
+TEST(ROS1SubscriberTest, Odometry)
+{
+  MyTestClass<nav_msgs::Odometry> client("odometry");
+
+  using namespace std::chrono_literals;
+  ros1_ign_bridge::testing::waitUntilBoolVarAndSpin(
+    client.callbackExecuted, 10ms, 200);
+
+  EXPECT_TRUE(client.callbackExecuted);
+}
+
 /////////////////////////////////////////////////
 int main(int argc, char **argv)
 {