diff --git a/jsk_spot_robot/jsk_spot.rosinstall b/jsk_spot_robot/jsk_spot.rosinstall index 55d4258041..25c0e11aa0 100644 --- a/jsk_spot_robot/jsk_spot.rosinstall +++ b/jsk_spot_robot/jsk_spot.rosinstall @@ -24,12 +24,12 @@ local-name: jsk-ros-pkg/jsk_3rdparty uri: https://github.com/jsk-ros-pkg/jsk_3rdparty.git version: master -# Until https://github.com/jsk-ros-pkg/jsk_common/pull/1685 is merged, this patch is -# required to build spoteus_demo +# Until sktometometo/feature/add-tf-relay-package is merged, this patch is +# required for spot_basic_behaviors - git: local-name: jsk-ros-pkg/jsk_common uri: https://github.com/sktometometo/jsk_common.git - version: PR/update-download-data + version: feature/add-tf-relay-package # This is a develop branch for jsk version. # We need to use it until it is merged to master - git: diff --git a/jsk_spot_robot/jsk_spot_behaviors/README.md b/jsk_spot_robot/jsk_spot_behaviors/README.md new file mode 100644 index 0000000000..1e86f5dc3d --- /dev/null +++ b/jsk_spot_robot/jsk_spot_behaviors/README.md @@ -0,0 +1,49 @@ +# jsk_spot_behaviors + +These packages enable for Spot to execute locomotoion behaviors to reach a desired position. + +## Concept + +In this framework, knowledge about positions and transtion behaviors between them are represented as a digraph like below. + +Each node represents specified positions and each edge represents a behavior to transition between them. + +![example_graph](https://user-images.githubusercontent.com/9410362/124147589-cc8ce700-dac9-11eb-930f-1c00c2a4777e.png) + +A graph is defined by a yaml file ( e.g. [map.yaml in spot_behavior_manager_demo](./spot_behavior_manager_demo/config/map.yaml) ) +Please see nodes and edges format section below. + +Knowledge representation and execution process of behaviors are separated from actual behavior implementation. +The iplementation of former is in spot_behavior_manager and spot_behavior_manager_demo, but actual behaviors like walk and elevator are in spot_basic_behaviors. +behavior_manager_demo node will dynamically load each behaviors defined in map.yaml so you can easilly add your behavior without editing these core implementation. +Please see spot_basic_behaviors package for behavior examples. + +## How to use it + +To run demo, please make Spot stand in front of the fiducial in 73B2 and run + +```bash +roslaunch spot_behavior_manager_demo demo.launch +``` + +```bash +$ rostopic pub -1 /spot_behavior_manager_demo/lead_person/goal spot_behavior_manager_msgs/LeadPersonActionGoal "header: + seq: 0 + stamp: + secs: 0 + nsecs: 0 + frame_id: '' +goal_id: + stamp: + secs: 0 + nsecs: 0 + id: '' +goal: + target_node_id: 'eng2_2FElevator'" +``` + +Then Spot will go to 2FElevator of eng2 by walk behavior and elevator behavior implemented in spot_basic_behaviors packages. + +https://user-images.githubusercontent.com/9410362/124338016-aad25380-dbe0-11eb-962f-b9a27e1e08cb.mp4 + +For more details, please see [spot_behavior_manager](./spot_behavior_manager), [spot_behavior_manager_demo](./spot_behavior_manager_demo) and each behavior documentation. (e.g. [spot_basic_behaviors](./spot_basic_behaviors) ) diff --git a/jsk_spot_robot/jsk_spot_behaviors/spot_basic_behaviors/CMakeLists.txt b/jsk_spot_robot/jsk_spot_behaviors/spot_basic_behaviors/CMakeLists.txt new file mode 100644 index 0000000000..c18b7735af --- /dev/null +++ b/jsk_spot_robot/jsk_spot_behaviors/spot_basic_behaviors/CMakeLists.txt @@ -0,0 +1,9 @@ +cmake_minimum_required(VERSION 3.0.2) +project(spot_basic_behaviors) + +find_package(catkin REQUIRED) + +catkin_python_setup() + +catkin_package( +) diff --git a/jsk_spot_robot/jsk_spot_behaviors/spot_basic_behaviors/README.md b/jsk_spot_robot/jsk_spot_behaviors/spot_basic_behaviors/README.md new file mode 100644 index 0000000000..fa341940b3 --- /dev/null +++ b/jsk_spot_robot/jsk_spot_behaviors/spot_basic_behaviors/README.md @@ -0,0 +1,124 @@ +# spot_basic_behaviors + +This package includes exaple implementation of base_behavior for spot_behavior_manager. + +## walk behavior + +This behavior enables Spot to walk a specified route in a autowalk data from a given waypoint to another waypoint. + +behavior name: `spot_basic_behaviors.walk_behavior.WalkBehavior` + +https://user-images.githubusercontent.com/9410362/124337233-896f6880-dbdc-11eb-9588-30d4a5193bbd.mp4 + +### Required Configuration + +Before using this behavior, belows are required. + +- autowalk data containing a route for the behavior +- edge and nodes configuration for the behavior + +#### Edge + +Fields below are required in args of a edge + +- `graph`: path to autowalk data + +e.g. + +```yaml +- from: 'eng2_73B2' + to: 'eng2_73A2' + behavior_type: 'spot_basic_behavior.walk_behavior.WalkBehavior' + cost: 10 + args: + graph: '$(find spot_autowalk_data)/autowalk/eng2_73B2_to_7FElevator.walk' +``` + +#### Start and End Node + +Fields below are required for nodes + +- `waypoints_on_graph`: list of dict. each dict has `graph`, `id`, `localization_method` + + `graph`: path to a autowalk data + + `id`: waypoint id of the start node in the graph + + `localization_method`: localization_method to be used when starting autowalk, this must be 'fiducial' or 'waypoint' + +e.g. + +```yaml + 'eng2_73B2': + waypoints_on_graph: + - graph: '$(find spot_autowalk_data)/autowalk/eng2_73B2_to_7FElevator.walk' + id: 'dyed-bat-t00VKo5XixLihCvpsZPRqw==' + localization_method: 'fiducial' +``` + +## elevator behavior + +This behavior enables Spot to use an elevator to move to anothor floor. + +behavior name: `spot_basic_behaviors.elevator_behavior.ElevatorBehavior` + +https://user-images.githubusercontent.com/9410362/124337679-cd636d00-dbde-11eb-8db9-c5fedfda4ad9.mp4 + +### Required Configuration + +Before using this behavior, belows are required. + +- autowalk data containing a route for the behavior. +- apriltag pose information for elevator door frame detection. +- edge and nodes configuration for the behavior + +#### autowalk data + +To use this behavior, you need to record autowalk data while Spot riding on the elevator and getting off. + +Here is an example. + +![124134162-86318b00-dabd-11eb-86e3-092fcc5e8719](https://user-images.githubusercontent.com/9410362/124337765-364ae500-dbdf-11eb-83d3-a99e4025102e.png) + +#### apriltag pose information + +To use this behavior, an Fiducial (april tag) must be placed on the wall near the elevator doors. +And for elevator door opening and closing detection, transform from elevator foor frame ( center point on the ground ) to the fiducial is required. +Please see https://github.com/sktometometo/jsk_robot/blob/b166ef04cb954b175bedd5653af808be35e42121/jsk_spot_robot/jsk_spot_behaviors/spot_basic_behaviors/config/apriltag/tags.yaml#L44-L54 for examples + +#### Edge + +Fields below are required in args of a edge + +- `graph`: path to autowalk data +- `rest_waypoint_id`: waypoint of rest position in a elevator + +e.g. + +```yaml +- from: 'eng2_7FElevator' + to: 'eng2_2FElevator' + behavior_type: 'spot_basic_behavior.elevator_behavior.ElevatorBehavior' + cost: 10 + args: + graph: '$(find spot_autowalk_data)/autowalk/eng2_elevator_7FElevator_to_2FElevator.walk' + rest_waypoint_id: 'unsaid-collie-jvatvS.7KX9jpzQz61GL4A==' +``` + +#### Start and End Node + +Fields below are required for nodes + +- `waypoints_on_graph`: list of dict. each dict has `graph`, `id`, `localization_method` + + `graph`: path to a autowalk data + + `id`: waypoint id of the start node in the graph + + `localization_method`: localization_method to be used when starting autowalk, this must be 'fiducial' or 'waypoint' +- `switchbot_device`: switchbot device name for elevator button + +e.g. + +```yaml + 'eng2_7FElevator': + waypoints_on_graph: + - graph: '$(find spot_autowalk_data)/autowalk/eng2_elevator_7FElevator_to_2FElevator.walk' + id: 'sly-chetah-IZ4pVY7vrqO36OoKCYk9Zg==' + localization_method: 'fiducial' + switchbot_device: '/eng2/7f/elevator/down/button' +``` diff --git a/jsk_spot_robot/jsk_spot_behaviors/spot_basic_behaviors/config/apriltag/settings.yaml b/jsk_spot_robot/jsk_spot_behaviors/spot_basic_behaviors/config/apriltag/settings.yaml new file mode 100644 index 0000000000..a603cfdf5d --- /dev/null +++ b/jsk_spot_robot/jsk_spot_behaviors/spot_basic_behaviors/config/apriltag/settings.yaml @@ -0,0 +1,9 @@ +tag_family: 'tag36h11' # options: tagStandard52h13, tagStandard41h12, tag36h11, tag25h9, tag16h5, tagCustom48h12, tagCircle21h7, tagCircle49h12 +tag_threads: 2 # default: 2 +tag_decimate: 1.0 # default: 1.0 +tag_blur: 0.0 # default: 0.0 +tag_refine_edges: 1 # default: 1 +tag_debug: 0 # default: 0 +max_hamming_dist: 2 # default: 2 (Tunable parameter with 2 being a good choice - values >=3 consume large amounts of memory. Choose the largest value possible.) +# Other parameters +publish_tf: true # default: false diff --git a/jsk_spot_robot/jsk_spot_behaviors/spot_basic_behaviors/config/apriltag/tags.yaml b/jsk_spot_robot/jsk_spot_behaviors/spot_basic_behaviors/config/apriltag/tags.yaml new file mode 100644 index 0000000000..f233c51bc3 --- /dev/null +++ b/jsk_spot_robot/jsk_spot_behaviors/spot_basic_behaviors/config/apriltag/tags.yaml @@ -0,0 +1,54 @@ +# # Definitions of tags to detect +# +# ## General remarks +# +# - All length in meters +# - Ellipsis (...) signifies that the previous element can be repeated multiple times. +# +# ## Standalone tag definitions +# ### Remarks +# +# - name is optional +# +# ### Syntax +# +# standalone_tags: +# [ +# {id: ID, size: SIZE, name: NAME}, +# ... +# ] +standalone_tags: + [ + ] +# ## Tag bundle definitions +# ### Remarks +# +# - name is optional +# - x, y, z have default values of 0 thus they are optional +# - qw has default value of 1 and qx, qy, qz have default values of 0 thus they are optional +# +# ### Syntax +# +# tag_bundles: +# [ +# { +# name: 'CUSTOM_BUNDLE_NAME', +# layout: +# [ +# {id: ID, size: SIZE, x: X_POS, y: Y_POS, z: Z_POS, qw: QUAT_W_VAL, qx: QUAT_X_VAL, qy: QUAT_Y_VAL, qz: QUAT_Z_VAL}, +# ... +# ] +# }, +# ... +# ] +tag_bundles: + [ + { + name: 'elevator_door_frame_raw', + layout: + [ + {id: 219, size: 0.146, x: -1.7, y: -1.0, z: 0.6, qx: 0.0, qy: 0.7071067811865475, qz: 0.7071067811865476, qw: 0.0, }, + {id: 211, size: 0.146, x: -1.7, y: -1.0, z: 0.4, qx: 0.0, qy: 0.7071067811865475, qz: 0.7071067811865476, qw: 0.0, }, + ] + } + ] diff --git a/jsk_spot_robot/jsk_spot_behaviors/spot_basic_behaviors/config/switchbot_ros/token.yaml b/jsk_spot_robot/jsk_spot_behaviors/spot_basic_behaviors/config/switchbot_ros/token.yaml new file mode 100644 index 0000000000..bfc61225f6 --- /dev/null +++ b/jsk_spot_robot/jsk_spot_behaviors/spot_basic_behaviors/config/switchbot_ros/token.yaml @@ -0,0 +1 @@ +token: XXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXX diff --git a/jsk_spot_robot/jsk_spot_behaviors/spot_basic_behaviors/launch/elevator_detection.launch b/jsk_spot_robot/jsk_spot_behaviors/spot_basic_behaviors/launch/elevator_detection.launch new file mode 100644 index 0000000000..4693e60ca2 --- /dev/null +++ b/jsk_spot_robot/jsk_spot_behaviors/spot_basic_behaviors/launch/elevator_detection.launch @@ -0,0 +1,213 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + max_duration: 5.0 + timeout_duration: 0.05 + timer_duration: 0.1 + reference_frame_id: elevator_door_frame_raw + output_frame_id: elevator_door_frame + fixed_frame_id: odom + + + + + + + + approximate_sync: true + output_frame: body + input_topics: + - $(arg TOPIC_DEPTH_FRONT_RIGHT)/points + - $(arg TOPIC_DEPTH_FRONT_LEFT)/points + - $(arg TOPIC_DEPTH_RIGHT)/points + - $(arg TOPIC_DEPTH_LEFT)/points + - $(arg TOPIC_DEPTH_BACK)/points + + + + + + + + initial_pos: [0, 0, 0.5] + initial_rot: [0, 0, 0] + dimension_x: 0.5 + dimension_y: 0.5 + dimension_z: 0.8 + frame_id: elevator_door_frame + + + + + + + + + + + + + + diff --git a/jsk_spot_robot/jsk_spot_behaviors/spot_basic_behaviors/package.xml b/jsk_spot_robot/jsk_spot_behaviors/spot_basic_behaviors/package.xml new file mode 100644 index 0000000000..a42abf012d --- /dev/null +++ b/jsk_spot_robot/jsk_spot_behaviors/spot_basic_behaviors/package.xml @@ -0,0 +1,16 @@ + + + spot_basic_behaviors + 1.1.0 + The spot_basic_behaviors package + + Kei Okada + Koki Shinjo + Koki Shinjo + BSD + + catkin + + + + diff --git a/jsk_spot_robot/jsk_spot_behaviors/spot_basic_behaviors/setup.py b/jsk_spot_robot/jsk_spot_behaviors/spot_basic_behaviors/setup.py new file mode 100644 index 0000000000..754694ff04 --- /dev/null +++ b/jsk_spot_robot/jsk_spot_behaviors/spot_basic_behaviors/setup.py @@ -0,0 +1,9 @@ +from setuptools import setup +from catkin_pkg.python_setup import generate_distutils_setup + +d = generate_distutils_setup( + packages=['spot_basic_behaviors'], + package_dir={'': 'src'} + ) + +setup(**d) diff --git a/jsk_spot_robot/jsk_spot_behaviors/spot_basic_behaviors/src/spot_basic_behaviors/__init__.py b/jsk_spot_robot/jsk_spot_behaviors/spot_basic_behaviors/src/spot_basic_behaviors/__init__.py new file mode 100644 index 0000000000..077d19e4ad --- /dev/null +++ b/jsk_spot_robot/jsk_spot_behaviors/spot_basic_behaviors/src/spot_basic_behaviors/__init__.py @@ -0,0 +1,2 @@ +import spot_basic_behaviors.walk_behavior as walk_behavior +import spot_basic_behaviors.elevator_behavior as elevator_behavior diff --git a/jsk_spot_robot/jsk_spot_behaviors/spot_basic_behaviors/src/spot_basic_behaviors/elevator_behavior.py b/jsk_spot_robot/jsk_spot_behaviors/spot_basic_behaviors/src/spot_basic_behaviors/elevator_behavior.py new file mode 100644 index 0000000000..3922e5df9f --- /dev/null +++ b/jsk_spot_robot/jsk_spot_behaviors/spot_basic_behaviors/src/spot_basic_behaviors/elevator_behavior.py @@ -0,0 +1,189 @@ +# -*- coding: utf-8 -*- + +from spot_behavior_manager.base_behavior import BaseBehavior + +import actionlib +import roslaunch +import rospkg +import rospy + +from switchbot_ros.msg import SwitchBotCommandGoal, SwitchBotCommandAction +from sensor_msgs.msg import PointCloud2 + +class ElevatorBehavior(BaseBehavior): + + def door_point_cloud_callback(self, msg): + if len(msg.data) == 0: + self.door_is_open = True + else: + self.door_is_open = False + + def run_initial(self, start_node, end_node, edge, pre_edge ): + + rospy.logdebug('run_initial() called') + + # launch recognition launch + uuid = roslaunch.rlutil.get_or_generate_uuid(None, False) + roslaunch_path = rospkg.RosPack().get_path('spot_basic_behaviors') +\ + '/launch/elevator_detection.launch' + roslaunch_cli_args = [roslaunch_path] + roslaunch_file = roslaunch.rlutil.resolve_launch_arguments(roslaunch_cli_args) + self.roslaunch_parent = roslaunch.parent.ROSLaunchParent( + uuid, + roslaunch_file + ) + self.roslaunch_parent.start() + + # value for door openring checker + self.door_is_open = False + self.subscriber_door_check = None + + # value for switchbot + self.action_client_switchbot = actionlib.SimpleActionClient( + '/switchbot_ros/switch', + SwitchBotCommandAction + ) + + def run_main(self, start_node, end_node, edge, pre_edge ): + + rospy.logdebug('run_main() called') + + graph_name = edge.properties['graph'] + start_id = filter( + lambda x: x['graph'] == graph_name, + start_node.properties['waypoints_on_graph'] + )[0]['id'] + end_id = filter( + lambda x: x['graph'] == graph_name, + end_node.properties['waypoints_on_graph'] + )[0]['id'] + rest_waypoint_id = edge.properties['rest_waypoint_id'] + localization_method = filter( + lambda x: x['graph'] == graph_name, + start_node.properties['waypoints_on_graph'] + )[0]['localization_method'] + + # graph uploading and localization + if pre_edge is not None and \ + graph_name == pre_edge.properties['graph']: + rospy.loginfo('graph upload and localization skipped.') + else: + # Upload + ret = self.spot_client.upload_graph(graph_name) + if ret[0]: + rospy.loginfo('graph {} uploaded.'.format(graph_name)) + else: + rospy.logerr('graph uploading failed: {}'.format(ret[1])) + return False + # Localization + if localization_method == 'fiducial': + ret = self.spot_client.set_localization_fiducial() + elif localization_method == 'waypoint': + ret = self.spot_client.set_localization_waypoint(start_id) + else: + ret = (False,'Unknown localization method') + if ret[0]: + rospy.loginfo('robot is localized on the graph.') + else: + rospy.logwarn('Localization failed: {}'.format(ret[1])) + return False + + # start door opening check from outside + self.subscriber_door_check = rospy.Subscriber( + '/spot_recognition/elevator_door_points', + PointCloud2, + self.door_point_cloud_callback) + + # push button with switchbot + rospy.loginfo('calling elevator when riding...') + if not self.action_client_switchbot.wait_for_server(rospy.Duration(10)): + rospy.logerr('switchbot server seems to fail.') + return False + else: + switchbot_goal = SwitchBotCommandGoal() + switchbot_goal.device_name = start_node.properties['switchbot_device'] + switchbot_goal.command = 'press' + self.action_client_switchbot.send_goal(switchbot_goal) + self.action_client_switchbot.wait_for_result() + result = self.action_client_switchbot.get_result() + rospy.loginfo('switchbot result: {}'.format(result)) + if not result.done: + rospy.logerr('switchbot calling failed.') + return False + rospy.loginfo('elevator calling when riding on has succeeded') + + # wait for elevator + rate = rospy.Rate(2) + while not rospy.is_shutdown(): + rate.sleep() + if self.door_is_open: + break + rospy.loginfo('elevator door opened.') + self.subscriber_door_check.unregister() + self.subscriber_door_check = None + + # start navigation to rest point + rate = rospy.Rate(10) + self.spot_client.navigate_to( rest_waypoint_id, blocking=True) + result = self.spot_client.get_navigate_to_result() + ## recovery when riding on + if not result.success: + rospy.logwarn('Navigation failed when riding on') + self.spot_client.navigate_to( start_id, blocking=True) + self.spot_client.wait_for_navigate_to_result() + return result.success + + # call elevator from destination floor + rospy.loginfo('calling elevator when getting off...') + if not self.action_client_switchbot.wait_for_server(rospy.Duration(10)): + rospy.logerr('switchbot server seems to fail.') + return False + else: + switchbot_goal = SwitchBotCommandGoal() + switchbot_goal.device_name = end_node.properties['switchbot_device'] + switchbot_goal.command = 'press' + self.action_client_switchbot.send_goal(switchbot_goal) + self.action_client_switchbot.wait_for_result() + result = self.action_client_switchbot.get_result() + rospy.loginfo('switchbot result: {}'.format(result)) + if not result.done: + rospy.logerr('switchbot calling failed.') + return False + rospy.loginfo('elevator calling when getting off has succeeded') + + # start door openning check from inside + self.subscriber_door_check = rospy.Subscriber( + '/spot_recognition/elevator_door_points', + PointCloud2, + self.door_point_cloud_callback) + + # check if the door is closed + rate = rospy.Rate(2) + while not rospy.is_shutdown(): + rate.sleep() + if not self.door_is_open: + break + rospy.loginfo('elevator door closed') + + # check if the door is open + rate = rospy.Rate(2) + while not rospy.is_shutdown(): + rate.sleep() + if self.door_is_open: + break + rospy.loginfo('elevator door opened') + + # get off the elevator + self.spot_client.navigate_to(end_id, blocking=True) + result = self.spot_client.get_navigate_to_result() + + return result.success + + def run_final(self, start_node, end_node, edge, pre_edge ): + + rospy.logdebug('run_finalize() called') + + if self.subscriber_door_check != None: + self.subscriber_door_check.unregister() + self.subscriber_door_check = None + self.roslaunch_parent.shutdown() diff --git a/jsk_spot_robot/jsk_spot_behaviors/spot_basic_behaviors/src/spot_basic_behaviors/walk_behavior.py b/jsk_spot_robot/jsk_spot_behaviors/spot_basic_behaviors/src/spot_basic_behaviors/walk_behavior.py new file mode 100644 index 0000000000..e9efc34f97 --- /dev/null +++ b/jsk_spot_robot/jsk_spot_behaviors/spot_basic_behaviors/src/spot_basic_behaviors/walk_behavior.py @@ -0,0 +1,79 @@ +# -*- coding: utf-8 -*- + +from spot_behavior_manager.base_behavior import BaseBehavior + +import rospy + +class WalkBehavior(BaseBehavior): + + def run_initial(self, start_node, end_node, edge, pre_edge ): + + rospy.logdebug('run_initial() called') + + def run_main(self, start_node, end_node, edge, pre_edge ): + + rospy.logdebug('run_main() called') + + graph_name = edge.properties['graph'] + start_id = filter( + lambda x: x['graph'] == graph_name, + start_node.properties['waypoints_on_graph'] + )[0]['id'] + end_id = filter( + lambda x: x['graph'] == graph_name, + end_node.properties['waypoints_on_graph'] + )[0]['id'] + localization_method = filter( + lambda x: x['graph'] == graph_name, + start_node.properties['waypoints_on_graph'] + )[0]['localization_method'] + + # graph uploading and localization + if pre_edge is not None and \ + graph_name == pre_edge.properties['graph']: + rospy.loginfo('graph upload and localization skipped.') + else: + # Upload + ret = self.spot_client.upload_graph(graph_name) + if ret[0]: + rospy.loginfo('graph {} uploaded.'.format(graph_name)) + else: + rospy.logerr('graph uploading failed: {}'.format(ret[1])) + return False + # Localization + if localization_method == 'fiducial': + ret = self.spot_client.set_localization_fiducial() + elif localization_method == 'waypoint': + ret = self.spot_client.set_localization_waypoint(start_id) + else: + ret = (False,'Unknown localization method') + if ret[0]: + rospy.loginfo('robot is localized on the graph.') + else: + rospy.logwarn('Localization failed: {}'.format(ret[1])) + return False + + # start navigation + success = False + rate = rospy.Rate(10) + self.sound_client.say('移動します',blocking=True) + self.spot_client.navigate_to( end_id, blocking=False) + while not rospy.is_shutdown(): + rate.sleep() + if self.spot_client.wait_for_navigate_to_result(rospy.Duration(0.1)): + result = self.spot_client.get_navigate_to_result() + success = result.success + rospy.loginfo('result: {}'.format(result)) + break + + # recovery on failure + if not success: + self.sound_client.say('失敗したので元に戻ります', blocking=True) + self.spot_client.navigate_to( start_id, blocking=True) + self.spot_client.wait_for_navigate_to_result() + + return success + + def run_final(self, start_node, end_node, edge, pre_edge ): + + rospy.logdebug('run_finalize() called') diff --git a/jsk_spot_robot/jsk_spot_behaviors/spot_behavior_manager/CMakeLists.txt b/jsk_spot_robot/jsk_spot_behaviors/spot_behavior_manager/CMakeLists.txt new file mode 100644 index 0000000000..2e3c06f211 --- /dev/null +++ b/jsk_spot_robot/jsk_spot_behaviors/spot_behavior_manager/CMakeLists.txt @@ -0,0 +1,9 @@ +cmake_minimum_required(VERSION 3.0.2) +project(spot_behavior_manager) + +find_package(catkin REQUIRED) + +catkin_python_setup() + +catkin_package( +) diff --git a/jsk_spot_robot/jsk_spot_behaviors/spot_behavior_manager/package.xml b/jsk_spot_robot/jsk_spot_behaviors/spot_behavior_manager/package.xml new file mode 100644 index 0000000000..d37580ccb5 --- /dev/null +++ b/jsk_spot_robot/jsk_spot_behaviors/spot_behavior_manager/package.xml @@ -0,0 +1,39 @@ + + + spot_behavior_manager + 1.1.0 + The spot_behavior_manager package + + Kei Okada + Koki Shinjo + Koki Shinjo + + BSD + + catkin + + rospy + + actionlib + networkx + rospkg + rospy + roslaunch + sound_play + spot_behavior_manager_msgs + spot_ros_client + std_msgs + + actionlib + networkx + rospkg + rospy + roslaunch + sound_play + spot_behavior_manager_msgs + spot_ros_client + std_msgs + + + + diff --git a/jsk_spot_robot/jsk_spot_behaviors/spot_behavior_manager/setup.py b/jsk_spot_robot/jsk_spot_behaviors/spot_behavior_manager/setup.py new file mode 100644 index 0000000000..c3eab0d109 --- /dev/null +++ b/jsk_spot_robot/jsk_spot_behaviors/spot_behavior_manager/setup.py @@ -0,0 +1,9 @@ +from setuptools import setup +from catkin_pkg.python_setup import generate_distutils_setup + +d = generate_distutils_setup( + packages=['spot_behavior_manager'], + package_dir={'': 'src'} + ) + +setup(**d) diff --git a/jsk_spot_robot/jsk_spot_behaviors/spot_behavior_manager/src/spot_behavior_manager/__init__.py b/jsk_spot_robot/jsk_spot_behaviors/spot_behavior_manager/src/spot_behavior_manager/__init__.py new file mode 100644 index 0000000000..defc635b8b --- /dev/null +++ b/jsk_spot_robot/jsk_spot_behaviors/spot_behavior_manager/src/spot_behavior_manager/__init__.py @@ -0,0 +1,2 @@ +import spot_behavior_manager.support_behavior_graph as support_behavior_graph +import spot_behavior_manager.base_behavior as base_behavior diff --git a/jsk_spot_robot/jsk_spot_behaviors/spot_behavior_manager/src/spot_behavior_manager/base_behavior.py b/jsk_spot_robot/jsk_spot_behaviors/spot_behavior_manager/src/spot_behavior_manager/base_behavior.py new file mode 100644 index 0000000000..925e2e8384 --- /dev/null +++ b/jsk_spot_robot/jsk_spot_behaviors/spot_behavior_manager/src/spot_behavior_manager/base_behavior.py @@ -0,0 +1,47 @@ +import rospy +import roslaunch + + +def load_behavior_class(class_string): + package_name = class_string.split('.')[0] + module_name = class_string.split('.')[1] + class_name = class_string.split('.')[2] + behavior_class = getattr(getattr(__import__(package_name),module_name),class_name) + return behavior_class + + +class BaseBehavior(object): + + def __init__(self, spot_client, sound_client): + + self.spot_client = spot_client + self.sound_client = sound_client + + def run_initial(self, start_node, end_node, edge, pre_edge ): + pass + + def run_main(self, start_node, end_node, edge, pre_edge ): + pass + + def run_final(self, start_node, end_node, edge, pre_edge ): + pass + + +class SimpleBehavior(BaseBehavior): + + def run_initial(self, start_node, end_node, edge, pre_edge ): + + rospy.loginfo('__run_initial() called') + + def run_main(self, start_node, end_node, edge, pre_edge ): + + rospy.loginfo('__run_main() called') + rospy.loginfo('start_node: {}'.format(start_node)) + rospy.loginfo('end_node: {}'.format(end_node)) + rospy.loginfo('edge: {}'.format(edge)) + rospy.loginfo('pre_edge: {}'.format(pre_edge)) + return True + + def run_final(self, start_node, end_node, edge, pre_edge ): + + rospy.loginfo('__run_finalize() called') diff --git a/jsk_spot_robot/jsk_spot_behaviors/spot_behavior_manager/src/spot_behavior_manager/behavior_manager_node.py b/jsk_spot_robot/jsk_spot_behaviors/spot_behavior_manager/src/spot_behavior_manager/behavior_manager_node.py new file mode 100644 index 0000000000..10f71292b7 --- /dev/null +++ b/jsk_spot_robot/jsk_spot_behaviors/spot_behavior_manager/src/spot_behavior_manager/behavior_manager_node.py @@ -0,0 +1,155 @@ +# -*- coding: utf-8 -*- + +import actionlib +import rospy +import roslaunch + +from sound_play.libsoundplay import SoundClient +from spot_ros_client.libspotros import SpotRosClient + +from spot_behavior_manager.support_behavior_graph import SupportBehaviorGraph +from spot_behavior_manager.base_behavior import BaseBehavior, load_behavior_class + +from std_msgs.msg import String +from spot_behavior_manager_msgs.msg import LeadPersonAction, LeadPersonFeedback, LeadPersonResult +from spot_behavior_manager_msgs.srv import ResetCurrentNode, ResetCurrentNodeResponse + + +class BehaviorManagerNode(object): + + def __init__(self): + + # navigation dictonary + raw_edges = rospy.get_param('~map/edges') + raw_nodes = rospy.get_param('~map/nodes') + self.graph = SupportBehaviorGraph(raw_edges,raw_nodes) + self.current_node_id = rospy.get_param('~initial_node_id') + self.pre_edge = None + + # action clients + self.spot_client = SpotRosClient(); + self.sound_client = SoundClient( + blocking=False, + sound_action='/robotsound_jp', + sound_topic='/robotsound_jp' + ) + + # publisher + self.pub_current_node_id = rospy.Publisher('~current_node_id',String,queue_size=1) + + # reset service + self.service_reset_current_node_id = rospy.Service( + '~reset_current_node_id', + ResetCurrentNode, + self.handler_reset_current_node_id + ) + + # + roslaunch.pmon._init_signal_handlers() + + # action server + self.server_lead_person = actionlib.SimpleActionServer( + '~lead_person', + LeadPersonAction, + execute_cb=self.handler_lead_person, + auto_start=False + ) + self.server_lead_person.start() + + rospy.loginfo('Initialized!') + + + def run(self): + + rate = rospy.Rate(1) + while not rospy.is_shutdown(): + rate.sleep() + self.pub_current_node_id.publish(String(data=self.current_node_id)) + + + def handler_reset_current_node_id(self, req): + + rospy.loginfo('current_node_id is reset to {}'.format(req.current_node_id)) + self.current_node_id = req.current_node_id + self.pre_edge = None + return ResetCurrentNodeResponse(success=True) + + + def handler_lead_person(self, goal): + + rospy.loginfo('Lead Action started. goal: {}'.format(goal)) + + # path calculation + path = self.graph.calcPath( self.current_node_id, goal.target_node_id ) + if path is None: + rospy.logerr('No path from {} to {}'.format(self.current_node_id,goal.target_node_id)) + self.sound_client.say('パスが見つかりませんでした') + result = LeadPersonResult(success=False) + self.server_lead_person.set_aborted(result) + return + + # navigation of edges in the path + self.sound_client.say('目的地に向かいます',blocking=True) + for edge in path: + rospy.loginfo('Navigating Edge {}...'.format(edge)) + try: + if self.navigate_edge(edge): + rospy.loginfo('Edge {} succeeded.'.format(edge)) + self.current_node_id = edge.node_id_to + self.pre_edge = edge + else: + rospy.logerr('Edge {} failed'.format(edge)) + self.sound_client.say('目的地に到達できませんでした',blocking=True) + result = LeadPersonResult(success=False) + self.server_lead_person.set_aborted(result) + return + except Exception as e: + rospy.logerr('Got an error while navigating edge {}: {}'.format(edge, e)) + self.sound_client.say('エラーが発生しました',blocking=True) + result = LeadPersonResult(success=False) + self.server_lead_person.set_aborted(result) + return + + self.sound_client.say('目的地に到着しました.',blocking=True) + + result = LeadPersonResult(success=True) + self.server_lead_person.set_succeeded(result) + return + + + def navigate_edge(self, edge): + + # start node id validation + if self.current_node_id != edge.node_id_from: + rospy.logwarn( + 'current_node_id {} does not match node_id_from of edge ({})'.format( + self.current_node_id, + edge.node_id_from) + ) + return False + + # load behavior class + try: + behavior_class = load_behavior_class(edge.behavior_type) + behavior = behavior_class( + self.spot_client, + self.sound_client + ) + except Exception as e: + rospy.logerr('Failed to load and initialize behavior class: {}'.format(e)) + self.sound_client.say('行動クラスを読み込めませんでした',blocking=True) + return False + + node_from = self.graph.nodes[edge.node_id_from] + node_to = self.graph.nodes[edge.node_id_to] + + try: + behavior.run_initial( node_from, node_to, edge, self.pre_edge ) + success = behavior.run_main( node_from, node_to, edge, self.pre_edge ) + behavior.run_final( node_from, node_to, edge, self.pre_edge ) + except Exception as e: + rospy.logerr('Got error while running a behavior: {}'.format(e)) + self.sound_client.say('行動中にエラーが発生しました',blocking=True) + return False + + return success diff --git a/jsk_spot_robot/jsk_spot_behaviors/spot_behavior_manager/src/spot_behavior_manager/support_behavior_graph.py b/jsk_spot_robot/jsk_spot_behaviors/spot_behavior_manager/src/spot_behavior_manager/support_behavior_graph.py new file mode 100644 index 0000000000..d991c78a60 --- /dev/null +++ b/jsk_spot_robot/jsk_spot_behaviors/spot_behavior_manager/src/spot_behavior_manager/support_behavior_graph.py @@ -0,0 +1,64 @@ +import networkx as nx + +class GraphEdge: + + def __init__(self, + node_id_from, + node_id_to, + behavior_type, + cost, + properties + ): + self.node_id_from = node_id_from + self.node_id_to = node_id_to + self.behavior_type = behavior_type + self.cost = cost + self.properties = properties + +class GraphNode: + + def __init__(self, + node_id, + properties + ): + self.node_id = node_id + self.properties = properties + +class SupportBehaviorGraph: + + def __init__(self, raw_edges=[], raw_nodes={}): + + self.edges = {} + self.nodes = {} + self.network = nx.DiGraph() + + edges = [] + for raw_edge in raw_edges: + edges.append( GraphEdge( raw_edge['from'], + raw_edge['to'], + raw_edge['behavior_type'], + int(raw_edge['cost']), + raw_edge['args'] )) + nodes = {} + for key, raw_node in raw_nodes.items(): + nodes[key] = GraphNode( key, raw_node ) + + for key, node in nodes.items(): + self.nodes[key] = node + for edge in edges: + self.edges[edge.node_id_from,edge.node_id_to] = edge + self.network.add_edge( + edge.node_id_from, + edge.node_id_to, + weight=edge.cost) + + def calcPath(self, node_id_from, node_id_to): + + try: + node_id_list = nx.shortest_path( self.network, node_id_from, node_id_to ) + except nx.NetworkXNoPath as e: + return None + path = [] + for index in range(len(node_id_list)-1): + path.append(self.edges[node_id_list[index],node_id_list[index+1]]) + return path diff --git a/jsk_spot_robot/jsk_spot_behaviors/spot_behavior_manager_demo/CMakeLists.txt b/jsk_spot_robot/jsk_spot_behaviors/spot_behavior_manager_demo/CMakeLists.txt new file mode 100644 index 0000000000..c410beb1e5 --- /dev/null +++ b/jsk_spot_robot/jsk_spot_behaviors/spot_behavior_manager_demo/CMakeLists.txt @@ -0,0 +1,7 @@ +cmake_minimum_required(VERSION 3.0.2) +project(spot_behavior_manager_demo) + +find_package(catkin REQUIRED) + +catkin_package( +) diff --git a/jsk_spot_robot/jsk_spot_behaviors/spot_behavior_manager_demo/README.md b/jsk_spot_robot/jsk_spot_behaviors/spot_behavior_manager_demo/README.md new file mode 100644 index 0000000000..22d855792f --- /dev/null +++ b/jsk_spot_robot/jsk_spot_behaviors/spot_behavior_manager_demo/README.md @@ -0,0 +1 @@ +# spot_behavior_manager_demo diff --git a/jsk_spot_robot/jsk_spot_behaviors/spot_behavior_manager_demo/config/map.yaml b/jsk_spot_robot/jsk_spot_behaviors/spot_behavior_manager_demo/config/map.yaml new file mode 100644 index 0000000000..17132c5167 --- /dev/null +++ b/jsk_spot_robot/jsk_spot_behaviors/spot_behavior_manager_demo/config/map.yaml @@ -0,0 +1,147 @@ +edges: + - from: 'eng2_73B2' + to: 'dummy' + behavior_type: 'spot_behavior_manager.base_behavior.SimpleBehavior' + cost: 10 + args: {} + - from: 'dummy' + to: 'eng2_73B2' + behavior_type: 'spot_behavior_manager.base_behavior.SimpleBehavior' + cost: 10 + args: {} + - from: 'eng2_73B2' + to: 'eng2_73A2' + behavior_type: 'spot_basic_behaviors.walk_behavior.WalkBehavior' + cost: 10 + args: + graph: '$(find spot_autowalk_data)/autowalk/eng2_73B2_to_7FElevator.walk' + - from: 'eng2_73A2' + to: 'eng2_73B2' + behavior_type: 'spot_basic_behaviors.walk_behavior.WalkBehavior' + cost: 10 + args: + graph: '$(find spot_autowalk_data)/autowalk/eng2_73B2_to_7FElevator.walk' + - from: 'eng2_73B2' + to: 'eng2_7FElevator' + behavior_type: 'spot_basic_behaviors.walk_behavior.WalkBehavior' + cost: 10 + args: + graph: '$(find spot_autowalk_data)/autowalk/eng2_73B2_to_7FElevator.walk' + - from: 'eng2_7FElevator' + to: 'eng2_73B2' + behavior_type: 'spot_basic_behaviors.walk_behavior.WalkBehavior' + cost: 10 + args: + graph: '$(find spot_autowalk_data)/autowalk/eng2_73B2_to_7FElevator.walk' + - from: 'eng2_7FElevator' + to: 'eng2_2FElevator' + behavior_type: 'spot_basic_behaviors.elevator_behavior.ElevatorBehavior' + cost: 10 + args: + graph: '$(find spot_autowalk_data)/autowalk/eng2_elevator_7FElevator_to_2FElevator.walk' + rest_waypoint_id: 'unsaid-collie-jvatvS.7KX9jpzQz61GL4A==' + - from: 'eng2_2FElevator' + to: 'eng2_7FElevator' + behavior_type: 'spot_basic_behaviors.elevator_behavior.ElevatorBehavior' + cost: 10 + args: + graph: '$(find spot_autowalk_data)/autowalk/eng2_elevator_7FElevator_to_2FElevator.walk' + rest_waypoint_id: 'unsaid-collie-jvatvS.7KX9jpzQz61GL4A==' + - from: 'eng2_2FElevator' + to: 'eng_TelephoneBox' + behavior_type: 'spot_basic_behaviors.walk_behavior.WalkBehavior' + cost: 10 + args: + graph: '$(find spot_autowalk_data)/autowalk/eng2_2FElevator_to_eng8_2FElevator.walk' + - from: 'eng_TelephoneBox' + to: 'eng2_2FElevator' + behavior_type: 'spot_basic_behaviors.walk_behavior.WalkBehavior' + cost: 10 + args: + graph: '$(find spot_autowalk_data)/autowalk/eng2_2FElevator_to_eng8_2FElevator.walk' + - from: 'eng8_Piloti_Center' + to: 'eng2_2FElevator' + behavior_type: 'spot_basic_behaviors.walk_behavior.WalkBehavior' + cost: 10 + args: + graph: '$(find spot_autowalk_data)/autowalk/eng2_2FElevator_to_eng8_2FElevator.walk' + - from: 'eng8_Piloti_Center' + to: 'eng_TelephoneBox' + behavior_type: 'spot_basic_behaviors.walk_behavior.WalkBehavior' + cost: 10 + args: + graph: '$(find spot_autowalk_data)/autowalk/eng2_2FElevator_to_eng8_2FElevator.walk' +nodes: + 'dummy': + name_en: 'dummy' + name_jp: 'ダミー' + 'eng2_73B2': + name_en: '73B2' + name_jp: '7 3 B 2' + waypoints_on_graph: + - graph: '$(find spot_autowalk_data)/autowalk/eng2_73B2_to_7FElevator.walk' + id: 'dyed-bat-t00VKo5XixLihCvpsZPRqw==' + localization_method: 'fiducial' + - graph: '$(find spot_autowalk_data)/autowalk/eng2_73B2_to_81C1.walk' + id: 'fuzzed-medusa-wyXWCEjmWas1kMo8EJW8Bw==' + localization_method: 'fiducial' + 'eng2_73A2': + name_en: '73A2' + name_jp: '7 3 A 2' + waypoints_on_graph: + - graph: '$(find spot_autowalk_data)/autowalk/eng2_73B2_to_7FElevator.walk' + id: 'affine-drake-Jfb0ZPtNcqcrq9yhnEl2DA==' + localization_method: 'fiducial' + 'eng2_7FElevator': + name_en: 'Elevator' + name_jp: 'エレベーター' + waypoints_on_graph: + - graph: '$(find spot_autowalk_data)/autowalk/eng2_73B2_to_7FElevator.walk' + id: 'looted-cougar-RWT2C0zLJXG.ezYnCuLUeQ==' + localization_method: 'fiducial' + - graph: '$(find spot_autowalk_data)/autowalk/eng2_7FElevator_to_2FElevator.walk' + id: 'larger-mudcat-0x3bt3Dif.1QU9JfXpyQPw==' + localization_method: 'fiducial' + - graph: '$(find spot_autowalk_data)/autowalk/eng2_elevator_7FElevator_to_2FElevator.walk' + id: 'sly-chetah-IZ4pVY7vrqO36OoKCYk9Zg==' + localization_method: 'fiducial' + switchbot_device: '/eng2/7f/elevator/down/button' + floor: 7 + 'eng2_2FElevator': + name_en: 'Elevator' + name_jp: 'エレベーター' + waypoints_on_graph: + - graph: '$(find spot_autowalk_data)/autowalk/eng2_7FElevator_to_2FElevator.walk' + id: 'rainy-collie-uwFW2KVYg2YCUi8FF6N00g==' + localization_method: 'fiducial' + - graph: '$(find spot_autowalk_data)/autowalk/eng2_2FElevator_to_2FEntrance.walk' + id: 'snaky-beagle-.fmRm2JflzlV6Dv5fTdnIQ==' + localization_method: 'fiducial' + - graph: '$(find spot_autowalk_data)/autowalk/eng2_2FElevator_to_MainGate.walk' + id: 'banner-oxen-UrEwTUbZYL0IQnY5TCFwrA==' + localization_method: 'fiducial' + - graph: '$(find spot_autowalk_data)/autowalk/eng2_elevator_7FElevator_to_2FElevator.walk' + id: 'shelfy-dassie-vK9Tf3cAQpdA.25gPJl8OQ==' + localization_method: 'fiducial' + - graph: '$(find spot_autowalk_data)/autowalk/eng2_2FElevator_to_eng8_2FElevator.walk' + id: 'inland-remora-FqtX8ScP+.inEd4xFYKq4A==' + localization_method: 'fiducial' + switchbot_device: '/eng2/2f/elevator/up/button' + floor: 2 + 'eng_TelephoneBox': + name_en: 'telephone box' + name_jp: '電話ボックス' + waypoints_on_graph: + - graph: '$(find spot_autowalk_data)/autowalk/eng2_2FElevator_to_eng8_2FElevator.walk' + id: 'slain-pika-VdtPz3lWcp5T+pEArUB3PA==' + localization_method: 'fiducial' + - graph: '$(find spot_autowalk_data)/autowalk/eng_TelephoneBox_to_HongoMainGate.walk' + id: 'aired-hyrax-BF+hXHaUufi.DLd3pEWm5Q==' + localization_method: 'fiducial' + 'eng8_Piloti_Center': + name_en: 'Piloti' + name_jp: 'ピロティ' + waypoints_on_graph: + - graph: '$(find spot_autowalk_data)/autowalk/eng2_2FElevator_to_eng8_2FElevator.walk' + id: 'azoic-lion-Iapbto7rzlzuFF0MvXdyPA==' + localization_method: 'fiducial' diff --git a/jsk_spot_robot/jsk_spot_behaviors/spot_behavior_manager_demo/launch/demo.launch b/jsk_spot_robot/jsk_spot_behaviors/spot_behavior_manager_demo/launch/demo.launch new file mode 100644 index 0000000000..b236a5a6d6 --- /dev/null +++ b/jsk_spot_robot/jsk_spot_behaviors/spot_behavior_manager_demo/launch/demo.launch @@ -0,0 +1,19 @@ + + + + + + + initial_node_id: 'eng2_73B2' + + + diff --git a/jsk_spot_robot/jsk_spot_behaviors/spot_behavior_manager_demo/node_scripts/demo.py b/jsk_spot_robot/jsk_spot_behaviors/spot_behavior_manager_demo/node_scripts/demo.py new file mode 100755 index 0000000000..cb1a90adc1 --- /dev/null +++ b/jsk_spot_robot/jsk_spot_behaviors/spot_behavior_manager_demo/node_scripts/demo.py @@ -0,0 +1,14 @@ +#!/usr/bin/env python +# -*- coding: utf-8 -*- + +import rospy +from spot_behavior_manager.behavior_manager_node import BehaviorManagerNode + +def main(): + + rospy.init_node('behavior_manager_node') + node = BehaviorManagerNode() + node.run() + +if __name__ == '__main__': + main() diff --git a/jsk_spot_robot/jsk_spot_behaviors/spot_behavior_manager_demo/package.xml b/jsk_spot_robot/jsk_spot_behaviors/spot_behavior_manager_demo/package.xml new file mode 100644 index 0000000000..c72c286a4d --- /dev/null +++ b/jsk_spot_robot/jsk_spot_behaviors/spot_behavior_manager_demo/package.xml @@ -0,0 +1,22 @@ + + + spot_behavior_manager_demo + 1.1.0 + The spot_behavior_manager_demo package + + Kei Okada + Koki Shinjo + Koki Shinjo + + BSD + + catkin + + rospy + spot_behavior_manager + spot_basic_behavior + spot_autowalk_data + + + + diff --git a/jsk_spot_robot/jsk_spot_behaviors/spot_behavior_manager_msgs/CMakeLists.txt b/jsk_spot_robot/jsk_spot_behaviors/spot_behavior_manager_msgs/CMakeLists.txt new file mode 100644 index 0000000000..e106338e3d --- /dev/null +++ b/jsk_spot_robot/jsk_spot_behaviors/spot_behavior_manager_msgs/CMakeLists.txt @@ -0,0 +1,22 @@ +cmake_minimum_required(VERSION 3.0.2) +project(spot_behavior_manager_msgs) + +find_package(catkin REQUIRED COMPONENTS genmsg actionlib actionlib_msgs) + + +add_action_files( + DIRECTORY action + FILES LeadPerson.action +) + +add_service_files( + DIRECTORY srv + FILES ResetCurrentNode.srv +) + +generate_messages(DEPENDENCIES actionlib_msgs) + +catkin_package( + LIBRARIES spot_behavior_manager_msgs + CATKIN_DEPENDS message_runtime +) diff --git a/jsk_spot_robot/jsk_spot_behaviors/spot_behavior_manager_msgs/action/LeadPerson.action b/jsk_spot_robot/jsk_spot_behaviors/spot_behavior_manager_msgs/action/LeadPerson.action new file mode 100644 index 0000000000..e660e68f01 --- /dev/null +++ b/jsk_spot_robot/jsk_spot_behaviors/spot_behavior_manager_msgs/action/LeadPerson.action @@ -0,0 +1,5 @@ +string target_node_id +--- +bool success +--- +string current_node_id diff --git a/jsk_spot_robot/jsk_spot_behaviors/spot_behavior_manager_msgs/package.xml b/jsk_spot_robot/jsk_spot_behaviors/spot_behavior_manager_msgs/package.xml new file mode 100644 index 0000000000..98f45f148c --- /dev/null +++ b/jsk_spot_robot/jsk_spot_behaviors/spot_behavior_manager_msgs/package.xml @@ -0,0 +1,25 @@ + + + spot_behavior_manager_msgs + 1.1.0 + The spot_behavior_manager_msgs package + + Kei Okada + Koki Shinjo + Koki Shinjo + + BSD + + catkin + + actionlib + actionlib_msgs + message_generation + + actionlib + actionlib_msgs + message_runtime + + + + diff --git a/jsk_spot_robot/jsk_spot_behaviors/spot_behavior_manager_msgs/srv/ResetCurrentNode.srv b/jsk_spot_robot/jsk_spot_behaviors/spot_behavior_manager_msgs/srv/ResetCurrentNode.srv new file mode 100644 index 0000000000..8fa01bd4c0 --- /dev/null +++ b/jsk_spot_robot/jsk_spot_behaviors/spot_behavior_manager_msgs/srv/ResetCurrentNode.srv @@ -0,0 +1,3 @@ +string current_node_id +--- +bool success diff --git a/jsk_spot_robot/jsk_spot_startup/package.xml b/jsk_spot_robot/jsk_spot_startup/package.xml index b3158f67c5..92af38e587 100644 --- a/jsk_spot_robot/jsk_spot_startup/package.xml +++ b/jsk_spot_robot/jsk_spot_startup/package.xml @@ -5,7 +5,9 @@ The jsk_spot_startup package Kei Okada + Koki Shinjo Koki Shinjo + BSD catkin diff --git a/jsk_spot_robot/spot_autowalk_data/CMakeLists.txt b/jsk_spot_robot/spot_autowalk_data/CMakeLists.txt new file mode 100644 index 0000000000..c150c49a71 --- /dev/null +++ b/jsk_spot_robot/spot_autowalk_data/CMakeLists.txt @@ -0,0 +1,9 @@ +cmake_minimum_required(VERSION 3.0.2) +project(spot_autowalk_data) + +find_package(catkin REQUIRED) + +add_custom_target(${PROJECT_NAME}_download_autowalk_data ALL COMMAND python$ENV{ROS_PYTHON_VERSION} ${PROJECT_SOURCE_DIR}/scripts/download_autowalk_data.py) + +catkin_package( +) diff --git a/jsk_spot_robot/spot_autowalk_data/README.md b/jsk_spot_robot/spot_autowalk_data/README.md new file mode 100644 index 0000000000..7e66b1d2d6 --- /dev/null +++ b/jsk_spot_robot/spot_autowalk_data/README.md @@ -0,0 +1,62 @@ +# spot_autowalk_data + +This package includes autowalk data downloaded from public Googledrive folder with jsk_data. + +## scripts + +### download_autowalk_data.py + +this scripts download autowalk data from google drive. this script will be run when catkin build. + +### view_map.py + +this script visualize autowalk data and each waypoint id + +## autowalk data in this packages + +### eng2_2FElevator_to_2FEntrance.walk + +![eng2_2FElevator_to_2FEntrance](https://user-images.githubusercontent.com/9410362/124133634-fbe92700-dabc-11eb-90d5-84b434729233.png) + +### eng2_2FElevator_to_MainGate + +![eng2_2FElevator_to_MainGate](https://user-images.githubusercontent.com/9410362/124133994-55e9ec80-dabd-11eb-8ba1-1bd118146244.png) + +### eng2_2Felevator_to_eng8_2FElevator + +![eng2_2FElevator_to_eng8_2FElevator](https://user-images.githubusercontent.com/9410362/124134033-600beb00-dabd-11eb-823c-483e75318f8b.png) + +### eng2_7FElevator_to_2FElevator + +![eng2_73B2_to_2FElevator](https://user-images.githubusercontent.com/9410362/124134079-6f8b3400-dabd-11eb-8c99-4fe616f84eee.png) + +### eng2_73B2_to_81C1 + +![eng2_73B2_to_81C1](https://user-images.githubusercontent.com/9410362/124134118-7b76f600-dabd-11eb-829b-c36f105d7051.png) + +### eng2_elevator_7FElevator_to_2FElevator + +![eng2_elevator_7FElevator_to_2FElevator](https://user-images.githubusercontent.com/9410362/124134162-86318b00-dabd-11eb-86e3-092fcc5e8719.png) + +### eng_TelephoneBox_to_HongoMainGate + +![eng_TelephoneBox_to_HongoMainGate](https://user-images.githubusercontent.com/9410362/124134207-90ec2000-dabd-11eb-988b-62ffbf98ca67.png) + +## How to add autowalk data in this package + +1. record a new autowalk data +2. archive and compress an autowalk data directory (e.g. `autowalk.walk`) to `autowalk.walk.tar.gz` by `tar -zcvf autowalk.walk.tar.gz autowalk.walk` +3. upload `autowalk.walk.tar.gz` to [jsk_data public google drive folger](https://drive.google.com/drive/u/0/folders/0B9P1L--7Wd2vUGplQkVLTFBWcFE?resourcekey=0-qlPyB_oRQm887pgRGiPhgg) +4. add new entry to download_autowalk_data.py like + +``` + download_data( + pkg_name=PKG, + path='autowalk/autowalk.walk.tar.gz', + url='https://drive.google.com/uc?id=', + md5='', + extract=True + ) +``` + +5. update README.md diff --git a/jsk_spot_robot/spot_autowalk_data/autowalk/.gitignore b/jsk_spot_robot/spot_autowalk_data/autowalk/.gitignore new file mode 100644 index 0000000000..72e8ffc0db --- /dev/null +++ b/jsk_spot_robot/spot_autowalk_data/autowalk/.gitignore @@ -0,0 +1 @@ +* diff --git a/jsk_spot_robot/spot_autowalk_data/package.xml b/jsk_spot_robot/spot_autowalk_data/package.xml new file mode 100644 index 0000000000..715f494d26 --- /dev/null +++ b/jsk_spot_robot/spot_autowalk_data/package.xml @@ -0,0 +1,18 @@ + + + spot_autowalk_data + 1.1.0 + The spot_autowalk_data package + + Kei Okada + Koki Shinjo + Koki Shinjo + + BSD + + catkin + jsk_data + + + + diff --git a/jsk_spot_robot/spot_autowalk_data/scripts/download_autowalk_data.py b/jsk_spot_robot/spot_autowalk_data/scripts/download_autowalk_data.py new file mode 100755 index 0000000000..7a2d8bc616 --- /dev/null +++ b/jsk_spot_robot/spot_autowalk_data/scripts/download_autowalk_data.py @@ -0,0 +1,75 @@ +#!/usr/bin/env python + +from jsk_data import download_data + + +def main(): + PKG = 'spot_autowalk_data' + + download_data( + pkg_name=PKG, + path='autowalk/eng2_73B2_to_81C1.walk.tar.gz', + url='https://drive.google.com/uc?id=1IDTCP7n4LCowizW3mFQvTOQtE4qOH9tx', + md5='65f09629c0ac2aa21df6f179c9875bd0', + extract=True + ) + + download_data( + pkg_name=PKG, + path='autowalk/eng2_73B2_to_7FElevator.walk.tar.gz', + url='https://drive.google.com/uc?id=1O8o6voq2v8WenfaUYcmpSU-IwJSsXW5_', + md5='ecd4d8dc043995f7675a59fce950676b', + extract=True + ) + + download_data( + pkg_name=PKG, + path='autowalk/eng2_7FElevator_to_2FElevator.walk.tar.gz', + url='https://drive.google.com/uc?id=12MOg5okckmQlYiM6flkdeMYxqjn9-C9l', + md5='67ae3210cbfb55791fff6494f84abb3b', + extract=True + ) + + download_data( + pkg_name=PKG, + path='autowalk/eng2_elevator_7FElevator_to_2FElevator.walk.tar.gz', + url='https://drive.google.com/uc?id=1iyx0y1dPu4HUPMNepR_VaZd_WEaC5Lku', + md5='915916d084abd54c2c17f0738a726da3', + extract=True + ) + + download_data( + pkg_name=PKG, + path='autowalk/eng2_2FElevator_to_2FEntrance.walk.tar.gz', + url='https://drive.google.com/uc?id=1cYUn_qnRslWuH0ZMEBN6ovqmdcUB0MzY', + md5='78c6e1e8e5967b216c9f53e38893750e', + extract=True + ) + + download_data( + pkg_name=PKG, + path='autowalk/eng2_2FElevator_to_MainGate.walk.tar.gz', + url='https://drive.google.com/uc?id=1CIuStpjxIA188MLxUsfUjI-47Ev36Wiv', + md5='47d669bcb1394b97c95e5d77f78da3e5', + extract=True + ) + + download_data( + pkg_name=PKG, + path='autowalk/eng2_2FElevator_to_eng8_2FElevator.walk.tar.gz', + url='https://drive.google.com/uc?id=1oyU1ufqy9gryPXw8YZ45Ff7AR8K825dT', + md5='ac9a67567104df2ddd78b8c53fa61ba2', + extract=True + ) + + download_data( + pkg_name=PKG, + path='autowalk/eng_TelephoneBox_to_HongoMainGate.walk.tar.gz', + url='https://drive.google.com/uc?id=120WC6SE4C_9XIvy0j3HEraljIigjcJ5U', + md5='6f89cd74bea3934171e0ae720747da24', + extract=True + ) + + +if __name__ == '__main__': + main() diff --git a/jsk_spot_robot/spot_autowalk_data/scripts/view_map.py b/jsk_spot_robot/spot_autowalk_data/scripts/view_map.py new file mode 100755 index 0000000000..966b2faa20 --- /dev/null +++ b/jsk_spot_robot/spot_autowalk_data/scripts/view_map.py @@ -0,0 +1,391 @@ +#!/usr/bin/env python3 +# Copyright (c) 2021 Boston Dynamics, Inc. All rights reserved. +# +# Downloading, reproducing, distributing or otherwise using the SDK Software +# is subject to the terms and conditions of the Boston Dynamics Software +# Development Kit License (20191101-BDSDK-SL). + +from vtk.util import numpy_support +import google.protobuf.timestamp_pb2 +import math +import numpy as np +import numpy.linalg +import os +import sys +import time +import vtk + +from bosdyn.api.graph_nav import map_pb2 +from bosdyn.api import geometry_pb2 +from bosdyn.client.frame_helpers import * +from bosdyn.client.math_helpers import * +""" +This example shows how to load and view a graph nav map. + +""" + + +def numpy_to_poly_data(pts): + """ + Converts numpy array data into vtk poly data. + :param pts: the numpy array to convert (3 x N). + :return: a vtkPolyData. + """ + pd = vtk.vtkPolyData() + pd.SetPoints(vtk.vtkPoints()) + # Makes a deep copy + pd.GetPoints().SetData(numpy_support.numpy_to_vtk(pts.copy())) + + f = vtk.vtkVertexGlyphFilter() + f.SetInputData(pd) + f.Update() + pd = f.GetOutput() + + return pd + + +def mat_to_vtk(mat): + """ + Converts a 4x4 homogenous transform into a vtk transform object. + :param mat: A 4x4 homogenous transform (numpy array). + :return: A VTK transform object representing the transform. + """ + t = vtk.vtkTransform() + t.SetMatrix(mat.flatten()) + return t + + +def vtk_to_mat(transform): + """ + Converts a VTK transform object to 4x4 homogenous numpy matrix. + :param transform: an object of type vtkTransform + : return: a numpy array with a 4x4 matrix representation of the transform. + """ + tf_matrix = transform.GetMatrix() + out = np.array(np.eye(4)) + for r in range(4): + for c in range(4): + out[r, c] = tf_matrix.GetElement(r, c) + return out + + +def api_to_vtk_se3_pose(se3_pose): + """ + Convert a bosdyn SDK SE3Pose into a VTK pose. + :param se3_pose: the bosdyn SDK SE3 Pose. + :return: A VTK pose representing the bosdyn SDK SE3 Pose. + """ + return mat_to_vtk(se3_pose.to_matrix()) + + +def create_fiducial_object(world_object, waypoint, renderer): + """ + Creates a VTK object representing a fiducial. + :param world_object: A WorldObject representing a fiducial. + :param waypoint: The waypoint the AprilTag is associated with. + :param renderer: The VTK renderer + :return: a tuple of (vtkActor, 4x4 homogenous transform) representing the vtk actor for the fiducial, and its + transform w.r.t the waypoint. + """ + fiducial_object = world_object.apriltag_properties + odom_tform_fiducial_filtered = get_a_tform_b( + world_object.transforms_snapshot, ODOM_FRAME_NAME, + world_object.apriltag_properties.frame_name_fiducial_filtered) + waypoint_tform_odom = SE3Pose.from_obj(waypoint.waypoint_tform_ko) + waypoint_tform_fiducial_filtered = api_to_vtk_se3_pose( + waypoint_tform_odom * odom_tform_fiducial_filtered) + plane_source = vtk.vtkPlaneSource() + plane_source.SetCenter(0.0, 0.0, 0.0) + plane_source.SetNormal(0.0, 0.0, 1.0) + plane_source.Update() + plane = plane_source.GetOutput() + mapper = vtk.vtkPolyDataMapper() + mapper.SetInputData(plane) + + actor = vtk.vtkActor() + actor.SetMapper(mapper) + actor.GetProperty().SetColor(0.5, 0.7, 0.9) + actor.SetScale(fiducial_object.dimensions.x, fiducial_object.dimensions.y, 1.0) + renderer.AddActor(actor) + return actor, waypoint_tform_fiducial_filtered + + +def create_point_cloud_object(waypoints, snapshots, waypoint_id): + """ + Create a VTK object representing the point cloud in a snapshot. Note that in graph_nav, "point cloud" refers to the + feature cloud of a waypoint -- that is, a collection of visual features observed by all five cameras at a particular + point in time. The visual features are associated with points that are rigidly attached to a waypoint. + :param waypoints: dict of waypoint ID to waypoint. + :param snapshots: dict of waypoint snapshot ID to waypoint snapshot. + :param waypoint_id: the waypoint ID of the waypoint whose point cloud we want to render. + :return: a vtkActor containing the point cloud data. + """ + wp = waypoints[waypoint_id] + snapshot = snapshots[wp.snapshot_id] + cloud = snapshot.point_cloud + odom_tform_cloud = get_a_tform_b(cloud.source.transforms_snapshot, ODOM_FRAME_NAME, + cloud.source.frame_name_sensor) + waypoint_tform_odom = SE3Pose.from_obj(wp.waypoint_tform_ko) + waypoint_tform_cloud = api_to_vtk_se3_pose(waypoint_tform_odom * odom_tform_cloud) + + point_cloud_data = np.frombuffer(cloud.data, dtype=np.float32).reshape(int(cloud.num_points), 3) + poly_data = numpy_to_poly_data(point_cloud_data) + arr = vtk.vtkFloatArray() + for i in range(cloud.num_points): + arr.InsertNextValue(point_cloud_data[i, 2]) + arr.SetName("z_coord") + poly_data.GetPointData().AddArray(arr) + poly_data.GetPointData().SetActiveScalars("z_coord") + actor = vtk.vtkActor() + mapper = vtk.vtkPolyDataMapper() + mapper.SetInputData(poly_data) + mapper.ScalarVisibilityOn() + actor.SetMapper(mapper) + actor.GetProperty().SetPointSize(2) + actor.SetUserTransform(waypoint_tform_cloud) + return actor + + +def create_waypoint_object(renderer, waypoints, snapshots, waypoint_id): + """ + Creates a VTK object representing a waypoint and its point cloud. + :param renderer: The VTK renderer. + :param waypoints: dict of waypoint ID to waypoint. + :param snapshots: dict of snapshot ID to snapshot. + :param waypoint_id: the waypoint id of the waypoint object we wish to create. + :return: A vtkAssembly representing the waypoint (an axis) and its point cloud. + """ + assembly = vtk.vtkAssembly() + actor = vtk.vtkAxesActor() + actor.SetXAxisLabelText("") + actor.SetYAxisLabelText("") + actor.SetZAxisLabelText("") + actor.SetTotalLength(0.2, 0.2, 0.2) + point_cloud_actor = create_point_cloud_object(waypoints, snapshots, waypoint_id) + assembly.AddPart(actor) + assembly.AddPart(point_cloud_actor) + renderer.AddActor(assembly) + return assembly + + +def make_line(pt_A, pt_B, renderer): + """ + Creates a VTK object which is a white line between two points. + :param pt_A: starting point of the line. + :param pt_B: ending point of the line. + :param renderer: the VTK renderer. + :return: A VTK object that is a while line between pt_A and pt_B. + """ + line_source = vtk.vtkLineSource() + line_source.SetPoint1(pt_A[0], pt_A[1], pt_A[2]) + line_source.SetPoint2(pt_B[0], pt_B[1], pt_B[2]) + mapper = vtk.vtkPolyDataMapper() + mapper.SetInputConnection(line_source.GetOutputPort()) + + actor = vtk.vtkActor() + actor.SetMapper(mapper) + actor.GetProperty().SetLineWidth(2) + actor.GetProperty().SetColor(0.7, 0.7, 0.7) + renderer.AddActor(actor) + return actor + + +def make_text(name, pt, renderer): + """ + Creates white text on a black background at a particular point. + :param name: The text to display. + :param pt: The point in the world where the text will be displayed. + :param renderer: The VTK renderer + :return: the vtkActor representing the text. + """ + actor = vtk.vtkTextActor() + actor.SetInput(name) + prop = actor.GetTextProperty() + prop.SetBackgroundColor(0.0, 0.0, 0.0) + prop.SetBackgroundOpacity(0.5) + prop.SetFontSize(16) + coord = actor.GetPositionCoordinate() + coord.SetCoordinateSystemToWorld() + coord.SetValue((pt[0], pt[1], pt[2])) + + renderer.AddActor(actor) + return actor + + +def create_edge_object(curr_wp_tform_to_wp, world_tform_curr_wp, renderer): + # Concatenate the edge transform. + world_tform_to_wp = np.dot(world_tform_curr_wp, curr_wp_tform_to_wp) + # Make a line between the current waypoint and the neighbor. + make_line(world_tform_curr_wp[:3, 3], world_tform_to_wp[:3, 3], renderer) + return world_tform_to_wp + + +def load_map(path): + """ + Load a map from the given file path. + :param path: Path to the root directory of the map. + :return: the graph, waypoints, waypoint snapshots and edge snapshots. + """ + with open(os.path.join(path, "graph"), "rb") as graph_file: + # Load the graph file and deserialize it. The graph file is a protobuf containing only the waypoints and the + # edges between them. + data = graph_file.read() + current_graph = map_pb2.Graph() + current_graph.ParseFromString(data) + + # Set up maps from waypoint ID to waypoints, edges, snapshots, etc. + current_waypoints = {} + current_waypoint_snapshots = {} + current_edge_snapshots = {} + + # For each waypoint, load any snapshot associated with it. + for waypoint in current_graph.waypoints: + current_waypoints[waypoint.id] = waypoint + + # Load the snapshot. Note that snapshots contain all of the raw data in a waypoint and may be large. + file_name = os.path.join(path, "waypoint_snapshots", waypoint.snapshot_id) + if not os.path.exists(file_name): + continue + with open(file_name, "rb") as snapshot_file: + waypoint_snapshot = map_pb2.WaypointSnapshot() + waypoint_snapshot.ParseFromString(snapshot_file.read()) + current_waypoint_snapshots[waypoint_snapshot.id] = waypoint_snapshot + # Similarly, edges have snapshot data. + for edge in current_graph.edges: + file_name = os.path.join(path, "edge_snapshots", edge.snapshot_id) + if not os.path.exists(file_name): + continue + with open(file_name, "rb") as snapshot_file: + edge_snapshot = map_pb2.EdgeSnapshot() + edge_snapshot.ParseFromString(snapshot_file.read()) + current_edge_snapshots[edge_snapshot.id] = edge_snapshot + print("Loaded graph with {} waypoints and {} edges".format( + len(current_graph.waypoints), len(current_graph.edges))) + return (current_graph, current_waypoints, current_waypoint_snapshots, + current_edge_snapshots) + + +def create_graph_objects(current_graph, current_waypoint_snapshots, current_waypoints, renderer): + """ + Creates all the VTK objects associated with the graph. + :param current_graph: the graph to use. + :param current_waypoint_snapshots: dict from snapshot id to snapshot. + :param current_waypoints: dict from waypoint id to waypoint. + :param renderer: The VTK renderer + :return: the average position in world space of all the waypoints. + """ + waypoint_objects = {} + # Create VTK objects associated with each waypoint. + for waypoint in current_graph.waypoints: + waypoint_objects[waypoint.id] = create_waypoint_object(renderer, current_waypoints, + current_waypoint_snapshots, + waypoint.id) + # Now, perform a breadth first search of the graph starting from an arbitrary waypoint. Graph nav graphs + # have no global reference frame. The only thing we can say about waypoints is that they have relative + # transformations to their neighbors via edges. So the goal is to get the whole graph into a global reference + # frame centered on some waypoint as the origin. + queue = [] + queue.append((current_graph.waypoints[0], np.eye(4))) + visited = {} + # Get the camera in the ballpark of the right position by centering it on the average position of a waypoint. + avg_pos = np.array([0.0, 0.0, 0.0]) + + # Breadth first search. + while len(queue) > 0: + # Visit a waypoint. + curr_element = queue[0] + queue.pop(0) + curr_waypoint = curr_element[0] + visited[curr_waypoint.id] = True + + # We now know the global pose of this waypoint, so set the pose. + waypoint_objects[curr_waypoint.id].SetUserTransform(mat_to_vtk(curr_element[1])) + world_tform_current_waypoint = curr_element[1] + # Add text to the waypoint. + print('{}: {}'.format(curr_waypoint.annotations.name, curr_waypoint.id)) + make_text( + '{}: {}'.format(curr_waypoint.annotations.name, curr_waypoint.id), + world_tform_current_waypoint[:3, 3], + renderer) + + # For each fiducial in the waypoint's snapshot, add an object at the world pose of that fiducial. + if (curr_waypoint.snapshot_id in current_waypoint_snapshots): + snapshot = current_waypoint_snapshots[curr_waypoint.snapshot_id] + for fiducial in snapshot.objects: + if fiducial.HasField("apriltag_properties"): + (fiducial_object, + curr_wp_tform_fiducial) = create_fiducial_object(fiducial, curr_waypoint, + renderer) + world_tform_fiducial = np.dot(world_tform_current_waypoint, + vtk_to_mat(curr_wp_tform_fiducial)) + fiducial_object.SetUserTransform(mat_to_vtk(world_tform_fiducial)) + make_text( + 'fid: {}'.format(fiducial.apriltag_properties.tag_id), world_tform_fiducial[:3, 3], + renderer) + + # Now, for each edge, walk along the edge and concatenate the transform to the neighbor. + for edge in current_graph.edges: + # If the edge is directed away from us... + if edge.id.from_waypoint == curr_waypoint.id and edge.id.to_waypoint not in visited: + current_waypoint_tform_to_waypoint = SE3Pose.from_obj( + edge.from_tform_to).to_matrix() + world_tform_to_wp = create_edge_object(current_waypoint_tform_to_waypoint, + world_tform_current_waypoint, renderer) + # Add the neighbor to the queue. + queue.append((current_waypoints[edge.id.to_waypoint], world_tform_to_wp)) + avg_pos += world_tform_to_wp[:3, 3] + # If the edge is directed toward us... + elif edge.id.to_waypoint == curr_waypoint.id and edge.id.from_waypoint not in visited: + current_waypoint_tform_from_waypoint = (SE3Pose.from_obj( + edge.from_tform_to).inverse()).to_matrix() + world_tform_from_wp = create_edge_object(current_waypoint_tform_from_waypoint, + world_tform_current_waypoint, renderer) + # Add the neighbor to the queue. + queue.append((current_waypoints[edge.id.from_waypoint], world_tform_from_wp)) + avg_pos += world_tform_from_wp[:3, 3] + return avg_pos + + +def main(argv): + # Load the map from the given file. + path = argv[0] + (current_graph, current_waypoints, current_waypoint_snapshots, + current_edge_snapshots) = load_map(path) + + # Create the renderer. + renderer = vtk.vtkRenderer() + renderer.SetBackground(0.05, 0.1, 0.15) + + avg_pos = create_graph_objects(current_graph, current_waypoint_snapshots, current_waypoints, + renderer) + + # Compute the average waypoint position to place the camera appropriately. + avg_pos /= len(current_waypoints) + camera_pos = avg_pos + np.array([-1, 0, 5]) + + camera = renderer.GetActiveCamera() + camera.SetViewUp(0, 0, 1) + camera.SetPosition(camera_pos[0], camera_pos[1], camera_pos[2]) + + # Create the VTK renderer and interactor. + renderWindow = vtk.vtkRenderWindow() + renderWindow.SetWindowName(path) + renderWindow.AddRenderer(renderer) + renderWindowInteractor = vtk.vtkRenderWindowInteractor() + renderWindowInteractor.SetRenderWindow(renderWindow) + renderWindow.SetSize(1280, 720) + style = vtk.vtkInteractorStyleTerrain() + renderWindowInteractor.SetInteractorStyle(style) + renderer.ResetCamera() + + # Start rendering. + renderWindow.Render() + renderWindow.Start() + renderWindowInteractor.Start() + + +if __name__ == '__main__': + if (len(sys.argv) != 2): + print("Usage: view_map.py ") + sys.exit(-1) + main(sys.argv[1:])