[WIP][Spot] Spot's person following demo

spot_person_follower/CMakeLists.txt

@@ -0,0 +1,12 @@
+cmake_minimum_required(VERSION 3.0.2)
+project(spot_person_follower)
+
+add_compile_options(-std=c++11 -Wall)
+
+find_package(catkin REQUIRED COMPONENTS genmsg actionlib_msgs geometry_msgs rospy)
+
+catkin_package(
+)
+
+include_directories(
+)

spot_person_follower/README.md

@@ -0,0 +1,32 @@
+# Person Follow Demo
+
+<TODO image>
+
+This demo enables Spot to follow person.
+
+## Prerequities
+
+This demo requires packages below.
+
+- [spot_ros]() with [this patch](https://github.com/clearpathrobotics/spot_ros/pull/25)
+- [common_msgs]() with [this patch](https://github.com/ros/common_msgs/pull/171)
+- [jsk_recognition]() with [this patch](https://github.com/jsk-ros-pkg/jsk_recognition/pull/2579) and [this patch](https://github.com/jsk-ros-pkg/jsk_recognition/pull/2581)
+- [jsk_spot_startup]() with [this patch](https://github.com/jsk-ros-pkg/jsk_robot/pull/1325)
+
+## How to run
+
+Before running this demo, please launch and prepair a controller.
+
+- `jsk_spot_bringup.launch`
+- `object_detection_and_tracking.launch`
+- `multi_object_detector.launch`
+
+And then, please run
+
+```bash
+roslaunch spot_person_follower demo.launch
+```
+
+After this, you can start following behavior by pressing L2 button of the controller.
+Spot will follow the nearest person at the time of pressing.
+If you want to stop the behavior, please press the L2 button again.

spot_person_follower/launch/demo.launch

@@ -0,0 +1,50 @@
+<launch>
+    <node
+        pkg="spot_person_follower"
+        type="main.py"
+        name="spot_person_follower"
+        output="screen"
+        >
+        <rosparam>
+            frame_fixed: odom
+            frame_robot: body
+            duration_timeout: 0.05
+            num_max_track: 20
+            thresholds_distance:
+                '0': 5.0 # person 
+            threshold_angle: 0.8
+            threshold_lost_duration: 5.0
+            threshold_move_velocity: 1.0
+            threshold_tracking_switching_distance: 1.0
+            threshold_target_close_distance: 0.5
+            destination_offset_from_target_x: -0.5
+            destination_offset_from_target_y: 0
+            destination_offset_from_target_theta: 0
+            publish_bbox_array: True
+            use_trajectory: True
+            rate_control: 1
+        </rosparam>
+
+        <remap from="~go_pos" to="/spot/trajectory" />
+        <remap from="~cmd_vel" to="/develop_input/cmd_vel" />
+        <remap from="~input_bbox_array" to="/spot_recognition/bbox_array" />
+        <remap from="~input_tracking_labels" to="/spot_recognition/tracking_labels" />
+
+    </node>
+
+    <node
+        pkg="spot_person_follower"
+        type="follow_on_button.py"
+        name="follow_on_button"
+        output="screen"
+        >
+        <rosparam>
+            duration_timeout: 10.0
+            follow_button: 6 # L2
+            duration_impersity: 1.0
+        </rosparam>
+
+        <remap from="~follow" to="/spot_person_follower/follow_person" />
+        <remap from="~joy" to="/spot_teleop/joy" />
+    </node>
+</launch>

spot_person_follower/node_scripts/follow_on_button.py

@@ -0,0 +1,56 @@
+#!/usr/bin/env python
+# -*- coding: utf-8 -*-
+
+import rospy
+from sensor_msgs.msg import Joy
+from std_srvs.srv import Trigger, TriggerRequest
+import sys
+
+class Node(object):
+
+    def __init__(self):
+
+        self._duration_timeout = rospy.get_param('~duration_timeout', 10.0)
+        self._follow_button = rospy.get_param('~follow_button', -1)
+        self._duration_impersity = rospy.get_param('~duration_impersity', 1.0)
+
+        self._is_pressed = False
+        self._last_pressed = rospy.Time.now()
+
+        #
+        try:
+            rospy.wait_for_service('~follow', timeout=rospy.Duration(self._duration_timeout))
+        except Exception as e:
+            rospy.logwarn('{}'.format(e))
+            sys.exit(1)
+        self._service_proxy = rospy.ServiceProxy('~follow',Trigger)
+
+        #
+        self._sub_joy = rospy.Subscriber('~joy', Joy, self._callback)
+
+        rospy.loginfo('now inialized')
+
+    def _callback(self,msg):
+
+        if self._follow_button < len(msg.buttons):
+            if msg.buttons[self._follow_button] > 0 and not self._is_pressed:
+                # become pressed
+                self._is_pressed = True
+                rospy.loginfo('button pressed')
+                if (rospy.Time.now() - self._last_pressed).to_sec() > self._duration_impersity:
+                    self._last_pressed = rospy.Time.now()
+                    self.callService()
+            elif msg.buttons[self._follow_button] <= 0 and self._is_pressed:
+                # become not pressed
+                self._is_pressed = False
+                rospy.loginfo('button releaseed')
+        else:
+            rospy.logwarn('button id is out of range')
+
+    def callService(self):
+        self._service_proxy()
+
+if __name__ == '__main__':
+    rospy.init_node('follow_on_button')
+    node = Node()
+    rospy.spin()