Merge pull request #1330 from knorth55/check-room-light

add check_room_light.py to check if room light is on

jsk_fetch_robot/jsk_fetch_startup/launch/fetch_bringup.launch

@@ -298,6 +298,11 @@
     <remap from="~output" to="/odom_corrected" />
   </node>
 
+  <!-- check if room light is on -->
+  <node name="check_room_light" pkg="jsk_robot_startup" type="check_room_light.py">
+    <remap from="~input" to="/head_camera/rgb/image_raw" />
+  </node>
+
   <!-- robot_pose_publisher for rwt_nav -->
   <node name="robot_pose_publisher" pkg="robot_pose_publisher" type="robot_pose_publisher">
     <rosparam>

jsk_pr2_robot/jsk_pr2_startup/pr2.launch

@@ -164,4 +164,10 @@
   <node pkg="jsk_robot_startup" type="finish_launch_sound.py"
         machine="c2"
         name="finish_launch_sound" />
+
+  <!-- check if room light is on -->
+  <node name="check_room_light" pkg="jsk_robot_startup" type="check_room_light.py">
+    <remap from="~input" to="/kinect_head/rgb/image_raw" />
+  </node>
+
 </launch>

jsk_robot_common/jsk_robot_startup/CMakeLists.txt

@@ -24,6 +24,7 @@ endif()
 find_package(catkin COMPONENTS # remove REQUIRED for mongodb
   dynamic_reconfigure
   jsk_topic_tools
+  message_generation
   mongodb_store
   sensor_msgs
   urdf
@@ -53,7 +54,18 @@ generate_dynamic_reconfigure_options(
   cfg/JointStatesThrottle.cfg
 )
 
+add_message_files(
+  FILES
+  RoomLight.msg
+)
+
+generate_messages(
+  DEPENDENCIES
+  std_msgs
+)
+
 catkin_package(
+  CATKIN_DEPENDS message_runtime
   INCLUDE_DIRS include
   LIBRARIES ${PROJECT_NAME}
 )

jsk_robot_common/jsk_robot_startup/README.md

@@ -112,6 +112,33 @@ rosrun jsk_robot_startup mux_selector.py /joy1 'm.buttons[9]==1' /cmd_vel1 /joy2
 The topic specified in the argument is subscribed.
 
 
+## scripts/check_room_light.py
+
+This node publish the luminance calculated from input image and room light status.
+
+### Subscribing Topics
+
+* `~input` (`sensor_msgs/Image` or `sensor_msgs/CompressedImage`)
+
+  Input topic image
+
+### Publishing Topics
+
+* `~output` (`jsk_robot_startup/RoomLight`)
+
+  Room light status and room luminance
+
+
+### Parameters
+
+* `~luminance_threshold` (Float, default: 50)
+
+  Luminance threshold to deteremine whether room light is on or off
+
+* `~image_transport` (String, default: raw)
+
+  Image transport hint.
+
 ## launch/safe_teleop.launch
 
 This launch file provides a set of nodes for safe teleoperation common to mobile robots. Robot-specific nodes such as `/joy`, `/teleop` or `/cable_warning` must be included in the teleop launch file for each robot, such as [safe_teleop.xml for PR2](https://github.com/jsk-ros-pkg/jsk_robot/blob/master/jsk_pr2_robot/jsk_pr2_startup/jsk_pr2_move_base/safe_teleop.xml) or [safe_teleop.xml for fetch](https://github.com/jsk-ros-pkg/jsk_robot/blob/master/jsk_fetch_robot/jsk_fetch_startup/launch/fetch_teleop.xml).

jsk_robot_common/jsk_robot_startup/msg/RoomLight.msg

@@ -0,0 +1,5 @@
+std_msgs/Header header
+bool light_on
+# relative luminance (Digital ITU BT.601)
+# relative luminance is not illuminance and not in Lux unit
+float32 relative_luminance

jsk_robot_common/jsk_robot_startup/package.xml

@@ -8,6 +8,7 @@
 
   <buildtool_depend>catkin</buildtool_depend>
   <build_depend>jsk_topic_tools</build_depend>
+  <build_depend>message_generation</build_depend>
   <build_depend>mongodb_store</build_depend>
   <build_depend>roscpp</build_depend>
   <build_depend>rospy</build_depend>
@@ -24,6 +25,7 @@
   <run_depend>gmapping</run_depend>
   <run_depend>jsk_recognition_msgs</run_depend>
   <run_depend version_gte="2.2.7">jsk_topic_tools</run_depend>
+  <run_depend>message_runtime</run_depend>
   <run_depend>mongodb_store</run_depend>
   <run_depend>nodelet</run_depend>
   <run_depend>pointcloud_to_laserscan</run_depend>

jsk_robot_common/jsk_robot_startup/scripts/check_room_light.py

@@ -0,0 +1,71 @@
+#!/usr/bin/env python
+
+import cv2
+from cv_bridge import CvBridge
+import numpy as np
+import rospy
+
+from jsk_topic_tools import ConnectionBasedTransport
+
+from sensor_msgs.msg import CompressedImage
+from sensor_msgs.msg import Image
+
+from jsk_robot_startup.msg import RoomLight
+
+
+class CheckRoomLightNode(ConnectionBasedTransport):
+    def __init__(self):
+        super(CheckRoomLightNode, self).__init__()
+        self.bridge = CvBridge()
+        self.pub = self.advertise('~output', RoomLight, queue_size=1)
+        self.luminance_threshold = rospy.get_param('~luminance_threshold', 50)
+        self.transport_hint = rospy.get_param('~image_transport', 'raw')
+        rospy.loginfo("Using transport {}".format(self.transport_hint))
+
+    def subscribe(self):
+        if self.transport_hint == 'compressed':
+            self.sub = rospy.Subscriber(
+                '{}/compressed'.format(rospy.resolve_name('~input')),
+                CompressedImage, self._image_cb, queue_size=1, buff_size=2**26)
+        else:
+            self.sub = rospy.Subscriber(
+                '~input', Image, self._image_cb, queue_size=1, buff_size=2**26)
+
+    def unsubscribe(self):
+        self.sub.unregister()
+
+    def _image_cb(self, msg):
+        if self.transport_hint == 'compressed':
+            encoding = msg.format.split(';')[0]
+        else:
+            encoding = msg.encoding
+        if encoding == 'mono8':
+            if self.transport_hint == 'compressed':
+                np_arr = np.fromstring(msg.data, np.uint8)
+                img = cv2.imdecode(np_arr, cv2.IMREAD_GRAYSCALE)
+            else:
+                img = self.bridge.imgmsg_to_cv2(msg, desired_encoding='mono8')
+            lum_img = img
+        else:
+            if self.transport_hint == 'compressed':
+                np_arr = np.fromstring(msg.data, np.uint8)
+                img = cv2.imdecode(np_arr, cv2.IMREAD_COLOR)
+                img = img[:, :, ::-1]
+            else:
+                img = self.bridge.imgmsg_to_cv2(msg, desired_encoding='rgb8')
+            # relative luminance calculation with RGB from Digital ITU BT.601
+            # relative luminance is not illuminance and not in Lux unit
+            # lum = 0. 299 * R + 0.587 * G + 0.114 * B
+            lum_img = 0.299 * img[:, :, 0] + 0.587 * img[:, :, 1] \
+                + 0.114 * img[:, :, 2]
+        luminance = np.mean(lum_img)
+        light_msg = RoomLight(header=msg.header)
+        light_msg.light_on = luminance > self.luminance_threshold
+        light_msg.relative_luminance = luminance
+        self.pub.publish(light_msg)
+
+
+if __name__ == '__main__':
+    rospy.init_node('check_room_light')
+    app = CheckRoomLightNode()
+    rospy.spin()