diff --git a/jsk_pcl_ros/CMakeLists.txt b/jsk_pcl_ros/CMakeLists.txt
index 0076d779f3..12e92b6d4b 100644
--- a/jsk_pcl_ros/CMakeLists.txt
+++ b/jsk_pcl_ros/CMakeLists.txt
@@ -562,6 +562,7 @@ if (CATKIN_ENABLE_TESTING)
     add_rostest(test/test_dump_depth_error.test)
     add_rostest(test/test_octree_voxel_grid.test)
     add_rostest(test/test_octomap_contact.test)
+    add_rostest(test/test_octomap_server_contact_pr2.test)
     add_rostest(test/test_handle_estimator.test)
     add_rostest(test/test_hsi_color_filter.test)
     add_rostest(test/test_icp_registration.test)
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..092793ee96 100644
--- a/jsk_pcl_ros/sample/sample_octomap_server_contact_pr2.launch
+++ b/jsk_pcl_ros/sample/sample_octomap_server_contact_pr2.launch
@@ -1,4 +1,6 @@
 <launch>
+  <arg name="rviz" default="true" />
+
   <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">
@@ -62,6 +64,7 @@
   <node pkg="jsk_pcl_ros" type="test_contact_sensor.py" name="test_contact_sensor"
         output="screen" />
 
-  <node pkg="rviz" type="rviz" name="rviz"
+  <node if="$(arg rviz)"
+        pkg="rviz" type="rviz" name="rviz"
         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..e610accbfd
--- /dev/null
+++ b/jsk_pcl_ros/test/test_octomap_server_contact_pr2.test
@@ -0,0 +1,25 @@
+<launch>
+
+  <include file="$(find jsk_pcl_ros)/sample/sample_octomap_server_contact_pr2.launch">
+    <arg name="rviz" 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"
+        time-limit="30" 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>