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>