Add support for nav_msgs/OccupancyGrid

ros_ign_bridge/README.md

@@ -26,6 +26,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/OccupancyGrid         | ignition::msgs::OccupancyGrid    |
 | nav_msgs/Odometry              | ignition::msgs::Odometry         |
 | rosgraph_msgs/Clock            | ignition::msgs::Clock            |
 | sensor_msgs/BatteryState       | ignition::msgs::BatteryState     |

ros_ign_bridge/include/ros_ign_bridge/convert.hpp

@@ -27,6 +27,7 @@
 #include <geometry_msgs/TransformStamped.h>
 #include <geometry_msgs/Twist.h>
 #include <mav_msgs/Actuators.h>
+#include <nav_msgs/OccupancyGrid.h>
 #include <nav_msgs/Odometry.h>
 #include <sensor_msgs/BatteryState.h>
 #include <sensor_msgs/CameraInfo.h>
@@ -302,6 +303,18 @@ convert_ign_to_ros(
   mav_msgs::Actuators & ros_msg);
 
 // nav_msgs
+template<>
+void
+convert_ros_to_ign(
+  const nav_msgs::OccupancyGrid & ros_msg,
+  ignition::msgs::OccupancyGrid & ign_msg);
+
+template<>
+void
+convert_ign_to_ros(
+  const ignition::msgs::OccupancyGrid& ign_msg,
+  nav_msgs::OccupancyGrid & ros_msg);
+
 template<>
 void
 convert_ros_to_ign(

ros_ign_bridge/src/convert.cpp

@@ -534,6 +534,52 @@ convert_ign_to_ros(
     ros_msg.normalized.push_back(ign_msg.normalized(i));
 }
 
+template<>
+void
+convert_ros_to_ign(
+  const nav_msgs::OccupancyGrid & ros_msg,
+  ignition::msgs::OccupancyGrid & ign_msg)
+{
+  convert_ros_to_ign(ros_msg.header, (*ign_msg.mutable_header()));
+
+  ign_msg.mutable_info()->mutable_map_load_time()->set_sec(
+      ros_msg.info.map_load_time.sec);
+  ign_msg.mutable_info()->mutable_map_load_time()->set_nsec(
+      ros_msg.info.map_load_time.nsec);
+
+  ign_msg.mutable_info()->set_resolution(
+      ros_msg.info.resolution);
+  ign_msg.mutable_info()->set_width(
+      ros_msg.info.width);
+  ign_msg.mutable_info()->set_height(
+      ros_msg.info.height);
+
+  convert_ros_to_ign(ros_msg.info.origin, 
+      (*ign_msg.mutable_info()->mutable_origin()));
+
+  ign_msg.set_data(&ros_msg.data[0], ros_msg.data.size());
+}
+
+template<>
+void
+convert_ign_to_ros(
+  const ignition::msgs::OccupancyGrid & ign_msg,
+  nav_msgs::OccupancyGrid & ros_msg)
+{
+  convert_ign_to_ros(ign_msg.header(), ros_msg.header);
+
+  ros_msg.info.map_load_time.sec = ign_msg.info().map_load_time().sec();
+  ros_msg.info.map_load_time.nsec = ign_msg.info().map_load_time().nsec();
+  ros_msg.info.resolution = ign_msg.info().resolution();
+  ros_msg.info.width = ign_msg.info().width();
+  ros_msg.info.height = ign_msg.info().height();
+
+  convert_ign_to_ros(ign_msg.info().origin(), ros_msg.info.origin);
+
+  ros_msg.data.resize(ign_msg.data().size());
+  memcpy(&ros_msg.data[0], ign_msg.data().c_str(), ign_msg.data().size());
+}
+
 template<>
 void
 convert_ros_to_ign(

ros_ign_bridge/src/factories.cpp

@@ -247,6 +247,17 @@ get_factory_impl(
       >
     >("mav_msgs/Actuators", ign_type_name);
   }
+  if (
+    (ros_type_name == "nav_msgs/OccupancyGrid" || ros_type_name == "") &&
+     ign_type_name == "ignition.msgs.OccupancyGrid")
+  {
+    return std::make_shared<
+      Factory<
+        nav_msgs::OccupancyGrid,
+        ignition::msgs::OccupancyGrid
+      >
+    >("nav_msgs/OccupancyGrid", ign_type_name);
+  }
   if (
     (ros_type_name == "nav_msgs/Odometry" || ros_type_name == "") &&
      ign_type_name == "ignition.msgs.Odometry")
@@ -361,7 +372,6 @@ get_factory_impl(
     (ros_type_name == "visualization_msgs/Marker" || ros_type_name == "") &&
      ign_type_name == "ignition.msgs.Marker")
   {
-    ROS_ERROR_STREAM("visualization_msgs/Marker");
     return std::make_shared<
       Factory<
         visualization_msgs::Marker,
@@ -373,7 +383,6 @@ get_factory_impl(
     (ros_type_name == "visualization_msgs/MarkerArray" || ros_type_name == "") &&
      ign_type_name == "ignition.msgs.Marker_V")
   {
-    ROS_ERROR_STREAM("visualization_msgs/MarkerArray");
     return std::make_shared<
       Factory<
         visualization_msgs::MarkerArray,
@@ -883,6 +892,30 @@ Factory<
 }
 
 // nav_msgs
+template<>
+void
+Factory<
+  nav_msgs::OccupancyGrid,
+  ignition::msgs::OccupancyGrid
+>::convert_ros_to_ign(
+  const nav_msgs::OccupancyGrid & ros_msg,
+  ignition::msgs::OccupancyGrid & ign_msg)
+{
+  ros_ign_bridge::convert_ros_to_ign(ros_msg, ign_msg);
+}
+
+template<>
+void
+Factory<
+  nav_msgs::OccupancyGrid,
+  ignition::msgs::OccupancyGrid
+>::convert_ign_to_ros(
+  const ignition::msgs::OccupancyGrid & ign_msg,
+  nav_msgs::OccupancyGrid & ros_msg)
+{
+  ros_ign_bridge::convert_ign_to_ros(ign_msg, ros_msg);
+}
+
 template<>
 void
 Factory<

ros_ign_bridge/src/factories.hpp

@@ -26,6 +26,7 @@
 #include <geometry_msgs/Twist.h>
 #include <geometry_msgs/Vector3.h>
 #include <mav_msgs/Actuators.h>
+#include <nav_msgs/OccupancyGrid.h>
 #include <nav_msgs/Odometry.h>
 #include <rosgraph_msgs/Clock.h>
 #include <sensor_msgs/BatteryState.h>
@@ -430,6 +431,24 @@ Factory<
   mav_msgs::Actuators & ros_msg);
 
 // nav_msgs
+template<>
+void
+Factory<
+  nav_msgs::OccupancyGrid,
+  ignition::msgs::OccupancyGrid
+>::convert_ros_to_ign(
+  const nav_msgs::OccupancyGrid & ros_msg,
+  ignition::msgs::OccupancyGrid & ign_msg);
+
+template<>
+void
+Factory<
+  nav_msgs::OccupancyGrid,
+  ignition::msgs::OccupancyGrid
+>::convert_ign_to_ros(
+  const ignition::msgs::OccupancyGrid & ign_msg,
+  nav_msgs::OccupancyGrid & ros_msg);
+
 template<>
 void
 Factory<

ros_ign_bridge/test/launch/test_ign_subscriber.launch

@@ -29,6 +29,7 @@
               /laserscan@sensor_msgs/LaserScan@ignition.msgs.LaserScan
               /magnetic@sensor_msgs/MagneticField@ignition.msgs.Magnetometer
               /actuators@mav_msgs/Actuators@ignition.msgs.Actuators
+              /map@nav_msgs/OccupancyGrid@ignition.msgs.OccupancyGrid
               /odometry@nav_msgs/Odometry@ignition.msgs.Odometry
               /pointcloud2@sensor_msgs/PointCloud2@ignition.msgs.PointCloudPacked
               /joint_states@sensor_msgs/JointState@ignition.msgs.Model

ros_ign_bridge/test/launch/test_ros_subscriber.launch

@@ -29,6 +29,7 @@
               /laserscan@sensor_msgs/LaserScan@ignition.msgs.LaserScan
               /magnetic@sensor_msgs/MagneticField@ignition.msgs.Magnetometer
               /actuators@mav_msgs/Actuators@ignition.msgs.Actuators
+              /map@nav_msgs/OccupancyGrid@ignition.msgs.OccupancyGrid
               /odometry@nav_msgs/Odometry@ignition.msgs.Odometry
               /pointcloud2@sensor_msgs/PointCloud2@ignition.msgs.PointCloudPacked
               /joint_states@sensor_msgs/JointState@ignition.msgs.Model

ros_ign_bridge/test/publishers/ign_publisher.cpp

@@ -176,6 +176,11 @@ int main(int /*argc*/, char **/*argv*/)
   ignition::msgs::Actuators actuators_msg;
   ros_ign_bridge::testing::createTestMsg(actuators_msg);
 
+  // ignition::msgs::OccupancyGrid
+  auto map_pub = node.Advertise<ignition::msgs::OccupancyGrid>("map");
+  ignition::msgs::OccupancyGrid map_msg;
+  ros_ign_bridge::testing::createTestMsg(map_msg);
+
   // ignition::msgs::Odometry.
   auto odometry_pub = node.Advertise<ignition::msgs::Odometry>("odometry");
   ignition::msgs::Odometry odometry_msg;
@@ -238,6 +243,7 @@ int main(int /*argc*/, char **/*argv*/)
     laserscan_pub.Publish(laserscan_msg);
     magnetic_pub.Publish(magnetometer_msg);
     actuators_pub.Publish(actuators_msg);
+    map_pub.Publish(map_msg);
     odometry_pub.Publish(odometry_msg);
     joint_states_pub.Publish(joint_states_msg);
     twist_pub.Publish(twist_msg);

ros_ign_bridge/test/publishers/ros_publisher.cpp

@@ -32,6 +32,7 @@
 #include <geometry_msgs/TransformStamped.h>
 #include <geometry_msgs/Twist.h>
 #include <mav_msgs/Actuators.h>
+#include <nav_msgs/OccupancyGrid.h>
 #include <nav_msgs/Odometry.h>
 #include <sensor_msgs/BatteryState.h>
 #include <sensor_msgs/CameraInfo.h>
@@ -165,6 +166,12 @@ int main(int argc, char ** argv)
   mav_msgs::Actuators actuators_msg;
   ros_ign_bridge::testing::createTestMsg(actuators_msg);
 
+  // nav_msgs::OccupancyGrid.
+  ros::Publisher map_pub =
+    n.advertise<nav_msgs::OccupancyGrid>("map", 1000);
+  nav_msgs::OccupancyGrid map_msg;
+  ros_ign_bridge::testing::createTestMsg(map_msg);
+
   // nav_msgs::Odometry.
   ros::Publisher odometry_pub =
     n.advertise<nav_msgs::Odometry>("odometry", 1000);
@@ -260,6 +267,7 @@ int main(int argc, char ** argv)
     tf2_message_pub.publish(tf2_msg);
     twist_pub.publish(twist_msg);
     actuators_pub.publish(actuators_msg);
+    map_pub.publish(map_msg);
     odometry_pub.publish(odometry_msg);
     image_pub.publish(image_msg);
     camera_info_pub.publish(camera_info_msg);

ros_ign_bridge/test/subscribers/ign_subscriber.cpp

@@ -347,6 +347,18 @@ TEST(IgnSubscriberTest, Actuators)
   EXPECT_TRUE(client.callbackExecuted);
 }
 
+/////////////////////////////////////////////////
+TEST(IgnSubscriberTest, OccupancyGrid)
+{
+  MyTestClass<ignition::msgs::OccupancyGrid> client("map");
+
+  using namespace std::chrono_literals;
+  ros_ign_bridge::testing::waitUntilBoolVar(
+    client.callbackExecuted, 10ms, 200);
+
+  EXPECT_TRUE(client.callbackExecuted);
+}
+
 /////////////////////////////////////////////////
 TEST(IgnSubscriberTest, Odometry)
 {

ros_ign_bridge/test/subscribers/ros_subscriber.cpp

@@ -34,6 +34,7 @@
 #include <geometry_msgs/Quaternion.h>
 #include <geometry_msgs/Vector3.h>
 #include <mav_msgs/Actuators.h>
+#include <nav_msgs/OccupancyGrid.h>
 #include <nav_msgs/Odometry.h>
 #include <rosgraph_msgs/Clock.h>
 #include <sensor_msgs/BatteryState.h>
@@ -395,6 +396,18 @@ TEST(ROSSubscriberTest, Actuators)
 {
   MyTestClass<mav_msgs::Actuators> client("actuators");
 
+  using namespace std::chrono_literals;
+  ros_ign_bridge::testing::waitUntilBoolVarAndSpin(
+    client.callbackExecuted, 10ms, 200);
+
+  EXPECT_TRUE(client.callbackExecuted); 
+}
+
+/////////////////////////////////////////////////
+TEST(ROSSubscriberTest, OccupancyGrid)
+{
+  MyTestClass<nav_msgs::OccupancyGrid> client("map");
+
   using namespace std::chrono_literals;
   ros_ign_bridge::testing::waitUntilBoolVarAndSpin(
     client.callbackExecuted, 10ms, 200);