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/package.xml b/jsk_pcl_ros/package.xml
index 40507cb90f..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>
@@ -73,12 +74,15 @@
   <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>robot_state_publisher</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>
@@ -89,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>
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 ea7b776367..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,8 +1,10 @@
 <launch>
-  <param name="robot_description" command="$(find xacro)/xacro.py '$(find pr2_description)/robots/pr2.urdf.xacro'"/>
+  <arg name="use_gui" default="true" doc="argument to launch rviz" />
+
+  <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="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..1b7a6cb42c
--- /dev/null
+++ b/jsk_pcl_ros/test/test_octomap_server_contact_pr2.test
@@ -0,0 +1,23 @@
+<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: /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>