[WIP] Merge PR1040 branch to master

jsk_pr2_robot/jsk_pr2_startup/jsk_pr2.rosinstall

@@ -1,64 +1,132 @@
 - git:
     local-name: PR2/app_manager
-    uri: https://github.com/furushchev/app_manager
-    version: indigo
+    uri: https://github.com/PR2/app_manager
+    version: pr1040
 - git:
-    local-name: PR2/pr2_navigation
-    uri: https://github.com/PR2/pr2_navigation.git
-    version: hydro-devel
+    local-name: PR2/pr2_common
+    uri: https://github.com/PR2/pr2_common.git
+    version: melodic-devel
 - git:
-    local-name: code-iai/snap_map_icp
-    uri: https://github.com/code-iai/snap_map_icp.git
-    version: hydro-devel
+    local-name: PR2/pr2_controllers
+    uri: https://github.com/PR2/pr2_controllers.git
+    version: melodic-devel 
+- git:
+    local-name: PR2/pr2_self_test
+    uri: https://github.com/PR2/pr2_self_test.git
+    version: kinetic-devel
+- git:
+    local-name: RobotWebTools/rosbridge_suite
+    uri: https://github.com/RobotWebTools/rosbridge_suite.git
+    version: 0.11.9
 - git:
-    local-name: iai_common_msgs
+    local-name: code-iai/iai_common_msgs
     uri: https://github.com/code-iai/iai_common_msgs.git
     version: 0.0.6
+- git:
+    local-name: code-iai/snap_map_icp
+    uri: https://github.com/code-iai/snap_map_icp.git
+    version: da2d590c5b4923c0776d990bb3ba9ec8e9122ecf
+# - git:
+#     local-name: euslisp/Euslisp
+#     uri: https://github.com/euslisp/Euslisp.git
+#     version: master 
+- 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/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: 2.1.6
+    version: pr1040
 - git:
     local-name: jsk-ros-pkg/jsk_common
     uri: https://github.com/jsk-ros-pkg/jsk_common.git
-    version: 2.2.6
-- git:
-    local-name: jsk-ros-pkg/jsk_common_msgs
-    uri: https://github.com/jsk-ros-pkg/jsk_common_msgs.git
-    version: 4.3.1
-- git:
-    local-name: jsk-ros-pkg/jsk_control
-    uri: https://github.com/jsk-ros-pkg/jsk_control.git
-    version: 0.1.14
+    version: pr1040
 - git:
     local-name: jsk-ros-pkg/jsk_demos
     uri: https://github.com/jsk-ros-pkg/jsk_demos.git
-    version: 0.0.4
-- git:
-    local-name: jsk-ros-pkg/jsk_model_tools
-    uri: https://github.com/jsk-ros-pkg/jsk_model_tools.git
-    version: 0.3.5
+    version: pr1040
 - git:
     local-name: jsk-ros-pkg/jsk_planning
     uri: https://github.com/jsk-ros-pkg/jsk_planning.git
-    version: 0.1.10
+    version: master 
 - git:
     local-name: jsk-ros-pkg/jsk_pr2eus
     uri: https://github.com/jsk-ros-pkg/jsk_pr2eus.git
-    version: 0.3.13
+    version: knorth55/pr1040
 - git:
     local-name: jsk-ros-pkg/jsk_recognition
     uri: https://github.com/jsk-ros-pkg/jsk_recognition.git
-    version: 1.2.4
+    version: master 
+- git:
+    local-name: jsk-ros-pkg/jsk_robot
+    uri: https://github.com/jsk-ros-pkg/jsk_robot.git
+    version: pr1040
 - git:
     local-name: jsk-ros-pkg/jsk_roseus
     uri: https://github.com/jsk-ros-pkg/jsk_roseus.git
-    version: 1.6.3
+    version: knorth55/pr1040 
+- git:
+    local-name: Affonso-Gui/roseus_resume
+    uri: https://github.com/Affonso-Gui/roseus_resume.git
+    version: eus10
 - git:
     local-name: jsk-ros-pkg/jsk_smart_apps
     uri: https://github.com/jsk-ros-pkg/jsk_smart_apps.git
-    version: 0.0.2
+    version: 57a355333a4f001d219003b52ecefdb0ff5b0acf
 - git:
     local-name: jsk-ros-pkg/jsk_visualization
     uri: https://github.com/jsk-ros-pkg/jsk_visualization.git
-    version: 2.1.3
+    version: master 
+- git:
+    local-name: knorth55/app_manager_utils
+    uri: https://github.com/knorth55/app_manager_utils
+    version: master
+- git:
+    local-name: locusrobotics/catkin_virtualenv
+    uri: https://github.com/locusrobotics/catkin_virtualenv.git
+    version: 0.5.2
+- git:
+    local-name: ros-drivers/audio_common
+    uri: https://github.com/ros-drivers/audio_common.git
+    version: master 
+- git:
+    local-name: ros-drivers/prosilica_driver
+    uri: https://github.com/ros-drivers/prosilica_driver
+    version: 3991df761ce0400b9993166c09a2529ef684cdc1
+- tar:
+    local-name: ros/common_msgs/visualization_msgs
+    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
+- git:
+    local-name: ros-visualization/interactive_markers
+    uri: https://github.com/ros-visualization/interactive_markers.git
+    version: noetic-devel
+- tar:
+    local-name: ros/ros_comm/message_filters
+    uri: https://github.com/ros-gbp/ros_comm-release/archive/release/kinetic/message_filters/1.12.14-0.tar.gz
+    version: ros_comm-release-release-kinetic-message_filters-1.12.14-0
+- tar:
+    local-name: ros/ros_comm/topic_tools
+    uri: https://github.com/ros-gbp/ros_comm-release/archive/release/kinetic/topic_tools/1.12.14-0.tar.gz
+    version: ros_comm-release-release-kinetic-topic_tools-1.12.14-0
+- git:
+    local-name: strands-project/mongodb_store
+    uri: https://github.com/strands-project/mongodb_store
+    version: 0.4.5
+- git:
+    local-name: tork-a/roswww
+    uri: https://github.com/tork-a/roswww.git
+    version: develop
+- git:
+    local-name: tork-a/visualization_rwt
+    uri: https://github.com/tork-a/visualization_rwt.git
+    version: kinetic-devel

jsk_pr2_robot/jsk_pr2_startup/jsk_pr2_lifelog/pr2_tweet.launch

@@ -3,6 +3,7 @@
 
   <include file="$(find jsk_robot_startup)/lifelog/tweet.launch">
     <arg name="robot_name" value="PR2"/>
+    <arg name="image_topics" default="/edgetpu_human_pose_estimator/output/image" />
 
     <arg name="machine" value="c2"/>
     <arg name="output" value="screen"/>

jsk_pr2_robot/jsk_pr2_startup/jsk_pr2_sensors/check_openni_node.py

@@ -89,7 +89,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)
 

jsk_pr2_robot/jsk_pr2_startup/jsk_pr2_sensors/kinect_head.launch

@@ -232,9 +232,9 @@
   <group if="$(arg respawn)">
     <node pkg="jsk_pr2_startup" name="check_openni_node" type="check_openni_node.py"
           output="screen" machine="c1" if="$(arg monitor_driver)">
+      <remap from="image" to="/$(arg camera)/rgb/image_raw" />
       <param name="camera" value="$(arg camera)" />
       <param name="sleep_cycle" value="60" />
-      <remap from="image" to="/$(arg camera)/rgb/image_rect_color" />
     </node>
   </group>
 

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):

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()

jsk_pr2_robot/jsk_pr2_startup/jsk_pr2_warning/jsk_pr2_analyzers.yaml

@@ -6,9 +6,9 @@ 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

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

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.*"

jsk_pr2_robot/jsk_pr2_startup/pr2.launch

@@ -67,7 +67,9 @@
 
   <!-- kinect node -->
   <include if="$(arg launch_kinect)"
-           file="$(find jsk_pr2_startup)/jsk_pr2_sensors/kinect_head.launch" />
+           file="$(find jsk_pr2_startup)/jsk_pr2_sensors/kinect_head.launch" >
+    <arg name="monitor_driver" value="true" />  <!-- iory 2022/05/11-->
+  </include>
 
   <!-- logging node for PR2  -->
   <include if="$(arg launch_db)"
@@ -195,4 +197,54 @@
     <remap from="~input" to="/kinect_head/rgb/image_raw" />
   </node>
 
+  <node name="audible_warning"
+        pkg="jsk_tools" type="audible_warning.py"
+        clear_params="true"
+        output="screen" >
+    <rosparam command="load" file="$(find jsk_pr2_startup)/jsk_pr2_warning/warning_blacklist.yaml" />
+    <rosparam>
+      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
+    </rosparam>
+  </node>
+
+  <group ns="audible_warning" >
+    <!-- tweet with image -->
+    <node name="publish_to_tweet_server"
+          pkg="topic_tools" type="transform"
+          args="/audible_warning/output/original_text
+                /tweet_image_server/tweet/goal
+                rostwitter/TweetActionGoal
+                --wait-for-start
+                'rostwitter.msg.TweetActionGoal(goal=rostwitter.msg.TweetGoal(text=&quot;[Audible warning] &quot; + m.data, image=True, image_topic_name=&quot;/edgetpu_human_pose_estimator/output/image&quot;))'
+                --import rostwitter">
+    </node>
+  </group>
+
+  <node name="sanity_diagnostics"
+        pkg="jsk_tools" type="sanity_diagnostics.py" >
+    <rosparam command="load" file="$(find jsk_pr2_startup)/jsk_pr2_warning/sanity_targets.yaml" />
+    <rosparam subst_value="true">
+      duration: 60
+    </rosparam>
+  </node>
+
+  <!-- send email by rostopic -->
+  <node name="email_topic" pkg="jsk_robot_startup" type="email_topic.py" output="screen">
+    <rosparam>
+      email_info: /var/lib/robot/email_topic.yaml
+    </rosparam>
+  </node>
+
+  <!-- send email, twitter with smach -->
+  <node name="smach_to_mail_server" pkg="jsk_robot_startup" type="smach_to_mail.py" output="screen">
+    <rosparam>
+      email_info: /var/lib/robot/email_topic.yaml
+    </rosparam>
+  </node>
+
 </launch>

jsk_robot_common/jsk_robot_startup/apps/robot_apps.installed

@@ -3,3 +3,7 @@ apps:
     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

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

jsk_robot_common/jsk_robot_startup/apps/roseus_resume_interrupt/roseus_resume_interrupt.interface

@@ -0,0 +1,2 @@
+published_topics: {}
+subscribed_topics: {}

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

jsk_robot_common/jsk_robot_startup/apps/roseus_resume_resume/roseus_resume_resume.interface

@@ -0,0 +1,2 @@
+published_topics: {}
+subscribed_topics: {}