initial running a demo of footstep and baseauto

free_gait_action_loader/CHANGELOG.rst

@@ -0,0 +1,11 @@
+^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^
+Changelog for package free_gait_action_loader
+^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^
+
+Forthcoming
+-----------
+
+0.1.1 (2017-01-31)
+------------------
+* Initial release.
+* Contributors: Peter Fankhauser, Marko Bjelonic, Remo Diethelm

free_gait_action_loader/CMakeLists.txt

@@ -0,0 +1,38 @@
+# Project configuration
+cmake_minimum_required (VERSION 2.8)
+project(free_gait_action_loader)
+
+## Find catkin macros and libraries
+## if COMPONENTS list like find_package(catkin REQUIRED COMPONENTS xyz)
+## is used, also find other catkin packages
+find_package(catkin REQUIRED COMPONENTS
+    rospy
+    free_gait_python
+    free_gait_msgs
+)
+
+catkin_package(
+  #INCLUDE_DIRS
+  #LIBRARIES
+  #CATKIN_DEPENDS
+  #DEPENDS
+)
+
+## Uncomment this if the package has a setup.py. This macro ensures
+## modules and global scripts declared therein get installed
+## See http://ros.org/doc/api/catkin/html/user_guide/setup_dot_py.html
+catkin_python_setup()
+
+#############
+## Install ##
+#############
+
+# all install targets should use catkin DESTINATION variables
+# See http://ros.org/doc/api/catkin/html/adv_user_guide/variables.html
+
+## Mark executable scripts (Python etc.) for installation
+## in contrast to setup.py, you can choose the destination
+catkin_install_python(PROGRAMS
+  bin/free_gait_action_loader/action_loader.py
+  DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION}
+)

free_gait_action_loader/actions/test.yaml

@@ -0,0 +1,3 @@
+steps:
+  - step:
+    - base_auto:

free_gait_action_loader/bin/free_gait_action_loader/action_loader.py

@@ -0,0 +1,243 @@
+#! /usr/bin/env python
+
+import roslib
+roslib.load_manifest('free_gait_action_loader')
+from free_gait_action_loader import *
+from free_gait import *
+import rospy
+import actionlib
+import free_gait_msgs.msg
+import free_gait_msgs.srv
+import std_srvs.srv
+import traceback
+from actionlib_msgs.msg import *
+import thread
+
+class ActionLoader:
+
+    def __init__(self):
+        self.name = rospy.get_name()[1:]
+        self.action_list = ActionList(self.name)
+        self.action_list.update()
+        self.collection_list = CollectionList(self.name)
+        self.collection_list.update()
+        self.action_sequence_queue = []
+        self.action = None
+        # Reference to the action client or preview publisher.
+        self.execute_steps_relay = None
+
+        step_action_server_topic = rospy.get_param('/free_gait/action_server')
+        self.execute_steps_client = actionlib.SimpleActionClient(step_action_server_topic, free_gait_msgs.msg.ExecuteStepsAction)
+
+        self.execute_action_server = actionlib.SimpleActionServer("~execute_action", free_gait_msgs.msg.ExecuteActionAction, \
+                                                          execute_cb=self._execute_action_callback, auto_start = False)
+        self.execute_action_server.register_preempt_callback(self.preempt)
+        self.execute_action_server.start()
+
+        step_preview_topic = rospy.get_param('/free_gait/preview_topic')
+        self.preview_publisher = rospy.Publisher(step_preview_topic, free_gait_msgs.msg.ExecuteStepsActionGoal, queue_size=1)
+
+        rospy.Service('~update', std_srvs.srv.Trigger, self.update)
+        rospy.Service('~list_actions', free_gait_msgs.srv.GetActions, self.list_actions)
+        rospy.Service('~list_collections', free_gait_msgs.srv.GetCollections, self.list_collections)
+        rospy.Service('~send_action', free_gait_msgs.srv.SendAction, self._send_action_callback)
+        rospy.Service('~preview_action', free_gait_msgs.srv.SendAction, self._preview_action_callback)
+        rospy.Service('~send_action_sequence', free_gait_msgs.srv.SendActionSequence, self._send_action_sequence_callback)
+        rospy.on_shutdown(self.preempt)
+
+    def update(self, request):
+        success = self.action_list.update() and self.collection_list.update()
+        response = std_srvs.srv.TriggerResponse()
+        response.success = success
+        return response
+
+    def list_actions(self, request):
+        response = free_gait_msgs.srv.GetActionsResponse()
+        action_ids = []
+        if request.collection_id:
+            collection = self.collection_list.get(request.collection_id)
+            if collection is None:
+                return response
+            action_ids = collection.action_ids
+        response.actions = self.action_list.to_ros_message(action_ids)
+        return response
+
+    def list_collections(self, request):
+        response = free_gait_msgs.srv.GetCollectionsResponse()
+        response.collections = self.collection_list.to_ros_message()
+        return response
+
+    def _execute_action_callback(self, goal):
+        self.action_sequence_queue = []
+        result = self.send_action(goal.action_id, False)
+        if result.status != result.RESULT_NOT_FOUND:
+            self.action.wait_for_state([ActionState.ERROR, ActionState.DONE])
+        result.status = result.RESULT_DONE
+        self.execute_action_server.set_succeeded(result)
+
+    def _send_action_callback(self, request):
+        self.action_sequence_queue = []
+        response = free_gait_msgs.srv.SendActionResponse()
+        response.result = self.send_action(request.goal.action_id, False)
+        return response
+
+    def _send_action_sequence_callback(self, request):
+        self.action_sequence_queue = []
+        for goal in request.goals:
+            self.action_sequence_queue.append(goal.action_id)
+        # Start first goal.
+        response = free_gait_msgs.srv.SendActionSequenceResponse()
+        response.result = self.send_action(self.action_sequence_queue[0], False)
+        self.action_sequence_queue.pop(0)
+        return response
+
+    def _preview_action_callback(self, request):
+        response = free_gait_msgs.srv.SendActionResponse()
+        response.result = self.send_action(request.goal.action_id, True)
+        return response
+
+    def send_action(self, action_id, use_preview):
+        self.reset()
+        if use_preview:
+            self.execute_steps_relay = self.preview_publisher
+        else:
+            self.execute_steps_relay = self.execute_steps_client
+
+        action_entry = self.action_list.get(action_id)
+        result = free_gait_msgs.msg.ExecuteActionResult()
+
+        if action_entry is None:
+            rospy.logerr('Action with id "' + action_id + '" does not exists.')
+            result.status = result.RESULT_NOT_FOUND
+            return result
+
+        if action_entry.file is None:
+            rospy.logerr('File for action with id "' + action_id + '" does not exists.')
+            result.status = result.RESULT_NOT_FOUND
+            return result
+
+        try:
+            if action_entry.type == ActionType.YAML:
+                self._load_yaml_action(action_entry.file)
+            if action_entry.type == ActionType.COMBINED_YAML:
+                self._load_combined_yaml_action(action_entry.file)
+            elif action_entry.type == ActionType.PYTHON:
+                self._load_python_action(action_entry.file)
+            elif action_entry.type == ActionType.LAUNCH:
+                self._load_launch_action(action_entry.file)
+
+            if self.action is None:
+                result.status = result.RESULT_UNKNOWN
+                rospy.logerr('An unkown state has been reached while reading the action.')
+                return result
+
+            if self.action.state == ActionState.ERROR or self.action.state == ActionState.UNINITIALIZED:
+                result.status = result.RESULT_FAILED
+                rospy.logerr('An error occurred while loading the action.')
+                return result
+
+            self.action.register_callback(self._action_feedback_callback, self._action_done_callback)
+            self.action.wait_for_state([ActionState.ERROR, ActionState.ACTIVE, ActionState.IDLE, ActionState.DONE])
+
+            if self.action.state == ActionState.ERROR:
+                result.status = result.RESULT_FAILED
+                rospy.logerr('An error occurred while initializing the action.')
+            else:
+                result.status = result.RESULT_STARTED
+                rospy.loginfo('Action was successfully started.')
+
+        except:
+            rospy.logerr('An exception occurred while loading the action.')
+            result.status = result.RESULT_FAILED
+            rospy.logerr(traceback.print_exc())
+
+        return result
+
+    def _load_yaml_action(self, file_path):
+        # Load action from YAML file.
+        rospy.loginfo('Loading Free Gait action from YAML file "' + file_path + '".')
+        goal = load_action_from_file(file_path)
+        rospy.logdebug(goal)
+        self.action = SimpleAction(self.execute_steps_relay, goal)
+        if goal is None:
+            rospy.logerr('Could not load action from YAML file.')
+            self.action.set_state(ActionState.ERROR)
+
+    def _load_combined_yaml_action(self, file_path):
+        # Load combined action from YAML file listing the combination of YAML actions.
+        rospy.loginfo('Loading Free Gait action from combined YAML file "' + file_path + '".')
+        self.action = CombinedYamlAction(self.execute_steps_relay)
+        self.action.set_goal_from_file(file_path)
+
+    def _load_python_action(self, file_path):
+        # Load action from Python script.
+        rospy.loginfo('Loading Free Gait action from Python script "' + file_path + '".')
+        # action_locals = dict()
+        # Kind of nasty, but currently only way to make external imports work.
+        execfile(file_path, globals(), globals())
+        self.action = action
+
+    def _load_launch_action(self, file_path):
+        # Load action with external launch file.
+        rospy.loginfo('Loading Free Gait action with launch file "' + file_path + '".')
+        self.action = LaunchAction(file_path, self.execute_steps_relay)
+
+    def _action_feedback_callback(self):
+        rospy.loginfo('Action switched to state: ' + ActionState.to_text(self.action.state) + '.')
+        if self.execute_action_server.is_active():
+            feedback = free_gait_msgs.msg.ExecuteActionFeedback()
+            feedback.status = self.action.state
+            self.execute_action_server.publish_feedback(feedback)
+
+    def _action_done_callback(self):
+        # If action sequence exists, continue with next action.
+        if len(self.action_sequence_queue) > 0:
+            thread.start_new_thread(self._load_next_action_in_sequence, ())
+        rospy.loginfo('Action switched to state: ' + ActionState.to_text(self.action.state) + '.')
+
+    def _load_next_action_in_sequence(self):
+        if self.action.state == ActionState.DONE:
+            result = self.send_action(self.action_sequence_queue[0], False)
+            self.action_sequence_queue.pop(0)
+            if (result.status == result.RESULT_STARTED):
+                return
+        # Error occured.
+        rospy.loginfo('Purging remaining actions from sequence.')
+        self.action_sequence_queue = []
+
+    def _check_and_start_action(self):
+        if self.action is not None:
+            if self.action.state == ActionState.INITIALIZED:
+                self.action.start()
+
+    def reset(self):
+        self.execute_steps_relay = None
+        try:
+            if self.action:
+                self.action.stop()
+            del self.action
+            self.action = None
+            # rospy.logwarn('Canceling action.')
+        except NameError:
+            rospy.logerr(traceback.print_exc())
+
+    def preempt(self):
+        self.reset()
+
+
+if __name__ == '__main__':
+    try:
+        rospy.init_node('free_gait_action_loader')
+        action_loader = ActionLoader()
+        rospy.loginfo('Ready to load actions from service call.')
+
+        updateRate = rospy.Rate(10)
+        while not rospy.is_shutdown():
+            # This is required for having the actions run in the main thread
+            # (instead through the thread by the service callback).
+            action_loader._check_and_start_action()
+            updateRate.sleep()
+
+    except rospy.ROSInterruptException:
+        # rospy.logerr(traceback.print_exc())
+        pass

free_gait_action_loader/package.xml

@@ -0,0 +1,17 @@
+<?xml version="1.0" encoding="UTF-8"?>
+<package format="2">
+  <name>free_gait_action_loader</name>
+  <version>0.3.0</version>
+  <description>Dynamic loading of Free Gait actions.</description>
+  <maintainer email="[email protected]">Peter Fankhauser</maintainer>
+  <author email="[email protected]">Peter Fankhauser</author>
+  <license>BSD</license>
+  <url type="website">https://github.com/leggedrobotics/free_gait</url>
+  <url type="bugtracker">https://github.com/leggedrobotics/free_gait/issues</url>
+
+  <buildtool_depend>catkin</buildtool_depend>
+  <depend>rospy</depend>
+  <depend>free_gait_python</depend>
+  <depend>free_gait_msgs</depend>
+
+</package>

free_gait_action_loader/setup.py

@@ -0,0 +1,9 @@
+from distutils.core import setup
+from catkin_pkg.python_setup import generate_distutils_setup
+
+d = generate_distutils_setup(
+    packages=['free_gait_action_loader'],
+    package_dir={'': 'src'}
+)
+
+setup(**d)

free_gait_action_loader/src/free_gait_action_loader/__init__.py

@@ -0,0 +1,5 @@
+from action_handling import ActionType
+from action_handling import ActionEntry
+from action_handling import ActionList
+from collection_handling import Collection
+from collection_handling import CollectionList