From 30dc137fd4eddfd794b04634001e5f3518f1a73c Mon Sep 17 00:00:00 2001 From: Yuki Furuta Date: Fri, 18 May 2018 11:12:29 +0900 Subject: [PATCH 01/70] jsk_robot_startup: add option 'approximate_sync' --- .../lifelog/common_logger.launch | 36 +++++++++++-------- 1 file changed, 22 insertions(+), 14 deletions(-) diff --git a/jsk_robot_common/jsk_robot_startup/lifelog/common_logger.launch b/jsk_robot_common/jsk_robot_startup/lifelog/common_logger.launch index d35e17287b..f1f5439583 100644 --- a/jsk_robot_common/jsk_robot_startup/lifelog/common_logger.launch +++ b/jsk_robot_common/jsk_robot_startup/lifelog/common_logger.launch @@ -30,19 +30,25 @@ - + - + + + + + + + - approximate_sync: false + approximate_sync: $(arg approximate_sync) update_rate: $(arg log_rate) topics: - - /$(arg camera_ns)/$(arg rgb_ns)/$(arg camera_info_topic) - /$(arg camera_ns)/$(arg rgb_ns)/$(arg rgb_topic)$(arg rgb_suffix) - - /$(arg camera_ns)/$(arg depth_ns)/$(arg camera_info_topic) + - /$(arg camera_ns)/$(arg rgb_ns)/$(arg camera_info_topic) - /$(arg camera_ns)/$(arg depth_ns)/$(arg depth_topic)$(arg depth_suffix) + - /$(arg camera_ns)/$(arg depth_ns)/$(arg camera_info_topic) + - approximate_sync: false + approximate_sync: $(arg approximate_sync) update_rate: $(arg log_rate) topics: - /$(arg camera_ns)/$(arg rgb_ns)/$(arg camera_info_topic) @@ -101,7 +109,7 @@ - approximate_sync: false + approximate_sync: $(arg approximate_sync) update_rate: $(arg log_rate) topics: - /$(arg camera_ns)/$(arg depth_ns)/$(arg camera_info_topic) @@ -123,7 +131,7 @@ @@ -134,7 +142,7 @@ @@ -147,7 +155,7 @@ @@ -158,7 +166,7 @@ From 88b891db88b24ca1a6f951f675fee7209efb2518 Mon Sep 17 00:00:00 2001 From: Yuki Furuta Date: Tue, 22 May 2018 00:00:41 +0900 Subject: [PATCH 02/70] jsk_fetch_startup: enable approximate_sync --- jsk_fetch_robot/jsk_fetch_startup/launch/fetch_lifelog.xml | 1 + 1 file changed, 1 insertion(+) diff --git a/jsk_fetch_robot/jsk_fetch_startup/launch/fetch_lifelog.xml b/jsk_fetch_robot/jsk_fetch_startup/launch/fetch_lifelog.xml index a6ba1ad147..7ebb7e2120 100644 --- a/jsk_fetch_robot/jsk_fetch_startup/launch/fetch_lifelog.xml +++ b/jsk_fetch_robot/jsk_fetch_startup/launch/fetch_lifelog.xml @@ -18,6 +18,7 @@ + From a4dd3c4b9ffabd17dfa1b7335bad4cc48179e21a Mon Sep 17 00:00:00 2001 From: Yuki Furuta Date: Sat, 30 Jun 2018 21:42:11 +0900 Subject: [PATCH 03/70] new battery_warning --- .../jsk_pr2_warning/battery_warning.py | 131 ++++++++++++++++++ jsk_pr2_robot/jsk_pr2_startup/pr2.launch | 3 +- 2 files changed, 132 insertions(+), 2 deletions(-) create mode 100755 jsk_pr2_robot/jsk_pr2_startup/jsk_pr2_warning/battery_warning.py diff --git a/jsk_pr2_robot/jsk_pr2_startup/jsk_pr2_warning/battery_warning.py b/jsk_pr2_robot/jsk_pr2_startup/jsk_pr2_warning/battery_warning.py new file mode 100755 index 0000000000..128982613f --- /dev/null +++ b/jsk_pr2_robot/jsk_pr2_startup/jsk_pr2_warning/battery_warning.py @@ -0,0 +1,131 @@ +#!/usr/bin/env python +# -*- coding: utf-8 -*- +# Author: Yuki Furuta + +import actionlib +from collections import defaultdict +import os.path as osp +import math +import rospy +import pandas as pd + +from sound_play.msg import SoundRequestAction +from sound_play.msg import SoundRequest, SoundRequestGoal +from pr2_msgs.msg import PowerState +from diagnostic_msgs.msg import DiagnosticArray + + +class BatteryWarning(object): + def __init__(self): + # speak + self.speak_client = actionlib.SimpleActionClient( + "/robotsound_jp", SoundRequestAction) + ok = self.speak_client.wait_for_server(rospy.Duration(10)) + if not ok: + rospy.logfatal("Action /robotsound_jp is not advertised") + exit(1) + + # param + self.monitor_rate = rospy.get_param("~monitor_rate", 4) + self.warning_temp = rospy.get_param("~warning_temperature", 41.0) + self.min_capacity = rospy.get_param("~min_capacity", 800) + self.warn_repeat_rate = rospy.get_param("~warn_repeat_rate", 120) + self.log_rate = rospy.get_param("~log_rate", 10) + self.log_path = rospy.get_param("~log_path", None) + if self.log_path is None: + self.log_path = osp.expanduser("~/.ros/pr2_battery_status.pkl") + + self.prev_plugged_in = None + self.latest_status = None + self.speech_history = defaultdict(lambda: rospy.Time(0)) + + self.diag_sub = rospy.Subscriber( + "/diagnostics_agg", DiagnosticArray, self.diag_cb) + self.stat_timer = rospy.Timer( + rospy.Duration(self.monitor_rate), self.stat_cb) + self.log_timer = rospy.Timer( + 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(): + return + self.speech_history[sentence] = rospy.Time.now() + req = SoundRequest() + req.command = SoundRequest.PLAY_ONCE + req.sound = SoundRequest.SAY + req.arg = sentence + req.arg2 = "ja" + self.speak_client.send_goal(SoundRequestGoal(sound_request=req)) + self.speak_client.wait_for_result(timeout=rospy.Duration(10)) + + def log_cb(self, event): + try: + if osp.exists(self.log_path): + orig_df = pd.read_pickle(self.log_path) + df = orig_df.append(self.latest_status, ignore_index=True) + else: + df = self.latest_status + df.to_pickle(self.log_path) + except Exception as e: + if osp.exists(self.log_path): + try: + import shutil + shutil.move(self.log_path, self.log_path + ".bak") + rospy.logwarn("Moved old file to %s" % (self.log_path + ".bak")) + except: + pass + + def stat_cb(self, event): + df = self.latest_status + + if self.latest_status is None: + return + + max_temp = df["Temperature (C)"].astype(float).max() + rospy.logdebug("temperature: %s" % max_temp) + if max_temp > self.warning_temp: + self.speak("バッテリ温度%.1f度。暑いです。部屋の温度を下げてください。" % max_temp) + + plugged_in = df["Power Present"].eq("True").any() + if self.prev_plugged_in is None: + self.prev_plugged_in = not plugged_in + prev_plugged_in, self.prev_plugged_in = self.prev_plugged_in, plugged_in + if plugged_in: + if not prev_plugged_in: + attf_max = df["Average Time To Full (min)"].astype(int).max() + rospy.loginfo("Average Time to full: %s" % attf_max) + if attf_max > 0: + self.speak("フル充電まで%s分です。" % attf_max) + return + + rc = df["Remaining Capacity (mAh)"].astype(float).sub(self.min_capacity) + fc = df["Full Charge Capacity (mAh)"].astype(int).sub(self.min_capacity) + min_perc = int(rc.div(fc).min() * 100.0) + + if prev_plugged_in or min_perc < 50: + self.speak("電池残り%s％です。" % min_perc) + if 15 < min_perc < 30: + self.speak("充電してください。") + elif 0 <= min_perc < 15: + self.speak("もう限界です！") + + def diag_cb(self, msg): + stamp = msg.header.stamp.secs + batt_status = filter( + lambda s: s.name.startswith("/Power System/Smart Battery"), + msg.status) + status = [] + for s in sorted(batt_status, key=lambda s: s.name): + stat = {d.key: d.value for d in s.values} + stat["Name"] = s.name + stat["Time Stamp"] = stamp + status.append(stat) + df = pd.DataFrame(status) + if self.latest_status is not None: + self.prev_status = self.latest_status + self.latest_status = df + +if __name__ == '__main__': + rospy.init_node("battery_warning") + b = BatteryWarning() + rospy.spin() diff --git a/jsk_pr2_robot/jsk_pr2_startup/pr2.launch b/jsk_pr2_robot/jsk_pr2_startup/pr2.launch index b83eedf08c..ca306057bf 100644 --- a/jsk_pr2_robot/jsk_pr2_startup/pr2.launch +++ b/jsk_pr2_robot/jsk_pr2_startup/pr2.launch @@ -102,8 +102,7 @@ - + Date: Thu, 12 Jul 2018 14:27:45 +0900 Subject: [PATCH 04/70] update battery_warning --- .../jsk_pr2_startup/jsk_pr2_warning/battery_warning.py | 2 +- jsk_pr2_robot/jsk_pr2_startup/pr2_bringup.launch | 1 + .../jsk_robot_startup/lifelog/app_manager.launch | 9 +++++---- 3 files changed, 7 insertions(+), 5 deletions(-) diff --git a/jsk_pr2_robot/jsk_pr2_startup/jsk_pr2_warning/battery_warning.py b/jsk_pr2_robot/jsk_pr2_startup/jsk_pr2_warning/battery_warning.py index 128982613f..768b0a9398 100755 --- a/jsk_pr2_robot/jsk_pr2_startup/jsk_pr2_warning/battery_warning.py +++ b/jsk_pr2_robot/jsk_pr2_startup/jsk_pr2_warning/battery_warning.py @@ -83,7 +83,7 @@ def stat_cb(self, event): max_temp = df["Temperature (C)"].astype(float).max() rospy.logdebug("temperature: %s" % max_temp) - if max_temp > self.warning_temp: + if 60 > max_temp > self.warning_temp: self.speak("バッテリ温度%.1f度。暑いです。部屋の温度を下げてください。" % max_temp) plugged_in = df["Power Present"].eq("True").any() diff --git a/jsk_pr2_robot/jsk_pr2_startup/pr2_bringup.launch b/jsk_pr2_robot/jsk_pr2_startup/pr2_bringup.launch index d7126e6921..9ffe9fa873 100644 --- a/jsk_pr2_robot/jsk_pr2_startup/pr2_bringup.launch +++ b/jsk_pr2_robot/jsk_pr2_startup/pr2_bringup.launch @@ -190,5 +190,6 @@ + diff --git a/jsk_robot_common/jsk_robot_startup/lifelog/app_manager.launch b/jsk_robot_common/jsk_robot_startup/lifelog/app_manager.launch index ebddd770aa..ae7d767f6f 100644 --- a/jsk_robot_common/jsk_robot_startup/lifelog/app_manager.launch +++ b/jsk_robot_common/jsk_robot_startup/lifelog/app_manager.launch @@ -15,6 +15,7 @@ + + machine="$(arg machine)" respawn="$(arg respawn)"/> + output="screen" machine="$(arg machine)" respawn="$(arg respawn)"> From dd294d8fa0ecd8e705cf2b7c674e028a6007f7db Mon Sep 17 00:00:00 2001 From: Yuki Furuta Date: Sun, 15 Jul 2018 16:12:21 +0900 Subject: [PATCH 05/70] update --- jsk_pr2_robot/jsk_pr2_startup/jsk_pr2_lifelog/db_client.launch | 1 + jsk_robot_common/jsk_robot_startup/lifelog/common_logger.launch | 2 +- 2 files changed, 2 insertions(+), 1 deletion(-) diff --git a/jsk_pr2_robot/jsk_pr2_startup/jsk_pr2_lifelog/db_client.launch b/jsk_pr2_robot/jsk_pr2_startup/jsk_pr2_lifelog/db_client.launch index 2fb176a457..7e1d727d71 100644 --- a/jsk_pr2_robot/jsk_pr2_startup/jsk_pr2_lifelog/db_client.launch +++ b/jsk_pr2_robot/jsk_pr2_startup/jsk_pr2_lifelog/db_client.launch @@ -23,6 +23,7 @@ + diff --git a/jsk_robot_common/jsk_robot_startup/lifelog/common_logger.launch b/jsk_robot_common/jsk_robot_startup/lifelog/common_logger.launch index f1f5439583..4195cceeb6 100644 --- a/jsk_robot_common/jsk_robot_startup/lifelog/common_logger.launch +++ b/jsk_robot_common/jsk_robot_startup/lifelog/common_logger.launch @@ -40,7 +40,7 @@ - + From 794f2075c94b2b5723b9462765db270669944f51 Mon Sep 17 00:00:00 2001 From: Furushchev Date: Sun, 15 Jul 2018 18:19:46 +0900 Subject: [PATCH 06/70] jsk_robot_startup: add diagnostic for logger nodelet --- .../jsk_robot_startup/lightweight_logger.h | 12 +++++ .../src/lightweight_logger_nodelet.cpp | 44 +++++++++++++++++++ 2 files changed, 56 insertions(+) diff --git a/jsk_robot_common/jsk_robot_startup/include/jsk_robot_startup/lightweight_logger.h b/jsk_robot_common/jsk_robot_startup/include/jsk_robot_startup/lightweight_logger.h index 034d24a4eb..aa84f36d6e 100644 --- a/jsk_robot_common/jsk_robot_startup/include/jsk_robot_startup/lightweight_logger.h +++ b/jsk_robot_common/jsk_robot_startup/include/jsk_robot_startup/lightweight_logger.h @@ -41,8 +41,13 @@ #ifndef LIGHTWEIGHT_LOGGER_H__ #define LIGHTWEIGHT_LOGGER_H__ +#include +#include #include +#include #include +#include +#include #include @@ -55,11 +60,18 @@ namespace jsk_robot_startup protected: virtual void onInit(); virtual void inputCallback(const ros::MessageEvent& event); + virtual void updateDiagnostic(diagnostic_updater::DiagnosticStatusWrapper &stat); boost::shared_ptr msg_store_; bool wait_for_insert_; bool initialized_; std::string input_topic_name_; + + // diagnostics + ros::Time init_time_; + uint64_t inserted_count_, insert_error_count_, prev_insert_error_count_; + jsk_topic_tools::VitalChecker::Ptr vital_checker_; + jsk_topic_tools::TimeredDiagnosticUpdater::Ptr diagnostic_updater_; }; } } diff --git a/jsk_robot_common/jsk_robot_startup/src/lightweight_logger_nodelet.cpp b/jsk_robot_common/jsk_robot_startup/src/lightweight_logger_nodelet.cpp index 8563488809..9a03449567 100644 --- a/jsk_robot_common/jsk_robot_startup/src/lightweight_logger_nodelet.cpp +++ b/jsk_robot_common/jsk_robot_startup/src/lightweight_logger_nodelet.cpp @@ -62,7 +62,26 @@ namespace jsk_robot_startup NODELET_INFO_STREAM("Connecting to database " << db_name << "/" << col_name << "..."); msg_store_.reset(new mongodb_store::MessageStoreProxy(*nh_, col_name, db_name)); NODELET_INFO_STREAM("Successfully connected to database!"); + input_topic_name_ = pnh_->resolveName("input", true); + + diagnostic_updater_.reset( + new jsk_topic_tools::TimeredDiagnosticUpdater(*pnh_, ros::Duration(1.0))); + diagnostic_updater_->setHardwareID("LightweightLogger"); + diagnostic_updater_->add( + "LightweightLogger::" + input_topic_name_, + boost::bind(&LightweightLogger::updateDiagnostic, this, _1)); + double vital_rate; + pnh_->param("vital_rate", vital_rate, 1.0); + vital_checker_.reset( + new jsk_topic_tools::VitalChecker(1.0 / vital_rate)); + diagnostic_updater_->start(); + + inserted_count_ = 0; + insert_error_count_ = 0; + prev_insert_error_count_ = 0; + init_time_ = ros::Time::now(); + initialized_ = true; } @@ -74,6 +93,8 @@ namespace jsk_robot_startup if (!initialized_) return; + vital_checker_->poke(); + try { mongo::BSONObjBuilder meta; @@ -84,11 +105,34 @@ namespace jsk_robot_startup NODELET_DEBUG_STREAM("Inserted (" << input_topic_name_ << ")"); else NODELET_DEBUG_STREAM("Inserted (" << input_topic_name_ << "): " << doc_id); + inserted_count_++; } catch (...) { NODELET_ERROR_STREAM("Failed to insert to db"); + insert_error_count_++; } } + + void LightweightLogger::updateDiagnostic(diagnostic_updater::DiagnosticStatusWrapper &stat) + { + if (ros::Time::now() - init_time_ < ros::Duration(10.0)) { + stat.summary(diagnostic_msgs::DiagnosticStatus::OK, + getName() + " initialized"); + } else if (!vital_checker_->isAlive()) { + jsk_topic_tools::addDiagnosticErrorSummary(getName(), vital_checker_, stat); + } else if (insert_error_count_ != prev_insert_error_count_) { + stat.summary(diagnostic_msgs::DiagnosticStatus::ERROR, + (boost::format("%s fails to insert to db for %d times") % getName() % insert_error_count_).str()); + prev_insert_error_count_ = insert_error_count_; + } else { + stat.summary(diagnostic_msgs::DiagnosticStatus::OK, + getName() + " is running"); + } + + stat.add("Inserted", inserted_count_); + stat.add("Insert Failure", insert_error_count_); + vital_checker_->registerStatInfo(stat, "Last Insert"); + } } } From 97b805946a7cd6ea66e9497682e5e7deed470c04 Mon Sep 17 00:00:00 2001 From: Yuki Furuta Date: Sun, 15 Jul 2018 23:04:20 +0900 Subject: [PATCH 07/70] jsk_pr2_startup: cleanup bringup nodes --- .../jsk_pr2_startup/pr2_bringup.launch | 20 ++++++++++++------- 1 file changed, 13 insertions(+), 7 deletions(-) diff --git a/jsk_pr2_robot/jsk_pr2_startup/pr2_bringup.launch b/jsk_pr2_robot/jsk_pr2_startup/pr2_bringup.launch index d7126e6921..aca0a049d0 100644 --- a/jsk_pr2_robot/jsk_pr2_startup/pr2_bringup.launch +++ b/jsk_pr2_robot/jsk_pr2_startup/pr2_bringup.launch @@ -1,7 +1,8 @@ - + + @@ -116,8 +117,10 @@ if="$(arg launch_runtime_logger)" /> - - + + + + @@ -127,11 +130,12 @@ - + + - - + + @@ -145,7 +149,9 @@ - + + + From 56b3921bdf2f3d0a6c8ae82ae1cf8684ad1d2873 Mon Sep 17 00:00:00 2001 From: Yuki Furuta Date: Sun, 15 Jul 2018 23:04:20 +0900 Subject: [PATCH 08/70] jsk_pr2_startup: cleanup bringup nodes --- .../jsk_pr2_warning/jsk_pr2_analyzers.yaml | 223 ++++++++++++++++++ .../jsk_pr2_startup/pr2_bringup.launch | 20 +- 2 files changed, 236 insertions(+), 7 deletions(-) create mode 100644 jsk_pr2_robot/jsk_pr2_startup/jsk_pr2_warning/jsk_pr2_analyzers.yaml diff --git a/jsk_pr2_robot/jsk_pr2_startup/jsk_pr2_warning/jsk_pr2_analyzers.yaml b/jsk_pr2_robot/jsk_pr2_startup/jsk_pr2_warning/jsk_pr2_analyzers.yaml new file mode 100644 index 0000000000..b85284faef --- /dev/null +++ b/jsk_pr2_robot/jsk_pr2_startup/jsk_pr2_warning/jsk_pr2_analyzers.yaml @@ -0,0 +1,223 @@ +analyzers: + devices: + type: diagnostic_aggregator/AnalyzerGroup + path: Devices + analyzers: + joystick: + type: diagnostic_aggregator/GenericAnalyzer + path: Joystick + expected: 'joy: Joystick Driver Status' + num_items: 1 + remove_prefix: 'joy' + sound: + type: diagnostic_aggregator/GenericAnalyzer + path: Sound + num_items: 1 + contains: 'sound_play' + imu: + type: diagnostic_aggregator/GenericAnalyzer + path: IMU + find_and_remove_prefix: 'imu_node' + num_items: 4 + lasers: + type: diagnostic_aggregator/AnalyzerGroup + path: Lasers + analyzers: + base_hk: + type: diagnostic_aggregator/GenericAnalyzer + path: Base Hokuyo + find_and_remove_prefix: base_hokuyo_node + num_items: 2 + tilt_hk: + type: diagnostic_aggregator/GenericAnalyzer + path: Tilt Hokuyo + find_and_remove_prefix: tilt_hokuyo_node + num_items: 2 + + cameras: + type: diagnostic_aggregator/AnalyzerGroup + path: Cameras + analyzers: + camera_synchronizer: + type: diagnostic_aggregator/GenericAnalyzer + path: Camera Synchronizer + find_and_remove_prefix: camera_synchronizer_node + num_items: 1 + narrow_left: + type: diagnostic_aggregator/GenericAnalyzer + path: Narrow (Left) + find_and_remove_prefix: narrow_stereo_left + num_items: 2 + narrow_right: + type: diagnostic_aggregator/GenericAnalyzer + path: Narrow (Right) + find_and_remove_prefix: narrow_stereo_right + num_items: 2 + wide_left: + type: diagnostic_aggregator/GenericAnalyzer + path: Wide (Left) + find_and_remove_prefix: wide_stereo_left + num_items: 2 + wide_right: + type: diagnostic_aggregator/GenericAnalyzer + path: Wide (Right) + find_and_remove_prefix: wide_stereo_right + num_items: 2 + prosilica: + type: diagnostic_aggregator/GenericAnalyzer + path: Prosilica + find_and_remove_prefix: prosilica_driver + num_items: 4 + forearm_r: + type: diagnostic_aggregator/GenericAnalyzer + path: Forearm (Right) + find_and_remove_prefix: r_forearm_cam + num_items: 2 + forearm_l: + type: diagnostic_aggregator/GenericAnalyzer + path: Forearm (Left) + find_and_remove_prefix: l_forearm_cam + num_items: 2 + kinect_head: + type: diagnostic_aggregator/DiscardAnalyzer + path: Kinect (Head) + find_and_remove_prefix: kinect_head + computers: + type: diagnostic_aggregator/AnalyzerGroup + path: Computers + analyzers: + cpu: + type: diagnostic_aggregator/GenericAnalyzer + path: CPU + expected: ['c1 CPU Temperature', + 'c1 CPU Usage', + 'c1 HD Temperature', + 'c1 HD Usage', + 'c2 CPU Temperature', + 'c2 CPU Usage', + 'c2 HD Temperature'] + motors: + type: diagnostic_aggregator/GenericAnalyzer + path: Motors + startswith: [ + 'EtherCAT Device'] + expected: [ + 'EtherCAT Master', + 'EtherCAT Device (bl_caster_l_wheel_motor)', + 'EtherCAT Device (bl_caster_r_wheel_motor)', + 'EtherCAT Device (bl_caster_rotation_motor)', + 'EtherCAT Device (br_caster_l_wheel_motor)', + 'EtherCAT Device (br_caster_r_wheel_motor)', + 'EtherCAT Device (br_caster_rotation_motor)', + 'EtherCAT Device (fl_caster_l_wheel_motor)', + 'EtherCAT Device (fl_caster_r_wheel_motor)', + 'EtherCAT Device (fl_caster_rotation_motor)', + 'EtherCAT Device (fr_caster_l_wheel_motor)', + 'EtherCAT Device (fr_caster_r_wheel_motor)', + 'EtherCAT Device (fr_caster_rotation_motor)', + 'EtherCAT Device (laser_tilt_mount_motor)', + 'EtherCAT Device (head_pan_motor)', + 'EtherCAT Device (head_tilt_motor)', + 'EtherCAT Device (led_projector)', + 'EtherCAT Device (torso_lift_motor)', + 'EtherCAT Device (r_gripper_motor)', + 'EtherCAT Device (r_wrist_l_motor)', + 'EtherCAT Device (r_wrist_r_motor)', + 'EtherCAT Device (r_forearm_roll_motor)', + 'EtherCAT Device (r_elbow_flex_motor)', + 'EtherCAT Device (r_upper_arm_roll_motor)', + 'EtherCAT Device (r_shoulder_lift_motor)', + 'EtherCAT Device (r_shoulder_pan_motor)', + 'EtherCAT Device (l_gripper_motor)', + 'EtherCAT Device (l_wrist_l_motor)', + 'EtherCAT Device (l_wrist_r_motor)', + 'EtherCAT Device (l_forearm_roll_motor)', + 'EtherCAT Device (l_elbow_flex_motor)', + 'EtherCAT Device (l_upper_arm_roll_motor)', + 'EtherCAT Device (l_shoulder_lift_motor)', + 'EtherCAT Device (l_shoulder_pan_motor)'] + joints: + type: diagnostic_aggregator/GenericAnalyzer + path: Joints + startswith: [ + 'Joint'] + expected: [ + 'Joint (bl_caster_l_wheel_joint)', + 'Joint (bl_caster_r_wheel_joint)', + 'Joint (bl_caster_rotation_joint)', + 'Joint (br_caster_l_wheel_joint)', + 'Joint (br_caster_r_wheel_joint)', + 'Joint (br_caster_rotation_joint)', + 'Joint (fl_caster_l_wheel_joint)', + 'Joint (fl_caster_r_wheel_joint)', + 'Joint (fl_caster_rotation_joint)', + 'Joint (fr_caster_l_wheel_joint)', + 'Joint (fr_caster_r_wheel_joint)', + 'Joint (fr_caster_rotation_joint)', + 'Joint (laser_tilt_mount_joint)', + 'Joint (head_pan_joint)', + 'Joint (head_tilt_joint)', + 'Joint (torso_lift_joint)', + 'Joint (r_gripper_joint)', + 'Joint (r_wrist_flex_joint)', + 'Joint (r_wrist_roll_joint)', + 'Joint (r_forearm_roll_joint)', + 'Joint (r_elbow_flex_joint)', + 'Joint (r_upper_arm_roll_joint)', + 'Joint (r_shoulder_lift_joint)', + 'Joint (r_shoulder_pan_joint)', + 'Joint (l_gripper_joint)', + 'Joint (l_wrist_flex_joint)', + 'Joint (l_wrist_roll_joint)', + 'Joint (l_forearm_roll_joint)', + 'Joint (l_elbow_flex_joint)', + 'Joint (l_upper_arm_roll_joint)', + 'Joint (l_shoulder_lift_joint)', + 'Joint (l_shoulder_pan_joint)'] + power: + type: diagnostic_aggregator/GenericAnalyzer + path: 'Power System' + timeout: 10 + num_items: 21 + discard_stale: true + expected: [ + 'IBPS 0', + 'IBPS 1', + 'IBPS 2', + 'IBPS 3', + 'Smart Battery 0.0', + 'Smart Battery 0.1', + 'Smart Battery 0.2', + 'Smart Battery 0.3', + 'Smart Battery 1.0', + 'Smart Battery 1.1', + 'Smart Battery 1.2', + 'Smart Battery 1.3', + 'Smart Battery 2.0', + 'Smart Battery 2.1', + 'Smart Battery 2.2', + 'Smart Battery 2.3', + 'Smart Battery 3.0', + 'Smart Battery 3.1', + 'Smart Battery 3.2', + 'Smart Battery 3.3'] + startswith: [ + 'Power board', + 'IBPS', + 'Smart Battery'] + + controllers: + type: diagnostic_aggregator/GenericAnalyzer + path: 'Realtime Controllers' + expected: [ + 'Realtime Control Loop', + 'Controller (head_camera_trigger)', + 'Controller (projector_controller)', + 'Controller (projector_trigger)', + 'Controller (prosilica_inhibit_projector_controller)', + 'Controller (r_forearm_cam_trigger)', + 'Controller (l_forearm_cam_trigger)' ] + startswith: [ + 'Controller', + 'Calibration'] + discard_stale: true diff --git a/jsk_pr2_robot/jsk_pr2_startup/pr2_bringup.launch b/jsk_pr2_robot/jsk_pr2_startup/pr2_bringup.launch index d7126e6921..aca0a049d0 100644 --- a/jsk_pr2_robot/jsk_pr2_startup/pr2_bringup.launch +++ b/jsk_pr2_robot/jsk_pr2_startup/pr2_bringup.launch @@ -1,7 +1,8 @@ - + + @@ -116,8 +117,10 @@ if="$(arg launch_runtime_logger)" /> - - + + + + @@ -127,11 +130,12 @@ - + + - - + + @@ -145,7 +149,9 @@ - + + + From 80305e7cba8f41dcfeff4025ed8bb6cd69dec838 Mon Sep 17 00:00:00 2001 From: Yuki Furuta Date: Mon, 16 Jul 2018 00:33:37 +0900 Subject: [PATCH 09/70] jsk_pr2_startup: move sound jp to bringup launch --- jsk_pr2_robot/jsk_pr2_startup/pr2.launch | 10 ---------- .../jsk_pr2_startup/pr2_bringup.launch | 17 ++++++++++++++--- 2 files changed, 14 insertions(+), 13 deletions(-) diff --git a/jsk_pr2_robot/jsk_pr2_startup/pr2.launch b/jsk_pr2_robot/jsk_pr2_startup/pr2.launch index b83eedf08c..f1ee6d23a5 100644 --- a/jsk_pr2_robot/jsk_pr2_startup/pr2.launch +++ b/jsk_pr2_robot/jsk_pr2_startup/pr2.launch @@ -8,7 +8,6 @@ - @@ -55,15 +54,6 @@ - - - - - - - - diff --git a/jsk_pr2_robot/jsk_pr2_startup/pr2_bringup.launch b/jsk_pr2_robot/jsk_pr2_startup/pr2_bringup.launch index aca0a049d0..cc980d71fd 100644 --- a/jsk_pr2_robot/jsk_pr2_startup/pr2_bringup.launch +++ b/jsk_pr2_robot/jsk_pr2_startup/pr2_bringup.launch @@ -5,6 +5,7 @@ + @@ -107,9 +108,19 @@ - - - + + + + + + + + + + + + + Date: Mon, 16 Jul 2018 15:34:58 +0900 Subject: [PATCH 10/70] jsk_pr2_startup: enable approximate sync for logging --- jsk_pr2_robot/jsk_pr2_startup/jsk_pr2_lifelog/db_client.launch | 1 + 1 file changed, 1 insertion(+) diff --git a/jsk_pr2_robot/jsk_pr2_startup/jsk_pr2_lifelog/db_client.launch b/jsk_pr2_robot/jsk_pr2_startup/jsk_pr2_lifelog/db_client.launch index 7e1d727d71..86387c3b3a 100644 --- a/jsk_pr2_robot/jsk_pr2_startup/jsk_pr2_lifelog/db_client.launch +++ b/jsk_pr2_robot/jsk_pr2_startup/jsk_pr2_lifelog/db_client.launch @@ -24,6 +24,7 @@ + From 1ae03d9b56b7ba33cf2db01595fce668f829b17e Mon Sep 17 00:00:00 2001 From: Yuki Furuta Date: Thu, 9 Aug 2018 19:12:54 +0900 Subject: [PATCH 11/70] add app_manager tag --- jsk_fetch_robot/jsk_fetch_startup/package.xml | 2 ++ jsk_pr2_robot/jsk_pr2_startup/package.xml | 1 + 2 files changed, 3 insertions(+) diff --git a/jsk_fetch_robot/jsk_fetch_startup/package.xml b/jsk_fetch_robot/jsk_fetch_startup/package.xml index 65da1c4f40..250df66460 100644 --- a/jsk_fetch_robot/jsk_fetch_startup/package.xml +++ b/jsk_fetch_robot/jsk_fetch_startup/package.xml @@ -30,6 +30,7 @@ move_base + app_manager fetch_bringup safe_teleop_base joy @@ -39,5 +40,6 @@ rostest + diff --git a/jsk_pr2_robot/jsk_pr2_startup/package.xml b/jsk_pr2_robot/jsk_pr2_startup/package.xml index fea042a85c..bba6aa5cd0 100644 --- a/jsk_pr2_robot/jsk_pr2_startup/package.xml +++ b/jsk_pr2_robot/jsk_pr2_startup/package.xml @@ -74,5 +74,6 @@ + From b5c4f3fa70c5db83401b5cfb62d21cfeab8b8ffc Mon Sep 17 00:00:00 2001 From: Yuki Furuta Date: Mon, 17 Sep 2018 17:42:52 +0900 Subject: [PATCH 12/70] update jsk_pr2_startup --- .../jsk_pr2_warning/battery_warning.py | 68 +++++++++++-------- jsk_pr2_robot/jsk_pr2_startup/package.xml | 1 + .../jsk_pr2_startup/pr2_bringup.launch | 1 + 3 files changed, 41 insertions(+), 29 deletions(-) diff --git a/jsk_pr2_robot/jsk_pr2_startup/jsk_pr2_warning/battery_warning.py b/jsk_pr2_robot/jsk_pr2_startup/jsk_pr2_warning/battery_warning.py index 768b0a9398..dd08620479 100755 --- a/jsk_pr2_robot/jsk_pr2_startup/jsk_pr2_warning/battery_warning.py +++ b/jsk_pr2_robot/jsk_pr2_startup/jsk_pr2_warning/battery_warning.py @@ -27,9 +27,9 @@ 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.warn_repeat_rate = rospy.get_param("~warn_repeat_rate", 120) + self.warn_repeat_rate = rospy.get_param("~warn_repeat_rate", 180) self.log_rate = rospy.get_param("~log_rate", 10) self.log_path = rospy.get_param("~log_path", None) if self.log_path is None: @@ -47,9 +47,10 @@ 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(): + 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 @@ -81,33 +82,42 @@ def stat_cb(self, event): if self.latest_status is None: return - max_temp = df["Temperature (C)"].astype(float).max() - rospy.logdebug("temperature: %s" % max_temp) - if 60 > max_temp > self.warning_temp: - self.speak("バッテリ温度%.1f度。暑いです。部屋の温度を下げてください。" % max_temp) - - plugged_in = df["Power Present"].eq("True").any() - if self.prev_plugged_in is None: - self.prev_plugged_in = not plugged_in - prev_plugged_in, self.prev_plugged_in = self.prev_plugged_in, plugged_in - if plugged_in: - if not prev_plugged_in: - attf_max = df["Average Time To Full (min)"].astype(int).max() - rospy.loginfo("Average Time to full: %s" % attf_max) - if attf_max > 0: - self.speak("フル充電まで%s分です。" % attf_max) - return + try: + max_temp = df["Temperature (C)"].astype(float).max() + rospy.logdebug("temperature: %s" % max_temp) + if 60 > max_temp > self.warning_temp: + self.speak("バッテリ温度%.1f度。暑いです。部屋の温度を下げてください。" % max_temp) + except KeyError: + pass - rc = df["Remaining Capacity (mAh)"].astype(float).sub(self.min_capacity) - fc = df["Full Charge Capacity (mAh)"].astype(int).sub(self.min_capacity) - min_perc = int(rc.div(fc).min() * 100.0) + try: + plugged_in = df["Power Present"].eq("True").any() + if self.prev_plugged_in is None: + self.prev_plugged_in = not plugged_in + prev_plugged_in, self.prev_plugged_in = self.prev_plugged_in, plugged_in + if plugged_in: + if not prev_plugged_in: + attf_max = df["Average Time To Full (min)"].astype(int).max() + rospy.loginfo("Average Time to full: %s" % attf_max) + if attf_max > 0: + self.speak("フル充電まで%s分です。" % attf_max) + return + except KeyError: + pass - if prev_plugged_in or min_perc < 50: - self.speak("電池残り%s％です。" % min_perc) - if 15 < min_perc < 30: - self.speak("充電してください。") - elif 0 <= min_perc < 15: - self.speak("もう限界です！") + try: + rc = df["Remaining Capacity (mAh)"].astype(float).sub(self.min_capacity) + fc = df["Full Charge Capacity (mAh)"].astype(int).sub(self.min_capacity) + min_perc = int(rc.div(fc).min() * 100.0) + + if (prev_plugged_in and plugged_in) or min_perc < 50: + self.speak("電池残り%s％です。" % min_perc) + if 15 < min_perc < 30: + self.speak("充電してください。") + elif 0 <= min_perc < 15: + self.speak("もう限界です！") + except KeyError: + pass def diag_cb(self, msg): stamp = msg.header.stamp.secs diff --git a/jsk_pr2_robot/jsk_pr2_startup/package.xml b/jsk_pr2_robot/jsk_pr2_startup/package.xml index bba6aa5cd0..ef4e156b16 100644 --- a/jsk_pr2_robot/jsk_pr2_startup/package.xml +++ b/jsk_pr2_robot/jsk_pr2_startup/package.xml @@ -12,6 +12,7 @@ amcl app_manager + cmd_vel_smoother cv_bridge diagnostic_aggregator dwa_local_planner diff --git a/jsk_pr2_robot/jsk_pr2_startup/pr2_bringup.launch b/jsk_pr2_robot/jsk_pr2_startup/pr2_bringup.launch index fe7907f9a4..d1379023a7 100644 --- a/jsk_pr2_robot/jsk_pr2_startup/pr2_bringup.launch +++ b/jsk_pr2_robot/jsk_pr2_startup/pr2_bringup.launch @@ -206,6 +206,7 @@ + From 291ae8d82a17aa8b9f9950cdb174092cee05d8dd Mon Sep 17 00:00:00 2001 From: Yuki Furuta Date: Sat, 22 Sep 2018 16:18:08 +0900 Subject: [PATCH 13/70] update app_manager.launch --- .../lifelog/app_manager.launch | 18 +++++++----------- 1 file changed, 7 insertions(+), 11 deletions(-) diff --git a/jsk_robot_common/jsk_robot_startup/lifelog/app_manager.launch b/jsk_robot_common/jsk_robot_startup/lifelog/app_manager.launch index ae7d767f6f..ed1cc38888 100644 --- a/jsk_robot_common/jsk_robot_startup/lifelog/app_manager.launch +++ b/jsk_robot_common/jsk_robot_startup/lifelog/app_manager.launch @@ -1,6 +1,6 @@ - + + @@ -49,13 +49,9 @@ - - - - - + + + + + From 408ae1767bf74e9beb53ef98e323ca7f62af9cef Mon Sep 17 00:00:00 2001 From: Yuki Furuta Date: Sat, 22 Sep 2018 16:18:25 +0900 Subject: [PATCH 14/70] fix tweet_client_uptime --- .../jsk_robot_startup/lifelog/tweet_client_uptime.l | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/jsk_robot_common/jsk_robot_startup/lifelog/tweet_client_uptime.l b/jsk_robot_common/jsk_robot_startup/lifelog/tweet_client_uptime.l index 9425ec1f56..23072ecf97 100755 --- a/jsk_robot_common/jsk_robot_startup/lifelog/tweet_client_uptime.l +++ b/jsk_robot_common/jsk_robot_startup/lifelog/tweet_client_uptime.l @@ -12,10 +12,10 @@ (setq *robot-name* (ros::get-param "/active_user/robot_name")) ) -(ros::rate 0.1) -(do-until-key +(while (ros::ok) (setq *user-name* (ros::get-param "/active_user/launch_user_name") *elapsed-time* (ros::get-param "/active_user/elapsed_time")) + (ros::rate 0.1) ;; tweet depend on up time (let ((st (ros::get-param "/active_user/start_time"))) (when st From 6efcb150cc8b01676212ac0090078eb987f9d805 Mon Sep 17 00:00:00 2001 From: Yuki Furuta Date: Sat, 22 Sep 2018 16:18:35 +0900 Subject: [PATCH 15/70] fix minor issue --- jsk_robot_common/jsk_robot_startup/package.xml | 1 + jsk_robot_common/jsk_robot_startup/util/finish_launch_sound.py | 2 +- 2 files changed, 2 insertions(+), 1 deletion(-) diff --git a/jsk_robot_common/jsk_robot_startup/package.xml b/jsk_robot_common/jsk_robot_startup/package.xml index c4c32e81b5..546ee68d41 100644 --- a/jsk_robot_common/jsk_robot_startup/package.xml +++ b/jsk_robot_common/jsk_robot_startup/package.xml @@ -28,6 +28,7 @@ nodelet pointcloud_to_laserscan posedetection_msgs + pr2_mechanism_controllers rosbridge_server roscpp roseus_mongo diff --git a/jsk_robot_common/jsk_robot_startup/util/finish_launch_sound.py b/jsk_robot_common/jsk_robot_startup/util/finish_launch_sound.py index 84a9682318..e4f230daa6 100755 --- a/jsk_robot_common/jsk_robot_startup/util/finish_launch_sound.py +++ b/jsk_robot_common/jsk_robot_startup/util/finish_launch_sound.py @@ -5,7 +5,7 @@ # topic --> /robotsound or /robotsound_jp rospy.init_node("finish_launch_sound") -p = rospy.Publisher("/robotsound", SoundRequest) +p = rospy.Publisher("/robotsound", SoundRequest, queue_size=1) rospy.sleep(5) # sleep to wait for connection msg = SoundRequest() From 3202cdb3033efe5824a7d4b76b358425dfcc0ad9 Mon Sep 17 00:00:00 2001 From: Yuki Furuta Date: Sat, 22 Sep 2018 22:49:04 +0900 Subject: [PATCH 16/70] fix active user --- .../jsk_robot_startup/lifelog/active_user.l | 340 ++++++++---------- 1 file changed, 157 insertions(+), 183 deletions(-) diff --git a/jsk_robot_common/jsk_robot_startup/lifelog/active_user.l b/jsk_robot_common/jsk_robot_startup/lifelog/active_user.l index a6fc6a208f..8c1ba85a3a 100755 --- a/jsk_robot_common/jsk_robot_startup/lifelog/active_user.l +++ b/jsk_robot_common/jsk_robot_startup/lifelog/active_user.l @@ -5,192 +5,166 @@ (ros::load-ros-manifest "sensor_msgs") (ros::load-ros-manifest "diagnostic_msgs") -(setq *start-distance* nil) -(setq *start-angle* nil) -(setq *servo-on* nil) - -(ros::roseus "active_user_statistics") - -(defun motor-cb (msg) - (ros::ros-debug "motor_state -> ~A" (send msg :data)) - (setq *servo-on* (not (send msg :data)))) - -(defun odom-cb (msg) - (ros::ros-debug "odom_state -> distance ~A, angle ~A" (send msg :distance) (send msg :angle)) - (if (or (equal *start-distance* nil) (equal *start-angle* nil)) - (setq *start-distance* (send msg :distance) - *start-angle* (send msg :angle)) - (setq *distance* (- (send msg :distance) *start-distance*) - *angle* (- (send msg :angle) *start-angle*)))) - -(defun joint-cb (msg) - ;; - (cond ((and (ros::has-param "/robot/type") ;; for pr2 - (string= (ros::has-param "/robot/type") "pr2")) - (setq *position* (concatenate float-vector (subseq (send msg :position) 12 16) (subseq (send msg :position) 17)))) - ((ros::has-param "/rethink/software_version") ;; baxter - (if (< (length (send msg :position)) 12) - (return-from joint-cb nil)) - (setq *position* (send msg :position))) - (t - (setq *position* (send msg :position)))) - (update-activeness) - ) - -(when (ros::has-param "/active_user/motor_subscribe") - (if (ros::get-param "/active_user/motor_subscribe") - (ros::subscribe "motor_state" std_msgs::Bool #'motor-cb 1) - (setq *servo-on* t) - ) - ) - -(when (ros::has-param "/active_user/odom_subscribe") - (if (ros::get-param "/active_user/odom_subscribe") - (ros::subscribe "odom_state" pr2_mechanism_controllers::Odometer #'odom-cb 1) - ) - ) - -(when (ros::has-param "/active_user/joint_state_subscribe") - (if (ros::get-param "/active_user/joint_state_subscribe") - (ros::subscribe "joint_states" sensor_msgs::JointState #'joint-cb 1) - ) - ) - -(when (ros::has-param "/active_user/position_diff_threshold") - (setq *position-diff-threshold* (ros::get-param "/active_user/position_diff_threshold"))) - - - -(ros::advertise "diagnostics" diagnostic_msgs::DiagnosticArray 1) - -(setq *prev-distance* nil *prev-angle* nil *prev-position* nil) -(setq *distance* nil *angle* nil *position* nil) - -(setq *status* 'stop) -(setq *movingp* nil) -(setq *start-time* (ros::time 0)) -(setq *elapsed-sec* 0) -(setq *seq* 0) - -(when (ros::has-param "/active_user/launch_user_name") - (let ((tname (ros::get-param "/active_user/launch_user_name"))) - (warn "~%;; launch_user_name = ~A~%" tname) - (unless (string= tname "false") - (setq *user-name* tname)))) -(unless (and (boundp '*user-name*) *user-name*) - (unix::system - (format nil "getent passwd `whoami` | cut -d ':' -f 5 | cut -d ',' -f 1 > /tmp/username_~d.txt" - (unix::getpid))) - (warn "Get PID ~A" (unix::getpid)) - (with-open-file - (f (format nil "/tmp/username_~d.txt" (unix::getpid))) - (setq *user-name* (read-line f))) - (ros::set-param "/active_user/launch_user_name" *user-name*) +(defparameter *init-status* '(nil)) +(defparameter *prev-status* '(nil)) +(defparameter *current-status* '(nil)) +(defparameter *diagnostic-seq* 0) +(defparameter *position-diff-threshold* 0.05) + +(defun set-alist (k v alist &key (key 'car) (test 'eq)) + (let ((cur-cons (assoc k alist :key key :test test))) + (if cur-cons + (progn (setf (cdr cur-cons) v) alist) + (nconc alist (list (cons k v)))))) + +(defun status-cb (key msg) + (unless (assoc key *init-status*) + (set-alist key msg *init-status*)) + (when (assoc key *current-status*) + (set-alist key (cdr (assoc key *current-status*)) *prev-status*)) + (set-alist key msg *current-status*)) + +(defun init-status () + (setq *position-diff-threshold* (ros::get-param "/active_user/position_diff_threshold" 0.05)) + (setq *init-status* '(nil)) + ;; user name + (let ((name (ros::get-param "/active_user/launch_user_name" nil))) + (when (or (null name) (null-string-p name) (string= name "false")) + (unix::system + (format nil "getent passwd `whoami` | cut -d ':' -f 5 | cut -d ',' -f 1 > /tmp/username_~d.txt" + (unix::getpid))) + (with-open-file + (f (format nil "/tmp/username_~d.txt" (unix::getpid))) + (setq name (read-line f))) + (when (or (null name) (null-string-p name)) + (setq name *user*))) + (set-alist :user-name name *init-status*)) + ;; robot name + (let ((name (ros::get-param "/robot/type" nil))) + (when (or (null name) (string= name "false")) + (if (ros::has-param "/rethink/software_version") + (setq name "baxter"))) + (set-alist :robot-name name *init-status*)) + ;; elapsed time + (set-alist :elapsed-time + (ros::time (ros::get-param "/active_user/elapsed_time" 0)) + *init-status*) + ;; start time + (let ((cur-time (send (ros::time-now) :to-sec))) + (set-alist :start-time + (ros::time (ros::get-param "/active_user/start_time" cur-time)) + *init-status*) + (ros::set-param "/active_user/start_time" cur-time)) ) -(warn "~%;; start user_name = ~A~%" *user-name*) -(if (and (ros::has-param "/active_user/elapsed_time") - (ros::has-param "/active_user/launch_user_name") - (string= (ros::get-param "/active_user/launch_user_name") *user-name*)) - (setq *elapsed* (ros::time (ros::get-param "/active_user/elapsed_time"))) - (setq *elapsed* (ros::time 0))) -(warn "~%;; start elapsed_time with ~A sec~%~%" *elapsed*);; - -(let ((tm-now (ros::time-now))) - (cond - ((and (ros::has-param "/active_user/start_time") - (not (ros::time= *elapsed* (ros::time 0)))) - (setq *start-time* (ros::time (ros::get-param "/active_user/start_time")))) - (t - (setq *start-time* tm-now) - (ros::set-param "/active_user/start_time" (send *start-time* :to-sec))) - )) - -(defun update-activeness() - (ros::ros-debug "user -> ~A" *user-name*) - (ros::ros-debug "status-> ~A" *status*) - (ros::ros-debug "moving-> ~A" *movingp*) - (setq *odom-disable* (not (ros::get-param "/active_user/odom_subscribe"))) - ;; check if the robot is moving - (when (and *user-name* (or (and *prev-distance* *prev-angle* *prev-position*) *odom-disable*)) - (let ((diff-distance (if *odom-disable* nil (- *distance* *prev-distance*))) - (diff-angle (if *odom-disable* nil (- *angle* *prev-angle*))) - (diff-position (if (not *prev-position*) nil (norm (v- *position* *prev-position*))))) - ;; check servo on - (ros::ros-debug " servo on -> ~A" *servo-on*) - ;; check move_base - (if (not *odom-disable*) - (ros::ros-debug " move base -> ~A ~A" diff-distance diff-angle)) - ;; check arms - (ros::ros-debug " joint-angle -> ~A" diff-position) - ;;for odom-enable machine - (if (and *servo-on* - (or (> diff-distance 0.001) (> diff-angle 0.001) (> diff-position *position-diff-threshold*)) - (not *odom-disable*)) - (setq *movingp* t) - ;;for odom-disable machine - (if (and (and (numberp diff-position) (> diff-position *position-diff-threshold*) ) - *odom-disable*) - (setq *movingp* t) - (setq *movingp* nil)) - ) - - ;; +(defun joint-position (robot msg) + (setq robot (string-downcase robot)) + (when msg + (cond + ((string= robot "pr2") + (concatenate float-vector + (subseq (send msg :position) 12 16) + (subseq (send msg :position) 17))) + ((string= robot "baxter") + (if (< (length (send msg :position)) 12) + nil (send msg :position))) + (t (send msg :position))))) + +(defun publish-diagnostics (username elapsed) + (when (derivedp elapsed ros::time) + (setq elapsed (format nil "~A" (send elapsed :to-sec)))) + (let (kvs) + (when username + (push (instance diagnostic_msgs::KeyValue :init + :key "User Name" + :value username) kvs)) + (when elapsed + (push (instance diagnostic_msgs::KeyValue :init + :key "Usage Time" + :value elapsed) kvs)) + (ros::publish "diagnostics" + (instance diagnostic_msgs::DiagnosticArray :init + :header (instance std_msgs::Header :init :seq (incf *diagnostic-seq*) :stamp (ros::time-now)) + :status (list + (instance diagnostic_msgs::DiagnosticStatus :init + :level 0 + :name"Active User" + :message (format nil "~A ~A sec" username elapsed) + :values kvs)))))) + +(defun update () + (let ((movingp nil) (servo-on nil)) + (ros::ros-info "current: ~A" *current-status*) + ;; motor + (cond + ((assoc :motor *current-status*) + (setq servo-on (send (cdr (assoc :motor *current-status*)) :data))) + (t (setq servo-on t))) + ;; + (when servo-on + ;; odom + (let ((odom (cdr (assoc :odom *current-status*))) + (prev-odom (cdr (assoc :odom *prev-status*))) + (diff-distance 0.0) + (diff-angle 0.0)) + (when (and odom prev-odom) + (setq diff-distance (apply #'- (send-all (list odom prev-odom) :distance))) + (setq diff-angle (apply #'- (send-all (list odom prev-odom) :angle))) + (when (or (> diff-distance 0.001) (> diff-angle 0.001)) + (setq movingp t)))) + ;; joint + (unless movingp + (let* ((robot-name (cdr (assoc :robot-name *init-status*))) + (pos (joint-position robot-name (cdr (assoc :joint *current-status*)))) + (prev-pos (joint-position robot-name (cdr (assoc :joint *prev-status*)))) + (diff-position 0.0)) + (when (and pos prev-pos) + (setq diff-position (norm (v- pos prev-pos))) + (when (> diff-position *position-diff-threshold*) + (setq movingp t)))))) + ;; + (let ((prev-movingp (cdr (assoc :movingp *prev-status*))) + (start-time (or (cdr (assoc :start-time *current-status*)) + (cdr (assoc :start-time *init-status*)))) + (elapsed (or (cdr (assoc :elapsed-time *current-status*)) + (cdr (assoc :elapsed-time *init-status*))))) (cond - ((and (eq *status* 'stop) *movingp*) ;; stop->move - (ros::ros-debug "[active_user] start moving") - (setq *status* 'move) - (setq *start-time* (ros::time-now)) - (setq *tmp-elapsed* *elapsed*) - ) - ((and (eq *status* 'stop) (not *movingp*) (not *odom-disable*)) ;; stop->stop - (setq *tmp-elapsed* (ros::time 0)) - (ros::ros-debug "~A :tmp-elapsed" *tmp-elapsed*) - ) - ((and (eq *status* 'move) *movingp*) ;; move-> move - ;; temp elapsed - (setq *tmp-elapsed* (ros::time+ *elapsed* (ros::time- (ros::time-now) *start-time*))) - ) - ((and (eq *status* 'move) (not *movingp*)) ;; move-stop - (ros::ros-debug "[active user] stop moving") - (setq *status* 'stop) - (setq *elapsed* (ros::time+ *elapsed* (ros::time- (ros::time-now) *start-time*))) - ) - ) - (setq *elapsed-sec* (send (if *movingp* *tmp-elapsed* *elapsed*) :to-sec)) - (ros::ros-debug " status -> ~A (currently moving? ~A), elapsed ~A" *status* *movingp* *elapsed-sec*) - (ros::publish "diagnostics" - (instance diagnostic_msgs::DiagnosticArray :init - :header (instance std_msgs::Header :init :seq *seq* :stamp (ros::time-now)) - :status (list - (instance diagnostic_msgs::DiagnosticStatus :init - :level 0 - :name"Active User" - :message (format nil "~A ~A sec" *user-name* *elapsed-sec*) - :values (list - (instance diagnostic_msgs::KeyValue :init - :key "User Name" - :value *user-name*) - (instance diagnostic_msgs::KeyValue :init - :key "Usage Time" - :value *elapsed-sec*)))))) - (incf *seq*) - )) - - (if (not *odom-disable*) - (setq *prev-distance* *distance* - *prev-angle* *angle* - *prev-position* *position*) - (setq *prev-position* *position*) - ) - (when (and (> (length *user-name*) 0) *elapsed-sec*) - (ros::set-param "/active_user/user_name" *user-name*) - (ros::set-param "/active_user/elapsed_time" *elapsed-sec*)) - ) + ((and (not prev-movingp) movingp) + ;; stop -> move + (setq start-time (ros::time-now))) + ((and prev-movingp (not movingp)) + ;; move -> stop + (setq elapsed (ros::time+ elapsed (ros::time- (ros::time-now) start-time))))) + ;; + (set-alist :start-time start-time *current-status*) + (set-alist :elapsed-time elapsed *current-status*) + ;; + (publish-diagnostics (cdr (assoc :user-name *init-status*)) elapsed) + (ros::set-param "/active_user/elapsed_time" (send elapsed :to-sec))) -(ros::rate 1) -(while (ros::ok) - (ros::spin-once) - (ros::sleep)) + (set-alist :movingp (cdr (assoc :movingp *current-status*)) *prev-status*) + (set-alist :movingp movingp *current-status*) + )) +(defun main () + (ros::roseus "active_user_statistics") + ;; + (init-status) + ;; + (ros::advertise "diagnostics" diagnostic_msgs::DiagnosticArray 1) + ;; + (when (ros::get-param "/active_user/motor_subscribe" t) + (ros::subscribe "motor_state" std_msgs::Bool #'status-cb :motor 1)) + (when (ros::get-param "/active_user/odom_subscribe" t) + (ros::subscribe "odom_state" pr2_mechanism_controllers::Odometer #'status-cb :odom 1)) + (when (ros::get-param "/active_user/joint_state_subscribe" t) + (ros::subscribe "joint_states" sensor_msgs::JointState #'status-cb :joint 1)) + + (ros::rate 0.1) + (while (ros::ok) + (ros::spin-once) + (update) + (ros::sleep))) + +(main) (exit) From 7c4b5bbbd78f167848b7f4fc2375349d11b95411 Mon Sep 17 00:00:00 2001 From: Yuki Furuta Date: Sat, 22 Sep 2018 22:49:20 +0900 Subject: [PATCH 17/70] fix battery_warning --- .../jsk_pr2_startup/jsk_pr2_warning/battery_warning.py | 6 ++++++ 1 file changed, 6 insertions(+) diff --git a/jsk_pr2_robot/jsk_pr2_startup/jsk_pr2_warning/battery_warning.py b/jsk_pr2_robot/jsk_pr2_startup/jsk_pr2_warning/battery_warning.py index dd08620479..5caad7ee86 100755 --- a/jsk_pr2_robot/jsk_pr2_startup/jsk_pr2_warning/battery_warning.py +++ b/jsk_pr2_robot/jsk_pr2_startup/jsk_pr2_warning/battery_warning.py @@ -89,6 +89,8 @@ def stat_cb(self, event): self.speak("バッテリ温度%.1f度。暑いです。部屋の温度を下げてください。" % max_temp) except KeyError: pass + except ValueError: + pass try: plugged_in = df["Power Present"].eq("True").any() @@ -104,6 +106,8 @@ def stat_cb(self, event): return except KeyError: pass + except ValueError: + pass try: rc = df["Remaining Capacity (mAh)"].astype(float).sub(self.min_capacity) @@ -118,6 +122,8 @@ def stat_cb(self, event): self.speak("もう限界です！") except KeyError: pass + except ValueError: + pass def diag_cb(self, msg): stamp = msg.header.stamp.secs From 575342256ee256d1b55058f29c3d6118b53206d4 Mon Sep 17 00:00:00 2001 From: Yuki Furuta Date: Sun, 23 Sep 2018 12:41:18 +0900 Subject: [PATCH 18/70] fix active user --- jsk_robot_common/jsk_robot_startup/lifelog/active_user.l | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/jsk_robot_common/jsk_robot_startup/lifelog/active_user.l b/jsk_robot_common/jsk_robot_startup/lifelog/active_user.l index 8c1ba85a3a..a1a9a7a263 100755 --- a/jsk_robot_common/jsk_robot_startup/lifelog/active_user.l +++ b/jsk_robot_common/jsk_robot_startup/lifelog/active_user.l @@ -94,7 +94,7 @@ (defun update () (let ((movingp nil) (servo-on nil)) - (ros::ros-info "current: ~A" *current-status*) + (ros::ros-debug "current: ~A" *current-status*) ;; motor (cond ((assoc :motor *current-status*) From c13d1ac4eb8d3dd41af536ce230065787a379abb Mon Sep 17 00:00:00 2001 From: Yuki Furuta Date: Sun, 23 Sep 2018 14:41:40 +0900 Subject: [PATCH 19/70] fix battery_warning.py --- .../jsk_pr2_startup/jsk_pr2_warning/battery_warning.py | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/jsk_pr2_robot/jsk_pr2_startup/jsk_pr2_warning/battery_warning.py b/jsk_pr2_robot/jsk_pr2_startup/jsk_pr2_warning/battery_warning.py index 5caad7ee86..b77061ac59 100755 --- a/jsk_pr2_robot/jsk_pr2_startup/jsk_pr2_warning/battery_warning.py +++ b/jsk_pr2_robot/jsk_pr2_startup/jsk_pr2_warning/battery_warning.py @@ -112,7 +112,7 @@ def stat_cb(self, event): try: rc = df["Remaining Capacity (mAh)"].astype(float).sub(self.min_capacity) fc = df["Full Charge Capacity (mAh)"].astype(int).sub(self.min_capacity) - min_perc = int(rc.div(fc).min() * 100.0) + min_perc = int(rc.where(rc > 0).div(fc).min() * 100.0) if (prev_plugged_in and plugged_in) or min_perc < 50: self.speak("電池残り%s％です。" % min_perc) From d7ea61c860809f5b932664cd90a48c2075233cc6 Mon Sep 17 00:00:00 2001 From: Yuki Furuta Date: Tue, 25 Sep 2018 20:26:55 +0900 Subject: [PATCH 20/70] minor fix --- jsk_pr2_robot/jsk_pr2_startup/jsk_pr2_lifelog/pr2-tweet-log.l | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/jsk_pr2_robot/jsk_pr2_startup/jsk_pr2_lifelog/pr2-tweet-log.l b/jsk_pr2_robot/jsk_pr2_startup/jsk_pr2_lifelog/pr2-tweet-log.l index 18f331c413..8930f3f32b 100755 --- a/jsk_pr2_robot/jsk_pr2_startup/jsk_pr2_lifelog/pr2-tweet-log.l +++ b/jsk_pr2_robot/jsk_pr2_startup/jsk_pr2_lifelog/pr2-tweet-log.l @@ -9,7 +9,7 @@ (require :tweet-client "package://jsk_robot_startup/lifelog/tweet_client.l") (defparameter *mongo-database* "jsk_robot_lifelog") (defparameter *mongo-collection* "pr1012") -(defparameter *json-parse-key-function* #'string->keyword) +(defparameter *json-parse-key-function* #'json::string->keyword) (setq json::*tzoffset* -8) (defvar *tweet* t) (defparameter *query-limit* 100) From 20a555aa2869f50ce521a484c939fd28849c4b9b Mon Sep 17 00:00:00 2001 From: Yuki Furuta Date: Tue, 25 Sep 2018 21:25:15 +0900 Subject: [PATCH 21/70] add respeaker to pr2.launch --- .../jsk_pr2_lifelog/pr2_hark.launch | 14 ------- .../jsk_pr2_lifelog/pr2_microphone.launch | 41 +++++++++++++++++++ jsk_pr2_robot/jsk_pr2_startup/package.xml | 1 + jsk_pr2_robot/jsk_pr2_startup/pr2.launch | 8 ++-- .../jsk_pr2_startup/pr2_gazebo.launch | 2 +- 5 files changed, 47 insertions(+), 19 deletions(-) delete mode 100644 jsk_pr2_robot/jsk_pr2_startup/jsk_pr2_lifelog/pr2_hark.launch create mode 100644 jsk_pr2_robot/jsk_pr2_startup/jsk_pr2_lifelog/pr2_microphone.launch diff --git a/jsk_pr2_robot/jsk_pr2_startup/jsk_pr2_lifelog/pr2_hark.launch b/jsk_pr2_robot/jsk_pr2_startup/jsk_pr2_lifelog/pr2_hark.launch deleted file mode 100644 index 73a3368e21..0000000000 --- a/jsk_pr2_robot/jsk_pr2_startup/jsk_pr2_lifelog/pr2_hark.launch +++ /dev/null @@ -1,14 +0,0 @@ - - - - - - diff --git a/jsk_pr2_robot/jsk_pr2_startup/jsk_pr2_lifelog/pr2_microphone.launch b/jsk_pr2_robot/jsk_pr2_startup/jsk_pr2_lifelog/pr2_microphone.launch new file mode 100644 index 0000000000..aeee9e1f70 --- /dev/null +++ b/jsk_pr2_robot/jsk_pr2_startup/jsk_pr2_lifelog/pr2_microphone.launch @@ -0,0 +1,41 @@ + + + + + + + + + + + + + + + + + + + + + + sensor_frame_id: head_mount_link + + + + + + + + language: $(arg language) + self_cancellation: true + tts_tolerance: 0.5 + + + + diff --git a/jsk_pr2_robot/jsk_pr2_startup/package.xml b/jsk_pr2_robot/jsk_pr2_startup/package.xml index ef4e156b16..70a294c6da 100644 --- a/jsk_pr2_robot/jsk_pr2_startup/package.xml +++ b/jsk_pr2_robot/jsk_pr2_startup/package.xml @@ -61,6 +61,7 @@ pr2eus python-numpy resized_image_transport + respeaker_ros robot_self_filter rosbridge_server rostwitter diff --git a/jsk_pr2_robot/jsk_pr2_startup/pr2.launch b/jsk_pr2_robot/jsk_pr2_startup/pr2.launch index 4be8a72afd..c53abbbe84 100644 --- a/jsk_pr2_robot/jsk_pr2_startup/pr2.launch +++ b/jsk_pr2_robot/jsk_pr2_startup/pr2.launch @@ -14,7 +14,7 @@ - + @@ -135,9 +135,9 @@ - - + + - + From 59f59ca0d1f83a9a6bac32acd55804f5629d7511 Mon Sep 17 00:00:00 2001 From: Furushchev Date: Thu, 27 Sep 2018 21:30:33 +0900 Subject: [PATCH 22/70] jsk_pr2_startup: change tilt min_obstacle_height 0.0 -> 0.1 --- .../jsk_pr2_move_base/costmap_common_params.yaml | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/jsk_pr2_robot/jsk_pr2_startup/jsk_pr2_move_base/costmap_common_params.yaml b/jsk_pr2_robot/jsk_pr2_startup/jsk_pr2_move_base/costmap_common_params.yaml index f1dad6d26f..60bb390928 100644 --- a/jsk_pr2_robot/jsk_pr2_startup/jsk_pr2_move_base/costmap_common_params.yaml +++ b/jsk_pr2_robot/jsk_pr2_startup/jsk_pr2_move_base/costmap_common_params.yaml @@ -30,7 +30,7 @@ obstacle_layer: clearing: true observation_persistence: 1.0 expected_update_rate: 0.25 - min_obstacle_height: 0.0 + min_obstacle_height: 0.1 max_obstacle_height: 2.0 obstacle_range: 2.5 raytrace_range: 3.0 From 2ed28e307bf6c4154be9599bb5ab0ff6f315900b Mon Sep 17 00:00:00 2001 From: Furushchev Date: Fri, 12 Oct 2018 11:12:43 +0900 Subject: [PATCH 23/70] jsk_pr2_startup: use 2d costmap --- .../jsk_pr2_move_base/global_costmap_params.yaml | 2 +- .../jsk_pr2_startup/jsk_pr2_move_base/local_costmap_params.yaml | 2 +- 2 files changed, 2 insertions(+), 2 deletions(-) diff --git a/jsk_pr2_robot/jsk_pr2_startup/jsk_pr2_move_base/global_costmap_params.yaml b/jsk_pr2_robot/jsk_pr2_startup/jsk_pr2_move_base/global_costmap_params.yaml index b3fd1d494b..76a847bbed 100644 --- a/jsk_pr2_robot/jsk_pr2_startup/jsk_pr2_move_base/global_costmap_params.yaml +++ b/jsk_pr2_robot/jsk_pr2_startup/jsk_pr2_move_base/global_costmap_params.yaml @@ -1,6 +1,6 @@ plugins: - {name: static_layer, type: "costmap_2d::StaticLayer" } - - {name: obstacle_layer, type: "costmap_2d::VoxelLayer" } + - {name: obstacle_layer, type: "costmap_2d::ObstacleLayer" } - {name: inflation_layer, type: "costmap_2d::InflationLayer" } footprint: [[-0.325, -0.325], [-0.425, -0.175], [-0.425, 0.175], [-0.325, 0.325], [0.325, 0.325], [0.375, 0.0], [0.325, -0.325]] diff --git a/jsk_pr2_robot/jsk_pr2_startup/jsk_pr2_move_base/local_costmap_params.yaml b/jsk_pr2_robot/jsk_pr2_startup/jsk_pr2_move_base/local_costmap_params.yaml index dc0ada58b8..1560f39d71 100644 --- a/jsk_pr2_robot/jsk_pr2_startup/jsk_pr2_move_base/local_costmap_params.yaml +++ b/jsk_pr2_robot/jsk_pr2_startup/jsk_pr2_move_base/local_costmap_params.yaml @@ -1,5 +1,5 @@ plugins: - - {name: obstacle_layer, type: "costmap_2d::VoxelLayer" } + - {name: obstacle_layer, type: "costmap_2d::ObstacleLayer" } - {name: inflation_layer, type: "costmap_2d::InflationLayer" } # copied from pr2_navigation_global/config/local_costmap_params.yaml From 3db5a1f9385e87363ef59f78958fc482320d7405 Mon Sep 17 00:00:00 2001 From: Furushchev Date: Mon, 22 Oct 2018 20:04:52 +0900 Subject: [PATCH 24/70] common_logger.launch: fix duplicate arg approximate_sync --- jsk_robot_common/jsk_robot_startup/lifelog/common_logger.launch | 1 - 1 file changed, 1 deletion(-) diff --git a/jsk_robot_common/jsk_robot_startup/lifelog/common_logger.launch b/jsk_robot_common/jsk_robot_startup/lifelog/common_logger.launch index f8af5076bd..c7330e2b10 100644 --- a/jsk_robot_common/jsk_robot_startup/lifelog/common_logger.launch +++ b/jsk_robot_common/jsk_robot_startup/lifelog/common_logger.launch @@ -41,7 +41,6 @@ - From 68adc969bfec10b1d9fbb29090e677bf454884ce Mon Sep 17 00:00:00 2001 From: iory Date: Mon, 12 Nov 2018 06:21:03 +0900 Subject: [PATCH 25/70] [jsk_pr2_move_base] Add teb_local_planner_params.yaml --- .../teb_local_planner_params.yaml | 94 +++++++++++++++++++ 1 file changed, 94 insertions(+) create mode 100644 jsk_pr2_robot/jsk_pr2_startup/jsk_pr2_move_base/teb_local_planner_params.yaml diff --git a/jsk_pr2_robot/jsk_pr2_startup/jsk_pr2_move_base/teb_local_planner_params.yaml b/jsk_pr2_robot/jsk_pr2_startup/jsk_pr2_move_base/teb_local_planner_params.yaml new file mode 100644 index 0000000000..332c7ec8c6 --- /dev/null +++ b/jsk_pr2_robot/jsk_pr2_startup/jsk_pr2_move_base/teb_local_planner_params.yaml @@ -0,0 +1,94 @@ +# NOTE: When using the teb_local_planner, make sure to set the local costmap +# resolution high (for example 0.1 m), otherwise the optimization will take +# forever (around 10 minutes for each iteration). +base_local_planner: teb_local_planner/TebLocalPlannerROS + +controller_frequency: 10.0 + +TebLocalPlannerROS: + + odom_topic: odom + map_frame: /map + + # Trajectory + teb_autosize: True + dt_ref: 0.3 + dt_hysteresis: 0.1 + global_plan_overwrite_orientation: True + max_global_plan_lookahead_dist: 3.0 + feasibility_check_no_poses: 5 + allow_init_with_backwards_motion: False + + # Robot + max_vel_x: 0.5 + max_vel_y: 0.5 + max_vel_x_backwards: 0.2 + max_vel_theta: 0.5 + acc_lim_x: 0.6 + acc_lim_y: 0.6 + acc_lim_theta: 0.6 + min_turning_radius: 0.0 + footprint_model: # types: "point", "circular", "two_circles", "line", "polygon" + type: "line" + line_start: [-0.1, 0.0] # for type "line" + line_end: [0.1, 0.0] # for type "line" + +# GoalTolerance + xy_goal_tolerance: 0.03 + yaw_goal_tolerance: 0.05 + free_goal_vel: False + +# Obstacles + min_obstacle_dist: 0.3 + include_costmap_obstacles: True + costmap_obstacles_behind_robot_dist: 0.5 + obstacle_poses_affected: 10 + costmap_converter_plugin: "costmap_converter::CostmapToLinesDBSRANSAC" + costmap_converter_spin_thread: True + costmap_converter_rate: 5 + +# Optimization + no_inner_iterations: 5 + no_outer_iterations: 4 + optimization_activate: True + optimization_verbose: False + penalty_epsilon: 0.1 + weight_max_vel_x: 2 + weight_max_vel_y: 2 + weight_max_vel_theta: 1 + weight_acc_lim_x: 1 + weight_acc_lim_y: 0 + weight_acc_lim_theta: 1 + weight_kinematics_nh: 100 + weight_kinematics_forward_drive: 1 + weight_kinematics_turning_radius: 0.1 + weight_optimaltime: 1 + weight_obstacle: 50 + #weight_dynamic_obstacle: 10 # not in use yet + #selection_alternative_time_cost: False # not in use yet + +# Homotopy Class Planner + enable_homotopy_class_planning: False + enable_multithreading: True + simple_exploration: False + max_number_classes: 2 + roadmap_graph_no_samples: 15 + roadmap_graph_area_width: 5 + h_signature_prescaler: 0.5 + h_signature_threshold: 0.1 + obstacle_keypoint_offset: 0.1 + obstacle_heading_threshold: 0.45 + visualize_hc_graph: False + +## Configure plugins (namespace move_base/TebLocalPlannerROS/PLUGINNAME) +## The parameters must be added for each plugin separately +#costmap_converter/CostmapToLinesDBSRANSAC: + cluster_max_distance: 0.4 + cluster_min_pts: 2 + ransac_inlier_distance: 0.15 + ransac_min_inliers: 10 + ransac_no_iterations: 2000 + ransac_remainig_outliers: 3 + ransac_convert_outlier_pts: True + ransac_filter_remaining_outlier_pts: False + convex_hull_min_pt_separation: 0.1 From 081249a7fe411dfab85f2a4566941cbf8601915f Mon Sep 17 00:00:00 2001 From: iory Date: Mon, 12 Nov 2018 06:21:49 +0900 Subject: [PATCH 26/70] [jsk_pr2_move_base] Modified default local planner --- .../jsk_pr2_startup/jsk_pr2_move_base/move_base.xml | 8 ++++++-- 1 file changed, 6 insertions(+), 2 deletions(-) diff --git a/jsk_pr2_robot/jsk_pr2_startup/jsk_pr2_move_base/move_base.xml b/jsk_pr2_robot/jsk_pr2_startup/jsk_pr2_move_base/move_base.xml index f2c52110fe..5df84e2311 100644 --- a/jsk_pr2_robot/jsk_pr2_startup/jsk_pr2_move_base/move_base.xml +++ b/jsk_pr2_robot/jsk_pr2_startup/jsk_pr2_move_base/move_base.xml @@ -1,5 +1,6 @@ + @@ -19,11 +20,14 @@ - + + + ns="DWAPlannerROS" unless="$(arg use_teb_local_planner)" /> + From 250fb708858a5138d47808a0a7b3c7dbb6c829d4 Mon Sep 17 00:00:00 2001 From: iory Date: Mon, 12 Nov 2018 06:22:19 +0900 Subject: [PATCH 27/70] [jsk_pr2_move_base] Modified polygon of local cost map's footprint --- .../jsk_pr2_startup/jsk_pr2_move_base/local_costmap_params.yaml | 2 ++ 1 file changed, 2 insertions(+) diff --git a/jsk_pr2_robot/jsk_pr2_startup/jsk_pr2_move_base/local_costmap_params.yaml b/jsk_pr2_robot/jsk_pr2_startup/jsk_pr2_move_base/local_costmap_params.yaml index 1560f39d71..ec253b5a57 100644 --- a/jsk_pr2_robot/jsk_pr2_startup/jsk_pr2_move_base/local_costmap_params.yaml +++ b/jsk_pr2_robot/jsk_pr2_startup/jsk_pr2_move_base/local_costmap_params.yaml @@ -10,3 +10,5 @@ rolling_window: true width: 4.0 # 3.0 height: 4.0 # 3.0 resolution: 0.025 # 0.06 + +footprint: [[-0.325, -0.325], [-0.425, -0.175], [-0.425, 0.175], [-0.325, 0.325], [0.325, 0.325], [0.375, 0.0], [0.325, -0.325]] From c395b409f48965f8fc5c7d6cac741c48d3e821c6 Mon Sep 17 00:00:00 2001 From: Yuki Furuta Date: Tue, 11 Dec 2018 19:09:00 +0900 Subject: [PATCH 28/70] jsk_robot_startup: add message_store_singleton --- .../jsk_robot_startup/CMakeLists.txt | 5 +- .../message_store_singleton.h | 73 +++++++++++++++++++ .../src/message_store_singleton.cpp | 69 ++++++++++++++++++ 3 files changed, 146 insertions(+), 1 deletion(-) create mode 100644 jsk_robot_common/jsk_robot_startup/include/jsk_robot_startup/message_store_singleton.h create mode 100644 jsk_robot_common/jsk_robot_startup/src/message_store_singleton.cpp diff --git a/jsk_robot_common/jsk_robot_startup/CMakeLists.txt b/jsk_robot_common/jsk_robot_startup/CMakeLists.txt index d125e618c0..2b851269ad 100644 --- a/jsk_robot_common/jsk_robot_startup/CMakeLists.txt +++ b/jsk_robot_common/jsk_robot_startup/CMakeLists.txt @@ -61,7 +61,10 @@ endmacro() add_lifelog_nodelet(src/lightweight_logger_nodelet.cpp "jsk_robot_lifelog/LightweightLogger" "lightweight_logger") -add_library(jsk_robot_lifelog SHARED ${${PROJECT_NAME}_LIFELOG_NODELET_SRCS}) +add_library(jsk_robot_lifelog SHARED + ${${PROJECT_NAME}_LIFELOG_NODELET_SRCS} + src/message_store_singleton.cpp) + target_link_libraries(jsk_robot_lifelog ${Boost_LIBRARIES} ${OpenCV_LIBRARIES} diff --git a/jsk_robot_common/jsk_robot_startup/include/jsk_robot_startup/message_store_singleton.h b/jsk_robot_common/jsk_robot_startup/include/jsk_robot_startup/message_store_singleton.h new file mode 100644 index 0000000000..10fd85a052 --- /dev/null +++ b/jsk_robot_common/jsk_robot_startup/include/jsk_robot_startup/message_store_singleton.h @@ -0,0 +1,73 @@ +// -*- mode: C++ -*- +/********************************************************************* + * Software License Agreement (BSD License) + * + * Copyright (c) 2017, JSK Lab + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above + * copyright notice, this list of conditions and the following + * disclaimer in the documentation and/o2r other materials provided + * with the distribution. + * * Neither the name of the JSK Lab nor the names of its + * contributors may be used to endorse or promote products derived + * from this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; + * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + *********************************************************************/ +/* + * message_store_singleton.h + * Author: + */ + + +#ifndef MESSAGE_STORE_SINGLETON_H__ +#define MESSAGE_STORE_SINGLETON_H__ + +#include +#include + + +namespace jsk_robot_startup +{ +namespace lifelog +{ +class MessageStoreSingleton +{ + public: + typedef std::map M_Proxy; + static mongodb_store::MessageStoreProxy* getInstance( + const std::string& collection="message_store", + const std::string& database="message_store", + const std::string& prefix="/message_store"); + static void destroy(); + protected: + static ros::NodeHandle nh_; + static M_Proxy instances_; + static boost::mutex mutex_; + private: + // prohibit constructor + MessageStoreSingleton(MessageStoreSingleton const&) {}; + MessageStoreSingleton& operator=(MessageStoreSingleton const&) {}; +}; +} // lifelog +} // jsk_robot_startup + +#endif // MESSAGE_STORE_SINGLETON_H__ diff --git a/jsk_robot_common/jsk_robot_startup/src/message_store_singleton.cpp b/jsk_robot_common/jsk_robot_startup/src/message_store_singleton.cpp new file mode 100644 index 0000000000..0181ed1400 --- /dev/null +++ b/jsk_robot_common/jsk_robot_startup/src/message_store_singleton.cpp @@ -0,0 +1,69 @@ +// -*- mode: C++ -*- +/********************************************************************* + * Software License Agreement (BSD License) + * + * Copyright (c) 2017, JSK Lab + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above + * copyright notice, this list of conditions and the following + * disclaimer in the documentation and/o2r other materials provided + * with the distribution. + * * Neither the name of the JSK Lab nor the names of its + * contributors may be used to endorse or promote products derived + * from this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; + * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + *********************************************************************/ +/* + * message_store_singleton.cpp + * Author: + */ + +#include + +namespace jsk_robot_startup +{ +namespace lifelog +{ + mongodb_store::MessageStoreProxy* MessageStoreSingleton::getInstance( + const std::string& collection, + const std::string& database, + const std::string& prefix) { + boost::mutex::scoped_lock lock(mutex_); + std::string key = prefix + database + "/" + collection; + if (instances_.count(key) == 0) { + instances_[key] = new mongodb_store::MessageStoreProxy(nh_, collection, database, prefix); + } + return instances_[key]; + } + + void MessageStoreSingleton::destroy() { + for (M_Proxy::iterator it = instances_.begin(); it != instances_.end(); ++it) + { + if (it->second) delete it->second; + } + } + +MessageStoreSingleton::M_Proxy MessageStoreSingleton::instances_; +boost::mutex MessageStoreSingleton::mutex_; +ros::NodeHandle MessageStoreSingleton::nh_; +} // lifelog +} // jsk_robot_startup From 195d543b4994070d1d8f615ef70831efa7bf50d7 Mon Sep 17 00:00:00 2001 From: Yuki Furuta Date: Tue, 11 Dec 2018 19:10:02 +0900 Subject: [PATCH 29/70] lightweight_logger_nodelet: deferred initialize message_store_proxy instance! --- .../jsk_robot_startup/lightweight_logger.h | 9 ++-- .../src/lightweight_logger_nodelet.cpp | 45 ++++++++++++++----- 2 files changed, 40 insertions(+), 14 deletions(-) diff --git a/jsk_robot_common/jsk_robot_startup/include/jsk_robot_startup/lightweight_logger.h b/jsk_robot_common/jsk_robot_startup/include/jsk_robot_startup/lightweight_logger.h index aa84f36d6e..7f49f8233f 100644 --- a/jsk_robot_common/jsk_robot_startup/include/jsk_robot_startup/lightweight_logger.h +++ b/jsk_robot_common/jsk_robot_startup/include/jsk_robot_startup/lightweight_logger.h @@ -48,8 +48,7 @@ #include #include #include -#include - +#include namespace jsk_robot_startup { @@ -59,13 +58,17 @@ namespace jsk_robot_startup { protected: virtual void onInit(); + virtual ~LightweightLogger(); + virtual void loadThread(); virtual void inputCallback(const ros::MessageEvent& event); virtual void updateDiagnostic(diagnostic_updater::DiagnosticStatusWrapper &stat); - boost::shared_ptr msg_store_; + mongodb_store::MessageStoreProxy* msg_store_; + boost::thread deferred_load_thread_; bool wait_for_insert_; bool initialized_; std::string input_topic_name_; + std::string db_name_, col_name_; // diagnostics ros::Time init_time_; diff --git a/jsk_robot_common/jsk_robot_startup/src/lightweight_logger_nodelet.cpp b/jsk_robot_common/jsk_robot_startup/src/lightweight_logger_nodelet.cpp index 9a03449567..b538c48d75 100644 --- a/jsk_robot_common/jsk_robot_startup/src/lightweight_logger_nodelet.cpp +++ b/jsk_robot_common/jsk_robot_startup/src/lightweight_logger_nodelet.cpp @@ -48,10 +48,9 @@ namespace jsk_robot_startup initialized_ = false; jsk_topic_tools::StealthRelay::onInit(); - std::string db_name, col_name; - nh_->param("/robot/database", db_name, "jsk_robot_lifelog"); - nh_->param("/robot/name", col_name, std::string()); - if (col_name.empty()) + nh_->param("/robot/database", db_name_, "jsk_robot_lifelog"); + nh_->param("/robot/name", col_name_, std::string()); + if (col_name_.empty()) { NODELET_FATAL_STREAM("Please specify param 'robot/name' (e.g. pr1012, olive)"); return; @@ -59,10 +58,6 @@ namespace jsk_robot_startup pnh_->param("wait_for_insert", wait_for_insert_, false); - NODELET_INFO_STREAM("Connecting to database " << db_name << "/" << col_name << "..."); - msg_store_.reset(new mongodb_store::MessageStoreProxy(*nh_, col_name, db_name)); - NODELET_INFO_STREAM("Successfully connected to database!"); - input_topic_name_ = pnh_->resolveName("input", true); diagnostic_updater_.reset( @@ -71,10 +66,12 @@ namespace jsk_robot_startup diagnostic_updater_->add( "LightweightLogger::" + input_topic_name_, boost::bind(&LightweightLogger::updateDiagnostic, this, _1)); + double vital_rate; pnh_->param("vital_rate", vital_rate, 1.0); vital_checker_.reset( new jsk_topic_tools::VitalChecker(1.0 / vital_rate)); + diagnostic_updater_->start(); inserted_count_ = 0; @@ -82,6 +79,25 @@ namespace jsk_robot_startup prev_insert_error_count_ = 0; init_time_ = ros::Time::now(); + deferred_load_thread_ = boost::thread( + boost::bind(&LightweightLogger::loadThread, this)); + } + + LightweightLogger::~LightweightLogger() { + if (!initialized_) { + NODELET_DEBUG_STREAM("Shutting down deferred load thread"); + deferred_load_thread_.join(); + NODELET_DEBUG_STREAM("deferred load thread stopped"); + } + } + + void LightweightLogger::loadThread() + { + NODELET_INFO_STREAM("Connecting to database " << db_name_ << "/" << col_name_ << "..."); + msg_store_ = MessageStoreSingleton::getInstance(col_name_, db_name_); + //.reset(new mongodb_store::MessageStoreProxy(*nh_, col_name_, db_name_)); + NODELET_INFO_STREAM("Successfully connected to database!"); + initialized_ = true; } @@ -92,7 +108,6 @@ namespace jsk_robot_startup jsk_topic_tools::StealthRelay::inputCallback(msg); if (!initialized_) return; - vital_checker_->poke(); try @@ -116,8 +131,16 @@ namespace jsk_robot_startup void LightweightLogger::updateDiagnostic(diagnostic_updater::DiagnosticStatusWrapper &stat) { if (ros::Time::now() - init_time_ < ros::Duration(10.0)) { - stat.summary(diagnostic_msgs::DiagnosticStatus::OK, - getName() + " initialized"); + if (initialized_) { + stat.summary(diagnostic_msgs::DiagnosticStatus::OK, + getName() + " initialized"); + } else { + stat.summary(diagnostic_msgs::DiagnosticStatus::OK, + getName() + " is in initialization"); + } + } else if (!initialized_) { + stat.summary(diagnostic_msgs::DiagnosticStatus::ERROR, + getName() + " is taking too long to be initialized"); } else if (!vital_checker_->isAlive()) { jsk_topic_tools::addDiagnosticErrorSummary(getName(), vital_checker_, stat); } else if (insert_error_count_ != prev_insert_error_count_) { From d2fba8f0600163a6c18f0dbf56b6335451fd5705 Mon Sep 17 00:00:00 2001 From: Furushchev Date: Thu, 13 Dec 2018 16:43:36 +0900 Subject: [PATCH 30/70] add automated_logger --- .../lifelog/automated_logger.py | 596 ++++++++++++++++++ 1 file changed, 596 insertions(+) create mode 100755 jsk_robot_common/jsk_robot_startup/lifelog/automated_logger.py diff --git a/jsk_robot_common/jsk_robot_startup/lifelog/automated_logger.py b/jsk_robot_common/jsk_robot_startup/lifelog/automated_logger.py new file mode 100755 index 0000000000..34d6f8c9a6 --- /dev/null +++ b/jsk_robot_common/jsk_robot_startup/lifelog/automated_logger.py @@ -0,0 +1,596 @@ +#!/usr/bin/env python +# -*- coding: utf-8 -*- +# Author: Yuki Furuta + +from __future__ import print_function +from collections import defaultdict +import copy +import itertools +import subprocess as sp +import re +import rosgraph.masterapi +import rosservice +import rospy +import socket +import sys +import traceback +try: + from xmlrpc.client import ServerProxy +except ImportError: + from xmlrpclib import ServerProxy + +from nodelet.srv import NodeletLoad, NodeletUnload, NodeletList + + +KEYDELIM = '|' +TOPICPREFIX = 'topic:' + + +def error(*msg): + if rospy.get_name() == '/unnamed': + print(*msg, file=sys.stderr) + else: + rospy.logerr(*msg) + + +def match(regex, s): + for r in regex: + if re.match(r, s): + return True + return False + + +def entopic(name): + return TOPICPREFIX + name + + +def detopic(name): + if name.startswith(TOPICPREFIX): + name = name[len(TOPICPREFIX):] + return name + + +def escape(name): + # if name.startswith('/'): + # name = name[1:] + # return name.replace('/', '_') + return name + "_logger" + + +class Edge(object): + def __init__(self, start, end, label=None): + super(Edge, self).__init__() + + if not start or not end: + raise ValueError('invalid graph {}->{}'.format(start, end)) + self.start = start + self.end = end + if label is None: + label = str() + elif not isinstance(label, str): + raise ValueError('invalid label {}'.format(label)) + self.label = label + self.key = '{}{}{}'.format(self.start, KEYDELIM, self.end) + + def __eq__(self, e): + return self.key == e.key + + def __ne__(self, e): + return not self.__eq__(e) + + def __str__(self): + return '{}->{}'.format(self.start, self.end) + + +class EdgeList(object): + def __init__(self): + super(EdgeList, self).__init__() + + self.edges = dict() + + @property + def ranks(self): + base = self.edges.values()[0][0] + ranks = {base.start: 0, base.end: 1} + q = set([base.start, base.end]) + while q: + n = q.pop() + if n in ranks: + continue + rank = ranks[n] + edges = self.find_edge_by_node(n, upward=True, downward=True) + for edge in edges: + if edge.start == n: + ranks[edge.end] = rank + 1 + q.add(edge.end) + elif edge.end == n: + ranks[edge.start] = rank - 1 + q.add(edge.start) + else: + raise RuntimeError( + 'invalid edge {} for finding node {}'.format(edge, n)) + min_rank = min(ranks.values()) + for k in ranks: + ranks[k] += -min_rank + return ranks + + def __iter__(self): + return itertools.chain(*[v for v in self.edges.values()]) + + def __contains__(self, edge): + return edge.key in self.edges + + def add(self, edge): + key = edge.key + if key in self.edges: + edge_list = self.edges[key] + if edge in edge_list: + label_list = [e.label for e in edge_list] + if edge.label not in label_list: + edge_list.append(edge) + return True + else: + return False + else: + self.edges[key] = [edge] + return True + + def add_edge(self, start, end, label=None): + return self.add(Edge(start, end, label)) + + def add_edges(self, start, end, direction, label=None): + updated = False + if direction in ['o', 'b']: + updated = self.add_edge(start, end, label) or updated + if direction in ['i', 'b']: + updated = self.add_edge(end, start, label) or updated + return updated + + def delete(self, edge): + key = edge.key + if key in self.edges: + l = self.edges[key] + if edge in l: + l.remove(edge) + return True + + def delete_by_node(self, node): + edge_list = [v for k, v in self.edges.items()\ + if k.startswith(node + KEYDELIM) or k.endswith(KEYDELIM + node)] + for l in edge_list: + for edge in l: + self.delete(edge) + + def find_edge_by_node(self, node, upward=True, downward=True): + edges = [] + if upward: + edges += [v for k, v in self.edges.items() if k.endswith(KEYDELIM + node)] + if downward: + edges += [v for k, v in self.edges.items() if k.startswith(node + KEYDELIM)] + return itertools.chain(*edges) + + +class Graph(object): + def __init__(self, base_node): + super(Graph, self).__init__() + + self.base_node = base_node + + self.nodes = set() + self.topics = set() + self.srvs = set() + + self.blacklist_nodes = set(['^/rosout', '.*lifelog.*']) + self.blacklist_topics = set([ + '.*/bond$', # nodelets + '^/tf', '^/tf_static', # tfs + '.*/depth/.*', '.*compressed.*', '.*image_color$', # images + '/lifelog.*$', # lifelog topics + ]) + self.bad_nodes = {} + self.node_node_conn = EdgeList() + self.node_topic_conn = EdgeList() + self.node_srv_conn = EdgeList() + + name = rospy.get_name() + if name == '/unnamed': + name = '/automated_logger' + self.this_name = name + self.master = rosgraph.masterapi.Master(self.this_name) + self.node_uri_cache = dict() + self.uri_node_cache = dict() + + def add_blacklist_topic(self, topic): + self.blacklist_topics.add(topic) + + def add_blacklist_node(self, node): + self.blacklist_nodes.add(node) + + def update(self, upward=True, downward=True): + updated = self.update_nodes(upward=upward, downward=downward) + return self.update_conns() or updated + + def update_nodes(self, upward=True, downward=True): + try: + res = self.master.getSystemState() + except rosgraph.masterapi.MasterException as e: + error('Unable to contact master', str(e), traceback.format_exc()) + return False + + node_topics = defaultdict(lambda: defaultdict(set)) + topic_nodes = defaultdict(lambda: defaultdict(set)) + pubs, subs, srvs = res + for topic, nodes in pubs: + for node in nodes: + node_topics[node]["pub"].add(topic) + topic_nodes[topic]["pub"] = nodes + for topic, nodes in subs: + for node in nodes: + node_topics[node]["sub"].add(topic) + topic_nodes[topic]["sub"] = nodes + + if self.base_node not in node_topics: + raise ValueError('base_node {} not found'.format(self.base_node)) + + node_set = set() + topic_set = set() + srv_set = set() + + if downward: + q = set([self.base_node]) + while q: + n = q.pop() + for t in node_topics[n]["pub"]: + if match(self.blacklist_topics, t): + continue + added = False + for nn in topic_nodes[t]["sub"]: + if match(self.blacklist_nodes, nn): + continue + if nn not in node_set: + q.add(nn) + node_set.add(nn) + added = True + if added: + topic_set.add(t) + + if upward: + q = set([self.base_node]) + while q: + n = q.pop() + for t in node_topics[n]["sub"]: + if match(self.blacklist_topics, t): + continue + added = False + for nn in topic_nodes[t]["pub"]: + if match(self.blacklist_nodes, nn): + continue + if nn not in node_set: + q.add(nn) + node_set.add(nn) + added = True + if added: + topic_set.add(t) + node_set.add(self.base_node) + + node_srv_conn = EdgeList() + for info in srvs: + node, srv = info[1][0], info[0] + if node in node_set: + srv_set.add(srv) + node_srv_conn.add_edge(node, srv) + + self.nodes = node_set + self.topics = topic_set + self.srvs = srv_set + self.node_srv_conn = node_srv_conn + + return True + + def get_node_uri(self, node): + try: + uri = self.master.lookupNode(node) + self.node_uri_cache[node] = uri + self.uri_node_cache[uri] = node + return uri + except Exception as e: + error('Failed to lookup node', str(e), traceback.format_exc()) + if node in self.node_uri_cache: + return self.node_uri_cache[node] + else: + raise e + + def update_conns(self): + updated = False + + for node in self.nodes: + try: + uri = self.get_node_uri(node) + api = ServerProxy(uri) + except: + continue + + timeout = socket.getdefaulttimeout() + try: + if node in self.bad_nodes: + socket.setdefaulttimeout(0.2) + else: + socket.setdefaulttimeout(1.0) + code, msg, businfo = api.getBusInfo(self.this_name) + except Exception as e: + self.bad_nodes[node] = e + code = -1 + msg = traceback.format_exc() + finally: + socket.setdefaulttimeout(timeout) + + if code != 1: + error('Could not get bus info of node {}: {}'.format( + node, msg)) + continue + + for info in businfo: + if len(info) < 5: continue + + conn_id, dest_id, direction, transport, topic = info[:5] + if len(info) > 5: + connected = info[5] + else: + connected = True + + + if connected: + self.topics.add(topic) + updated = self.node_topic_conn.add_edges( + node, entopic(topic), direction) or updated + if dest_id.startswith('http://'): + dest_id = self.uri_node_cache.get(dest_id, None) + if dest_id in self.nodes and\ + not match(self.blacklist_nodes, dest_id) and\ + not match(self.blacklist_topics, topic): + updated = self.node_node_conn.add_edges( + node, dest_id, direction, topic) or updated + + for n, err in self.bad_nodes: + error('bad node {}: {}'.format(n, err)) + + return updated + + def to_graph(self, out, view=False): + import os + from graphviz import Digraph + + g = Digraph() + g.attr("graph", rankdir="LR") + for edges in self.node_node_conn.edges.values(): + start, end = edges[0].start, edges[0].end + label = ''.join([e.label + '\l' for e in edges]) + size = len(edges) + g.edge(start, end, label=label, penwidth=str(size)) + g.node(self.base_node, peripheries="2") + + # compute ranks + ranks = self.node_node_conn.ranks + rank_max = max(ranks.values()) + for i in range(rank_max): + nodes = [k for k, v in ranks.items() if v == i] + body = '{rank=same; ' + for n in nodes: + body += '"' + n + '"; ' + body += '}' + g.body.append(body) + + save_dir = os.path.dirname(os.path.abspath(out)) + if not os.path.exists(save_dir): + os.makedirs(save_dir) + g.render(out, view=view) + return True + + +class NodeletManager(object): + def __init__(self, name): + super(NodeletManager, self).__init__() + self.name = name + self.manager = None + + self._list_nodelet_timeout = rospy.Duration(5.0) + self._last_list_nodelet_stamp = rospy.Time(0) + self._nodelets = [] + + self._list_srv = rospy.ServiceProxy( + self.name + "/list", NodeletList) + self._load_srv = rospy.ServiceProxy( + self.name + "/load_nodelet", NodeletLoad) + self._unload_srv = rospy.ServiceProxy( + self.name + "/unload_nodelet", NodeletUnload) + + try: + self._list_srv.wait_for_service(timeout=0.1) + except rospy.exceptions.ROSException: + self.start_manager() + + def __del__(self): + self.stop_manager() + + def start_manager(self): + if self.manager is None: + name = self.name + if name.startswith('/'): + name = name[1:] + self.manager = sp.Popen( + ["rosrun", "nodelet", "nodelet", "manager", + "__name:={}".format(name)],) + self.manager.daemon = True + rospy.loginfo('started manager {}'.format(self.name)) + + try: + self._list_srv.wait_for_service(timeout=5.0) + except rospy.exceptions.ROSException: + raise rospy.exceptions.ROSException( + "Failed to contact a manager {}." + "Is manager started?".format(self.name)) + + def stop_manager(self): + self.unload_all() + if self.manager is not None and\ + self.manager.poll() is None: + self.manager.kill() + + @property + def nodelets(self): + now = rospy.Time.now() + if rospy.Time.now() - self.last_list_nodelet_stamp > self.list_nodelet_timeout: + self._nodelets = self._list_srv().nodelets + return self._nodelets + + def load(self, name, type, remappings=None): + if name in self.nodelets: + rospy.logwarn('nodelet {} is already loaded'.format(name)) + return False + + source, target = {}, {} + if remappings: + source = remappings.keys() + target = [remappings[k] for k in source] + res = self._load_srv( + name=name, + type=type, + remap_source_args=source, + remap_target_args=target, + my_argv=list(), + bond_id=str() + ) + + return res.success + + def unload(self, name): + if name not in self.nodelets: + rospy.logwarn('nodelet {} is not loaded.'.format(name)) + return False + res = self._unload_srv(name=name) + return res.success + + def unload_all(self): + unloaded = False + for name in self.nodelets: + unloaded = self.unload(name) and unloaded + return unloaded + + +class LoggerManager(NodeletManager): + def __init__(self, name): + super(LoggerManager, self).__init__(name=name) + self.topics = set() + + def escape_topic(self, topic): + name = topic + if name.startswith('/'): + name = name[1:] + name = name.replace('/', '_') + '_logger' + return name + + def watch(self, topic): + if topic in self.topics: + return False + ok = self.load( + name=self.escape_topic(topic), + type="jsk_robot_lifelog/LightweightLogger", + remappings={'~input': topic}) + if ok: + rospy.loginfo('start watching {} in manager {}'.format( + topic, self.name)) + self.topics.add(topic) + return ok + + def unwatch(self, topic): + if topic not in self.topics: + return False + ok = self.unload(name=self.escape_topic(topic)) + if ok: + rospy.loginfo('stop watching {} in manager {}'.format( + topic, self.name)) + self.topics.remove(topic) + return ok + + +class AutomatedLogger(object): + def __init__(self): + super(AutomatedLogger, self).__init__() + self.managers = {} + rospy.on_shutdown(self.shutdown_cb) + + default_manager_name = rospy.get_name() + "_manager" + self.managers['default'] = LoggerManager(default_manager_name) + + monitor_node = rospy.get_param("~monitor_node") + if not monitor_node.startswith('/'): + raise ValueError('~monitor_node param must start with /') + self.graph = Graph(monitor_node) + + update_rate = rospy.get_param('~update_rate', 5.0) + self.update_timer = rospy.Timer( + rospy.Duration(update_rate), self.update_cb) + + def shutdown_cb(self): + for m in self.managers.values(): + m.stop_manager() + + def get_manager(self, topic): + for m in self.managers.values(): + if topic in m.topics: + return m + + def list_managers(self): + suffix = '/load_nodelet' + managers = [e.start for e in self.graph.node_srv_conn if e.end.endswith(suffix)] + return managers + + def update_cb(self, event=None): + self.graph.update(upward=True, downward=False) + topics = copy.deepcopy(self.graph.topics) + node_topics = self.graph.node_topic_conn + current = set() + for m in self.managers.values(): + current = current | m.topics + + canceled = current - topics + for topic in canceled: + manager = self.get_manager(topic) + if not manager: + rospy.logerr('no manager found for topic {}'.format(topic)) + continue + ok = manager.unwatch(topic) + if not ok: + rospy.logerr('failed to unwatch topic {} from manager {}'.format( + topic, manager.name)) + + added = topics - current + managers = set(self.list_managers()) + for topic in added: + pubs = [e.start for e in node_topics.find_edge_by_node( + entopic(topic), upward=True, downward=False)] + pub_set = set(pubs) + pub_manager = managers & pub_set + print(topic, pubs, pub_manager) + if pub_manager: + manager = list(pub_manager)[0] + else: + manager = 'default' + if manager not in self.managers: + self.managers[manager] = LoggerManager(manager) + self.managers[manager].watch(topic) + + +if __name__ == '__main__': + # g = Graph("/hogehoge") + # g.update(upward=True, downward=False) + # print(g.nodes) + # suffix = '/load_nodelet' + # managers = [e.start for e in g.node_srv_conn if e.end.endswith(suffix)] + # print(managers) + # # print(g.srvs) + # g.update_conns() + # g.to_graph("graph") + rospy.init_node("automated_logger") + n = AutomatedLogger() + rospy.spin() From 2b8048b605782d49bcab5cf8ae80693feb975593 Mon Sep 17 00:00:00 2001 From: Furushchev Date: Fri, 14 Dec 2018 17:32:24 +0900 Subject: [PATCH 31/70] update automated_logger --- .../lifelog/automated_logger.py | 212 ++++++++++++------ .../lifelog/test/lazy_publisher.py | 37 +++ .../lifelog/test/lazy_subscriber.py | 31 +++ .../lifelog/test/simulate_lazy.launch | 55 +++++ 4 files changed, 266 insertions(+), 69 deletions(-) create mode 100755 jsk_robot_common/jsk_robot_startup/lifelog/test/lazy_publisher.py create mode 100755 jsk_robot_common/jsk_robot_startup/lifelog/test/lazy_subscriber.py create mode 100644 jsk_robot_common/jsk_robot_startup/lifelog/test/simulate_lazy.launch diff --git a/jsk_robot_common/jsk_robot_startup/lifelog/automated_logger.py b/jsk_robot_common/jsk_robot_startup/lifelog/automated_logger.py index 34d6f8c9a6..1dfe29fa39 100755 --- a/jsk_robot_common/jsk_robot_startup/lifelog/automated_logger.py +++ b/jsk_robot_common/jsk_robot_startup/lifelog/automated_logger.py @@ -5,6 +5,7 @@ from __future__ import print_function from collections import defaultdict import copy +import click import itertools import subprocess as sp import re @@ -13,6 +14,7 @@ import rospy import socket import sys +import time import traceback try: from xmlrpc.client import ServerProxy @@ -26,13 +28,6 @@ TOPICPREFIX = 'topic:' -def error(*msg): - if rospy.get_name() == '/unnamed': - print(*msg, file=sys.stderr) - else: - rospy.logerr(*msg) - - def match(regex, s): for r in regex: if re.match(r, s): @@ -50,13 +45,6 @@ def detopic(name): return name -def escape(name): - # if name.startswith('/'): - # name = name[1:] - # return name.replace('/', '_') - return name + "_logger" - - class Edge(object): def __init__(self, start, end, label=None): super(Edge, self).__init__() @@ -214,7 +202,7 @@ def update_nodes(self, upward=True, downward=True): try: res = self.master.getSystemState() except rosgraph.masterapi.MasterException as e: - error('Unable to contact master', str(e), traceback.format_exc()) + rospy.logerr('Unable to contact master', str(e), traceback.format_exc()) return False node_topics = defaultdict(lambda: defaultdict(set)) @@ -271,6 +259,7 @@ def update_nodes(self, upward=True, downward=True): added = True if added: topic_set.add(t) + node_set.add(self.base_node) node_srv_conn = EdgeList() @@ -294,7 +283,7 @@ def get_node_uri(self, node): self.uri_node_cache[uri] = node return uri except Exception as e: - error('Failed to lookup node', str(e), traceback.format_exc()) + rospy.logerr('Failed to lookup node', str(e), traceback.format_exc()) if node in self.node_uri_cache: return self.node_uri_cache[node] else: @@ -325,7 +314,7 @@ def update_conns(self): socket.setdefaulttimeout(timeout) if code != 1: - error('Could not get bus info of node {}: {}'.format( + rospy.logerr('Could not get bus info of node {}: {}'.format( node, msg)) continue @@ -340,19 +329,19 @@ def update_conns(self): if connected: - self.topics.add(topic) - updated = self.node_topic_conn.add_edges( - node, entopic(topic), direction) or updated if dest_id.startswith('http://'): dest_id = self.uri_node_cache.get(dest_id, None) if dest_id in self.nodes and\ not match(self.blacklist_nodes, dest_id) and\ not match(self.blacklist_topics, topic): + self.topics.add(topic) + updated = self.node_topic_conn.add_edges( + node, entopic(topic), direction) or updated updated = self.node_node_conn.add_edges( node, dest_id, direction, topic) or updated for n, err in self.bad_nodes: - error('bad node {}: {}'.format(n, err)) + rospy.logerr('bad node {}: {}'.format(n, err)) return updated @@ -388,14 +377,20 @@ def to_graph(self, out, view=False): class NodeletManager(object): - def __init__(self, name): + _managers = list() + + def __init__(self, name, color): super(NodeletManager, self).__init__() self.name = name self.manager = None + if color is None: + color = 'white' + self.color = color self._list_nodelet_timeout = rospy.Duration(5.0) self._last_list_nodelet_stamp = rospy.Time(0) - self._nodelets = [] + self._nodelets = set() + self._loaded_nodelets = set() self._list_srv = rospy.ServiceProxy( self.name + "/list", NodeletList) @@ -406,11 +401,34 @@ def __init__(self, name): try: self._list_srv.wait_for_service(timeout=0.1) + self.loginfo("Found existing manager") except rospy.exceptions.ROSException: self.start_manager() - def __del__(self): - self.stop_manager() + @classmethod + def get_manager(cls, name): + colors = ['green', 'yellow', 'blue', 'magenta', 'cyan', 'white', + 'bright_black', 'bright_green', 'bright_yellow', + 'bright_blue', 'bright_magenta', 'bright_cyan', 'bright_white'] + for m in cls._managers: + if m.name == name: + return m + index = len(cls._managers) % len(colors) + m = cls(name, colors[index]) + cls._managers.append(m) + return m + + def logfmt(self, func, msg): + func(click.style('[{}] {}'.format(self.name, msg), fg=self.color)) + + def loginfo(self, msg): + self.logfmt(rospy.loginfo, msg) + + def logwarn(self, msg): + self.logfmt(rospy.logwarn, msg) + + def logerr(self, msg): + self.logfmt(rospy.logerr, msg) def start_manager(self): if self.manager is None: @@ -421,65 +439,121 @@ def start_manager(self): ["rosrun", "nodelet", "nodelet", "manager", "__name:={}".format(name)],) self.manager.daemon = True - rospy.loginfo('started manager {}'.format(self.name)) + self.loginfo('started manager') try: self._list_srv.wait_for_service(timeout=5.0) + return True except rospy.exceptions.ROSException: - raise rospy.exceptions.ROSException( - "Failed to contact a manager {}." - "Is manager started?".format(self.name)) + self.logerr('Failed to start manager') + self.logerr(traceback.format_exc()) + return False def stop_manager(self): self.unload_all() if self.manager is not None and\ - self.manager.poll() is None: - self.manager.kill() + self.manager.poll() is not None: + start = time.time() + timeout = 10.0 + self.manager.terminate() + while self.manager.poll() is None: + time.sleep(0.1) + elapsed = time.time() - start + if elapsed > timeout: + self.manager.kill() + self.manager.wait() + break + retcode = self.manager.poll() + if retcode == 0: + self.loginfo('stoped manager') + else: + self.logerr('Failed to stop manager (code: {})'.format(retcode)) + return False + return True @property def nodelets(self): now = rospy.Time.now() - if rospy.Time.now() - self.last_list_nodelet_stamp > self.list_nodelet_timeout: - self._nodelets = self._list_srv().nodelets + if rospy.Time.now() - self._last_list_nodelet_stamp > self._list_nodelet_timeout: + try: + self._list_srv.wait_for_service(timeout=1) + self._nodelets = set(self._list_srv().nodelets) + except Exception as e: + self.logerr("Failed to list nodelet") + self._nodelets = set() return self._nodelets + @property + def loaded_nodelets(self): + nodelets = self.nodelets + if nodelets: + self._loaded_nodelets = nodelets & self._loaded_nodelets + return self._loaded_nodelets + def load(self, name, type, remappings=None): - if name in self.nodelets: - rospy.logwarn('nodelet {} is already loaded'.format(name)) - return False + if name in self.loaded_nodelets: + self.logwarn('{} already loaded'.format(name)) + return True source, target = {}, {} if remappings: source = remappings.keys() target = [remappings[k] for k in source] - res = self._load_srv( - name=name, - type=type, - remap_source_args=source, - remap_target_args=target, - my_argv=list(), - bond_id=str() - ) + try: + self._load_srv.wait_for_service(timeout=1) + res = self._load_srv( + name=name, + type=type, + remap_source_args=source, + remap_target_args=target, + my_argv=list(), + bond_id=str() + ) + if res.success: + self.loginfo("Loaded {}".format(name)) + self._loaded_nodelets.add(name) + else: + self.logerr("Failed to load {}".format(name)) + except rospy.exceptions.ROSException: + self.logerr("Failed to load {}".format(name)) + self.logerr(traceback.format_exc()) + return False return res.success def unload(self, name): - if name not in self.nodelets: - rospy.logwarn('nodelet {} is not loaded.'.format(name)) + if name not in self.loaded_nodelets: + self.logwarn('nodelet {} is not loaded.'.format(name)) + return True + try: + self._unload_srv.wait_for_service(timeout=1) + res = self._unload_srv(name=name) + if res.success: + self.loginfo("Unloaded {}".format(name)) + self._loaded_nodelets.remove(name) + else: + self.logerr("Failed to unload {}".format(name)) + except rospy.exceptions.ROSException: + self.logerr("Failed to unload {}".format(name)) + self.logerr(traceback.format_exc()) return False - res = self._unload_srv(name=name) return res.success def unload_all(self): unloaded = False - for name in self.nodelets: - unloaded = self.unload(name) and unloaded + try: + for name in self.loaded_nodelets: + unloaded = self.unload(name) and unloaded + except Exception: + self.logerr("Failed to unload all nodelets") + self.logerr(traceback.format_exc()) + unloaded = False return unloaded class LoggerManager(NodeletManager): - def __init__(self, name): - super(LoggerManager, self).__init__(name=name) + def __init__(self, name, color): + super(LoggerManager, self).__init__(name=name, color=color) self.topics = set() def escape_topic(self, topic): @@ -497,9 +571,10 @@ def watch(self, topic): type="jsk_robot_lifelog/LightweightLogger", remappings={'~input': topic}) if ok: - rospy.loginfo('start watching {} in manager {}'.format( - topic, self.name)) + self.loginfo('Started watching {}'.format(topic)) self.topics.add(topic) + else: + self.logerr("Failed to watch {}".format(topic)) return ok def unwatch(self, topic): @@ -507,9 +582,10 @@ def unwatch(self, topic): return False ok = self.unload(name=self.escape_topic(topic)) if ok: - rospy.loginfo('stop watching {} in manager {}'.format( - topic, self.name)) + self.loginfo('Stopped watching {}'.format(topic)) self.topics.remove(topic) + else: + self.logerr("Failed to unwatch {}".format(topic)) return ok @@ -520,14 +596,14 @@ def __init__(self): rospy.on_shutdown(self.shutdown_cb) default_manager_name = rospy.get_name() + "_manager" - self.managers['default'] = LoggerManager(default_manager_name) + self.managers['default'] = LoggerManager.get_manager(default_manager_name) monitor_node = rospy.get_param("~monitor_node") if not monitor_node.startswith('/'): raise ValueError('~monitor_node param must start with /') self.graph = Graph(monitor_node) - update_rate = rospy.get_param('~update_rate', 5.0) + update_rate = rospy.get_param('~update_rate', 1.0) self.update_timer = rospy.Timer( rospy.Duration(update_rate), self.update_cb) @@ -554,43 +630,41 @@ def update_cb(self, event=None): current = current | m.topics canceled = current - topics + if canceled: + rospy.loginfo("Removing topics: {}".format(canceled)) for topic in canceled: manager = self.get_manager(topic) if not manager: - rospy.logerr('no manager found for topic {}'.format(topic)) + rospy.logerr('No manager found for topic {}'.format(topic)) continue + rospy.logdebug('before cancel: {}'.format(manager.loaded_nodelets)) ok = manager.unwatch(topic) + rospy.logdebug('after cancel: {}'.format(manager.loaded_nodelets)) if not ok: rospy.logerr('failed to unwatch topic {} from manager {}'.format( topic, manager.name)) added = topics - current managers = set(self.list_managers()) + if added: + rospy.loginfo("Adding topics: {}".format(added)) for topic in added: pubs = [e.start for e in node_topics.find_edge_by_node( entopic(topic), upward=True, downward=False)] pub_set = set(pubs) pub_manager = managers & pub_set - print(topic, pubs, pub_manager) if pub_manager: manager = list(pub_manager)[0] else: manager = 'default' if manager not in self.managers: - self.managers[manager] = LoggerManager(manager) + self.managers[manager] = LoggerManager.get_manager(manager) + rospy.logdebug('before add: {}'.format(self.managers[manager].loaded_nodelets)) self.managers[manager].watch(topic) + rospy.logdebug('after add: {}'.format(self.managers[manager].loaded_nodelets)) if __name__ == '__main__': - # g = Graph("/hogehoge") - # g.update(upward=True, downward=False) - # print(g.nodes) - # suffix = '/load_nodelet' - # managers = [e.start for e in g.node_srv_conn if e.end.endswith(suffix)] - # print(managers) - # # print(g.srvs) - # g.update_conns() - # g.to_graph("graph") rospy.init_node("automated_logger") n = AutomatedLogger() rospy.spin() diff --git a/jsk_robot_common/jsk_robot_startup/lifelog/test/lazy_publisher.py b/jsk_robot_common/jsk_robot_startup/lifelog/test/lazy_publisher.py new file mode 100755 index 0000000000..97a53c3904 --- /dev/null +++ b/jsk_robot_common/jsk_robot_startup/lifelog/test/lazy_publisher.py @@ -0,0 +1,37 @@ +#!/usr/bin/env python +# -*- coding: utf-8 -*- +# Author: Yuki Furuta + +import numpy as np +import rospy +from jsk_topic_tools import ConnectionBasedTransport +from geometry_msgs.msg import PoseStamped + + +class LazyPublisher(ConnectionBasedTransport): + def __init__(self): + super(LazyPublisher, self).__init__() + self.pub = self.advertise( + "~output", PoseStamped, queue_size=1) + self.timer = None + self.i = 0 + + def subscribe(self): + self.timer = rospy.Timer(rospy.Duration(0.5), self.timer_cb) + + def unsubscribe(self): + self.timer.shutdown() + self.timer = None + + def timer_cb(self, event): + msg = PoseStamped() + msg.header.stamp = rospy.Time.now() + msg.pose.position.x = np.sin(np.radians(self.i * 30)) + self.pub.publish(msg) + self.i += 1 + + +if __name__ == '__main__': + rospy.init_node("lazy_publisher") + p = LazyPublisher() + rospy.spin() diff --git a/jsk_robot_common/jsk_robot_startup/lifelog/test/lazy_subscriber.py b/jsk_robot_common/jsk_robot_startup/lifelog/test/lazy_subscriber.py new file mode 100755 index 0000000000..8220aeba97 --- /dev/null +++ b/jsk_robot_common/jsk_robot_startup/lifelog/test/lazy_subscriber.py @@ -0,0 +1,31 @@ +#!/usr/bin/env python +# -*- coding: utf-8 -*- +# Author: Yuki Furuta + +import rospy +from geometry_msgs.msg import PoseStamped + + +def callback(msg): + rospy.loginfo(msg.pose.position.x) + + +def main(): + rospy.init_node("lazy_subscriber") + sub = None + i = 0 + r = rospy.Rate(1.0 / 5) + while not rospy.is_shutdown(): + r.sleep() + if sub is None: + sub = rospy.Subscriber( + "~input", PoseStamped, callback) + rospy.loginfo("started subscribe") + else: + sub.unregister() + sub = None + rospy.loginfo("stopped subscribe") + + +if __name__ == '__main__': + main() diff --git a/jsk_robot_common/jsk_robot_startup/lifelog/test/simulate_lazy.launch b/jsk_robot_common/jsk_robot_startup/lifelog/test/simulate_lazy.launch new file mode 100644 index 0000000000..8cdac3618d --- /dev/null +++ b/jsk_robot_common/jsk_robot_startup/lifelog/test/simulate_lazy.launch @@ -0,0 +1,55 @@ + + + robot: + type: sample_robot + name: test + + + + + + + + + + + + + lazy: true + + + + + + lazy: true + + + + + + + + + + + + + + + + + From e4b917885160355d982b7d718bd44253e8159542 Mon Sep 17 00:00:00 2001 From: Furushchev Date: Fri, 14 Dec 2018 19:17:07 +0900 Subject: [PATCH 32/70] fix remappings --- .../jsk_robot_startup/lifelog/automated_logger.py | 7 ++++++- 1 file changed, 6 insertions(+), 1 deletion(-) diff --git a/jsk_robot_common/jsk_robot_startup/lifelog/automated_logger.py b/jsk_robot_common/jsk_robot_startup/lifelog/automated_logger.py index 1dfe29fa39..e8809cc101 100755 --- a/jsk_robot_common/jsk_robot_startup/lifelog/automated_logger.py +++ b/jsk_robot_common/jsk_robot_startup/lifelog/automated_logger.py @@ -495,10 +495,15 @@ def load(self, name, type, remappings=None): self.logwarn('{} already loaded'.format(name)) return True - source, target = {}, {} + source, target = [], [] if remappings: source = remappings.keys() target = [remappings[k] for k in source] + # resolve private namespace + source = [t.replace('~', name + '/') for t in source] + target = [t.replace('~', name + '/') for t in target] + for s, t in zip(source, target): + self.loginfo('Remap {} -> {}'.format(s, t)) try: self._load_srv.wait_for_service(timeout=1) res = self._load_srv( From e4c7f5d0b15976bd7f612b8d0a468a6435db20f7 Mon Sep 17 00:00:00 2001 From: Furushchev Date: Sat, 15 Dec 2018 15:20:27 +0900 Subject: [PATCH 33/70] fix merge conflicts --- .../jsk_robot_startup/src/lightweight_logger_nodelet.cpp | 6 +----- 1 file changed, 1 insertion(+), 5 deletions(-) diff --git a/jsk_robot_common/jsk_robot_startup/src/lightweight_logger_nodelet.cpp b/jsk_robot_common/jsk_robot_startup/src/lightweight_logger_nodelet.cpp index 6d5340ae9e..69d0fb54a6 100644 --- a/jsk_robot_common/jsk_robot_startup/src/lightweight_logger_nodelet.cpp +++ b/jsk_robot_common/jsk_robot_startup/src/lightweight_logger_nodelet.cpp @@ -155,13 +155,9 @@ namespace jsk_robot_startup stat.add("Insert Failure", insert_error_count_); vital_checker_->registerStatInfo(stat, "Last Insert"); } -<<<<<<< HEAD - } -} -======= } // lifelog } // jsk_robot_startup ->>>>>>> automate-logger + #include typedef jsk_robot_startup::lifelog::LightweightLogger LightweightLogger; From ead6bd8fa353840598aed309919a7f0a4a17833a Mon Sep 17 00:00:00 2001 From: Furushchev Date: Sat, 15 Dec 2018 23:56:18 +0900 Subject: [PATCH 34/70] lightweight_logger: fix double free bug --- .../jsk_robot_startup/lightweight_logger.h | 6 ++-- .../src/lightweight_logger_nodelet.cpp | 31 +++++++++++++------ 2 files changed, 25 insertions(+), 12 deletions(-) diff --git a/jsk_robot_common/jsk_robot_startup/include/jsk_robot_startup/lightweight_logger.h b/jsk_robot_common/jsk_robot_startup/include/jsk_robot_startup/lightweight_logger.h index 7f49f8233f..89a90475fa 100644 --- a/jsk_robot_common/jsk_robot_startup/include/jsk_robot_startup/lightweight_logger.h +++ b/jsk_robot_common/jsk_robot_startup/include/jsk_robot_startup/lightweight_logger.h @@ -48,7 +48,7 @@ #include #include #include -#include +#include namespace jsk_robot_startup { @@ -60,10 +60,10 @@ namespace jsk_robot_startup virtual void onInit(); virtual ~LightweightLogger(); virtual void loadThread(); - virtual void inputCallback(const ros::MessageEvent& event); + virtual void inputCallback(const ros::MessageEvent& event); virtual void updateDiagnostic(diagnostic_updater::DiagnosticStatusWrapper &stat); - mongodb_store::MessageStoreProxy* msg_store_; + boost::shared_ptr msg_store_; boost::thread deferred_load_thread_; bool wait_for_insert_; bool initialized_; diff --git a/jsk_robot_common/jsk_robot_startup/src/lightweight_logger_nodelet.cpp b/jsk_robot_common/jsk_robot_startup/src/lightweight_logger_nodelet.cpp index 02cb324398..1facef6e7f 100644 --- a/jsk_robot_common/jsk_robot_startup/src/lightweight_logger_nodelet.cpp +++ b/jsk_robot_common/jsk_robot_startup/src/lightweight_logger_nodelet.cpp @@ -84,29 +84,42 @@ namespace jsk_robot_startup } LightweightLogger::~LightweightLogger() { - if (!initialized_) { + if (msg_store_) { + msg_store_.reset(); + } + if (deferred_load_thread_.joinable()) { NODELET_DEBUG_STREAM("Shutting down deferred load thread"); - deferred_load_thread_.join(); - NODELET_DEBUG_STREAM("deferred load thread stopped"); + bool ok = deferred_load_thread_.try_join_for(boost::chrono::seconds(5)); + if (!ok) { + NODELET_WARN_STREAM("Timed out to join deferred load thread."); + deferred_load_thread_.interrupt(); + ok = deferred_load_thread_.try_join_for(boost::chrono::seconds(3)); + if (!ok) { + NODELET_ERROR_STREAM("Failed to shutdown deferred load thread."); + } + } + if (ok) { + NODELET_DEBUG_STREAM("deferred load thread stopped"); + } } } void LightweightLogger::loadThread() { - NODELET_INFO_STREAM("Connecting to database " << db_name_ << "/" << col_name_ << "..."); - msg_store_ = MessageStoreSingleton::getInstance(col_name_, db_name_); - NODELET_INFO_STREAM("Successfully connected to database!"); - + msg_store_.reset(new mongodb_store::MessageStoreProxy(*nh_, col_name_, db_name_)); initialized_ = true; } - void LightweightLogger::inputCallback(const ros::MessageEvent& event) + void LightweightLogger::inputCallback(const ros::MessageEvent& event) { const std::string& publisher_name = event.getPublisherName(); const boost::shared_ptr& msg = event.getConstMessage(); jsk_topic_tools::StealthRelay::inputCallback(msg); - if (!initialized_) return; + if (!initialized_) { + NODELET_WARN_THROTTLE(1.0, "nodelet is not yet initialized"); + return; + } vital_checker_->poke(); try From cb9bab0ea88466b3846915bcd0fe98581179b7f4 Mon Sep 17 00:00:00 2001 From: Furushchev Date: Sat, 15 Dec 2018 23:57:02 +0900 Subject: [PATCH 35/70] lightweight_logger: add param ~enable_diagnostics --- .../src/lightweight_logger_nodelet.cpp | 19 +++++++++++-------- 1 file changed, 11 insertions(+), 8 deletions(-) diff --git a/jsk_robot_common/jsk_robot_startup/src/lightweight_logger_nodelet.cpp b/jsk_robot_common/jsk_robot_startup/src/lightweight_logger_nodelet.cpp index 1facef6e7f..0ce4d12658 100644 --- a/jsk_robot_common/jsk_robot_startup/src/lightweight_logger_nodelet.cpp +++ b/jsk_robot_common/jsk_robot_startup/src/lightweight_logger_nodelet.cpp @@ -60,19 +60,22 @@ namespace jsk_robot_startup input_topic_name_ = pnh_->resolveName("input", true); - diagnostic_updater_.reset( - new jsk_topic_tools::TimeredDiagnosticUpdater(*pnh_, ros::Duration(1.0))); - diagnostic_updater_->setHardwareID("LightweightLogger"); - diagnostic_updater_->add( - "LightweightLogger::" + input_topic_name_, - boost::bind(&LightweightLogger::updateDiagnostic, this, _1)); - double vital_rate; pnh_->param("vital_rate", vital_rate, 1.0); vital_checker_.reset( new jsk_topic_tools::VitalChecker(1.0 / vital_rate)); - diagnostic_updater_->start(); + bool enable_diagnostics; + pnh_->param("enable_diagnostics", enable_diagnostics, true); + if (enable_diagnostics) { + diagnostic_updater_.reset( + new jsk_topic_tools::TimeredDiagnosticUpdater(*pnh_, ros::Duration(1.0))); + diagnostic_updater_->setHardwareID("LightweightLogger"); + diagnostic_updater_->add( + "LightweightLogger::" + input_topic_name_, + boost::bind(&LightweightLogger::updateDiagnostic, this, _1)); + diagnostic_updater_->start(); + } inserted_count_ = 0; insert_error_count_ = 0; From 5e2403cf5aa9b923702805504fb25b870337c911 Mon Sep 17 00:00:00 2001 From: Furushchev Date: Sat, 15 Dec 2018 23:57:24 +0900 Subject: [PATCH 36/70] automated_logger: minor bugfix --- jsk_robot_common/jsk_robot_startup/lifelog/automated_logger.py | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/jsk_robot_common/jsk_robot_startup/lifelog/automated_logger.py b/jsk_robot_common/jsk_robot_startup/lifelog/automated_logger.py index e8809cc101..c495529acc 100755 --- a/jsk_robot_common/jsk_robot_startup/lifelog/automated_logger.py +++ b/jsk_robot_common/jsk_robot_startup/lifelog/automated_logger.py @@ -340,7 +340,7 @@ def update_conns(self): updated = self.node_node_conn.add_edges( node, dest_id, direction, topic) or updated - for n, err in self.bad_nodes: + for n, err in self.bad_nodes.items(): rospy.logerr('bad node {}: {}'.format(n, err)) return updated From aceecbe544a7025deb643e1042c0809a37e84717 Mon Sep 17 00:00:00 2001 From: Furushchev Date: Sat, 15 Dec 2018 23:58:37 +0900 Subject: [PATCH 37/70] automated_logger: variable for nodelet api sanity check --- .../jsk_robot_startup/lifelog/automated_logger.py | 9 +++++---- 1 file changed, 5 insertions(+), 4 deletions(-) diff --git a/jsk_robot_common/jsk_robot_startup/lifelog/automated_logger.py b/jsk_robot_common/jsk_robot_startup/lifelog/automated_logger.py index c495529acc..10f579632e 100755 --- a/jsk_robot_common/jsk_robot_startup/lifelog/automated_logger.py +++ b/jsk_robot_common/jsk_robot_startup/lifelog/automated_logger.py @@ -391,6 +391,7 @@ def __init__(self, name, color): self._last_list_nodelet_stamp = rospy.Time(0) self._nodelets = set() self._loaded_nodelets = set() + self._srv_wait_timeout = 0.3 self._list_srv = rospy.ServiceProxy( self.name + "/list", NodeletList) @@ -400,7 +401,7 @@ def __init__(self, name, color): self.name + "/unload_nodelet", NodeletUnload) try: - self._list_srv.wait_for_service(timeout=0.1) + self._list_srv.wait_for_service(timeout=self._srv_wait_timeout) self.loginfo("Found existing manager") except rospy.exceptions.ROSException: self.start_manager() @@ -476,7 +477,7 @@ def nodelets(self): now = rospy.Time.now() if rospy.Time.now() - self._last_list_nodelet_stamp > self._list_nodelet_timeout: try: - self._list_srv.wait_for_service(timeout=1) + self._list_srv.wait_for_service(timeout=self._srv_wait_timeout) self._nodelets = set(self._list_srv().nodelets) except Exception as e: self.logerr("Failed to list nodelet") @@ -505,7 +506,7 @@ def load(self, name, type, remappings=None): for s, t in zip(source, target): self.loginfo('Remap {} -> {}'.format(s, t)) try: - self._load_srv.wait_for_service(timeout=1) + self._load_srv.wait_for_service(timeout=self._srv_wait_timeout) res = self._load_srv( name=name, type=type, @@ -531,7 +532,7 @@ def unload(self, name): self.logwarn('nodelet {} is not loaded.'.format(name)) return True try: - self._unload_srv.wait_for_service(timeout=1) + self._unload_srv.wait_for_service(timeout=self._srv_wait_timeout) res = self._unload_srv(name=name) if res.success: self.loginfo("Unloaded {}".format(name)) From d64e5879f933840a19e8bddcb6ca5502583c61b3 Mon Sep 17 00:00:00 2001 From: Furushchev Date: Sat, 15 Dec 2018 23:59:39 +0900 Subject: [PATCH 38/70] automated_logger: pass params/my_argv on load nodelet --- .../jsk_robot_startup/lifelog/automated_logger.py | 11 ++++++++--- 1 file changed, 8 insertions(+), 3 deletions(-) diff --git a/jsk_robot_common/jsk_robot_startup/lifelog/automated_logger.py b/jsk_robot_common/jsk_robot_startup/lifelog/automated_logger.py index 10f579632e..417cc7ed2c 100755 --- a/jsk_robot_common/jsk_robot_startup/lifelog/automated_logger.py +++ b/jsk_robot_common/jsk_robot_startup/lifelog/automated_logger.py @@ -491,7 +491,7 @@ def loaded_nodelets(self): self._loaded_nodelets = nodelets & self._loaded_nodelets return self._loaded_nodelets - def load(self, name, type, remappings=None): + def load(self, name, type, remappings=None, my_argv=None, params=None): if name in self.loaded_nodelets: self.logwarn('{} already loaded'.format(name)) return True @@ -505,6 +505,10 @@ def load(self, name, type, remappings=None): target = [t.replace('~', name + '/') for t in target] for s, t in zip(source, target): self.loginfo('Remap {} -> {}'.format(s, t)) + if params: + for n, v in params.items(): + self.loginfo('setparam {} -> {}'.format(n, v)) + rospy.set_param(name, params) try: self._load_srv.wait_for_service(timeout=self._srv_wait_timeout) res = self._load_srv( @@ -512,7 +516,7 @@ def load(self, name, type, remappings=None): type=type, remap_source_args=source, remap_target_args=target, - my_argv=list(), + my_argv=my_argv or list(), bond_id=str() ) if res.success: @@ -575,7 +579,8 @@ def watch(self, topic): ok = self.load( name=self.escape_topic(topic), type="jsk_robot_lifelog/LightweightLogger", - remappings={'~input': topic}) + remappings={'~input': topic}, + params={'enable_monitor': False}) if ok: self.loginfo('Started watching {}'.format(topic)) self.topics.add(topic) From 2cdd45bfa95bd4eeea7bed982b65c274586ea7b7 Mon Sep 17 00:00:00 2001 From: Furushchev Date: Sun, 16 Dec 2018 00:00:51 +0900 Subject: [PATCH 39/70] automated_logger: record watch log --- .../lifelog/automated_logger.py | 81 +++++++++++++++---- 1 file changed, 67 insertions(+), 14 deletions(-) diff --git a/jsk_robot_common/jsk_robot_startup/lifelog/automated_logger.py b/jsk_robot_common/jsk_robot_startup/lifelog/automated_logger.py index 417cc7ed2c..d1eb4ee727 100755 --- a/jsk_robot_common/jsk_robot_startup/lifelog/automated_logger.py +++ b/jsk_robot_common/jsk_robot_startup/lifelog/automated_logger.py @@ -7,15 +7,17 @@ import copy import click import itertools -import subprocess as sp +import pymongo import re import rosgraph.masterapi -import rosservice import rospy +import rosservice import socket +import subprocess as sp import sys import time import traceback +import uuid try: from xmlrpc.client import ServerProxy except ImportError: @@ -614,13 +616,51 @@ def __init__(self): raise ValueError('~monitor_node param must start with /') self.graph = Graph(monitor_node) + host = rospy.get_param('/mongodb_host', 'localhost') + port = rospy.get_param('/mongodb_port', 27017) + database = rospy.get_param('/robot/database') + collection = rospy.get_param('~collection', 'automated_logger') + client = pymongo.MongoClient(host, port) + try: + ok = (client[database].command('ping')['ok'] == 1.0) + if not ok: + raise RuntimeError() + except: + rospy.logfatal('Could not connect to database {}:{}/{}'.format(host, port, database)) + rospy.logfatal(traceback.format_exc()) + return + self.log_db = client[database][collection] + self.log_id = str(uuid.uuid1()) + + self.record_log(rospy.Time.now(), 'start') + update_rate = rospy.get_param('~update_rate', 1.0) self.update_timer = rospy.Timer( rospy.Duration(update_rate), self.update_cb) + def record_log(self, stamp, event, **data): + data.update({ + 'stamp': { + 'secs': stamp.secs, + 'nsecs': stamp.nsecs, + 'seconds': float(stamp.secs) + float(stamp.nsecs) * 1e-9 + }, + 'event': event, + 'monitor_node': self.graph.base_node, + 'logger_id': self.log_id, + }) + try: + self.log_db.insert(data) + return True + except Exception as e: + rospy.logerr('Failed to insert record log: {}'.format(str(e))) + rospy.logerr(traceback.format_exc()) + return False + def shutdown_cb(self): for m in self.managers.values(): m.stop_manager() + self.record_log(rospy.Time.now(), 'shutdown') def get_manager(self, topic): for m in self.managers.values(): @@ -633,6 +673,7 @@ def list_managers(self): return managers def update_cb(self, event=None): + stamp = event.current_real self.graph.update(upward=True, downward=False) topics = copy.deepcopy(self.graph.topics) node_topics = self.graph.node_topic_conn @@ -640,10 +681,11 @@ def update_cb(self, event=None): for m in self.managers.values(): current = current | m.topics - canceled = current - topics - if canceled: - rospy.loginfo("Removing topics: {}".format(canceled)) - for topic in canceled: + cancels = current - topics + canceled = [] + if cancels: + rospy.loginfo("Removing topics: {}".format(cancels)) + for topic in cancels: manager = self.get_manager(topic) if not manager: rospy.logerr('No manager found for topic {}'.format(topic)) @@ -651,15 +693,19 @@ def update_cb(self, event=None): rospy.logdebug('before cancel: {}'.format(manager.loaded_nodelets)) ok = manager.unwatch(topic) rospy.logdebug('after cancel: {}'.format(manager.loaded_nodelets)) - if not ok: + if ok: + canceled.append(topic) + else: rospy.logerr('failed to unwatch topic {} from manager {}'.format( topic, manager.name)) + self.record_log(stamp, 'unwatch', topics=canceled) - added = topics - current + adds = topics - current + added = [] managers = set(self.list_managers()) - if added: - rospy.loginfo("Adding topics: {}".format(added)) - for topic in added: + if adds: + rospy.loginfo("Adding topics: {}".format(adds)) + for topic in adds: pubs = [e.start for e in node_topics.find_edge_by_node( entopic(topic), upward=True, downward=False)] pub_set = set(pubs) @@ -670,9 +716,16 @@ def update_cb(self, event=None): manager = 'default' if manager not in self.managers: self.managers[manager] = LoggerManager.get_manager(manager) - rospy.logdebug('before add: {}'.format(self.managers[manager].loaded_nodelets)) - self.managers[manager].watch(topic) - rospy.logdebug('after add: {}'.format(self.managers[manager].loaded_nodelets)) + rospy.loginfo('before add: {}'.format(self.managers[manager].loaded_nodelets)) + ok = self.managers[manager].watch(topic) + rospy.loginfo('after add: {}'.format(self.managers[manager].loaded_nodelets)) + if ok: + added.append(topic) + else: + rospy.logerr('failed to watch topic {} from manager {}'.format( + topic, self.managers[manager].name)) + + self.record_log(stamp, 'watch', topics=added) if __name__ == '__main__': From a7746037169922b98d8bfa8a08926b423b1494c8 Mon Sep 17 00:00:00 2001 From: Furushchev Date: Sun, 16 Dec 2018 00:06:27 +0900 Subject: [PATCH 40/70] automated_logger: log watch/unwatch only topic exists --- .../jsk_robot_startup/lifelog/automated_logger.py | 7 ++++--- 1 file changed, 4 insertions(+), 3 deletions(-) diff --git a/jsk_robot_common/jsk_robot_startup/lifelog/automated_logger.py b/jsk_robot_common/jsk_robot_startup/lifelog/automated_logger.py index d1eb4ee727..ca614ff482 100755 --- a/jsk_robot_common/jsk_robot_startup/lifelog/automated_logger.py +++ b/jsk_robot_common/jsk_robot_startup/lifelog/automated_logger.py @@ -698,7 +698,8 @@ def update_cb(self, event=None): else: rospy.logerr('failed to unwatch topic {} from manager {}'.format( topic, manager.name)) - self.record_log(stamp, 'unwatch', topics=canceled) + if canceled: + self.record_log(stamp, 'unwatch', topics=canceled) adds = topics - current added = [] @@ -724,8 +725,8 @@ def update_cb(self, event=None): else: rospy.logerr('failed to watch topic {} from manager {}'.format( topic, self.managers[manager].name)) - - self.record_log(stamp, 'watch', topics=added) + if added: + self.record_log(stamp, 'watch', topics=added) if __name__ == '__main__': From 65bd73d7f8ffd4e88bd0b6c464ead61473cc2f04 Mon Sep 17 00:00:00 2001 From: Furushchev Date: Sun, 16 Dec 2018 15:54:09 +0900 Subject: [PATCH 41/70] add circular buffer for lazy insertion --- .../jsk_robot_startup/lightweight_logger.h | 61 +++++++++++++++++++ 1 file changed, 61 insertions(+) diff --git a/jsk_robot_common/jsk_robot_startup/include/jsk_robot_startup/lightweight_logger.h b/jsk_robot_common/jsk_robot_startup/include/jsk_robot_startup/lightweight_logger.h index 89a90475fa..f341cef31d 100644 --- a/jsk_robot_common/jsk_robot_startup/include/jsk_robot_startup/lightweight_logger.h +++ b/jsk_robot_common/jsk_robot_startup/include/jsk_robot_startup/lightweight_logger.h @@ -41,6 +41,8 @@ #ifndef LIGHTWEIGHT_LOGGER_H__ #define LIGHTWEIGHT_LOGGER_H__ +#include +#include #include #include #include @@ -54,6 +56,64 @@ namespace jsk_robot_startup { namespace lifelog { + + template + 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 buffer_; + boost::condition_variable empty_wait_; + boost::mutex mutex_; + }; + class LightweightLogger : public jsk_topic_tools::StealthRelay { protected: @@ -69,6 +129,7 @@ namespace jsk_robot_startup bool initialized_; std::string input_topic_name_; std::string db_name_, col_name_; + blocking_circular_buffer > buffer_; // diagnostics ros::Time init_time_; From 0693058b489d54f17e563c21701c4abf002c2dda Mon Sep 17 00:00:00 2001 From: Furushchev Date: Sun, 16 Dec 2018 15:55:00 +0900 Subject: [PATCH 42/70] use deferred load thead for lazy insertion --- .../jsk_robot_startup/lightweight_logger.h | 4 +- .../src/lightweight_logger_nodelet.cpp | 93 +++++++++++++++---- 2 files changed, 77 insertions(+), 20 deletions(-) diff --git a/jsk_robot_common/jsk_robot_startup/include/jsk_robot_startup/lightweight_logger.h b/jsk_robot_common/jsk_robot_startup/include/jsk_robot_startup/lightweight_logger.h index f341cef31d..52167b4e88 100644 --- a/jsk_robot_common/jsk_robot_startup/include/jsk_robot_startup/lightweight_logger.h +++ b/jsk_robot_common/jsk_robot_startup/include/jsk_robot_startup/lightweight_logger.h @@ -119,12 +119,12 @@ namespace jsk_robot_startup protected: virtual void onInit(); virtual ~LightweightLogger(); - virtual void loadThread(); + virtual void loggerThread(); virtual void inputCallback(const ros::MessageEvent& event); virtual void updateDiagnostic(diagnostic_updater::DiagnosticStatusWrapper &stat); boost::shared_ptr msg_store_; - boost::thread deferred_load_thread_; + boost::thread logger_thread_; bool wait_for_insert_; bool initialized_; std::string input_topic_name_; diff --git a/jsk_robot_common/jsk_robot_startup/src/lightweight_logger_nodelet.cpp b/jsk_robot_common/jsk_robot_startup/src/lightweight_logger_nodelet.cpp index 0ce4d12658..21adf2e677 100644 --- a/jsk_robot_common/jsk_robot_startup/src/lightweight_logger_nodelet.cpp +++ b/jsk_robot_common/jsk_robot_startup/src/lightweight_logger_nodelet.cpp @@ -56,6 +56,12 @@ namespace jsk_robot_startup return; } + // settings for buffering logged messages + int buffer_capacity; + pnh_->param("buffer_capacity", buffer_capacity, 100); + buffer_.set_capacity(buffer_capacity); + + // settings for blocking/non-blocking message insertion pnh_->param("wait_for_insert", wait_for_insert_, false); input_topic_name_ = pnh_->resolveName("input", true); @@ -82,35 +88,86 @@ namespace jsk_robot_startup prev_insert_error_count_ = 0; init_time_ = ros::Time::now(); - deferred_load_thread_ = boost::thread( - boost::bind(&LightweightLogger::loadThread, this)); + // start logger thread + logger_thread_ = boost::thread( + boost::bind(&LightweightLogger::loggerThread, this)); } LightweightLogger::~LightweightLogger() { - if (msg_store_) { - msg_store_.reset(); - } - if (deferred_load_thread_.joinable()) { - NODELET_DEBUG_STREAM("Shutting down deferred load thread"); - bool ok = deferred_load_thread_.try_join_for(boost::chrono::seconds(5)); - if (!ok) { - NODELET_WARN_STREAM("Timed out to join deferred load thread."); - deferred_load_thread_.interrupt(); - ok = deferred_load_thread_.try_join_for(boost::chrono::seconds(3)); - if (!ok) { - NODELET_ERROR_STREAM("Failed to shutdown deferred load thread."); - } - } + NODELET_DEBUG_STREAM("destructor called"); + + // stop logger thread + if (logger_thread_.joinable()) { + NODELET_DEBUG_STREAM("Shutting down logger thread"); + logger_thread_.interrupt(); + bool ok = logger_thread_.try_join_for(boost::chrono::seconds(5)); if (ok) { - NODELET_DEBUG_STREAM("deferred load thread stopped"); + NODELET_INFO_STREAM("successsfully stopped logger thread"); + } else { + NODELET_WARN_STREAM("Timed out to join logger thread"); } } + + // deinit message store object + if (msg_store_) { + msg_store_.reset(); + } } - void LightweightLogger::loadThread() + void LightweightLogger::loggerThread() { + NODELET_DEBUG_STREAM("logger thread started"); + + // The message store object is initialized here, since the object waits for connection + // until the connection to the server is established. msg_store_.reset(new mongodb_store::MessageStoreProxy(*nh_, col_name_, db_name_)); initialized_ = true; + + // After message store object is initialized, this thread is re-used for + // lazy document insertion. + bool being_interrupted = false; + while (ros::ok()) { + try { + // check interruption + if (being_interrupted) { + if (!buffer_.empty()) { + NODELET_WARN_STREAM( + "The thread is interrupted through buffer is still not empty. " + "Continue to insert"); + } else { + NODELET_DEBUG_STREAM("buffer is empty. interrupted."); + break; + } + } + + // lazy document insertion + ros::MessageEvent event; + const double timeout = 0.5; + if (buffer_.get(event, timeout)) { + const std::string& publisher_name = event.getPublisherName(); + const boost::shared_ptr& msg = event.getConstMessage(); + jsk_topic_tools::StealthRelay::inputCallback(msg); + try + { + mongo::BSONObjBuilder meta; + meta.append("input_topic", input_topic_name_); + meta.append("published_by", publisher_name); + std::string doc_id = msg_store_->insert(*msg, meta.obj(), true); + NODELET_DEBUG_STREAM("Lazy inserted (" << input_topic_name_ << "): " << doc_id); + } + catch (...) { + NODELET_ERROR_STREAM("Failed to lazy insert"); + } + } else { + NODELET_DEBUG_STREAM("waiting for buffer..."); + } + } catch (boost::thread_interrupted e) { + NODELET_DEBUG_STREAM("logger thread being interrupted"); + being_interrupted = true; + } + } + + NODELET_DEBUG_STREAM("logger thread ended"); } void LightweightLogger::inputCallback(const ros::MessageEvent& event) From 148b0c587d20b6611d97f6ab80dfa53b9f3e1f1b Mon Sep 17 00:00:00 2001 From: Furushchev Date: Sun, 16 Dec 2018 15:55:24 +0900 Subject: [PATCH 43/70] auto select lazy/normal insertion using buffer --- .../src/lightweight_logger_nodelet.cpp | 47 ++++++++++++------- 1 file changed, 31 insertions(+), 16 deletions(-) diff --git a/jsk_robot_common/jsk_robot_startup/src/lightweight_logger_nodelet.cpp b/jsk_robot_common/jsk_robot_startup/src/lightweight_logger_nodelet.cpp index 21adf2e677..07feebc5a3 100644 --- a/jsk_robot_common/jsk_robot_startup/src/lightweight_logger_nodelet.cpp +++ b/jsk_robot_common/jsk_robot_startup/src/lightweight_logger_nodelet.cpp @@ -48,6 +48,7 @@ namespace jsk_robot_startup initialized_ = false; jsk_topic_tools::StealthRelay::onInit(); + // settings for database nh_->param("/robot/database", db_name_, "jsk_robot_lifelog"); nh_->param("/robot/name", col_name_, std::string()); if (col_name_.empty()) @@ -66,6 +67,7 @@ namespace jsk_robot_startup input_topic_name_ = pnh_->resolveName("input", true); + // settings for diagnostics double vital_rate; pnh_->param("vital_rate", vital_rate, 1.0); vital_checker_.reset( @@ -176,25 +178,38 @@ namespace jsk_robot_startup const boost::shared_ptr& msg = event.getConstMessage(); jsk_topic_tools::StealthRelay::inputCallback(msg); - if (!initialized_) { - NODELET_WARN_THROTTLE(1.0, "nodelet is not yet initialized"); - return; - } vital_checker_->poke(); - try - { - mongo::BSONObjBuilder meta; - meta.append("input_topic", input_topic_name_); - meta.append("published_by", publisher_name); - std::string doc_id = msg_store_->insert(*msg, meta.obj(), wait_for_insert_); - if (doc_id.empty()) - NODELET_DEBUG_STREAM("Inserted (" << input_topic_name_ << ")"); - else - NODELET_DEBUG_STREAM("Inserted (" << input_topic_name_ << "): " << doc_id); + bool on_the_fly = initialized_ && buffer_.empty(); + if (!wait_for_insert_ && msg_store_->getNumInsertSubscribers() == 0) { + // subscriber for message_store/insert does not exists + on_the_fly = false; } - catch (...) { - NODELET_ERROR_STREAM("Failed to insert to db"); + + if (on_the_fly) { + try + { + mongo::BSONObjBuilder meta; + meta.append("input_topic", input_topic_name_); + meta.append("published_by", publisher_name); + std::string doc_id = msg_store_->insert(*msg, meta.obj(), wait_for_insert_); + if (doc_id.empty()) + NODELET_DEBUG_STREAM("Inserted (" << input_topic_name_ << ")"); + else + NODELET_DEBUG_STREAM("Inserted (" << input_topic_name_ << "): " << doc_id); + } + catch (...) { + NODELET_ERROR_STREAM("Failed to insert to db"); + } + } else { + if (!initialized_) { + NODELET_WARN_THROTTLE(1.0, "nodelet is not yet initialized"); + } + if (buffer_.full()) { + NODELET_WARN_THROTTLE(1.0, "buffer is full. discarded old elements"); + } + buffer_.put(event); + NODELET_DEBUG_STREAM("Put into buffer for lazy insertion"); } } From e641cb6df9657733ecc16b00454bb51f55756e72 Mon Sep 17 00:00:00 2001 From: Furushchev Date: Sun, 16 Dec 2018 15:55:38 +0900 Subject: [PATCH 44/70] minor change --- .../jsk_robot_startup/lifelog/test/simulate_lazy.launch | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/jsk_robot_common/jsk_robot_startup/lifelog/test/simulate_lazy.launch b/jsk_robot_common/jsk_robot_startup/lifelog/test/simulate_lazy.launch index 8cdac3618d..25e793905a 100644 --- a/jsk_robot_common/jsk_robot_startup/lifelog/test/simulate_lazy.launch +++ b/jsk_robot_common/jsk_robot_startup/lifelog/test/simulate_lazy.launch @@ -33,7 +33,7 @@ + pkg="nodelet" type="nodelet" args="manager" output="screen"/> Date: Sun, 16 Dec 2018 16:00:30 +0900 Subject: [PATCH 45/70] force exit if insertion is failed on interruption --- .../jsk_robot_startup/src/lightweight_logger_nodelet.cpp | 4 ++++ 1 file changed, 4 insertions(+) diff --git a/jsk_robot_common/jsk_robot_startup/src/lightweight_logger_nodelet.cpp b/jsk_robot_common/jsk_robot_startup/src/lightweight_logger_nodelet.cpp index 07feebc5a3..bca92f2395 100644 --- a/jsk_robot_common/jsk_robot_startup/src/lightweight_logger_nodelet.cpp +++ b/jsk_robot_common/jsk_robot_startup/src/lightweight_logger_nodelet.cpp @@ -159,6 +159,10 @@ namespace jsk_robot_startup } catch (...) { NODELET_ERROR_STREAM("Failed to lazy insert"); + if (being_interrupted) { + NODELET_WARN_STREAM("Force exiting"); + break; + } } } else { NODELET_DEBUG_STREAM("waiting for buffer..."); From 48d04e570e06ab1f16b5ad7efe4ae518178da9b3 Mon Sep 17 00:00:00 2001 From: Furushchev Date: Tue, 18 Dec 2018 22:56:01 +0900 Subject: [PATCH 46/70] disable teb --- jsk_pr2_robot/jsk_pr2_startup/jsk_pr2_move_base/move_base.xml | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/jsk_pr2_robot/jsk_pr2_startup/jsk_pr2_move_base/move_base.xml b/jsk_pr2_robot/jsk_pr2_startup/jsk_pr2_move_base/move_base.xml index 5df84e2311..ef9a5ee80d 100644 --- a/jsk_pr2_robot/jsk_pr2_startup/jsk_pr2_move_base/move_base.xml +++ b/jsk_pr2_robot/jsk_pr2_startup/jsk_pr2_move_base/move_base.xml @@ -1,6 +1,6 @@ - + From 5fd224dd56f3c6d49bc4c68eaa61d542f6d48c12 Mon Sep 17 00:00:00 2001 From: Furushchev Date: Sat, 22 Dec 2018 19:21:59 +0900 Subject: [PATCH 47/70] active_user.l: set username if empty --- jsk_robot_common/jsk_robot_startup/lifelog/active_user.l | 3 ++- 1 file changed, 2 insertions(+), 1 deletion(-) diff --git a/jsk_robot_common/jsk_robot_startup/lifelog/active_user.l b/jsk_robot_common/jsk_robot_startup/lifelog/active_user.l index a1a9a7a263..1147ceef9c 100755 --- a/jsk_robot_common/jsk_robot_startup/lifelog/active_user.l +++ b/jsk_robot_common/jsk_robot_startup/lifelog/active_user.l @@ -37,7 +37,8 @@ (f (format nil "/tmp/username_~d.txt" (unix::getpid))) (setq name (read-line f))) (when (or (null name) (null-string-p name)) - (setq name *user*))) + (setq name *user*)) + (ros::set-param "/active_user/launch_user_name" name)) (set-alist :user-name name *init-status*)) ;; robot name (let ((name (ros::get-param "/robot/type" nil))) From b509f373823661e678aab432e9213ac00c09888d Mon Sep 17 00:00:00 2001 From: Furushchev Date: Sat, 22 Dec 2018 19:22:16 +0900 Subject: [PATCH 48/70] tweet_client.l: cleanup --- .../jsk_robot_startup/lifelog/tweet_client.l | 135 +++++++----------- 1 file changed, 49 insertions(+), 86 deletions(-) diff --git a/jsk_robot_common/jsk_robot_startup/lifelog/tweet_client.l b/jsk_robot_common/jsk_robot_startup/lifelog/tweet_client.l index 6f6a15bbe3..9ee788003b 100755 --- a/jsk_robot_common/jsk_robot_startup/lifelog/tweet_client.l +++ b/jsk_robot_common/jsk_robot_startup/lifelog/tweet_client.l @@ -2,96 +2,59 @@ (ros::load-ros-manifest "roseus") (ros::load-ros-manifest "topic_tools") -(load "package://pr2eus/speak.l") +(require :speak "package://pr2eus/speak.l") (ros::roseus "tweet_client") -(ros::advertise "/tweet" std_msgs::String 1) -;; next tweet timing ( x(i+1) = x(i) * 2, 5 * 60 = 300 sec) -(while (not (ros::has-param "/active_user/elapsed_time")) - (unix::sleep 3) - (ros::ros-info "Wait for /active_user/elapsed_time parameter ...")) +(defun tweet-string (twit-str + &key (with-speech t) + (with-image t) + (tweet-topic "/tweet") + (wait-for-camera 5)) + (when (and with-image + (ros::wait-for-service "/tweet_image_saver/save" 1) + (ros::wait-for-service "/tweet_image_mux/list" 1)) + (unix::system (format nil "rm -f /tmp/tweet_image.jpg")) + (when with-image + (setq wait-for-camera (floor wait-for-camera)) + (when (and with-speech (> wait-for-camera 0)) + (speak-jp (format nil "~A秒前" wait-for-camera) + :wait nil)) + (unix::sleep wait-for-camera) + ;; specify camera + (when (stringp with-image) + (let ((image-topics (send (ros::service-call "/tweet_image_mux/list" + (instance topic_tools::MuxListRequest :init)) + :topics)) + (selected-topic (send (one-shot-subscribe "/tweet_image_mux/selected" std_msgs::String) :data)) + prev-image-topic) + (unless (find with-image image-topics :test #'string=) + (ros::service-call "/tweet_image_mux/add" + (instance topic_tools::MuxAddRequest :init :topic with-image))) + (unless (string= with-image selected-topic) + (setq prev-iamge-topic + (send (ros::service-call "/tweet_image_mux/select" + (instance topic_tools::MuxSelectRequest :init :topic with-image)) + :prev_topic))))) + ;; camera shot sound + (when with-speech + (play-sound + (pathname (ros::resolve-ros-path "package://jsk_pr2_startup/jsk_pr2_lifelog/camera.wav")) + :wait t)) + ;; retrieve image + (call-empty-service "/tweet_image_saver/save" :wait t))) -(cond - ((ros::has-param "/active_user/tweet_second") - (setq *tweet-second* (ros::get-param "/active_user/tweet_second"))) - (t - (setq *tweet-second* 300) - )) - -(setq *target-second* (+ (ros::get-param "/active_user/elapsed_time") - *tweet-second*)) - -(setq *waking-tweet-second* 3600.0) -(cond - ((ros::has-param "/active_user/start_time") - (let ((st (ros::get-param "/active_user/start_time"))) - (setq *waking-target-second* - (+ (- (send (ros::time-now) :to-sec) st) - *waking-tweet-second*)))) - (t - (setq *waking-target-second* *waking-tweet-second*))) - -(defun tweet-string (twit-str &key (warning-time) (with-image) (image-wait 30) (speek t)) - (let (prev-image-topic) - (when warning-time - (unless (numberp warning-time) - (setq warning-time 3)) - (speak-jp (format nil "~Aびょうまえ" - (case warning-time - (0 "ぜろ") - (1 "いち") - (2 "に") - (3 "さん") - (4 "よん") - (5 "ご") - (6 "ろく") - (7 "なな") - (8 "はち") - (9 "きゅう") - (10 "じゅう") - (t "じゅういじょう")))) - (unix::sleep warning-time)) - - (when with-image - (unix::system (format nil "rm -f /tmp/tweet_image.jpg"))) - - ;; camera shot sound - (unless (ros::get-topic-publisher "robotsound_jp") - (ros::advertise "robotsound_jp" sound_play::SoundRequest 5) + (ros::ros-info "tweeting ~A" twit-str) + (unless (ros::get-topic-publisher tweet-topic) + (ros::advertise tweet-topic std_msgs::String 1) (unix:sleep 1)) - (ros::publish "robotsound_jp" - (instance sound_play::SoundRequest :init - :sound sound_play::SoundRequest::*play_file* - :command sound_play::SoundRequest::*play_once* - :arg (ros::resolve-ros-path "package://jsk_pr2_startup/jsk_pr2_lifelog/camera.wav"))) - - ;; specify camera - (when (stringp with-image) - (ros::wait-for-service "/tweet_image_mux/list") - (let ((current-image-list - (send (ros::service-call "/tweet_image_mux/list" (instance topic_tools::muxlistrequest :init)) :topics))) - (unless (find with-image current-image-list :test #'string=) - (ros::service-call "/tweet_image_mux/add" (instance topic_tools::muxaddrequest :init :topic with-image))) - (setq prev-image-topic - (send (ros::service-call "/tweet_image_mux/select" (instance topic_tools::muxselectrequest :init :topic with-image)) :prev_topic)))) - (unless prev-image-topic - (send (one-shot-subscribe "/tweet_image_mux/selected" std_msgs::String) :data)) + (ros::publish tweet-topic + (instance std_msgs::String :init + :data (if with-image + (format nil "~A /tmp/tweet_image.jpg" twit-str) + twit-str))) - ;; retrieve image - (call-empty-service "/tweet_image_saver/save" :wait t) - (ros::service-call "/tweet_image_mux/select" (instance topic_tools::muxselectrequest :init :topic prev-image-topic)) - - (ros::ros-info "tweeting ~A" twit-str) - (cond - (with-image - (ros::publish "/tweet" - (instance std_msgs::String :init - :data (format nil "~A ~A" - twit-str (format nil "/tmp/tweet_image.jpg"))))) - (t - (ros::publish "/tweet" (instance std_msgs::String :init :data twit-str)) - )) - (when speek (speak-jp "ついーとしました")) - ) + (when with-speech (speak-jp "ついーとしました" :wait t)) + t) +(provide :tweet_client) From 0d17e1f44257bca7f801bfd99e39e861febc91d0 Mon Sep 17 00:00:00 2001 From: Furushchev Date: Sat, 22 Dec 2018 19:22:40 +0900 Subject: [PATCH 49/70] tweet_client_*.l: cleanup --- .../lifelog/tweet_client_tablet.l | 3 +- .../lifelog/tweet_client_uptime.l | 98 +++++++++---------- .../lifelog/tweet_client_warning.l | 48 +++++---- .../lifelog/tweet_client_worktime.l | 93 ++++++++---------- 4 files changed, 112 insertions(+), 130 deletions(-) diff --git a/jsk_robot_common/jsk_robot_startup/lifelog/tweet_client_tablet.l b/jsk_robot_common/jsk_robot_startup/lifelog/tweet_client_tablet.l index ff5b640c83..6fcd57ad30 100755 --- a/jsk_robot_common/jsk_robot_startup/lifelog/tweet_client_tablet.l +++ b/jsk_robot_common/jsk_robot_startup/lifelog/tweet_client_tablet.l @@ -2,13 +2,14 @@ (ros::roseus "twitter_client_tablet") -(load "package://jsk_robot_startup/lifelog/tweet_client.l") +(require :tweet_client "package://jsk_robot_startup/lifelog/tweet_client.l") (defun twit-cb (msg) (let ((twit-str (send msg :data))) (tweet-string twit-str :warning-time nil :with-image "/tablet/marked/image_rect_color"))) + (ros::subscribe "/pr2twit_from_tablet" roseus::StringStamped #'twit-cb) (ros::spin) diff --git a/jsk_robot_common/jsk_robot_startup/lifelog/tweet_client_uptime.l b/jsk_robot_common/jsk_robot_startup/lifelog/tweet_client_uptime.l index 23072ecf97..cc19b7b51f 100755 --- a/jsk_robot_common/jsk_robot_startup/lifelog/tweet_client_uptime.l +++ b/jsk_robot_common/jsk_robot_startup/lifelog/tweet_client_uptime.l @@ -2,61 +2,55 @@ (ros::roseus "twitter_client_uptime") -(load "package://jsk_robot_startup/lifelog/tweet_client.l") +(require :tweet_client "package://jsk_robot_startup/lifelog/tweet_client.l") (setq *src-lines* nil) +(setq *start-time* nil) (setq *random-state* (coerce (unix::gettimeofday) integer-vector)) -(defvar *robot-name* "robot") -(when (ros::has-param "/active_user/robot_name") - (setq *robot-name* (ros::get-param "/active_user/robot_name")) - ) +(defvar *uptime-tweet-interval* (ros::get-param "~uptime_tweet_interval" 3600.0)) ;; [secs] -(while (ros::ok) - (setq *user-name* (ros::get-param "/active_user/launch_user_name") - *elapsed-time* (ros::get-param "/active_user/elapsed_time")) - (ros::rate 0.1) - ;; tweet depend on up time - (let ((st (ros::get-param "/active_user/start_time"))) - (when st - (let ((waking-time (- (send (ros::time-now) :to-sec) st))) - (ros::ros-info "~A waking ~A sec (~A)" *robot-name* waking-time *waking-target-second*) - (when (> waking-time *waking-target-second*) - (incf *waking-target-second* *waking-tweet-second*) - ;; tweet source of robot-interface - (unless *src-lines* - (let* ((dirname (ros::rospack-find "pr2eus")) - (fname (format nil "~A/robot-interface.l" dirname)) - str) - (with-open-file (f fname) - (while (setq str (read-line f nil nil)) - (push str *src-lines*))) - (setq *src-lines* (nreverse *src-lines*)) - )) +(defun tweet-uptime (&optional event) + ;; tweet source of robot-interface + (unless *start-time* + (setq *start-time* (ros::get-param "/active_user/start_time" nil))) + (unless *start-time* (return-from tweet-uptime nil)) + (unless *src-lines* + (let* ((dirname (ros::rospack-find "pr2eus")) + (fname (format nil "~A/robot-interface.l" dirname)) + str) + (with-open-file (f fname) + (while (setq str (read-line f nil nil)) + (push str *src-lines*))) + (setq *src-lines* (nreverse *src-lines*)))) + ;; + (let* ((len (length *src-lines*)) + (uptime (- (send (ros::time-now) :to-sec) *start-time*)) + (start-n (floor (random (float len) *random-state*))) + (spos 0) (str-len 0) lines) + (push (format nil "I am running ~A min." (round (/ uptime 60.0))) + lines) + (incf str-len (length (car lines))) + (while (< (+ start-n spos) len) + (let ((str (elt *src-lines* (+ start-n spos)))) + (incf str-len (length str)) + (if (> str-len 140) (return)) + (push str lines)) + (incf spos)) + (let* ((ln (apply #'+ (length lines) + (mapcar #'(lambda (x) (length x)) lines))) + (dt (make-string (1- ln))) + (pos 0)) + (dolist (s (nreverse lines)) + (replace dt s :start1 pos) + (incf pos (length s)) + (if (< pos (- ln 2)) (setf (elt dt pos) 10)) + (incf pos)) + (tweet-string dt) + ))) - (let* ((len (length *src-lines*)) - (start-n (floor (random (float len) *random-state*))) - (spos 0) (str-len 0) lines) - (push (format nil "I am running ~A min." (round (/ waking-time 60.0))) - lines) - (incf str-len (length (car lines))) - (while (< (+ start-n spos) len) - (let ((str (elt *src-lines* (+ start-n spos)))) - (incf str-len (length str)) - (if (> str-len 140) (return)) - (push str lines)) - (incf spos)) - (let* ((ln (apply #'+ (length lines) - (mapcar #'(lambda (x) (length x)) lines))) - (dt (make-string (1- ln))) - (pos 0)) - (dolist (s (nreverse lines)) - (replace dt s :start1 pos) - (incf pos (length s)) - (if (< pos (- ln 2)) (setf (elt dt pos) 10)) - (incf pos)) - (tweet-string dt) - )) - )))) - (ros::sleep) - ) +(defun main () + (ros::create-timer *uptime-tweet-interval* #'tweet-uptime) + (ros::spin)) + +(main) diff --git a/jsk_robot_common/jsk_robot_startup/lifelog/tweet_client_warning.l b/jsk_robot_common/jsk_robot_startup/lifelog/tweet_client_warning.l index 5bdadc4ca9..71e555b8ac 100755 --- a/jsk_robot_common/jsk_robot_startup/lifelog/tweet_client_warning.l +++ b/jsk_robot_common/jsk_robot_startup/lifelog/tweet_client_warning.l @@ -2,43 +2,39 @@ (ros::roseus "twitter_client_warning") -(load "package://jsk_robot_startup/lifelog/tweet_client.l") +(require :tweet_client "package://jsk_robot_startup/lifelog/tweet_client.l") (ros::load-ros-manifest "diagnostic_msgs") (defun diagnostics-cb (msg) (let ((diagnostics (make-hash-table :test #'equal)) - (tm (ros::time-now)) - status id) + (tm (ros::time-now)) + status id) (format t "~0,3f diagnostics_msgs~%" (send tm :to-sec)) (dolist (status (send msg :status)) ;; diagnostic_msgs::DiagnosticStatus::*WARN* (when (>= (send status :level) diagnostic_msgs::DiagnosticStatus::*WARN*) ;; diagnostic_msgs::DiagnosticStatus::*ERROR*) - (cond ((substringp "/Motors" (send status :name)) - t) ;; skip motors - ((substringp "/Other/Accelerometer" (send status :name)) t) - ((substringp "/Other/Pressure" (send status :name)) t) - ((and (string= "/Computers/Network/Wifi Status (ddwrt)" (send status :name)) - (string= "Updates Stale" (send status :message))) t) - ((and (string= "/Computers/Network" (send status :name)) - (string= "Error" (send status :message))) t) - ((position #\/ (send status :name) :count 2) ;; check depth of name - (setq key (subseq (send status :name) 0 (position #\/ (send status :name) :count 2))) - (when (> (length (send status :name)) (length (gethash key diagnostics))) - (setf (gethash key diagnostics) (cons (send status :name) (send status :message))) - ) ;; when - )) - )) ;; when / dolist + (cond ((substringp "/Motors" (send status :name)) t) ;; skip motors + ((substringp "/Other/Accelerometer" (send status :name)) t) + ((substringp "/Other/Pressure" (send status :name)) t) + ((and (string= "/Computers/Network/Wifi Status (ddwrt)" (send status :name)) + (string= "Updates Stale" (send status :message))) t) + ((and (string= "/Computers/Network" (send status :name)) + (string= "Error" (send status :message))) t) + ((position #\/ (send status :name) :count 2) ;; check depth of name + (setq key (subseq (send status :name) 0 (position #\/ (send status :name) :count 2))) + (when (> (length (send status :name)) (length (gethash key diagnostics))) + (setf (gethash key diagnostics) (cons (send status :name) (send status :message))) + ) ;; when + )) ;; cond + )) ;; when / dolist (maphash #'(lambda (k v) (format t "Warnings ~A ~A~%" (length status) v) (push v status)) diagnostics) (when status (setq id (random (length status))) (when (= (mod (round (send tm :sec)) 1000) 0) - (tweet-string (format nil "Warning!! ~A is ~A at ~0,3f" (car (elt status id)) (cdr (elt status id)) (send tm :to-sec))) - )) ;; when - )) ;; let + (tweet-string (format nil "Warning!! ~A is ~A at ~0,3f" (car (elt status id)) (cdr (elt status id)) (send tm :to-sec)) :with-image nil) + )) ;; when + )) (ros::subscribe "/diagnostics_agg" diagnostic_msgs::DiagnosticArray #'diagnostics-cb) -(ros::rate (/ 1.0 3.0)) -(while (ros::ok) - (ros::spin-once) - (ros::sleep) - ) +(ros::rate 0.1) +(ros::spin) diff --git a/jsk_robot_common/jsk_robot_startup/lifelog/tweet_client_worktime.l b/jsk_robot_common/jsk_robot_startup/lifelog/tweet_client_worktime.l index 398334bd73..b5b92b711c 100755 --- a/jsk_robot_common/jsk_robot_startup/lifelog/tweet_client_worktime.l +++ b/jsk_robot_common/jsk_robot_startup/lifelog/tweet_client_worktime.l @@ -2,55 +2,46 @@ (ros::roseus "twitter_client_worktime") -(load "package://jsk_robot_startup/lifelog/tweet_client.l") - -(defvar *robot-name* "robot") -(when (ros::has-param "/active_user/robot_name") - (setq *robot-name* (ros::get-param "/active_user/robot_name")) - ) - -(ros::rate 0.1) -(do-until-key - (setq *user-name* (ros::get-param "/active_user/launch_user_name") - *elapsed-time* (ros::get-param "/active_user/elapsed_time")) - (ros::ros-debug "user -> ~A, time -> ~A (~A) " - *user-name* *elapsed-time* *target-second*) - ;; tweet depend on working time - (when (> *elapsed-time* *target-second*) - (incf *target-second* *tweet-second* ) - (ros::ros-info "tweet ~A ~A" *user-name* *elapsed-time*) - (let ((mainstr (format nil "~A has used ~A for ~d minutes" - ;; why delete *user-name* after space ? - ;;(subseq *user-name* 0 - ;;(or (position #\space *user-name*) - ;;(length *user-name*))) - *user-name* - *robot-name* - (round (/ *elapsed-time* 60)))) - presubstr postsubstr) - (cond - ((< *elapsed-time* 600) ;; 5 min - (setq presubstr "Congratulations! " - postsubstr ", Let's get started!")) - ((< *elapsed-time* 910) ;; 15 min - (setq presubstr "Gooood! " - postsubstr ", Go ahead!")) - ((< *elapsed-time* 1820) ;; 30 min - (setq presubstr "So Nice! " +(require :tweet_client "package://jsk_robot_startup/lifelog/tweet_client.l") + +(defvar *robot-name* (ros::get-param "/active_user/robot_name" "robot")) +(defvar *user-name* (ros::get-param "/active_user/launch_user_name" "user")) +(defvar *worktime-tweet-interval* (ros::get-param "~worktime_tweet_interval" 300.0)) + + +(defun tweet-worktime (&optional event) + (unless (ros::has-param "/active_user/elapsed_time") + (return-from tweet-worktime nil)) + (let ((elapsed-time (ros::get-param "/active_user/elapsed_time")) + mainstr presubstr postsubstr) + (ros::ros-info "tweet ~A ~A" *user-name* elapsed-time) + (setq mainstr (format nil "~A has used ~A for ~d minutes" + *user-name* *robot-name* (round (/ elapsed-time 60)))) + (cond + ((< elapsed-time 600) ;; 5 min + (setq presubstr "Congratulations! " + postsubstr ", Let's get started!")) + ((< elapsed-time 910) ;; 15 min + (setq presubstr "Gooood! " + postsubstr ", Go ahead!")) + ((< elapsed-time 1820) ;; 30 min + (setq presubstr "So Nice! " postsubstr ", Go ahead!")) - ((< *elapsed-time* 2730) ;; 45 min - (setq presubstr "Fantastic! " - postsubstr ", Keep going!")) - ((< *elapsed-time* 3640) ;; 60 min - (setq presubstr "Amazing! " - postsubstr ", I'm not tired!")) - (t - (setq presubstr "Awesome! " - postsubstr ", Got some rest?"))) - - (tweet-string (format nil "~A~A~A" presubstr mainstr postsubstr) - :warning-time 1 :with-image t) - - )) - (ros::sleep) - ) + ((< elapsed-time 2730) ;; 45 min + (setq presubstr "Fantastic! " + postsubstr ", Keep going!")) + ((< elapsed-time 3640) ;; 60 min + (setq presubstr "Amazing! " + postsubstr ", I'm not tired!")) + (t + (setq presubstr "Awesome! " + postsubstr ", Got some rest?"))) + + (tweet-string (format nil "~A~A~A" presubstr mainstr postsubstr) + :wait-for-camera 3 :with-image t))) + +(defun main () + (ros::create-timer *worktime-tweet-interval* #'tweet-worktime) + (ros::spin)) + +(main) From 691e0c31124a2b5d058c33bb2d8ff7d271e3014f Mon Sep 17 00:00:00 2001 From: Furushchev Date: Sat, 22 Dec 2018 19:22:51 +0900 Subject: [PATCH 50/70] tweet.launch: update --- .../jsk_pr2_startup/jsk_pr2_lifelog/pr2-tweet-log.l | 2 +- jsk_robot_common/jsk_robot_startup/lifelog/tweet.launch | 7 +++++-- 2 files changed, 6 insertions(+), 3 deletions(-) diff --git a/jsk_pr2_robot/jsk_pr2_startup/jsk_pr2_lifelog/pr2-tweet-log.l b/jsk_pr2_robot/jsk_pr2_startup/jsk_pr2_lifelog/pr2-tweet-log.l index 8930f3f32b..aa3fadf061 100755 --- a/jsk_pr2_robot/jsk_pr2_startup/jsk_pr2_lifelog/pr2-tweet-log.l +++ b/jsk_pr2_robot/jsk_pr2_startup/jsk_pr2_lifelog/pr2-tweet-log.l @@ -75,7 +75,7 @@ (setq ret (send w :tweet)) (when ret (if *tweet* - (tweet-string ret :warning-time nil :with-image with-image :speek nil :image-wait 10)) + (tweet-string ret :wait-for-camera 0 :with-image with-image :with-speech nil)) (warn "tweeted: ~A...~%" (subseq ret 0 100)) (setq with-image nil) )))) diff --git a/jsk_robot_common/jsk_robot_startup/lifelog/tweet.launch b/jsk_robot_common/jsk_robot_startup/lifelog/tweet.launch index f647b7fbfc..64e161b807 100644 --- a/jsk_robot_common/jsk_robot_startup/lifelog/tweet.launch +++ b/jsk_robot_common/jsk_robot_startup/lifelog/tweet.launch @@ -17,7 +17,7 @@ - + @@ -34,9 +34,10 @@ + machine="$(arg machine)" output="screen"> + @@ -55,6 +56,7 @@ + + From b0441e760837c5e71ecb24541e698edb034ae61e Mon Sep 17 00:00:00 2001 From: Furushchev Date: Sun, 23 Dec 2018 13:44:50 +0900 Subject: [PATCH 51/70] pr2_tweet.launch: ignore pr2_tweet_log --- jsk_pr2_robot/jsk_pr2_startup/jsk_pr2_lifelog/pr2_tweet.launch | 2 ++ 1 file changed, 2 insertions(+) diff --git a/jsk_pr2_robot/jsk_pr2_startup/jsk_pr2_lifelog/pr2_tweet.launch b/jsk_pr2_robot/jsk_pr2_startup/jsk_pr2_lifelog/pr2_tweet.launch index 3e194c06b9..831a61e9fc 100644 --- a/jsk_pr2_robot/jsk_pr2_startup/jsk_pr2_lifelog/pr2_tweet.launch +++ b/jsk_pr2_robot/jsk_pr2_startup/jsk_pr2_lifelog/pr2_tweet.launch @@ -11,7 +11,9 @@ + From 7436b3e040457fd64e302dc3215c273ed0a2538e Mon Sep 17 00:00:00 2001 From: Furushchev Date: Sun, 23 Dec 2018 19:57:17 +0900 Subject: [PATCH 52/70] fix tweet_client_worktime --- .../lifelog/tweet_client_worktime.l | 13 +++++++++---- 1 file changed, 9 insertions(+), 4 deletions(-) diff --git a/jsk_robot_common/jsk_robot_startup/lifelog/tweet_client_worktime.l b/jsk_robot_common/jsk_robot_startup/lifelog/tweet_client_worktime.l index b5b92b711c..80586c3600 100755 --- a/jsk_robot_common/jsk_robot_startup/lifelog/tweet_client_worktime.l +++ b/jsk_robot_common/jsk_robot_startup/lifelog/tweet_client_worktime.l @@ -4,19 +4,24 @@ (require :tweet_client "package://jsk_robot_startup/lifelog/tweet_client.l") -(defvar *robot-name* (ros::get-param "/active_user/robot_name" "robot")) -(defvar *user-name* (ros::get-param "/active_user/launch_user_name" "user")) +(defvar *robot-name* (ros::get-param "/robot/name" "robot")) +(defvar *user-name* (ros::get-param "/active_user/launch_user_name" nil)) (defvar *worktime-tweet-interval* (ros::get-param "~worktime_tweet_interval" 300.0)) - +(defvar *prev-elapsed-time* 0.0) (defun tweet-worktime (&optional event) (unless (ros::has-param "/active_user/elapsed_time") (return-from tweet-worktime nil)) + (unless *user-name* + (setq *user-name* (ros::get-param "/active_user/launch_user_name" nil))) (let ((elapsed-time (ros::get-param "/active_user/elapsed_time")) mainstr presubstr postsubstr) (ros::ros-info "tweet ~A ~A" *user-name* elapsed-time) + (if (eq elapsed-time *prev-elapsed-time*) + (return-from tweet-worktime t) + (setq *prev-elapsed-time* elapsed-time)) (setq mainstr (format nil "~A has used ~A for ~d minutes" - *user-name* *robot-name* (round (/ elapsed-time 60)))) + (or *user-name* "An user") *robot-name* (round (/ elapsed-time 60)))) (cond ((< elapsed-time 600) ;; 5 min (setq presubstr "Congratulations! " From c01109e7c8ccba1c6a30250b37bc579217207c7c Mon Sep 17 00:00:00 2001 From: Furushchev Date: Mon, 24 Dec 2018 15:51:07 +0900 Subject: [PATCH 53/70] minor fix --- jsk_robot_common/jsk_robot_startup/util/start_launch_sound.py | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/jsk_robot_common/jsk_robot_startup/util/start_launch_sound.py b/jsk_robot_common/jsk_robot_startup/util/start_launch_sound.py index 3485bd35aa..1857bdd00c 100755 --- a/jsk_robot_common/jsk_robot_startup/util/start_launch_sound.py +++ b/jsk_robot_common/jsk_robot_startup/util/start_launch_sound.py @@ -5,7 +5,7 @@ # topic --> /robotsound or /robotsound_jp rospy.init_node("finish_launch_sound") -p = rospy.Publisher("/robotsound", SoundRequest) +p = rospy.Publisher("/robotsound", SoundRequest, queue_size=1) rospy.sleep(5) # sleep to wait for connection msg = SoundRequest() From a9991d68ed52a0a0f060fd38d8a172f0606cc120 Mon Sep 17 00:00:00 2001 From: Furushchev Date: Tue, 25 Dec 2018 12:31:26 +0900 Subject: [PATCH 54/70] periodic_replicator_client: show status text --- .../jsk_robot_startup/lifelog/periodic_replicator_client.py | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/jsk_robot_common/jsk_robot_startup/lifelog/periodic_replicator_client.py b/jsk_robot_common/jsk_robot_startup/lifelog/periodic_replicator_client.py index a6152d1455..3e606b8b8b 100755 --- a/jsk_robot_common/jsk_robot_startup/lifelog/periodic_replicator_client.py +++ b/jsk_robot_common/jsk_robot_startup/lifelog/periodic_replicator_client.py @@ -168,7 +168,7 @@ def done_cb(self, status, result): rospy.loginfo("Replication suceeded") self.insert_replicate_date() else: - rospy.logwarn("Replication finished with status %s" % status) + rospy.logwarn("Replication finished with status %s" % actionlib.get_name_of_constant(GoalStatus, status)) def active_cb(self): if not self.network_ok(): From c9556e81f206f5b8ea37543774d8421dd6ccfa16 Mon Sep 17 00:00:00 2001 From: Furushchev Date: Wed, 26 Dec 2018 23:19:19 +0900 Subject: [PATCH 55/70] pr2.launch: cleanup --- ...ow_filter.yaml => base_laser_filters.yaml} | 0 ...aml => base_laser_filters_for_gazebo.yaml} | 0 .../jsk_pr2_sensors/tilt_scan_filters.yaml | 20 ----------- .../jsk_pr2_sensors/tilt_shadow_filter.yaml | 15 -------- jsk_pr2_robot/jsk_pr2_startup/pr2.launch | 34 +++---------------- 5 files changed, 5 insertions(+), 64 deletions(-) rename jsk_pr2_robot/jsk_pr2_startup/jsk_pr2_sensors/{shadow_filter.yaml => base_laser_filters.yaml} (100%) rename jsk_pr2_robot/jsk_pr2_startup/jsk_pr2_sensors/{shadow_filter_for_gazebo.yaml => base_laser_filters_for_gazebo.yaml} (100%) delete mode 100644 jsk_pr2_robot/jsk_pr2_startup/jsk_pr2_sensors/tilt_scan_filters.yaml delete mode 100644 jsk_pr2_robot/jsk_pr2_startup/jsk_pr2_sensors/tilt_shadow_filter.yaml diff --git a/jsk_pr2_robot/jsk_pr2_startup/jsk_pr2_sensors/shadow_filter.yaml b/jsk_pr2_robot/jsk_pr2_startup/jsk_pr2_sensors/base_laser_filters.yaml similarity index 100% rename from jsk_pr2_robot/jsk_pr2_startup/jsk_pr2_sensors/shadow_filter.yaml rename to jsk_pr2_robot/jsk_pr2_startup/jsk_pr2_sensors/base_laser_filters.yaml diff --git a/jsk_pr2_robot/jsk_pr2_startup/jsk_pr2_sensors/shadow_filter_for_gazebo.yaml b/jsk_pr2_robot/jsk_pr2_startup/jsk_pr2_sensors/base_laser_filters_for_gazebo.yaml similarity index 100% rename from jsk_pr2_robot/jsk_pr2_startup/jsk_pr2_sensors/shadow_filter_for_gazebo.yaml rename to jsk_pr2_robot/jsk_pr2_startup/jsk_pr2_sensors/base_laser_filters_for_gazebo.yaml diff --git a/jsk_pr2_robot/jsk_pr2_startup/jsk_pr2_sensors/tilt_scan_filters.yaml b/jsk_pr2_robot/jsk_pr2_startup/jsk_pr2_sensors/tilt_scan_filters.yaml deleted file mode 100644 index 5102765bfa..0000000000 --- a/jsk_pr2_robot/jsk_pr2_startup/jsk_pr2_sensors/tilt_scan_filters.yaml +++ /dev/null @@ -1,20 +0,0 @@ -#### laser_scan_assembler -filters: - - name: shadows - type: laser_filters/ScanShadowsFilter - params: - min_angle: 10 - max_angle: 170 - neighbors: 20 - window: 1 - - name: through - type: laser_filters/LaserScanRangeFilter - params: - lower_threshold: 0.3 - upper_threshold: 2.8 -# - name: intent -# type: laser_filters/LaserScanIntensityFilter -# params: -# lower_threshold: 750.0 -# upper_threshold: 100000.0 -# disp_histogram: 0 diff --git a/jsk_pr2_robot/jsk_pr2_startup/jsk_pr2_sensors/tilt_shadow_filter.yaml b/jsk_pr2_robot/jsk_pr2_startup/jsk_pr2_sensors/tilt_shadow_filter.yaml deleted file mode 100644 index 5f736357e8..0000000000 --- a/jsk_pr2_robot/jsk_pr2_startup/jsk_pr2_sensors/tilt_shadow_filter.yaml +++ /dev/null @@ -1,15 +0,0 @@ -scan_filter_chain: -- name: shadows - type: laser_filters/ScanShadowsFilter - params: - min_angle: 10 - max_angle: 170 - neighbors: 20 - window: 1 -- name: dark_shadows - type: laser_filters/LaserScanIntensityFilter - params: - lower_threshold: 100 - upper_threshold: 10000 - disp_histogram: 0 - diff --git a/jsk_pr2_robot/jsk_pr2_startup/pr2.launch b/jsk_pr2_robot/jsk_pr2_startup/pr2.launch index c53abbbe84..1826e5f113 100644 --- a/jsk_pr2_robot/jsk_pr2_startup/pr2.launch +++ b/jsk_pr2_robot/jsk_pr2_startup/pr2.launch @@ -38,12 +38,11 @@ - - - - - - + + + + - - - - - - - - - - - - - - - - - - - - - - From aecae518f36184e0a6b272a2d9bd5fbd964ea2ec Mon Sep 17 00:00:00 2001 From: Furushchev Date: Wed, 26 Dec 2018 23:31:22 +0900 Subject: [PATCH 56/70] publish_empty_cloud: support lazy --- .../jsk_pr2_sensors/publish_empty_cloud.py | 143 ++++++++++-------- 1 file changed, 79 insertions(+), 64 deletions(-) diff --git a/jsk_pr2_robot/jsk_pr2_startup/jsk_pr2_sensors/publish_empty_cloud.py b/jsk_pr2_robot/jsk_pr2_startup/jsk_pr2_sensors/publish_empty_cloud.py index d44e09e90a..124bc0029a 100755 --- a/jsk_pr2_robot/jsk_pr2_startup/jsk_pr2_sensors/publish_empty_cloud.py +++ b/jsk_pr2_robot/jsk_pr2_startup/jsk_pr2_sensors/publish_empty_cloud.py @@ -1,74 +1,89 @@ #!/usr/bin/env python -import roslib; roslib.load_manifest('sensor_msgs') + import rospy +from jsk_topic_tools import ConnectionBasedTransport import math from struct import * -from sensor_msgs.msg import * - -# rotate x -def talker(): - rospy.init_node('empty_cloud_publisher') - pub = rospy.Publisher('empty_cloud', PointCloud2, queue_size=1) - frame_id = rospy.get_param('~frame_id', '/base_laser_link') - max_range = rospy.get_param('~max_range', 25.0) - rate = rospy.get_param('~rate', 10) ## Hz - rotate_points = rospy.get_param('~rotate_points', False) - - irange = range(-314, 315) - jrange = range(-314, 315) - msg = PointCloud2() - msg.header.frame_id = frame_id - msg.height = 1 - if rotate_points: - msg.width = len(irange) * len(jrange) - else: - msg.width = len(irange) - - msg_fieldx = PointField() - msg_fieldy = PointField() - msg_fieldz = PointField() - msg_fieldx.name = 'x' - msg_fieldx.offset = 0 - msg_fieldx.datatype = 7 - msg_fieldx.count = 1 - msg_fieldy.name = 'y' - msg_fieldy.offset = 4 - msg_fieldy.datatype = 7 - msg_fieldy.count = 1 - msg_fieldz.name = 'z' - msg_fieldz.offset = 8 - msg_fieldz.datatype = 7 - msg_fieldz.count = 1 - - msg.fields = [msg_fieldx, msg_fieldy, msg_fieldz] - msg.point_step = 16 - msg.row_step = msg.point_step * msg.width; - msg.is_dense = True - - points = [] - for i in irange: - x = max_range * math.cos(i*0.005) - y = max_range * math.sin(i*0.005) +from sensor_msgs.msg import PointCloud2, PointField + + +class PublishEmptyCloud(ConnectionBasedTransport): + def __init__(self): + super(PublishEmptyCloud, self).__init__() + + frame_id = rospy.get_param('~frame_id', '/base_laser_link') + max_range = rospy.get_param('~max_range', 25.0) + rotate_points = rospy.get_param('~rotate_points', False) + + self.pub_msg = self.make_empty_cloud(frame_id, max_range, rotate_points) + + self.rate = 1.0 / rospy.get_param('~rate', 10) ## Hz + self.timer = None + self.pub_cloud = self.advertise('empty_cloud', PointCloud2, queue_size=1) + + def subscribe(self): + self.timer = rospy.Timer(rospy.Duration(self.rate), self.timer_callback) + + def unsubscribe(self): + self.timer.stop() + self.timer = None + + def make_empty_cloud(self, frame_id, max_range, rotate_points): + irange = range(-314, 315) + jrange = range(-314, 315) + msg = PointCloud2() + msg.header.frame_id = frame_id + msg.height = 1 if rotate_points: - for j in jrange: - # x rotation - # x = max_range * math.cos(i*0.005) - # y = max_range * math.sin(i*0.005) * math.cos(j * 0.005) - # z = -1 * max_range * math.sin(i*0.005) * math.sin(j * 0.005) - # y rotation - points.append(pack(' Date: Wed, 26 Dec 2018 23:33:01 +0900 Subject: [PATCH 57/70] fix laser topics --- .../jsk_pr2_move_base/amcl_node.xml | 2 +- .../costmap_common_params.yaml | 10 +- .../jsk_pr2_move_base/move_base.xml | 5 - .../jsk_pr2_move_base/pr2_2dnav.launch | 6 +- .../jsk_pr2_move_base/safe_teleop.xml | 2 +- .../safe_teleop_pr2_params.yaml | 38 +++-- .../jsk_pr2_sensors/lasers_and_filters.xml | 147 +++++++++++------- .../jsk_pr2_sensors/tilt_laser_filters.yaml | 37 +++++ 8 files changed, 160 insertions(+), 87 deletions(-) create mode 100644 jsk_pr2_robot/jsk_pr2_startup/jsk_pr2_sensors/tilt_laser_filters.yaml diff --git a/jsk_pr2_robot/jsk_pr2_startup/jsk_pr2_move_base/amcl_node.xml b/jsk_pr2_robot/jsk_pr2_startup/jsk_pr2_move_base/amcl_node.xml index e1663d7c2a..fae6012982 100644 --- a/jsk_pr2_robot/jsk_pr2_startup/jsk_pr2_move_base/amcl_node.xml +++ b/jsk_pr2_robot/jsk_pr2_startup/jsk_pr2_move_base/amcl_node.xml @@ -2,7 +2,7 @@ - + diff --git a/jsk_pr2_robot/jsk_pr2_startup/jsk_pr2_move_base/costmap_common_params.yaml b/jsk_pr2_robot/jsk_pr2_startup/jsk_pr2_move_base/costmap_common_params.yaml index 60bb390928..56b29e5438 100644 --- a/jsk_pr2_robot/jsk_pr2_startup/jsk_pr2_move_base/costmap_common_params.yaml +++ b/jsk_pr2_robot/jsk_pr2_startup/jsk_pr2_move_base/costmap_common_params.yaml @@ -4,15 +4,15 @@ transform_tolerance: 1.0 # 0.2 # parameters for static_layer static_layer: - map_topic: map + map_topic: /map subscribe_to_updates: true # parameters for obstacle_layer obstacle_layer: - observation_sources: base_scan_marking tilt_scan_filtered - base_scan_marking: + observation_sources: base_scan_filtered tilt_scan_filtered + base_scan_filtered: data_type: PointCloud2 - topic: base_scan_marking + topic: /base_scan_filtered sensor_frame: base_laser_link marking: true clearing: true @@ -24,7 +24,7 @@ obstacle_layer: raytrace_range: 3.0 tilt_scan_filtered: data_type: PointCloud2 - topic: tilt_scan_filtered + topic: /tilt_scan_filtered/navigation sensor_frame: laser_tilt_link marking: true clearing: true diff --git a/jsk_pr2_robot/jsk_pr2_startup/jsk_pr2_move_base/move_base.xml b/jsk_pr2_robot/jsk_pr2_startup/jsk_pr2_move_base/move_base.xml index ef9a5ee80d..d8f4af8b67 100644 --- a/jsk_pr2_robot/jsk_pr2_startup/jsk_pr2_move_base/move_base.xml +++ b/jsk_pr2_robot/jsk_pr2_startup/jsk_pr2_move_base/move_base.xml @@ -7,11 +7,6 @@ - - - diff --git a/jsk_pr2_robot/jsk_pr2_startup/jsk_pr2_move_base/pr2_2dnav.launch b/jsk_pr2_robot/jsk_pr2_startup/jsk_pr2_move_base/pr2_2dnav.launch index 9445889a09..779a193756 100644 --- a/jsk_pr2_robot/jsk_pr2_startup/jsk_pr2_move_base/pr2_2dnav.launch +++ b/jsk_pr2_robot/jsk_pr2_startup/jsk_pr2_move_base/pr2_2dnav.launch @@ -17,13 +17,15 @@ - + args="tilt_scan_filtered/navigation tilt_scan_filtered empty_cloud"> + + lazy: true + diff --git a/jsk_pr2_robot/jsk_pr2_startup/jsk_pr2_move_base/safe_teleop.xml b/jsk_pr2_robot/jsk_pr2_startup/jsk_pr2_move_base/safe_teleop.xml index c33f98f6c2..f4f3f52710 100644 --- a/jsk_pr2_robot/jsk_pr2_startup/jsk_pr2_move_base/safe_teleop.xml +++ b/jsk_pr2_robot/jsk_pr2_startup/jsk_pr2_move_base/safe_teleop.xml @@ -33,11 +33,11 @@ + - [[-0.325, -0.325], [-0.325, 0.325], [0.325, 0.325], [0.46, 0.0], [0.325, -0.325]] diff --git a/jsk_pr2_robot/jsk_pr2_startup/jsk_pr2_move_base/safe_teleop_pr2_params.yaml b/jsk_pr2_robot/jsk_pr2_startup/jsk_pr2_move_base/safe_teleop_pr2_params.yaml index 44b24f2bfd..9ffc4550cf 100644 --- a/jsk_pr2_robot/jsk_pr2_startup/jsk_pr2_move_base/safe_teleop_pr2_params.yaml +++ b/jsk_pr2_robot/jsk_pr2_startup/jsk_pr2_move_base/safe_teleop_pr2_params.yaml @@ -37,17 +37,29 @@ local_costmap: unknown_threshold: 8 mark_threshold: 0 - observation_sources: base_scan_marking base_scan tilt_scan ground_object_cloud - - base_scan_marking: {sensor_frame: base_laser_link, topic: /base_scan_marking, data_type: PointCloud2, expected_update_rate: 0.2, - observation_persistence: 0.0, marking: true, clearing: false, min_obstacle_height: 0.08, max_obstacle_height: 2.0} - - base_scan: {sensor_frame: base_laser_link, topic: /base_scan, data_type: LaserScan, expected_update_rate: 0.2, - observation_persistence: 0.0, marking: false, clearing: true, min_obstacle_height: -0.10, max_obstacle_height: 2.0} - - tilt_scan: {sensor_frame: laser_tilt_link, topic: /tilt_scan, data_type: LaserScan, expected_update_rate: 0.2, - observation_persistence: 0.2, marking: false, clearing: true, min_obstacle_height: -20.00, max_obstacle_height: 40.0} - - ground_object_cloud: {sensor_frame: laser_tilt_link, topic: /ground_object_cloud, data_type: PointCloud2, expected_update_rate: 0.2, - observation_persistence: 4.6, marking: true, clearing: false, min_obstacle_height: -0.10, max_obstacle_height: 2.0} + observation_sources: base_scan_filtered tilt_scan_filtered + base_scan_filtered: + sensor_frame: base_laser_link + topic: /base_scan_filtered + data_type: PointCloud2 + expected_update_rate: 0.2 + observation_persistence: 0.0 + marking: true + clearing: true + min_obstacle_height: 0.25 + max_obstacle_height: 0.35 + obstacle_range: 5.0 + raytrace_range: 3.0 + tilt_scan_filtered: + sensor_frame: laser_tilt_link + topic: /tilt_scan_filtered + data_type: LaserScan + expected_update_rate: 0.25 + observation_persistence: 2.5 + marking: true + clearing: true + min_obstacle_height: 0.05 + max_obstacle_height: 3.0 + obstacle_range: 5.0 + raytrace_range: 3.0 diff --git a/jsk_pr2_robot/jsk_pr2_startup/jsk_pr2_sensors/lasers_and_filters.xml b/jsk_pr2_robot/jsk_pr2_startup/jsk_pr2_sensors/lasers_and_filters.xml index 9b61dc8f96..35e5875a2e 100644 --- a/jsk_pr2_robot/jsk_pr2_startup/jsk_pr2_sensors/lasers_and_filters.xml +++ b/jsk_pr2_robot/jsk_pr2_startup/jsk_pr2_sensors/lasers_and_filters.xml @@ -1,75 +1,102 @@ + + - - - - - - - - - - - + - - - - - + + + + + + + + + + + - - + + + + + - - + + + + + + + - - + + + + + min_height: -0.01 + max_height: 0.01 + angle_min: -1.57079637051 + angle_max: 1.56643295288 + angle_increment: 0.00436332309619 + scan_time: 0.0250000003725 + range_min: 0.023 + range_max: 60.0 + concurrency_level: 0 + + - + - - - + + + + - - - - - - - - - + + + + + + + + + - - + + + + + + + - - + + + + + min_height: 0.25 + max_height: 0.35 + angle_min: -2.26892805099 + angle_max: 2.26456475258 + angle_increment: 0.00436332309619 + scan_time: 0.0500000007451 + range_min: 0.023 + range_max: 60.0 + concurrency_level: 0 + + + - - - - - - - - - - - - - - - diff --git a/jsk_pr2_robot/jsk_pr2_startup/jsk_pr2_sensors/tilt_laser_filters.yaml b/jsk_pr2_robot/jsk_pr2_startup/jsk_pr2_sensors/tilt_laser_filters.yaml new file mode 100644 index 0000000000..b8667ef73c --- /dev/null +++ b/jsk_pr2_robot/jsk_pr2_startup/jsk_pr2_sensors/tilt_laser_filters.yaml @@ -0,0 +1,37 @@ +scan_filter_chain: +- name: shadows + type: laser_filters/ScanShadowsFilter + params: + min_angle: 10 + max_angle: 170 + neighbors: 20 + window: 1 +- name: dark_shadows + type: laser_filters/LaserScanIntensityFilter + params: + lower_threshold: 100 + upper_threshold: 10000 + disp_histogram: 0 +- name: angular_bounds + type: laser_filters/LaserScanAngularBoundsFilter + params: + lower_angle: -1.22173048 + upper_angle: 1.22173048 +- name: downscan_filter + type: laser_tilt_controller_filter/LaserTiltControllerFilter + params: + filter_sections: [1] + # The 1.75 entry here is .05 seconds less than the 1.8 entry in pr2_move_base/scripts/pr2_move_base.py. + # The reduction prevents marking obstacles at the top edge of the tilt scan, which for some reason do + # not reliably get cleared by later clearing scans. + tilt_profile_times: [0.0, 1.75, 2.3125] +- name: remove_in_floor + type: laser_filters/LaserScanBoxFilter + params: + box_frame: base_footprint + min_x: -60.0 + max_x: 60.0 + min_y: -60.0 + max_y: 60.0 + min_z: 0.05 + max_z: 10.0 From d14a91a9c2904325824b1390041991ed78040f9f Mon Sep 17 00:00:00 2001 From: Furushchev Date: Sun, 6 Jan 2019 23:58:07 +0900 Subject: [PATCH 58/70] update safe_teleop --- .../jsk_pr2_move_base/safe_teleop_pr2_params.yaml | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/jsk_pr2_robot/jsk_pr2_startup/jsk_pr2_move_base/safe_teleop_pr2_params.yaml b/jsk_pr2_robot/jsk_pr2_startup/jsk_pr2_move_base/safe_teleop_pr2_params.yaml index 9ffc4550cf..816f33b962 100644 --- a/jsk_pr2_robot/jsk_pr2_startup/jsk_pr2_move_base/safe_teleop_pr2_params.yaml +++ b/jsk_pr2_robot/jsk_pr2_startup/jsk_pr2_move_base/safe_teleop_pr2_params.yaml @@ -54,7 +54,7 @@ local_costmap: tilt_scan_filtered: sensor_frame: laser_tilt_link topic: /tilt_scan_filtered - data_type: LaserScan + data_type: PointCloud2 expected_update_rate: 0.25 observation_persistence: 2.5 marking: true From 627f0f8eb2e493daf810036f2117e832e0ce1040 Mon Sep 17 00:00:00 2001 From: Furushchev Date: Mon, 7 Jan 2019 14:43:21 +0900 Subject: [PATCH 59/70] adjust params for move base tilt observation persistence --- .../jsk_pr2_move_base/costmap_common_params.yaml | 4 ++-- jsk_pr2_robot/jsk_pr2_startup/jsk_pr2_move_base/move_base.xml | 2 +- 2 files changed, 3 insertions(+), 3 deletions(-) diff --git a/jsk_pr2_robot/jsk_pr2_startup/jsk_pr2_move_base/costmap_common_params.yaml b/jsk_pr2_robot/jsk_pr2_startup/jsk_pr2_move_base/costmap_common_params.yaml index 56b29e5438..5fbfaf8819 100644 --- a/jsk_pr2_robot/jsk_pr2_startup/jsk_pr2_move_base/costmap_common_params.yaml +++ b/jsk_pr2_robot/jsk_pr2_startup/jsk_pr2_move_base/costmap_common_params.yaml @@ -28,9 +28,9 @@ obstacle_layer: sensor_frame: laser_tilt_link marking: true clearing: true - observation_persistence: 1.0 + observation_persistence: 2.5 expected_update_rate: 0.25 - min_obstacle_height: 0.1 + min_obstacle_height: 0.025 max_obstacle_height: 2.0 obstacle_range: 2.5 raytrace_range: 3.0 diff --git a/jsk_pr2_robot/jsk_pr2_startup/jsk_pr2_move_base/move_base.xml b/jsk_pr2_robot/jsk_pr2_startup/jsk_pr2_move_base/move_base.xml index d8f4af8b67..12f97172e9 100644 --- a/jsk_pr2_robot/jsk_pr2_startup/jsk_pr2_move_base/move_base.xml +++ b/jsk_pr2_robot/jsk_pr2_startup/jsk_pr2_move_base/move_base.xml @@ -8,7 +8,7 @@ respawn="true" machine="c2" /> + machine="c2" clear_params="true" output="screen" respawn="true"> From 4ca3fb5b8f538225319c1a17d38ee029191fefc6 Mon Sep 17 00:00:00 2001 From: Furushchev Date: Mon, 7 Jan 2019 15:00:52 +0900 Subject: [PATCH 60/70] update safe teleop params --- .../jsk_pr2_move_base/costmap_common_params.yaml | 2 +- .../jsk_pr2_startup/jsk_pr2_move_base/safe_teleop.xml | 4 ++-- .../jsk_pr2_move_base/safe_teleop_pr2_params.yaml | 9 +++++---- 3 files changed, 8 insertions(+), 7 deletions(-) diff --git a/jsk_pr2_robot/jsk_pr2_startup/jsk_pr2_move_base/costmap_common_params.yaml b/jsk_pr2_robot/jsk_pr2_startup/jsk_pr2_move_base/costmap_common_params.yaml index 5fbfaf8819..16756e5cd4 100644 --- a/jsk_pr2_robot/jsk_pr2_startup/jsk_pr2_move_base/costmap_common_params.yaml +++ b/jsk_pr2_robot/jsk_pr2_startup/jsk_pr2_move_base/costmap_common_params.yaml @@ -30,7 +30,7 @@ obstacle_layer: clearing: true observation_persistence: 2.5 expected_update_rate: 0.25 - min_obstacle_height: 0.025 + min_obstacle_height: 0.05 max_obstacle_height: 2.0 obstacle_range: 2.5 raytrace_range: 3.0 diff --git a/jsk_pr2_robot/jsk_pr2_startup/jsk_pr2_move_base/safe_teleop.xml b/jsk_pr2_robot/jsk_pr2_startup/jsk_pr2_move_base/safe_teleop.xml index f4f3f52710..9ead3d9fe6 100644 --- a/jsk_pr2_robot/jsk_pr2_startup/jsk_pr2_move_base/safe_teleop.xml +++ b/jsk_pr2_robot/jsk_pr2_startup/jsk_pr2_move_base/safe_teleop.xml @@ -31,8 +31,8 @@ - + diff --git a/jsk_pr2_robot/jsk_pr2_startup/jsk_pr2_move_base/safe_teleop_pr2_params.yaml b/jsk_pr2_robot/jsk_pr2_startup/jsk_pr2_move_base/safe_teleop_pr2_params.yaml index 816f33b962..f673029883 100644 --- a/jsk_pr2_robot/jsk_pr2_startup/jsk_pr2_move_base/safe_teleop_pr2_params.yaml +++ b/jsk_pr2_robot/jsk_pr2_startup/jsk_pr2_move_base/safe_teleop_pr2_params.yaml @@ -5,6 +5,7 @@ local_costmap: transform_tolerance: 0.2 obstacle_range: 2.5 raytrace_range: 3.0 + footprint_clearing_enabled: true inflation_layer: inflation_radius: 0.2 inflation_radius: 0.2 @@ -21,7 +22,7 @@ local_costmap: origin_x: 0.0 origin_y: 0.0 - publish_voxel_map: true + publish_voxel_map: false map_type: voxel obstacle_layer: origin_z: 0.0 @@ -44,7 +45,7 @@ local_costmap: topic: /base_scan_filtered data_type: PointCloud2 expected_update_rate: 0.2 - observation_persistence: 0.0 + observation_persistence: 1.0 marking: true clearing: true min_obstacle_height: 0.25 @@ -59,7 +60,7 @@ local_costmap: observation_persistence: 2.5 marking: true clearing: true - min_obstacle_height: 0.05 - max_obstacle_height: 3.0 + min_obstacle_height: 0.075 + max_obstacle_height: 2.0 obstacle_range: 5.0 raytrace_range: 3.0 From 058d192b9e77568757432092d1a54d5280c5fb4d Mon Sep 17 00:00:00 2001 From: Furushchev Date: Tue, 12 Feb 2019 15:25:13 +0900 Subject: [PATCH 61/70] change costmap settings --- .../local_costmap_params.yaml | 4 ++-- .../safe_teleop_pr2_params.yaml | 21 +++++++------------ 2 files changed, 9 insertions(+), 16 deletions(-) diff --git a/jsk_pr2_robot/jsk_pr2_startup/jsk_pr2_move_base/local_costmap_params.yaml b/jsk_pr2_robot/jsk_pr2_startup/jsk_pr2_move_base/local_costmap_params.yaml index ec253b5a57..7ec2fe9d73 100644 --- a/jsk_pr2_robot/jsk_pr2_startup/jsk_pr2_move_base/local_costmap_params.yaml +++ b/jsk_pr2_robot/jsk_pr2_startup/jsk_pr2_move_base/local_costmap_params.yaml @@ -7,8 +7,8 @@ global_frame: odom_combined publish_frequency: 5.0 static_map: false rolling_window: true -width: 4.0 # 3.0 -height: 4.0 # 3.0 +width: 5.0 # 3.0 +height: 5.0 # 3.0 resolution: 0.025 # 0.06 footprint: [[-0.325, -0.325], [-0.425, -0.175], [-0.425, 0.175], [-0.325, 0.325], [0.325, 0.325], [0.375, 0.0], [0.325, -0.325]] diff --git a/jsk_pr2_robot/jsk_pr2_startup/jsk_pr2_move_base/safe_teleop_pr2_params.yaml b/jsk_pr2_robot/jsk_pr2_startup/jsk_pr2_move_base/safe_teleop_pr2_params.yaml index f673029883..7ee3f30d97 100644 --- a/jsk_pr2_robot/jsk_pr2_startup/jsk_pr2_move_base/safe_teleop_pr2_params.yaml +++ b/jsk_pr2_robot/jsk_pr2_startup/jsk_pr2_move_base/safe_teleop_pr2_params.yaml @@ -6,9 +6,6 @@ local_costmap: obstacle_range: 2.5 raytrace_range: 3.0 footprint_clearing_enabled: true - inflation_layer: - inflation_radius: 0.2 - inflation_radius: 0.2 global_frame: odom_combined robot_base_frame: base_footprint update_frequency: 5.0 @@ -16,27 +13,23 @@ local_costmap: static_map: false rolling_window: true - width: 10.0 - height: 10.0 + width: 5.0 + height: 5.0 resolution: 0.025 origin_x: 0.0 origin_y: 0.0 - publish_voxel_map: false map_type: voxel obstacle_layer: origin_z: 0.0 z_voxels: 16 - z_resolution: 0.081 + z_resolution: 0.1 unknown_cost_value: 0 - unknown_threshold: 8 + unknown_threshold: 4 mark_threshold: 0 - origin_z: 0.0 - z_voxels: 16 - z_resolution: 0.081 - unknown_cost_value: 0 - unknown_threshold: 8 - mark_threshold: 0 + publish_voxel_map: false + inflation_layer: + inflation_radius: 0.2 observation_sources: base_scan_filtered tilt_scan_filtered From 12290f8e1f9ddc5b95c1957909f2883fc5400ffc Mon Sep 17 00:00:00 2001 From: Furushchev Date: Tue, 12 Feb 2019 15:25:26 +0900 Subject: [PATCH 62/70] fix battery_warning --- .../jsk_pr2_startup/jsk_pr2_warning/battery_warning.py | 9 ++++----- 1 file changed, 4 insertions(+), 5 deletions(-) diff --git a/jsk_pr2_robot/jsk_pr2_startup/jsk_pr2_warning/battery_warning.py b/jsk_pr2_robot/jsk_pr2_startup/jsk_pr2_warning/battery_warning.py index b77061ac59..291bf97412 100755 --- a/jsk_pr2_robot/jsk_pr2_startup/jsk_pr2_warning/battery_warning.py +++ b/jsk_pr2_robot/jsk_pr2_startup/jsk_pr2_warning/battery_warning.py @@ -32,19 +32,18 @@ def __init__(self): self.warn_repeat_rate = rospy.get_param("~warn_repeat_rate", 180) self.log_rate = rospy.get_param("~log_rate", 10) self.log_path = rospy.get_param("~log_path", None) - if self.log_path is None: - self.log_path = osp.expanduser("~/.ros/pr2_battery_status.pkl") self.prev_plugged_in = None self.latest_status = None self.speech_history = defaultdict(lambda: rospy.Time(0)) self.diag_sub = rospy.Subscriber( - "/diagnostics_agg", DiagnosticArray, self.diag_cb) + "/diagnostics_agg", DiagnosticArray, self.diag_cb, queue_size=1) self.stat_timer = rospy.Timer( rospy.Duration(self.monitor_rate), self.stat_cb) - self.log_timer = rospy.Timer( - rospy.Duration(self.log_rate), self.log_cb) + if self.log_path is not None: + self.log_timer = rospy.Timer( + rospy.Duration(self.log_rate), self.log_cb) def speak(self, sentence): key = sentence[:4] From 3ea898dffdc09f630d2ab2b13a7f122e0cc3041c Mon Sep 17 00:00:00 2001 From: Furushchev Date: Tue, 12 Feb 2019 15:25:36 +0900 Subject: [PATCH 63/70] add boost components for building --- jsk_robot_common/jsk_robot_startup/CMakeLists.txt | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/jsk_robot_common/jsk_robot_startup/CMakeLists.txt b/jsk_robot_common/jsk_robot_startup/CMakeLists.txt index 2b851269ad..67abe1cd39 100644 --- a/jsk_robot_common/jsk_robot_startup/CMakeLists.txt +++ b/jsk_robot_common/jsk_robot_startup/CMakeLists.txt @@ -30,7 +30,7 @@ find_package(catkin REQUIRED COMPONENTS urdf ) -find_package(Boost REQUIRED) +find_package(Boost REQUIRED COMPONENTS program_options chrono thread system) find_package(OpenCV REQUIRED) catkin_python_setup() From b465ec25a563603cd75054f650a1975924dfe7ee Mon Sep 17 00:00:00 2001 From: Furushchev Date: Tue, 12 Feb 2019 15:25:55 +0900 Subject: [PATCH 64/70] update automated_logger --- .../lifelog/automated_logger.py | 74 +++++++++++++++---- 1 file changed, 58 insertions(+), 16 deletions(-) diff --git a/jsk_robot_common/jsk_robot_startup/lifelog/automated_logger.py b/jsk_robot_common/jsk_robot_startup/lifelog/automated_logger.py index ca614ff482..aa1c51976b 100755 --- a/jsk_robot_common/jsk_robot_startup/lifelog/automated_logger.py +++ b/jsk_robot_common/jsk_robot_startup/lifelog/automated_logger.py @@ -170,12 +170,14 @@ def __init__(self, base_node): self.topics = set() self.srvs = set() - self.blacklist_nodes = set(['^/rosout', '.*lifelog.*']) + self.blacklist_nodes = set(['^/rosout', '.*lifelog.*', '/realtime_loop', '/move_base_node']) self.blacklist_topics = set([ '.*/bond$', # nodelets '^/tf', '^/tf_static', # tfs - '.*/depth/.*', '.*compressed.*', '.*image_color$', # images + '.*/depth/.*', '.*compressed.*', '.*theora.*', '.*image_color$', # images '/lifelog.*$', # lifelog topics + '.*/parameter_updates', '.*/parameter_descriptions', # dynamic reconfigure + '.*/result', '.*/goal', '.*/status$', '.*/cancel$', '.*/feedback$', # actionlib ]) self.bad_nodes = {} self.node_node_conn = EdgeList() @@ -530,6 +532,10 @@ def load(self, name, type, remappings=None, my_argv=None, params=None): self.logerr("Failed to load {}".format(name)) self.logerr(traceback.format_exc()) return False + except rospy.ServiceException: + self.logerr("Failed to load {}".format(name)) + self.logerr(traceback.format_exc()) + return False return res.success @@ -549,6 +555,10 @@ def unload(self, name): self.logerr("Failed to unload {}".format(name)) self.logerr(traceback.format_exc()) return False + except rospy.ServiceException: + self.logerr("Failed to load {}".format(name)) + self.logerr(traceback.format_exc()) + return False return res.success def unload_all(self): @@ -603,7 +613,7 @@ def unwatch(self, topic): class AutomatedLogger(object): - def __init__(self): + def __init__(self, monitor_node, **kwargs): super(AutomatedLogger, self).__init__() self.managers = {} rospy.on_shutdown(self.shutdown_cb) @@ -611,15 +621,14 @@ def __init__(self): default_manager_name = rospy.get_name() + "_manager" self.managers['default'] = LoggerManager.get_manager(default_manager_name) - monitor_node = rospy.get_param("~monitor_node") if not monitor_node.startswith('/'): raise ValueError('~monitor_node param must start with /') self.graph = Graph(monitor_node) - host = rospy.get_param('/mongodb_host', 'localhost') - port = rospy.get_param('/mongodb_port', 27017) - database = rospy.get_param('/robot/database') - collection = rospy.get_param('~collection', 'automated_logger') + host = kwargs.get('mongo_host', 'localhost') + port = kwargs.get('mongo_port', 27017) + database = kwargs.get('mongo_database') + collection = kwargs.get('mongo_collection') client = pymongo.MongoClient(host, port) try: ok = (client[database].command('ping')['ok'] == 1.0) @@ -632,9 +641,13 @@ def __init__(self): self.log_db = client[database][collection] self.log_id = str(uuid.uuid1()) + self.standalone = kwargs.get('standalone', False) + if self.standalone: + rospy.loginfo('Standalone mode') + self.record_log(rospy.Time.now(), 'start') - update_rate = rospy.get_param('~update_rate', 1.0) + update_rate = kwargs.get('update_rate', 1.0) self.update_timer = rospy.Timer( rospy.Duration(update_rate), self.update_cb) @@ -660,7 +673,10 @@ def record_log(self, stamp, event, **data): def shutdown_cb(self): for m in self.managers.values(): m.stop_manager() - self.record_log(rospy.Time.now(), 'shutdown') + try: + self.record_log(rospy.Time.now(), 'shutdown') + except Exception as e: + rospy.logerr(e) def get_manager(self, topic): for m in self.managers.values(): @@ -690,9 +706,7 @@ def update_cb(self, event=None): if not manager: rospy.logerr('No manager found for topic {}'.format(topic)) continue - rospy.logdebug('before cancel: {}'.format(manager.loaded_nodelets)) ok = manager.unwatch(topic) - rospy.logdebug('after cancel: {}'.format(manager.loaded_nodelets)) if ok: canceled.append(topic) else: @@ -711,15 +725,13 @@ def update_cb(self, event=None): entopic(topic), upward=True, downward=False)] pub_set = set(pubs) pub_manager = managers & pub_set - if pub_manager: + if pub_manager and not self.standalone: manager = list(pub_manager)[0] else: manager = 'default' if manager not in self.managers: self.managers[manager] = LoggerManager.get_manager(manager) - rospy.loginfo('before add: {}'.format(self.managers[manager].loaded_nodelets)) ok = self.managers[manager].watch(topic) - rospy.loginfo('after add: {}'.format(self.managers[manager].loaded_nodelets)) if ok: added.append(topic) else: @@ -729,7 +741,37 @@ def update_cb(self, event=None): self.record_log(stamp, 'watch', topics=added) +def parse_args(): + if len(rospy.myargv()) == len(sys.argv): + # launch from command line + import argparse + p = argparse.ArgumentParser() + p.add_argument('monitor_node', help='Name of monitoring node') + p.add_argument('--standalone', action='store_true') + p.add_argument('--update-rate', '-r', help='Update rate') + p.add_argument('--mongo-host', '--host', help='Mongodb hostname') + p.add_argument('--mongo-port', '--port', help='Mongodb port') + p.add_argument('--mongo-database', '--db', help='Mongodb database name') + p.add_argument('--mongo-collection', '--col', help='Mongodb collection name to write log') + args = p.parse_args() + kwargs = {k: v for k, v in args._get_kwargs() if v is not None} + else: + # launch from roslaunch + kwargs = { + 'monitor_node': rospy.get_param('~monitor_node'), + 'update_rate': rospy.get_param('~update_rate', None), + 'mongo_host': rospy.get_param('/mongodb_host', None), + 'mongo_port': rospy.get_param('/mongodb_port', None), + 'mongo_database': rospy.get_param('/robot/database'), + 'mongo_collection': rospy.get_param('~collection', None), + 'standalone': rospy.get_param('~standalone', None), + } + kwargs = {k: v for k, v in kwargs if v is not None} + return kwargs + if __name__ == '__main__': rospy.init_node("automated_logger") - n = AutomatedLogger() + kwargs = parse_args() + rospy.loginfo('Arguments: %s' % kwargs) + n = AutomatedLogger(**kwargs) rospy.spin() From 6bb671e8e1183e538901425065c41c88b56a9b27 Mon Sep 17 00:00:00 2001 From: Shingo Kitagawa Date: Fri, 29 Mar 2019 15:42:55 +0900 Subject: [PATCH 65/70] fix tweet/tweet.l --- jsk_pr2_robot/jsk_pr2_startup/apps/tweet/tweet.l | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/jsk_pr2_robot/jsk_pr2_startup/apps/tweet/tweet.l b/jsk_pr2_robot/jsk_pr2_startup/apps/tweet/tweet.l index 381e44ef9d..0c06dc1652 100644 --- a/jsk_pr2_robot/jsk_pr2_startup/apps/tweet/tweet.l +++ b/jsk_pr2_robot/jsk_pr2_startup/apps/tweet/tweet.l @@ -17,6 +17,6 @@ (speak-jp "カシャ！") (tweet-string "Hello!!" - :warning-time nil + :wait-for-camera 0 :with-image "/kinect_head/rgb/image_rect_color") (exit) From d57267338b8c5af01fa299be95ce352977a7e69a Mon Sep 17 00:00:00 2001 From: Shingo Kitagawa Date: Thu, 18 Apr 2019 04:44:32 +0900 Subject: [PATCH 66/70] fix typo --- jsk_robot_common/jsk_robot_startup/lifelog/tweet_client.l | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/jsk_robot_common/jsk_robot_startup/lifelog/tweet_client.l b/jsk_robot_common/jsk_robot_startup/lifelog/tweet_client.l index 9ee788003b..34e7fc66ad 100755 --- a/jsk_robot_common/jsk_robot_startup/lifelog/tweet_client.l +++ b/jsk_robot_common/jsk_robot_startup/lifelog/tweet_client.l @@ -32,7 +32,7 @@ (ros::service-call "/tweet_image_mux/add" (instance topic_tools::MuxAddRequest :init :topic with-image))) (unless (string= with-image selected-topic) - (setq prev-iamge-topic + (setq prev-image-topic (send (ros::service-call "/tweet_image_mux/select" (instance topic_tools::MuxSelectRequest :init :topic with-image)) :prev_topic))))) From 32344ce9b1e9be24d94f2ee9a558195bcb064905 Mon Sep 17 00:00:00 2001 From: Shingo Kitagawa Date: Sun, 28 Apr 2019 22:01:13 +0900 Subject: [PATCH 67/70] suppress log for cable warning --- jsk_pr2_robot/jsk_pr2_startup/jsk_pr2_warning/cable_warning.l | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/jsk_pr2_robot/jsk_pr2_startup/jsk_pr2_warning/cable_warning.l b/jsk_pr2_robot/jsk_pr2_startup/jsk_pr2_warning/cable_warning.l index 36a9dee43e..03f1fee3b3 100755 --- a/jsk_pr2_robot/jsk_pr2_startup/jsk_pr2_warning/cable_warning.l +++ b/jsk_pr2_robot/jsk_pr2_startup/jsk_pr2_warning/cable_warning.l @@ -40,7 +40,7 @@ (let ((p (send msg :relative_capacity)) (a (send msg :ac_present)) (tm (ros::time-now))) - (ros::ros-info "power-state pc:~A cap:~A" p a) + (ros::ros-debug "power-state pc:~A cap:~A" p a) (setq *ac* (> a 0)) (if (or *ac* (> 180.0 (send (ros::time- tm *ac-tm*) :to-sec))) From 513be2111fbe4fd2a3cd10a17a09d3278adc4601 Mon Sep 17 00:00:00 2001 From: Yuto Uchimi Date: Wed, 22 May 2019 16:53:34 +0900 Subject: [PATCH 68/70] Speak warning message when battery voltage is low Conflicts: jsk_pr2_robot/jsk_pr2_startup/jsk_pr2_warning/battery_warning.py --- .../jsk_pr2_warning/battery_warning.py | 18 ++++++++++++++++++ 1 file changed, 18 insertions(+) diff --git a/jsk_pr2_robot/jsk_pr2_startup/jsk_pr2_warning/battery_warning.py b/jsk_pr2_robot/jsk_pr2_startup/jsk_pr2_warning/battery_warning.py index 291bf97412..02e9e43ef0 100755 --- a/jsk_pr2_robot/jsk_pr2_startup/jsk_pr2_warning/battery_warning.py +++ b/jsk_pr2_robot/jsk_pr2_startup/jsk_pr2_warning/battery_warning.py @@ -2,6 +2,8 @@ # -*- coding: utf-8 -*- # Author: Yuki Furuta +from __future__ import division + import actionlib from collections import defaultdict import os.path as osp @@ -30,6 +32,8 @@ def __init__(self): self.warning_temp = rospy.get_param("~warning_temperature", 42.0) self.min_capacity = rospy.get_param("~min_capacity", 800) self.warn_repeat_rate = rospy.get_param("~warn_repeat_rate", 180) + self.warning_voltage = rospy.get_param("~warning_voltage", 14.0) + self.critical_voltage = rospy.get_param("~critical_voltage", 13.7) self.log_rate = rospy.get_param("~log_rate", 10) self.log_path = rospy.get_param("~log_path", None) @@ -124,6 +128,20 @@ def stat_cb(self, event): except ValueError: pass + try: + voltage = df["Voltage (mV)"].astype(int).min() / 1000 + if (prev_plugged_in and plugged_in) or \ + voltage < self.warning_voltage: + self.speak("電池電圧%.1fボルトです。" % voltage) + if self.critical_voltage < voltage < self.warning_voltage: + self.speak("充電してください。") + elif voltage <= self.critical_voltage: + self.speak("もう限界です！") + except KeyError: + pass + except ValueError: + pass + def diag_cb(self, msg): stamp = msg.header.stamp.secs batt_status = filter( From f4c321f2e9c9c4948cb08a4764267a1634c73d7d Mon Sep 17 00:00:00 2001 From: Shingo Kitagawa Date: Thu, 29 Aug 2019 16:46:56 +0900 Subject: [PATCH 69/70] add robotsound and robotsound_jp in tts_action_names/speech_to_text --- .../jsk_pr2_startup/jsk_pr2_lifelog/pr2_microphone.launch | 3 +++ jsk_pr2_robot/jsk_pr2_startup/pr2_bringup.launch | 6 +++--- 2 files changed, 6 insertions(+), 3 deletions(-) diff --git a/jsk_pr2_robot/jsk_pr2_startup/jsk_pr2_lifelog/pr2_microphone.launch b/jsk_pr2_robot/jsk_pr2_startup/jsk_pr2_lifelog/pr2_microphone.launch index aeee9e1f70..2c5fc9c894 100644 --- a/jsk_pr2_robot/jsk_pr2_startup/jsk_pr2_lifelog/pr2_microphone.launch +++ b/jsk_pr2_robot/jsk_pr2_startup/jsk_pr2_lifelog/pr2_microphone.launch @@ -35,6 +35,9 @@ language: $(arg language) self_cancellation: true tts_tolerance: 0.5 + tts_action_names: + - robotsound + - robotsound_jp diff --git a/jsk_pr2_robot/jsk_pr2_startup/pr2_bringup.launch b/jsk_pr2_robot/jsk_pr2_startup/pr2_bringup.launch index d1379023a7..ed3121b680 100644 --- a/jsk_pr2_robot/jsk_pr2_startup/pr2_bringup.launch +++ b/jsk_pr2_robot/jsk_pr2_startup/pr2_bringup.launch @@ -2,7 +2,7 @@ - + @@ -129,8 +129,8 @@ - - + + From 82ae5b3902b68d35218832225f2a1ba130c9e3de Mon Sep 17 00:00:00 2001 From: Shingo Kitagawa Date: Wed, 2 Oct 2019 16:12:28 +0900 Subject: [PATCH 70/70] log voltage, current and temperature for pr2 battery --- .../jsk_pr2_startup/jsk_pr2_warning/battery_logger.py | 9 ++++++++- 1 file changed, 8 insertions(+), 1 deletion(-) diff --git a/jsk_pr2_robot/jsk_pr2_startup/jsk_pr2_warning/battery_logger.py b/jsk_pr2_robot/jsk_pr2_startup/jsk_pr2_warning/battery_logger.py index cdb79dbb22..d80cb5038a 100755 --- a/jsk_pr2_robot/jsk_pr2_startup/jsk_pr2_warning/battery_logger.py +++ b/jsk_pr2_robot/jsk_pr2_startup/jsk_pr2_warning/battery_logger.py @@ -49,7 +49,8 @@ def write(self, date, info): filename = os.path.join(self.out_dir, datetime.now().strftime("battery_%Y-%m-%d.log")) lines = [] - index = ["HardwareID", "CycleCount", "FullCapacity", "RemainingCapacity"] + index = ["HardwareID", "CycleCount", "FullCapacity", "RemainingCapacity", + "Voltage", "Current", "Temperature"] if not os.path.exists(filename): # write index lines.append(["Date", "Name"] + index) @@ -134,6 +135,12 @@ def diag_callback(self, msg): results[s.name]["CycleCount"] = int(kv.value) elif kv.key.startswith("Manufacture Date"): results[s.name]["ManufactureDate"] = kv.value + elif kv.key.startswith("Voltage (mV)"): + results[s.name]["Voltage"] = int(kv.value) + elif kv.key.startswith("Current (mA)"): + results[s.name]["Current"] = int(kv.value) + elif kv.key.startswith("Temperature (C)"): + results[s.name]["Temperature"] = float(kv.value) # log for logger in self.loggers: