diff --git a/jsk_fetch_robot/jsk_fetch_startup/euslisp/navigation-utils.l b/jsk_fetch_robot/jsk_fetch_startup/euslisp/navigation-utils.l
index 2b2d9d0116..45089df18a 100644
--- a/jsk_fetch_robot/jsk_fetch_startup/euslisp/navigation-utils.l
+++ b/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 (if room-light-msg (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)
diff --git a/jsk_fetch_robot/jsk_fetch_startup/launch/fetch_bringup.launch b/jsk_fetch_robot/jsk_fetch_startup/launch/fetch_bringup.launch
index a594a2a226..687468e067 100644
--- a/jsk_fetch_robot/jsk_fetch_startup/launch/fetch_bringup.launch
+++ b/jsk_fetch_robot/jsk_fetch_startup/launch/fetch_bringup.launch
@@ -232,4 +232,8 @@
+
+
+
+
diff --git a/jsk_pr2_robot/jsk_pr2_startup/pr2.launch b/jsk_pr2_robot/jsk_pr2_startup/pr2.launch
index 9082d34462..d6766dbb1f 100644
--- a/jsk_pr2_robot/jsk_pr2_startup/pr2.launch
+++ b/jsk_pr2_robot/jsk_pr2_startup/pr2.launch
@@ -174,4 +174,10 @@
+
+
+
+
+
+
diff --git a/jsk_robot_common/jsk_robot_startup/CMakeLists.txt b/jsk_robot_common/jsk_robot_startup/CMakeLists.txt
index 90492f12d5..04ea9a08aa 100644
--- a/jsk_robot_common/jsk_robot_startup/CMakeLists.txt
+++ b/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}
)
diff --git a/jsk_robot_common/jsk_robot_startup/README.md b/jsk_robot_common/jsk_robot_startup/README.md
index 3666388ec3..815db0ffb5 100644
--- a/jsk_robot_common/jsk_robot_startup/README.md
+++ b/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).
diff --git a/jsk_robot_common/jsk_robot_startup/msg/RoomLight.msg b/jsk_robot_common/jsk_robot_startup/msg/RoomLight.msg
new file mode 100644
index 0000000000..f5c4842ced
--- /dev/null
+++ b/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
diff --git a/jsk_robot_common/jsk_robot_startup/package.xml b/jsk_robot_common/jsk_robot_startup/package.xml
index 9c1a71d2f2..e75874f906 100644
--- a/jsk_robot_common/jsk_robot_startup/package.xml
+++ b/jsk_robot_common/jsk_robot_startup/package.xml
@@ -8,6 +8,7 @@
catkin
jsk_topic_tools
+ message_generation
mongodb_store
roscpp
rospy
@@ -24,6 +25,7 @@
gmapping
jsk_recognition_msgs
jsk_topic_tools
+ message_runtime
mongodb_store
nodelet
pointcloud_to_laserscan
diff --git a/jsk_robot_common/jsk_robot_startup/scripts/check_room_light.py b/jsk_robot_common/jsk_robot_startup/scripts/check_room_light.py
new file mode 100755
index 0000000000..c862235c53
--- /dev/null
+++ b/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()