Merge branch 'master' into fetch-go-to-kitchen

.travis.rosinstall.indigo

@@ -11,3 +11,10 @@
     local-name: jsk_3rdparty/respeaker_ros
     uri: https://github.com/tork-a/jsk_3rdparty-release/archive/release/kinetic/respeaker_ros/2.1.13-1.tar.gz
     version: jsk_3rdparty-release-release-kinetic-respeaker_ros-2.1.13-1
+# jsk_robot_startup LightweightLogger requires mongodb_store 0.4.5.
+# indigo is already EOL and 0.4.5 is never released. (2019/08/20)
+# more detailed information, see https://github.com/jsk-ros-pkg/jsk_robot/pull/1121
+- git:
+    local-name: strands-project/mongodb_store
+    uri: https://github.com/strands-project/mongodb_store.git
+    version: 0.4.5

jsk_fetch_robot/jsk_fetch_startup/scripts/battery_warning.py

@@ -21,6 +21,7 @@ def __init__(self):
         self.timer = rospy.Timer(rospy.Duration(self.duration), self._timer_cb)
         self.charge_level = None
         self.prev_charge_level = None
+        self.is_charging = False
 
     def _speak(self, sentence):
         req = SoundRequest()
@@ -33,7 +34,7 @@ def _speak(self, sentence):
         self.speak_client.wait_for_result(timeout=rospy.Duration(10))
 
     def _warn(self):
-        if self.charge_level < self.threshold:
+        if self.charge_level < self.threshold and not self.is_charging:
             rospy.logerr("Low battery: only {}% remaining".format(self.charge_level))
             sentence = "バッテリー残り{}パーセントです。".format(self.charge_level)
             sentence += "もう限界ですので、僕をお家にかえしてください。"
@@ -48,6 +49,7 @@ def _warn(self):
     def _cb(self, msg):
         is_first_time = self.charge_level is None
         self.charge_level = int(msg.charge_level * 100)
+        self.is_charging = msg.is_charging
         if is_first_time:
             self.prev_charge_level = self.charge_level + self.step
             self._warn()

jsk_pr2_robot/jsk_pr2_startup/jsk_pr2_move_base/look-forward-in-nav.l

@@ -3,6 +3,7 @@
 (ros::roseus-add-msgs "nav_msgs")
 (ros::roseus-add-msgs "sound_play")
 (ros::roseus-add-msgs "move_base_msgs")
+(ros::roseus-add-srvs "std_srvs")
 (load "package://pr2eus/pr2-interface.l")
 
 (ros::roseus "look_forward")
@@ -17,6 +18,8 @@
 
 (defun result-cb (msg)
   (let ((av (send *ri* :state :potentio-vector)))
+    (unless *look-enable*
+      (return-from result-cb nil))
     (send *pr2* :angle-vector av)
     (send *pr2* :head :angle-vector #f(0 0))
     (send *ri* :angle-vector (send *pr2* :angle-vector) 1000 :head-controller)
@@ -41,6 +44,7 @@
       (ros::ros-error "not transform")
       (return-from look-at-front nil))
     (ros::ros-info "msg received ~A~%" *msg* (norm (send tra :worldpos)))
+    (ros::ros-info "look-forward enabled: ~A" *look-enable*)
 
     (when *look-enable*
       (if (< 500 (norm (send tra :worldpos)))
@@ -65,15 +69,26 @@
     (ros::ros-info "~A plan trajectory end point ~A, head angle ~A" *msg* (send tra :worldpos) (send *pr2* :head :angle-vector))
     ))
 
+(defun start-look-front (req)
+  (setq *look-enable* t)
+  (instance std_srvs::EmptyResponse :init))
+
+(defun stop-look-front (req)
+  (setq *look-enable* nil)
+  (instance std_srvs::EmptyResponse :init))
+
 ;; init
 (pr2-init)
 (defparameter *tfl* (instance ros::transform-listener :init))
 (ros::subscribe "/move_base_node/DWAPlannerROS/global_plan"
-		nav_msgs::Path #'global-path-cb 1)
+                nav_msgs::Path #'global-path-cb 1)
 ;; look straight when navigation retries
 (ros::subscribe "/move_base/result"
                 move_base_msgs::MoveBaseActionResult #'result-cb 1)
 
+(ros::advertise-service "~start" std_srvs::Empty #'start-look-front)
+(ros::advertise-service "~stop" std_srvs::Empty #'stop-look-front)
+
 (unix::sleep 1)
 
 (ros::spin-once)

jsk_pr2_robot/jsk_pr2_startup/jsk_pr2_warning/battery_warning.py

@@ -3,6 +3,7 @@
 # Author: Yuki Furuta <[email protected]>
 
 from __future__ import division
+from __future__ import unicode_literals
 
 import actionlib
 from collections import defaultdict
@@ -29,7 +30,7 @@ def __init__(self):
 
         # param
         self.monitor_rate = rospy.get_param("~monitor_rate", 4)
-        self.warning_temp = rospy.get_param("~warning_temperature", 41.0)
+        self.warning_temp = rospy.get_param("~warning_temperature", 42.0)
         self.min_capacity = rospy.get_param("~min_capacity", 800)
         self.warning_voltage = rospy.get_param("~warning_voltage", 14.0)
         self.critical_voltage = rospy.get_param("~critical_voltage", 13.7)
@@ -50,14 +51,18 @@ def __init__(self):
                 rospy.Duration(self.log_rate), self.log_cb)
 
     def speak(self, sentence):
-        if self.speech_history[sentence] + rospy.Duration(self.warn_repeat_rate) > rospy.Time.now():
+        # Pick first 4 characters as a keyword instead of using whole sentence
+        # because sentence can have variables like 100%, 90%, etc.
+        key = sentence[:4]
+        if self.speech_history[key] + rospy.Duration(self.warn_repeat_rate) > rospy.Time.now():
             return
-        self.speech_history[sentence] = rospy.Time.now()
+        self.speech_history[key] = rospy.Time.now()
         req = SoundRequest()
         req.command = SoundRequest.PLAY_ONCE
         req.sound = SoundRequest.SAY
         req.arg = sentence
         req.arg2 = "ja"
+        req.volume = 1.0
         self.speak_client.send_goal(SoundRequestGoal(sound_request=req))
         self.speak_client.wait_for_result(timeout=rospy.Duration(10))
 

jsk_pr2_robot/jsk_pr2_startup/pr2_bringup.launch

@@ -50,8 +50,14 @@
     <param name="port3" type="string" value="/etc/ros/sensors/battery3" />
     <param name="debug_level" type="int" value="0" />
   </node>
-  <node pkg="power_monitor" type="power_monitor"  name="power_monitor" respawn="true"/>
-
+  <node name="power_monitor"
+        pkg="power_monitor" type="power_monitor"
+        respawn="true" >
+    <rosparam>
+      frequency: 1.0
+    </rosparam>
+  </node>
+
   <!-- Base Laser -->
   <node machine="c2" pkg="hokuyo_node" type="hokuyo_node" name="base_hokuyo_node" args="scan:=base_scan">
     <param name="port" type="string" value="/etc/ros/sensors/base_hokuyo" />

jsk_robot_common/jsk_robot_startup/CMakeLists.txt

@@ -29,7 +29,11 @@ find_package(catkin REQUIRED COMPONENTS
   urdf
 )
 
-find_package(Boost REQUIRED)
+find_package(Boost REQUIRED COMPONENTS program_options thread system)
+if ("${BOOST_MAJOR_VERSION}" VERSION_EQUAL "1" AND "${Boost_MINOR_VERSION}" VERSION_GREATER "46")
+  find_package(Boost REQUIRED COMPONENTS chrono)
+endif()
+
 find_package(OpenCV REQUIRED)
 
 catkin_python_setup()
@@ -59,8 +63,13 @@ macro(add_lifelog_nodelet _cpp _cls _bin)
 endmacro()
 
 # jsk_topic_tools >= 2.2.27 only available from indigo
+# mongodb_store >= 0.4.4 required in kinetic
+# mongodb_store >= 0.5.2 required in melodic
 if($ENV{ROS_DISTRO} STRGREATER "hydro")
-   add_lifelog_nodelet(src/lightweight_logger_nodelet.cpp "jsk_robot_lifelog/LightweightLogger" "lightweight_logger")
+  if (("${mongodb_store_VERSION}" VERSION_GREATER "0.4.3" AND "${mongodb_store_VERSION}" VERSION_LESS "0.5.0")
+      OR "${mongodb_store_VERSION}" VERSION_GREATER "0.5.1")
+    add_lifelog_nodelet(src/lightweight_logger_nodelet.cpp "jsk_robot_lifelog/LightweightLogger" "lightweight_logger")
+  endif()
 endif()
 
 add_library(jsk_robot_lifelog SHARED

jsk_robot_common/jsk_robot_startup/include/jsk_robot_startup/lightweight_logger.h

@@ -41,35 +41,96 @@
 #ifndef LIGHTWEIGHT_LOGGER_H__
 #define LIGHTWEIGHT_LOGGER_H__
 
+#include <boost/circular_buffer.hpp>
+#include <boost/date_time/posix_time/posix_time.hpp>
 #include <diagnostic_updater/diagnostic_updater.h>
 #include <diagnostic_updater/publisher.h>
 #include <topic_tools/shape_shifter.h>
 #include <jsk_topic_tools/diagnostic_utils.h>
 #include <jsk_topic_tools/stealth_relay.h>
 #include <jsk_topic_tools/timered_diagnostic_updater.h>
 #include <jsk_topic_tools/vital_checker.h>
-#include <jsk_robot_startup/message_store_singleton.h>
+#include <mongodb_store/message_store.h>
 
 namespace jsk_robot_startup
 {
   namespace lifelog
   {
+
+    template <typename T>
+    class blocking_circular_buffer {
+      /* Thread safe blocking circular buffer */
+    public:
+      blocking_circular_buffer() {}
+
+      void put(const T& value) {
+        boost::mutex::scoped_lock lock(mutex_);
+        const bool prev_empty = empty();
+        buffer_.push_back(value);
+        if (prev_empty) empty_wait_.notify_all();
+      }
+
+      bool get(T& value, double timeout=0.0) {
+        boost::posix_time::ptime start = boost::posix_time::microsec_clock::local_time();
+        boost::posix_time::time_duration elapsed;
+        while (ros::ok()) {
+          elapsed = boost::posix_time::microsec_clock::local_time() - start;
+          if (timeout > 0 &&
+              (double)(elapsed.total_milliseconds() / 1000.0) > timeout)
+            break;
+          boost::mutex::scoped_lock lock(mutex_);
+          if (empty()) empty_wait_.wait(lock);
+          if (!empty()) {
+            value = buffer_.front();
+            buffer_.pop_front();
+            return true;
+          }
+        }
+        return false;
+      }
+
+      void clear() {
+        buffer_.clear();
+      }
+
+      const bool empty() const {
+        return buffer_.empty();
+      }
+
+      const bool full() const {
+        return buffer_.full();
+      }
+
+      const int size() const {
+        return buffer_.size();
+      }
+
+      void set_capacity(int cap) {
+        buffer_.set_capacity(cap);
+      }
+    protected:
+      boost::circular_buffer<T> buffer_;
+      boost::condition_variable empty_wait_;
+      boost::mutex mutex_;
+    };
+
     class LightweightLogger : public jsk_topic_tools::StealthRelay
     {
     protected:
       virtual void onInit();
       virtual ~LightweightLogger();
-      virtual void loadThread();
-      virtual void inputCallback(const ros::MessageEvent<topic_tools::ShapeShifter>& event);
+      virtual void loggerThread();
+      virtual void inputCallback(const ros::MessageEvent<topic_tools::ShapeShifter const>& event);
       virtual void updateDiagnostic(diagnostic_updater::DiagnosticStatusWrapper &stat);
 
-      mongodb_store::MessageStoreProxy* msg_store_;
-      boost::thread deferred_load_thread_;
+      boost::shared_ptr<mongodb_store::MessageStoreProxy> msg_store_;
+      boost::thread logger_thread_;
       bool wait_for_insert_;
       bool vital_check_;
       bool initialized_;
       std::string input_topic_name_;
       std::string db_name_, col_name_;
+      blocking_circular_buffer<ros::MessageEvent<topic_tools::ShapeShifter const> > buffer_;
 
       // diagnostics
       ros::Time init_time_;