diff --git a/jsk_fetch_robot/jsk_fetch_startup/scripts/update_workspace.sh b/jsk_fetch_robot/jsk_fetch_startup/scripts/update_workspace.sh index f27e401366..6fd0ba00d2 100755 --- a/jsk_fetch_robot/jsk_fetch_startup/scripts/update_workspace.sh +++ b/jsk_fetch_robot/jsk_fetch_startup/scripts/update_workspace.sh @@ -1,43 +1,9 @@ #!/usr/bin/env bash -. $HOME/ros/melodic/devel/setup.bash -# Filename should end with .txt to preview the contents in a web browser -LOGFILE=$HOME/ros/melodic/update_workspace.txt - -{ -set -x -# Update workspace -cd $HOME/ros/melodic/src -ln -sf $(rospack find jsk_fetch_startup)/../jsk_fetch.rosinstall.$ROS_DISTRO $HOME/ros/melodic/src/.rosinstall -wstool foreach --git 'git stash' -wstool update --delete-changed-uris -WSTOOL_UPDATE_RESULT=$? -cd $HOME/ros/melodic -catkin clean aques_talk collada_urdf_jsk_patch -y -catkin init -catkin config -DCMAKE_BUILD_TYPE=Release -catkin build -CATKIN_BUILD_RESULT=$? -# Send mail -MAIL_BODY="" -if [ $WSTOOL_UPDATE_RESULT -ne 0 ]; then - MAIL_BODY=$MAIL_BODY"Please wstool update workspace manually. " -fi -if [ $CATKIN_BUILD_RESULT -ne 0 ]; then - MAIL_BODY=$MAIL_BODY"Please catkin build workspace manually." -fi -set +x -} > $LOGFILE 2>&1 -if [ -n "$MAIL_BODY" ]; then - rostopic pub -1 /email jsk_robot_startup/Email "header: - seq: 0 - stamp: {secs: 0, nsecs: 0} - frame_id: '' -subject: 'Daily workspace update fails' -body: '$MAIL_BODY' -sender_address: '$(hostname)@jsk.imi.i.u-tokyo.ac.jp' -receiver_address: 'fetch@jsk.imi.i.u-tokyo.ac.jp' -smtp_server: '' -smtp_port: '' -attached_files: ['$LOGFILE']" -fi +cp $(rospack find jsk_robot_startup)/scripts/update_workspace_main.sh /tmp/update_workspace.sh +# jsk_footstep_planner is not released for melodic +# jsk_footstep_controller is not released for melodic +# librealsense2 should not be installed from ROS repository +# realsense-ros should not be installed from ROS repository +/tmp/update_workspace.sh -r $(rospack find jsk_fetch_startup)/../jsk_fetch.rosinstall.$ROS_DISTRO -t fetch -s "jsk_footstep_controller jsk_footstep_planner librealsense2 realsense2_camera realsense2_description" +rm /tmp/update_workspace.sh diff --git a/jsk_pr2_robot/jsk_pr2_startup/apps/tweet/tweet.app b/jsk_pr2_robot/jsk_pr2_startup/apps/tweet/tweet.app index ac930d0528..ec8ff6dae9 100644 --- a/jsk_pr2_robot/jsk_pr2_startup/apps/tweet/tweet.app +++ b/jsk_pr2_robot/jsk_pr2_startup/apps/tweet/tweet.app @@ -1,6 +1,7 @@ display: PR2 Tweet description: PR2 likes tweeting platform: pr2 -launch: jsk_pr2_startup/tweet.xml +# launch: jsk_pr2_startup/tweet.xml +run: jsk_pr2_startup/tweet.l interface: jsk_pr2_startup/tweet.interface icon: jsk_pr2_startup/tweet.png diff --git a/jsk_pr2_robot/jsk_pr2_startup/apps/tweet/tweet.l b/jsk_pr2_robot/jsk_pr2_startup/apps/tweet/tweet.l old mode 100644 new mode 100755 diff --git a/jsk_pr2_robot/jsk_pr2_startup/config/jsk_startup.rviz b/jsk_pr2_robot/jsk_pr2_startup/config/jsk_startup.rviz index ec5d660fe5..0c08b99a42 100644 --- a/jsk_pr2_robot/jsk_pr2_startup/config/jsk_startup.rviz +++ b/jsk_pr2_robot/jsk_pr2_startup/config/jsk_startup.rviz @@ -5,6 +5,8 @@ Panels: Property Tree Widget: Expanded: - /Diagnostics1/Power1 + - /Vision1/NarrowStereoLeft1 + - /Planning1/SmachImage1 Splitter Ratio: 0.557554007 Tree Height: 1855 - Class: rviz/Selection @@ -899,9 +901,9 @@ Visualization Manager: Enabled: false Invert Rainbow: false Max Color: 255; 255; 255 - Max Intensity: 1.08762002 + Max Intensity: 4096 Min Color: 0; 0; 0 - Min Intensity: 0.190980002 + Min Intensity: 0 Name: TiltScan Position Transformer: XYZ Queue Size: 10 @@ -976,6 +978,23 @@ Visualization Manager: Value: true Enabled: true Name: Vision + - Class: rviz/Group + Displays: + - Class: jsk_rviz_plugin/OverlayImage + Enabled: true + Name: SmachImage + Topic: /smach_image_publisher/image + Value: true + alpha: 0.850000024 + height: 200 + keep aspect ratio: true + left: 2700 + overwrite alpha value: false + top: 50 + transport hint: compressed + width: 600 + Enabled: true + Name: Planning Enabled: true Global Options: Background Color: 48; 48; 48 @@ -1003,25 +1022,25 @@ Visualization Manager: Views: Current: Class: rviz/Orbit - Distance: 10.3789015 + Distance: 8.56335258 Enable Stereo Rendering: Stereo Eye Separation: 0.0599999987 Stereo Focal Distance: 1 Swap Stereo Eyes: false Value: false Focal Point: - X: 1.27238953 - Y: -0.646221399 - Z: 1.45365202 + X: 1.20282674 + Y: 0.220421076 + Z: 1.34202754 Focal Shape Fixed Size: true Focal Shape Size: 0.0500000007 Invert Z Axis: false Name: Current View Near Clip Distance: 0.00999999978 - Pitch: 0.449796855 + Pitch: 0.429796875 Target Frame: base_link Value: Orbit (rviz) - Yaw: 0.113857321 + Yaw: 0.71885699 Saved: ~ Window Geometry: Displays: diff --git a/jsk_pr2_robot/jsk_pr2_startup/config/pr2_app_schedule.yaml b/jsk_pr2_robot/jsk_pr2_startup/config/pr2_app_schedule.yaml new file mode 100644 index 0000000000..70b87582ec --- /dev/null +++ b/jsk_pr2_robot/jsk_pr2_startup/config/pr2_app_schedule.yaml @@ -0,0 +1,23 @@ +# check use_sim_time +- name: check_use_sim_time + app_name: jsk_robot_startup/check_use_sim_time + app_schedule: + start: every().hour.at(":00") +# volume for monday +- name: volume_zero_monday + app_name: jsk_robot_startup/volume_zero + app_schedule: + start: every().monday.at("12:00") +- name: volume_reset_monday + app_name: jsk_robot_startup/volume_reset + app_schedule: + start: every().monday.at("16:00") +# volume for tuesday +- name: volume_zero_tuesday + app_name: jsk_robot_startup/volume_zero + app_schedule: + start: every().tuesday.at("12:00") +- name: volume_reset_tuesday + app_name: jsk_robot_startup/volume_reset + app_schedule: + start: every().tuesday.at("16:00") diff --git a/jsk_pr2_robot/jsk_pr2_startup/jsk_pr2.rosinstall b/jsk_pr2_robot/jsk_pr2_startup/jsk_pr2.rosinstall.indigo similarity index 95% rename from jsk_pr2_robot/jsk_pr2_startup/jsk_pr2.rosinstall rename to jsk_pr2_robot/jsk_pr2_startup/jsk_pr2.rosinstall.indigo index 3d7fe87744..0675f96ed3 100644 --- a/jsk_pr2_robot/jsk_pr2_startup/jsk_pr2.rosinstall +++ b/jsk_pr2_robot/jsk_pr2_startup/jsk_pr2.rosinstall.indigo @@ -15,7 +15,7 @@ - git: local-name: PR2/pr2_controllers uri: https://github.com/PR2/pr2_controllers.git - version: melodic-devel + version: 1.10.18 # we need to install from source because pr2 is indigo - git: local-name: PR2/pr2_self_test @@ -85,8 +85,8 @@ # we need this for eus10 and roseus_resume - git: local-name: jsk-ros-pkg/jsk_pr2eus - uri: https://github.com/knorth55/jsk_pr2eus.git - version: pr1040 + uri: https://github.com/jsk-ros-pkg/jsk_pr2eus.git + version: master # we need to install from source because pr2 is indigo - git: local-name: jsk-ros-pkg/jsk_recognition @@ -143,6 +143,11 @@ uri: https://github.com/ros-gbp/common_msgs-release/archive/release/kinetic/visualization_msgs/1.12.7-0.tar.gz version: common_msgs-release-release-kinetic-visualization_msgs-1.12.7-0 # we need to install from source because pr2 is indigo +- git: + local-name: ros-visualization/executive_smach_visualization + uri: https://github.com/ros-visualization/executive_smach_visualization.git + version: melodic-devel +# we need to install from source because pr2 is indigo - git: local-name: ros-visualization/interactive_markers uri: https://github.com/ros-visualization/interactive_markers.git diff --git a/jsk_pr2_robot/jsk_pr2_startup/jsk_pr2.rosinstall.noetic b/jsk_pr2_robot/jsk_pr2_startup/jsk_pr2.rosinstall.noetic new file mode 100644 index 0000000000..45be962e8e --- /dev/null +++ b/jsk_pr2_robot/jsk_pr2_startup/jsk_pr2.rosinstall.noetic @@ -0,0 +1,148 @@ +- git: + local-name: Affonso-Gui/euslime + uri: https://github.com/Affonso-Gui/euslime.git + version: eus10 +- git: + local-name: Affonso-Gui/roseus_resume + uri: https://github.com/Affonso-Gui/roseus_resume.git + version: eus10 +- git: + local-name: BehaviorTree/behaviortree_ros + uri: https://github.com/BehaviorTree/BehaviorTree.ROS.git + version: master +- git: + local-name: PR2/app_manager + uri: https://github.com/PR2/app_manager.git + version: kinetic-devel +- git: + local-name: PR2/pr2_apps + uri: https://github.com/pr2/pr2_apps.git + version: melodic-devel +- git: + local-name: PR2/pr2_gripper_sensor + uri: https://github.com/pr2/pr2_gripper_sensor.git + version: hydro-devel +- git: + local-name: PR2/pr2_navigation + uri: https://github.com/PR2/pr2_navigation.git + version: kinetic-devel +- git: + local-name: PR2/pr2_robot + uri: https://github.com/TAMS-Group/pr2_robot.git + version: pr-various +- git: + local-name: PR2/rqt_pr2_dashboard + uri: https://github.com/PR2/rqt_pr2_dashboard.git + version: kinetic-devel +- git: + local-name: SharedAutonomyToolkit/shared_autonomy_manipulation + uri: https://github.com/SharedAutonomyToolkit/shared_autonomy_manipulation.git + version: hydro-devel +- git: + local-name: euslisp/Euslisp + uri: https://github.com/Affonso-Gui/EusLisp.git + version: eus10 +- git: + local-name: euslisp/jskeus + uri: https://github.com/euslisp/jskeus.git + version: master +- git: + local-name: jsk-ros-pkg/coral_usb_ros + uri: https://github.com/jsk-ros-pkg/coral_usb_ros.git + version: master +- git: + local-name: jsk-ros-pkg/geneus + uri: https://github.com/jsk-ros-pkg/geneus.git + version: master +- git: + local-name: jsk-ros-pkg/jsk_3rdparty + uri: https://github.com/jsk-ros-pkg/jsk_3rdparty.git + version: develop/pr2 +- git: + local-name: jsk-ros-pkg/jsk_common + uri: https://github.com/jsk-ros-pkg/jsk_common.git + version: develop/pr2 +- git: + local-name: jsk-ros-pkg/jsk_control + uri: https://github.com/jsk-ros-pkg/jsk_control.git + version: master +- git: + local-name: jsk-ros-pkg/jsk_demos + uri: https://github.com/jsk-ros-pkg/jsk_demos.git + version: master +- git: + local-name: jsk-ros-pkg/jsk_model_tools + uri: https://github.com/jsk-ros-pkg/jsk_model_tools.git + version: master +- git: + local-name: jsk-ros-pkg/jsk_planning + uri: https://github.com/jsk-ros-pkg/jsk_planning.git + version: master +- git: + local-name: jsk-ros-pkg/jsk_pr2eus + uri: https://github.com/jsk-ros-pkg/jsk_pr2eus.git + version: master +- git: + local-name: jsk-ros-pkg/jsk_recognition + uri: https://github.com/jsk-ros-pkg/jsk_recognition.git + version: master +- git: + local-name: jsk-ros-pkg/jsk_robot + uri: https://github.com/jsk-ros-pkg/jsk_robot.git + version: develop/pr2-noetic +- git: + local-name: jsk-ros-pkg/jsk_roseus + uri: https://github.com/knorth55/jsk_roseus.git + version: pr1040 +- git: + local-name: jsk-ros-pkg/jsk_visualization + uri: https://github.com/jsk-ros-pkg/jsk_visualization.git + version: master +- git: + local-name: knorth55/app_manager_utils + uri: https://github.com/knorth55/app_manager_utils.git + version: master +- git: + local-name: ros-drivers/audio_common + uri: https://github.com/ros-drivers/audio_common.git + version: master +- git: + local-name: ros-drivers/hokuyo_node + uri: https://github.com/ros-drivers/hokuyo_node.git + version: indigo-devel +- git: + local-name: ros-drivers/joystick_drivers + uri: https://github.com/knorth55/joystick_drivers.git + version: pr1040-noetic +- git: + local-name: ros-drivers/prosilica_driver + uri: https://github.com/knorth55/prosilica_driver.git + version: plugin-install-fix +- git: + local-name: ros-drivers/wge100_driver + uri: https://github.com/ros-drivers/wge100_driver.git + version: hydro-devel +- git: + local-name: ros-drivers/wifi_ddwrt + uri: https://github.com/TAMS-Group/wifi_ddwrt.git + version: noetic-devel +- git: + local-name: ros-visualization/executive_smach_visualization + uri: https://github.com/knorth55/executive_smach_visualization.git + version: fix-python3 +- git: + local-name: ros/collada_urdf + uri: https://github.com/werner291/collada_urdf.git + version: patch-1 +- git: + local-name: strands-project/mongodb_store + uri: https://github.com/strands-project/mongodb_store.git + version: noetic-devel +- git: + local-name: tork-a/roswww + uri: https://github.com/knorth55/roswww.git + version: fix-python3 +- git: + local-name: tork-a/visualization_rwt + uri: https://github.com/tork-a/visualization_rwt.git + version: kinetic-devel 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 009a11efeb..8a2725fa3e 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 @@ -4,9 +4,7 @@ - - - + @@ -29,7 +27,7 @@ + machine="$(arg machine)" respawn="$(arg respawn)"> 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..dad96d703c 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 @@ -3,6 +3,8 @@ + + diff --git a/jsk_pr2_robot/jsk_pr2_startup/jsk_pr2_move_base/ds3joy_indigo.yaml b/jsk_pr2_robot/jsk_pr2_startup/jsk_pr2_move_base/ds3joy_indigo.yaml new file mode 100644 index 0000000000..ace9ca6ef4 --- /dev/null +++ b/jsk_pr2_robot/jsk_pr2_startup/jsk_pr2_move_base/ds3joy_indigo.yaml @@ -0,0 +1,48 @@ +# Do not remap buttons in indigo +mappings: + axes: + - axes[0] + - axes[1] + - axes[2] + - axes[3] + - axes[4] + - axes[5] + - axes[6] + - axes[7] + - axes[8] + - axes[9] + - axes[10] + - axes[11] + - axes[12] + - axes[13] + - axes[14] + - axes[15] + - axes[16] + - axes[17] + - axes[18] + - axes[19] + - axes[20] + - axes[21] + - axes[22] + - axes[23] + - axes[24] + - axes[25] + - axes[26] + buttons: + - buttons[0] + - buttons[1] + - buttons[2] + - buttons[3] + - buttons[4] + - buttons[5] + - buttons[6] + - buttons[7] + - buttons[8] + - buttons[9] + - buttons[10] + - buttons[11] + - buttons[12] + - buttons[13] + - buttons[14] + - buttons[15] + - buttons[16] diff --git a/jsk_pr2_robot/jsk_pr2_startup/jsk_pr2_move_base/ds3joy_noetic.yaml b/jsk_pr2_robot/jsk_pr2_startup/jsk_pr2_move_base/ds3joy_noetic.yaml new file mode 100644 index 0000000000..2ac6264e99 --- /dev/null +++ b/jsk_pr2_robot/jsk_pr2_startup/jsk_pr2_move_base/ds3joy_noetic.yaml @@ -0,0 +1,49 @@ +# Because we use PS3 joystick with ubuntu 18.04 default connection instead of sixad, +# we need to remap buttons. +mappings: + axes: + - axes[0] + - axes[1] + - axes[3] + - axes[4] + - 0 + - 0 + - 0 + - 0 + - -1.0 * buttons[13] + - -1.0 * buttons[16] + - -1.0 * buttons[14] + - -1.0 * buttons[15] + - 0.5 * ( axes[2] - 1 ) + - 0.5 * ( axes[5] - 1 ) + - -1.0 * buttons[4] + - -1.0 * buttons[5] + - -1.0 * buttons[2] + - -1.0 * buttons[1] + - -1.0 * buttons[0] + - -1.0 * buttons[3] + - 0 + - 0 + - 0 + - 0 + - 0 + - 0 + - 0 + buttons: + - buttons[8] + - buttons[11] + - buttons[12] + - buttons[9] + - buttons[13] + - buttons[16] + - buttons[14] + - buttons[15] + - buttons[6] + - buttons[7] + - buttons[4] + - buttons[5] + - buttons[2] + - buttons[1] + - buttons[0] + - buttons[3] + - buttons[10] 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 e06259fa25..50d1d16fb1 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 @@ -32,7 +32,7 @@ - + diff --git a/jsk_pr2_robot/jsk_pr2_startup/jsk_pr2_sensors/base_hokuyo.launch b/jsk_pr2_robot/jsk_pr2_startup/jsk_pr2_sensors/base_hokuyo.launch new file mode 100644 index 0000000000..e1c00a7a90 --- /dev/null +++ b/jsk_pr2_robot/jsk_pr2_startup/jsk_pr2_sensors/base_hokuyo.launch @@ -0,0 +1,12 @@ + + + + + + + + + + + + diff --git a/jsk_pr2_robot/jsk_pr2_startup/jsk_pr2_sensors/check_hokuyo_scan_node.py b/jsk_pr2_robot/jsk_pr2_startup/jsk_pr2_sensors/check_hokuyo_scan_node.py new file mode 100755 index 0000000000..e401697d6d --- /dev/null +++ b/jsk_pr2_robot/jsk_pr2_startup/jsk_pr2_sensors/check_hokuyo_scan_node.py @@ -0,0 +1,113 @@ +#!/usr/bin/env python +# -*- coding: utf-8 -*- + +import sys,subprocess,traceback +import rospy +import time +import os +from sensor_msgs.msg import Image, CompressedImage, LaserScan +from sound_play.msg import SoundRequest, SoundRequestAction, SoundRequestGoal +import actionlib +import roslib + +class HokuyoScanChecker: + def __init__(self): + rospy.init_node('hokuyo_scan_checker_node', anonymous=True) + self.no_topic_flag = False ## True if topic is currently not coming in. + self.once_topic_flag = False ## True if the topic has been received at least once since the start of node execution. + + self.launch_process = None + self.hokuyo_name = rospy.get_param('~hokuyo_name', 'base') + + # Create an Action client for the sound_play node + self.sound_client = actionlib.SimpleActionClient('/robotsound', SoundRequestAction) + self.sound_client.wait_for_server() + + # Threshold (in seconds) when a topic is not updated for a certain period of time + self.timeout_threshold = 10.0 + + # Time the topic was last updated + self.last_image_time = time.time() + + self.say_something("{} scan check start".format(self.hokuyo_name)) + + # Subscribing topic + self.topic_sub = rospy.Subscriber('/{}_scan'.format(self.hokuyo_name), LaserScan, self.topic_callback) + + def topic_callback(self, msg): + # Callback to be called when topic is updated + self.last_image_time = time.time() + if self.no_topic_flag or self.once_topic_flag==False: + self.no_topic_flag = False + self.once_topic_flag = True + self.say_something("{} scan topic is arrive.".format(self.hokuyo_name)) + + def check_timeout(self): + # Check if the topic has not been updated for a certain period of time + if time.time() - self.last_image_time > self.timeout_threshold: + if self.no_topic_flag: + return + else: + self.no_topic_flag = True + self.say_something("I haven't seen the {} scan topic for {} seconds.".format(self.hokuyo_name, self.timeout_threshold)) + self.restart_hokuyo() + + def say_something(self, text): + # Let the robot talk + rospy.loginfo(text) + + # Create a SoundRequestGoal message + sound_goal = SoundRequestGoal() + sound_goal.sound_request.sound = SoundRequest.SAY + sound_goal.sound_request.command = SoundRequest.PLAY_ONCE + sound_goal.sound_request.volume = 1.0 + sound_goal.sound_request.arg = text + + # Send the SoundRequestGoal to the sound_play node + self.sound_client.send_goal(sound_goal) + + # Wait for the result (you can add timeout if needed) + self.sound_client.wait_for_result() + + def restart_hokuyo(self): + rospy.logerr("Restarting {} hokuyo".format(self.hokuyo_name)) + retcode = -1 + if self.launch_process: ## Force termination if launch process exists + self.launch_process.terminate() + self.launch_process.wait() + try: + # 1. kill hokuyo node + retcode = subprocess.call('rosnode kill /{}_hokuyo_node'.format(self.hokuyo_name), shell=True) + retcode = subprocess.call('pkill -f {}_hokuyo_node'.format(self.hokuyo_name), shell=True) + rospy.loginfo("Killed {} hokuyo node".format(self.hokuyo_name)) + time.sleep(10) + + # 2. reset hokuyo + package_path = roslib.packages.get_pkg_dir('jsk_pr2_startup') + script_path = os.path.join(package_path, 'jsk_pr2_sensors/hokuyo_reset_scripts') + retcode = subprocess.call('{}/upgrade /etc/ros/sensors/{}_hokuyo {}/reset.cmd'.format(script_path, self.hokuyo_name, script_path), shell=True) + self.say_something("Reset {} hokuyo".format(self.hokuyo_name)) + time.sleep(10) + + # 3 Restarting hokuyo node + os.environ['ROS_ENV_LOADER'] = '/home/applications/ros/noetic/devel/env.sh' + self.launch_process = subprocess.Popen(['roslaunch', 'jsk_pr2_startup', '{}_hokuyo.launch'.format(self.hokuyo_name)], env=os.environ) + rospy.loginfo("Restart {} hokuyo node".format(self.hokuyo_name)) + time.sleep(30) + rospy.loginfo("Restarting {} hokuyo node is Done".format(self.hokuyo_name)) + + except Exception as e: + rospy.logerr('[%s] Unable to kill %s hokuyo node, caught exception:\n%s', self.__class__.__name__, self.hokuyo_name, traceback.format_exc()) + + def run(self): + rate = rospy.Rate(1) # Router plate: 1 Hz + while not rospy.is_shutdown(): + self.check_timeout() + rate.sleep() + +if __name__ == '__main__': + try: + topic_checker = HokuyoScanChecker() + topic_checker.run() + except rospy.ROSInterruptException: + pass diff --git a/jsk_pr2_robot/jsk_pr2_startup/jsk_pr2_sensors/check_openni_node.py b/jsk_pr2_robot/jsk_pr2_startup/jsk_pr2_sensors/check_openni_node.py index 547df4c7ad..af0e3b05fa 100755 --- a/jsk_pr2_robot/jsk_pr2_startup/jsk_pr2_sensors/check_openni_node.py +++ b/jsk_pr2_robot/jsk_pr2_startup/jsk_pr2_sensors/check_openni_node.py @@ -63,7 +63,7 @@ def restart_openni_node(self): self.speak("resetting u s b") p = subprocess.Popen("lsusb", shell=True, stdout=subprocess.PIPE, stderr=subprocess.PIPE) stdout, stderr = p.communicate() - lines = stdout.split("\n") + lines = stdout.decode('utf-8').split("\n") ms_line = [l for l in lines if "Microsoft" in l][0] # it should be like Bus 002 Device 013: ID 045e:02ad Microsoft Corp. Xbox NUI Audio bus_id = ms_line.split(' ')[1] @@ -91,7 +91,7 @@ def restart_openni_node(self): def run(self): while not rospy.is_shutdown(): - if self.image_sub == None or self.image_sub.impl == None: + if self.image_sub is None or self.image_sub.impl is None: self.image_sub = rospy.Subscriber("image", Image, self.image_callback, None, 1) rospy.sleep(self.sleep_cycle) diff --git a/jsk_pr2_robot/jsk_pr2_startup/jsk_pr2_sensors/hokuyo_reset_scripts/reset.cmd b/jsk_pr2_robot/jsk_pr2_startup/jsk_pr2_sensors/hokuyo_reset_scripts/reset.cmd new file mode 100644 index 0000000000..b84f61c508 --- /dev/null +++ b/jsk_pr2_robot/jsk_pr2_startup/jsk_pr2_sensors/hokuyo_reset_scripts/reset.cmd @@ -0,0 +1,5 @@ +*** 1 O 4 +$WP +*** 2 O 5 +$WCE +*** 3 E 0 diff --git a/jsk_pr2_robot/jsk_pr2_startup/jsk_pr2_sensors/hokuyo_reset_scripts/upgrade b/jsk_pr2_robot/jsk_pr2_startup/jsk_pr2_sensors/hokuyo_reset_scripts/upgrade new file mode 100755 index 0000000000..edaff4dbdf Binary files /dev/null and b/jsk_pr2_robot/jsk_pr2_startup/jsk_pr2_sensors/hokuyo_reset_scripts/upgrade differ diff --git a/jsk_pr2_robot/jsk_pr2_startup/jsk_pr2_sensors/kinect_head.launch b/jsk_pr2_robot/jsk_pr2_startup/jsk_pr2_sensors/kinect_head.launch index d312d3d69f..c6864d75a0 100644 --- a/jsk_pr2_robot/jsk_pr2_startup/jsk_pr2_sensors/kinect_head.launch +++ b/jsk_pr2_robot/jsk_pr2_startup/jsk_pr2_sensors/kinect_head.launch @@ -234,9 +234,9 @@ + - diff --git a/jsk_pr2_robot/jsk_pr2_startup/jsk_pr2_sensors/pr2_coral.launch b/jsk_pr2_robot/jsk_pr2_startup/jsk_pr2_sensors/pr2_coral.launch new file mode 100644 index 0000000000..1491c87bab --- /dev/null +++ b/jsk_pr2_robot/jsk_pr2_startup/jsk_pr2_sensors/pr2_coral.launch @@ -0,0 +1,46 @@ + + + + + + + + + + + + + + edgetpu_human_node_manager: + nodes: + - name: edgetpu_human_pose_estimator + type: human_pose_estimator + default: edgetpu_human_pose_estimator + edgetpu_human_pose_estimator: + input_topic: $(arg input_image) + image_transport: raw + device_id: 0 + + + + + + + edgetpu_object_node_manager: + nodes: + - name: edgetpu_object_detector + type: object_detector + default: edgetpu_object_detector + edgetpu_object_detector: + input_topic: $(arg input_image) + image_transport: raw + device_id: 1 + model_file: /etc/ros/jsk_data/coral_usb/mobilenet_v2_ssd_73b2_kitchen_finetuning_20200528.tflite + label_file: /etc/ros/jsk_data/coral_usb/73b2_kitchen_labels.txt + + + 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 124bc0029a..c988c61071 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 @@ -11,7 +11,7 @@ class PublishEmptyCloud(ConnectionBasedTransport): def __init__(self): super(PublishEmptyCloud, self).__init__() - frame_id = rospy.get_param('~frame_id', '/base_laser_link') + 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) @@ -75,7 +75,7 @@ def make_empty_cloud(self, frame_id, max_range, rotate_points): else: points.append(pack(' + + + + + + + + + + + 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 ede2eb2e28..a8b118c019 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 @@ -87,7 +87,13 @@ def write(self, date, info): rospy.logerr("{}".format(e)) rospy.logerr("Could not write to dweet.io") return - assert res.ok and json.loads(res.content)["this"] == "succeeded" + try: + contents = json.loads(res.content) + except ValueError as e: + rospy.logerr("{}".format(e)) + rospy.logerr("Request successed, but return value is invalid.") + return + assert res.ok and contents["this"] == "succeeded" class BatteryInfoAggregator(object): 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 a0fedf9293..11e1a390ee 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 @@ -23,6 +23,8 @@ def __init__(self): # speak self.speak_client = actionlib.SimpleActionClient( "/robotsound_jp", SoundRequestAction) + self.sound_client = actionlib.SimpleActionClient( + "/robotsound", SoundRequestAction) ok = self.speak_client.wait_for_server(rospy.Duration(10)) if not ok: rospy.logfatal("Action /robotsound_jp is not advertised") @@ -31,6 +33,7 @@ def __init__(self): # param self.monitor_rate = rospy.get_param("~monitor_rate", 4) self.warning_temp = rospy.get_param("~warning_temperature", 42.0) + self.cable_warning_temp = rospy.get_param("~cable_warning_temperature", 44.0) self.min_capacity = rospy.get_param("~min_capacity", 800) self.warning_voltage = rospy.get_param("~warning_voltage", 14.0) self.critical_voltage = rospy.get_param("~critical_voltage", 13.7) @@ -41,6 +44,7 @@ def __init__(self): self.prev_plugged_in = None self.latest_status = None self.speech_history = defaultdict(lambda: rospy.Time(0)) + self.sound_history = defaultdict(lambda: rospy.Time(0)) self.diag_sub = rospy.Subscriber( "/diagnostics_agg", DiagnosticArray, self.diag_cb, queue_size=1) @@ -94,6 +98,20 @@ def stat_cb(self, event): rospy.logdebug("temperature: %s" % max_temp) if 60 > max_temp > self.warning_temp: self.speak("バッテリ温度%.1f度。暑いです。部屋の温度を下げてください。" % max_temp) + if max_temp > self.cable_warning_temp: + # play sound 2 (kan kan) + key = 2 + if not self.sound_history[key] + rospy.Duration(self.warn_repeat_rate) > rospy.Time.now(): + self.sound_history[key] = rospy.Time.now() + sound_req = SoundRequest() + sound_req.sound = 2 # kan kan + sound_req.command = SoundRequest.PLAY_ONCE # 1: PLAY_ONCE + sound_req.volume = 1.0 # Volume（0.0〜1.0） + sound_req.arg = '' + sound_req.arg2 = '' + self.sound_client.send_goal(SoundRequestGoal(sound_request=sound_req)) + self.sound_client.wait_for_result(timeout=rospy.Duration(10)) + self.speak("危険な温度です。電源ケーブルのコネクタを確認して下さい。") except KeyError: pass except ValueError: diff --git a/jsk_pr2_robot/jsk_pr2_startup/jsk_pr2_warning/diagnostics_ethercat_summarizer.py b/jsk_pr2_robot/jsk_pr2_startup/jsk_pr2_warning/diagnostics_ethercat_summarizer.py new file mode 100755 index 0000000000..3e8a6bf958 --- /dev/null +++ b/jsk_pr2_robot/jsk_pr2_startup/jsk_pr2_warning/diagnostics_ethercat_summarizer.py @@ -0,0 +1,34 @@ +#!/usr/bin/env python + +import rospy + +from diagnostic_msgs.msg import DiagnosticArray + + +def callback(data): + flag = False + for e in data.status: + #EtherCAT Device + for v in e.values: + if v.key =='Drops': + flag = True + print(e.name + "Drops: {}".format(v.value)) + #EtherCAT Master + if e.name == 'EtherCAT Master': + for v in e.values: + if v.key =='Dropped Packets': + print(e.name + " Dropped Packets: {}".format(v.value)) + if v.key =='RX Late Packet': + print(e.name + " RX Late Packet: {}".format(v.value)) + + if flag: + print("--------------------------------------------") + + +def listener(): + rospy.init_node('diagnostics_ethercat_summarizer', anonymous=True) + rospy.Subscriber('/diagnostics', DiagnosticArray, callback) + rospy.spin() + +if __name__ == '__main__': + listener() diff --git a/jsk_pr2_robot/jsk_pr2_startup/jsk_pr2_warning/diagnostics_rx_error_summarizer.py b/jsk_pr2_robot/jsk_pr2_startup/jsk_pr2_warning/diagnostics_rx_error_summarizer.py new file mode 100755 index 0000000000..bf3da104ba --- /dev/null +++ b/jsk_pr2_robot/jsk_pr2_startup/jsk_pr2_warning/diagnostics_rx_error_summarizer.py @@ -0,0 +1,87 @@ +#!/usr/bin/env python + +import csv +import rospy +import argparse +from diagnostic_msgs.msg import DiagnosticArray + + +class DiagnosticsRxErrorSummarizer(): + def __init__(self, is_csv=False): + self.subscribe() + self.keys = ['timestamp'] + self.count = 0 + self.output_file = 'diagnostic_agg_rx_error.csv' + self.is_csv = is_csv + + def subscribe(self): + self.sub = rospy.Subscriber("/diagnostics_agg", + DiagnosticArray, + self.callback, + queue_size=1) + + def write_keys(self): + with open(self.output_file, 'w') as f: + writer = csv.writer(f) + writer.writerow(self.keys) + + def write_values(self): + with open(self.output_file, 'a') as f: + writer = csv.writer(f) + writer.writerow(self.values) + + def callback(self, msg): + status = msg.status + self.values = [msg.header.stamp.secs] + if self.is_csv: + print(self.count) + for s in status: + if 'values' not in dir(s): + continue + for v in s.values: + if 'RX Error' in v.key: + if self.count == 0: + self.keys.append(s.name + ' - ' + v.key) + self.values.append(v.value) + + if self.count == 0: + self.write_keys() + self.write_values() + + self.count += 1 + else: + flag = False + thre_val = 0 + s_name_list = [] + key_list = [] + value_list = [] + for s in status: + if 'values' not in dir(s): + continue + for v in s.values: + if 'RX Error' in v.key and not ('Forwarded' in v.key): + print(s.name + " " + v.key + ": {}".format(v.value)) + if not ('Master' in s.name): + flag = True + if int(v.value) > thre_val: + s_name_list.append(s.name) + value_list.append(v.value) + key_list.append(v.key) + + if flag: + if len(s_name_list) > 0: + print("[Eroor Exist Dvice]") + for s_name, key, value in zip(s_name_list, key_list, value_list): + print(s_name + " " + key + ": {}".format(value)) + print("--------------------------------------------") + + +if __name__ == '__main__': + parser = argparse.ArgumentParser() + parser.add_argument('--csv', action='store_true', help='save data in csv or not') + args = parser.parse_args() + is_csv= args.csv + + rospy.init_node('diagnostics_rx_errror_summarizer', anonymous=True) + diagnostics_rx_errror_summarizer = DiagnosticsRxErrorSummarizer(is_csv=is_csv) + rospy.spin() 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 index b85284faef..62bb80ddb0 100644 --- 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 @@ -6,13 +6,13 @@ analyzers: joystick: type: diagnostic_aggregator/GenericAnalyzer path: Joystick - expected: 'joy: Joystick Driver Status' + expected: 'joy_node: Joystick Driver Status' num_items: 1 - remove_prefix: 'joy' + remove_prefix: 'joy_node' sound: type: diagnostic_aggregator/GenericAnalyzer path: Sound - num_items: 1 + num_items: 2 contains: 'sound_play' imu: type: diagnostic_aggregator/GenericAnalyzer @@ -96,6 +96,13 @@ analyzers: 'c2 CPU Temperature', 'c2 CPU Usage', 'c2 HD Temperature'] + clocks: + type: diagnostic_aggregator/GenericAnalyzer + path: Clocks + timeout: 20.0 + expected: ['NTP offset from c2 to c1', + 'NTP self-offset for c1', + 'NTP self-offset for c2'] motors: type: diagnostic_aggregator/GenericAnalyzer path: Motors diff --git a/jsk_pr2_robot/jsk_pr2_startup/jsk_pr2_warning/pr2_reset_motors.py b/jsk_pr2_robot/jsk_pr2_startup/jsk_pr2_warning/pr2_reset_motors.py index d915b27213..74b92cca87 100755 --- a/jsk_pr2_robot/jsk_pr2_startup/jsk_pr2_warning/pr2_reset_motors.py +++ b/jsk_pr2_robot/jsk_pr2_startup/jsk_pr2_warning/pr2_reset_motors.py @@ -47,9 +47,9 @@ def motors_cb(self, msg): if (self.enable_reset_motors and self.run_stop and self.halted): rospy.logwarn("motor halted, but run stop is true") stamp = rospy.Time.now() - history = filter( + history = list(filter( lambda s: (stamp-s).to_sec() < self.watch_duration, - self.history) + self.history)) if len(history) > self.max_retry_num: rospy.logerr("Maximum retry count reached. Give up resetting motors") rospy.logerr("Waiting for {} seconds for other retries".format(self.watch_duration)) diff --git a/jsk_pr2_robot/jsk_pr2_startup/jsk_pr2_warning/sanity_targets.yaml b/jsk_pr2_robot/jsk_pr2_startup/jsk_pr2_warning/sanity_targets.yaml new file mode 100644 index 0000000000..2b298ea54b --- /dev/null +++ b/jsk_pr2_robot/jsk_pr2_startup/jsk_pr2_warning/sanity_targets.yaml @@ -0,0 +1,6 @@ +topics: + - /kinect_head/rgb/image_raw/compressed + - /kinect_head/depth_registered/image_raw/compressedDepth + - /edgetpu_human_pose_estimator/output/image/compressed + - /edgetpu_object_detector/output/image/compressed + - /audio diff --git a/jsk_pr2_robot/jsk_pr2_startup/jsk_pr2_warning/warning_blacklist.yaml b/jsk_pr2_robot/jsk_pr2_startup/jsk_pr2_warning/warning_blacklist.yaml new file mode 100644 index 0000000000..4959e68d21 --- /dev/null +++ b/jsk_pr2_robot/jsk_pr2_startup/jsk_pr2_warning/warning_blacklist.yaml @@ -0,0 +1,24 @@ +blacklist: + - name: "/Cameras/Forearm.*" + - name: "/Cameras/Narrow.*" + - name: "/Cameras/Prosilica" + - name: "/Cameras/Wide.*" + - name: "/Computers/CPU/c1 CPU Usage" + - name: "/Computers/CPU/c2 CPU Usage" + - name: "/Devices/Joystick/Joystick Driver Status" + - name: "/Devices/Sound" + - name: "/Joints/Joint.*" + message: "Uncalibrated" + - name: "/Other/Accelerometer.*" + - name: "/Other/NTP offset from" + - name: "/Other/Pressure sensors.*" + - name: "/Other/lifelog joint_states_logger: LightweightLogger:: lifelog joint_states_throttle output" + - name: "/Other/prosilica prosilica_nodelet_manager: prosilica prosilica_driver" + - name: "/Power System/.*" + - name: "/Motors/EtherCAT.*" + message: "Safety Lockout.*" + - name: "/Realtime Controllers/Controller.*" + - name: "/Realtime Controllers/Calibration stuck" + - name: "/Computers/CPU/c\\d CPU Temperature" +run_stop_blacklist: + - name: "/Motors.*" diff --git a/jsk_pr2_robot/jsk_pr2_startup/pr2.launch b/jsk_pr2_robot/jsk_pr2_startup/pr2.launch index 0d39447df2..b2662c401a 100644 --- a/jsk_pr2_robot/jsk_pr2_startup/pr2.launch +++ b/jsk_pr2_robot/jsk_pr2_startup/pr2.launch @@ -32,6 +32,12 @@ doc="whether launch gdrive_server on c1 or not."/> + + + + file="$(find jsk_pr2_startup)/jsk_pr2_sensors/kinect_head.launch" > + + + @@ -197,4 +206,93 @@ + + + + run_stop_topic: /power_board/state + run_stop_condition: "m.run_stop is False" + seconds_to_start_speaking: 60 + speak_interval: 600 + ignore_time_after_runstop_is_enabled: 5.0 + ignore_time_after_runstop_is_disabled: 5.0 + + + + + + + + + + + + + duration: 60 + + + + + + + email_info: /var/lib/robot/email_topic.yaml + + + + + + + + + use_mail: true + use_twitter: $(arg launch_twitter) + use_google_chat: $(arg launch_google_chat) + google_chat_space: spaces/AAAAT_OCXCw + + + + + + + duration: 0.5 + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/jsk_pr2_robot/jsk_pr2_startup/pr2_bringup.launch b/jsk_pr2_robot/jsk_pr2_startup/pr2_bringup.launch index 7486bc07d0..f4d0a9cf08 100644 --- a/jsk_pr2_robot/jsk_pr2_startup/pr2_bringup.launch +++ b/jsk_pr2_robot/jsk_pr2_startup/pr2_bringup.launch @@ -40,12 +40,19 @@ - + + + + + + + + @@ -70,24 +77,14 @@ - - - - - - - + + + - - - - - - - - + @@ -105,7 +102,8 @@ - + + - + @@ -175,7 +173,7 @@ - + @@ -237,7 +235,7 @@ - + diff --git a/jsk_pr2_robot/jsk_pr2_startup/pr2_gazebo.launch b/jsk_pr2_robot/jsk_pr2_startup/pr2_gazebo.launch index 5d576705c4..25230efe12 100644 --- a/jsk_pr2_robot/jsk_pr2_startup/pr2_gazebo.launch +++ b/jsk_pr2_robot/jsk_pr2_startup/pr2_gazebo.launch @@ -142,7 +142,7 @@ - + @@ -154,6 +154,7 @@ + diff --git a/jsk_pr2_robot/jsk_pr2_startup/sample/pr2_gazebo_empty_world.launch b/jsk_pr2_robot/jsk_pr2_startup/sample/pr2_gazebo_empty_world.launch new file mode 100644 index 0000000000..c44fc2f3f7 --- /dev/null +++ b/jsk_pr2_robot/jsk_pr2_startup/sample/pr2_gazebo_empty_world.launch @@ -0,0 +1,22 @@ + + + + + + + + + + + + + + + transform_frame: /map + initial_pose_x: $(arg INITIAL_POSE_X) + initial_pose_y: $(arg INITIAL_POSE_Y) + initial_pose_yaw: $(arg INITIAL_POSE_YAW) + + + diff --git a/jsk_pr2_robot/jsk_pr2_startup/scripts/update_workspace.sh b/jsk_pr2_robot/jsk_pr2_startup/scripts/update_workspace.sh new file mode 100755 index 0000000000..2af7fdebef --- /dev/null +++ b/jsk_pr2_robot/jsk_pr2_startup/scripts/update_workspace.sh @@ -0,0 +1,5 @@ +#!/usr/bin/env bash + +cp $(rospack find jsk_robot_startup)/scripts/update_workspace_main.sh /tmp/update_workspace.sh +/tmp/update_workspace.sh -w $HOME/ros/$ROS_DISTRO -r $(rospack find jsk_pr2_startup)/jsk_pr2.rosinstall.$ROS_DISTRO -t pr2 -u +rm /tmp/update_workspace.sh diff --git a/jsk_pr2_robot/jsk_pr2_startup/systemd_scripts/jsk_pr2_startup.bash b/jsk_pr2_robot/jsk_pr2_startup/systemd_scripts/jsk_pr2_startup.bash new file mode 100755 index 0000000000..b54dbc1d75 --- /dev/null +++ b/jsk_pr2_robot/jsk_pr2_startup/systemd_scripts/jsk_pr2_startup.bash @@ -0,0 +1,11 @@ +#!/usr/bin/env bash + +set -x +. /tmp/ros_run_id +. /var/lib/robot/config.bash +echo "ROS_IP: $ROS_IP"; +echo "ROS_MASTER_URI: $ROS_MASTER_URI"; +echo "ROS_RUN_ID: $ROS_RUN_ID"; +echo "ROS_ENV_LOADER: $ROS_ENV_LOADER"; +$ROS_ENV_LOADER roslaunch jsk_pr2_startup pr2.launch --screen --wait map_frame:=$MAP_FRAME launch_coral:=true +set +x diff --git a/jsk_pr2_robot/jsk_pr2_startup/systemd_scripts/jsk_pr2_startup_prestart.bash b/jsk_pr2_robot/jsk_pr2_startup/systemd_scripts/jsk_pr2_startup_prestart.bash new file mode 100755 index 0000000000..54114d0697 --- /dev/null +++ b/jsk_pr2_robot/jsk_pr2_startup/systemd_scripts/jsk_pr2_startup_prestart.bash @@ -0,0 +1,6 @@ +#!/usr/bin/env bash + +set -x +exec >"/tmp/ros_run_id" +echo "ROS_IP=$(ip -o route get 8.8.8.8 | awk '{print $7;}')" +set +x diff --git a/jsk_pr2_robot/jsk_pr2_startup/systemd_scripts/robot.bash b/jsk_pr2_robot/jsk_pr2_startup/systemd_scripts/robot.bash new file mode 100755 index 0000000000..1751332b90 --- /dev/null +++ b/jsk_pr2_robot/jsk_pr2_startup/systemd_scripts/robot.bash @@ -0,0 +1,14 @@ +#!/usr/bin/env bash + +set -x +. /tmp/ros_run_id +. /var/lib/robot/config.bash +echo "ROS_IP: $ROS_IP"; +echo "ROS_MASTER_URI: $ROS_MASTER_URI"; +echo "ROS_RUN_ID: $ROS_RUN_ID"; +echo "ROS_ENV_LOADER: $ROS_ENV_LOADER"; +robot claim -f; +robot stop -f; +sleep 3; +$ROS_ENV_LOADER roslaunch /etc/ros/robot.launch c2:=false --pid /var/tmp/ros.id --run_id=$ROS_RUN_ID +set +x diff --git a/jsk_pr2_robot/jsk_pr2_startup/systemd_scripts/robot_poststart.bash b/jsk_pr2_robot/jsk_pr2_startup/systemd_scripts/robot_poststart.bash new file mode 100755 index 0000000000..a20158f336 --- /dev/null +++ b/jsk_pr2_robot/jsk_pr2_startup/systemd_scripts/robot_poststart.bash @@ -0,0 +1,40 @@ +#!/usr/bin/env bash + +echo "waiting for roscore to come up" +. /opt/ros/noetic/setup.sh +ret=`rosnode list` +for i in `seq 0 60` +do + if [ "$ret" != '' ] + then + break + fi + ret=`rosnode list` + sleep 1 + echo "Waiting for roscore $i/60" +done + +if [ "$ret" = '' ] +then + echo "Roscore is down" +else + sleep 10 # omajinai + ret=`timeout 30.0 rostopic echo /calibrated/data -n1 | grep True` + while true + do + if [ "$ret" = "True" ] + then + echo "Robot is successfully started." + # initctl emit robot-is-up + break + fi + ret=`timeout 30.0 rostopic echo /calibrated/data -n1 | grep True` + sleep 1 + echo "Waiting for topic /calibrated is advertised" + ret=`rosnode list` + if [ "$ret" != '' ] + then + break + fi + done +fi diff --git a/jsk_pr2_robot/jsk_pr2_startup/systemd_scripts/robot_prestart.bash b/jsk_pr2_robot/jsk_pr2_startup/systemd_scripts/robot_prestart.bash new file mode 100755 index 0000000000..a88e3aefe7 --- /dev/null +++ b/jsk_pr2_robot/jsk_pr2_startup/systemd_scripts/robot_prestart.bash @@ -0,0 +1,7 @@ +#!/usr/bin/env bash + +set -x +exec >"/tmp/ros_run_id" +echo "export ROS_IP=$(ip -o route get 8.8.8.8 | awk '{print $7;}')" +echo "export ROS_RUN_ID=$(date +%Y%m%d-%H%M%S)_$(uuidgen -t)" +set +x diff --git a/jsk_pr2_robot/jsk_pr2_startup/tmuxinator_yaml/log.yml b/jsk_pr2_robot/jsk_pr2_startup/tmuxinator_yaml/log.yml new file mode 100644 index 0000000000..9480e2cef2 --- /dev/null +++ b/jsk_pr2_robot/jsk_pr2_startup/tmuxinator_yaml/log.yml @@ -0,0 +1,36 @@ +# ~/.tmuxinator/log.yml + +name: log +root: ~/ + +# Optional tmux socket +# socket_name: foo + +# Runs before everything. Use it to start daemons etc. +# pre: sudo /etc/rc.d/mysqld start + +# Runs in each window and pane before window/pane specific commands. Useful for setting up interpreter versions. +# pre_window: rbenv shell 2.0.0-p247 + +# Pass command line options to tmux. Useful for specifying a different tmux.conf. +# tmux_options: -f ~/.tmux.mac.conf + +# Change the command to call tmux. This can be used by derivatives/wrappers like byobu. +# tmux_command: byobu + +# Specifies (by name or index) which window will be selected on project startup. If not set, the first window is used. +# startup_window: editor + +# Specitifes (by index) which pane of the specified window will be selected on project startup. If not set, the first pane is used. +# startup_pane: 1 + +# Controls whether the tmux session should be attached to automatically. Defaults to true. +# attach: false + +# Runs after everything. Use it to attach to tmux with custom options etc. +# post: tmux -CC attach -t log + +windows: + - check-c2-bootup: sudo journalctl -f -u check-c2-bootup + - robot: sudo tail -f /var/log/ros/robot.log + - jsk-pr2-startup: sudo tail -f /var/log/ros/jsk-pr2-startup.log diff --git a/jsk_robot_common/jsk_robot_startup/apps/check_use_sim_time/check_use_sim_time.app b/jsk_robot_common/jsk_robot_startup/apps/check_use_sim_time/check_use_sim_time.app index aab3f89f5a..5870ad309a 100644 --- a/jsk_robot_common/jsk_robot_startup/apps/check_use_sim_time/check_use_sim_time.app +++ b/jsk_robot_common/jsk_robot_startup/apps/check_use_sim_time/check_use_sim_time.app @@ -1,5 +1,6 @@ display: Check /use_sim_time param platform: all -launch: jsk_robot_startup/check_use_sim_time.xml +run: jsk_robot_startup/check_use_sim_time.py +# launch: jsk_robot_startup/check_use_sim_time.xml interface: jsk_robot_startup/check_use_sim_time.interface icon: jsk_robot_startup/check_use_sim_time.png diff --git a/jsk_robot_common/jsk_robot_startup/apps/personal_use/personal_use.app b/jsk_robot_common/jsk_robot_startup/apps/personal_use/personal_use.app index e5eea4894f..1a64fabc2d 100644 --- a/jsk_robot_common/jsk_robot_startup/apps/personal_use/personal_use.app +++ b/jsk_robot_common/jsk_robot_startup/apps/personal_use/personal_use.app @@ -1,5 +1,7 @@ display: Personal Use platform: all -launch: jsk_robot_startup/personal_use.xml +run: jsk_robot_startup/personal_use.py +# launch: jsk_robot_startup/personal_use.xml interface: jsk_robot_startup/personal_use.interface icon: jsk_robot_startup/personal_use.png +allow_parallel: false diff --git a/jsk_robot_common/jsk_robot_startup/apps/robot_apps.installed b/jsk_robot_common/jsk_robot_startup/apps/robot_apps.installed index e142d9282c..1350da5b19 100644 --- a/jsk_robot_common/jsk_robot_startup/apps/robot_apps.installed +++ b/jsk_robot_common/jsk_robot_startup/apps/robot_apps.installed @@ -1,5 +1,15 @@ apps: - app: jsk_robot_startup/personal_use - display: Personal use + display: Personal use - app: jsk_robot_startup/check_use_sim_time display: Check /use_sim_time param + - app: jsk_robot_startup/roseus_resume_interrupt + display: Interrupt + - app: jsk_robot_startup/roseus_resume_resume + display: Resume + - app: jsk_robot_startup/volume_lower + display: Volume lower + - app: jsk_robot_startup/volume_reset + display: Volume reset + - app: jsk_robot_startup/volume_zero + display: Volume zero diff --git a/jsk_robot_common/jsk_robot_startup/apps/roseus_resume_interrupt/roseus_resume_interrupt.app b/jsk_robot_common/jsk_robot_startup/apps/roseus_resume_interrupt/roseus_resume_interrupt.app new file mode 100644 index 0000000000..d6ea2dad0f --- /dev/null +++ b/jsk_robot_common/jsk_robot_startup/apps/roseus_resume_interrupt/roseus_resume_interrupt.app @@ -0,0 +1,6 @@ +display: Interrupt +platform: all +run: rostopic/rostopic +run_args: "pub -1 /roseus_resume/interrupt std_msgs/Empty" +interface: jsk_robot_startup/roseus_resume_interrupt.interface +icon: jsk_robot_startup/roseus_resume_interrupt.png diff --git a/jsk_robot_common/jsk_robot_startup/apps/roseus_resume_interrupt/roseus_resume_interrupt.interface b/jsk_robot_common/jsk_robot_startup/apps/roseus_resume_interrupt/roseus_resume_interrupt.interface new file mode 100644 index 0000000000..044105d644 --- /dev/null +++ b/jsk_robot_common/jsk_robot_startup/apps/roseus_resume_interrupt/roseus_resume_interrupt.interface @@ -0,0 +1,2 @@ +published_topics: {} +subscribed_topics: {} diff --git a/jsk_robot_common/jsk_robot_startup/apps/roseus_resume_interrupt/roseus_resume_interrupt.png b/jsk_robot_common/jsk_robot_startup/apps/roseus_resume_interrupt/roseus_resume_interrupt.png new file mode 100644 index 0000000000..352d08ee47 Binary files /dev/null and b/jsk_robot_common/jsk_robot_startup/apps/roseus_resume_interrupt/roseus_resume_interrupt.png differ diff --git a/jsk_robot_common/jsk_robot_startup/apps/roseus_resume_resume/roseus_resume_resume.app b/jsk_robot_common/jsk_robot_startup/apps/roseus_resume_resume/roseus_resume_resume.app new file mode 100644 index 0000000000..c21b0d3a49 --- /dev/null +++ b/jsk_robot_common/jsk_robot_startup/apps/roseus_resume_resume/roseus_resume_resume.app @@ -0,0 +1,6 @@ +display: Resume +platform: all +run: rostopic/rostopic +run_args: "pub -1 /roseus_resume/resume std_msgs/Empty" +interface: jsk_robot_startup/roseus_resume_resume.interface +icon: jsk_robot_startup/roseus_resume_resume.png diff --git a/jsk_robot_common/jsk_robot_startup/apps/roseus_resume_resume/roseus_resume_resume.interface b/jsk_robot_common/jsk_robot_startup/apps/roseus_resume_resume/roseus_resume_resume.interface new file mode 100644 index 0000000000..044105d644 --- /dev/null +++ b/jsk_robot_common/jsk_robot_startup/apps/roseus_resume_resume/roseus_resume_resume.interface @@ -0,0 +1,2 @@ +published_topics: {} +subscribed_topics: {} diff --git a/jsk_robot_common/jsk_robot_startup/apps/roseus_resume_resume/roseus_resume_resume.png b/jsk_robot_common/jsk_robot_startup/apps/roseus_resume_resume/roseus_resume_resume.png new file mode 100644 index 0000000000..235869778e Binary files /dev/null and b/jsk_robot_common/jsk_robot_startup/apps/roseus_resume_resume/roseus_resume_resume.png differ diff --git a/jsk_robot_common/jsk_robot_startup/apps/volume_lower/volume_lower.app b/jsk_robot_common/jsk_robot_startup/apps/volume_lower/volume_lower.app new file mode 100644 index 0000000000..e9648c034f --- /dev/null +++ b/jsk_robot_common/jsk_robot_startup/apps/volume_lower/volume_lower.app @@ -0,0 +1,6 @@ +display: Volume Lower +description: robot speaks lower volume +platform: all +run: jsk_robot_startup/auto_speak_volume_lower.l +interface: jsk_robot_startup/volume_lower.interface +icon: jsk_robot_startup/volume_lower.png \ No newline at end of file diff --git a/jsk_robot_common/jsk_robot_startup/apps/volume_lower/volume_lower.interface b/jsk_robot_common/jsk_robot_startup/apps/volume_lower/volume_lower.interface new file mode 100644 index 0000000000..5cb4ae9816 --- /dev/null +++ b/jsk_robot_common/jsk_robot_startup/apps/volume_lower/volume_lower.interface @@ -0,0 +1,2 @@ +published_topics: {} +subscribed_topics: {} \ No newline at end of file diff --git a/jsk_robot_common/jsk_robot_startup/apps/volume_lower/volume_lower.png b/jsk_robot_common/jsk_robot_startup/apps/volume_lower/volume_lower.png new file mode 100644 index 0000000000..d79825e859 Binary files /dev/null and b/jsk_robot_common/jsk_robot_startup/apps/volume_lower/volume_lower.png differ diff --git a/jsk_robot_common/jsk_robot_startup/apps/volume_reset/volume_reset.app b/jsk_robot_common/jsk_robot_startup/apps/volume_reset/volume_reset.app new file mode 100644 index 0000000000..9b71b6cfb6 --- /dev/null +++ b/jsk_robot_common/jsk_robot_startup/apps/volume_reset/volume_reset.app @@ -0,0 +1,6 @@ +display: Volume Reset +description: robot speaks reset volume +platform: all +run: jsk_robot_startup/auto_speak_volume_reset.l +interface: jsk_robot_startup/volume_reset.interface +icon: jsk_robot_startup/volume_reset.png \ No newline at end of file diff --git a/jsk_robot_common/jsk_robot_startup/apps/volume_reset/volume_reset.interface b/jsk_robot_common/jsk_robot_startup/apps/volume_reset/volume_reset.interface new file mode 100644 index 0000000000..5cb4ae9816 --- /dev/null +++ b/jsk_robot_common/jsk_robot_startup/apps/volume_reset/volume_reset.interface @@ -0,0 +1,2 @@ +published_topics: {} +subscribed_topics: {} \ No newline at end of file diff --git a/jsk_robot_common/jsk_robot_startup/apps/volume_reset/volume_reset.png b/jsk_robot_common/jsk_robot_startup/apps/volume_reset/volume_reset.png new file mode 100644 index 0000000000..d9dbfbc14f Binary files /dev/null and b/jsk_robot_common/jsk_robot_startup/apps/volume_reset/volume_reset.png differ diff --git a/jsk_robot_common/jsk_robot_startup/apps/volume_zero/volume_zero.app b/jsk_robot_common/jsk_robot_startup/apps/volume_zero/volume_zero.app new file mode 100644 index 0000000000..99cc7aecb2 --- /dev/null +++ b/jsk_robot_common/jsk_robot_startup/apps/volume_zero/volume_zero.app @@ -0,0 +1,6 @@ +display: Volume Zero +description: robot speaks zero volume +platform: all +run: jsk_robot_startup/auto_speak_volume_zero.l +interface: jsk_robot_startup/volume_zero.interface +icon: jsk_robot_startup/volume_zero.png \ No newline at end of file diff --git a/jsk_robot_common/jsk_robot_startup/apps/volume_zero/volume_zero.interface b/jsk_robot_common/jsk_robot_startup/apps/volume_zero/volume_zero.interface new file mode 100644 index 0000000000..5cb4ae9816 --- /dev/null +++ b/jsk_robot_common/jsk_robot_startup/apps/volume_zero/volume_zero.interface @@ -0,0 +1,2 @@ +published_topics: {} +subscribed_topics: {} \ No newline at end of file diff --git a/jsk_robot_common/jsk_robot_startup/apps/volume_zero/volume_zero.png b/jsk_robot_common/jsk_robot_startup/apps/volume_zero/volume_zero.png new file mode 100644 index 0000000000..cb736a528e Binary files /dev/null and b/jsk_robot_common/jsk_robot_startup/apps/volume_zero/volume_zero.png differ diff --git a/jsk_robot_common/jsk_robot_startup/lifelog/auto_speak_volume_lower.l b/jsk_robot_common/jsk_robot_startup/lifelog/auto_speak_volume_lower.l new file mode 100755 index 0000000000..48c236a58c --- /dev/null +++ b/jsk_robot_common/jsk_robot_startup/lifelog/auto_speak_volume_lower.l @@ -0,0 +1,14 @@ +#!/usr/bin/env roseus + +(ros::roseus "speak_volume_lower") +(ros::set-dynamic-reconfigure-param "/audible_warning" "enable" :bool t) +(ros::set-dynamic-reconfigure-param "/tweet_client_tablet" "speak_enable" :bool t) +(ros::set-dynamic-reconfigure-param "/tweet_client_uptime" "speak_enable" :bool t) +(ros::set-dynamic-reconfigure-param "/tweet_client_warning" "speak_enable" :bool t) +(ros::set-dynamic-reconfigure-param "/tweet_client_worktime" "speak_enable" :bool t) +(ros::set-dynamic-reconfigure-param "/audible_warning" "volume" :double 0.2) +(ros::set-dynamic-reconfigure-param "/tweet_client_tablet" "volume" :double 0.2) +(ros::set-dynamic-reconfigure-param "/tweet_client_uptime" "volume" :double 0.2) +(ros::set-dynamic-reconfigure-param "/tweet_client_warning" "volume" :double 0.2) +(ros::set-dynamic-reconfigure-param "/tweet_client_worktime" "volume" :double 0.2) +(exit) diff --git a/jsk_robot_common/jsk_robot_startup/lifelog/auto_speak_volume_reset.l b/jsk_robot_common/jsk_robot_startup/lifelog/auto_speak_volume_reset.l new file mode 100755 index 0000000000..a57c51c262 --- /dev/null +++ b/jsk_robot_common/jsk_robot_startup/lifelog/auto_speak_volume_reset.l @@ -0,0 +1,14 @@ +#!/usr/bin/env roseus + +(ros::roseus "speak_volume_reset") +(ros::set-dynamic-reconfigure-param "/audible_warning" "enable" :bool t) +(ros::set-dynamic-reconfigure-param "/tweet_client_tablet" "speak_enable" :bool t) +(ros::set-dynamic-reconfigure-param "/tweet_client_uptime" "speak_enable" :bool t) +(ros::set-dynamic-reconfigure-param "/tweet_client_warning" "speak_enable" :bool t) +(ros::set-dynamic-reconfigure-param "/tweet_client_worktime" "speak_enable" :bool t) +(ros::set-dynamic-reconfigure-param "/audible_warning" "volume" :double 1.0) +(ros::set-dynamic-reconfigure-param "/tweet_client_tablet" "volume" :double 1.0) +(ros::set-dynamic-reconfigure-param "/tweet_client_uptime" "volume" :double 1.0) +(ros::set-dynamic-reconfigure-param "/tweet_client_warning" "volume" :double 1.0) +(ros::set-dynamic-reconfigure-param "/tweet_client_worktime" "volume" :double 1.0) +(exit) diff --git a/jsk_robot_common/jsk_robot_startup/lifelog/auto_speak_volume_zero.l b/jsk_robot_common/jsk_robot_startup/lifelog/auto_speak_volume_zero.l new file mode 100755 index 0000000000..e0bb7d2c43 --- /dev/null +++ b/jsk_robot_common/jsk_robot_startup/lifelog/auto_speak_volume_zero.l @@ -0,0 +1,14 @@ +#!/usr/bin/env roseus + +(ros::roseus "speak_volume_zero") +(ros::set-dynamic-reconfigure-param "/audible_warning" "enable" :bool nil) +(ros::set-dynamic-reconfigure-param "/tweet_client_tablet" "speak_enable" :bool nil) +(ros::set-dynamic-reconfigure-param "/tweet_client_uptime" "speak_enable" :bool nil) +(ros::set-dynamic-reconfigure-param "/tweet_client_warning" "speak_enable" :bool nil) +(ros::set-dynamic-reconfigure-param "/tweet_client_worktime" "speak_enable" :bool nil) +(ros::set-dynamic-reconfigure-param "/audible_warning" "volume" :double 0.0) +(ros::set-dynamic-reconfigure-param "/tweet_client_tablet" "volume" :double 0.0) +(ros::set-dynamic-reconfigure-param "/tweet_client_uptime" "volume" :double 0.0) +(ros::set-dynamic-reconfigure-param "/tweet_client_warning" "volume" :double 0.0) +(ros::set-dynamic-reconfigure-param "/tweet_client_worktime" "volume" :double 0.0) +(exit) 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 b99511f06d..bb8135caac 100644 --- a/jsk_robot_common/jsk_robot_startup/lifelog/common_logger.launch +++ b/jsk_robot_common/jsk_robot_startup/lifelog/common_logger.launch @@ -236,6 +236,7 @@ topics: - /Tablet/voice + - /speech_to_text diff --git a/jsk_robot_common/jsk_robot_startup/lifelog/mongodb.launch b/jsk_robot_common/jsk_robot_startup/lifelog/mongodb.launch index 13c62d42f4..1022e41002 100644 --- a/jsk_robot_common/jsk_robot_startup/lifelog/mongodb.launch +++ b/jsk_robot_common/jsk_robot_startup/lifelog/mongodb.launch @@ -1,5 +1,6 @@ + @@ -26,6 +27,7 @@ + diff --git a/jsk_robot_common/jsk_robot_startup/lifelog/tweet.launch b/jsk_robot_common/jsk_robot_startup/lifelog/tweet.launch index b22b0d066c..0e4f93ecb0 100644 --- a/jsk_robot_common/jsk_robot_startup/lifelog/tweet.launch +++ b/jsk_robot_common/jsk_robot_startup/lifelog/tweet.launch @@ -16,6 +16,7 @@ + @@ -27,12 +28,28 @@ output="$(arg output)" respawn="true" > + + + account_info: $(arg account_info) + + + + + + + + + + image img)) - (ros::ros-info "tweeting ~A" twit-str) - (ros::publish tweet-topic-name - (instance std_msgs::String :init - :data (format nil "~A ~A" - twit-str (format nil "/tmp/tweet_image.jpg"))))) - (ros::ros-error "fail saving image and tweeting")) + ;;rostwitter.msg.TweetActionGoal(goal=rostwitter.msg.TweetGoal(text="[Audible warning] " + m.data, image=True, image_topic_name="/edgetpu_human_pose_estimator/output/image"))' + + (ros::ros-info "tweeting ~A" twit-str) + (ros::publish "/tweet_image_server/tweet/goal" + (instance rostwitter::TweetActionGoal :init + :goal + (instance rostwitter::TweetGoal :init + :text twit-str + :image t + :image_topic_name "/tweet_image"))) (ros::service-call "/tweet_image_mux/select" (instance topic_tools::muxselectrequest :init :topic prev-image-topic))) (t - (ros::publish tweet-topic-name (instance std_msgs::String :init :data twit-str)))) - (when speak (speak-jp "ついーとしました" :wait t)))) - + (ros::publish "/tweet" (instance std_msgs::String :init :data twit-str)))) + (when speak (speak-jp "ついーとしました" :wait t :volume volume)))) 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 ed1be21a4e..a199212465 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 @@ -3,15 +3,45 @@ (ros::roseus "twitter_client_tablet") (ros::roseus-add-msgs "roseus") +(load "package://roseus/euslisp/dynamic-reconfigure-server.l") (load "package://jsk_robot_startup/lifelog/tweet_client.l") +(setq *volume* (ros::get-param "~volume" 1.0)) +(setq *speak-enable* (ros::get-param "~speak_enable" t)) +(setq *enable* (ros::get-param "~enable" t)) +(setq *reconfigure-server* + (def-dynamic-reconfigure-server + ;;; ((name type level description (default) (min) (max) (edit_method)) ... ) + (("volume" double_t 0 "tweet speak volume" 1.0 0.0 1.0) + ("speak_enable" bool_t 0 "tweet speak enable" t) + ("enable" bool_t 0 "tweet enable" t)) + ;; use lamda-closure to avoid memory error + '(lambda-closure nil 0 0 (cfg level) + (let ((prev-volume *volume*) + (prev-speak-enable *speak-enable*) + (prev-enable *enable*)) + (setq *volume* (cdr (assoc "volume" cfg :test #'equal))) + (setq *speak-enable* (cdr (assoc "speak_enable" cfg :test #'equal))) + (setq *enable* (cdr (assoc "enable" cfg :test #'equal))) + (if (null (equal *volume* prev-volume)) + (ros::ros-warn "Volume changed to: ~A" *volume*)) + (if (null (equal *enable* prev-enable)) + (ros::ros-warn "Enable changed to: ~A" *enable*)) + (if (null (equal *speak-enable* prev-speak-enable)) + (ros::ros-warn "Speak enable changed to: ~A" *speak-enable*))) + cfg))) + (defun twit-cb (msg) - (let ((twit-str (send msg :data))) - (tweet-string twit-str - :warning-time nil - :with-image "/tablet/marked/image_rect_color"))) + (if *enable* + (let ((twit-str (send msg :data))) + (tweet-string twit-str + :warning-time nil + :with-image "/tablet/marked/image_rect_color" + :speak *speak-enable* + :volume *volume*)))) (ros::advertise "/tweet" std_msgs::String 1) +(ros::advertise "/tweet_image_server/tweet/goal" rostwitter::TweetActionGoal 1) (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 d49af0e6e4..e9a7224631 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,6 +2,7 @@ (ros::roseus "twitter_client_uptime") +(load "package://roseus/euslisp/dynamic-reconfigure-server.l") (load "package://jsk_robot_startup/lifelog/tweet_client.l") (setq *src-lines* nil) @@ -37,52 +38,82 @@ (t (setq *waking-target-second* *waking-tweet-second*))) +(setq *volume* (ros::get-param "~volume" 1.0)) +(setq *speak-enable* (ros::get-param "~speak_enable" t)) +(setq *enable* (ros::get-param "~enable" t)) +(setq *reconfigure-server* + (def-dynamic-reconfigure-server + ;;; ((name type level description (default) (min) (max) (edit_method)) ... ) + (("volume" double_t 0 "tweet speak volume" 1.0 0.0 1.0) + ("speak_enable" bool_t 0 "tweet speak enable" t) + ("enable" bool_t 0 "tweet enable" t)) + ;; use lamda-closure to avoid memory error + '(lambda-closure nil 0 0 (cfg level) + (let ((prev-volume *volume*) + (prev-speak-enable *speak-enable*) + (prev-enable *enable*)) + (setq *volume* (cdr (assoc "volume" cfg :test #'equal))) + (setq *speak-enable* (cdr (assoc "speak_enable" cfg :test #'equal))) + (setq *enable* (cdr (assoc "enable" cfg :test #'equal))) + (if (null (equal *volume* prev-volume)) + (ros::ros-warn "Volume changed to: ~A" *volume*)) + (if (null (equal *enable* prev-enable)) + (ros::ros-warn "Enable changed to: ~A" *enable*)) + (if (null (equal *speak-enable* prev-speak-enable)) + (ros::ros-warn "Speak enable changed to: ~A" *speak-enable*))) + cfg))) + (ros::advertise "/tweet" std_msgs::String 1) +(ros::advertise "/tweet_image_server/tweet/goal" rostwitter::TweetActionGoal 1) (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")) - ;; 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-debug "~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*)) - )) + (if *enable* + (progn + (setq *user-name* (ros::get-param "/active_user/launch_user_name") + *elapsed-time* (ros::get-param "/active_user/elapsed_time")) + ;; 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-debug "~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*)) + )) - (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 :warning-time 1 :with-image t) - )) - )))) + (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 :warning-time 1 :with-image t + :speak *speak-enable* + :volume *volume*) + )))))))) + (ros::spin-once) (ros::sleep) ) 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 7ec7e3dceb..93f74332fa 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,70 @@ (ros::roseus "twitter_client_warning") +(load "package://roseus/euslisp/dynamic-reconfigure-server.l") (load "package://jsk_robot_startup/lifelog/tweet_client.l") (ros::load-ros-manifest "diagnostic_msgs") +(setq *volume* (ros::get-param "~volume" 1.0)) +(setq *speak-enable* (ros::get-param "~speak_enable" t)) +(setq *enable* (ros::get-param "~enable" t)) +(setq *reconfigure-server* + (def-dynamic-reconfigure-server + ;;; ((name type level description (default) (min) (max) (edit_method)) ... ) + (("volume" double_t 0 "tweet speak volume" 1.0 0.0 1.0) + ("speak_enable" bool_t 0 "tweet speak enable" t) + ("enable" bool_t 0 "tweet enable" t)) + ;; use lamda-closure to avoid memory error + '(lambda-closure nil 0 0 (cfg level) + (let ((prev-volume *volume*) + (prev-speak-enable *speak-enable*) + (prev-enable *enable*)) + (setq *volume* (cdr (assoc "volume" cfg :test #'equal))) + (setq *speak-enable* (cdr (assoc "speak_enable" cfg :test #'equal))) + (setq *enable* (cdr (assoc "enable" cfg :test #'equal))) + (if (null (equal *volume* prev-volume)) + (ros::ros-warn "Volume changed to: ~A" *volume*)) + (if (null (equal *enable* prev-enable)) + (ros::ros-warn "Enable changed to: ~A" *enable*)) + (if (null (equal *speak-enable* prev-speak-enable)) + (ros::ros-warn "Speak enable changed to: ~A" *speak-enable*))) + cfg))) + (defun diagnostics-cb (msg) - (let ((diagnostics (make-hash-table :test #'equal)) - (tm (ros::time-now)) - status id) - (ros::ros-debug (format nil "~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) - ((substringp "/Peripherals/PS3 Controller" (send status :name)) t) ;; fetch joystick warning - ((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 - (maphash #'(lambda (k v) (ros::ros-debug (format nil "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)) - :warning-time 1 :with-image t) - )) ;; when - )) ;; let + (if *enable* + (let ((diagnostics (make-hash-table :test #'equal)) + (tm (ros::time-now)) + status id) + (ros::ros-debug (format nil "~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) + ((substringp "/Peripherals/PS3 Controller" (send status :name)) t) ;; fetch joystick warning + ((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 + (maphash #'(lambda (k v) (ros::ros-debug (format nil "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)) + :warning-time 1 :with-image t :volume *volume* :speak *speak-enable*))) + ))) (ros::advertise "/tweet" std_msgs::String 1) +(ros::advertise "/tweet_image_server/tweet/goal" rostwitter::TweetActionGoal 1) (ros::subscribe "/diagnostics_agg" diagnostic_msgs::DiagnosticArray #'diagnostics-cb) (ros::rate (/ 1.0 3.0)) (while (ros::ok) 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 dac8769842..a52ecc171e 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,6 +2,7 @@ (ros::roseus "twitter_client_worktime") +(load "package://roseus/euslisp/dynamic-reconfigure-server.l") (load "package://jsk_robot_startup/lifelog/tweet_client.l") (defvar *robot-name* "robot") @@ -34,49 +35,79 @@ (t (setq *waking-target-second* *waking-tweet-second*))) +(setq *volume* (ros::get-param "~volume" 1.0)) +(setq *speak-enable* (ros::get-param "~speak_enable" t)) +(setq *enable* (ros::get-param "~enable" t)) +(setq *reconfigure-server* + (def-dynamic-reconfigure-server + ;;; ((name type level description (default) (min) (max) (edit_method)) ... ) + (("volume" double_t 0 "tweet speak volume" 1.0 0.0 1.0) + ("speak_enable" bool_t 0 "tweet speak enable" t) + ("enable" bool_t 0 "tweet enable" t)) + ;; use lamda-closure to avoid memory error + '(lambda-closure nil 0 0 (cfg level) + (let ((prev-volume *volume*) + (prev-speak-enable *speak-enable*) + (prev-enable *enable*)) + (setq *volume* (cdr (assoc "volume" cfg :test #'equal))) + (setq *speak-enable* (cdr (assoc "speak_enable" cfg :test #'equal))) + (setq *enable* (cdr (assoc "enable" cfg :test #'equal))) + (if (null (equal *volume* prev-volume)) + (ros::ros-warn "Volume changed to: ~A" *volume*)) + (if (null (equal *enable* prev-enable)) + (ros::ros-warn "Enable changed to: ~A" *enable*)) + (if (null (equal *speak-enable* prev-speak-enable)) + (ros::ros-warn "Speak enable changed to: ~A" *speak-enable*))) + cfg))) + (ros::advertise "/tweet" std_msgs::String 1) +(ros::advertise "/tweet_image_server/tweet/goal" rostwitter::TweetActionGoal 1) (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! " - 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) + (if *enable* + (progn + (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! " + 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 + :speak *speak-enable* + :volume *volume*) + )))) + (ros::spin-once) (ros::sleep) ) diff --git a/jsk_robot_common/jsk_robot_startup/scripts/smach_to_mail.py b/jsk_robot_common/jsk_robot_startup/scripts/smach_to_mail.py index ac911446b0..cf73a32f11 100755 --- a/jsk_robot_common/jsk_robot_startup/scripts/smach_to_mail.py +++ b/jsk_robot_common/jsk_robot_startup/scripts/smach_to_mail.py @@ -148,6 +148,7 @@ def _status_cb(self, msg): # If we received START/INIT status, restart storing smach_state_list if status_str in ["START", "INIT"]: + self.smach_start_time[caller_id] = rospy.Time.now() self.smach_state_list[caller_id] = [] # DESCRIPTION of START is MAIL SUBJECT if 'DESCRIPTION' in local_data_str: diff --git a/jsk_robot_common/jsk_robot_startup/scripts/update_workspace_main.sh b/jsk_robot_common/jsk_robot_startup/scripts/update_workspace_main.sh new file mode 100755 index 0000000000..053f0fcfd3 --- /dev/null +++ b/jsk_robot_common/jsk_robot_startup/scripts/update_workspace_main.sh @@ -0,0 +1,142 @@ +#!/usr/bin/env bash + +function usage() +{ + echo "Usage: $0 [-w workspace_directory] [-r rosinstall_path] [-t robot_type] [-s skip keys] [-h] [-u] [-l] + +optional arguments: + -h show this help + -w WORKSPACE_PATH specify target workspace + -r ROSINTALL_PATH rosinstall path + -t ROBOT robot type + -s SKIP_KEYS rosdep install skip keys + -u do not run apt-get upgrade and rosdep install + -l do not send a mail +" +} + +function get_full_path() +{ + echo "$(cd $(dirname $1) && pwd)/$(basename $1)" +} + +ROSDEP_INSTALL=true +SEND_MAIL=true +WORKSPACE=$(get_full_path $HOME/ros/$ROS_DISTRO) + +while getopts w:r:t:s:ulh OPT +do + case $OPT in + w) + WORKSPACE=$(get_full_path $OPTARG) + ;; + r) + ROSINSTALL=$(get_full_path $OPTARG) + ;; + t) + ROBOT_TYPE=$OPTARG + ;; + s) + SKIP_KEYS=$OPTARG + ;; + u) + ROSDEP_INSTALL=false + ;; + l) + SEND_MAIL=false + ;; + h) + usage + exit 1 + ;; + *) + usage + exit 1 + ;; + esac +done + +if [ "$WORKSPACE" = "" ]; then + echo "Please set valid workspace -w $WORKSPACE" + exit 1 +fi +if [ "$ROSINSTALL" = "" ]; then + echo "Please set valid rosinstall -r $ROSINSTALL" + exit 1 +fi +if [ "$ROBOT_TYPE" = "" ]; then + echo "Please set valid robot type -t $ROBOT_TYPE" + exit 1 +fi + + +. $WORKSPACE/devel/setup.bash +# Filename should end with .txt to preview the contents in a web browser +LOGFILE=$WORKSPACE/update_workspace.txt + +TMP_MAIL_BODY_FILE=/tmp/update_workspace_mailbody.txt + +{ +set -x +# Update workspace + +wstool foreach -t $WORKSPACE/src --git 'git stash' +wstool foreach -t $WORKSPACE/src --git 'git fetch origin --prune' +wstool update -t $WORKSPACE/src jsk-ros-pkg/jsk_robot --delete-changed-uris +ln -sf $ROSINSTALL $WORKSPACE/src/.rosinstall +wstool update -t $WORKSPACE/src --delete-changed-uris +# Forcefully checkout specified branch +wstool foreach -t $WORKSPACE/src --git --shell 'branchname=$(git rev-parse --abbrev-ref HEAD); if [ $branchname != "HEAD" ]; then git reset --hard HEAD; git checkout origin/$branchname; git branch -D $branchname; git checkout -b $branchname --track origin/$branchname; fi' +wstool update -t $WORKSPACE/src +WSTOOL_UPDATE_RESULT=$? +# Rosdep Install +if [ "${ROSDEP_INSTALL}" == "true" ]; then + sudo apt-get update -y; + rosdep update; + rosdep install --from-paths $WORKSPACE/src --ignore-src -y -r --skip-keys "$SKIP_KEYS"; + ROSDEP_INSTALL_RESULT=$?; +else + ROSDEP_INSTALL_RESULT=0; +fi +# Build workspace +cd $WORKSPACE +catkin clean aques_talk collada_urdf_jsk_patch libcmt -y +catkin init +catkin config -DCMAKE_BUILD_TYPE=Release +catkin build --continue-on-failure +CATKIN_BUILD_RESULT=$? +# Send mail +echo "" > $TMP_MAIL_BODY_FILE +if [ $WSTOOL_UPDATE_RESULT -ne 0 ]; then + echo "Please wstool update workspace manually.\n" >> $TMP_MAIL_BODY_FILE +fi +if [ $ROSDEP_INSTALL_RESULT -ne 0 ]; then + echo "Please install dependencies manually.\n" >> $TMP_MAIL_BODY_FILE +fi +if [ $CATKIN_BUILD_RESULT -ne 0 ]; then + echo "Please catkin build workspace manually.\n" >> $TMP_MAIL_BODY_FILE +fi +set +x +} 2>&1 | tee $LOGFILE + +# MAIL_BODY variable cannot be set directly in a subshell. So it is set from temporary mail body text file. +# The mail body text is put as $TMP_MAIL_BODY_FILE. +# See https://github.com/jsk-ros-pkg/jsk_robot/issues/1569 +MAIL_BODY=$(cat $TMP_MAIL_BODY_FILE) +echo "MAIL_BODY: $MAIL_BODY" + +if [ -n "$MAIL_BODY" ] && [ "${SEND_MAIL}" == "true" ]; then + echo "Sent a mail" + rostopic pub -1 /email jsk_robot_startup/Email "header: + seq: 0 + stamp: {secs: 0, nsecs: 0} + frame_id: '' +subject: 'Daily workspace update fails' +body: +- {type: 'text', message: '${MAIL_BODY}', file_path: '', img_data: '', img_size: 0} +sender_address: '$(hostname)@jsk.imi.i.u-tokyo.ac.jp' +receiver_address: '$ROBOT_TYPE@jsk.imi.i.u-tokyo.ac.jp' +smtp_server: '' +smtp_port: '' +attached_files: ['$LOGFILE']" +fi