Convert between ign Pose and ROS Pose[Stamped]/Transform[Stamped] msgs (

#4)

README.md

@@ -8,16 +8,21 @@ between ROS 1 and Ignition Transport.
 The bridge is currently implemented in C++. At this point there's no support for
 service calls.Its support is limited to only the following message types:
 
-| ROS 1 type                  | Ignition Transport type      |
-|-----------------------------|:----------------------------:|
-| std_msgs/Header             | ignition::msgs::Header       |
-| std_msgs/String             | ignition::msgs::StringMsg    |
-| geometry_msgs/Quaternion    | ignition::msgs::Quaternion   |
-| geometry_msgs/Vector3       | ignition::msgs::Vector3d     |
-| sensor_msgs/Imu             | ignition::msgs::IMU          |
-| sensor_msgs/Image           | ignition::msgs::Image        |
-| sensor_msgs/LaserScan       | ignition::msgs::LaserScan    |
-| sensor_msgs/MagneticField   | ignition::msgs::Magnetometer |
+| ROS 1 type                     | Ignition Transport type      |
+|--------------------------------|:----------------------------:|
+| std_msgs/Header                | ignition::msgs::Header       |
+| std_msgs/String                | ignition::msgs::StringMsg    |
+| geometry_msgs/Quaternion       | ignition::msgs::Quaternion   |
+| geometry_msgs/Vector3          | ignition::msgs::Vector3d     |
+| geometry_msgs/Point            | ignition::msgs::Vector3d     |
+| geometry_msgs/Pose             | ignition::msgs::Pose         |
+| geometry_msgs/PoseStamped      | ignition::msgs::Pose         |
+| geometry_msgs/Transform        | ignition::msgs::Pose         |
+| geometry_msgs/TransformStamped | ignition::msgs::Pose         |
+| sensor_msgs/Imu                | ignition::msgs::IMU          |
+| sensor_msgs/Image              | ignition::msgs::Image        |
+| sensor_msgs/LaserScan          | ignition::msgs::LaserScan    |
+| sensor_msgs/MagneticField      | ignition::msgs::Magnetometer |
 
 Run `parameter_bridge -h` for instructions.
 

include/ros1_ign_bridge/builtin_interfaces_factories.hpp

@@ -17,6 +17,11 @@
 
 // include ROS 1 messages
 #include <geometry_msgs/Quaternion.h>
+#include <geometry_msgs/Point.h>
+#include <geometry_msgs/Pose.h>
+#include <geometry_msgs/PoseStamped.h>
+#include <geometry_msgs/Transform.h>
+#include <geometry_msgs/TransformStamped.h>
 #include <geometry_msgs/Vector3.h>
 #include <sensor_msgs/FluidPressure.h>
 #include <sensor_msgs/Image.h>
@@ -123,6 +128,96 @@ Factory<
   const ignition::msgs::Vector3d & ign_msg,
   geometry_msgs::Vector3 & ros1_msg);
 
+template<>
+void
+Factory<
+  geometry_msgs::Point,
+  ignition::msgs::Vector3d
+>::convert_1_to_ign(
+  const geometry_msgs::Point & ros1_msg,
+  ignition::msgs::Vector3d & ign_msg);
+
+template<>
+void
+Factory<
+  geometry_msgs::Point,
+  ignition::msgs::Vector3d
+>::convert_ign_to_1(
+  const ignition::msgs::Vector3d & ign_msg,
+  geometry_msgs::Point & ros1_msg);
+
+template<>
+void
+Factory<
+  geometry_msgs::Pose,
+  ignition::msgs::Pose
+>::convert_1_to_ign(
+  const geometry_msgs::Pose & ros1_msg,
+  ignition::msgs::Pose & ign_msg);
+
+template<>
+void
+Factory<
+  geometry_msgs::Pose,
+  ignition::msgs::Pose
+>::convert_ign_to_1(
+  const ignition::msgs::Pose & ign_msg,
+  geometry_msgs::Pose & ros1_msg);
+
+template<>
+void
+Factory<
+  geometry_msgs::PoseStamped,
+  ignition::msgs::Pose
+>::convert_1_to_ign(
+  const geometry_msgs::PoseStamped & ros1_msg,
+  ignition::msgs::Pose & ign_msg);
+
+template<>
+void
+Factory<
+  geometry_msgs::PoseStamped,
+  ignition::msgs::Pose
+>::convert_ign_to_1(
+  const ignition::msgs::Pose & ign_msg,
+  geometry_msgs::PoseStamped & ros1_msg);
+
+template<>
+void
+Factory<
+  geometry_msgs::Transform,
+  ignition::msgs::Pose
+>::convert_1_to_ign(
+  const geometry_msgs::Transform & ros1_msg,
+  ignition::msgs::Pose & ign_msg);
+
+template<>
+void
+Factory<
+  geometry_msgs::Transform,
+  ignition::msgs::Pose
+>::convert_ign_to_1(
+  const ignition::msgs::Pose & ign_msg,
+  geometry_msgs::Transform & ros1_msg);
+
+template<>
+void
+Factory<
+  geometry_msgs::TransformStamped,
+  ignition::msgs::Pose
+>::convert_1_to_ign(
+  const geometry_msgs::TransformStamped & ros1_msg,
+  ignition::msgs::Pose & ign_msg);
+
+template<>
+void
+Factory<
+  geometry_msgs::TransformStamped,
+  ignition::msgs::Pose
+>::convert_ign_to_1(
+  const ignition::msgs::Pose & ign_msg,
+  geometry_msgs::TransformStamped & ros1_msg);
+
 // sensor_msgs
 template<>
 void

include/ros1_ign_bridge/convert_builtin_interfaces.hpp

@@ -17,7 +17,12 @@
 
 // include ROS 1 builtin messages
 #include <geometry_msgs/Quaternion.h>
+#include <geometry_msgs/Point.h>
+#include <geometry_msgs/Pose.h>
+#include <geometry_msgs/PoseStamped.h>
 #include <geometry_msgs/Vector3.h>
+#include <geometry_msgs/Transform.h>
+#include <geometry_msgs/TransformStamped.h>
 #include <sensor_msgs/FluidPressure.h>
 #include <sensor_msgs/Image.h>
 #include <sensor_msgs/Imu.h>
@@ -85,6 +90,66 @@ convert_ign_to_1(
   const ignition::msgs::Vector3d & ign_msg,
   geometry_msgs::Vector3 & ros1_msg);
 
+template<>
+void
+convert_1_to_ign(
+  const geometry_msgs::Point & ros1_msg,
+  ignition::msgs::Vector3d & ign_msg);
+
+template<>
+void
+convert_ign_to_1(
+  const ignition::msgs::Vector3d & ign_msg,
+  geometry_msgs::Point & ros1_msg);
+
+template<>
+void
+convert_1_to_ign(
+  const geometry_msgs::Pose & ros1_msg,
+  ignition::msgs::Pose & ign_msg);
+
+template<>
+void
+convert_ign_to_1(
+  const ignition::msgs::Pose & ign_msg,
+  geometry_msgs::Pose & ros1_msg);
+
+template<>
+void
+convert_1_to_ign(
+  const geometry_msgs::PoseStamped & ros1_msg,
+  ignition::msgs::Pose & ign_msg);
+
+template<>
+void
+convert_ign_to_1(
+  const ignition::msgs::Pose & ign_msg,
+  geometry_msgs::PoseStamped & ros1_msg);
+
+template<>
+void
+convert_1_to_ign(
+  const geometry_msgs::Transform & ros1_msg,
+  ignition::msgs::Pose & ign_msg);
+
+template<>
+void
+convert_ign_to_1(
+  const ignition::msgs::Pose & ign_msg,
+  geometry_msgs::Transform & ros1_msg);
+
+template<>
+void
+convert_1_to_ign(
+  const geometry_msgs::TransformStamped & ros1_msg,
+  ignition::msgs::Pose & ign_msg);
+
+template<>
+void
+convert_ign_to_1(
+  const ignition::msgs::Pose & ign_msg,
+  geometry_msgs::TransformStamped & ros1_msg);
+
 // sensor_msgs
 template<>
 void