Use room light node

jsk_fetch_robot/jsk_fetch_startup/euslisp/navigation-utils.l

@@ -1,6 +1,7 @@
 (load "package://jsk_robot_startup/lifelog/tweet_client.l")
 
 (ros::load-ros-manifest "fetch_auto_dock_msgs")
+(ros::load-ros-manifest "jsk_robot_startup")
 (ros::load-ros-manifest "power_msgs")
 
 (defparameter *dock-action* nil)
@@ -166,28 +167,15 @@
   (send *ri* :speak-jp "キッチンに向かいます。" :wait t)
   (unix::sleep 1)
   ;; Check if the lights are on in the room
-  (let ((initial-light-on nil)
-        (success-go-to-kitchen nil)
-        (success-auto-dock nil))
-    (let* ((ros-img (one-shot-subscribe "/head_camera/rgb/quater/image_rect_color" sensor_msgs::Image))
-           (eus-img (ros::sensor_msgs/Image->image ros-img))
-           (luminance 0) rgb-oct r g b)
-      (dotimes (w (send eus-img :width))
-        (dotimes (h (send eus-img :height))
-          (setq rgb-oct (send eus-img :pixel-hex-string w h))
-          (setq r (read-from-string (format nil "#X~A" (subseq rgb-oct 0 2))))
-          (setq g (read-from-string (format nil "#X~A" (subseq rgb-oct 2 4))))
-          (setq b (read-from-string (format nil "#X~A" (subseq rgb-oct 4 6))))
-          (setq luminance (+ luminance (+ (* 0.299 r) (* 0.587 g) (* 0.114 b))))))
-      (setq luminance (/ luminance (* (send eus-img :width) (send eus-img :height))))
-      (if (> luminance 50)
-        (setq initial-light-on t)
-        (setq initial-light-on nil))
-      (if initial-light-on
-        (send *ri* :speak-jp "すでに電気がついています。" :wait t)
-        (progn
-          (send *ri* :speak-jp "オッケー、グーグル" :wait t)
-          (send *ri* :speak-jp "電気をつけて" :wait t))))
+  (let* ((room-light-msg (one-shot-subscribe "/check_room_light/output" jsk_robot_startup::RoomLight))
+         (initial-light-on (send room-light-msg :light_on))
+         (success-go-to-kitchen nil)
+         (success-auto-dock nil))
+    (if initial-light-on
+      (send *ri* :speak-jp "すでに電気がついています。" :wait t)
+      (progn
+        (send *ri* :speak-jp "オッケー、グーグル" :wait t)
+        (send *ri* :speak-jp "電気をつけて" :wait t))))
     (unix::sleep 1)
     ;; stove
     (dotimes (i n-kitchen-trial)

jsk_fetch_robot/jsk_fetch_startup/launch/fetch_bringup.launch

@@ -232,4 +232,8 @@
     </rosparam>
   </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>
 </launch>

jsk_pr2_robot/jsk_pr2_startup/pr2.launch

@@ -174,4 +174,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 REQUIRED COMPONENTS
   dynamic_reconfigure
   jsk_topic_tools
+  message_generation
   mongodb_store
   sensor_msgs
   urdf
@@ -45,7 +46,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()