From fa73c37fa8ee24d426ffac9aad2eecf12bf4266c Mon Sep 17 00:00:00 2001
From: Kei Okada <k-okada@jsk.t.u-tokyo.ac.jp>
Date: Fri, 2 Feb 2024 12:07:46 +0900
Subject: [PATCH 1/5] add test_octomap_server_contact_pr2.test for xacro or
 xacro.py, see #2817

---
 jsk_pcl_ros/CMakeLists.txt                        |  1 +
 .../sample_octomap_server_contact_pr2.launch      |  6 ++++--
 .../test/test_octomap_server_contact_pr2.test     | 15 +++++++++++++++
 3 files changed, 20 insertions(+), 2 deletions(-)
 create mode 100644 jsk_pcl_ros/test/test_octomap_server_contact_pr2.test

diff --git a/jsk_pcl_ros/CMakeLists.txt b/jsk_pcl_ros/CMakeLists.txt
index 0076d779f3..985b04abf5 100644
--- a/jsk_pcl_ros/CMakeLists.txt
+++ b/jsk_pcl_ros/CMakeLists.txt
@@ -659,6 +659,7 @@ if (CATKIN_ENABLE_TESTING)
     if(tf2_eigen_FOUND)
       add_rostest(test/test_container_occupancy_detector.test)
     endif()
+    add_rostest(test/test_octomap_server_contact_pr2.test)
   endif()
   roslaunch_add_file_check(launch/openni_local.launch)
   roslaunch_add_file_check(launch/openni2_local.launch)
diff --git a/jsk_pcl_ros/sample/sample_octomap_server_contact_pr2.launch b/jsk_pcl_ros/sample/sample_octomap_server_contact_pr2.launch
index ea7b776367..04ebcaa440 100644
--- a/jsk_pcl_ros/sample/sample_octomap_server_contact_pr2.launch
+++ b/jsk_pcl_ros/sample/sample_octomap_server_contact_pr2.launch
@@ -1,8 +1,10 @@
 <launch>
+  <arg name="use_gui" default="true" doc="argument to launch rviz" />
+
   <param name="robot_description" command="$(find xacro)/xacro.py '$(find pr2_description)/robots/pr2.urdf.xacro'"/>
 
   <node name="joint_state_publisher" pkg="joint_state_publisher" type="joint_state_publisher">
-    <param name="use_gui" value="true"/>
+    <param name="use_gui" value="$(arg use_gui)"/>
   </node>
 
   <node name="robot_state_publisher" pkg="robot_state_publisher" type="robot_state_publisher"
@@ -62,6 +64,6 @@
   <node pkg="jsk_pcl_ros" type="test_contact_sensor.py" name="test_contact_sensor"
         output="screen" />
 
-  <node pkg="rviz" type="rviz" name="rviz"
+  <node pkg="rviz" type="rviz" name="rviz" if="$(arg use_gui)"
         args="-d  $(find jsk_pcl_ros)/config/octomap_server_contact_pr2.rviz" />
 </launch>
diff --git a/jsk_pcl_ros/test/test_octomap_server_contact_pr2.test b/jsk_pcl_ros/test/test_octomap_server_contact_pr2.test
new file mode 100644
index 0000000000..63e801641c
--- /dev/null
+++ b/jsk_pcl_ros/test/test_octomap_server_contact_pr2.test
@@ -0,0 +1,15 @@
+<launch>
+  <include file="$(find jsk_pcl_ros)/sample/sample_octomap_server_contact_pr2.launch" >
+    <arg name="use_gui" value="false" />
+  </include>
+
+  <test test-name="test_octomap_server_contact_pr2"
+        name="test_octomap_server_contact_pr2"
+        pkg="jsk_tools" type="test_topic_published.py"
+        retry="3">
+    <rosparam>
+      topic_0: /octomap_binary
+      timeout_0: 30
+    </rosparam>
+  </test>
+</launch>

From 9832d50a35fc189aee00a71784052e81881e4b71 Mon Sep 17 00:00:00 2001
From: Shun Hasegawa <pazeshun3684@gmail.com>
Date: Thu, 25 Jan 2024 19:57:32 +0900
Subject: [PATCH 2/5] xacro.py is removed on noetic

---
 jsk_pcl_ros/sample/include/play_rosbag_pr2_sink.xml         | 2 +-
 jsk_pcl_ros/sample/sample_octomap_server_contact_pr2.launch | 2 +-
 2 files changed, 2 insertions(+), 2 deletions(-)

diff --git a/jsk_pcl_ros/sample/include/play_rosbag_pr2_sink.xml b/jsk_pcl_ros/sample/include/play_rosbag_pr2_sink.xml
index cb7289c05b..0814384af8 100644
--- a/jsk_pcl_ros/sample/include/play_rosbag_pr2_sink.xml
+++ b/jsk_pcl_ros/sample/include/play_rosbag_pr2_sink.xml
@@ -2,7 +2,7 @@
   <arg name="LOAD_URDF" default="false"/>
 
   <param name="/use_sim_time" value="true"/>
-  <param name="robot_description" command="$(find xacro)/xacro.py '$(find pr2_description)/robots/pr2.urdf.xacro'"
+  <param name="robot_description" command="$(find xacro)/xacro '$(find pr2_description)/robots/pr2.urdf.xacro'"
          if="$(arg LOAD_URDF)"/>
 
   <node name="rosbag_play" pkg="rosbag" type="play"
diff --git a/jsk_pcl_ros/sample/sample_octomap_server_contact_pr2.launch b/jsk_pcl_ros/sample/sample_octomap_server_contact_pr2.launch
index 04ebcaa440..789d28ffcd 100644
--- a/jsk_pcl_ros/sample/sample_octomap_server_contact_pr2.launch
+++ b/jsk_pcl_ros/sample/sample_octomap_server_contact_pr2.launch
@@ -1,7 +1,7 @@
 <launch>
   <arg name="use_gui" default="true" doc="argument to launch rviz" />
 
-  <param name="robot_description" command="$(find xacro)/xacro.py '$(find pr2_description)/robots/pr2.urdf.xacro'"/>
+  <param name="robot_description" command="$(find xacro)/xacro '$(find pr2_description)/robots/pr2.urdf.xacro'"/>
 
   <node name="joint_state_publisher" pkg="joint_state_publisher" type="joint_state_publisher">
     <param name="use_gui" value="$(arg use_gui)"/>

From 169112216f40bad5322a41ed263706a9db7f26ae Mon Sep 17 00:00:00 2001
From: Shun Hasegawa <pazeshun3684@gmail.com>
Date: Thu, 8 Feb 2024 02:14:46 +0900
Subject: [PATCH 3/5] [jsk_pcl_ros] Check more topics in
 test_octomap_server_contact_pr2

---
 jsk_pcl_ros/test/test_octomap_server_contact_pr2.test | 10 +++++++++-
 1 file changed, 9 insertions(+), 1 deletion(-)

diff --git a/jsk_pcl_ros/test/test_octomap_server_contact_pr2.test b/jsk_pcl_ros/test/test_octomap_server_contact_pr2.test
index 63e801641c..1b7a6cb42c 100644
--- a/jsk_pcl_ros/test/test_octomap_server_contact_pr2.test
+++ b/jsk_pcl_ros/test/test_octomap_server_contact_pr2.test
@@ -8,8 +8,16 @@
         pkg="jsk_tools" type="test_topic_published.py"
         retry="3">
     <rosparam>
-      topic_0: /octomap_binary
+      topic_0: /occupied_cells_vis_array
       timeout_0: 30
+      topic_1: /free_cells_vis_array
+      timeout_1: 30
+      topic_2: /unknown_cells_vis_array
+      timeout_2: 30
+      topic_3: /octomap_full
+      timeout_3: 30
+      topic_4: /octomap_binary
+      timeout_4: 30
     </rosparam>
   </test>
 </launch>

From abbef14c4326162f8ae614030b85bac635a4baa5 Mon Sep 17 00:00:00 2001
From: Shun Hasegawa <pazeshun3684@gmail.com>
Date: Thu, 8 Feb 2024 01:59:13 +0900
Subject: [PATCH 4/5] [jsk_pcl_ros] Add missing dependency of
 sample_octomap_server_contact_pr2.launch

---
 jsk_pcl_ros/package.xml | 2 ++
 1 file changed, 2 insertions(+)

diff --git a/jsk_pcl_ros/package.xml b/jsk_pcl_ros/package.xml
index 40507cb90f..bc46e05ee4 100644
--- a/jsk_pcl_ros/package.xml
+++ b/jsk_pcl_ros/package.xml
@@ -73,12 +73,14 @@
   <exec_depend>pcl_conversions</exec_depend>
   <exec_depend>pcl_msgs</exec_depend>
   <exec_depend>pcl_ros</exec_depend>
+  <exec_depend>pr2_description</exec_depend>
   <exec_depend condition="$ROS_PYTHON_VERSION == 2">python-sklearn</exec_depend>
   <exec_depend condition="$ROS_PYTHON_VERSION == 3">python3-sklearn</exec_depend>
   <exec_depend>resized_image_transport</exec_depend>
   <exec_depend>robot_self_filter</exec_depend>
   <exec_depend>rosboost_cfg</exec_depend>
   <exec_depend>roscpp_tutorials</exec_depend>
+  <exec_depend>roseus</exec_depend>
   <exec_depend>sensor_msgs</exec_depend>
   <exec_depend>std_msgs</exec_depend>
   <exec_depend>std_srvs</exec_depend>

From 952c35e34a5713af61a5739bc6ca5bba44fcc0f3 Mon Sep 17 00:00:00 2001
From: Shun Hasegawa <pazeshun3684@gmail.com>
Date: Thu, 8 Feb 2024 13:09:46 +0900
Subject: [PATCH 5/5] [jsk_pcl_ros] Explicitly add all dependencies of
 test_octomap_server_contact_pr2

---
 jsk_pcl_ros/package.xml | 3 +++
 1 file changed, 3 insertions(+)

diff --git a/jsk_pcl_ros/package.xml b/jsk_pcl_ros/package.xml
index bc46e05ee4..d8780c998c 100644
--- a/jsk_pcl_ros/package.xml
+++ b/jsk_pcl_ros/package.xml
@@ -50,6 +50,7 @@
   <exec_depend>image_transport</exec_depend>
   <exec_depend>image_view2</exec_depend>
   <exec_depend>interactive_markers</exec_depend>
+  <exec_depend>joint_state_publisher</exec_depend>
   <exec_depend>jsk_data</exec_depend>
   <exec_depend>jsk_footstep_msgs</exec_depend>
   <exec_depend>jsk_interactive_marker</exec_depend>
@@ -78,6 +79,7 @@
   <exec_depend condition="$ROS_PYTHON_VERSION == 3">python3-sklearn</exec_depend>
   <exec_depend>resized_image_transport</exec_depend>
   <exec_depend>robot_self_filter</exec_depend>
+  <exec_depend>robot_state_publisher</exec_depend>
   <exec_depend>rosboost_cfg</exec_depend>
   <exec_depend>roscpp_tutorials</exec_depend>
   <exec_depend>roseus</exec_depend>
@@ -91,6 +93,7 @@
   <exec_depend>tf_conversions</exec_depend>
   <exec_depend>topic_tools</exec_depend>
   <exec_depend>visualization_msgs</exec_depend>
+  <exec_depend>xacro</exec_depend>
   <exec_depend>yaml-cpp</exec_depend>
 
   <test_depend>compressed_depth_image_transport</test_depend>