[WIP][Spot] add mech office demo

jsk_spot_go_to_spot/CMakeLists.txt

@@ -0,0 +1,17 @@
+cmake_minimum_required(VERSION 3.0.2)
+project(jsk_spot_go_to_spot)
+
+find_package(catkin REQUIRED COMPONENTS
+  rospy
+)
+
+catkin_package(
+)
+
+catkin_install_python(PROGRAMS node_scripts/go_to_spot.py
+  DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION}
+)
+
+install(DIRECTORY launch
+  DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}
+)

jsk_spot_go_to_spot/launch/demo.launch

@@ -0,0 +1,12 @@
+<launch>
+  <arg name="target_node_id" default="eng2_7FElevator" />
+
+  <node
+    pkg="jsk_spot_go_to_spot"
+    type="go_to_spot.py"
+    name="go_to_spot"
+    output="screen"
+    >
+    <param name="target_node_id" value="$(arg target_node_id)" />
+  </node>
+</launch>

jsk_spot_go_to_spot/node_scripts/go_to_spot.py

@@ -0,0 +1,44 @@
+#!/usr/bin/env python
+# -*- encoding: utf-8 -*-
+
+"""this script is a demo that spot go to a specified spot"""
+
+import actionlib
+import rospy
+from spot_behavior_manager_msgs.msg import LeadPersonAction, LeadPersonGoal
+
+
+def main():
+
+    rospy.init_node('spot_go_to_spot')
+
+    target_node_id = rospy.get_param('~target_node_id', 'eng2_7FElevator')
+    num_retry = rospy.get_param('~num_retry', 3)
+
+    client = actionlib.SimpleActionClient(
+        '/spot_behavior_manager_demo/lead_person', LeadPersonAction)
+
+    rospy.loginfo('waiting for server...')
+
+    if not client.wait_for_server(rospy.Duration(10)):
+        rospy.logerr('Server down.')
+        return
+
+    rospy.loginfo('Start to go to {}'.format(target_node_id))
+
+    for i in range(num_retry):
+
+        client.send_goal_and_wait(
+            LeadPersonGoal(target_node_id=target_node_id))
+        result = client.get_result()
+        if result.success:
+            rospy.loginfo('Finished.')
+            return
+        else:
+            rospy.logwarn('Failed. retrying...')
+
+    rospy.logerr('Failed even after retrying.')
+
+
+if __name__ == '__main__':
+    main()

jsk_spot_go_to_spot/package.xml

@@ -0,0 +1,18 @@
+<?xml version="1.0"?>
+<package format="2">
+  <name>jsk_spot_go_to_spot</name>
+  <version>0.0.4</version>
+  <description>The jsk_spot_go_to_spot package</description>
+
+  <maintainer email="[email protected]">Koki Shinjo</maintainer>
+
+  <license>BSD</license>
+
+  <buildtool_depend>catkin</buildtool_depend>
+  <exec_depend>rospy</exec_depend>
+  <exec_depend>actionlib</exec_depend>
+  <exec_depend>spot_behavior_manager_msgs</exec_depend>
+
+  <export>
+  </export>
+</package>

jsk_spot_take_envelop_to_mech_office/CMakeLists.txt

@@ -0,0 +1,17 @@
+cmake_minimum_required(VERSION 3.0.2)
+project(jsk_spot_take_envelop_to_mech_office)
+
+find_package(catkin REQUIRED COMPONENTS
+  rospy
+)
+
+catkin_package(
+)
+
+catkin_install_python(PROGRAMS node_scripts/take_envelop_to_mech_office.py
+  DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION}
+)
+
+install(DIRECTORY launch
+  DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}
+)

jsk_spot_take_envelop_to_mech_office/README.md

@@ -0,0 +1,20 @@
+# jsk_spot_take_envelop_to_mech_office
+
+With this demo, Spot will take envelop to mech office at building 2 of engineering faculty.
+
+TODO: add video or picture
+
+## Prerequities
+
+This demo requires
+
+- spot_behavior_manager node from spot_behavior_manager_demo
+- object_detection launch from jsk_spot_startup
+
+And before launching demo, please move Spot to 73B2 front.
+
+## how to launch
+
+```
+roslaunch jsk_spot_take_envelop_to_mech_office take_envelop_to_mech_office.launch
+```

jsk_spot_take_envelop_to_mech_office/launch/take_envelop_to_mech_office.launch

@@ -0,0 +1,9 @@
+<launch>
+    <node
+        pkg="jsk_spot_take_envelop_to_mech_office"
+        type="take_envelop_to_mech_office.py"
+        name="take_envelop_to_mech_office"
+        output="screen"
+        >
+    </node>
+</launch>

jsk_spot_take_envelop_to_mech_office/node_scripts/take_envelop_to_mech_office.py

@@ -0,0 +1,115 @@
+#!/usr/bin/env python
+# -*- encoding: utf-8 -*-
+
+""""""
+
+import sys
+import actionlib
+from sound_play.libsoundplay import SoundClient
+import rospy
+from spot_behavior_manager_msgs.msg import LeadPersonAction, LeadPersonGoal
+
+
+class Demo:
+
+    def __init__(self):
+
+        self.node_id_73b2 = rospy.get_param('~node_id_73b2', 'eng2_73B2')
+        self.node_id_mech_office = rospy.get_param(
+            '~node_id_mech_office', 'eng2_Mech_Office')
+        self.num_max_retry = rospy.get_param('~num_max_retry', 3)
+
+        self.actionclient = actionlib.SimpleActionClient(
+            '/spot_behavior_manager_demo/lead_person', LeadPersonAction)
+        self.soundclient = SoundClient(sound_action='/robotsound_jp',sound_topic='/robotsound_jp')
+
+        rospy.loginfo('waiting for server...')
+        if not self.actionclient.wait_for_server(rospy.Duration(10)):
+            rospy.logerr('Server down.')
+            self.initialized = False
+        else:
+            self.initialized = True
+
+    def exit(self):
+
+        rospy.logwarn('Trying to go back to 73b2...')
+        for num_trial in range(self.num_max_retry):
+            self.actionclient.send_goal_and_wait(
+                LeadPersonGoal(target_node_id=self.node_id_73b2))
+            result = self.actionclient.get_result()
+            if result.success:
+                rospy.loginfo('Finished.')
+                break
+            else:
+                if num_trial < self.num_max_retry - 1:
+                    rospy.logwarn('Failed. retrying...')
+                else:
+                    rospy.logerr('Failed even after retrying.')
+                    sys.exit(1)
+
+        rospy.loginfo('I am back to 73B2.')
+        sys.exit(1)
+
+    def run(self):
+
+        # wait for envelop
+        self.soundclient.say('事務室へ持っていくものを置いてください。',blocking=True)
+        ## checking if something is placed on the robot
+        ### TODO
+        ## ask if OK
+        ### TODO
+
+        # go to mech office
+        rospy.loginfo('Start to go to {}'.format(self.node_id_mech_office))
+        self.soundclient.say('事務室へ向かいます',blocking=True)
+        for num_trial in range(self.num_max_retry):
+            self.actionclient.send_goal_and_wait(
+                LeadPersonGoal(target_node_id=self.node_id_mech_office))
+            result = self.actionclient.get_result()
+            if result.success:
+                rospy.loginfo('Finished.')
+                break
+            else:
+                if num_trial < self.num_max_retry - 1:
+                    rospy.logwarn('Failed. retrying...')
+                else:
+                    rospy.logerr('Failed even after retrying.')
+                    self.exit()
+
+        rospy.loginfo('I am now at the Mech Office.')
+
+        self.soundclient.say('荷物を回収してください。',blocking=True)
+        ## checking if package is taken from the robot
+        ### TODO
+        ## ask if OK
+        ### TODO
+
+        # go back to 73b2
+        rospy.loginfo('Return to {}'.format(self.node_id_73b2))
+        for num_trial in range(self.num_max_retry):
+            self.actionclient.send_goal_and_wait(
+                LeadPersonGoal(target_node_id=self.node_id_73b2))
+            result = self.actionclient.get_result()
+            if result.success:
+                rospy.loginfo('Finished.')
+                break
+            else:
+                if num_trial < self.num_max_retry - 1:
+                    rospy.logwarn('Failed. retrying...')
+                else:
+                    rospy.logerr('Failed even after retrying.')
+                    self.exit()
+
+        rospy.loginfo('I am back to 73B2.')
+
+
+def main():
+    rospy.init_node('spot_go_to_spot')
+    demo = Demo()
+    if not demo.initialized:
+        sys.exit(1)
+    demo.run()
+
+
+if __name__ == '__main__':
+    main()

jsk_spot_take_envelop_to_mech_office/package.xml

@@ -0,0 +1,18 @@
+<?xml version="1.0"?>
+<package format="2">
+  <name>jsk_spot_take_envelop_to_mech_office</name>
+  <version>0.0.4</version>
+  <description>The jsk_spot_take_envelop_to_mech_office package</description>
+
+  <maintainer email="[email protected]">Koki Shinjo</maintainer>
+
+  <license>BSD</license>
+
+  <buildtool_depend>catkin</buildtool_depend>
+  <exec_depend>rospy</exec_depend>
+  <exec_depend>actionlib</exec_depend>
+  <exec_depend>spot_behavior_manager_msgs</exec_depend>
+
+  <export>
+  </export>
+</package>