From da044a55500c838903043fd80219eccc8cb6e0d7 Mon Sep 17 00:00:00 2001
From: Louise Poubel <louise@osrfoundation.org>
Date: Tue, 16 Oct 2018 17:38:40 -0700
Subject: [PATCH] Use image_transport remapping

---
 .../test/test_gazebo_ros_camera.cpp           |  6 ++--
 .../test_gazebo_ros_camera_distortion.cpp     | 14 ++++----
 .../test/test_gazebo_ros_camera_triggered.cpp |  4 +--
 .../test/worlds/gazebo_ros_camera.world       |  3 +-
 .../test/worlds/gazebo_ros_camera_16bit.world |  2 +-
 .../gazebo_ros_camera_distortion_barrel.world |  9 ++---
 ...ebo_ros_camera_distortion_pincushion.world | 34 ++++---------------
 .../worlds/gazebo_ros_camera_triggered.world  |  7 ++--
 8 files changed, 24 insertions(+), 55 deletions(-)

diff --git a/gazebo_plugins/test/test_gazebo_ros_camera.cpp b/gazebo_plugins/test/test_gazebo_ros_camera.cpp
index d115b089b..773991e1d 100644
--- a/gazebo_plugins/test/test_gazebo_ros_camera.cpp
+++ b/gazebo_plugins/test/test_gazebo_ros_camera.cpp
@@ -76,12 +76,10 @@ TEST_P(GazeboRosCameraTest, CameraSubscribeTest)
 }
 
 INSTANTIATE_TEST_CASE_P(GazeboRosCamera, GazeboRosCameraTest, ::testing::Values(
-    // TODO(louise) Use mapped topics once this issue is solved:
-    // https://github.com/ros-perception/image_common/issues/93
     TestParams({"worlds/gazebo_ros_camera.world",
-      "test_cam/camera1/image_raw"}),
+      "test_cam/camera/image_test"}),
     TestParams({"worlds/gazebo_ros_camera_16bit.world",
-      "test_cam_16bit/test_camera_name/image_raw"})
+      "test_cam_16bit/image_test_16bit"})
   ), );
 
 int main(int argc, char ** argv)
diff --git a/gazebo_plugins/test/test_gazebo_ros_camera_distortion.cpp b/gazebo_plugins/test/test_gazebo_ros_camera_distortion.cpp
index d8bcd5351..f5c16103c 100644
--- a/gazebo_plugins/test/test_gazebo_ros_camera_distortion.cpp
+++ b/gazebo_plugins/test/test_gazebo_ros_camera_distortion.cpp
@@ -200,18 +200,16 @@ TEST_P(GazeboRosCameraDistortionTest, CameraSubscribeTest)
 
 INSTANTIATE_TEST_CASE_P(GazeboRosCameraDistortion, GazeboRosCameraDistortionTest,
   ::testing::Values(
-    // TODO(louise) Use mapped topics once this issue is solved:
-    // https://github.com/ros-perception/image_common/issues/93
     TestParams({
   "worlds/gazebo_ros_camera_distortion_barrel.world",
-  "undistorted_camera/image_raw",
-  "distorted_camera/image_raw",
-  "distorted_camera/camera_info"
+  "undistorted_image",
+  "distorted_image",
+  "distorted_info"
 })
 //  TestParams({"worlds/gazebo_ros_camera_distortion_pincushion.world",
-//              "undistorted_camera/image_raw",
-//              "distorted_camera/image_raw",
-//              "distorted_camera/camera_info"})
+//              "undistorted_image",
+//              "distorted_image",
+//              "distorted_info"})
   ), );
 
 int main(int argc, char ** argv)
diff --git a/gazebo_plugins/test/test_gazebo_ros_camera_triggered.cpp b/gazebo_plugins/test/test_gazebo_ros_camera_triggered.cpp
index 9a2fe4694..19e153080 100644
--- a/gazebo_plugins/test/test_gazebo_ros_camera_triggered.cpp
+++ b/gazebo_plugins/test/test_gazebo_ros_camera_triggered.cpp
@@ -48,7 +48,7 @@ TEST_F(GazeboRosTriggeredCameraTest, CameraSubscribeTest)
   builtin_interfaces::msg::Time image_stamp;
 
   auto sub = image_transport::create_subscription(node,
-      "test_triggered_cam/camera1/image_raw",
+      "test_triggered_cam/image_raw_test",
       [&](const sensor_msgs::msg::Image::ConstSharedPtr & msg) {
         image_stamp = msg->header.stamp;
         ++msg_count;
@@ -63,7 +63,7 @@ TEST_F(GazeboRosTriggeredCameraTest, CameraSubscribeTest)
 
   // Trigger camera once
   auto pub = node->create_publisher<std_msgs::msg::Empty>(
-    "test_triggered_cam/camera1/image_trigger");
+    "test_triggered_cam/image_trigger_test");
   std_msgs::msg::Empty msg;
   pub->publish(msg);
 
diff --git a/gazebo_plugins/test/worlds/gazebo_ros_camera.world b/gazebo_plugins/test/worlds/gazebo_ros_camera.world
index e3b07c912..38b7c1c06 100644
--- a/gazebo_plugins/test/worlds/gazebo_ros_camera.world
+++ b/gazebo_plugins/test/worlds/gazebo_ros_camera.world
@@ -29,8 +29,7 @@
           <plugin name="camera_controller" filename="libgazebo_ros_camera.so">
             <ros>
               <namespace>test_cam</namespace>
-              <!-- TODO(louise) Remapping not working due to https://github.com/ros-perception/image_common/issues/93 -->
-              <argument>image_raw:=image_test</argument>
+              <argument>camera1/image_raw:=camera/image_test</argument>
               <argument>camera_info:=camera_info_test</argument>
             </ros>
             <!-- camera_name>omit so it defaults to sensor name</camera_name-->
diff --git a/gazebo_plugins/test/worlds/gazebo_ros_camera_16bit.world b/gazebo_plugins/test/worlds/gazebo_ros_camera_16bit.world
index dc597caf6..247838a6e 100644
--- a/gazebo_plugins/test/worlds/gazebo_ros_camera_16bit.world
+++ b/gazebo_plugins/test/worlds/gazebo_ros_camera_16bit.world
@@ -29,7 +29,7 @@
           <plugin name="camera_controller" filename="libgazebo_ros_camera.so">
             <ros>
               <namespace>test_cam_16bit</namespace>
-              <argument>image_raw:=image_test_16bit</argument>
+              <argument>test_camera_name/image_raw:=image_test_16bit</argument>
               <argument>camera_info:=camera_info_test_16bit</argument>
             </ros>
             <camera_name>test_camera_name</camera_name>
diff --git a/gazebo_plugins/test/worlds/gazebo_ros_camera_distortion_barrel.world b/gazebo_plugins/test/worlds/gazebo_ros_camera_distortion_barrel.world
index 5926713a0..b0f95dfcf 100644
--- a/gazebo_plugins/test/worlds/gazebo_ros_camera_distortion_barrel.world
+++ b/gazebo_plugins/test/worlds/gazebo_ros_camera_distortion_barrel.world
@@ -39,9 +39,8 @@
           </camera>
           <plugin name="camera_controller" filename="libgazebo_ros_camera.so">
             <ros>
-              <!-- TODO(louise) Remapping not working due to https://github.com/ros-perception/image_common/issues/93 -->
-              <argument>image_raw:=image_test</argument>
-              <argument>camera_info:=camera_info_test</argument>
+              <argument>distorted_camera/image_raw:=distorted_image</argument>
+              <argument>distorted_camera/camera_info:=distorted_info</argument>
             </ros>
             <camera_name>distorted_camera</camera_name>
             <frame_name>frame_name</frame_name>
@@ -80,9 +79,7 @@
           </camera>
           <plugin name="camera_controller" filename="libgazebo_ros_camera.so">
             <ros>
-              <!-- TODO(louise) Remapping not working due to https://github.com/ros-perception/image_common/issues/93 -->
-              <argument>image_raw:=image_test</argument>
-              <argument>camera_info:=camera_info_test</argument>
+              <argument>undistorted_camera/image_raw:=undistorted_image</argument>
             </ros>
             <camera_name>undistorted_camera</camera_name>
             <frame_name>frame_name</frame_name>
diff --git a/gazebo_plugins/test/worlds/gazebo_ros_camera_distortion_pincushion.world b/gazebo_plugins/test/worlds/gazebo_ros_camera_distortion_pincushion.world
index d22cfb0b2..4aba7f1a0 100644
--- a/gazebo_plugins/test/worlds/gazebo_ros_camera_distortion_pincushion.world
+++ b/gazebo_plugins/test/worlds/gazebo_ros_camera_distortion_pincushion.world
@@ -1,5 +1,5 @@
 <?xml version="1.0" ?>
-<sdf version="1.4">
+<sdf version="1.6">
   <world name="default">
 
     <!-- Global light source -->
@@ -11,13 +11,6 @@
       <static>true</static>
       <pose>0.0 0.0 2.5 0.0 1.5707 0.0</pose>
       <link name="camera_link">
-        <visual name="camera_visual">
-          <geometry>
-            <box>
-              <size>0.1 0.1 0.1</size>
-            </box>
-          </geometry>
-        </visual>
         <sensor type="camera" name="camera_distorted">
           <update_rate>30.0</update_rate>
           <camera name="head">
@@ -46,12 +39,10 @@
           </camera>
           <plugin name="camera_controller" filename="libgazebo_ros_camera.so">
             <ros>
-              <namespace>test_cam_distorted</namespace>
-              <!-- TODO(louise) Remapping not working due to https://github.com/ros-perception/image_common/issues/93 -->
-              <argument>image_raw:=image_test</argument>
-              <argument>camera_info:=camera_info_test</argument>
+              <argument>distorted_camera/image_raw:=distorted_image</argument>
+              <argument>distorted_camera/camera_info:=distorted_info</argument>
             </ros>
-            <camera_name>camera_name</camera_name>
+            <camera_name>distorted_camera</camera_name>
             <frame_name>frame_name</frame_name>
             <hack_baseline>0.07</hack_baseline>
             <border_crop>false</border_crop>
@@ -64,13 +55,6 @@
       <static>true</static>
       <pose>0.0 0.0 2.5 0.0 1.5707 0.0</pose>
       <link name="camera_link">
-        <visual name="camera_visual">
-          <geometry>
-            <box>
-              <size>0.1 0.1 0.1</size>
-            </box>
-          </geometry>
-        </visual>
         <sensor type="camera" name="camera_undistorted">
           <update_rate>30.0</update_rate>
           <camera name="head">
@@ -95,14 +79,10 @@
           </camera>
           <plugin name="camera_controller" filename="libgazebo_ros_camera.so">
             <ros>
-              <namespace>test_cam_undistorted</namespace>
-              <!-- TODO(louise) Remapping not working due to https://github.com/ros-perception/image_common/issues/93 -->
-              <argument>image_raw:=image_test</argument>
-              <argument>camera_info:=camera_info_test</argument>
+              <argument>undistorted_camera/image_raw:=undistorted_image</argument>
             </ros>
-            <camera_name>camera_name</camera_name>
+            <camera_name>undistorted_camera</camera_name>
             <frame_name>frame_name</frame_name>
-            <hack_baseline>0.0</hack_baseline>
           </plugin>
         </sensor>
       </link>
@@ -111,5 +91,5 @@
     <include>
       <uri>model://checkerboard_plane</uri>
     </include>
-</world>
+  </world>
 </sdf>
diff --git a/gazebo_plugins/test/worlds/gazebo_ros_camera_triggered.world b/gazebo_plugins/test/worlds/gazebo_ros_camera_triggered.world
index d61d56ba8..cdd6d6016 100644
--- a/gazebo_plugins/test/worlds/gazebo_ros_camera_triggered.world
+++ b/gazebo_plugins/test/worlds/gazebo_ros_camera_triggered.world
@@ -36,11 +36,8 @@
           <plugin name="camera_controller" filename="libgazebo_ros_camera.so">
             <ros>
               <namespace>test_triggered_cam</namespace>
-              <!-- TODO(louise) Remapping not working due to
-                   https://github.com/ros-perception/image_common/issues/93 -->
-              <argument>image_raw:=image_test</argument>
-              <argument>camera_info:=camera_info_test</argument>
-              <argument>image_trigger:=image_trigger_test</argument>
+              <argument>camera1/image_raw:=image_raw_test</argument>
+              <argument>camera1/image_trigger:=image_trigger_test</argument>
             </ros>
             <triggered>true</triggered>
             <hack_baseline>0.07</hack_baseline>