diff --git a/free_gait_action_loader/CHANGELOG.rst b/free_gait_action_loader/CHANGELOG.rst new file mode 100644 index 0000000..3caf8cc --- /dev/null +++ b/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 diff --git a/free_gait_action_loader/CMakeLists.txt b/free_gait_action_loader/CMakeLists.txt new file mode 100644 index 0000000..8a77ab5 --- /dev/null +++ b/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} +) diff --git a/free_gait_action_loader/actions/test.yaml b/free_gait_action_loader/actions/test.yaml new file mode 100644 index 0000000..8289fd8 --- /dev/null +++ b/free_gait_action_loader/actions/test.yaml @@ -0,0 +1,3 @@ +steps: + - step: + - base_auto: diff --git a/free_gait_action_loader/bin/free_gait_action_loader/action_loader.py b/free_gait_action_loader/bin/free_gait_action_loader/action_loader.py new file mode 100755 index 0000000..79d0d33 --- /dev/null +++ b/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 diff --git a/free_gait_action_loader/package.xml b/free_gait_action_loader/package.xml new file mode 100644 index 0000000..301520a --- /dev/null +++ b/free_gait_action_loader/package.xml @@ -0,0 +1,17 @@ + + + free_gait_action_loader + 0.3.0 + Dynamic loading of Free Gait actions. + Peter Fankhauser + Peter Fankhauser + BSD + https://github.com/leggedrobotics/free_gait + https://github.com/leggedrobotics/free_gait/issues + + catkin + rospy + free_gait_python + free_gait_msgs + + diff --git a/free_gait_action_loader/setup.py b/free_gait_action_loader/setup.py new file mode 100644 index 0000000..a6a6a91 --- /dev/null +++ b/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) diff --git a/free_gait_action_loader/src/free_gait_action_loader/__init__.py b/free_gait_action_loader/src/free_gait_action_loader/__init__.py new file mode 100644 index 0000000..bcfc8bb --- /dev/null +++ b/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 diff --git a/free_gait_action_loader/src/free_gait_action_loader/action_handling.py b/free_gait_action_loader/src/free_gait_action_loader/action_handling.py new file mode 100644 index 0000000..331dfeb --- /dev/null +++ b/free_gait_action_loader/src/free_gait_action_loader/action_handling.py @@ -0,0 +1,150 @@ +#! /usr/bin/env python + +import os +import rospy +import roslib +import rospkg +from rosparam import load_file +from os.path import * +import free_gait_msgs.msg + +class ActionType: + YAML = 0 + COMBINED_YAML = 1 + PYTHON = 2 + LAUNCH = 3 + + @staticmethod + def to_text(action_type): + if action_type == ActionType.YAML: + return 'yaml' + elif action_type == ActionType.COMBINED_YAML: + return 'combined_yaml' + elif action_type == ActionType.PYTHON: + return 'python' + elif action_type == ActionType.LAUNCH: + return 'launch' + else: + return None + + @staticmethod + def from_text(action_type): + if action_type == 'yaml': + return ActionType.YAML + elif action_type == 'combined_yaml': + return ActionType.COMBINED_YAML + elif action_type == 'python': + return ActionType.PYTHON + elif action_type == 'launch': + return ActionType.LAUNCH + else: + return None + + +class ActionEntry: + + def __init__(self, package_path, parameters): + self.id = None + self.name = None + self.file = None + self.type = None + self.description = None + self.directory = None + self._initialize(package_path, parameters); + + def __str__(self): + output = 'ID: ' + self.id + if self.name: + output = output + ", Name: " + self.name + if self.file: + output = output + ", File: " + self.file + if self.file: + output = output + ", Type: " + ActionType.to_text(self.type) + return output + + def _initialize(self, package_path, parameters): + if 'id' in parameters: + self.id = parameters['id'] + if 'name' in parameters: + self.name = parameters['name'] + if 'file' in parameters: + self.file = abspath(join(package_path, parameters['file'])) + if 'type' in parameters: + self.type = ActionType.from_text(parameters['type']) + if 'description' in parameters: + self.description = parameters['description'] + self.directory = dirname(abspath(self.file)) + + def to_ros_message(self): + message = free_gait_msgs.msg.ActionDescription() + message.id = self.id + message.name = self.name + message.file = self.file + message.type = ActionType.to_text(self.type) + message.description = self.description + return message + + +class ActionList: + + def __init__(self, name): + self.name = name + self.actions = [] + + def update(self): + self.actions = [] + rospack = rospkg.RosPack() + packages = rospack.get_depends_on(self.name, implicit=False) + rospy.loginfo(packages) + for package in packages: + rospy.loginfo(package) + manifest = rospack.get_manifest(package) + file_path = manifest.get_export(self.name, 'actions') + if not file_path: + continue + elif len(file_path) != 1: + rospy.logwarn("Cannot load actions [%s]: invalid 'actions' attribute."%(pkg)) + continue + + file_path = file_path[0] + try: + if not os.path.isfile(file_path): + rospy.logwarn('Actions parameter file with path "' + file_path + '" does not exists.') + continue + + package_path = rospack.get_path(package) + parameters = load_file(file_path) + for action_parameters in parameters[0][0]['actions']: + entry = ActionEntry(package_path, action_parameters['action']) + self.actions.append(entry) + + except Exception: + rospy.logwarn("Unable to load actions [%s] from package [%s]."%(file_path, package)) + + self.actions = sorted(self.actions, key = lambda x: (x.id)) + return True + + def get(self, id): + entry = [ e for e in self.actions if (e.id == id) ] + if len(entry) == 0: + return None + return entry[0] + + def get_multiple(self, ids): + entries = [] + for id in ids: + entry = self.get(id) + if entry: + entries.append(entry) + return entries + + def to_ros_message(self, ids = []): + actions = [] + if len(ids): + actions = self.get_multiple(ids) + else: + actions = self.actions + message = [] + for action in actions: + message.append(action.to_ros_message()) + return message diff --git a/free_gait_action_loader/src/free_gait_action_loader/action_handling.pyc b/free_gait_action_loader/src/free_gait_action_loader/action_handling.pyc new file mode 100644 index 0000000..f5a41c4 Binary files /dev/null and b/free_gait_action_loader/src/free_gait_action_loader/action_handling.pyc differ diff --git a/free_gait_action_loader/src/free_gait_action_loader/collection_handling.py b/free_gait_action_loader/src/free_gait_action_loader/collection_handling.py new file mode 100644 index 0000000..2ca346d --- /dev/null +++ b/free_gait_action_loader/src/free_gait_action_loader/collection_handling.py @@ -0,0 +1,130 @@ +#! /usr/bin/env python + +import os +import rospy +import roslib +import rospkg +from rosparam import load_file +from os.path import * +import free_gait_msgs.msg + + +class Collection: + + def __init__(self, parameters): + self.id = None + self.name = None + self.action_ids = [] + self.is_sequence = None + self._initialize(parameters) + + def __str__(self): + output = 'ID: ' + self.id + if self.name: + output = output + ", Name: " + self.name + if self.action_ids: + output = output + ", Action IDs: " + for action_id in self.action_ids: + output = output + ", " + action_id + return output + + def _initialize(self, parameters): + if 'id' in parameters: + self.id = parameters['id'] + if 'name' in parameters: + self.name = parameters['name'] + if 'actions' in parameters: + for action_id in parameters['actions']: + self.action_ids.append(action_id) + self.is_sequence = False + if 'is_sequence' in parameters: + if parameters['is_sequence']: + self.is_sequence = True + if not self.is_sequence: + self.action_ids = sorted(self.action_ids) + + def to_ros_message(self): + message = free_gait_msgs.msg.CollectionDescription() + message.id = self.id + message.name = self.name + for action_id in self.action_ids: + message.action_ids.append(action_id) + message.is_sequence = self.is_sequence + return message + + +class CollectionList: + + def __init__(self, name): + self.name = name + self.collections = [] + self.collections_to_merge = [] + self.collections_to_ignore = [] + + def update(self): + self.collections = [] + self.collections_to_merge = [] + self.collections_to_ignore = [] + rospack = rospkg.RosPack() + packages = rospack.get_depends_on(self.name, implicit=False) + for package in packages: + manifest = rospack.get_manifest(package) + file_path = manifest.get_export(self.name, 'collections') + if not file_path: + continue + elif len(file_path) != 1: + rospy.logwarn("Cannot load collections [%s]: invalid 'collections' attribute."%(pkg)) + continue + + file_path = file_path[0] + try: + if not os.path.isfile(file_path): + rospy.logwarn('Collections parameter file with path "' + file_path + '" does not exists.') + continue + + parameters = load_file(file_path) + for collections_parameters in parameters[0][0]['collections']: + if 'collection' in collections_parameters: + collection = Collection(collections_parameters['collection']) + self.collections.append(collection) + elif 'add_to_collection' in collections_parameters: + collection = Collection(collections_parameters['add_to_collection']) + self.collections_to_merge.append(collection) + elif 'ignore_collection' in collections_parameters: + collection = Collection(collections_parameters['ignore_collection']) + self.collections_to_ignore.append(collection) + + except Exception: + rospy.logwarn("Unable to load collections [%s] from package [%s]."%(file_path, package)) + + self._merge_collections() + self._ignore_collections() + self.collections = sorted(self.collections, key = lambda x: (x.id)) + return True + + def get(self, id): + collection = [ c for c in self.collections if (c.id == id) ] + if len(collection) == 0: + return None + return collection[0] + + def remove(self, id): + self.collections[:] = [c for c in self.collections if (c.id != id) ] + + def to_ros_message(self): + message = [] + for collection in self.collections: + message.append(collection.to_ros_message()) + return message + + def _merge_collections(self): + for collection_to_merge in self.collections_to_merge: + collection = self.get(collection_to_merge.id) + if not collection: + rospy.logwarn('Could not find collection with id "%s" to add actions to.'%(collection_to_merge.id)) + continue + collection.action_ids.extend(collection_to_merge.action_ids) + + def _ignore_collections(self): + for collection_to_ignore in self.collections_to_ignore: + self.remove(collection_to_ignore.id) diff --git a/free_gait_action_loader/src/free_gait_action_loader/collection_handling.pyc b/free_gait_action_loader/src/free_gait_action_loader/collection_handling.pyc new file mode 100644 index 0000000..3baf1f7 Binary files /dev/null and b/free_gait_action_loader/src/free_gait_action_loader/collection_handling.pyc differ diff --git a/free_gait_core/CMakeLists.txt b/free_gait_core/CMakeLists.txt index 7232da2..44fe48b 100644 --- a/free_gait_core/CMakeLists.txt +++ b/free_gait_core/CMakeLists.txt @@ -5,10 +5,11 @@ set(CMAKE_CXX_FLAGS "-std=c++11 ${CMAKE_CXX_FLAGS}") ## Find catkin macros and libraries find_package(catkin REQUIRED COMPONENTS - #quadruped_model + quadruped_model #numopt_common #numopt_quadprog #numopt_sqp + qp_solver curves grid_map_core #robot_utils @@ -35,13 +36,13 @@ catkin_package( LIBRARIES ${PROJECT_NAME} CATKIN_DEPENDS - quadruped_model - numopt_common - numopt_quadprog - numopt_sqp + #quadruped_model + #numopt_common + #numopt_quadprog + #numopt_sqp curves grid_map_core - robot_utils + #robot_utils std_utils message_logger ) @@ -98,7 +99,7 @@ add_library(${PROJECT_NAME} src/pose_optimization/PoseOptimizationGeometric.cpp src/pose_optimization/PoseOptimizationQP.cpp src/pose_optimization/PoseOptimizationSQP.cpp - src/pose_optimization/PoseParameterization.cpp + src/pose_optimization/poseparameterization.cpp src/pose_optimization/PoseOptimizationObjectiveFunction.cpp src/pose_optimization/PoseOptimizationFunctionConstraints.cpp src/pose_optimization/PoseOptimizationProblem.cpp diff --git a/free_gait_core/include/free_gait_core/TypeDefs.hpp b/free_gait_core/include/free_gait_core/TypeDefs.hpp index 91125fc..f1cb7cf 100644 --- a/free_gait_core/include/free_gait_core/TypeDefs.hpp +++ b/free_gait_core/include/free_gait_core/TypeDefs.hpp @@ -21,14 +21,14 @@ namespace free_gait { -struct EnumClassHash -{ - template - std::size_t operator()(T t) const - { - return static_cast(t); - } -}; +//struct EnumClassHash +//{ +// template +// std::size_t operator()(T t) const +// { +// return static_cast(t); +// } +//}; // Import enum aliases. using QD = quadruped_model::QuadrupedModel::QuadrupedDescription; diff --git a/free_gait_core/include/free_gait_core/executor/AdapterBase.hpp b/free_gait_core/include/free_gait_core/executor/AdapterBase.hpp index d47fba2..8898c74 100644 --- a/free_gait_core/include/free_gait_core/executor/AdapterBase.hpp +++ b/free_gait_core/include/free_gait_core/executor/AdapterBase.hpp @@ -109,11 +109,12 @@ class AdapterBase const LimbEnum& limb, const LinearAcceleration& endEffectorLinearAccelerationInWorldFrame) const = 0; //! Hook to write data to internal robot representation from state. - virtual bool setInternalDataFromState(const State& state) const = 0; -// virtual bool setInternalDataFromState(const State& state, bool updateContacts = true, bool updatePosition = true, -// bool updateVelocity = true, bool updateAcceleration = false) const = 0; -// virtual void createCopyOfState() const = 0; -// virtual void resetToCopyOfState() const = 0; +// virtual bool setInternalDataFromState(const State& state) const = 0; + virtual bool setInternalDataFromState(const State& state, bool updateContacts = true, bool updatePosition = true, + bool updateVelocity = true, bool updateAcceleration = false) const = 0; + virtual void createCopyOfState() const = 0; + virtual void resetToCopyOfState() const = 0; + virtual const State& getState() const = 0; }; } /* namespace free_gait */ diff --git a/free_gait_core/include/free_gait_core/executor/State.hpp b/free_gait_core/include/free_gait_core/executor/State.hpp index 06199d2..a8e04ea 100644 --- a/free_gait_core/include/free_gait_core/executor/State.hpp +++ b/free_gait_core/include/free_gait_core/executor/State.hpp @@ -10,6 +10,7 @@ #include "free_gait_core/TypeDefs.hpp" //#include +#include "quadruped_model/quadruped_state.h" #include // STD @@ -24,7 +25,7 @@ namespace free_gait { class State : public quadruped_model::QuadrupedState { public: -// using QD = quadruped_model::QuadrupedModel::QuadrupedDescription; + using QD = quadruped_model::QuadrupedModel::QuadrupedDescription; State(); virtual ~State(); @@ -80,7 +81,8 @@ class State : public quadruped_model::QuadrupedState void setEmptyControlSetup(const LimbEnum& limb); void getAllJointNames(std::vector& jointNames) const; - + Position getSupportFootPosition(const LimbEnum& limb); + void setSupportFootStance(const Stance& footInSupport); friend std::ostream& operator << (std::ostream& out, const State& state); private: @@ -100,6 +102,8 @@ class State : public quadruped_model::QuadrupedState std::unordered_map surfaceNormals_; bool robotExecutionStatus_; std::string stepId_; // empty if undefined. + + Stance footHoldInSupport_; }; } /* namespace */ diff --git a/free_gait_core/include/free_gait_core/pose_optimization/PoseOptimizationFunctionConstraints.hpp b/free_gait_core/include/free_gait_core/pose_optimization/PoseOptimizationFunctionConstraints.hpp index 48dce51..bdf8b20 100644 --- a/free_gait_core/include/free_gait_core/pose_optimization/PoseOptimizationFunctionConstraints.hpp +++ b/free_gait_core/include/free_gait_core/pose_optimization/PoseOptimizationFunctionConstraints.hpp @@ -12,15 +12,16 @@ #include "free_gait_core/executor/AdapterBase.hpp" #include "free_gait_core/executor/State.hpp" #include "free_gait_core/pose_optimization/PoseConstraintsChecker.hpp" - -#include +#include "qp_solver/quadraticproblemsolver.h" +#include "free_gait_core/pose_optimization/poseparameterization.h" +//#include #include #include namespace free_gait { -class PoseOptimizationFunctionConstraints : public numopt_common::NonlinearFunctionConstraints +class PoseOptimizationFunctionConstraints : public qp_solver::LinearFunctionConstraints//public numopt_common::NonlinearFunctionConstraints { public: using LimbLengths = PoseConstraintsChecker::LimbLengths; @@ -39,18 +40,18 @@ class PoseOptimizationFunctionConstraints : public numopt_common::NonlinearFunct void setCenterOfMass(const Position& centerOfMassInBaseFrame); - bool getGlobalBoundConstraintMinValues(numopt_common::Vector& values); - bool getGlobalBoundConstraintMaxValues(numopt_common::Vector& values); + bool getGlobalBoundConstraintMinValues(Eigen::VectorXd& values); + bool getGlobalBoundConstraintMaxValues(Eigen::VectorXd& values); //! d <= c(p) <= f - bool getInequalityConstraintValues(numopt_common::Vector& values, - const numopt_common::Parameterization& p, + bool getInequalityConstraintValues(Eigen::VectorXd& values, + const PoseParameterization& p, bool newParams = true); - bool getInequalityConstraintMinValues(numopt_common::Vector& d); - bool getInequalityConstraintMaxValues(numopt_common::Vector& f); + bool getInequalityConstraintMinValues(Eigen::VectorXd& d); + bool getInequalityConstraintMaxValues(Eigen::VectorXd& f); - bool getLocalInequalityConstraintJacobian(numopt_common::SparseMatrix& jacobian, - const numopt_common::Parameterization& params, bool newParams = true); + bool getLocalInequalityConstraintJacobian(Eigen::MatrixXd& jacobian, + const PoseParameterization& params, bool newParams = true); private: void updateNumberOfInequalityConstraints(); @@ -64,13 +65,13 @@ class PoseOptimizationFunctionConstraints : public numopt_common::NonlinearFunct grid_map::Polygon supportRegion_; size_t nLimbLengthInequalityConstraints_; - numopt_common::Vector limbLengthInequalityConstraintsMinValues_; - numopt_common::Vector limbLengthInequalityConstraintsMaxValues_; + Eigen::VectorXd limbLengthInequalityConstraintsMinValues_; + Eigen::VectorXd limbLengthInequalityConstraintsMaxValues_; //! A from A*x <= b. Eigen::MatrixXd supportRegionInequalityConstraintGlobalJacobian_; //! b from A*x <= b. - numopt_common::Vector supportRegionInequalityConstraintsMaxValues_; + Eigen::VectorXd supportRegionInequalityConstraintsMaxValues_; }; } /* namespace */ diff --git a/free_gait_core/include/free_gait_core/pose_optimization/PoseOptimizationGeometric.hpp b/free_gait_core/include/free_gait_core/pose_optimization/PoseOptimizationGeometric.hpp index 6c09cc6..9ec4c21 100644 --- a/free_gait_core/include/free_gait_core/pose_optimization/PoseOptimizationGeometric.hpp +++ b/free_gait_core/include/free_gait_core/pose_optimization/PoseOptimizationGeometric.hpp @@ -7,7 +7,6 @@ */ #pragma once - #include "free_gait_core/pose_optimization/PoseOptimizationBase.hpp" #include "free_gait_core/TypeDefs.hpp" diff --git a/free_gait_core/include/free_gait_core/pose_optimization/PoseOptimizationObjectiveFunction.hpp b/free_gait_core/include/free_gait_core/pose_optimization/PoseOptimizationObjectiveFunction.hpp index 001426f..154949a 100644 --- a/free_gait_core/include/free_gait_core/pose_optimization/PoseOptimizationObjectiveFunction.hpp +++ b/free_gait_core/include/free_gait_core/pose_optimization/PoseOptimizationObjectiveFunction.hpp @@ -12,13 +12,14 @@ #include "free_gait_core/executor/State.hpp" #include -#include - +#include "qp_solver/quadraticproblemsolver.h" +//#include +#include "free_gait_core/pose_optimization/poseparameterization.h" #include - +using namespace qp_solver; namespace free_gait { -class PoseOptimizationObjectiveFunction : public numopt_common::NonlinearObjectiveFunction +class PoseOptimizationObjectiveFunction : public qp_solver::QuadraticObjectiveFunction//public numopt_common::NonlinearObjectiveFunction { public: /*! @@ -55,7 +56,7 @@ class PoseOptimizationObjectiveFunction : public numopt_common::NonlinearObjecti * @param newParams true if this class has already seen the parameters * @return true if successful */ - bool computeValue(numopt_common::Scalar& value, const numopt_common::Parameterization& params, + bool computeValue(double& value, const PoseParameterization& params, bool newParams = true); @@ -65,7 +66,7 @@ class PoseOptimizationObjectiveFunction : public numopt_common::NonlinearObjecti * @param newParams true if this class has already seen the parameters * @returns true if successful */ - bool getLocalGradient(numopt_common::Vector& gradient, const numopt_common::Parameterization& params, + bool getLocalGradient(Eigen::VectorXd& gradient, const PoseParameterization& params, bool newParams = true); /*! Computes the local nxn Hessian matrix of the objective function. @@ -76,7 +77,7 @@ class PoseOptimizationObjectiveFunction : public numopt_common::NonlinearObjecti * @param newParams true if this class has already seen the parameters * @returns true if successful */ - bool getLocalHessian(numopt_common::SparseMatrix& hessian, const numopt_common::Parameterization& params, + bool getLocalHessian(Eigen::MatrixXd& hessian, const PoseParameterization& params, bool newParams = true); private: diff --git a/free_gait_core/include/free_gait_core/pose_optimization/PoseOptimizationProblem.hpp b/free_gait_core/include/free_gait_core/pose_optimization/PoseOptimizationProblem.hpp index f650ba4..4606b01 100644 --- a/free_gait_core/include/free_gait_core/pose_optimization/PoseOptimizationProblem.hpp +++ b/free_gait_core/include/free_gait_core/pose_optimization/PoseOptimizationProblem.hpp @@ -11,16 +11,18 @@ #include "free_gait_core/pose_optimization/PoseOptimizationObjectiveFunction.hpp" #include "free_gait_core/pose_optimization/PoseOptimizationFunctionConstraints.hpp" -#include +//#include namespace free_gait { -class PoseOptimizationProblem : public numopt_common::ConstrainedNonlinearProblem +class PoseOptimizationProblem //: public numopt_common::ConstrainedNonlinearProblem { public: PoseOptimizationProblem(std::shared_ptr objectiveFunction, std::shared_ptr functionConstraints); virtual ~PoseOptimizationProblem(); + std::shared_ptr objectiveFunction_; + std::shared_ptr functionConstraints_; }; } /* namespace */ diff --git a/free_gait_core/include/free_gait_core/pose_optimization/PoseOptimizationQP.hpp b/free_gait_core/include/free_gait_core/pose_optimization/PoseOptimizationQP.hpp index 440b320..bf00424 100644 --- a/free_gait_core/include/free_gait_core/pose_optimization/PoseOptimizationQP.hpp +++ b/free_gait_core/include/free_gait_core/pose_optimization/PoseOptimizationQP.hpp @@ -12,12 +12,14 @@ #include "free_gait_core/pose_optimization/PoseOptimizationBase.hpp" #include -#include +#include + #include #include + namespace free_gait { class PoseOptimizationQP : public PoseOptimizationBase @@ -25,7 +27,7 @@ class PoseOptimizationQP : public PoseOptimizationBase public: PoseOptimizationQP(const AdapterBase& adapter); virtual ~PoseOptimizationQP(); - PoseOptimizationQP(const PoseOptimizationQP& other); +// PoseOptimizationQP(const PoseOptimizationQP& other); /*! * Computes the optimized pose. @@ -35,7 +37,7 @@ class PoseOptimizationQP : public PoseOptimizationBase bool optimize(Pose& pose); private: - std::unique_ptr solver_; + std::unique_ptr solver_; unsigned int nStates_; unsigned int nDimensions_; }; diff --git a/free_gait_core/include/free_gait_core/pose_optimization/PoseOptimizationSQP.hpp b/free_gait_core/include/free_gait_core/pose_optimization/PoseOptimizationSQP.hpp index 89b2e28..5e4919e 100644 --- a/free_gait_core/include/free_gait_core/pose_optimization/PoseOptimizationSQP.hpp +++ b/free_gait_core/include/free_gait_core/pose_optimization/PoseOptimizationSQP.hpp @@ -16,7 +16,9 @@ #include "free_gait_core/pose_optimization/PoseOptimizationFunctionConstraints.hpp" #include -#include +//#include +#include "qp_solver/sequencequadraticproblemsolver.h" +#include "qp_solver/quadraticproblemsolver.h" #include #include @@ -51,7 +53,7 @@ class PoseOptimizationSQP : public PoseOptimizationBase */ bool optimize(Pose& pose); - void optimizationStepCallback(const size_t iterationStep, const numopt_common::Parameterization& parameters, + void optimizationStepCallback(const size_t iterationStep, const PoseParameterization& parameters, const double functionValue, const bool finalIteration); void callExternalOptimizationStepCallbackWithPose(const Pose& pose, const size_t iterationStep, diff --git a/free_gait_core/include/free_gait_core/pose_optimization/poseparameterization.h b/free_gait_core/include/free_gait_core/pose_optimization/poseparameterization.h new file mode 100644 index 0000000..fa47131 --- /dev/null +++ b/free_gait_core/include/free_gait_core/pose_optimization/poseparameterization.h @@ -0,0 +1,62 @@ +/* + * PoseParameterization.hpp + * + * Created on: Mar 22, 2017 + * Author: Péter Fankhauser + * Institute: ETH Zurich + */ + +#pragma once + +#include "free_gait_core/TypeDefs.hpp" + +#include "qp_solver/quadraticproblemsolver.h" + +namespace free_gait { + +class PoseParameterization //: public numopt_common::Parameterization +{ + public: + PoseParameterization(); + virtual ~PoseParameterization(); + +// PoseParameterization(const PoseParameterization& other); + + qp_solver::QuadraticProblemSolver::parameters& getParams(); + const qp_solver::QuadraticProblemSolver::parameters& getParams() const; + + bool plus(qp_solver::QuadraticProblemSolver::parameters& result, + const qp_solver::QuadraticProblemSolver::parameters& p, + const qp_solver::QuadraticProblemSolver::Delta& dp) const; + + bool getTransformMatrixLocalToGlobal(Eigen::MatrixXd& matrix, + const qp_solver::QuadraticProblemSolver::parameters& params) const; + + bool getTransformMatrixGlobalToLocal(Eigen::MatrixXd& matrix, + const qp_solver::QuadraticProblemSolver::parameters& params) const; + + int getGlobalSize() const; + static const size_t getGlobalSizeStatic(); + int getLocalSize() const; + + bool setRandom(qp_solver::QuadraticProblemSolver::parameters& p) const; + bool setIdentity(qp_solver::QuadraticProblemSolver::parameters& p) const; + +// Parameterization* clone() const; + + const Pose getPose() const; + void setPose(const Pose& pose); + const Position getPosition() const; + const RotationQuaternion getOrientation() const; + + private: + const static size_t nTransGlobal_ = 3; + const static size_t nRotGlobal_ = 4; + const static size_t nTransLocal_ = 3; + const static size_t nRotLocal_ = 3; + + //! Global state vector with position and orientation. + qp_solver::QuadraticProblemSolver::parameters params_; +}; + +} /* namespace free_gait */ diff --git a/free_gait_core/include/free_gait_core/step/StepParameters.hpp b/free_gait_core/include/free_gait_core/step/StepParameters.hpp index 5b8a9c5..4837f65 100644 --- a/free_gait_core/include/free_gait_core/step/StepParameters.hpp +++ b/free_gait_core/include/free_gait_core/step/StepParameters.hpp @@ -40,7 +40,7 @@ class StepParameters struct LegModeParameters { double duration = 0.5; - std::string frameId = "base"; + std::string frameId = "base_link"; } legModeParameters; struct BaseAutoParameters @@ -54,7 +54,7 @@ class StepParameters BaseAutoParameters() { Position2 position; - position << 0.33, 0.22; + position << 0.4, 0.25; nominalPlanarStanceInBaseFrame.emplace(LimbEnum::LF_LEG, position); nominalPlanarStanceInBaseFrame.emplace(LimbEnum::RF_LEG, Position2(Eigen::Vector2d(position(0), -position(1)))); nominalPlanarStanceInBaseFrame.emplace(LimbEnum::LH_LEG, Position2(Eigen::Vector2d(-position(0), position(1)))); diff --git a/free_gait_core/include/quadruped_model/QuadrupedModel.hpp b/free_gait_core/include/quadruped_model/QuadrupedModel.hpp deleted file mode 100644 index f891008..0000000 --- a/free_gait_core/include/quadruped_model/QuadrupedModel.hpp +++ /dev/null @@ -1,104 +0,0 @@ -#pragma once - -#include "kindr/Core" -#include -using namespace romo; -namespace quadruped_model{ -typedef kindr::VectorTypeless GeneralizedCoordinates; -typedef kindr::VectorTypeless GeneralizedVelocities; -typedef kindr::VectorTypeless GeneralizedAccelerations; -typedef kindr::VectorTypeless JointPositionsLimb; -typedef kindr::VectorTypeless JointVelocitiesLimb; -typedef kindr::VectorTypeless JointAccelerationsLimb; -typedef kindr::VectorTypeless JointTorquesLimb; -typedef kindr::VectorTypeless JointPositions; -typedef kindr::VectorTypeless JointVelocities; -typedef kindr::VectorTypeless JointAccelerations; -typedef kindr::VectorTypeless JointTorques; - -class QuadrupedModel -{ -private: - /* data */ -public: - QuadrupedModel(/* args */){}; - ~QuadrupedModel(){}; - - - class QuadrupedDescription - { - private: - /* data */ - public: - QuadrupedDescription(/* args */){}; - virtual ~QuadrupedDescription(); - enum class LimbEnum - { - LF_LEG, - RF_LEG, - LH_LEG, - RH_LEG, - }; - - enum class BranchEnum - { - BASE, - LF_LEG, - RF_LEG, - LH_LEG, - RH_LEG, - }; - enum class JointNodeEnum - { - LF_LEG_HAA, - LF_LEG_HFE, - LF_LEG_KFE, - RF_LEG_HAA, - RF_LEG_HFE, - RF_LEG_KFE, - LH_LEG_HAA, - LH_LEG_HFE, - LH_LEG_KFE, - RH_LEG_HAA, - RH_LEG_HFE, - RH_LEG_KFE, - }; - - - }; - -}; - -//QuadrupedModel::QuadrupedModel(/* args */) -//{ -//} - -//QuadrupedModel::~QuadrupedModel() -//{ -//} - -class QuadrupedState -{ -public: - QuadrupedState() {}; - ~QuadrupedState() {}; - Position getPositionWorldToBaseInWorldFrame() - { - return poseInWorldFrame_.getPosition(); - } - bool setPoseBaseToWorld(const Pose pose) - { - poseInWorldFrame_ = pose; - return true; - } - RotationQuaternion getOrientationBaseToWorld() - { - return poseInWorldFrame_.getRotation(); - } -private: - Pose poseInWorldFrame_; - Position positionWorldToBaseInWorldFrame_; -}; - - -} diff --git a/free_gait_core/package.xml b/free_gait_core/package.xml index 8d6cce6..6c82edd 100644 --- a/free_gait_core/package.xml +++ b/free_gait_core/package.xml @@ -13,9 +13,7 @@ eigen quadruped_model - numopt_common - numopt_quadprog - numopt_sqp + qp_solver curves grid_map_core robot_utils diff --git a/free_gait_core/src/base_motion/BaseAuto.cpp b/free_gait_core/src/base_motion/BaseAuto.cpp index 0f30b70..eacd037 100644 --- a/free_gait_core/src/base_motion/BaseAuto.cpp +++ b/free_gait_core/src/base_motion/BaseAuto.cpp @@ -96,17 +96,23 @@ bool BaseAuto::prepareComputation(const State& state, const Step& step, const St return false; } } + std::cout<<"computed height is "<<*height_< footholdsOrdered; getFootholdsCounterClockwiseOrdered(footholdsInSupport_, footholdsOrdered); + for (auto foothold : footholdsOrdered) { supportRegion.addVertex(foothold.vector().head<2>()); +// std::cout<<"footholds ordered is "<()<setStance(footholdsToReach_); poseOptimizationGeometric_->setSupportStance(footholdsInSupport_); @@ -307,6 +313,7 @@ bool BaseAuto::computeHeight(const State& state, const StepQueue& queue, const A } if (n == 0) return false; height_.reset(new double(heightSum / (double)(n))); + std::cout<<"height sum is "<( // QD::getLimbStartIndexInJ(limb)) @@ -125,18 +131,36 @@ const JointPositionsLeg State::getJointPositionsForLimb(const LimbEnum& limb) co void State::setJointPositionsForLimb(const LimbEnum& limb, const JointPositionsLeg& jointPositions) { -//TODO(Shunyao): fix state feedback - // quadruped_model::QuadrupedState::getJointPositions().setSegment( -// QD::getLimbStartIndexInJ(limb), jointPositions); +//TODO(Shunyao): fix state feedback, why can not use n replaced 3? +// int start, n; +// start = QD::getLimbStartIndexInJ(limb); +// n = QD::getNumDofLimb(); + // TODO(Shunyao) : something wrong with setSegment? +// JointPositions J; +// J.setZero(); + +// J = getJointPositions(); +// J.setSegment<3>(QD::getLimbStartIndexInJ(limb), jointPositions); + quadruped_model::QuadrupedState::getJointPositions().setSegment<3>( + QD::getLimbStartIndexInJ(limb), jointPositions); +// std::cout<<"***********************************"<( // QD::getLimbStartIndexInJ(limb) @@ -145,18 +169,23 @@ const JointVelocitiesLeg State::getJointVelocitiesForLimb(const LimbEnum& limb) void State::setJointVelocitiesForLimb(const LimbEnum& limb, const JointVelocitiesLeg& jointVelocities) { -// quadruped_model::QuadrupedState::getJointVelocities().setSegment( -// QD::getLimbStartIndexInJ(limb), jointVelocities); + quadruped_model::QuadrupedState::getJointVelocities().setSegment<3>( + QD::getLimbStartIndexInJ(limb), jointVelocities); } void State::setAllJointVelocities(const JointVelocities& jointVelocities) { -// quadruped_model::QuadrupedState::setJointVelocities(quadruped_model::JointVelocities(jointVelocities.vector())); + quadruped_model::QuadrupedState::setJointVelocities(quadruped_model::JointVelocities(jointVelocities.vector())); } const JointAccelerationsLeg State::getJointAccelerationsForLimb(const LimbEnum& limb) const { -// return JointAccelerationsLeg(jointAccelerations_.vector().segment(QD::getLimbStartIndexInJ(limb))); + int start, n; + start = QD::getLimbStartIndexInJ(limb); + n = QD::getNumDofLimb(); + return JointAccelerationsLeg(jointAccelerations_.vector().segment(start, n)); + + // return JointAccelerationsLeg(jointAccelerations_.vector().segment(QD::getLimbStartIndexInJ(limb))); } const JointAccelerations& State::getAllJointAccelerations() const @@ -166,6 +195,7 @@ const JointAccelerations& State::getAllJointAccelerations() const void State::setJointAccelerationsForLimb(const LimbEnum& limb, const JointAccelerationsLeg& jointAccelerations) { + jointAccelerations_.setSegment<3>(QD::getLimbStartIndexInJ(limb), jointAccelerations); // jointAccelerations_.setSegment(QD::getLimbStartIndexInJ(limb), jointAccelerations); } @@ -176,6 +206,10 @@ void State::setAllJointAccelerations(const JointAccelerations& jointAcceleration const JointEffortsLeg State::getJointEffortsForLimb(const LimbEnum& limb) const { + int start, n; + start = QD::getLimbStartIndexInJ(limb); + n = QD::getNumDofLimb(); + return JointEffortsLeg(getAllJointEfforts().vector().segment(start, n)); // return JointEffortsLeg(getAllJointEfforts().vector().segment(QD::getLimbStartIndexInJ(limb))); } @@ -186,6 +220,7 @@ const JointEfforts& State::getAllJointEfforts() const void State::setJointEffortsForLimb(const LimbEnum& limb, const JointEffortsLeg& jointEfforts) { + jointEfforts_.getSegment<3>(QD::getLimbStartIndexInJ(limb)) = jointEfforts; // jointEfforts_.getSegment(QD::getLimbStartIndexInJ(limb)) = jointEfforts; } @@ -201,6 +236,7 @@ const ControlSetup& State::getControlSetup(const BranchEnum& branch) const const ControlSetup& State::getControlSetup(const LimbEnum& limb) const { + return controlSetups_.at(QD::mapEnums(limb)); // return controlSetups_.at(QD::mapEnums(limb)); } @@ -214,6 +250,7 @@ bool State::isControlSetupEmpty(const BranchEnum& branch) const bool State::isControlSetupEmpty(const LimbEnum& limb) const { + return isControlSetupEmpty(QD::mapEnums(limb)); // return isControlSetupEmpty(QD::mapEnums(limb)); } @@ -224,6 +261,7 @@ void State::setControlSetup(const BranchEnum& branch, const ControlSetup& contro void State::setControlSetup(const LimbEnum& limb, const ControlSetup& controlSetup) { + controlSetups_[QD::mapEnums(limb)] =controlSetup; // controlSetups_[QD::mapEnums(limb)] = controlSetup; } @@ -239,18 +277,34 @@ void State::setEmptyControlSetup(const BranchEnum& branch) void State::setEmptyControlSetup(const LimbEnum& limb) { + setEmptyControlSetup(QD::mapEnums(limb)); // setEmptyControlSetup(QD::mapEnums(limb)); } void State::getAllJointNames(std::vector& jointNames) const { jointNames.clear(); + std::vector controller_joint_names{"front_left_1_joint", "front_left_2_joint", "front_left_3_joint" , + "front_right_1_joint", "front_right_2_joint", "front_right_3_joint", + "rear_right_1_joint", "rear_right_2_joint", "rear_right_3_joint", + "rear_left_1_joint", "rear_left_2_joint", "rear_left_3_joint"}; + jointNames.reserve(12); + jointNames = controller_joint_names; // jointNames.reserve(QD::getJointsDimension()); // for (const auto& jointKey : QD::getJointKeys()) { // jointNames.push_back(QD::mapKeyEnumToKeyName(jointKey.getEnum())); // } } +Position State::getSupportFootPosition(const LimbEnum& limb) +{ + return footHoldInSupport_.at(limb); +} +void State::setSupportFootStance(const Stance& footInSupport) +{ + footHoldInSupport_ =footInSupport; +} + std::ostream& operator<<(std::ostream& out, const State& state) { // out << static_cast(state) << std::endl; diff --git a/free_gait_core/src/leg_motion/Footstep.cpp b/free_gait_core/src/leg_motion/Footstep.cpp index aef53ee..17c6daf 100644 --- a/free_gait_core/src/leg_motion/Footstep.cpp +++ b/free_gait_core/src/leg_motion/Footstep.cpp @@ -28,8 +28,8 @@ Footstep::Footstep(LimbEnum limb) ignoreContact_(false), ignoreForPoseAdaptation_(false), isComputed_(false), - controlSetup_ { {ControlLevel::Position, true}, {ControlLevel::Velocity, true}, - {ControlLevel::Acceleration, true}, {ControlLevel::Effort, false} } + controlSetup_ { {ControlLevel::Position, true}, {ControlLevel::Velocity, false}, + {ControlLevel::Acceleration, false}, {ControlLevel::Effort, false} } { } diff --git a/free_gait_core/src/pose_optimization/PoseConstraintsChecker.cpp b/free_gait_core/src/pose_optimization/PoseConstraintsChecker.cpp index 6b177d3..cfb7234 100644 --- a/free_gait_core/src/pose_optimization/PoseConstraintsChecker.cpp +++ b/free_gait_core/src/pose_optimization/PoseConstraintsChecker.cpp @@ -28,17 +28,22 @@ void PoseConstraintsChecker::setTolerances(const double centerOfMassTolerance, c bool PoseConstraintsChecker::check(const Pose& pose) { - state_.setPoseBaseToWorld(pose); - adapter_.setInternalDataFromState(state_, false, true, false, false); // To guide IK. - if (!updateJointPositionsInState(state_)) { - return false; - } - adapter_.setInternalDataFromState(state_, false, true, false, false); + // TODO(Shunyao): findout the useness of those lines +//! state_ is a protected member in poseOptimizationBase, set it to AdapterBase, +//! implemented in AdapterDummy + state_.setPoseBaseToWorld(pose); // this member function is in the quadrupedModel class, which is a parent class of State +// adapter_.setInternalDataFromState(state_, false, true, false, false); // To guide IK. + adapter_.setInternalDataFromState(state_); // To guide IK. +// if (!updateJointPositionsInState(state_)) { +// return false; +// } +// adapter_.setInternalDataFromState(state_, false, true, false, false); // Check center of mass. grid_map::Polygon supportRegionCopy(supportRegion_); supportRegionCopy.offsetInward(centerOfMassTolerance_); if (!supportRegion_.isInside(adapter_.getCenterOfMassInWorldFrame().vector().head(2))) { + std::cout<<"Center of mass: "< maxLimbLenghts_[foot.first] + legLengthTolerance_) { + std::cout<<"Leg lenth is out of limits"< - +using namespace qp_solver; namespace free_gait { PoseOptimizationFunctionConstraints::PoseOptimizationFunctionConstraints() - : NonlinearFunctionConstraints(), + : //NonlinearFunctionConstraints(), + LinearFunctionConstraints(), nSupportRegionInequalityConstraints_(0), nLimbLengthInequalityConstraints_(0) { @@ -79,21 +79,21 @@ void PoseOptimizationFunctionConstraints::setCenterOfMass(const Position& center } bool PoseOptimizationFunctionConstraints::getGlobalBoundConstraintMinValues( - numopt_common::Vector& values) + Eigen::VectorXd& values) { values = Eigen::VectorXd::Constant(PoseParameterization::getGlobalSizeStatic(), std::numeric_limits::lowest()); return true; } bool PoseOptimizationFunctionConstraints::getGlobalBoundConstraintMaxValues( - numopt_common::Vector& values) + Eigen::VectorXd& values) { values = Eigen::VectorXd::Constant(PoseParameterization::getGlobalSizeStatic(), std::numeric_limits::max()); return true; } -bool PoseOptimizationFunctionConstraints::getInequalityConstraintValues(numopt_common::Vector& values, - const numopt_common::Parameterization& p, +bool PoseOptimizationFunctionConstraints::getInequalityConstraintValues(Eigen::VectorXd& values, + const PoseParameterization& p, bool newParams) { values.resize(getNumberOfInequalityConstraints()); @@ -122,9 +122,9 @@ bool PoseOptimizationFunctionConstraints::getInequalityConstraintValues(numopt_c return true; } -bool PoseOptimizationFunctionConstraints::getInequalityConstraintMinValues(numopt_common::Vector& d) +bool PoseOptimizationFunctionConstraints::getInequalityConstraintMinValues(Eigen::VectorXd& d) { - d = numopt_common::Vector::Constant(nInequalityConstraints_, std::numeric_limits::lowest()); + d = Eigen::VectorXd::Constant(nInequalityConstraints_, std::numeric_limits::lowest()); // Leg length. d.segment(nSupportRegionInequalityConstraints_, nLimbLengthInequalityConstraints_) = @@ -133,9 +133,9 @@ bool PoseOptimizationFunctionConstraints::getInequalityConstraintMinValues(numop return true; } -bool PoseOptimizationFunctionConstraints::getInequalityConstraintMaxValues(numopt_common::Vector& f) +bool PoseOptimizationFunctionConstraints::getInequalityConstraintMaxValues(Eigen::VectorXd& f) { - f = numopt_common::Vector::Constant(nInequalityConstraints_, std::numeric_limits::max()); + f = Eigen::VectorXd::Constant(nInequalityConstraints_, std::numeric_limits::max()); // Support region. f.segment(0, nSupportRegionInequalityConstraints_) = supportRegionInequalityConstraintsMaxValues_; @@ -148,7 +148,7 @@ bool PoseOptimizationFunctionConstraints::getInequalityConstraintMaxValues(numop } bool PoseOptimizationFunctionConstraints::getLocalInequalityConstraintJacobian( - numopt_common::SparseMatrix& jacobian, const numopt_common::Parameterization& params, bool newParams) + Eigen::MatrixXd& jacobian, const PoseParameterization& params, bool newParams) { // Numerical approach. // numopt_common::SparseMatrix numericalJacobian(getNumberOfInequalityConstraints(), params.getLocalSize()); diff --git a/free_gait_core/src/pose_optimization/PoseOptimizationGeometric.cpp b/free_gait_core/src/pose_optimization/PoseOptimizationGeometric.cpp index 80eb68c..2d6fd9f 100644 --- a/free_gait_core/src/pose_optimization/PoseOptimizationGeometric.cpp +++ b/free_gait_core/src/pose_optimization/PoseOptimizationGeometric.cpp @@ -82,12 +82,16 @@ bool PoseOptimizationGeometric::optimize(Pose& pose) RotationQuaternion desiredHeading; desiredHeading.setFromVectors(Vector::UnitX().toImplementation(), desiredHeadingDirectionInWorld.vector()); - // Apply roll/pitch adaptation factor (~0.7). + // Apply roll/pitch adaptation factor (~0.7). TODO :(Shunyao: WHY?) const RotationQuaternion yawRotation(RotationVector(RotationVector(pose.getRotation()).vector().cwiseProduct(Eigen::Vector3d::UnitZ()))); const RotationQuaternion rollPitchRotation(RotationVector(0.7 * RotationVector(yawRotation.inverted() * pose.getRotation()).vector())); // pose.getRotation() = yawRotation * rollPitchRotation; // Alternative. pose.getRotation() = desiredHeading * rollPitchRotation; + EulerAnglesZyx eular_zyx(pose.getRotation()); + std::cout<<"Pose Optiazation Geometric solve result:"<(params); @@ -147,8 +147,9 @@ bool PoseOptimizationObjectiveFunction::computeValue(numopt_common::Scalar& valu return true; } -bool PoseOptimizationObjectiveFunction::getLocalGradient(numopt_common::Vector& gradient, - const numopt_common::Parameterization& params, bool newParams) +bool PoseOptimizationObjectiveFunction::getLocalGradient(Eigen::VectorXd& gradient, + const PoseParameterization& params, + bool newParams) { // Numercical approach. // numopt_common::Vector numericalGradient(params.getLocalSize()); @@ -156,7 +157,7 @@ bool PoseOptimizationObjectiveFunction::getLocalGradient(numopt_common::Vector& // std::cout << "Numerical: " << numericalGradient.transpose() << std::endl; // Analytical approach. - numopt_common::Vector analyticalGradient(params.getLocalSize()); + Eigen::VectorXd analyticalGradient(params.getLocalSize()); analyticalGradient.setZero(); const auto& poseParameterization = dynamic_cast(params); const Pose pose = poseParameterization.getPose(); @@ -192,8 +193,8 @@ bool PoseOptimizationObjectiveFunction::getLocalGradient(numopt_common::Vector& return true; } -bool PoseOptimizationObjectiveFunction::getLocalHessian(numopt_common::SparseMatrix& hessian, - const numopt_common::Parameterization& params, bool newParams) +bool PoseOptimizationObjectiveFunction::getLocalHessian(Eigen::MatrixXd& hessian, + const PoseParameterization& params, bool newParams) { // Numerical approach. // numopt_common::SparseMatrix numericalHessian(params.getLocalSize(), params.getLocalSize()); diff --git a/free_gait_core/src/pose_optimization/PoseOptimizationProblem.cpp b/free_gait_core/src/pose_optimization/PoseOptimizationProblem.cpp index 8bf8e4f..156256d 100644 --- a/free_gait_core/src/pose_optimization/PoseOptimizationProblem.cpp +++ b/free_gait_core/src/pose_optimization/PoseOptimizationProblem.cpp @@ -12,7 +12,9 @@ namespace free_gait { PoseOptimizationProblem::PoseOptimizationProblem( std::shared_ptr objectiveFunction, std::shared_ptr functionConstraints) - : ConstrainedNonlinearProblem(objectiveFunction, functionConstraints) + //: ConstrainedNonlinearProblem(objectiveFunction, functionConstraints) + : objectiveFunction_(objectiveFunction), + functionConstraints_(functionConstraints) { } diff --git a/free_gait_core/src/pose_optimization/PoseOptimizationQP.cpp b/free_gait_core/src/pose_optimization/PoseOptimizationQP.cpp index 1d75477..468e014 100644 --- a/free_gait_core/src/pose_optimization/PoseOptimizationQP.cpp +++ b/free_gait_core/src/pose_optimization/PoseOptimizationQP.cpp @@ -10,12 +10,13 @@ #include #include +#include #include -#include -#include +//#include +//#include using namespace Eigen; - +using namespace std; namespace free_gait { PoseOptimizationQP::PoseOptimizationQP(const AdapterBase& adapter) @@ -23,29 +24,30 @@ PoseOptimizationQP::PoseOptimizationQP(const AdapterBase& adapter) nStates_(3), nDimensions_(3) { - solver_.reset(new numopt_quadprog::ActiveSetFunctionMinimizer()); +// solver_.reset(new numopt_quadprog::ActiveSetFunctionMinimizer()); } PoseOptimizationQP::~PoseOptimizationQP() { } -PoseOptimizationQP::PoseOptimizationQP(const PoseOptimizationQP& other) - : PoseOptimizationBase(other), - nStates_(other.nStates_), - nDimensions_(other.nDimensions_) -{ - solver_.reset(new numopt_quadprog::ActiveSetFunctionMinimizer()); -} +//PoseOptimizationQP::PoseOptimizationQP(const PoseOptimizationQP& other) +// : PoseOptimizationBase(other), +// nStates_(other.nStates_), +// nDimensions_(other.nDimensions_) +//{ +//// solver_.reset(new numopt_quadprog::ActiveSetFunctionMinimizer()); +//} bool PoseOptimizationQP::optimize(Pose& pose) { checkSupportRegion(); - state_.setPoseBaseToWorld(pose); - adapter_.setInternalDataFromState(state_, false, true, false, false); // To guide IK. - updateJointPositionsInState(state_); - adapter_.setInternalDataFromState(state_, false, true, false, false); + state_.setPoseBaseToWorld(pose); //TODO(Shunyao): fix bug in state class + adapter_.setInternalDataFromState(state_); +// adapter_.setInternalDataFromState(state_, false, true, false, false); // To guide IK. +// updateJointPositionsInState(state_); +// adapter_.setInternalDataFromState(state_, false, true, false, false); // Compute center of mass. const Position centerOfMassInBaseFrame( @@ -53,7 +55,7 @@ bool PoseOptimizationQP::optimize(Pose& pose) adapter_.getCenterOfMassInWorldFrame())); // Problem definition: - // min Ax - b, Gx <= h + // min Ax - b, Gx <= h, x is base center (x,y,z),minimaze foothold offsets(I_r_F_hat - I_r_F) unsigned int nFeet = stance_.size(); MatrixXd A = MatrixXd::Zero(nDimensions_ * nFeet, nStates_); VectorXd b = VectorXd::Zero(nDimensions_ * nFeet); @@ -84,25 +86,42 @@ bool PoseOptimizationQP::optimize(Pose& pose) // Formulation as QP: // min 1/2 x'Px + q'x + r - MatrixXd P = 2 * A.transpose() * A; - VectorXd q = -2 * A.transpose() * b; + Eigen::MatrixXd P = 2 * A.transpose() * A; + Eigen::VectorXd q = -2 * A.transpose() * b; + // MatrixXd r = b.transpose() * b; // Not used. // Cost function. - auto costFunction = std::shared_ptr(new numopt_common::QuadraticObjectiveFunction()); - numopt_common::SparseMatrix P_sparse = P.sparseView(); + //auto costFunction = std::shared_ptr(new numopt_common::QuadraticObjectiveFunction()); + auto costFunction = std::shared_ptr(new qp_solver::QuadraticObjectiveFunction()); + + Eigen::MatrixXd P_sparse = P.sparseView(); costFunction->setGlobalHessian(P_sparse); costFunction->setLinearTerm(q); // Constraints. - auto constraints = std::shared_ptr(new numopt_common::LinearFunctionConstraints()); - numopt_common::SparseMatrix G_sparse = G.sparseView(); + //auto constraints = std::shared_ptr(new numopt_common::LinearFunctionConstraints()); + auto constraints = std::shared_ptr(new qp_solver::LinearFunctionConstraints()); + Eigen::MatrixXd G_sparse = G.sparseView(); + Eigen::MatrixXd Aeq(P.cols(), 1); + Eigen::VectorXd beq(1); constraints->setGlobalInequalityConstraintJacobian(G_sparse); - constraints->setInequalityConstraintMinValues(std::numeric_limits::lowest() * numopt_common::Vector::Ones(h.size())); + //constraints->setInequalityConstraintMinValues(std::numeric_limits::lowest() * numopt_common::Vector::Ones(h.size())); constraints->setInequalityConstraintMaxValues(h); + constraints->setGlobalEqualityConstraintJacobian(Aeq.setZero()); + constraints->setEqualityConstraintMaxValues(beq.setZero()); + // BUG(shunyao,29/11/18) only the inequality max value is equal to Matlab, + // considering the -CI ?(fixed, optimization object different, here only for position(x,y,z)) + +// cout<<"Hessian Matrix is: "<minimize(&problem, params, cost)) return false; x = params.getParams(); -// std::cout << "x: " << std::endl << x << std::endl; - + std::cout << "x: " << std::endl << x << std::endl;*/ + Eigen::VectorXd params(P.cols()); + if (!solver_->minimize(*costFunction, *constraints, params)) return false; // Return optimized pose. - pose.getPosition().vector() = x; + std::cout << "quadratic solution is : " << std::endl << params << std::endl; + pose.getPosition().vector() = params; return true; } diff --git a/free_gait_core/src/pose_optimization/PoseOptimizationSQP.cpp b/free_gait_core/src/pose_optimization/PoseOptimizationSQP.cpp index 97ce3e0..1b994e1 100644 --- a/free_gait_core/src/pose_optimization/PoseOptimizationSQP.cpp +++ b/free_gait_core/src/pose_optimization/PoseOptimizationSQP.cpp @@ -7,15 +7,16 @@ */ #include "free_gait_core/pose_optimization/PoseOptimizationSQP.hpp" -#include "free_gait_core/pose_optimization/PoseParameterization.hpp" +#include "free_gait_core/pose_optimization/poseparameterization.h" #include "free_gait_core/pose_optimization/PoseOptimizationProblem.hpp" #include #include #include -#include -#include -#include +//#include + +//#include +//#include #include @@ -34,6 +35,7 @@ PoseOptimizationSQP::PoseOptimizationSQP(const AdapterBase& adapter) for (const auto& limb : adapter_.getLimbs()) { positionsBaseToHipInBaseFrame[limb] = adapter_.getPositionBaseToHipInBaseFrame(limb); } + constraints_->setPositionsBaseToHip(positionsBaseToHipInBaseFrame); timer_.setAlpha(1.0); @@ -70,9 +72,12 @@ bool PoseOptimizationSQP::optimize(Pose& pose) constraints_->setLimbLengthConstraints(minLimbLenghts_, maxLimbLenghts_); state_.setPoseBaseToWorld(pose); +// adapter_.setInternalDataFromState(state_); adapter_.setInternalDataFromState(state_, false, true, false, false); // To guide IK. updateJointPositionsInState(state_); // For CoM calculation. +// adapter_.setInternalDataFromState(state_); adapter_.setInternalDataFromState(state_, false, true, false, false); + const Position centerOfMassInBaseFrame(adapter_.transformPosition(adapter_.getWorldFrameId(), adapter_.getBaseFrameId(), adapter_.getCenterOfMassInWorldFrame())); objective_->setCenterOfMass(centerOfMassInBaseFrame); @@ -80,20 +85,25 @@ bool PoseOptimizationSQP::optimize(Pose& pose) callExternalOptimizationStepCallback(0); // Optimize. + PoseOptimizationProblem problem(objective_, constraints_); - std::shared_ptr qpSolver( - new numopt_quadprog::ActiveSetFunctionMinimizer); - numopt_sqp::SQPFunctionMinimizer solver(qpSolver, 30, 0.01, 3, -DBL_MAX); - solver.registerOptimizationStepCallback( - std::bind(&PoseOptimizationSQP::optimizationStepCallback, this, std::placeholders::_1, std::placeholders::_2, - std::placeholders::_3, std::placeholders::_4)); - solver.setCheckConstraints(false); - solver.setPrintOutput(false); +// std::shared_ptr qpSolver( +// new numopt_quadprog::ActiveSetFunctionMinimizer); +// numopt_sqp::SQPFunctionMinimizer solver(qpSolver, 30, 0.01, 3, -DBL_MAX); +// solver.registerOptimizationStepCallback( +// std::bind(&PoseOptimizationSQP::optimizationStepCallback, this, std::placeholders::_1, std::placeholders::_2, +// std::placeholders::_3, std::placeholders::_4)); +// solver.setCheckConstraints(false); +// solver.setPrintOutput(false); + auto quadratic_solver = std::shared_ptr(new QuadraticProblemSolver()); + sqp_solver::SequenceQuadraticProblemSolver solver(quadratic_solver, 0.05, 30); PoseParameterization params; params.setPose(pose); - double functionValue; - if (!solver.minimize(&problem, params, functionValue)) return false; +// double functionValue; +// if (!solver.minimize(&problem, params, functionValue)) return false; + if(!solver.minimize(problem, params)) return false; pose = params.getPose(); + // TODO Fix unit quaternion? timer_.splitTime("total"); @@ -101,7 +111,7 @@ bool PoseOptimizationSQP::optimize(Pose& pose) } void PoseOptimizationSQP::optimizationStepCallback(const size_t iterationStep, - const numopt_common::Parameterization& parameters, + const PoseParameterization& parameters, const double functionValue, const bool finalIteration) { @@ -154,7 +164,8 @@ void PoseOptimizationSQP::callExternalOptimizationStepCallback(const size_t iter { if (optimizationStepCallback_) { timer_.pinTime("callback"); - adapter_.setInternalDataFromState(state_, false, true, false, false); +// adapter_.setInternalDataFromState(state_, false, true, false, false); + adapter_.setInternalDataFromState(state_); State previewState(state_); updateJointPositionsInState(previewState); optimizationStepCallback_(iterationStep, previewState, functionValue, finalIteration); diff --git a/free_gait_core/src/pose_optimization/poseparameterization.cpp b/free_gait_core/src/pose_optimization/poseparameterization.cpp new file mode 100644 index 0000000..f5a0433 --- /dev/null +++ b/free_gait_core/src/pose_optimization/poseparameterization.cpp @@ -0,0 +1,139 @@ +/* + * PoseParameterization.cpp + * + * Created on: Mar 22, 2017 + * Author: Péter Fankhauser + * Institute: ETH Zurich, Robotic Systems Lab + */ + +#include "free_gait_core/pose_optimization/poseparameterization.h" + +namespace free_gait { + +PoseParameterization::PoseParameterization() +{ + setIdentity(params_); +} + +PoseParameterization::~PoseParameterization() +{ +} + +//PoseParameterization::PoseParameterization(const PoseParameterization& other) +// : params_(other.params_) +//{ +//} + +qp_solver::QuadraticProblemSolver::parameters& PoseParameterization::getParams() +{ + return params_; +} + +const qp_solver::QuadraticProblemSolver::parameters& PoseParameterization::getParams() const +{ + return params_; +} + +bool PoseParameterization::plus(qp_solver::QuadraticProblemSolver::parameters& result, + const qp_solver::QuadraticProblemSolver::parameters& p, + const qp_solver::QuadraticProblemSolver::Delta& dp) const +{ + // Position. + result.head(nTransGlobal_) = p.head(nTransGlobal_) + dp.head(nTransLocal_); + + // // Orientation. + //// result.tail(nRotGlobal_) = RotationQuaternion(p.tail(nRotGlobal_)).boxPlus( + //// dp.tail(nRotLocal_)).vector(); +// Eigen::VectorXd p1(7); +// result.tail(nRotGlobal_) = RotationQuaternion(p1.tail(nRotGlobal_)).boxPlus(dp.tail(nRotLocal_)).vector()); + result.tail(nRotGlobal_) = RotationQuaternion(p.tail(nRotGlobal_)).boxPlus(dp.tail(nRotLocal_)).vector(); + return true; +} + +bool PoseParameterization::getTransformMatrixLocalToGlobal(Eigen::MatrixXd& matrix, + const qp_solver::QuadraticProblemSolver::parameters& params) const +{ + Eigen::MatrixXd denseMatrix(Eigen::MatrixXd::Zero(getGlobalSize(), getLocalSize())); + denseMatrix.topLeftCorner(nTransGlobal_, nTransLocal_).setIdentity(); + denseMatrix.bottomRightCorner(nRotGlobal_, nRotLocal_) = + 0.5 * RotationQuaternion(params.tail(nRotGlobal_)) + .getLocalQuaternionDiffMatrix().transpose(); + matrix = denseMatrix.sparseView(); + return true; +} + +bool PoseParameterization::getTransformMatrixGlobalToLocal( + Eigen::MatrixXd& matrix, const qp_solver::QuadraticProblemSolver::parameters& params) const +{ + Eigen::MatrixXd denseMatrix(Eigen::MatrixXd::Zero(getLocalSize(), getGlobalSize())); + denseMatrix.topLeftCorner(nTransLocal_, nTransGlobal_).setIdentity(); + denseMatrix.bottomRightCorner(nRotLocal_, nRotGlobal_) = 2.0 + * RotationQuaternion(params.tail(nRotGlobal_)).getLocalQuaternionDiffMatrix(); + matrix = denseMatrix.sparseView(); + return true; +} + +int PoseParameterization::getGlobalSize() const +{ + return nTransGlobal_ + nRotGlobal_; +} + +const size_t PoseParameterization::getGlobalSizeStatic() +{ + return nTransGlobal_ + nRotGlobal_; +} + +int PoseParameterization::getLocalSize() const +{ + return nTransLocal_ + nRotLocal_; +} + +bool PoseParameterization::setRandom(qp_solver::QuadraticProblemSolver::parameters& p) const +{ + p.resize(getGlobalSize()); + p.head(nTransGlobal_).setRandom(); + RotationQuaternion randomQuaternion; + randomQuaternion.setIdentity(); //TODO(shunyao): set random + p.tail(nRotGlobal_) = randomQuaternion.vector(); + return true; +} + +bool PoseParameterization::setIdentity(qp_solver::QuadraticProblemSolver::parameters& p) const +{ + p.resize(getGlobalSize()); + p.head(nTransGlobal_).setZero(); + RotationQuaternion identityQuaternion; + identityQuaternion.setIdentity(); + std::cout<getPositionWorldToBaseInWorldFrame(); } RotationQuaternion AdapterDummy::getOrientationBaseToWorld() const { +// return RotationQuaternion(1,0,0,0); return state_->getOrientationBaseToWorld(); } @@ -214,6 +218,7 @@ Position AdapterDummy::getPositionWorldToFootInWorldFrame(const LimbEnum& limb) Position AdapterDummy::getCenterOfMassInWorldFrame() const { +// return Position(0,0,0); return state_->getPositionWorldToBaseInWorldFrame(); } @@ -257,12 +262,23 @@ JointAccelerationsLeg AdapterDummy::getJointAccelerationsFromEndEffectorLinearAc throw std::runtime_error("AdapterDummy::getJointAccelerationsFromEndEffectorLinearAccelerationInWorldFrame() is not implemented."); } -bool AdapterDummy::setInternalDataFromState(const State& state) const +bool AdapterDummy::setInternalDataFromState(const State& state, bool updateContacts, bool updatePosition, + bool updateVelocity, bool updateAcceleration) const { *state_ = state; return true; } +void AdapterDummy::createCopyOfState() const +{ + +} + +void AdapterDummy::resetToCopyOfState() const +{ + +} + const State& AdapterDummy::getState() const { return *state_; diff --git a/free_gait_core/test/AdapterDummy.hpp b/free_gait_core/test/AdapterDummy.hpp index 4558e44..8cee359 100644 --- a/free_gait_core/test/AdapterDummy.hpp +++ b/free_gait_core/test/AdapterDummy.hpp @@ -83,7 +83,12 @@ class AdapterDummy : public AdapterBase const LimbEnum& limb, const LinearAcceleration& endEffectorLinearAccelerationInWorldFrame) const; //! Hook to write data to internal robot representation from state. - virtual bool setInternalDataFromState(const State& state) const; +// virtual bool setInternalDataFromState(const State& state) const; + virtual bool setInternalDataFromState(const State& state, bool updateContacts = true, bool updatePosition = true, + bool updateVelocity = true, bool updateAcceleration = false) const; + + virtual void createCopyOfState() const; + virtual void resetToCopyOfState() const; const State& getState() const; diff --git a/free_gait_core/test/FootstepTest.cpp b/free_gait_core/test/FootstepTest.cpp index 7659152..b582a81 100644 --- a/free_gait_core/test/FootstepTest.cpp +++ b/free_gait_core/test/FootstepTest.cpp @@ -8,6 +8,8 @@ #include "free_gait_core/TypeDefs.hpp" #include "free_gait_core/leg_motion/Footstep.hpp" +#include "free_gait_core/free_gait_core.hpp" +#include "AdapterDummy.hpp" // gtest #include @@ -55,3 +57,30 @@ TEST(footstep, trianglehighLong) EXPECT_LT(position.x(), target.x() + 0.001); } } + +//TEST(executor, footstepExecute) +//{ +// AdapterDummy adapter; +// State state; +// StepParameters parameters; +// StepCompleter completer(parameters, adapter); +// StepComputer computer; +// Executor executor(completer, computer, adapter, state); +// StepRosConverter converter(adapter); +// std::vector steps; +// Step step; +// step.setId("01"); +// // Footstep +// Footstep footstep(LimbEnum::LF_LEG); +//// Position start(0.0, 0.0, 0.0); +// Position target(0.3, 0.0, 0.0); +// Vector sufaceNormal(0, 0, 1); +//// footstep.updateStartPosition(start); +// footstep.setTargetPosition("map", target); +// footstep.setProfileHeight(0.05); +// footstep.setProfileType("triangle"); +// footstep.setAverageVelocity(0.15); +// footstep.surfaceNormal_.reset(new Vector(sufaceNormal)); + + +//} diff --git a/free_gait_core/test/PoseOptimizationQpTest.cpp b/free_gait_core/test/PoseOptimizationQpTest.cpp index 6eeda3b..b0e7020 100644 --- a/free_gait_core/test/PoseOptimizationQpTest.cpp +++ b/free_gait_core/test/PoseOptimizationQpTest.cpp @@ -6,7 +6,7 @@ * Institute: ETH Zurich, Autonomous Systems Lab */ -#include "free_gait_core/pose_optimization/PoseOptimizationQP.hpp" +#include "qp_solver/pose_optimization/PoseOptimizationQP.hpp" #include "free_gait_core/TypeDefs.hpp" #include "AdapterDummy.hpp" @@ -35,6 +35,12 @@ TEST(PoseOptimizationQP, quadrupedSymmetricUnconstrained) {LimbEnum::LH_LEG, Position(-1.0, -0.5, -0.1)}, {LimbEnum::RH_LEG, Position(-1.0, 0.5, -0.1)} })); + optimization.setSupportStance(Stance({ + {LimbEnum::LF_LEG, Position(1.0, 0.5, -0.1)}, + {LimbEnum::RF_LEG, Position(1.0, -0.5, -0.1)}, + {LimbEnum::LH_LEG, Position(-1.0, -0.5, -0.1)}, + {LimbEnum::RH_LEG, Position(-1.0, 0.5, -0.1)} })); + Pose result; ASSERT_TRUE(optimization.optimize(result)); @@ -61,6 +67,12 @@ TEST(PoseOptimizationQP, quadrupedSymmetricWithOffsetUnconstrained) {LimbEnum::RF_LEG, Position(31.0, 19.5, 10.0)}, {LimbEnum::LH_LEG, Position(29.0, 19.5, 10.0)}, {LimbEnum::RH_LEG, Position(29.0, 20.5, 10.0)} })); +// add support leg + optimization.setSupportStance( Stance({ + {LimbEnum::LF_LEG, Position(31.0, 20.5, 10.0)}, + {LimbEnum::RF_LEG, Position(31.0, 19.5, 10.0)}, + {LimbEnum::LH_LEG, Position(29.0, 19.5, 10.0)}, + {LimbEnum::RH_LEG, Position(29.0, 20.5, 10.0)} })); Pose startPose; startPose.getPosition() << 30.0, 20.0, 10.0; @@ -71,36 +83,42 @@ TEST(PoseOptimizationQP, quadrupedSymmetricWithOffsetUnconstrained) kindr::assertNear(startPose.getTransformationMatrix(), result.getTransformationMatrix(), 1e-3, KINDR_SOURCE_FILE_POS); } -TEST(PoseOptimizationQP, quadrupedAsymmetricUnconstrained) -{ - AdapterDummy adapter; - PoseOptimizationQP optimization(adapter); - - optimization.setNominalStance( Stance({ - {LimbEnum::LF_LEG, Position(1.0, 0.5, 0.0)}, - {LimbEnum::RF_LEG, Position(1.0, -0.5, 0.0)}, - {LimbEnum::LH_LEG, Position(-1.0, -0.5, 0.0)}, - {LimbEnum::RH_LEG, Position(-1.0, 0.5, 0.0)} })); - - optimization.setStance( Stance({ - {LimbEnum::LF_LEG, Position(2.0, 0.5, 0.0)}, - {LimbEnum::RF_LEG, Position(1.0, -0.5, 0.0)}, - {LimbEnum::LH_LEG, Position(-1.0, -0.5, 0.0)}, - {LimbEnum::RH_LEG, Position(-1.0, 0.5, 0.0)} })); - - Pose result; - ASSERT_TRUE(optimization.optimize(result)); +//TEST(PoseOptimizationQP, quadrupedAsymmetricUnconstrained) +//{ +// AdapterDummy adapter; +// PoseOptimizationQP optimization(adapter); + +// optimization.setNominalStance( Stance({ +// {LimbEnum::LF_LEG, Position(1.0, 0.5, 0.0)}, +// {LimbEnum::RF_LEG, Position(1.0, -0.5, 0.0)}, +// {LimbEnum::LH_LEG, Position(-1.0, -0.5, 0.0)}, +// {LimbEnum::RH_LEG, Position(-1.0, 0.5, 0.0)} })); + +// optimization.setStance( Stance({ +// {LimbEnum::LF_LEG, Position(2.0, 0.5, 0.0)}, +// {LimbEnum::RF_LEG, Position(1.0, -0.5, 0.0)}, +// {LimbEnum::LH_LEG, Position(-1.0, -0.5, 0.0)}, +// {LimbEnum::RH_LEG, Position(-1.0, 0.5, 0.0)} })); + +// optimization.setSupportStance( Stance({ +// {LimbEnum::LF_LEG, Position(2.0, 0.5, 0.0)}, +// {LimbEnum::RF_LEG, Position(1.0, -0.5, 0.0)}, +// {LimbEnum::LH_LEG, Position(-1.0, -0.5, 0.0)}, +// {LimbEnum::RH_LEG, Position(-1.0, 0.5, 0.0)} })); + +// Pose result; +// ASSERT_TRUE(optimization.optimize(result)); - Eigen::Matrix3d expectedOrientation; - expectedOrientation << 1.0, 0.1, 0.0, - -0.1, 1.0, 0.0, - 0.0, 0.0, 1.0; +// Eigen::Matrix3d expectedOrientation; +// expectedOrientation << 1.0, 0.1, 0.0, +// -0.1, 1.0, 0.0, +// 0.0, 0.0, 1.0; - Eigen::Vector3d expectedPosition(0.25, 0.0, 0.0); +// Eigen::Vector3d expectedPosition(0.25, 0.0, 0.0); - kindr::assertNear(expectedOrientation, RotationMatrix(result.getRotation()).matrix(), 1e-2, KINDR_SOURCE_FILE_POS); - kindr::assertNear(expectedPosition, result.getPosition().vector(), 1e-3, KINDR_SOURCE_FILE_POS); -} +// kindr::assertNear(expectedOrientation, RotationMatrix(result.getRotation()).matrix(), 1e-2, KINDR_SOURCE_FILE_POS); +// kindr::assertNear(expectedPosition, result.getPosition().vector(), 1e-3, KINDR_SOURCE_FILE_POS); +//} TEST(PoseOptimizationQP, quadrupedSymmetricWithYawUnconstrained) { @@ -120,6 +138,8 @@ TEST(PoseOptimizationQP, quadrupedSymmetricWithYawUnconstrained) footPositions[LimbEnum::LH_LEG] = rotation.rotate(Position(-1.0, -0.5, 0.0)); footPositions[LimbEnum::RH_LEG] = rotation.rotate(Position(-1.0, 0.5, 0.0)); optimization.setStance(footPositions); +// + optimization.setSupportStance(footPositions); Pose startPose; startPose.getRotation() = rotation; diff --git a/free_gait_core/test/PoseOptimizationSQPTest.cpp b/free_gait_core/test/PoseOptimizationSQPTest.cpp index 8675773..eddffff 100644 --- a/free_gait_core/test/PoseOptimizationSQPTest.cpp +++ b/free_gait_core/test/PoseOptimizationSQPTest.cpp @@ -7,18 +7,18 @@ */ #include "free_gait_core/TypeDefs.hpp" -#include "free_gait_core/pose_optimization/PoseOptimizationSQP.hpp" -#include "free_gait_core/pose_optimization/PoseOptimizationObjectiveFunction.hpp" -#include "free_gait_core/pose_optimization/PoseParameterization.hpp" +#include "qp_solver/pose_optimization/PoseOptimizationSQP.hpp" +#include "qp_solver/pose_optimization/PoseOptimizationObjectiveFunction.hpp" +#include "qp_solver/pose_optimization/poseparameterization.h" #include "AdapterDummy.hpp" #include #include #include #include -#include -#include -#include +//#include +//#include +//#include #include @@ -28,7 +28,7 @@ TEST(PoseOptimizationSQP, PoseParameterization) { PoseParameterization result, p; p.setRandom(p.getParams()); - numopt_common::Vector dp = numopt_common::Vector::Random(p.getLocalSize()); + Eigen::VectorXd dp = Eigen::VectorXd::Random(p.getLocalSize()); result.plus(result.getParams(), p.getParams(), dp); Position translation = result.getPosition() - p.getPosition(); RotationVector rotation(result.getOrientation().boxMinus(p.getOrientation())); @@ -43,6 +43,7 @@ TEST(PoseOptimizationSQP, ObjectiveFunction) // state.setPoseBaseToWorld(Pose(Position(0.0, 0.0, 0.1), RotationQuaternion())); // state.setRandom(); // adapter.setInternalDataFromState(state); + PoseOptimizationObjectiveFunction objective; Stance nominalStance; nominalStance[LimbEnum::LF_LEG] = Position(0.3, 0.2, -0.5); @@ -57,14 +58,25 @@ TEST(PoseOptimizationSQP, ObjectiveFunction) currentStance[LimbEnum::RH_LEG] = Position(-0.3, -0.2, 0.0); objective.setStance(currentStance); + // Define support region. + grid_map::Polygon supportRegion; + std::vector footholdsOrdered; + getFootholdsCounterClockwiseOrdered(currentStance, footholdsOrdered); + for (auto foothold : footholdsOrdered) { + supportRegion.addVertex(foothold.vector().head<2>()); + } + objective.setSupportRegion(supportRegion); PoseParameterization params; + params.setIdentity(params.getParams()); - numopt_common::Scalar value; +// std::cout<<"Get Here"<()); + } + objective.setSupportRegion(supportRegion); objective.setStance(currentStance); objective.computeValue(value, params); // Each leg has 0.4 m error. @@ -92,22 +111,37 @@ TEST(PoseOptimizationSQP, ObjectiveFunction) TEST(PoseOptimizationSQP, OptimizationSquareUp) { AdapterDummy adapter; - PoseOptimizationSQP optimization(adapter, adapter.getState()); - + PoseOptimizationSQP optimization(adapter); + PoseOptimizationBase::LimbLengths minLimbLenghts_, maxLimbLenghts_; optimization.setNominalStance(Stance({ - {LimbEnum::LF_LEG, Position(1.0, 0.5, -0.4)}, - {LimbEnum::RF_LEG, Position(1.0, -0.5, -0.4)}, - {LimbEnum::LH_LEG, Position(-1.0, 0.5, -0.4)}, - {LimbEnum::RH_LEG, Position(-1.0, -0.5, -0.4)} })); + {LimbEnum::LF_LEG, Position(0.3, 0.2, -0.4)}, + {LimbEnum::RF_LEG, Position(0.3, -0.2, -0.4)}, + {LimbEnum::LH_LEG, Position(-0.3, 0.2, -0.4)}, + {LimbEnum::RH_LEG, Position(-0.3, -0.2, -0.4)} })); Stance stance; - stance[LimbEnum::LF_LEG] = Position(1.0, 0.5, -0.1); - stance[LimbEnum::RF_LEG] = Position(1.0, -0.5, -0.1); - stance[LimbEnum::LH_LEG] = Position(-1.0, 0.5, -0.1); - stance[LimbEnum::RH_LEG] = Position(-1.0, -0.5, -0.1); + stance[LimbEnum::LF_LEG] = Position(0.3, 0.2, -0.1); + stance[LimbEnum::RF_LEG] = Position(0.3, -0.2, -0.1); + stance[LimbEnum::LH_LEG] = Position(-0.3, 0.2, -0.1); + stance[LimbEnum::RH_LEG] = Position(-0.3, -0.2, -0.1); optimization.setStance(stance); + optimization.setSupportStance(stance); + // Define support region. + grid_map::Polygon supportRegion; + std::vector footholdsOrdered; + getFootholdsCounterClockwiseOrdered(stance, footholdsOrdered); + for (auto foothold : footholdsOrdered) { + supportRegion.addVertex(foothold.vector().head<2>()); + } + optimization.setSupportRegion(supportRegion); + // Define min./max. leg lengths. + for (const auto& limb : adapter.getLimbs()) { + minLimbLenghts_[limb] = 0.2; // TODO Make as parameters. + maxLimbLenghts_[limb] = 0.565; // Foot leaves contact. // 0.6 + } + optimization.setLimbLengthConstraints(minLimbLenghts_, maxLimbLenghts_); - Pose result; + Pose result = Pose(Position(0.0, 0.0, 0.3), RotationQuaternion()); ASSERT_TRUE(optimization.optimize(result)); Position expectedPosition(0.0, 0.0, 0.3); @@ -117,11 +151,20 @@ TEST(PoseOptimizationSQP, OptimizationSquareUp) // Translation only. result.setIdentity(); - expectedPosition += Position(0.5, 0.25, 0.0); + expectedPosition += Position(0.3, 0.2, 0.0); + result = Pose(expectedPosition, RotationQuaternion()); Stance translatedStance(stance); for (auto& stance : translatedStance) { stance.second += Position(expectedPosition.x(), expectedPosition.y(), 0.0); } + footholdsOrdered.clear(); + supportRegion.removeVertices(); + getFootholdsCounterClockwiseOrdered(translatedStance, footholdsOrdered); + for (auto foothold : footholdsOrdered) { + supportRegion.addVertex(foothold.vector().head<2>()); + } + optimization.setSupportRegion(supportRegion); + optimization.setStance(translatedStance); ASSERT_TRUE(optimization.optimize(result)); kindr::assertNear(expectedOrientation.matrix(), RotationMatrix(result.getRotation()).matrix(), 1e-2, KINDR_SOURCE_FILE_POS); @@ -130,13 +173,25 @@ TEST(PoseOptimizationSQP, OptimizationSquareUp) // Add yaw rotation. for (double yawRotation = 0.0; yawRotation <= 45.0; yawRotation += 10.0) { std::cerr << "yaw Rotation ====" << yawRotation << std::endl; - result.setIdentity(); +// result.setIdentity(); + expectedOrientation = EulerAnglesZyx(yawRotation/180.0 * M_PI, 0.0, 0.0); + result = Pose(expectedPosition, RotationQuaternion()); Stance rotatedStance(stance); for (auto& stance : rotatedStance) { stance.second = expectedOrientation.rotate(stance.second); stance.second += Position(expectedPosition.x(), expectedPosition.y(), 0.0); } + + footholdsOrdered.clear(); + supportRegion.removeVertices(); + getFootholdsCounterClockwiseOrdered(rotatedStance, footholdsOrdered); + for (auto foothold : footholdsOrdered) { + supportRegion.addVertex(foothold.vector().head<2>()); + } + optimization.setSupportRegion(supportRegion); + + optimization.setStance(rotatedStance); ASSERT_TRUE(optimization.optimize(result)); kindr::assertNear(expectedOrientation.matrix(), RotationMatrix(result.getRotation()).matrix(), 1e-2, KINDR_SOURCE_FILE_POS); diff --git a/free_gait_marker/CHANGELOG.rst b/free_gait_marker/CHANGELOG.rst new file mode 100644 index 0000000..7b21f6b --- /dev/null +++ b/free_gait_marker/CHANGELOG.rst @@ -0,0 +1,11 @@ +^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Changelog for package free_gait_marker +^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ + +Forthcoming +----------- + +0.1.1 (2017-01-31) +------------------ +* Initial release. +* Contributors: Peter Fankhauser, Dario Bellicoso diff --git a/free_gait_marker/CMakeLists.txt b/free_gait_marker/CMakeLists.txt new file mode 100644 index 0000000..6338303 --- /dev/null +++ b/free_gait_marker/CMakeLists.txt @@ -0,0 +1,123 @@ +cmake_minimum_required(VERSION 2.8.3) +project(free_gait_marker) + +set(CMAKE_CXX_FLAGS "-std=c++11 ${CMAKE_CXX_FLAGS}") + +## 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 + roscpp + interactive_markers + visualization_msgs + geometry_msgs + free_gait_msgs + curves +) + +find_package(Eigen3 REQUIRED) + +################################### +## catkin specific configuration ## +################################### +## The catkin_package macro generates cmake config files for your package +## Declare things to be passed to dependent projects +## INCLUDE_DIRS: uncomment this if you package contains header files +## LIBRARIES: libraries you create in this project that dependent projects also need +## CATKIN_DEPENDS: catkin_packages dependent projects also need +## DEPENDS: system dependencies of this project that dependent projects also need +catkin_package( +# INCLUDE_DIRS include +# LIBRARIES starleth_marker + CATKIN_DEPENDS + interactive_markers + roscpp + visualization_msgs + geometry_msgs + free_gait_msgs + curves + DEPENDS + eigen +) + +########### +## Build ## +########### + +## Specify additional locations of header files +## Your package locations should be listed before other locations +include_directories( + include + ${catkin_INCLUDE_DIRS} + ${EIGEN3_INCLUDE_DIR} +) + +## Declare a cpp executable +add_executable( + ${PROJECT_NAME} + src/free_gait_marker_node.cpp + src/marker_manager/MarkerManager.cpp + src/markers/MarkerFoot.cpp + src/markers/MarkerKnot.cpp +) + +## Add cmake target dependencies of the executable/library +## as an example, message headers may need to be generated before nodes +#add_dependencies( +# ${PROJECT_NAME} +# ${step_actionlib_EXPORTED_TARGETS} +#) + +## Specify libraries to link a library or executable target against + target_link_libraries( + ${PROJECT_NAME} + ${catkin_LIBRARIES} + ) + +############# +## 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 +# install(PROGRAMS +# scripts/my_python_script +# DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} +# ) + +install(TARGETS ${PROJECT_NAME} + ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} + LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} + RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} +) + +install(DIRECTORY include/${PROJECT_NAME}/ + DESTINATION ${CATKIN_PACKAGE_INCLUDE_DESTINATION} +) + +install(DIRECTORY launch config + DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION} +) + +## Mark other files for installation (e.g. launch and bag files, etc.) +# install(FILES +# # myfile1 +# # myfile2 +# DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION} +# ) + +############# +## Testing ## +############# + +## Add gtest based cpp test target and link libraries +# catkin_add_gtest(${PROJECT_NAME}-test test/test_starleth_marker.cpp) +# if(TARGET ${PROJECT_NAME}-test) +# target_link_libraries(${PROJECT_NAME}-test ${PROJECT_NAME}) +# endif() + +## Add folders to be run by python nosetests +# catkin_add_nosetests(test) diff --git a/free_gait_marker/config/locomotion_controller.yaml b/free_gait_marker/config/locomotion_controller.yaml new file mode 100644 index 0000000..be837e6 --- /dev/null +++ b/free_gait_marker/config/locomotion_controller.yaml @@ -0,0 +1,20 @@ +action_server_topic: "/free_gait/execute_steps" +wait_for_action_timeout: 5 +foothold: + frame_id: "map" + scale: 0.075 + radius: 0.06 + color: + r: 0.0 + g: 0.0 + b: 0.65 + a: 1.0 +knot: + frame_id: "map" + scale: 0.075 + radius: 0.03 + color: + r: 1.0 + g: 0.0 + b: 0.0 + a: 1.0 diff --git a/free_gait_marker/include/free_gait_marker/marker_manager/MarkerManager.hpp b/free_gait_marker/include/free_gait_marker/marker_manager/MarkerManager.hpp new file mode 100644 index 0000000..cb69812 --- /dev/null +++ b/free_gait_marker/include/free_gait_marker/marker_manager/MarkerManager.hpp @@ -0,0 +1,205 @@ +/* + * MarkerManager.hpp + * + * Created on: Feb 28, 2015 + * Author: Péter Fankhauser, Dario Bellicoso + * Institute: ETH Zurich, Autonomous Systems Lab + */ + +#pragma once + +// ROS +#include +#include +#include +#include +#include +#include +#include +#include +#include + +// Kindr ROS plugins +#include + +// STD +#include +#include + +// Curves +#include + +// Markers +#include "free_gait_marker/markers/MarkerKnot.hpp" +#include "free_gait_marker/markers/MarkerFoot.hpp" + +namespace free_gait_marker { + +class MarkerManager +{ + public: + + /*! + * Constructor. + * @param nodeHandle the ROS node handle. + */ + MarkerManager(ros::NodeHandle& nodeHandle); + + virtual ~MarkerManager(); + + /*! + * Definition of the container for the step data. + */ + struct Foothold + { + //! Step number. + unsigned int stepNumber; + + //! Leg name identifier. + std::string legName; + + //! Name of the interactive marker. + std::string markerName; + + //! Frame id of the corresponding foot. + std::string footFrameId; + + //! True if active, false otherwise. + bool isActive; + + bool operator<(const Foothold& foothold) const + { + return stepNumber < foothold.stepNumber; + } + }; + + void loadManagerParameters(); + + //Note: This change will not take effect until you call applyChanges() + void addFootholdMarker(const unsigned int stepNumber, + const std::string& legName); + + void addKnotMarker(const unsigned int markerNumber, + const std::string& markerName, + const geometry_msgs::Pose& pose); + + //Note: This change will not take effect until you call applyChanges() + bool activateFoothold(Foothold& foothold); + + //Note: This change will not take effect until you call applyChanges() + bool deactivateFoothold(Foothold& foothold); + + //Note: This change will not take effect until you call applyChanges() + bool deactivateAllFootholds(); + + void applyChanges(); + + MarkerManager::Foothold& getFootholdByMarkerName(const std::string& markerName); + + bool getMarkerFromFoothold(const MarkerManager::Foothold& foothold, + markers::MarkerFoot& marker); + + void publishKnots(); + void updateKnots(); + + private: + void footholdCallback(const visualization_msgs::InteractiveMarkerFeedbackConstPtr &feedback); + void knotCallback(const visualization_msgs::InteractiveMarkerFeedbackConstPtr &feedback); + + bool sendStepGoal(); + + void print(); + + void setupMenus(); + + //! ROS node handle. + ros::NodeHandle& nodeHandle_; + + //! Interactive marker server. + interactive_markers::InteractiveMarkerServer server_; + + //! Menu handler. + interactive_markers::MenuHandler footMenuHandler_; + interactive_markers::MenuHandler knotMenuHandler_; + + //! Step action client. + std::unique_ptr> stepActionClient_; + + //! TF listener. + tf::TransformListener transformListener_; + + //! Foothold marker list. + std::vector footholdList_; + + //! Topic name of the step action server. + std::string actionServerTopic_; + + //! Timeout duration for the connection to the action server. + ros::Duration waitForActionTimeout_; + + //! Foothold frame id. + std::string footholdFrameId_; + + //! Foothold marker scale. + double footholdScale_; + + //! Foothold marker radius. + double footholdRadius_; + + //! Foothold marker color. + std_msgs::ColorRGBA footholdColor_; + + //! Trajectory points markers. +// std::vector trajectories_; + std::vector trajectories_; + + //! Knot ids. + std::vector knotIds_; + + //! Trajectory points ids. + std::vector trajectoryIds_; + + std::vector showTrajectory_; + + //! Container of curves. + std::vector splines_; + + //! Knot publisher. + ros::Publisher knotsPublisher_; + + //! Trajectory publisher. + ros::Publisher trajectoriesPublisher_; + + void clearTrajectory(int legId); + void updateTrajectory(int legId, const std::string& markerName); + + + /****************** + * Menu callbacks * + ******************/ + //:: foothold marker callbacks + void activateCallback(const visualization_msgs::InteractiveMarkerFeedbackConstPtr &feedback); + + // step + void sendStepCallback(const visualization_msgs::InteractiveMarkerFeedbackConstPtr &feedback); + void resetCallback(const visualization_msgs::InteractiveMarkerFeedbackConstPtr &feedback); + // trajectory + void setKnotCallback(const visualization_msgs::InteractiveMarkerFeedbackConstPtr &feedback); + void sendTrajectoryCallback(const visualization_msgs::InteractiveMarkerFeedbackConstPtr &feedback); + void clearTrajectoryCallback(const visualization_msgs::InteractiveMarkerFeedbackConstPtr &feedback); + void showTrajectoryCallback(const visualization_msgs::InteractiveMarkerFeedbackConstPtr &feedback); + void deleteKnotsCallback(const visualization_msgs::InteractiveMarkerFeedbackConstPtr &feedback); + + //:: knot marker callbacks + void knotMenuUpdateCallback(const visualization_msgs::InteractiveMarkerFeedbackConstPtr &feedback); + void knotMenuResetCallback(const visualization_msgs::InteractiveMarkerFeedbackConstPtr &feedback); + void knotMenuDeleteCallback(const visualization_msgs::InteractiveMarkerFeedbackConstPtr &feedback); + /******************/ + + void getIdAndNameFromMarker(const visualization_msgs::InteractiveMarkerFeedbackConstPtr &feedback, + int& legId, + std::string& name); + +}; + +} /* namespace free_gait_marker */ diff --git a/free_gait_marker/include/free_gait_marker/markers/MarkerFoot.hpp b/free_gait_marker/include/free_gait_marker/markers/MarkerFoot.hpp new file mode 100644 index 0000000..61b629a --- /dev/null +++ b/free_gait_marker/include/free_gait_marker/markers/MarkerFoot.hpp @@ -0,0 +1,47 @@ +/* + * MarkerFoot.hpp + * + * Created on: Mar 18, 2015 + * Author: Dario Bellicoso + */ + +#pragma once + +#include +#include + +namespace free_gait_marker { +namespace markers { + +class MarkerFoot : public visualization_msgs::InteractiveMarker +{ + public: + MarkerFoot(); + virtual ~MarkerFoot(); + + // Setup the marker. + void setupFootholdMarker(const unsigned int stepNumber, + const std::string& legName); + + // Load marker parameters + void loadParameters( + ros::NodeHandle& nodeHandle); + + private: + //! Foothold frame id. + std::string footholdFrameId_; + + //! Foothold marker scale. + double footholdScale_; + + //! Foothold marker radius. + double footholdRadius_; + + //! Foothold marker color. + std_msgs::ColorRGBA footholdColor_; + +}; + +} /* namespace markers */ +} /* namespace free_gait_marker */ + diff --git a/free_gait_marker/include/free_gait_marker/markers/MarkerKnot.hpp b/free_gait_marker/include/free_gait_marker/markers/MarkerKnot.hpp new file mode 100644 index 0000000..18b3fe1 --- /dev/null +++ b/free_gait_marker/include/free_gait_marker/markers/MarkerKnot.hpp @@ -0,0 +1,45 @@ +/* + * MarkerKnot.hpp + * + * Created on: Mar 18, 2015 + * Author: Dario Bellicoso + */ + +#pragma once + +#include +#include + +namespace free_gait_marker { +namespace markers { + +class MarkerKnot : public visualization_msgs::InteractiveMarker +{ + public: + MarkerKnot(); + virtual ~MarkerKnot(); + + // Setup the marker. + void setupMarker(const unsigned int markerNumber, + const std::string& markerName); + + // Load marker parameters + void loadParameters( + ros::NodeHandle& nodeHandle); + + private: + //! Foothold marker scale. + double markerScale_; + + //! Foothold marker radius. + double markerRadius_; + + //! Foothold marker color. + std_msgs::ColorRGBA markerColor_; + + //! Timeout duration for the connection to the action server. + ros::Duration waitForActionTimeout_; +}; + +} /* namespace markers */ +} /* namespace free_gait_marker */ diff --git a/free_gait_marker/launch/free_gait_marker.launch b/free_gait_marker/launch/free_gait_marker.launch new file mode 100644 index 0000000..6c03ef6 --- /dev/null +++ b/free_gait_marker/launch/free_gait_marker.launch @@ -0,0 +1,12 @@ + + + + + + + + + + + + diff --git a/free_gait_marker/package.xml b/free_gait_marker/package.xml new file mode 100644 index 0000000..73ee437 --- /dev/null +++ b/free_gait_marker/package.xml @@ -0,0 +1,22 @@ + + + free_gait_marker + 0.3.0 + Interactive Rviz markers for + Peter Fankhauser + Peter Fankhauser + BSD + https://github.com/leggedrobotics/free_gait + https://github.com/leggedrobotics/free_gait/issues + + catkin + + roscpp + interactive_markers + visualization_msgs + geometry_msgs + free_gait_msgs + curves + eigen + multi_dof_joint_trajectory_rviz_plugins + diff --git a/free_gait_marker/src/free_gait_marker_node.cpp b/free_gait_marker/src/free_gait_marker_node.cpp new file mode 100644 index 0000000..fe42112 --- /dev/null +++ b/free_gait_marker/src/free_gait_marker_node.cpp @@ -0,0 +1,29 @@ +/* + * free_gait_marker_node.cpp + * + * Created on: Feb 18, 2015 + * Author: Péter Fankhauser, Dario Bellicoso + * Institute: ETH Zurich, Autonomous Systems Lab + */ + +#include + +#include "free_gait_marker/marker_manager/MarkerManager.hpp" + +int main(int argc, char** argv) +{ + ros::init(argc, argv, "free_gait_marker"); + + ros::NodeHandle nodeHandle("~"); + + free_gait_marker::MarkerManager markerManager(nodeHandle); + + ros::Rate loop_rate(30); + while (ros::ok()) { + ros::spinOnce(); + markerManager.publishKnots(); + loop_rate.sleep(); + } + + return 0; +} diff --git a/free_gait_marker/src/marker_manager/MarkerManager.cpp b/free_gait_marker/src/marker_manager/MarkerManager.cpp new file mode 100644 index 0000000..15d620e --- /dev/null +++ b/free_gait_marker/src/marker_manager/MarkerManager.cpp @@ -0,0 +1,609 @@ +/* + * MarkerManager.cpp + * + * Created on: Feb 28, 2015 + * Author: Péter Fankhauser, Dario Bellicoso + * Institute: ETH Zurich, Autonomous Systems Lab + */ + +#include "free_gait_marker/marker_manager/MarkerManager.hpp" + +// ROS +#include +#include + +// STD +#include +#include + +using namespace interactive_markers; +using namespace visualization_msgs; + +static const int NUMBER_OF_LEGS = 4; + +static const std::string LEFT_FORE_NAME = "LF_LEG"; +static const int LEFT_FORE_ID = 0; +static const std::string RIGHT_FORE_NAME = "RF_LEG"; +static const int RIGHT_FORE_ID = 1; +static const std::string LEFT_HIND_NAME = "LH_LEG"; +static const int LEFT_HIND_ID = 2; +static const std::string RH_LEG_NAME = "RH_LEG"; +static const int RH_LEG_ID = 3; + +namespace free_gait_marker { + +MarkerManager::MarkerManager(ros::NodeHandle& nodeHandle) + : nodeHandle_(nodeHandle), + server_(ros::this_node::getName()), + knotIds_(NUMBER_OF_LEGS, 0), + trajectoryIds_(NUMBER_OF_LEGS, 0), + splines_(NUMBER_OF_LEGS), + trajectories_(NUMBER_OF_LEGS), + showTrajectory_(NUMBER_OF_LEGS, false) +{ + loadManagerParameters(); + + stepActionClient_ = std::unique_ptr< + actionlib::SimpleActionClient>( + new actionlib::SimpleActionClient( + actionServerTopic_, true)); + + setupMenus(); + +// trajectoriesPublisher_ = +// nodeHandle.advertise("trajectory_points", +// 100); + + trajectoriesPublisher_ = + nodeHandle.advertise("trajectory_points", + 100); + + + // Add an interactive marker for each foot + addFootholdMarker(LEFT_FORE_ID, LEFT_FORE_NAME); + addFootholdMarker(RIGHT_FORE_ID, RIGHT_FORE_NAME); + addFootholdMarker(LEFT_HIND_ID, LEFT_HIND_NAME); + addFootholdMarker(RH_LEG_ID, RH_LEG_NAME); + applyChanges(); + + // By default, the foot markers are initially deactivated + for (auto& foothold : footholdList_) + deactivateFoothold(foothold); + applyChanges(); +} + +MarkerManager::~MarkerManager() +{ +} + +void MarkerManager::setupMenus() +{ + /********************************** + * Setup foot marker context menu * + **********************************/ + footMenuHandler_.insert( + "Activate", boost::bind(&MarkerManager::activateCallback, this, _1)); + footMenuHandler_.insert("Reset", + boost::bind(&MarkerManager::resetCallback, this, _1)); + + interactive_markers::MenuHandler::EntryHandle stepHandle = footMenuHandler_ + .insert("Step"); + interactive_markers::MenuHandler::EntryHandle trajHandle = footMenuHandler_ + .insert("Trajectory"); + + footMenuHandler_.insert( + stepHandle, "Send step goal", + boost::bind(&MarkerManager::sendStepCallback, this, _1)); + + footMenuHandler_.insert( + trajHandle, "Add knot", + boost::bind(&MarkerManager::setKnotCallback, this, _1)); + footMenuHandler_.insert( + trajHandle, "Clear", + boost::bind(&MarkerManager::clearTrajectoryCallback, this, _1)); + footMenuHandler_.insert( + trajHandle, "Show", + boost::bind(&MarkerManager::showTrajectoryCallback, this, _1)); + footMenuHandler_.insert( + trajHandle, "Send", + boost::bind(&MarkerManager::sendTrajectoryCallback, this, _1)); + footMenuHandler_.insert( + trajHandle, "Clear knots", + boost::bind(&MarkerManager::deleteKnotsCallback, this, _1)); + /**********************************/ + + /********************************** + * Setup knot marker context menu * + **********************************/ +// knotMenuHandler_.insert( +// "Update", boost::bind(&MarkerManager::knotMenuUpdateCallback, this, _1)); +// knotMenuHandler_.insert( +// "Reset", boost::bind(&MarkerManager::knotMenuResetCallback, this, _1)); + knotMenuHandler_.insert( + "Delete", boost::bind(&MarkerManager::knotMenuDeleteCallback, this, _1)); + /**********************************/ +} + +void MarkerManager::activateCallback( + const visualization_msgs::InteractiveMarkerFeedbackConstPtr &feedback) +{ + if (!activateFoothold(getFootholdByMarkerName(feedback->marker_name.c_str()))) + ROS_WARN_STREAM( + "Foothold marker with name '" << feedback->marker_name.c_str() << "' could not be activated."); + applyChanges(); +} + +void MarkerManager::sendStepCallback( + const visualization_msgs::InteractiveMarkerFeedbackConstPtr &feedback) +{ + sendStepGoal(); + applyChanges(); +} + +void MarkerManager::resetCallback( + const visualization_msgs::InteractiveMarkerFeedbackConstPtr &feedback) +{ + if (!deactivateFoothold( + getFootholdByMarkerName(feedback->marker_name.c_str()))) + ROS_WARN_STREAM( + "Foothold marker with name '" << feedback->marker_name.c_str() << "' could not be deactivated."); + applyChanges(); +} + +void MarkerManager::setKnotCallback( + const visualization_msgs::InteractiveMarkerFeedbackConstPtr &feedback) +{ + const int legId = getFootholdByMarkerName(feedback->marker_name.c_str()) + .stepNumber; + std::string knotMarkerName { feedback->marker_name + "_knot_" + + std::to_string(knotIds_[legId]) }; + addKnotMarker(knotIds_[legId]++, knotMarkerName, feedback->pose); + applyChanges(); +} + +void MarkerManager::sendTrajectoryCallback( + const visualization_msgs::InteractiveMarkerFeedbackConstPtr &feedback) +{ + +} + +void MarkerManager::clearTrajectoryCallback( + const visualization_msgs::InteractiveMarkerFeedbackConstPtr &feedback) +{ + const int legId = getFootholdByMarkerName(feedback->marker_name.c_str()) + .stepNumber; + showTrajectory_[legId] = false; + clearTrajectory(legId); +} + +void MarkerManager::showTrajectoryCallback( + const visualization_msgs::InteractiveMarkerFeedbackConstPtr &feedback) +{ + const int legId = getFootholdByMarkerName(feedback->marker_name.c_str()) + .stepNumber; + showTrajectory_[legId] = true; + updateTrajectory(legId, feedback->marker_name); +} + +void MarkerManager::knotMenuUpdateCallback( + const visualization_msgs::InteractiveMarkerFeedbackConstPtr &feedback) +{ + int legId = -1; + std::string name = ""; + getIdAndNameFromMarker(feedback, legId, name); + updateTrajectory(legId, name); +} + +void MarkerManager::deleteKnotsCallback( + const visualization_msgs::InteractiveMarkerFeedbackConstPtr &feedback) +{ + int legId = -1; + std::string name = ""; + getIdAndNameFromMarker(feedback, legId, name); + + for (size_t k = 0; k < knotIds_.size(); k++) { + server_.erase(feedback->marker_name + "_" + "knot_" + std::to_string(k)); + applyChanges(); + } + + clearTrajectory(legId); +} + +void MarkerManager::getIdAndNameFromMarker( + const visualization_msgs::InteractiveMarkerFeedbackConstPtr &feedback, + int& legId, std::string& name) +{ + if (feedback->marker_name.find(LEFT_FORE_NAME) != std::string::npos) { + legId = LEFT_FORE_ID; + name = LEFT_FORE_NAME; + } else if (feedback->marker_name.find(RIGHT_FORE_NAME) != std::string::npos) { + legId = RIGHT_FORE_ID; + name = RIGHT_FORE_NAME; + } else if (feedback->marker_name.find(LEFT_HIND_NAME) != std::string::npos) { + legId = LEFT_HIND_ID; + name = LEFT_HIND_NAME; + } else if (feedback->marker_name.find(RH_LEG_NAME) != std::string::npos) { + legId = RH_LEG_ID; + name = RH_LEG_NAME; + } else { + ROS_INFO_STREAM( + "[MarkerManager::getIdAndNameFromMarker] Could not find id and name."); + return; + } +} + +void MarkerManager::knotMenuResetCallback( + const visualization_msgs::InteractiveMarkerFeedbackConstPtr &feedback) +{ + +} + +void MarkerManager::knotMenuDeleteCallback( + const visualization_msgs::InteractiveMarkerFeedbackConstPtr &feedback) +{ + server_.erase(feedback->marker_name); + applyChanges(); + int legId = -1; + std::string name = ""; + getIdAndNameFromMarker(feedback, legId, name); + updateTrajectory(legId, name); +} + +void MarkerManager::loadManagerParameters() +{ + nodeHandle_.param("foothold/frame_id", footholdFrameId_, std::string("map")); + nodeHandle_.param("action_server_topic", actionServerTopic_, + std::string("/locomotion_controller/step")); + double duration; + nodeHandle_.param("wait_for_action_timeout", duration, 5.0); + waitForActionTimeout_.fromSec(duration); + nodeHandle_.param("foothold/frame_id", footholdFrameId_, std::string("map")); +} + +void MarkerManager::addFootholdMarker(const unsigned int stepNumber, + const std::string& legName) +{ + markers::MarkerFoot marker; + marker.loadParameters(nodeHandle_); + marker.setupFootholdMarker(stepNumber, legName); + + std::string footFrameId; + if (legName == LEFT_FORE_NAME) + footFrameId = "LF_FOOT"; + else if (legName == RIGHT_FORE_NAME) + footFrameId = "RF_FOOT"; + else if (legName == LEFT_HIND_NAME) + footFrameId = "LH_FOOT"; + else if (legName == RH_LEG_NAME) + footFrameId = "RH_FOOT"; + else { + ROS_WARN_STREAM( + ("No corresponding frame id for the foot found for leg name `" + legName + + "'.").c_str()); + return; + } + + footholdList_.emplace_back(Foothold { stepNumber, legName, marker.name, + footFrameId, true }); + server_.insert(marker, + boost::bind(&MarkerManager::footholdCallback, this, _1)); + footMenuHandler_.apply(server_, marker.name); +} + +void MarkerManager::addKnotMarker(const unsigned int markerNumber, + const std::string& markerName, + const geometry_msgs::Pose& pose) +{ + markers::MarkerKnot marker; + marker.loadParameters(nodeHandle_); + marker.setupMarker(markerNumber, markerName); + + server_.insert(marker, boost::bind(&MarkerManager::knotCallback, this, _1)); + applyChanges(); + + knotMenuHandler_.apply(server_, marker.name); + applyChanges(); + + // set knot pose + std_msgs::Header header; + header.frame_id = "map"; + header.stamp = ros::Time(0.0); + if (!server_.setPose(markerName, pose, header)) { + ROS_WARN_STREAM("Marker with name '" << markerName << "' not found."); + } + applyChanges(); + +} + +void MarkerManager::clearTrajectory(int legId) +{ + trajectories_[legId].points.clear(); + trajectories_[legId].joint_names.clear(); + trajectories_[legId].header.stamp = ros::Time::now(); + trajectories_[legId].header.frame_id = "map"; + + splines_[legId].clear(); + trajectoryIds_[legId] = 0; +} + + +void MarkerManager::updateTrajectory(int legId, const std::string& markerName) +{ + if (!showTrajectory_[legId]) + return; + + std::vector tData; + std::vector> vData; + double tf = 0.0; + + clearTrajectory(legId); + + tData.clear(); + vData.clear(); + + for (size_t k = 0; k < knotIds_[legId]; k++) { + std::string knotName = markerName + "_knot_" + std::to_string(k); + markers::MarkerKnot marker; + if (server_.get(knotName, marker)) { + tData.push_back(tf); + Eigen::Matrix vpoint; + vpoint << marker.pose.position.x, marker.pose.position.y, marker.pose + .position.z; + vData.push_back(vpoint); + // todo: set tf appropriately + tf += 1.0; + } + } + + // at least two knots are needed to fit a trajectory + if (tData.size() > 1) { + + splines_.at(legId).fitCurve(tData, vData); + double dt = 0.1; + + for (int k = 0; k * dt < splines_.at(legId).getMaxTime(); k++) { + trajectory_msgs::MultiDOFJointTrajectoryPoint point; + geometry_msgs::Transform transform; + geometry_msgs::Twist twist_vel; + geometry_msgs::Twist twist_acc; + + Eigen::Vector3d position; + splines_.at(legId).evaluate(position, k*dt); + transform.translation.x = position.x(); + transform.translation.y = position.y(); + transform.translation.z = position.z(); + + Eigen::Vector3d linearVelocity; + splines_.at(legId).evaluateDerivative(linearVelocity, k*dt,1); + twist_vel.linear.x = linearVelocity.x(); + twist_vel.linear.y = linearVelocity.y(); + twist_vel.linear.z = linearVelocity.z(); + + ROS_INFO_STREAM("vel: " << twist_vel.linear.x); + + Eigen::Vector3d linearAcc; + splines_.at(legId).evaluateDerivative(linearAcc, k*dt,2); + twist_acc.linear.x = linearAcc.x(); + twist_acc.linear.y = linearAcc.y(); + twist_acc.linear.z = linearAcc.z(); + + + point.transforms.push_back(transform); + point.velocities.push_back(twist_vel); + point.accelerations.push_back(twist_acc); + + point.time_from_start = ros::Duration(k*dt); + + trajectories_.at(legId).points.push_back(point); + + trajectories_.at(legId).header.stamp = ros::Time::now(); + trajectories_.at(legId).header.frame_id = "map"; + trajectories_.at(legId).joint_names.push_back(std::to_string(k)); + + } + } + +} + + +void MarkerManager::footholdCallback( + const visualization_msgs::InteractiveMarkerFeedbackConstPtr &feedback) +{ +// Foothold& foothold(getFootholdByMarkerName(feedback->marker_name.c_str())); +// if (foothold.isActive) return; +// if (!activateFoodhold(foothold)) +// ROS_WARN_STREAM("Foothold marker with name `%s` could not be activated.", +// feedback->marker_name.c_str()); +// applyChanges(); +} + +void MarkerManager::knotCallback( + const visualization_msgs::InteractiveMarkerFeedbackConstPtr &feedback) +{ + int legId = -1; + std::string name = ""; + getIdAndNameFromMarker(feedback, legId, name); + updateTrajectory(legId, name); + applyChanges(); +} + +bool MarkerManager::activateFoothold(Foothold& foothold) +{ + if (foothold.isActive) + return true; + + // Get pose. + markers::MarkerFoot marker; + if (!getMarkerFromFoothold(foothold, marker)) + return false; + tf::Stamped positionInPassiveFrame; // Foot frame. + tf::Stamped positionInActiveFrame; // Map frame. + tf::pointMsgToTF(marker.pose.position, positionInPassiveFrame); + positionInPassiveFrame.frame_id_ = foothold.footFrameId; + positionInPassiveFrame.stamp_ = ros::Time(0.0); + transformListener_.transformPoint(footholdFrameId_, positionInPassiveFrame, + positionInActiveFrame); + geometry_msgs::Pose pose; + tf::pointTFToMsg(positionInActiveFrame, pose.position); + + // Get header. + std_msgs::Header header; + header.frame_id = positionInActiveFrame.frame_id_; + header.stamp = positionInActiveFrame.stamp_; + + // Update marker pose. + if (!server_.setPose(foothold.markerName, pose, header)) { + ROS_WARN_STREAM( + "Marker with name '" << foothold.markerName.c_str() << "' not found."); + return false; + } + + foothold.isActive = true; + return true; +} + +bool MarkerManager::deactivateFoothold(Foothold& foothold) +{ + std_msgs::Header header; + header.frame_id = foothold.footFrameId; + header.stamp = ros::Time(0.0); + if (!server_.setPose(foothold.markerName, geometry_msgs::Pose(), header)) { + ROS_WARN_STREAM( + "Marker with name '" << foothold.markerName.c_str() << "' not found."); + return false; + } + + foothold.isActive = false; + return true; +} + +bool MarkerManager::deactivateAllFootholds() +{ + for (auto& foothold : footholdList_) { + if (!deactivateFoothold(foothold)) + return false; + } + return true; +} + +void MarkerManager::applyChanges() +{ + server_.applyChanges(); +} + +MarkerManager::Foothold& MarkerManager::getFootholdByMarkerName( + const std::string& markerName) +{ + for (auto& foothold : footholdList_) { + if (foothold.markerName == markerName) { + return foothold; + } + } + throw std::out_of_range( + "Foothold with marker name " + markerName + " not found."); +} + +bool MarkerManager::getMarkerFromFoothold( + const MarkerManager::Foothold& foothold, markers::MarkerFoot& marker) +{ + if (!server_.get(foothold.markerName, marker)) { + ROS_WARN_STREAM( + "Marker with name '" << foothold.markerName.c_str() << "' not found."); + return false; + } + return true; +} + +bool MarkerManager::sendStepGoal() +{ + if (!stepActionClient_->isServerConnected()) { + ROS_INFO("Waiting for step action server to start."); + if (!stepActionClient_->waitForServer(waitForActionTimeout_)) { + ROS_WARN_STREAM("No step action server found, ignoring action."); + return false; + } + } + + std::sort(footholdList_.begin(), footholdList_.end()); + free_gait_msgs::ExecuteStepsGoal goal; + + for (const auto& foothold : footholdList_) { + markers::MarkerFoot marker; + if (!getMarkerFromFoothold(foothold, marker)) { + ROS_WARN_STREAM("Goal not sent."); + return false; + } + + if (!foothold.isActive) + continue; + free_gait_msgs::Step preStep; + free_gait_msgs::BaseAuto baseMotion; + baseMotion.height = 0.42; + preStep.base_auto.push_back(baseMotion); + goal.steps.push_back(preStep); + + free_gait_msgs::Step step; + free_gait_msgs::Footstep footstep; + footstep.name = foothold.legName; + footstep.target.header.frame_id = footholdFrameId_; + footstep.target.point = marker.pose.position; +// swingData.profile.type = "square"; + step.footstep.push_back(footstep); + goal.steps.push_back(step); + } + + stepActionClient_->sendGoal(goal); + stepActionClient_->waitForResult(); + deactivateAllFootholds(); +// actionlib::SimpleClientGoalState state = ac.getState(); + + return true; +} + +void MarkerManager::publishKnots() +{ +// for (auto& msg : trajectories_) { +// visualization_msgs::MarkerArrayPtr msgPtr( +// new visualization_msgs::MarkerArray(msg)); +// trajectoriesPublisher_.publish( +// visualization_msgs::MarkerArrayConstPtr(msgPtr)); +// } + + for (auto& msg : trajectories_) { + trajectory_msgs::MultiDOFJointTrajectoryPtr msgPtr( + new trajectory_msgs::MultiDOFJointTrajectory(msg)); + trajectoriesPublisher_.publish( + trajectory_msgs::MultiDOFJointTrajectoryConstPtr(msgPtr)); + } + +} + +void MarkerManager::updateKnots() { + if (showTrajectory_[LEFT_FORE_ID]) { + updateTrajectory(LEFT_FORE_ID, LEFT_FORE_NAME); + } + if (showTrajectory_[RIGHT_FORE_ID]) { + updateTrajectory(RIGHT_FORE_ID, RIGHT_FORE_NAME); + } + if (showTrajectory_[LEFT_HIND_ID]) { + updateTrajectory(LEFT_HIND_ID, LEFT_HIND_NAME); + } + if (showTrajectory_[RH_LEG_ID]) { + updateTrajectory(RH_LEG_ID, RH_LEG_NAME); + } +} + +void MarkerManager::print() +{ + ROS_INFO_STREAM("Foothold List:" << std::endl); + for (const auto& foothold : footholdList_) { + ROS_INFO_STREAM("Step Nr.: " << foothold.stepNumber); + ROS_INFO_STREAM("Leg Name: " << foothold.legName); + ROS_INFO_STREAM("Marker Name: " << foothold.markerName); + ROS_INFO_STREAM("Foot Frame ID: " << foothold.footFrameId); + ROS_INFO_STREAM( + "Is Active: " << (foothold.isActive ? "Yes" : "No") << std::endl); + } +} + +} /* namespace free_gait_marker */ diff --git a/free_gait_marker/src/markers/MarkerFoot.cpp b/free_gait_marker/src/markers/MarkerFoot.cpp new file mode 100644 index 0000000..9b37234 --- /dev/null +++ b/free_gait_marker/src/markers/MarkerFoot.cpp @@ -0,0 +1,125 @@ +/* + * MarkerFoot.cpp + * + * Created on: Mar 18, 2015 + * Author: Dario Bellicoso + */ + +#include "free_gait_marker/markers/MarkerFoot.hpp" + +namespace free_gait_marker { +namespace markers { + +MarkerFoot::MarkerFoot() : + footholdScale_(0), + footholdRadius_(0) +{ + // TODO Auto-generated constructor stub +} + +MarkerFoot::~MarkerFoot() +{ + // TODO Auto-generated destructor stub +} + + +void MarkerFoot::loadParameters( + ros::NodeHandle& nodeHandle) +{ + nodeHandle.param("foothold/scale", footholdScale_, 0.075); + nodeHandle.param("foothold/radius", footholdRadius_, 0.07); + double color; + nodeHandle.param("foothold/color/r", color, 0.0); + footholdColor_.r = (float) color; + nodeHandle.param("foothold/color/g", color, 0.0); + footholdColor_.g = (float) color; + nodeHandle.param("foothold/color/b", color, 0.65); + footholdColor_.b = (float) color; + nodeHandle.param("foothold/color/a", color, 1.0); + footholdColor_.a = (float) color; +} + + +void MarkerFoot::setupFootholdMarker(const unsigned int stepNumber, + const std::string& legName) +{ + controls.clear(); + menu_entries.clear(); + + header.frame_id = footholdFrameId_; +// name = "foothold_" + std::to_string(stepNumber) + "_" + legName; + name = legName; + description = "Foothold " + std::to_string(stepNumber) + " " + legName; + scale = footholdScale_; + + // Add a control which contains the sphere and the menu. + visualization_msgs::InteractiveMarkerControl sphereControl; + sphereControl.name = name + "_menu"; + sphereControl.always_visible = true; + // Set the control to drag the sphere in space + sphereControl.orientation_mode = visualization_msgs::InteractiveMarkerControl::VIEW_FACING; + sphereControl.interaction_mode = visualization_msgs::InteractiveMarkerControl::MOVE_PLANE; + + visualization_msgs::Marker sphereMarker; + sphereMarker.type = visualization_msgs::Marker::SPHERE; + double radius = footholdRadius_; + sphereMarker.scale.x = radius; + sphereMarker.scale.y = radius; + sphereMarker.scale.z = radius; + sphereMarker.color = footholdColor_; + sphereControl.markers.push_back(sphereMarker); + + controls.push_back(sphereControl); + + // Add interactive controls. + visualization_msgs::InteractiveMarkerControl control; + control.orientation_mode = visualization_msgs::InteractiveMarkerControl::FIXED; + + // rotate and move about x axis + control.orientation.w = 1; + control.orientation.x = 1; + control.orientation.y = 0; + control.orientation.z = 0; + + control.name = "rotate_x"; + control.interaction_mode = visualization_msgs::InteractiveMarkerControl::ROTATE_AXIS; + controls.push_back(control); + + control.name = "move_x"; + control.interaction_mode = visualization_msgs::InteractiveMarkerControl::MOVE_AXIS; + controls.push_back(control); + + + // rotate and move about y axis + control.orientation.w = 1; + control.orientation.x = 0; + control.orientation.y = 1; + control.orientation.z = 0; + + control.name = "rotate_y"; + control.interaction_mode = visualization_msgs::InteractiveMarkerControl::ROTATE_AXIS; + controls.push_back(control); + + control.name = "move_y"; + control.interaction_mode = visualization_msgs::InteractiveMarkerControl::MOVE_AXIS; + controls.push_back(control); + + + // rotate and move about z axis + control.orientation.w = 1; + control.orientation.x = 0; + control.orientation.y = 0; + control.orientation.z = 1; + + control.name = "rotate_z"; + control.interaction_mode = visualization_msgs::InteractiveMarkerControl::ROTATE_AXIS; + controls.push_back(control); + + control.name = "move_z"; + control.interaction_mode = visualization_msgs::InteractiveMarkerControl::MOVE_AXIS; + controls.push_back(control); +} + + +} /* namespace markers */ +} /* namespace free_gait_marker */ diff --git a/free_gait_marker/src/markers/MarkerKnot.cpp b/free_gait_marker/src/markers/MarkerKnot.cpp new file mode 100644 index 0000000..f314259 --- /dev/null +++ b/free_gait_marker/src/markers/MarkerKnot.cpp @@ -0,0 +1,123 @@ +/* + * MarkerKnot.cpp + * + * Created on: Mar 18, 2015 + * Author: Dario Bellicoso + */ + +#include "free_gait_marker/markers/MarkerKnot.hpp" + +namespace free_gait_marker { +namespace markers { + +MarkerKnot::MarkerKnot() : + markerScale_(0), + markerRadius_(0) +{ + // TODO Auto-generated constructor stub + +} + +MarkerKnot::~MarkerKnot() +{ + // TODO Auto-generated destructor stub +} + +void MarkerKnot::loadParameters( + ros::NodeHandle& nodeHandle) +{ + nodeHandle.param("knot/scale", markerScale_, 0.075); + nodeHandle.param("knot/radius", markerRadius_, 0.03); + + double color; + nodeHandle.param("knot/color/r", color, 1.0); + markerColor_.r = (float) color; + nodeHandle.param("knot/color/g", color, 0.0); + markerColor_.g = (float) color; + nodeHandle.param("knot/color/b", color, 0.0); + markerColor_.b = (float) color; + nodeHandle.param("knot/color/a", color, 1.0); + markerColor_.a = (float) color; +} + +// Setup the marker. +void MarkerKnot::setupMarker(const unsigned int markerNumber, + const std::string& markerName) { + controls.clear(); + menu_entries.clear(); + + header.frame_id = "map"; + name = markerName; + description = "Knot: " + std::to_string(markerNumber) + " " + markerName; + scale = markerScale_; + + // Add a control which contains the sphere and the menu. + visualization_msgs::InteractiveMarkerControl sphereControl; + sphereControl.name = name + "_menu"; + sphereControl.always_visible = true; + // Set the control to drag the sphere in space + sphereControl.orientation_mode = visualization_msgs::InteractiveMarkerControl::VIEW_FACING; + sphereControl.interaction_mode = visualization_msgs::InteractiveMarkerControl::MOVE_PLANE; + + visualization_msgs::Marker sphereMarker; + sphereMarker.type = visualization_msgs::Marker::SPHERE; + double radius = markerRadius_; + sphereMarker.scale.x = radius; + sphereMarker.scale.y = radius; + sphereMarker.scale.z = radius; + sphereMarker.color = markerColor_; + sphereControl.markers.push_back(sphereMarker); + + controls.push_back(sphereControl); + + // Add interactive controls. + visualization_msgs::InteractiveMarkerControl control; + control.orientation_mode = visualization_msgs::InteractiveMarkerControl::FIXED; + + // rotate and move about x axis + control.orientation.w = 1; + control.orientation.x = 1; + control.orientation.y = 0; + control.orientation.z = 0; + +// control.name = "rotate_x"; +// control.interaction_mode = visualization_msgs::InteractiveMarkerControl::ROTATE_AXIS; +// controls.push_back(control); + + control.name = "move_x"; + control.interaction_mode = visualization_msgs::InteractiveMarkerControl::MOVE_AXIS; + controls.push_back(control); + + + // rotate and move about y axis + control.orientation.w = 1; + control.orientation.x = 0; + control.orientation.y = 1; + control.orientation.z = 0; + +// control.name = "rotate_y"; +// control.interaction_mode = visualization_msgs::InteractiveMarkerControl::ROTATE_AXIS; +// controls.push_back(control); + + control.name = "move_y"; + control.interaction_mode = visualization_msgs::InteractiveMarkerControl::MOVE_AXIS; + controls.push_back(control); + + + // rotate and move about z axis + control.orientation.w = 1; + control.orientation.x = 0; + control.orientation.y = 0; + control.orientation.z = 1; + +// control.name = "rotate_z"; +// control.interaction_mode = visualization_msgs::InteractiveMarkerControl::ROTATE_AXIS; +// controls.push_back(control); + + control.name = "move_z"; + control.interaction_mode = visualization_msgs::InteractiveMarkerControl::MOVE_AXIS; + controls.push_back(control); +} + +} /* namespace markers */ +} /* namespace free_gait_marker */ diff --git a/free_gait_msgs/CHANGELOG.rst b/free_gait_msgs/CHANGELOG.rst new file mode 100644 index 0000000..6da589e --- /dev/null +++ b/free_gait_msgs/CHANGELOG.rst @@ -0,0 +1,11 @@ +^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Changelog for package free_gait_msgs +^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ + +Forthcoming +----------- + +0.1.1 (2017-01-31) +------------------ +* Initial release. +* Contributors: Peter Fankhauser diff --git a/free_gait_msgs/CMakeLists.txt b/free_gait_msgs/CMakeLists.txt new file mode 100644 index 0000000..2be1301 --- /dev/null +++ b/free_gait_msgs/CMakeLists.txt @@ -0,0 +1,120 @@ +cmake_minimum_required(VERSION 2.8.3) +project(free_gait_msgs) + +## 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 + message_generation + std_msgs + actionlib + actionlib_msgs + geometry_msgs + trajectory_msgs + sensor_msgs + nav_msgs +) + +## Generate messages in the 'msg' folder +add_message_files( + FILES + ActionDescription.msg + CollectionDescription.msg + Step.msg + BaseAuto.msg + BaseTarget.msg + BaseTrajectory.msg + Footstep.msg + LegMode.msg + EndEffectorTarget.msg + EndEffectorTrajectory.msg + JointTarget.msg + JointTrajectory.msg + CustomCommand.msg + RobotState.msg +) + +## Generate services in the 'srv' folder +add_service_files( + FILES + GetActions.srv + GetCollections.srv + SendAction.srv + SendActionSequence.srv +) + +## Generate actions in the 'action' folder +add_action_files( + FILES + ExecuteSteps.action + ExecuteAction.action +) + +## Generate added messages and services with any dependencies listed here +generate_messages( + DEPENDENCIES + std_msgs + actionlib + actionlib_msgs + geometry_msgs + trajectory_msgs + sensor_msgs + nav_msgs + ) + +################################### +## catkin specific configuration ## +################################### +## The catkin_package macro generates cmake config files for your package +## Declare things to be passed to dependent projects +## INCLUDE_DIRS: uncomment this if you package contains header files +## LIBRARIES: libraries you create in this project that dependent projects also need +## CATKIN_DEPENDS: catkin_packages dependent projects also need +## DEPENDS: system dependencies of this project that dependent projects also need +catkin_package( +# INCLUDE_DIRS include +# LIBRARIES starleth_msgs + CATKIN_DEPENDS + message_runtime + std_msgs + actionlib + actionlib_msgs + geometry_msgs + trajectory_msgs +# DEPENDS system_lib +) + +############# +## 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 +# install(PROGRAMS +# scripts/my_python_script +# DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} +# ) + +## Mark executables and/or libraries for installation +# install(TARGETS starleth_msgs starleth_msgs_node +# ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} +# LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} +# RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} +# ) + +## Mark cpp header files for installation +# install(DIRECTORY include/${PROJECT_NAME}/ +# DESTINATION ${CATKIN_PACKAGE_INCLUDE_DESTINATION} +# FILES_MATCHING PATTERN "*.h" +# PATTERN ".svn" EXCLUDE +# ) + +## Mark other files for installation (e.g. launch and bag files, etc.) +# install(FILES +# # myfile1 +# # myfile2 +# DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION} +# ) diff --git a/free_gait_msgs/action/ExecuteAction.action b/free_gait_msgs/action/ExecuteAction.action new file mode 100644 index 0000000..ba623e9 --- /dev/null +++ b/free_gait_msgs/action/ExecuteAction.action @@ -0,0 +1,23 @@ +## Goal +string action_id + +--- + +## Result +int8 RESULT_FAILED = -2 +int8 RESULT_NOT_FOUND = -1 +int8 RESULT_UNKNOWN = 0 +int8 RESULT_STARTED = 1 +int8 RESULT_DONE = 2 +int8 status + +--- + +## Feedback +int8 STATE_ERROR = -1 +int8 STATE_UNINITIALIZED = 0 +int8 STATE_INITIALIZED = 1 +int8 STATE_PENDING = 2 +int8 STATE_ACTIVE = 3 +int8 STATE_IDLE = 4 +int8 status diff --git a/free_gait_msgs/action/ExecuteSteps.action b/free_gait_msgs/action/ExecuteSteps.action new file mode 100644 index 0000000..c9a8745 --- /dev/null +++ b/free_gait_msgs/action/ExecuteSteps.action @@ -0,0 +1,52 @@ +## Goal + +free_gait_msgs/Step[] steps + +# How action reacts on preemption. +int8 PREEMPT_IMMEDIATE = -1 +int8 PREEMPT_STEP = 0 # Default. +int8 PREEMPT_NO = 1 +int8 preempt + +--- + +## Result +# See goal status. + +--- + +## Feedback + +# Unique ID of the currently active step. +# If empty, step has no ID. +string step_id + +# Step number starting from 1, monotonically increasing during +# action, resets to 1 for new goal definition. Below 1 if new +# goal has not started yet. +uint8 step_number + +# Number of steps in a new/current goal. +uint8 number_of_steps_in_goal + +# Number of steps in the queue including the current step. +uint8 queue_size + +# Current state of the step. +int8 PROGRESS_PAUSED = -1 +int8 PROGRESS_UNKNOWN = 0 +int8 PROGRESS_EXECUTING = 1 +int8 status + +# Status description ('Preparing for step.', 'Regaining contact.', etc.) +# for human interpretation. +string description + +# Duration of the current step. +duration duration + +# Phase (0-1) of the current step. +float64 phase + +# Names of the branches active in the current step ('LF_LEG', 'base', etc.). +string[] active_branches diff --git a/free_gait_msgs/msg/ActionDescription.msg b/free_gait_msgs/msg/ActionDescription.msg new file mode 100644 index 0000000..3c2fd5e --- /dev/null +++ b/free_gait_msgs/msg/ActionDescription.msg @@ -0,0 +1,14 @@ +# Action unique id. +string id + +# Descriptive name. +string name + +# Absolute path to the action file. +string file + +# Action type ('yaml', 'python', 'launch'). +string type + +# Description of the action. +string description diff --git a/free_gait_msgs/msg/BaseAuto.msg b/free_gait_msgs/msg/BaseAuto.msg new file mode 100644 index 0000000..1241e10 --- /dev/null +++ b/free_gait_msgs/msg/BaseAuto.msg @@ -0,0 +1,23 @@ +# Command to let controller add auto generated base motion +# for stability and reachability of footsteps. + +# Base height in control frame. +# If 0, current height of the robot is used. +float64 height + +# If the duration of the base motion should +# NOT be adapted to the duration of the step's leg. +# If no leg motion is defined in the step, +# the average velocities are used. +# Default is false. +bool ignore_timing_of_leg_motion + +# Average linear velocity [m/s] and angular +# velocity [rad/s] of the base motion. +# If 0, default is used. +float64 average_linear_velocity +float64 average_angular_velocity + +# Margin for the support polygon [m]. +# If 0, default is used. +float64 support_margin diff --git a/free_gait_msgs/msg/BaseTarget.msg b/free_gait_msgs/msg/BaseTarget.msg new file mode 100644 index 0000000..22a4e57 --- /dev/null +++ b/free_gait_msgs/msg/BaseTarget.msg @@ -0,0 +1,17 @@ +# Definition of a base target pose. + +# Target pose of the base by the end of the motion. +geometry_msgs/PoseStamped target + +# If the duration of the base motion should +# NOT be adapted to the duration of the step's leg. +# If no leg motion is defined in the step, +# the average velocities are used. +# Default is false. +bool ignore_timing_of_leg_motion + +# Average linear velocity [m/s] and angular +# velocity [rad/s] of the base motion. +# If 0, default is used. +float64 average_linear_velocity +float64 average_angular_velocity diff --git a/free_gait_msgs/msg/BaseTrajectory.msg b/free_gait_msgs/msg/BaseTrajectory.msg new file mode 100644 index 0000000..53eafa2 --- /dev/null +++ b/free_gait_msgs/msg/BaseTrajectory.msg @@ -0,0 +1,4 @@ +# Definition of a base motion trajectory. + +# Define the entire trajectory for the base. +trajectory_msgs/MultiDOFJointTrajectory trajectory diff --git a/free_gait_msgs/msg/CollectionDescription.msg b/free_gait_msgs/msg/CollectionDescription.msg new file mode 100644 index 0000000..b156f47 --- /dev/null +++ b/free_gait_msgs/msg/CollectionDescription.msg @@ -0,0 +1,11 @@ +# Collection unique id. +string id + +# Descriptive name. +string name + +# List of actions IDs for this collection. +string[] action_ids + +# Indicates if collection is a sequence of actions with a fixed order. +bool is_sequence diff --git a/free_gait_msgs/msg/CustomCommand.msg b/free_gait_msgs/msg/CustomCommand.msg new file mode 100644 index 0000000..76046e9 --- /dev/null +++ b/free_gait_msgs/msg/CustomCommand.msg @@ -0,0 +1,6 @@ +################### +# Defines a custom command in YAML format. + +string type +duration duration +string command diff --git a/free_gait_msgs/msg/EndEffectorTarget.msg b/free_gait_msgs/msg/EndEffectorTarget.msg new file mode 100644 index 0000000..789d5bd --- /dev/null +++ b/free_gait_msgs/msg/EndEffectorTarget.msg @@ -0,0 +1,29 @@ +# Definition of a target for the end effector. + +# Leg name ('LF_LEG', 'RH_LEG' etc.). +string name + +# Target of the end effector. +# Target can be specified as position, velocity, acceleration, +# force, or a combination of these. +geometry_msgs/PointStamped[] target_position +geometry_msgs/Vector3Stamped[] target_velocity +geometry_msgs/Vector3Stamped[] target_acceleration +geometry_msgs/Vector3Stamped[] target_force + +# Average velocity of the end effector motion [m/s]. +# Determines the duration of the motion. +# If 0, default is used. +float64 average_velocity + +# Target surface normal. +# Leave empty of no contact is expected or not known. +geometry_msgs/Vector3Stamped surface_normal + +# If contact of the end effector should be ignored. +# Default is false. +bool ignore_contact + +# If pose adaptation should ignore this leg motion. +# Default is false. +bool ignore_for_pose_adaptation diff --git a/free_gait_msgs/msg/EndEffectorTrajectory.msg b/free_gait_msgs/msg/EndEffectorTrajectory.msg new file mode 100644 index 0000000..e319a1e --- /dev/null +++ b/free_gait_msgs/msg/EndEffectorTrajectory.msg @@ -0,0 +1,21 @@ +# Definition of a trajectory for the foot. + +# Leg name ('LF_LEG', 'RH_LEG' etc.). +string name + +# Trajectory for the end effector. +# Trajectory can contain transforms, twists, or accelerations, +# or combinations of these. +trajectory_msgs/MultiDOFJointTrajectory trajectory + +# Target surface normal. +# Leave empty of no contact is expected or not known. +geometry_msgs/Vector3Stamped surface_normal + +# If contact of the end effector should be ignored. +# Default is false. +bool ignore_contact + +# If pose adaptation should ignore this leg motion. +# Default is false. +bool ignore_for_pose_adaptation \ No newline at end of file diff --git a/free_gait_msgs/msg/Footstep.msg b/free_gait_msgs/msg/Footstep.msg new file mode 100644 index 0000000..bd81672 --- /dev/null +++ b/free_gait_msgs/msg/Footstep.msg @@ -0,0 +1,31 @@ +# Step defined by foothold position and swing profile. + +# Leg name ('LF_LEG', 'RH_LEG' etc.). +string name + +# Target position of the foot by the end of the motion. +geometry_msgs/PointStamped target + +# Step apex swing heights in control frame. +# If 0, default is used. +float64 profile_height + +# Average velocity of the foot motion [m/s]. +# If 0, default is used. +float64 average_velocity + +# Type of the swing trajectory ('triangle', 'square', etc.). +# If empty, default is used. +string profile_type + +# If a contact (touchdown) of the foot at the end of the swing is not expected. +# Default is false. +bool ignore_contact + +# Target foothold surface normal. +# Leave empty of no contact is expected or not known. +geometry_msgs/Vector3Stamped surface_normal + +# If pose adaptation should ignore this leg motion. +# Default is false. +bool ignore_for_pose_adaptation diff --git a/free_gait_msgs/msg/JointTarget.msg b/free_gait_msgs/msg/JointTarget.msg new file mode 100644 index 0000000..2d2979c --- /dev/null +++ b/free_gait_msgs/msg/JointTarget.msg @@ -0,0 +1,15 @@ +# Definition of trajectories for each joint of a leg. + +# Leg name ('LF_LEG', 'RH_LEG' etc.). +string name + +# Joint target. +trajectory_msgs/JointTrajectoryPoint target + +# Target surface normal. +# Leave empty of no contact is expected or not known. +geometry_msgs/Vector3Stamped surface_normal + +# If contact of the end effector should be ignored. +# Default is false. +bool ignore_contact \ No newline at end of file diff --git a/free_gait_msgs/msg/JointTrajectory.msg b/free_gait_msgs/msg/JointTrajectory.msg new file mode 100644 index 0000000..5493d86 --- /dev/null +++ b/free_gait_msgs/msg/JointTrajectory.msg @@ -0,0 +1,15 @@ +# Definition of trajectories for each joint of a leg. + +# Leg name ('LF_LEG', 'RH_LEG' etc.). +string name + +# Joint trajectories. +trajectory_msgs/JointTrajectory trajectory + +# Target surface normal. +# Leave empty of no contact is expected or not known. +geometry_msgs/Vector3Stamped surface_normal + +# If contact of the end effector should be ignored. +# Default is false. +bool ignore_contact \ No newline at end of file diff --git a/free_gait_msgs/msg/LegMode.msg b/free_gait_msgs/msg/LegMode.msg new file mode 100644 index 0000000..f5fc6be --- /dev/null +++ b/free_gait_msgs/msg/LegMode.msg @@ -0,0 +1,19 @@ +# Switch the leg mode between support leg and non-support leg mode. + +# Leg name ('LF_LEG', 'RH_LEG' etc.). +string name + +# If leg should be a support leg or not. +bool support_leg + +# Duration of the mode switch. +# If 0, default is used. +duration duration + +# If support leg: Target foothold surface normal. +# Leave empty if non-support leg or not known. +geometry_msgs/Vector3Stamped surface_normal + +# If pose adaptation should ignore this leg motion. +# Default is false. +bool ignore_for_pose_adaptation diff --git a/free_gait_msgs/msg/RobotState.msg b/free_gait_msgs/msg/RobotState.msg new file mode 100644 index 0000000..7b5ac0b --- /dev/null +++ b/free_gait_msgs/msg/RobotState.msg @@ -0,0 +1,5 @@ +sensor_msgs/JointState lf_leg_joints +sensor_msgs/JointState rf_leg_joints +sensor_msgs/JointState rh_leg_joints +sensor_msgs/JointState lh_leg_joints +nav_msgs/Odometry base_pose diff --git a/free_gait_msgs/msg/Step.msg b/free_gait_msgs/msg/Step.msg new file mode 100644 index 0000000..334b451 --- /dev/null +++ b/free_gait_msgs/msg/Step.msg @@ -0,0 +1,41 @@ +# Unique step ID (optional). +string id + +################### +# Define leg swing motions. Each leg can be defined by one +# of the following swing types: + +# Step defined by foothold position and swing profile. +free_gait_msgs/Footstep[] footstep + +# Switch the leg mode between support leg and non-support leg mode. +free_gait_msgs/LegMode[] leg_mode + +# Move end effector to a target point, with target velocity, or acceleration. +free_gait_msgs/EndEffectorTarget[] end_effector_target + +# Move the foot along a specified trajectory. +free_gait_msgs/EndEffectorTrajectory[] end_effector_trajectory + +# Move the foot to a target point. +free_gait_msgs/JointTarget[] joint_target + +# Defines leg motion as trajectory for each joint. +free_gait_msgs/JointTrajectory[] joint_trajectory + +################### +# Define the base motion by one of the base motion types: + +# Specify if base motion auto generation is desired or not. +free_gait_msgs/BaseAuto[] base_auto + +# Move base to a target pose. +free_gait_msgs/BaseTarget[] base_target + +# Move base along a trajectory. +free_gait_msgs/BaseTrajectory[] base_trajectory + +################### +# Defines a custom command: + +free_gait_msgs/CustomCommand[] custom_command diff --git a/free_gait_msgs/package.xml b/free_gait_msgs/package.xml new file mode 100644 index 0000000..ffa2354 --- /dev/null +++ b/free_gait_msgs/package.xml @@ -0,0 +1,24 @@ + + + free_gait_msgs + 0.3.0 + ROS API definition for the Free Gait. + Peter Fankhauser + Peter Fankhauser + BSD + https://github.com/leggedrobotics/free_gait + https://github.com/leggedrobotics/free_gait/issues + + catkin + + message_generation + message_runtime + std_msgs + geometry_msgs + trajectory_msgs + actionlib + actionlib_msgs + sensor_msgs + nav_msgs + + diff --git a/free_gait_msgs/srv/GetActions.srv b/free_gait_msgs/srv/GetActions.srv new file mode 100644 index 0000000..21c2a26 --- /dev/null +++ b/free_gait_msgs/srv/GetActions.srv @@ -0,0 +1,5 @@ +# Get available actions for this collection, +# returns all actions if `collection_id` is empty. +string collection_id +--- +ActionDescription[] actions diff --git a/free_gait_msgs/srv/GetCollections.srv b/free_gait_msgs/srv/GetCollections.srv new file mode 100644 index 0000000..8ea32e3 --- /dev/null +++ b/free_gait_msgs/srv/GetCollections.srv @@ -0,0 +1,3 @@ +# Get available collections. +--- +CollectionDescription[] collections diff --git a/free_gait_msgs/srv/SendAction.srv b/free_gait_msgs/srv/SendAction.srv new file mode 100644 index 0000000..1764afc --- /dev/null +++ b/free_gait_msgs/srv/SendAction.srv @@ -0,0 +1,10 @@ +# This sends a specified action to the step action server. +# This service call is will only block until the action has been +# successfully started or failed during initialization. +# If you need feedback about the progress or successful +# execution of the action (outside of the action itself), use +# the `ExecuteAction` ROS action. + +free_gait_msgs/ExecuteActionGoal goal +--- +free_gait_msgs/ExecuteActionResult result diff --git a/free_gait_msgs/srv/SendActionSequence.srv b/free_gait_msgs/srv/SendActionSequence.srv new file mode 100644 index 0000000..bc62b0b --- /dev/null +++ b/free_gait_msgs/srv/SendActionSequence.srv @@ -0,0 +1,10 @@ +# This sends a sequence of actions to the step action server. +# This service call is will only block until the action has been +# successfully started or failed during initialization. +# If you need feedback about the progress or successful +# execution of the action (outside of the action itself), use +# the `ExecuteActionSequence` ROS action. + +free_gait_msgs/ExecuteActionGoal[] goals +--- +free_gait_msgs/ExecuteActionResult result diff --git a/free_gait_python/CHANGELOG.rst b/free_gait_python/CHANGELOG.rst new file mode 100644 index 0000000..583185c --- /dev/null +++ b/free_gait_python/CHANGELOG.rst @@ -0,0 +1,11 @@ +^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Changelog for package free_gait_python +^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ + +Forthcoming +----------- + +0.1.1 (2017-01-31) +------------------ +* Initial release. +* Contributors: Peter Fankhauser, Georg Wiedebach diff --git a/free_gait_python/CMakeLists.txt b/free_gait_python/CMakeLists.txt new file mode 100644 index 0000000..23494a0 --- /dev/null +++ b/free_gait_python/CMakeLists.txt @@ -0,0 +1,29 @@ +# Project configuration +cmake_minimum_required (VERSION 2.8) +project(free_gait_python) + +## 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 + 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 diff --git a/free_gait_python/package.xml b/free_gait_python/package.xml new file mode 100644 index 0000000..4fb840b --- /dev/null +++ b/free_gait_python/package.xml @@ -0,0 +1,16 @@ + + + free_gait_python + 0.3.0 + A collection of python scripts for the Free Gait. + Peter Fankhauser + Peter Fankhauser + BSD + https://github.com/leggedrobotics/free_gait + https://github.com/leggedrobotics/free_gait/issues + + catkin + + rospy + free_gait_msgs + diff --git a/free_gait_python/setup.py b/free_gait_python/setup.py new file mode 100644 index 0000000..757f683 --- /dev/null +++ b/free_gait_python/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'], + package_dir={'': 'src'} +) + +setup(**d) diff --git a/free_gait_python/src/free_gait/__init__.py b/free_gait_python/src/free_gait/__init__.py new file mode 100644 index 0000000..e9515de --- /dev/null +++ b/free_gait_python/src/free_gait/__init__.py @@ -0,0 +1,18 @@ +from free_gait import get_package_path +from free_gait import load_action_from_file +from free_gait import parse_action +from free_gait import replace_placeholders +from free_gait import adapt_coordinates +from free_gait import transform_coordinates +from free_gait import get_transform +from free_gait import transform_transformation +from free_gait import transform_position +from action import ActionState +from action import ActionBase +from action import SimpleAction +from action import ContinuousAction +from action import CombinedYamlAction +from action import CombinedYamlActionDefinition +from action import LaunchAction +from action import TriggerOnFeedback +from action import WaitForState diff --git a/free_gait_python/src/free_gait/action.py b/free_gait_python/src/free_gait/action.py new file mode 100644 index 0000000..3ad420f --- /dev/null +++ b/free_gait_python/src/free_gait/action.py @@ -0,0 +1,369 @@ +#! /usr/bin/env python + +import rospy +import rospkg +import free_gait +import threading +from actionlib_msgs.msg import GoalStatus +import roslaunch +import tempfile +import os +import traceback + + +class ActionState: + ERROR = -1 # Error state. + UNINITIALIZED = 0 # Not initialized. + INITIALIZED = 1 # Successfully initialized. + PENDING = 2 # Waiting for action server to accept our goal. + ACTIVE = 3 # Action running. + IDLE = 4 # Waiting for input. + DONE = 5 # Finished (success or preempted). + + @staticmethod + def to_text(action_state): + if action_state == ActionState.ERROR: + return 'Error' + elif action_state == ActionState.UNINITIALIZED: + return 'Uninitialized' + elif action_state == ActionState.INITIALIZED: + return 'Initialized' + elif action_state == ActionState.PENDING: + return 'Pending' + elif action_state == ActionState.ACTIVE: + return 'Active' + elif action_state == ActionState.IDLE: + return 'Idle' + elif action_state == ActionState.DONE: + return 'Done' + else: + return None + + +class ActionBase(object): + """Base class for generic actions. See specialized implementations below.""" + + def __init__(self, relay): + """Initialization of the action. Implement here subscribers, service + servers etc.""" + self.state = ActionState.UNINITIALIZED + self.feedback_callback = None + self.done_callback = None + self.relay = relay + self.goal = None + self.feedback = None + self.timeout = rospy.Duration() + self.set_state(ActionState.INITIALIZED) + + def set_state(self, state): + """Set the state of the robot and call according callback functions.""" + if state == self.state: + return + self.state = state + if self.state == ActionState.ERROR \ + or self.state == ActionState.INITIALIZED \ + or self.state == ActionState.PENDING \ + or self.state == ActionState.ACTIVE \ + or self.state == ActionState.IDLE: + if self.feedback_callback: + self.feedback_callback() + elif self.state == ActionState.DONE: + if self.done_callback: + self.done_callback() + + def register_callback(self, feedback_callback = None, done_callback = None): + """The action loader registers here its callback methods. These are called + upon change of state in `set_state(...)`.""" + self.feedback_callback = feedback_callback + self.done_callback = done_callback + + def start(self): + """Action is started from the action loader if the actions state is + `ActionState.INITIALIZED`. Start here the computation of your action.""" + + def wait_for_state(self, state): + """Helper method to wait for a state of the action.""" + wait_for_state = WaitForState(self, state) + wait_for_state.wait(); + + def stop(self): + """Action is stopped from the action loader. Implement here destruction + and shutdown procedures.""" + if not self._use_preview(): + self.relay.stop_tracking_goal() + self.set_state(ActionState.DONE) + + def _send_goal(self): + """Sends the `self.goal` object to the Free Gait execute steps action server. + Typically, do not overwrite this method.""" + if self.goal is None: + self.result = free_gait.free_gait_msgs.msg.ExecuteStepsResult() + self.set_state(ActionState.DONE) + return + + if self._use_preview(): + actionGoal = free_gait.free_gait_msgs.msg.ExecuteStepsActionGoal() + actionGoal.goal = self.goal + self.relay.publish(actionGoal) + self.set_state(ActionState.ACTIVE) + else: + if self.relay.gh: + rospy.logdebug("[Action Loader] Stop tracking goal.") + self.relay.stop_tracking_goal() + rospy.logdebug("[Action Loader] Waiting for step action server.") + self.relay.wait_for_server() + rospy.logdebug("[Action Loader] Sending goal.") + self.relay.send_goal(self.goal, + done_cb=self._done_callback, + active_cb=self._active_callback, + feedback_cb=self._feedback_callback) + rospy.logdebug("[Action Loader] Goal sent, switching to state pending.") + self.set_state(ActionState.PENDING) + + def _active_callback(self): + """Callback from the execute steps action server when action becomes active + after it has been sent.""" + self.set_state(ActionState.ACTIVE) + + def _feedback_callback(self, feedback): + """Feedback callback from the execute steps action server on the progress + of execution. Overwrite this method to trigger actions based on feedback.""" + self.feedback = feedback + + def _done_callback(self, status, result): + """Done callback from the execute steps action server when action has finished.""" + self.result = result + if status != GoalStatus.SUCCEEDED \ + and status != GoalStatus.PREEMPTED \ + and status != GoalStatus.RECALLED: + self.set_state(ActionState.ERROR) + else: + self.set_state(ActionState.DONE) + + def _use_preview(self): + """Helper method to figure out if this actions is run for preview or not.""" + if type(self.relay) == rospy.topics.Publisher: + return True + else: + return False + + +class SimpleAction(ActionBase): + """Base class for simple actions with one known goal at initialization.""" + + def __init__(self, relay, goal): + """Initialization of the simple action with storing the goal.""" + ActionBase.__init__(self, relay) + self.goal = goal + + def start(self): + """Sends the goal at start to the execute steps action server.""" + ActionBase.start(self) + self._send_goal() + + +class ContinuousAction(ActionBase): + """Base class for actions the run forever if not stopped or preempted.""" + + def start(self): + """Sends the goal at start to the execute steps action server.""" + ActionBase.start(self) + self._send_goal() + + def _done_callback(self, status, result): + """Done callback from the execute steps action server when action has finished. + Insted of switching to state `DONE, continuous actions switch to state `IDLE`.""" + self.result = result + if status != GoalStatus.SUCCEEDED \ + and status != GoalStatus.PREEMPTED \ + and status != GoalStatus.RECALLED: + self.set_state(ActionState.ERROR) + else: + self.set_state(ActionState.IDLE) + + +class CombinedYamlAction(ActionBase): + """Class for an action defined as a combination of multiple YAML motion + definitions.""" + + def __init__(self, relay): + """Initialization of the YAML combined action class.""" + ActionBase.__init__(self, relay) + self.set_state(ActionState.UNINITIALIZED) + self.goal = None + + def set_goal_from_file(self, file_path): + """Set the goal from a YAML file containing the action combination.""" + from rosparam import load_file + if not os.path.isfile(file_path): + rospy.logerr('File with path "' + file_path + '" does not exists.') + self.set_state(ActionState.ERROR) + return + self.set_goal_from_yaml(load_file(file_path)) + + def set_goal_from_yaml(self, yaml_object): + """Set the goal from a YAML object containing the action combination.""" + if not yaml_object: + self._parse_error() + return + global_placeholders = None + if 'global_placeholders' in yaml_object[0][0]: + global_placeholders = yaml_object[0][0]['global_placeholders'] + if 'yaml_actions' not in yaml_object[0][0]: + self._parse_error() + return + yaml_actions = yaml_object[0][0]['yaml_actions'] + rospack = rospkg.RosPack() + for yaml_action in yaml_actions: + if 'action' in yaml_action: + package = file_path = placeholders = None + if 'package' in yaml_action['action']: + package = yaml_action['action']['package'] + if 'file_path' in yaml_action['action']: + file_path = yaml_action['action']['file_path'] + if not package or not file_path: + self._parse_error() + return + full_file_path = os.path.abspath(os.path.join(rospack.get_path(package), file_path)) + placeholders = None + if 'placeholders' in yaml_action['action']: + placeholders = yaml_action['action']['placeholders'] + all_placeholders = {} + if global_placeholders is not None: + all_placeholders.update(global_placeholders) + if placeholders is not None: + all_placeholders.update(placeholders) + goal = free_gait.load_action_from_file(full_file_path, all_placeholders) + if not goal: + self.set_state(ActionState.ERROR) + return + if self.goal is None: + self.goal = goal + else: + self.goal.steps = self.goal.steps + goal.steps + + self.set_state(ActionState.INITIALIZED) + + def start(self): + """Sends the goal at start to the execute steps action server.""" + ActionBase.start(self) + self._send_goal() + + def _parse_error(self): + rospy.logerr('Could not parse the combined YAML action.') + self.set_state(ActionState.ERROR) + + +class CombinedYamlActionDefinition: + """Class to hold information and helper functions to define a combined + YAML action.""" + + def __init__(self): + self.yaml_object = [({'global_placeholders': {}, 'yaml_actions': []}, '/')] + + def append_action(self, package, file_path, placeholders=None): + if placeholders is None: + action = {'action': {'package': package, + 'file_path': file_path}} + else: + action = {'action': {'package': package, + 'file_path': file_path, + 'placeholders': placeholders}} + self.yaml_object[0][0]['yaml_actions'].append(action) + + +class LaunchAction(ActionBase): + + def __init__(self, file_path, relay): + ActionBase.__init__(self, relay) + launch_file = open(file_path, "r") + launch_text = launch_file.read() + launch_file.close() + launch_text = self._replace_preview_argument(launch_text) + self.temp_launch_file = tempfile.NamedTemporaryFile(mode='w+t', delete=False) + self.temp_launch_file.write(launch_text) + self.temp_launch_file.close() + uuid = roslaunch.rlutil.get_or_generate_uuid(None, False) + roslaunch.configure_logging(uuid) + self.listener = roslaunch.pmon.ProcessListener() + self.listener.process_died = self._process_died + self.launch = roslaunch.parent.ROSLaunchParent(uuid, [self.temp_launch_file.name], process_listeners=[self.listener]) + + def start(self): + try: + self.launch.start() + except roslaunch.core.RLException: + rospy.logerr(traceback.print_exc()) + self.set_state(ActionState.ERROR) + return + + self.set_state(ActionState.ACTIVE) + + def stop(self): + self.launch.shutdown() + os.unlink(self.temp_launch_file.name) + ActionBase.stop(self) + + def _replace_preview_argument(self, launch_text): + preview_argument = '' + new_text = launch_text.replace('', preview_argument, 1) + new_text = new_text.replace('', preview_argument, 1) + return new_text + + def _process_died(self, process_name, exit_code): + # For now we assume only one process. + if exit_code == 0: + self.set_state(ActionState.DONE) + else: + self.set_state(ActionState.ERROR) + + +class TriggerOnFeedback: + + def __init__(self, n_steps_in_queue, phase_of_step): + self.n_steps_in_queue = n_steps_in_queue + self.phase_of_step = phase_of_step + + def check(self, feedback): + if feedback.queue_size <= self.n_steps_in_queue and feedback.phase >= self.phase_of_step: + return True + else: + return False + + +class WaitForState: + + def __init__(self, action, state, timeout = rospy.Duration(), loop_period = rospy.Duration(0.1)): + self.action = action + self.state = state + self.timeout = timeout + self.loop_period = loop_period + self.done_condition = threading.Condition() + + def wait(self): + timeout_time = rospy.get_rostime() + self.timeout + loop_period = rospy.Duration(0.1) + with self.done_condition: + while not rospy.is_shutdown(): + time_left = timeout_time - rospy.get_rostime() + if self.timeout > rospy.Duration(0.0) and time_left <= rospy.Duration(0.0): + break + if not isinstance(self.state, list): + if self.action.state == self.state: + break + else: + if self.action.state in self.state: + break + if time_left > loop_period or self.timeout == rospy.Duration(): + time_left = loop_period + self.done_condition.wait(time_left.to_sec()) + + if not isinstance(self.state, list): + return self.action.state == self.state + else: + return self.action.state in self.state diff --git a/free_gait_python/src/free_gait/action.pyc b/free_gait_python/src/free_gait/action.pyc new file mode 100644 index 0000000..1aeef71 Binary files /dev/null and b/free_gait_python/src/free_gait/action.pyc differ diff --git a/free_gait_python/src/free_gait/free_gait.py b/free_gait_python/src/free_gait/free_gait.py new file mode 100644 index 0000000..2340ccc --- /dev/null +++ b/free_gait_python/src/free_gait/free_gait.py @@ -0,0 +1,598 @@ +#! /usr/bin/env python + +import rospy +import rospkg +import free_gait_msgs.msg +from tf.transformations import * +import geometry_msgs.msg +import trajectory_msgs.msg +import tf2_ros + +from tf2_msgs.msg import TFMessage # For local LocalTransformListener. + + +def get_package_path(package): + rospack = rospkg.RosPack() + return rospack.get_path(package) + + +def load_action_from_file(file_path, placeholders=None): + import os + from rosparam import load_file + if not os.path.isfile(file_path): + rospy.logerr('File with path "' + file_path + '" does not exists.') + return None + + parameters = load_file(file_path) + + # Replace placeholders. + if placeholders is not None: + replace_placeholders(parameters[0][0], placeholders) + + # Adapt coordinates. + is_adapt = False + source_frame_id = '' + target_frame_id = '' + position = [0, 0, 0] + orientation = [0, 0, 0, 1] + if 'adapt_coordinates' in parameters[0][0]: + is_adapt = True + adapt_parameters = parameters[0][0]['adapt_coordinates'][0]['transform'] + source_frame_id = adapt_parameters['source_frame'] + target_frame_id = adapt_parameters['target_frame'] + if 'transform_in_source_frame' in adapt_parameters: + if 'position' in adapt_parameters['transform_in_source_frame']: + position = adapt_parameters['transform_in_source_frame']['position'] + if 'orientation' in adapt_parameters['transform_in_source_frame']: + orientation = adapt_parameters['transform_in_source_frame']['orientation'] + if len(orientation) == 3: + orientation = quaternion_from_euler(orientation[0], orientation[1], orientation[2]) + + if is_adapt: + try: + (position, orientation) = transform_coordinates(source_frame_id, target_frame_id, position, orientation) + except TypeError: + return None + + return parse_action(parameters, source_frame_id, target_frame_id, position, orientation) + + +def load_action_from_file_and_transform(file_path, source_frame_id='', position=[0, 0, 0], orientation=[0, 0, 0, 1]): + from rosparam import load_file + import os + + if not os.path.isfile(file_path): + rospy.logerr('File with path "' + file_path + '" does not exists.') + return None + + return parse_action(load_file(file_path), source_frame_id, position, orientation) + + +def parse_action(yaml_object, source_frame_id='', target_frame_id='', position=[0, 0, 0], orientation=[0, 0, 0, 1]): + goal = free_gait_msgs.msg.ExecuteStepsGoal() + + # For each step. + for step_parameter in yaml_object[0][0]['steps']: + + # Step. + step_parameter = step_parameter['step'] + step = free_gait_msgs.msg.Step() + if not step_parameter: + continue + + for motion_parameter in step_parameter: + if 'footstep' in motion_parameter: + step.footstep.append(parse_footstep(motion_parameter['footstep'])) + if 'end_effector_target' in motion_parameter: + step.end_effector_target.append(parse_end_effector_target(motion_parameter['end_effector_target'])) + if 'end_effector_trajectory' in motion_parameter: + step.end_effector_trajectory.append(parse_end_effector_trajectory(motion_parameter['end_effector_trajectory'])) + if 'leg_mode' in motion_parameter: + step.leg_mode.append(parse_leg_mode(motion_parameter['leg_mode'])) + if 'joint_trajectory' in motion_parameter: + step.joint_trajectory.append(parse_joint_trajectory(motion_parameter['joint_trajectory'])) + if 'base_auto' in motion_parameter: + step.base_auto.append(parse_base_auto(motion_parameter['base_auto'])) + if 'base_target' in motion_parameter: + step.base_target.append(parse_base_target(motion_parameter['base_target'])) + if 'base_trajectory' in motion_parameter: + step.base_trajectory.append(parse_base_trajectory(motion_parameter['base_trajectory'])) + if 'custom_command' in motion_parameter: + step.custom_command.append(parse_custom_command(motion_parameter['custom_command'])) + + goal.steps.append(step) + + # Adapt to local coordinates if desired. + if not (numpy.array_equal(position, [0, 0, 0]) and numpy.array_equal(orientation, [0, 0, 0, 1])): + adapt_coordinates(goal, source_frame_id, target_frame_id, position, orientation) + + # print goal + return goal + + +def replace_placeholders(yaml_object, placeholders): + if type(yaml_object) == dict: + for i, item in yaml_object.items(): + if type(item) == str: + if item in placeholders: + yaml_object[i] = placeholders[item] + else: + replace_placeholders(yaml_object[i], placeholders) + if type(yaml_object) == list: + for i, item in enumerate(yaml_object): + if type(item) == str: + if item in placeholders: + yaml_object[i] = placeholders[item] + else: + replace_placeholders(yaml_object[i], placeholders) + + +def parse_footstep(yaml_object): + footstep = free_gait_msgs.msg.Footstep() + if not yaml_object: + return footstep + if 'name' in yaml_object: + footstep.name = yaml_object['name'] + if 'target' in yaml_object: + footstep.target = parse_position_stamped(yaml_object['target']) + if 'profile_height' in yaml_object: + footstep.profile_height = yaml_object['profile_height'] + if 'average_velocity' in yaml_object: + footstep.average_velocity = yaml_object['average_velocity'] + if 'profile_type' in yaml_object: + footstep.profile_type = yaml_object['profile_type'] + if 'ignore_contact' in yaml_object: + footstep.ignore_contact = yaml_object['ignore_contact'] + if 'surface_normal' in yaml_object: + footstep.surface_normal = parse_vector_stamped(yaml_object['surface_normal']) + if 'ignore_for_pose_adaptation' in yaml_object: + footstep.ignore_for_pose_adaptation = yaml_object['ignore_for_pose_adaptation'] + return footstep + + +def parse_end_effector_target(yaml_object): + end_effector_target = free_gait_msgs.msg.EndEffectorTarget() + if not yaml_object: + return end_effector_target + if 'name' in yaml_object: + end_effector_target.name = yaml_object['name'] + if 'target_position' in yaml_object: + end_effector_target.target_position.append(parse_position_stamped(yaml_object['target_position'])) + if 'target_velocity' in yaml_object: + end_effector_target.target_velocity.append(parse_vector_stamped(yaml_object['target_velocity'])) + if 'target_acceleration' in yaml_object: + end_effector_target.target_acceleration.append(parse_vector_stamped(yaml_object['target_acceleration'])) + if 'target_force' in yaml_object: + end_effector_target.target_force.append(parse_vector_stamped(yaml_object['target_force'])) + if 'average_velocity' in yaml_object: + end_effector_target.average_velocity = yaml_object['average_velocity'] + if 'ignore_contact' in yaml_object: + end_effector_target.ignore_contact = yaml_object['ignore_contact'] + if 'surface_normal' in yaml_object: + end_effector_target.surface_normal = parse_vector_stamped(yaml_object['surface_normal']) + if 'ignore_for_pose_adaptation' in yaml_object: + end_effector_target.ignore_for_pose_adaptation = yaml_object['ignore_for_pose_adaptation'] + return end_effector_target + + +def parse_end_effector_trajectory(yaml_object): + end_effector_trajectory = free_gait_msgs.msg.EndEffectorTrajectory() + if not yaml_object: + return end_effector_trajectory + if 'name' in yaml_object: + end_effector_trajectory.name = yaml_object['name'] + if 'trajectory' in yaml_object: + end_effector_trajectory.trajectory = parse_translational_trajectory(end_effector_trajectory.name, yaml_object['trajectory']) + if 'surface_normal' in yaml_object: + end_effector_trajectory.surface_normal = parse_vector_stamped(yaml_object['surface_normal']) + if 'ignore_contact' in yaml_object: + end_effector_trajectory.ignore_contact = yaml_object['ignore_contact'] + if 'ignore_for_pose_adaptation' in yaml_object: + end_effector_trajectory.ignore_for_pose_adaptation = yaml_object['ignore_for_pose_adaptation'] + return end_effector_trajectory + + +def parse_leg_mode(yaml_object): + leg_mode = free_gait_msgs.msg.LegMode() + if not yaml_object: + return leg_mode + if 'name' in yaml_object: + leg_mode.name = yaml_object['name'] + if 'support_leg' in yaml_object: + leg_mode.support_leg = yaml_object['support_leg'] + if 'duration' in yaml_object: + leg_mode.duration = parse_duration(yaml_object['duration']) + if 'surface_normal' in yaml_object: + leg_mode.surface_normal = parse_vector_stamped(yaml_object['surface_normal']) + if 'ignore_for_pose_adaptation' in yaml_object: + leg_mode.ignore_for_pose_adaptation = yaml_object['ignore_for_pose_adaptation'] + return leg_mode + + +def parse_joint_trajectory(yaml_object): + joint_trajectory = free_gait_msgs.msg.JointTrajectory() + if not yaml_object: + return joint_trajectory + if 'name' in yaml_object: + joint_trajectory.name = yaml_object['name'] + if 'trajectory' in yaml_object: + joint_trajectory.trajectory = parse_joint_trajectories(yaml_object['trajectory']) + if 'ignore_contact' in yaml_object: + joint_trajectory.ignore_contact = yaml_object['ignore_contact'] + if 'surface_normal' in yaml_object: + joint_trajectory.surface_normal = parse_vector_stamped(yaml_object['surface_normal']) + return joint_trajectory + + +def parse_base_auto(yaml_object): + base_auto = free_gait_msgs.msg.BaseAuto() + if not yaml_object: + return base_auto + if 'height' in yaml_object: + base_auto.height = yaml_object['height'] + if 'ignore_timing_of_leg_motion' in yaml_object: + base_auto.ignore_timing_of_leg_motion = yaml_object['ignore_timing_of_leg_motion'] + if 'average_linear_velocity' in yaml_object: + base_auto.average_linear_velocity = yaml_object['average_linear_velocity'] + if 'average_angular_velocity' in yaml_object: + base_auto.average_angular_velocity = yaml_object['average_angular_velocity'] + if 'support_margin' in yaml_object: + base_auto.support_margin = yaml_object['support_margin'] + return base_auto + + +def parse_base_target(yaml_object): + base_target = free_gait_msgs.msg.BaseTarget() + if not yaml_object: + return base_target + if 'target' in yaml_object: + base_target.target = parse_pose_stamped(yaml_object['target']) + if 'ignore_timing_of_leg_motion' in yaml_object: + base_target.ignore_timing_of_leg_motion = yaml_object['ignore_timing_of_leg_motion'] + if 'average_linear_velocity' in yaml_object: + base_target.average_linear_velocity = yaml_object['average_linear_velocity'] + if 'average_angular_velocity' in yaml_object: + base_target.average_angular_velocity = yaml_object['average_angular_velocity'] + return base_target + + +def parse_base_trajectory(yaml_object): + base_trajectory = free_gait_msgs.msg.BaseTrajectory() + if not yaml_object: + return base_trajectory + if 'trajectory' in yaml_object: + base_trajectory.trajectory = parse_multi_dof_trajectory('base', yaml_object['trajectory']) + return base_trajectory + + +def parse_custom_command(yaml_object): + custom_command = free_gait_msgs.msg.CustomCommand() + if not yaml_object: + return custom_command + if 'type' in yaml_object: + custom_command.type = yaml_object['type'] + if 'duration' in yaml_object: + custom_command.duration = parse_duration(yaml_object['duration']) + if 'command' in yaml_object: + custom_command.command = yaml_object['command'] + return custom_command + + +def parse_duration(duration): + return rospy.Duration(duration) + + +def parse_position(yaml_object): + point = geometry_msgs.msg.Point() + point.x = yaml_object[0] + point.y = yaml_object[1] + point.z = yaml_object[2] + return point + + +def parse_orientation(yaml_object): + quaternion = geometry_msgs.msg.Quaternion() + if len(yaml_object) == 4: + quaternion.x = yaml_object[0] + quaternion.y = yaml_object[1] + quaternion.z = yaml_object[2] + quaternion.w = yaml_object[3] + elif len(yaml_object) == 3: + q = quaternion_from_euler(yaml_object[0], yaml_object[1], yaml_object[2]) + quaternion.x = q[0] + quaternion.y = q[1] + quaternion.z = q[2] + quaternion.w = q[3] + return quaternion + + +def parse_vector(yaml_object): + vector = geometry_msgs.msg.Vector3() + vector.x = yaml_object[0] + vector.y = yaml_object[1] + vector.z = yaml_object[2] + return vector + + +def parse_transform(yaml_object): + transform = geometry_msgs.msg.Transform() + if 'position' in yaml_object: + transform.translation = parse_vector(yaml_object['position']) + if 'orientation' in yaml_object: + transform.rotation = parse_orientation(yaml_object['orientation']) + return transform + + +def parse_position_stamped(yaml_object): + point = geometry_msgs.msg.PointStamped() + if 'frame' in yaml_object: + point.header.frame_id = yaml_object['frame'] + if 'position' in yaml_object: + point.point = parse_position(yaml_object['position']) + return point + + +def parse_pose_stamped(yaml_object): + pose = geometry_msgs.msg.PoseStamped() + if 'frame' in yaml_object: + pose.header.frame_id = yaml_object['frame'] + if 'position' in yaml_object: + pose.pose.position = parse_position(yaml_object['position']) + if 'orientation' in yaml_object: + pose.pose.orientation = parse_orientation(yaml_object['orientation']) + return pose + + +def parse_vector_stamped(yaml_object): + vector = geometry_msgs.msg.Vector3Stamped() + if 'frame' in yaml_object: + vector.header.frame_id = yaml_object['frame'] + if 'vector' in yaml_object: + vector.vector = parse_vector(yaml_object['vector']) + return vector + + +def parse_multi_dof_trajectory(joint_name, trajectory): + output = trajectory_msgs.msg.MultiDOFJointTrajectory() + if 'frame' in trajectory: + output.header.frame_id = trajectory['frame'] + output.joint_names.append(joint_name) + for knot in trajectory['knots']: + point = trajectory_msgs.msg.MultiDOFJointTrajectoryPoint() + point.time_from_start = rospy.Time(knot['time']) + transform = parse_transform(knot) + point.transforms.append(transform) + output.points.append(point) + return output + + +def parse_translational_trajectory(joint_name, trajectory): + output = trajectory_msgs.msg.MultiDOFJointTrajectory() + output.header.frame_id = trajectory['frame'] + output.joint_names.append(joint_name) + for knot in trajectory['knots']: + point = trajectory_msgs.msg.MultiDOFJointTrajectoryPoint() + point.time_from_start = rospy.Time(knot['time']) + transform = parse_transform(knot) + point.transforms.append(transform) + output.points.append(point) + return output + + +def parse_joint_trajectories(yaml_object): + joint_trajectory = trajectory_msgs.msg.JointTrajectory() + for joint_name in yaml_object['joint_names']: + joint_trajectory.joint_names.append(joint_name) + for knot in yaml_object['knots']: + point = trajectory_msgs.msg.JointTrajectoryPoint() + point.time_from_start = rospy.Time(knot['time']) + if 'positions' in knot: + point.positions = knot['positions'] + if 'velocities' in knot: + point.velocities = knot['velocities'] + if 'accelerations' in knot: + point.accelerations = knot['accelerations'] + if 'effort' in knot: + point.effort = knot['effort'] + joint_trajectory.points.append(point) + return joint_trajectory + + +def adapt_coordinates(goal, source_frame_id, target_frame_id, position, orientation): + # For each step. + translation = translation_matrix(position) + yaw = 0 + if len(orientation) == 4: + (roll, pitch, yaw) = euler_from_quaternion(orientation) + elif len(orientation) == 3: + yaw = orientation[2] + z_axis = [0, 0, 1] + rotation = rotation_matrix(yaw, z_axis) + transform = concatenate_matrices(translation, rotation) + adapt_coordinates_recursively(goal.steps, source_frame_id, target_frame_id, transform) + + +def adapt_coordinates_recursively(message, source_frame_id, target_frame_id, transform): + + # Stop recursion for methods and primitive types. + if hasattr(message, '__call__') or isinstance(message, int) or isinstance(message, str) or \ + isinstance(message, bool) or isinstance(message, float): + return + + # Transform known geometries. + if isinstance(message, geometry_msgs.msg.Vector3Stamped): + if check_if_vector_valid(message.vector) and message.header.frame_id == source_frame_id: + message.header.frame_id = target_frame_id + vector = transform_vector(transform, message.vector) + message.vector = vector + return + elif isinstance(message, geometry_msgs.msg.PointStamped): + if check_if_position_valid(message.point) and message.header.frame_id == source_frame_id: + message.header.frame_id = target_frame_id + position = transform_position(transform, message.point) + message.point = position + return + elif isinstance(message, geometry_msgs.msg.PoseStamped): + if check_if_pose_valid(message.pose) and message.header.frame_id == source_frame_id: + message.header.frame_id = target_frame_id + pose = transform_pose(transform, message.pose) + message.pose = pose + return + elif isinstance(message, trajectory_msgs.msg.MultiDOFJointTrajectory): + if message.header.frame_id == source_frame_id: + message.header.frame_id = target_frame_id + for i, point in enumerate(message.points): + for j, transformation in enumerate(message.points[i].transforms): + t = transform_transformation(transform, transformation) + message.points[i].transforms[j] = t + return + + # Do recursion for lists and members. + if hasattr(message, '__iter__'): + for m in message: # TODO Need enumerate? + adapt_coordinates_recursively(m, source_frame_id, target_frame_id, transform) + else: + for m in [a for a in dir(message) if not (a.startswith('__') or a.startswith('_') or \ + a == 'deserialize' or a == 'deserialize_numpy' or a == 'serialize' or a == 'serialize_numpy')]: + adapt_coordinates_recursively(eval("message." + m), source_frame_id, target_frame_id, transform) + +# Position and orientation defined in source frame. +def transform_coordinates(source_frame_id, target_frame_id, position = [0, 0, 0], orientation = [0, 0, 0, 1], tf_buffer = None): + + try: + (translation, rotation) = get_tf_transform(source_frame_id, target_frame_id, tf_buffer) + except TypeError: + return None + + transformed_position = translation + quaternion_matrix(rotation)[:3, :3].dot(position) + transformed_orientation = quaternion_multiply(rotation, orientation) + return transformed_position, transformed_orientation + + +def get_transform(source_frame_id, target_frame_id, tf_buffer = None): + + (translation, rotation) = get_tf_transform(source_frame_id, target_frame_id, tf_buffer) + translation_matrix_form = translation_matrix(translation) + rotation_matrix_form = quaternion_matrix(rotation) + return concatenate_matrices(translation_matrix_form, rotation_matrix_form) + + +def get_tf_transform(source_frame_id, target_frame_id, tf_buffer = None): + + listener = None + if tf_buffer is None: + tf_buffer = tf2_ros.Buffer() + listener = LocalTransformListener(tf_buffer) + + try: + transform = tf_buffer.lookup_transform(target_frame_id, source_frame_id, rospy.Time(), rospy.Duration(10.0)) + if listener is not None: + listener.unregister() + del listener + except (tf2_ros.LookupException, tf2_ros.ConnectivityException, tf2_ros.ExtrapolationException): + rospy.logerr('Could not look up TF transformation from "' + + source_frame_id + '" to "' + target_frame_id + '".') + return None + + t = transform.transform.translation + r = transform.transform.rotation + return [t.x, t.y, t.z], [r.x, r.y, r.z, r.w] + + +def transform_vector(transform, vector): + angle, direction, point = rotation_from_matrix(transform) + transformed_vector = rotation_matrix(angle, direction).dot([vector.x, vector.y, vector.z, 1.0]) + return geometry_msgs.msg.Vector3(transformed_vector[0], transformed_vector[1], transformed_vector[2]) + + +def transform_position(transform, position): + transformed_point = transform.dot([position.x, position.y, position.z, 1.0]) + return geometry_msgs.msg.Point(transformed_point[0], transformed_point[1], transformed_point[2]) + + +def transform_orientation(transform, orientation): + q1 = quaternion_from_matrix(transform) + q2 = [orientation.x, orientation.y, orientation.z, orientation.w] + q = quaternion_multiply(q1, q2) + return geometry_msgs.msg.Quaternion(q[0], q[1], q[2], q[3]) + + +def transform_pose(transform, pose): + pose.position = transform_position(transform, pose.position) + pose.orientation = transform_orientation(transform, pose.orientation) + return pose + + +def transform_transformation(transform, transformation): + transformation.translation = transform_position(transform, transformation.translation) + transformation.rotation = transform_orientation(transform, transformation.rotation) + return transformation + + +def check_if_vector_valid(vector): + if vector.x == 0 and vector.y == 0 and vector.z == 0: + return False + else: + return True + + +def check_if_position_valid(position): + if position.x == 0 and position.y == 0 and position.z == 0: + return False + else: + return True + + +def check_if_orientation_valid(orientation): + if orientation.x == 0 and orientation.y == 0 and orientation.z == 0 and orientation.w == 0: + return False + else: + return True + + +def check_if_pose_valid(pose): + if check_if_position_valid(pose.position) and check_if_orientation_valid(pose.orientation): + return True + else: + return False + + +# TODO: We are using this from a newer version. Remove once updated. +class LocalTransformListener(): + + """ + :class:`TransformListener` is a convenient way to listen for coordinate frame transformation info. + This class takes an object that instantiates the :class:`BufferInterface` interface, to which + it propagates changes to the tf frame graph. + """ + def __init__(self, buffer): + """ + .. function:: __init__(buffer) + + Constructor. + + :param buffer: The buffer to propagate changes to when tf info updates. + """ + self.buffer = buffer + self.tf_sub = rospy.Subscriber("/tf", TFMessage, self.callback) + self.tf_static_sub = rospy.Subscriber("/tf_static", TFMessage, self.static_callback) + + def __del__(self): + self.unregister() + + def unregister(self): + """ + Unregisters all tf subscribers. + """ + self.tf_sub.unregister() + self.tf_static_sub.unregister() + + def callback(self, data): + who = data._connection_header.get('callerid', "default_authority") + for transform in data.transforms: + self.buffer.set_transform(transform, who) + + def static_callback(self, data): + who = data._connection_header.get('callerid', "default_authority") + for transform in data.transforms: + self.buffer.set_transform_static(transform, who) diff --git a/free_gait_python/src/free_gait/free_gait.pyc b/free_gait_python/src/free_gait/free_gait.pyc new file mode 100644 index 0000000..9649322 Binary files /dev/null and b/free_gait_python/src/free_gait/free_gait.pyc differ diff --git a/free_gait_ros/CHANGELOG.rst b/free_gait_ros/CHANGELOG.rst new file mode 100644 index 0000000..1fcf4f6 --- /dev/null +++ b/free_gait_ros/CHANGELOG.rst @@ -0,0 +1,11 @@ +^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Changelog for package free_gait_ros +^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ + +Forthcoming +----------- + +0.1.1 (2017-01-31) +------------------ +* Initial release. +* Contributors: Peter Fankhauser, Dario Bellicoso, Thomas Bi, Georg Wiedebach diff --git a/free_gait_ros/CMakeLists.txt b/free_gait_ros/CMakeLists.txt new file mode 100644 index 0000000..feab4bc --- /dev/null +++ b/free_gait_ros/CMakeLists.txt @@ -0,0 +1,111 @@ +cmake_minimum_required(VERSION 2.8.3) +project(free_gait_ros) + +set(CMAKE_CXX_FLAGS "-std=c++11 ${CMAKE_CXX_FLAGS}") + +## Find catkin macros and libraries +find_package(catkin REQUIRED COMPONENTS + free_gait_msgs + free_gait_core + quadruped_model + qp_solver + curves_ros + grid_map_core + grid_map_ros + pluginlib + robot_state_publisher + kdl_parser +) + +## System dependencies are found with CMake's conventions +find_package(Eigen3 REQUIRED) + +################################### +## catkin specific configuration ## +################################### +## The catkin_package macro generates cmake config files for your package +## Declare things to be passed to dependent projects +## INCLUDE_DIRS: uncomment this if you package contains header files +## LIBRARIES: libraries you create in this project that dependent projects also need +## CATKIN_DEPENDS: catkin_packages dependent projects also need +## DEPENDS: system dependencies of this project that dependent projects also need +catkin_package( + INCLUDE_DIRS + include + ${EIGEN3_INCLUDE_DIR} + LIBRARIES + ${PROJECT_NAME} + CATKIN_DEPENDS + free_gait_msgs + free_gait_core + quadruped_model + qp_solver + curves_ros + grid_map_core + grid_map_ros + pluginlib + robot_state_publisher + kdl_parser +) + +# Attempt to find catkinized kindr +find_package(kindr QUIET) +if(NOT kindr_FOUND) + # Attempt to find package-based kindr + find_package(PkgConfig REQUIRED) + pkg_check_modules(kindr kindr REQUIRED) +endif() + +########### +## Build ## +########### + +## Specify additional locations of header files +include_directories( + include + ${catkin_INCLUDE_DIRS} + ${EIGEN3_INCLUDE_DIR} +) + +## Declare a cpp library +add_library(${PROJECT_NAME} + src/StepRosConverter.cpp + src/StepFrameConverter.cpp + src/FreeGaitActionServer.cpp + src/FreeGaitActionClient.cpp + src/AdapterRos.cpp + src/AdapterRosInterfaceBase.cpp + src/StateRosPublisher.cpp + src/RosVisualization.cpp +) + +target_link_libraries(${PROJECT_NAME} + ${catkin_LIBRARIES} +) +add_executable(steps_test test/test.cpp test/AdapterDummy.cpp) +target_link_libraries(steps_test ${PROJECT_NAME} + ${catkin_LIBRARIES}) +############# +## Testing ## +############# + +set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -pthread") +## Add gtest based cpp test target and link libraries +catkin_add_gtest(${PROJECT_NAME}-test + test/test_free_gait_ros.cpp + test/AdapterDummy.cpp + test/StepTest.cpp +) +if(TARGET ${PROJECT_NAME}-test) + target_link_libraries(${PROJECT_NAME}-test ${PROJECT_NAME}) +endif() + +install(TARGETS ${PROJECT_NAME} + ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} + LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} + RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} +) + +install(DIRECTORY include/${PROJECT_NAME}/ + DESTINATION ${CATKIN_PACKAGE_INCLUDE_DESTINATION} +) diff --git a/free_gait_ros/include/free_gait_ros/AdapterRos.hpp b/free_gait_ros/include/free_gait_ros/AdapterRos.hpp new file mode 100644 index 0000000..5a62b45 --- /dev/null +++ b/free_gait_ros/include/free_gait_ros/AdapterRos.hpp @@ -0,0 +1,48 @@ +/* + * AdapterRos.hpp + * + * Created on: Dec 1, 2016 + * Author: Péter Fankhauser + * Institute: ETH Zurich, Robotic Systems Lab + */ + +#pragma once + +#include +#include +#include +#include + +// STD +#include +#include + +namespace free_gait { + +class AdapterRos +{ + public: + enum class AdapterType { + Base, + Preview + }; + + AdapterRos(ros::NodeHandle& nodeHandle, const AdapterType type = AdapterType::Base); + virtual ~AdapterRos(); + bool subscribeToRobotState(const std::string& robotStateTopic = ""); + void unsubscribeFromRobotState(); + const std::string getRobotStateMessageType(); + bool isReady() const ; + bool updateAdapterWithState(); + const AdapterBase& getAdapter() const; + AdapterBase* getAdapterPtr(); + + private: + ros::NodeHandle& nodeHandle_; + pluginlib::ClassLoader adapterLoader_; + std::unique_ptr adapter_; + pluginlib::ClassLoader adapterRosInterfaceLoader_; + std::unique_ptr adapterRosInterface_; +}; + +} /* namespace free_gait */ diff --git a/free_gait_ros/include/free_gait_ros/AdapterRosInterfaceBase.hpp b/free_gait_ros/include/free_gait_ros/AdapterRosInterfaceBase.hpp new file mode 100644 index 0000000..6f8059e --- /dev/null +++ b/free_gait_ros/include/free_gait_ros/AdapterRosInterfaceBase.hpp @@ -0,0 +1,49 @@ +/* + * AdapterRosInterfaceBase.hpp + * + * Created on: Nov 29, 2016 + * Author: Péter Fankhauser + * Institute: ETH Zurich, Robotic Systems Lab + */ + +#pragma once + +#include +#include "free_gait_core/TypeDefs.hpp" +// ROS +#include +#include "free_gait_msgs/RobotState.h" + +// STD +#include + +namespace free_gait { + +class AdapterRosInterfaceBase +{ + public: + AdapterRosInterfaceBase(); + virtual ~AdapterRosInterfaceBase(); + void setNodeHandle(ros::NodeHandle& nodeHandle); + + virtual bool readRobotDescription(); + virtual bool subscribeToRobotState(const std::string& robotStateTopic) = 0; + virtual void unsubscribeFromRobotState() = 0; + virtual const std::string getRobotStateMessageType() = 0; + virtual bool isReady() const = 0; + + //! Update adapter. + virtual bool initializeAdapter(AdapterBase& adapter) const = 0; + virtual bool updateAdapterWithRobotState(AdapterBase& adapter) const = 0; + + void updateRobotState(const free_gait_msgs::RobotStateConstPtr& robotState); + + protected: + ros::NodeHandle* nodeHandle_; + std::string robotDescriptionUrdfString_; + ros::Subscriber joint_states_sub_; + JointPositions all_joint_positions_; + +}; + +} /* namespace free_gait */ diff --git a/free_gait_ros/include/free_gait_ros/FreeGaitActionClient.hpp b/free_gait_ros/include/free_gait_ros/FreeGaitActionClient.hpp new file mode 100644 index 0000000..4cf222f --- /dev/null +++ b/free_gait_ros/include/free_gait_ros/FreeGaitActionClient.hpp @@ -0,0 +1,66 @@ +/* + * FreeGaitActionClient.hpp + * + * Created on: Oct 21, 2015 + * Author: Péter Fankhauser, Georg Wiedebach + */ + +#pragma once + +// Free Gait +#include +#include + +// ROS +#include +#include + +// STD +#include +#include + +namespace free_gait { + +class FreeGaitActionClient +{ + public: + enum ActionState + { + ERROR, + UNINITIALIZED, + INITIALIZED, + PENDING, + ACTIVE, + IDLE, + DONE + }; + + FreeGaitActionClient(ros::NodeHandle& nodeHandle); + virtual ~FreeGaitActionClient() {}; + + void registerCallback(std::function activeCallback = nullptr, + std::function feedbackCallback = nullptr, + std::function doneCallback = nullptr); + void sendGoal(const free_gait_msgs::ExecuteStepsGoal& goal); + void sendGoal(const free_gait_msgs::ExecuteStepsGoal& goal, const bool usePreview); + void waitForResult(const double timeout = 1e6); // TODO + const ActionState& getState(); + + private: + void activeCallback(); + void feedbackCallback(const free_gait_msgs::ExecuteStepsFeedbackConstPtr& feedback); + void doneCallback(const actionlib::SimpleClientGoalState& state, + const free_gait_msgs::ExecuteStepsResultConstPtr& result); + + ros::NodeHandle& nodeHandle_; + std::function activeCallback_; + std::function feedbackCallback_; + std::function doneCallback_; + std::unique_ptr> client_; + ros::Publisher previewPublisher_; + ActionState state_; +}; + +} diff --git a/free_gait_ros/include/free_gait_ros/FreeGaitActionServer.hpp b/free_gait_ros/include/free_gait_ros/FreeGaitActionServer.hpp new file mode 100644 index 0000000..d114d43 --- /dev/null +++ b/free_gait_ros/include/free_gait_ros/FreeGaitActionServer.hpp @@ -0,0 +1,72 @@ +/* + * FreeGaitActionServer.hpp + * + * Created on: Feb 6, 2015 + * Author: Péter Fankhauser + * Institute: ETH Zurich, Autonomous Systems Lab + */ + +#pragma once + +// Free Gait +#include "free_gait_core/free_gait_core.hpp" +#include "free_gait_ros/StepRosConverter.hpp" + +// Quadruped model +#include "quadruped_model/QuadrupedModel.hpp" + +// ROS +#include +#include +#include + +// STD +#include +#include + +namespace free_gait { + +class FreeGaitActionServer +{ + public: + FreeGaitActionServer(ros::NodeHandle nodeHandle, const std::string& name, + Executor& executor, AdapterBase& adapter); + + virtual ~FreeGaitActionServer(); + + void initialize(); +// void setExecutor(std::shared_ptr executor); +// void setAdapter(std::shared_ptr adapter); + void start(); + void update(); + void shutdown(); + + bool isActive(); + void goalCallback(); + void preemptCallback(); + void publishFeedback(); + void setSucceeded(); + void setPreempted(); + void setAborted(); + + private: + //! ROS nodehandle. + ros::NodeHandle& nodeHandle_; + free_gait::Executor& executor_; + + //! ROS converter. + StepRosConverter adapter_; + + //! Action server. + std::string name_; + actionlib::SimpleActionServer server_; + free_gait_msgs::ExecuteStepsActionResult result_; + + //! True if in process of preempting. + bool isPreempting_; + + //! Number of steps of the current goal. + size_t nStepsInCurrentGoal_; +}; + +} /* namespace */ diff --git a/free_gait_ros/include/free_gait_ros/RosVisualization.hpp b/free_gait_ros/include/free_gait_ros/RosVisualization.hpp new file mode 100644 index 0000000..2f926af --- /dev/null +++ b/free_gait_ros/include/free_gait_ros/RosVisualization.hpp @@ -0,0 +1,38 @@ +/* + * RosVisualization.hpp + * + * Created on: Jul 4, 2017 + * Author: Péter Fankhauser + */ + +#pragma once + +#include + +#include +#include +#include + +namespace free_gait { + +class RosVisualization +{ + public: + RosVisualization(); + virtual ~RosVisualization(); + + static const visualization_msgs::Marker getStanceMarker(const Stance& stance, const std::string& frameId, + const std_msgs::ColorRGBA& color); + + static const visualization_msgs::Marker getComMarker(const Position& comPosition, const std::string& frameId, + const std_msgs::ColorRGBA& color, const double size); + + static const visualization_msgs::MarkerArray getComWithProjectionMarker(const Position& comPosition, + const std::string& frameId, + const std_msgs::ColorRGBA& color, + const double comMarkerSize, + const double projectionLenght, + const double projectionDiameter); +}; + +} diff --git a/free_gait_ros/include/free_gait_ros/StateRosPublisher.hpp b/free_gait_ros/include/free_gait_ros/StateRosPublisher.hpp new file mode 100644 index 0000000..1f7c42c --- /dev/null +++ b/free_gait_ros/include/free_gait_ros/StateRosPublisher.hpp @@ -0,0 +1,44 @@ +/* + * StateRosPublisher.hpp + * + * Created on: Dec 6, 2016 + * Author: Péter Fankhauser + * Institute: ETH Zurich, Autonomous Systems Lab + */ + +#pragma once + +// Free Gait +#include + +// ROS +#include +#include + +// STD +#include +#include + +namespace free_gait { + +class StateRosPublisher +{ + public: + StateRosPublisher(ros::NodeHandle& nodeHandle, AdapterBase& adapter); + virtual ~StateRosPublisher(); + StateRosPublisher(const StateRosPublisher& other); + + void setTfPrefix(const std::string tfPrefix); + bool publish(const State& state); + + private: + bool initializeRobotStatePublisher(); + + ros::NodeHandle& nodeHandle_; + std::string tfPrefix_; + std::unique_ptr robotStatePublisher_; + AdapterBase& adapter_; + tf2_ros::TransformBroadcaster tfBroadcaster_; +}; + +} /* namespace free_gait */ diff --git a/free_gait_ros/include/free_gait_ros/StepFrameConverter.hpp b/free_gait_ros/include/free_gait_ros/StepFrameConverter.hpp new file mode 100644 index 0000000..a05f1a3 --- /dev/null +++ b/free_gait_ros/include/free_gait_ros/StepFrameConverter.hpp @@ -0,0 +1,72 @@ +/* + * StepFrameConverter.hpp + * + * Created on: Nov 11, 2016 + * Author: Péter Fankhauser + * Institute: ETH Zurich, Robotic Systems Lab + */ + +#pragma once + +#include + +// ROS +#include +#include + +// STD +#include +#include + +namespace free_gait { + +class StepFrameConverter +{ + public: + StepFrameConverter(tf2_ros::Buffer& tfBuffer); + virtual ~StepFrameConverter(); + + bool adaptCoordinates(StepQueue& stepQueue, const std::string& sourceFrameId, + const std::string& targetFrameId, + const Transform& transformInSourceFrame = Transform(), + const ros::Time& time = ros::Time(0)); + + bool adaptCoordinates(Step& step, const std::string& sourceFrameId, + const std::string& targetFrameId, + const Transform& transformInSourceFrame = Transform(), + const ros::Time& time = ros::Time(0)); + + bool adaptCoordinates(Footstep& footstep, const std::string& sourceFrameId, + const std::string& targetFrameId, + const Transform& transformInSourceFrame = Transform(), + const ros::Time& time = ros::Time(0)); + + bool adaptCoordinates(EndEffectorTrajectory& endEffectorTrajectory, const std::string& sourceFrameId, + const std::string& targetFrameId, + const Transform& transformInSourceFrame = Transform(), + const ros::Time& time = ros::Time(0)); + + bool adaptCoordinates(BaseTrajectory& baseTrajectory, const std::string& sourceFrameId, + const std::string& targetFrameId, + const Transform& transformInSourceFrame, + const ros::Time& time); + + bool getTransform(const std::string& sourceFrameId, const std::string& targetFrameId, + const Transform& transformInSourceFrame, const ros::Time& time, + Transform& transform); + + private: + + /// TF buffer used to read the transformations. + /// Note: Needs to be updated from outside with + /// a TF Listener! + tf2_ros::Buffer& tfBuffer_; + + /// Cached transform for faster conversion. + std::string cachedTargetFrameId_, cachedSourceFrameId_; + ros::Time cachedTime_; + Transform cachedTransformInSourceFrame_; + Transform cachedTransform_; +}; + +} /* namespace free_gait */ diff --git a/free_gait_ros/include/free_gait_ros/StepRosConverter.hpp b/free_gait_ros/include/free_gait_ros/StepRosConverter.hpp new file mode 100644 index 0000000..a7d1ba0 --- /dev/null +++ b/free_gait_ros/include/free_gait_ros/StepRosConverter.hpp @@ -0,0 +1,71 @@ +/* + * StepRosConverter.hpp + * + * Created on: Feb 24, 2015 + * Author: Péter Fankhauser + * Institute: ETH Zurich, Autonomous Systems Lab + */ + +#pragma once + +// Free Gait +#include "free_gait_core/free_gait_core.hpp" + +// ROS +#include +#include +#include +#include +#include +#include +#include +#include + +// Quadruped model +#undef LOG +#include + +// STD +#include + +namespace free_gait { + +class StepRosConverter +{ + public: + StepRosConverter(const AdapterBase& adapter); + virtual ~StepRosConverter(); + + /*! + * Converts a ROS free gait step message to a step object. + * @param[in] message the step message. + * @param[out] gridMap the step object to be initialized. + * @return true if successful, false otherwise. + */ + bool fromMessage(const std::vector& message, std::vector& steps); + bool fromMessage(const free_gait_msgs::Step& message, Step& step); + bool fromMessage(const free_gait_msgs::Footstep& message, Footstep& footstep); + bool fromMessage(const free_gait_msgs::EndEffectorTarget& message, EndEffectorTarget& endEffectorTarget); + bool fromMessage(const free_gait_msgs::EndEffectorTrajectory& message, EndEffectorTrajectory& endEffectorTrajectory); + bool fromMessage(const free_gait_msgs::LegMode& message, LegMode& legMode); + bool fromMessage(const free_gait_msgs::JointTrajectory& message, JointTrajectory& jointTrajectory); + bool fromMessage(const free_gait_msgs::BaseAuto& message, BaseAuto& baseAuto); + bool fromMessage(const free_gait_msgs::BaseTarget& message, BaseTarget& baseTarget); + bool fromMessage(const free_gait_msgs::BaseTrajectory& message, BaseTrajectory& baseTrajectory); + bool fromMessage(const free_gait_msgs::CustomCommand& message, CustomCommand& customCommand); + + bool toMessage(const StepQueue& stepQueue, free_gait_msgs::ExecuteStepsGoal::_steps_type& message); + bool toMessage(const Step& step, free_gait_msgs::Step& message); + bool toMessage(const Footstep& footstep, free_gait_msgs::Footstep& message); + bool toMessage(const EndEffectorTrajectory& endEffectorTrajectory, free_gait_msgs::EndEffectorTrajectory& message); + bool toMessage(const JointTrajectory& jointTrajectory, free_gait_msgs::JointTrajectory& message); + bool toMessage(const BaseAuto& baseAuto, free_gait_msgs::BaseAuto& message); + bool toMessage(const BaseTrajectory& baseTrajectory, free_gait_msgs::BaseTrajectory& message); + bool toMessage(const CustomCommand& customCommand, free_gait_msgs::CustomCommand& message); + + private: + const AdapterBase& adapter_; +}; + +} +/* namespace */ diff --git a/free_gait_ros/include/free_gait_ros/free_gait_ros.hpp b/free_gait_ros/include/free_gait_ros/free_gait_ros.hpp new file mode 100644 index 0000000..2113ae8 --- /dev/null +++ b/free_gait_ros/include/free_gait_ros/free_gait_ros.hpp @@ -0,0 +1,19 @@ +/* + * free_gait_ros.hpp + * + * Created on: Jun 1, 2015 + * Author: Péter Fankhauser + * Institute: ETH Zurich, Autonomous Systems Lab + */ + +#pragma once + +#include "free_gait_ros/message_traits.hpp" +#include "free_gait_ros/AdapterRos.hpp" +#include "free_gait_ros/AdapterRosInterfaceBase.hpp" +#include "free_gait_ros/FreeGaitActionServer.hpp" +#include "free_gait_ros/FreeGaitActionClient.hpp" +#include "free_gait_ros/StepRosConverter.hpp" +#include "free_gait_ros/StepFrameConverter.hpp" +#include "free_gait_ros/StateRosPublisher.hpp" +#include "free_gait_ros/RosVisualization.hpp" diff --git a/free_gait_ros/include/free_gait_ros/message_traits.hpp b/free_gait_ros/include/free_gait_ros/message_traits.hpp new file mode 100644 index 0000000..62b4800 --- /dev/null +++ b/free_gait_ros/include/free_gait_ros/message_traits.hpp @@ -0,0 +1,32 @@ +#pragma once + +#include + +namespace ros { +namespace message_traits { + +namespace free_gait { + static std::string frameId("base_link"); +} + +template<> +struct FrameId >::type > +{ + static std::string* pointer(free_gait_msgs::ExecuteStepsActionGoal& m) + { + return &free_gait::frameId; + } + + static std::string const* pointer(const free_gait_msgs::ExecuteStepsActionGoal& m) + { + return &free_gait::frameId; + } + + static std::string value(const free_gait_msgs::ExecuteStepsActionGoal& m) + { + return free_gait::frameId; + } +}; + +} +} diff --git a/free_gait_ros/package.xml b/free_gait_ros/package.xml new file mode 100644 index 0000000..ae4e5f1 --- /dev/null +++ b/free_gait_ros/package.xml @@ -0,0 +1,25 @@ + + + free_gait_ros + 0.3.0 + ROS wrapper for free gait. + Peter Fankhauser + Peter Fankhauser + BSD + https://github.com/leggedrobotics/free_gait + https://github.com/leggedrobotics/free_gait/issues + + catkin + + free_gait_msgs + free_gait_core + quadruped_model + qp_solver + curves_ros + grid_map_core + grid_map_ros + pluginlib + robot_state_publisher + kdl_parser + eigen + diff --git a/free_gait_ros/src/AdapterRos.cpp b/free_gait_ros/src/AdapterRos.cpp new file mode 100644 index 0000000..eb8b7ec --- /dev/null +++ b/free_gait_ros/src/AdapterRos.cpp @@ -0,0 +1,89 @@ +/* + * AdapterRos.cpp + * + * Created on: Dec 1, 2016 + * Author: Péter Fankhauser + * Institute: ETH Zurich, Robotic Systems Lab + */ + +#include "free_gait_ros/AdapterRos.hpp" + +namespace free_gait { + +AdapterRos::AdapterRos(ros::NodeHandle& nodeHandle, const AdapterType type) + : nodeHandle_(nodeHandle), + adapterLoader_("free_gait_core", "free_gait::AdapterBase"), + adapterRosInterfaceLoader_("free_gait_ros", "free_gait::AdapterRosInterfaceBase") +{ + // Load and initialize adapter. + std::string adapterParameterName; + if (type == AdapterType::Base) { + adapterParameterName = "/free_gait/adapter_plugin/base"; + } else if (type == AdapterType::Preview){ + adapterParameterName = "/free_gait/adapter_plugin/preview"; + } + std::string adapterPluginName; + nodeHandle.getParam(adapterParameterName, adapterPluginName); + adapter_.reset(adapterLoader_.createUnmanagedInstance(adapterPluginName)); + + std::string adapterRosInterfacePluginName; + nodeHandle.getParam("/free_gait/adapter_ros_interface_plugin", adapterRosInterfacePluginName); + adapterRosInterface_.reset(adapterRosInterfaceLoader_.createUnmanagedInstance(adapterRosInterfacePluginName)); + + adapterRosInterface_->setNodeHandle(nodeHandle_); + adapterRosInterface_->readRobotDescription(); + adapterRosInterface_->initializeAdapter(*adapter_); +} + +AdapterRos::~AdapterRos() +{ +} + +bool AdapterRos::subscribeToRobotState(const std::string& robotStateTopic) +{ + // Get topic name parameter. + std::string topic(robotStateTopic); + if (topic.empty()) { + if (nodeHandle_.hasParam("/free_gait/robot_state")) { + nodeHandle_.getParam("/free_gait/robot_state", topic); + } else { + ROS_ERROR("Did not find ROS parameter for robot state topic '/free_gait/robot_state'."); + return false; + } + } + + // Subscribe. + return adapterRosInterface_->subscribeToRobotState(topic); +} + +void AdapterRos::unsubscribeFromRobotState() +{ + adapterRosInterface_->unsubscribeFromRobotState(); +} + +const std::string AdapterRos::getRobotStateMessageType() +{ + return adapterRosInterface_->getRobotStateMessageType(); +} + +bool AdapterRos::isReady() const +{ + return adapterRosInterface_->isReady(); +} + +bool AdapterRos::updateAdapterWithState() +{ + return adapterRosInterface_->updateAdapterWithRobotState(*adapter_); +} + +const AdapterBase& AdapterRos::getAdapter() const +{ + return *adapter_; +} + +AdapterBase* AdapterRos::getAdapterPtr() +{ + return adapter_.get(); +} + +} /* namespace free_gait */ diff --git a/free_gait_ros/src/AdapterRosInterfaceBase.cpp b/free_gait_ros/src/AdapterRosInterfaceBase.cpp new file mode 100644 index 0000000..d193c70 --- /dev/null +++ b/free_gait_ros/src/AdapterRosInterfaceBase.cpp @@ -0,0 +1,70 @@ +/* + * AdapterRosInterfaceBase.cpp + * + * Created on: Nov 29, 2016 + * Author: Péter Fankhauser + * Institute: ETH Zurich, Robotic Systems Lab + */ + +#include + +#include + +namespace free_gait { + +AdapterRosInterfaceBase::AdapterRosInterfaceBase() + : nodeHandle_(NULL) +{ +} + +AdapterRosInterfaceBase::~AdapterRosInterfaceBase() +{ +} + +void AdapterRosInterfaceBase::setNodeHandle(ros::NodeHandle& nodeHandle) +{ + nodeHandle_ = &nodeHandle; +} + +bool AdapterRosInterfaceBase::readRobotDescription() +{ + std::string robotDescriptionPath; + if (nodeHandle_->hasParam("/free_gait/robot_description")) { + nodeHandle_->getParam("/free_gait/robot_description", robotDescriptionPath); + } else { + throw pluginlib::PluginlibException("Did not find ROS parameter for robot description '/free_gait/robot_description'."); + } + + if (nodeHandle_->hasParam(robotDescriptionPath)) { + nodeHandle_->getParam(robotDescriptionPath, robotDescriptionUrdfString_); + } else { + throw pluginlib::PluginlibException("Did not find ROS parameter for robot description '" + robotDescriptionPath + "'."); + } + + return true; +} +bool AdapterRosInterfaceBase::updateAdapterWithRobotState(AdapterBase& adapter) const +{ + // TODO(Shunyao): using Extra Feedback +// State state = adapter.getState(); +// state.setAllJointPositions(all_joint_positions_); + +} + +bool AdapterRosInterfaceBase::subscribeToRobotState(const std::string& robotStateTopic) +{ + // topic: "/free_gait/robot_state", + joint_states_sub_ = nodeHandle_->subscribe("/robot_state", 1, &AdapterRosInterfaceBase::updateRobotState,this); + +} + +void AdapterRosInterfaceBase::updateRobotState(const free_gait_msgs::RobotStateConstPtr& robotState) +{ + for(int i = 1;i<12;i++) + { + all_joint_positions_(i) = robotState->lf_leg_joints.position[i]; + } + +} + +} /* namespace free_gait */ diff --git a/free_gait_ros/src/FreeGaitActionClient.cpp b/free_gait_ros/src/FreeGaitActionClient.cpp new file mode 100644 index 0000000..a75cb74 --- /dev/null +++ b/free_gait_ros/src/FreeGaitActionClient.cpp @@ -0,0 +1,118 @@ +/* + * FreeGaitActionClient.cpp + * + * Created on: Oct 21, 2015 + * Author: Georg Wiedebach, Péter Fankhauser + */ + +#include + +namespace free_gait { + +FreeGaitActionClient::FreeGaitActionClient(ros::NodeHandle& nodeHandle) + : nodeHandle_(nodeHandle), + state_(ActionState::UNINITIALIZED) +{ + // Get action server topic. + std::string actionServerTopic; + if (nodeHandle_.hasParam("/free_gait/action_server")) { + nodeHandle_.getParam("/free_gait/action_server", actionServerTopic); + } else { + throw std::runtime_error("Did not find ROS parameter for Free Gait Action Server '/free_gait/action_server'."); + } + + // Get preview topic. + std::string previewTopic; + if (nodeHandle_.hasParam("/free_gait/preview_topic")) { + nodeHandle_.getParam("/free_gait/preview_topic", previewTopic); + } else { + throw std::runtime_error( + "Did not find ROS parameter for Free Gait Preview Topic '/free_gait/preview_topic'."); + } + + // Initialize action client. + client_.reset(new actionlib::SimpleActionClient(nodeHandle, actionServerTopic)); + state_ = ActionState::DONE; + + // Initialize preview publisher. + previewPublisher_ = nodeHandle_.advertise(previewTopic, 1, false); + + state_ = ActionState::INITIALIZED; +} + +void FreeGaitActionClient::registerCallback( + std::function activeCallback, + std::function feedbackCallback, + std::function doneCallback) +{ + activeCallback_ = activeCallback; + feedbackCallback_ = feedbackCallback; + doneCallback_ = doneCallback; +} + +void FreeGaitActionClient::sendGoal(const free_gait_msgs::ExecuteStepsGoal& goal) +{ + if (!nodeHandle_.hasParam("use_preview")) { + ROS_ERROR("Did not find ROS parameter for 'use_preview'."); + return; + } + + bool usePreview; + nodeHandle_.getParam("use_preview", usePreview); + return sendGoal(goal, usePreview); +} + +void FreeGaitActionClient::sendGoal(const free_gait_msgs::ExecuteStepsGoal& goal, + const bool usePreview) +{ + if (usePreview) { + free_gait_msgs::ExecuteStepsActionGoal actionGoal; + actionGoal.goal = goal; + previewPublisher_.publish(actionGoal); + state_ = ActionState::ACTIVE; + actionlib::SimpleClientGoalState state(actionlib::SimpleClientGoalState::ACTIVE); + free_gait_msgs::ExecuteStepsResult result; + doneCallback_(state, result); + } else { + if (state_ == ActionState::ACTIVE || state_ == ActionState::PENDING) { + client_->stopTrackingGoal(); + } + state_ = ActionState::PENDING; + client_->waitForServer(); + client_->sendGoal(goal, boost::bind(&FreeGaitActionClient::doneCallback, this, _1, _2), + boost::bind(&FreeGaitActionClient::activeCallback, this), + boost::bind(&FreeGaitActionClient::feedbackCallback, this, _1)); + } +} + +void FreeGaitActionClient::waitForResult(const double timeout) +{ + client_->waitForResult(ros::Duration(timeout)); +} + +const FreeGaitActionClient::ActionState& FreeGaitActionClient::getState() +{ + return state_; +} + +void FreeGaitActionClient::activeCallback() +{ + state_ = ActionState::ACTIVE; + if (activeCallback_) activeCallback_(); +} + +void FreeGaitActionClient::feedbackCallback( + const free_gait_msgs::ExecuteStepsFeedbackConstPtr& feedback) +{ + if (feedbackCallback_) feedbackCallback_(feedback); +} + +void FreeGaitActionClient::doneCallback(const actionlib::SimpleClientGoalState& state, + const free_gait_msgs::ExecuteStepsResultConstPtr& result) +{ + state_ = ActionState::DONE; + if (doneCallback_) doneCallback_(state, *result); +} + +} diff --git a/free_gait_ros/src/FreeGaitActionServer.cpp b/free_gait_ros/src/FreeGaitActionServer.cpp new file mode 100644 index 0000000..88d7bd3 --- /dev/null +++ b/free_gait_ros/src/FreeGaitActionServer.cpp @@ -0,0 +1,210 @@ +/* + * FreeGaitActionServer.cpp + * + * Created on: Feb 6, 2015 + * Author: Péter Fankhauser + * Institute: ETH Zurich, Autonomous Systems Lab + */ + +#include +#include + +// ROS +#include +#include + +#include + +namespace free_gait { + +FreeGaitActionServer::FreeGaitActionServer(ros::NodeHandle nodeHandle, const std::string& name, + Executor& executor, AdapterBase& adapter) + : nodeHandle_(nodeHandle), + name_(name), + executor_(executor), + adapter_(adapter), + server_(nodeHandle_, name_, false), + isPreempting_(false), + nStepsInCurrentGoal_(0) +{ +} + +FreeGaitActionServer::~FreeGaitActionServer() +{ +} + +void FreeGaitActionServer::initialize() +{ + server_.registerGoalCallback(boost::bind(&FreeGaitActionServer::goalCallback, this)); + server_.registerPreemptCallback(boost::bind(&FreeGaitActionServer::preemptCallback, this)); +} + +//void FreeGaitActionServer::setExecutor(std::shared_ptr executor) +//{ +// Executor::Lock lock(executor_.getMutex()); +// executor_ = executor; +//} +// +//void FreeGaitActionServer::setAdapter(std::shared_ptr adapter) +//{ +// adapter_ = adapter; +//} + +void FreeGaitActionServer::start() +{ + server_.start(); + ROS_INFO_STREAM("Started " << name_ << " action server."); +} + +void FreeGaitActionServer::update() +{ + if (!server_.isActive()) return; + Executor::Lock lock(executor_.getMutex()); + bool stepQueueEmpty = executor_.getQueue().empty(); + lock.unlock(); + if (stepQueueEmpty) { + // Succeeded. + if (isPreempting_) { + // Preempted. + setPreempted(); + } else { + setSucceeded(); + } + } else { + // Ongoing. + lock.lock(); + if (executor_.getQueue().active()) publishFeedback(); + lock.unlock(); + } +} + +void FreeGaitActionServer::shutdown() +{ + ROS_INFO("Shutting down Free Gait Action Server."); + server_.shutdown(); +} + +bool FreeGaitActionServer::isActive() +{ + return server_.isActive(); +} + +void FreeGaitActionServer::goalCallback() +{ + ROS_INFO("Received goal for StepAction."); +// if (server_.isActive()) server_.setRejected(); + + const auto goal = server_.acceptNewGoal(); + std::vector steps; + for (auto& stepMessage : goal->steps) { + Step step; + adapter_.fromMessage(stepMessage, step); + steps.push_back(step); + } + Executor::Lock lock(executor_.getMutex()); + + // Check if last step and first step of new goal are + // pure `BaseAuto` commands: In this case, replace the + // last one with the new one for smooth motion. + if (executor_.getQueue().size() >= 2) { + const auto& step = *executor_.getQueue().getQueue().end(); + if (!step.hasLegMotion() && step.hasBaseMotion()) { + if (step.getBaseMotion().getType() == BaseMotionBase::Type::Auto) { + executor_.getQueue().clearLastNSteps(1); + } + } + } + executor_.getQueue().add(steps); + + Executor::PreemptionType preemptionType; + switch (goal->preempt) { + case free_gait_msgs::ExecuteStepsGoal::PREEMPT_IMMEDIATE: + preemptionType = Executor::PreemptionType::PREEMPT_IMMEDIATE; + break; + case free_gait_msgs::ExecuteStepsGoal::PREEMPT_STEP: + preemptionType = Executor::PreemptionType::PREEMPT_STEP; + break; + case free_gait_msgs::ExecuteStepsGoal::PREEMPT_NO: + preemptionType = Executor::PreemptionType::PREEMPT_NO; + break; + default: + break; + } + executor_.setPreemptionType(preemptionType); + nStepsInCurrentGoal_ = goal->steps.size(); + isPreempting_ = false; + lock.unlock(); +} + +void FreeGaitActionServer::preemptCallback() +{ + ROS_INFO("StepAction is requested to preempt."); + Executor::Lock lock(executor_.getMutex()); + executor_.stop(); + isPreempting_ = true; +} + +void FreeGaitActionServer::publishFeedback() +{ + free_gait_msgs::ExecuteStepsFeedback feedback; + Executor::Lock lock(executor_.getMutex()); + if (executor_.getQueue().empty()) return; + feedback.step_id = executor_.getState().getStepId(); + feedback.queue_size = executor_.getQueue().size(); + feedback.number_of_steps_in_goal = nStepsInCurrentGoal_; + feedback.step_number = feedback.number_of_steps_in_goal - feedback.queue_size + 1; + + if (executor_.getState().getRobotExecutionStatus() == false + || executor_.getQueue().active() == false) { + feedback.status = free_gait_msgs::ExecuteStepsFeedback::PROGRESS_PAUSED; + } else { + feedback.status = free_gait_msgs::ExecuteStepsFeedback::PROGRESS_EXECUTING; +// default: +// feedback.status = free_gait_msgs::ExecuteStepsFeedback::PROGRESS_UNKNOWN; +// feedback.description = "Unknown."; +// break; + } + + feedback.description = executor_.getFeedbackDescription(); + executor_.clearFeedbackDescription(); + + if (executor_.getQueue().active()) { + const auto& step = executor_.getQueue().getCurrentStep(); + feedback.duration = ros::Duration(step.getTotalDuration()); + feedback.phase = step.getTotalPhase(); + for (const auto& legMotion : step.getLegMotions()) { + const std::string legName(executor_.getAdapter().getLimbStringFromLimbEnum(legMotion.first)); + feedback.active_branches.push_back(legName); + } + if (step.hasBaseMotion()) { + feedback.active_branches.push_back(executor_.getAdapter().getBaseString()); + } + } + + lock.unlock(); + server_.publishFeedback(feedback); +} + +void FreeGaitActionServer::setSucceeded() +{ + ROS_INFO("StepAction succeeded."); + free_gait_msgs::ExecuteStepsResult result; + server_.setSucceeded(result, "Step action has been reached."); +} + +void FreeGaitActionServer::setPreempted() +{ + ROS_INFO("StepAction preempted."); + free_gait_msgs::ExecuteStepsResult result; + server_.setPreempted(result, "Step action has been preempted."); + isPreempting_ = false; +} + +void FreeGaitActionServer::setAborted() +{ + ROS_INFO("StepAction aborted."); + free_gait_msgs::ExecuteStepsResult result; + server_.setAborted(result, "Step action has failed (aborted)."); +} + +} /* namespace */ diff --git a/free_gait_ros/src/RosVisualization.cpp b/free_gait_ros/src/RosVisualization.cpp new file mode 100644 index 0000000..69417e5 --- /dev/null +++ b/free_gait_ros/src/RosVisualization.cpp @@ -0,0 +1,94 @@ +/* + * RosVisualization.cpp + * + * Created on: Jul 4, 2017 + * Author: Péter Fankhauser + */ + +#include + +#include +#include +#include + +namespace free_gait { + +RosVisualization::RosVisualization() +{ +} + +RosVisualization::~RosVisualization() +{ +} + +const visualization_msgs::Marker RosVisualization::getStanceMarker(const Stance& stance, const std::string& frameId, + const std_msgs::ColorRGBA& color) +{ + grid_map::Polygon polygon; + polygon.setFrameId(frameId); + std::vector footholdsOrdered; + getFootholdsCounterClockwiseOrdered(stance, footholdsOrdered); + double height = 0.0; + for (auto foothold : footholdsOrdered) { + polygon.addVertex(foothold.vector().head<2>()); + height += foothold.z(); + } + height = height / footholdsOrdered.size(); + + visualization_msgs::Marker marker; + grid_map::PolygonRosConverter::toTriangleListMarker(polygon, color, height, marker); + marker.ns = "Stance"; + marker.lifetime = ros::Duration(0.0); + return marker; +} + +const visualization_msgs::Marker RosVisualization::getComMarker(const Position& comPosition, const std::string& frameId, + const std_msgs::ColorRGBA& color, + const double size) +{ + visualization_msgs::Marker marker; + marker.ns = "Center of Mass"; + marker.header.frame_id = frameId; + marker.lifetime = ros::Duration(0.0); + marker.action = visualization_msgs::Marker::ADD; + marker.type = visualization_msgs::Marker::SPHERE; + marker.color = color; + marker.scale.x = size; + marker.scale.y = size; + marker.scale.z = size; + Pose pose; + pose.getPosition() = comPosition; + kindr_ros::convertToRosGeometryMsg(pose, marker.pose); + return marker; +} + +const visualization_msgs::MarkerArray RosVisualization::getComWithProjectionMarker(const Position& comPosition, + const std::string& frameId, + const std_msgs::ColorRGBA& color, + const double comMarkerSize, + const double projectionLenght, + const double projectionDiameter) +{ + visualization_msgs::MarkerArray markerArray; + markerArray.markers.push_back(getComMarker(comPosition, frameId, color, comMarkerSize)); + + visualization_msgs::Marker marker; + marker.ns = "Center of Mass Projection"; + marker.header.frame_id = frameId; + marker.lifetime = ros::Duration(0.0); + marker.action = visualization_msgs::Marker::ADD; + marker.type = visualization_msgs::Marker::CYLINDER; + marker.color = color; + marker.scale.x = projectionDiameter; + marker.scale.y = projectionDiameter; + marker.scale.z = projectionLenght; + Pose pose; + pose.getPosition() = comPosition; + kindr_ros::convertToRosGeometryMsg(pose, marker.pose); + marker.pose.position.z -= 0.5 * projectionLenght; + markerArray.markers.push_back(marker); + + return markerArray; +} + +} diff --git a/free_gait_ros/src/StateRosPublisher.cpp b/free_gait_ros/src/StateRosPublisher.cpp new file mode 100644 index 0000000..c7b742c --- /dev/null +++ b/free_gait_ros/src/StateRosPublisher.cpp @@ -0,0 +1,130 @@ +/* + * StateRosPublisher.cpp + * + * Created on: Dec 6, 2016 + * Author: Péter Fankhauser + * Institute: ETH Zurich, Autonomous Systems Lab + */ + +#include + +// KDL +#include + +// Kindr +#include + +// STD +#include +#include + +#include + +namespace free_gait { + +StateRosPublisher::StateRosPublisher(ros::NodeHandle& nodeHandle, + AdapterBase& adapter) + : nodeHandle_(nodeHandle), + adapter_(adapter) +{ + tfPrefix_ = nodeHandle_.param("/free_gait/preview_tf_prefix", std::string("")); + initializeRobotStatePublisher(); +} + +StateRosPublisher::~StateRosPublisher() +{ +} + +StateRosPublisher::StateRosPublisher(const StateRosPublisher& other) : + nodeHandle_(other.nodeHandle_), + tfPrefix_(other.tfPrefix_), + adapter_(other.adapter_), + tfBroadcaster_(other.tfBroadcaster_) +{ + if (other.robotStatePublisher_) { + robotStatePublisher_.reset( + new robot_state_publisher::RobotStatePublisher(*other.robotStatePublisher_)); + } +} + +void StateRosPublisher::setTfPrefix(const std::string tfPrefix) +{ + tfPrefix_ = tfPrefix; +} + +bool StateRosPublisher::initializeRobotStatePublisher() +{ + std::string robotDescriptionPath; + if (nodeHandle_.hasParam("/free_gait/robot_description")) { + nodeHandle_.getParam("/free_gait/robot_description", robotDescriptionPath); + } else { + ROS_ERROR("Did not find ROS parameter for robot description '/free_gait/robot_description'."); + return false; + } + + urdf::Model model; +// if (!model.initParam(robotDescriptionPath)) return false;initFile + if (!model.initFile(robotDescriptionPath)) return false; + + KDL::Tree tree; + if (!kdl_parser::treeFromUrdfModel(model, tree)){ + ROS_ERROR("Failed to extract KDL tree from XML robot description"); + return false; + } + + robotStatePublisher_.reset(new robot_state_publisher::RobotStatePublisher(tree)); + return true; +} + +bool StateRosPublisher::publish(const State& state) +{ + const ros::Time time = ros::Time::now(); + + // Publish joint states. + std::vector jointNames; + + state.getAllJointNames(jointNames); + + JointPositions jointPositions = state.getJointPositions(); + + if (jointNames.size() != jointPositions.vector().size()) { + ROS_ERROR("Joint name vector and joint position are not of equal size!"); + return false; + } + std::map jointPositionsMap; + for (size_t i = 0; i < jointNames.size(); ++i) { + jointPositionsMap[jointNames[i]] = jointPositions(i); + std::cout<publishTransforms(jointPositionsMap, time, tfPrefix_); + robotStatePublisher_->publishFixedTransforms(tfPrefix_); +ROS_INFO("In ros state publisher"); + // Publish base position. + geometry_msgs::TransformStamped tfTransform; + tfTransform.header.stamp = time; + tfTransform.header.frame_id = adapter_.getWorldFrameId(); + tfTransform.child_frame_id = tf::resolve(tfPrefix_, adapter_.getBaseFrameId()); + kindr_ros::convertToRosGeometryMsg(state.getPositionWorldToBaseInWorldFrame(), tfTransform.transform.translation); + kindr_ros::convertToRosGeometryMsg(state.getOrientationBaseToWorld(), tfTransform.transform.rotation); + tfBroadcaster_.sendTransform(tfTransform); + + // Publish frame transforms. + std::vector frameTransforms; + //TODO(Shunyao) : what else tranform needed to be broadcasted? +// adapter_.getAvailableFrameTransforms(frameTransforms); + for (const auto& frameId : frameTransforms) { + geometry_msgs::TransformStamped tfTransform; + tfTransform.header.stamp = time; + tfTransform.header.frame_id = adapter_.getWorldFrameId(); + tfTransform.child_frame_id = tf::resolve(tfPrefix_, frameId); + kindr_ros::convertToRosGeometryMsg(adapter_.getFrameTransform(frameId), tfTransform.transform); + tfBroadcaster_.sendTransform(tfTransform); + } + + return true; +} + +//void StateRosPublisher::publishSupportRegion(const State& state) + +} /* namespace free_gait */ diff --git a/free_gait_ros/src/StepFrameConverter.cpp b/free_gait_ros/src/StepFrameConverter.cpp new file mode 100644 index 0000000..675fcc1 --- /dev/null +++ b/free_gait_ros/src/StepFrameConverter.cpp @@ -0,0 +1,166 @@ +/* + * StepFrameConverter.cpp + * + * Created on: Nov 11, 2016 + * Author: Péter Fankhauser + * Institute: ETH Zurich, Robotic Systems Lab + */ + +#include +#include + +namespace free_gait { + +StepFrameConverter::StepFrameConverter(tf2_ros::Buffer& tfBuffer) + : tfBuffer_(tfBuffer) +{ +} + +StepFrameConverter::~StepFrameConverter() +{ +} + +bool StepFrameConverter::adaptCoordinates(StepQueue& stepQueue, const std::string& sourceFrameId, + const std::string& targetFrameId, + const Transform& transformInSourceFrame, + const ros::Time& time) +{ + for (Step& step : stepQueue.queue_) { + if (!adaptCoordinates(step, sourceFrameId, targetFrameId, transformInSourceFrame, time)) return false; + } + return true; +} + + +bool StepFrameConverter::adaptCoordinates(Step& step, const std::string& sourceFrameId, + const std::string& targetFrameId, + const Transform& transformInSourceFrame, + const ros::Time& time) +{ + // Leg motions. + for (const auto& legMotion : step.getLegMotions()) { + + // Foostep. + if (legMotion.second->getType() == LegMotionBase::Type::Footstep) { + Footstep& footstep = dynamic_cast(*(legMotion.second)); + if (!adaptCoordinates(footstep, sourceFrameId, targetFrameId, transformInSourceFrame, time)) return false; + } + + // EndEffectorTrajectory. + if (legMotion.second->getType() == LegMotionBase::Type::EndEffectorTrajectory) { + EndEffectorTrajectory& endEffectorTrajectory = dynamic_cast(*(legMotion.second)); + if (!adaptCoordinates(endEffectorTrajectory, sourceFrameId, targetFrameId, transformInSourceFrame, time)) return false; + } + + } + + // Base motion. + if (step.hasBaseMotion()) { + + const auto& baseMotion = step.getBaseMotion(); + + // Base Auto. +// if (baseMotion.getType() == BaseMotionBase::Type::Auto) { +// BaseAuto& baseAuto = dynamic_cast(baseMotion); +// if (!adaptCoordinates(baseAuto, sourceFrameId, targetFrameId, transformInTargetFrame, time)) return false; +// } + + // Base Trajectory. + if (baseMotion.getType() == BaseMotionBase::Type::Trajectory){ + const BaseTrajectory& baseTrajectory = dynamic_cast(baseMotion); + if (!adaptCoordinates(const_cast (baseTrajectory), sourceFrameId, targetFrameId, transformInSourceFrame, time)) return false; + } + } + return true; +} + +bool StepFrameConverter::adaptCoordinates(Footstep& footstep, const std::string& sourceFrameId, + const std::string& targetFrameId, + const Transform& transformInSourceFrame, + const ros::Time& time) +{ + Transform transform; + if (footstep.getFrameId(ControlLevel::Position) == sourceFrameId) { + if (!getTransform(sourceFrameId, targetFrameId, transformInSourceFrame, time, transform)) return false; + footstep.target_ = transform.transform(footstep.target_); + footstep.frameId_ = targetFrameId; + } + + return true; +} + +bool StepFrameConverter::adaptCoordinates(EndEffectorTrajectory& endEffectorTrajectory, const std::string& sourceFrameId, + const std::string& targetFrameId, + const Transform& transformInSourceFrame, + const ros::Time& time) +{ + Transform transform; + if (endEffectorTrajectory.getFrameId(ControlLevel::Position) == sourceFrameId) { + if (!getTransform(sourceFrameId, targetFrameId, transformInSourceFrame, time, transform)) return false; + for (auto& knot : endEffectorTrajectory.values_.at(ControlLevel::Position)){ + knot = (transform.transform(Position(knot))).vector(); + } + endEffectorTrajectory.frameIds_.at(ControlLevel::Position) = targetFrameId; + } + + return true; +} + +bool StepFrameConverter::adaptCoordinates(BaseTrajectory& baseTrajectory, const std::string& sourceFrameId, + const std::string& targetFrameId, + const Transform& transformInSourceFrame, + const ros::Time& time) +{ + Transform transform; + if (baseTrajectory.getFrameId(ControlLevel::Position) == sourceFrameId) { + if (!getTransform(sourceFrameId, targetFrameId, transformInSourceFrame, time, transform)) return false; + for (auto& knot : baseTrajectory.values_.at(ControlLevel::Position)){ + knot.getPosition() = transform.transform(knot.getPosition()); + knot.getRotation() = transform.getRotation()*knot.getRotation(); + } + baseTrajectory.frameIds_.at(ControlLevel::Position) = targetFrameId; + } + + return true; +} + +bool StepFrameConverter::getTransform(const std::string& sourceFrameId, + const std::string& targetFrameId, + const Transform& transformInSourceFrame, + const ros::Time& time, Transform& transform) +{ + if (sourceFrameId == cachedSourceFrameId_ && targetFrameId == cachedTargetFrameId_ + && transformInSourceFrame.getPosition() == cachedTransformInSourceFrame_.getPosition() + && transformInSourceFrame.getRotation() == cachedTransformInSourceFrame_.getRotation() + && time == cachedTime_ && !time.isZero()) { + // No lookup required if already cached. + transform = cachedTransform_; + return true; + } + + // Adding this leads to blocking `lookupTransform`. Why? + std::string errorMessage; + tfBuffer_.canTransform(targetFrameId, sourceFrameId, time, ros::Duration(5.0), &errorMessage); + geometry_msgs::TransformStamped transformStamped; + try { + // TODO Why is `lookupTransform` not blocking here? + transformStamped = tfBuffer_.lookupTransform(targetFrameId, sourceFrameId, time, ros::Duration(5.0)); + } catch (tf2::TransformException &ex) { + ROS_ERROR("%s", ex.what()); + return false; + } + + kindr_ros::convertFromRosGeometryMsg(transformStamped.transform, transform); + transform = transform * transformInSourceFrame; + + // Cache transform. + cachedSourceFrameId_ = sourceFrameId; + cachedTargetFrameId_ = targetFrameId; + cachedTransformInSourceFrame_ = transformInSourceFrame; + cachedTime_ = time; + cachedTransform_ = transform; + + return true; +} + +} /* namespace free_gait */ diff --git a/free_gait_ros/src/StepRosConverter.cpp b/free_gait_ros/src/StepRosConverter.cpp new file mode 100644 index 0000000..b1353e9 --- /dev/null +++ b/free_gait_ros/src/StepRosConverter.cpp @@ -0,0 +1,660 @@ +/* + * StepRosConverter.cpp + * + * Created on: Feb 25, 2015 + * Author: Péter Fankhauser + * Institute: ETH Zurich, Autonomous Systems Lab + */ + +#include "free_gait_ros/StepRosConverter.hpp" + +// Quadruped model +#include "quadruped_model/QuadrupedModel.hpp" + +// Kindr +#include "kindr_ros/kindr_ros.hpp" + +namespace free_gait { + +StepRosConverter::StepRosConverter(const AdapterBase& adapter) + : adapter_(adapter) +{ +} + +StepRosConverter::~StepRosConverter() +{ +} + +bool StepRosConverter::fromMessage(const std::vector& message, std::vector& steps) +{ + steps.resize(message.size()); + for (const auto& stepMessage : message) { + Step step; + fromMessage(stepMessage, step); + steps.push_back(step); + } + return true; +} + +bool StepRosConverter::fromMessage(const free_gait_msgs::Step& message, Step& step) +{ + // ID. + step.setId(message.id); + + // Leg motion. + for (const auto& footstepMessage : message.footstep) { + const auto limb = adapter_.getLimbEnumFromLimbString(footstepMessage.name); + Footstep footstep(limb); + if (!fromMessage(footstepMessage, footstep)) return false; + step.addLegMotion(footstep); + } + + for (const auto& endEffectorTargetMessage : message.end_effector_target) { + const auto limb = adapter_.getLimbEnumFromLimbString(endEffectorTargetMessage.name); + EndEffectorTarget endEffectorTarget(limb); + if (!fromMessage(endEffectorTargetMessage, endEffectorTarget)) return false; + step.addLegMotion(endEffectorTarget); + } + + for (const auto& endEffectorTrajectoryMessage : message.end_effector_trajectory) { + const auto limb = adapter_.getLimbEnumFromLimbString(endEffectorTrajectoryMessage.name); + EndEffectorTrajectory endEffectorTrajectory(limb); + if (!fromMessage(endEffectorTrajectoryMessage, endEffectorTrajectory)) return false; + step.addLegMotion(endEffectorTrajectory); + } + + for (const auto& legModeMessage : message.leg_mode) { + const auto limb = adapter_.getLimbEnumFromLimbString(legModeMessage.name); + LegMode legMode(limb); + if (!fromMessage(legModeMessage, legMode)) return false; + step.addLegMotion(legMode); + } + + for (const auto& jointTrajectoryMessage : message.joint_trajectory) { + const auto limb = adapter_.getLimbEnumFromLimbString(jointTrajectoryMessage.name); + JointTrajectory jointTrajectory(limb); + if (!fromMessage(jointTrajectoryMessage, jointTrajectory)) return false; + step.addLegMotion(jointTrajectory); + } + + // Base motion. + for (const auto& baseAutoMessage : message.base_auto) { + BaseAuto baseAuto; + if (!fromMessage(baseAutoMessage, baseAuto)) return false; + step.addBaseMotion(baseAuto); + } + + for (const auto& baseTargetMessage : message.base_target) { + BaseTarget baseTarget; + if (!fromMessage(baseTargetMessage, baseTarget)) return false; + step.addBaseMotion(baseTarget); + } + + for (const auto& baseTrajectoryMessage : message.base_trajectory) { + BaseTrajectory baseTrajectory; + if (!fromMessage(baseTrajectoryMessage, baseTrajectory)) return false; + step.addBaseMotion(baseTrajectory); + } + + // Custom command. + for (const auto& customCommandMessage : message.custom_command) { + CustomCommand customCommand; + if (!fromMessage(customCommandMessage, customCommand)) return false; + step.addCustomCommand(customCommand); + } + + return true; +} + +bool StepRosConverter::fromMessage(const free_gait_msgs::Footstep& message, + Footstep& footstep) +{ + // Limb. + footstep.limb_ = adapter_.getLimbEnumFromLimbString(message.name); + + // Target. + footstep.frameId_ = message.target.header.frame_id; + Position target; + kindr_ros::convertFromRosGeometryMsg(message.target.point, target); + footstep.target_ = target; + + // Profile. + footstep.profileHeight_ = message.profile_height; + footstep.profileType_ = message.profile_type; + + // Average Velocity. + footstep.averageVelocity_ = message.average_velocity; + + // Surface normal. + Vector surfaceNormal; + kindr_ros::convertFromRosGeometryMsg(message.surface_normal.vector, surfaceNormal); + footstep.surfaceNormal_.reset(new Vector(surfaceNormal)); + + // Ignore contact. + footstep.ignoreContact_ = message.ignore_contact; + + // Ignore for pose adaptation. + footstep.ignoreForPoseAdaptation_ = message.ignore_for_pose_adaptation; + + return true; +} + +bool StepRosConverter::fromMessage(const free_gait_msgs::EndEffectorTarget& message, + EndEffectorTarget& endEffectorTarget) +{ + // Limb. + endEffectorTarget.limb_ = adapter_.getLimbEnumFromLimbString(message.name); + + // Target position. + endEffectorTarget.controlSetup_[ControlLevel::Position] = !message.target_position.empty(); + if (endEffectorTarget.controlSetup_[ControlLevel::Position]) { + endEffectorTarget.frameIds_[ControlLevel::Position] = message.target_position[0].header.frame_id; + Position targetPosition; + kindr_ros::convertFromRosGeometryMsg(message.target_position[0].point, targetPosition); + endEffectorTarget.targetPosition_ = targetPosition; + } + + // Target velocity. + endEffectorTarget.controlSetup_[ControlLevel::Velocity] = !message.target_velocity.empty(); + if (endEffectorTarget.controlSetup_[ControlLevel::Velocity]) { + endEffectorTarget.frameIds_[ControlLevel::Velocity] = message.target_velocity[0].header.frame_id; + LinearVelocity targetVelocity; + kindr_ros::convertFromRosGeometryMsg(message.target_velocity[0].vector, targetVelocity); + endEffectorTarget.targetVelocity_ = targetVelocity; + } + + // TODO. + endEffectorTarget.controlSetup_[ControlLevel::Acceleration] = !message.target_acceleration.empty(); + endEffectorTarget.controlSetup_[ControlLevel::Effort] = !message.target_force.empty(); + + // Average Velocity. + endEffectorTarget.averageVelocity_ = message.average_velocity; + + // Surface normal. + Vector surfaceNormal; + kindr_ros::convertFromRosGeometryMsg(message.surface_normal.vector, surfaceNormal); + endEffectorTarget.surfaceNormal_.reset(new Vector(surfaceNormal)); + + // Ignore contact. + endEffectorTarget.ignoreContact_ = message.ignore_contact; + + // Ignore for pose adaptation. + endEffectorTarget.ignoreForPoseAdaptation_ = message.ignore_for_pose_adaptation; + + return true; +} + +bool StepRosConverter::fromMessage(const free_gait_msgs::EndEffectorTrajectory& message, + EndEffectorTrajectory& endEffectorTrajectory) +{ + // Limb. + endEffectorTrajectory.limb_ = adapter_.getLimbEnumFromLimbString(message.name); + + // Trajectory. + endEffectorTrajectory.frameIds_[ControlLevel::Position] = message.trajectory.header.frame_id; + + for (const auto& point : message.trajectory.points) { + if (!point.transforms.empty()) endEffectorTrajectory.controlSetup_[ControlLevel::Position] = true; + if (!point.velocities.empty()) endEffectorTrajectory.controlSetup_[ControlLevel::Velocity] = true; + if (!point.accelerations.empty()) endEffectorTrajectory.controlSetup_[ControlLevel::Acceleration] = true; + } + + for (const auto& controlSetup : endEffectorTrajectory.controlSetup_) { + if (!controlSetup.second) continue; + endEffectorTrajectory.values_[controlSetup.first] = std::vector(); + } + + // TODO Copy times correctly for pure velocity or acceleration trajectories. + for (const auto& point : message.trajectory.points) { + if (!point.transforms.empty()) { + endEffectorTrajectory.times_.push_back(ros::Duration(point.time_from_start).toSec()); + } else { + std::cerr << "StepRosConverter: Could not read from ROS message, only position trajectories are supported for now." << std::endl; + break; + } + } + + for (const auto& controlSetup : endEffectorTrajectory.controlSetup_) { + if (!controlSetup.second)continue; + for (const auto& point : message.trajectory.points) { + if (controlSetup.first == ControlLevel::Position && !point.transforms.empty()) { + Position position; + kindr_ros::convertFromRosGeometryMsg(point.transforms[0].translation, position); + endEffectorTrajectory.values_[controlSetup.first].push_back(position.vector()); + } else if (controlSetup.first == ControlLevel::Velocity && !point.velocities.empty()) { +// baseTrajectory.derivatives_[controlSetup.first][j].push_back(point.velocities[j]); + } else if (controlSetup.first == ControlLevel::Acceleration && !point.accelerations.empty()) { +// baseTrajectory.derivatives_[controlSetup.first][j].push_back(point.accelerations[j]); + } /*else if (controlSetup.first == ControlLevel::Effort && !point.effort.empty()) { + baseTrajectory.derivatives_[controlSetup.first][j].push_back(point.effort[j]); + }*/ + } + } + + // Surface normal. + Vector surfaceNormal; + kindr_ros::convertFromRosGeometryMsg(message.surface_normal.vector, surfaceNormal); + endEffectorTrajectory.surfaceNormal_.reset(new Vector(surfaceNormal)); + + // Ignore contact. + endEffectorTrajectory.ignoreContact_ = message.ignore_contact; + + // Ignore for pose adaptation. + endEffectorTrajectory.ignoreForPoseAdaptation_ = message.ignore_for_pose_adaptation; + + return true; +} + +bool StepRosConverter::fromMessage(const free_gait_msgs::LegMode& message, LegMode& legMode) +{ + // Limb. + legMode.limb_ = adapter_.getLimbEnumFromLimbString(message.name); + + // Frame id. // TODO +// foostep.frameId_ = message.target.header.frame_id; + + // Support mode. + legMode.ignoreContact_ = !message.support_leg; + + // Duration. + legMode.duration_ = ros::Duration(message.duration).toSec(); + + // Surface normal. + Vector surfaceNormal; + kindr_ros::convertFromRosGeometryMsg(message.surface_normal.vector, surfaceNormal); + legMode.surfaceNormal_.reset(new Vector(surfaceNormal)); + + // Ignore for pose adaptation. + legMode.ignoreForPoseAdaptation_ = message.ignore_for_pose_adaptation; + + return true; +} + +bool StepRosConverter::fromMessage(const free_gait_msgs::JointTrajectory& message, JointTrajectory& jointTrajectory) +{ + // Limb. + jointTrajectory.limb_ = adapter_.getLimbEnumFromLimbString(message.name); + + // Trajectory. // TODO + jointTrajectory.controlSetup_[ControlLevel::Position] = false; + jointTrajectory.controlSetup_[ControlLevel::Velocity] = false; + jointTrajectory.controlSetup_[ControlLevel::Acceleration] = false; + jointTrajectory.controlSetup_[ControlLevel::Effort] = false; + + jointTrajectory.jointNodeEnums_.clear(); + for (const auto& jointName : message.trajectory.joint_names) { + jointTrajectory.jointNodeEnums_.push_back(adapter_.getJointNodeEnumFromJointNodeString(jointName)); + } + + for (const auto& point : message.trajectory.points) { + if (!point.positions.empty()) jointTrajectory.controlSetup_[ControlLevel::Position] = true; + if (!point.velocities.empty()) jointTrajectory.controlSetup_[ControlLevel::Velocity] = true; + if (!point.accelerations.empty()) jointTrajectory.controlSetup_[ControlLevel::Acceleration] = true; + if (!point.effort.empty()) jointTrajectory.controlSetup_[ControlLevel::Effort] = true; + } + + for (const auto& controlSetup : jointTrajectory.controlSetup_) { + if (!controlSetup.second) continue; + jointTrajectory.times_[controlSetup.first] = std::vector(); + jointTrajectory.values_[controlSetup.first] = std::vector>(); + jointTrajectory.trajectories_[controlSetup.first] = std::vector(); + } + + for (const auto& point : message.trajectory.points) { + if (!point.positions.empty()) jointTrajectory.times_[ControlLevel::Position].push_back(ros::Duration(point.time_from_start).toSec()); + if (!point.velocities.empty()) jointTrajectory.times_[ControlLevel::Velocity].push_back(ros::Duration(point.time_from_start).toSec()); + if (!point.accelerations.empty()) jointTrajectory.times_[ControlLevel::Acceleration].push_back(ros::Duration(point.time_from_start).toSec()); + if (!point.effort.empty()) jointTrajectory.times_[ControlLevel::Effort].push_back(ros::Duration(point.time_from_start).toSec()); + } + + size_t nJoints = message.trajectory.joint_names.size(); + for (const auto& controlSetup : jointTrajectory.controlSetup_) { + for (size_t j = 0; j < nJoints; ++j) { + if (!controlSetup.second) continue; + jointTrajectory.values_[controlSetup.first].push_back(std::vector()); + for (const auto& point : message.trajectory.points) { + if (controlSetup.first == ControlLevel::Position && !point.positions.empty()) { + jointTrajectory.values_[controlSetup.first][j].push_back(point.positions[j]); + } else if (controlSetup.first == ControlLevel::Velocity && !point.velocities.empty()) { + jointTrajectory.values_[controlSetup.first][j].push_back(point.velocities[j]); + } else if (controlSetup.first == ControlLevel::Acceleration && !point.accelerations.empty()) { + jointTrajectory.values_[controlSetup.first][j].push_back(point.accelerations[j]); + } else if (controlSetup.first == ControlLevel::Effort && !point.effort.empty()) { + jointTrajectory.values_[controlSetup.first][j].push_back(point.effort[j]); + } + } + } + } + + // Surface normal. + Vector surfaceNormal; + kindr_ros::convertFromRosGeometryMsg(message.surface_normal.vector, surfaceNormal); + jointTrajectory.surfaceNormal_.reset(new Vector(surfaceNormal)); + + // Ignore contact. + jointTrajectory.ignoreContact_ = message.ignore_contact; + + return true; +} + +bool StepRosConverter::fromMessage(const free_gait_msgs::BaseAuto& message, + BaseAuto& baseAuto) +{ + baseAuto.height_.reset(new double(message.height)); + baseAuto.ignoreTimingOfLegMotion_ = message.ignore_timing_of_leg_motion; + baseAuto.averageLinearVelocity_ = message.average_linear_velocity; + baseAuto.averageAngularVelocity_ = message.average_angular_velocity; + baseAuto.supportMargin_ = message.support_margin; + return true; +} + +bool StepRosConverter::fromMessage(const free_gait_msgs::BaseTarget& message, + BaseTarget& baseTarget) +{ + // Target. + baseTarget.frameId_ = message.target.header.frame_id; + Pose target; + kindr_ros::convertFromRosGeometryMsg(message.target.pose, target); + baseTarget.target_ = target; + + baseTarget.ignoreTimingOfLegMotion_ = message.ignore_timing_of_leg_motion; + baseTarget.averageLinearVelocity_ = message.average_linear_velocity; + baseTarget.averageAngularVelocity_ = message.average_angular_velocity; + return true; +} + +bool StepRosConverter::fromMessage(const free_gait_msgs::BaseTrajectory& message, + BaseTrajectory& baseTrajectory) +{ + // Trajectory. + baseTrajectory.frameIds_[ControlLevel::Position] = message.trajectory.header.frame_id; + + // We assume there is only one multi dof joint (for the base) in the message. +// size_t nJoints = message.trajectory.joint_names.size(); +// size_t j = 0; +// for (; j < nJoints; ++j) { +// if (message.trajectory.joint_names[j] == "base") break; +// if (j == nJoints - 1) return false; // Joint name not found. +// } + + // TODO. + baseTrajectory.controlSetup_[ControlLevel::Position] = false; + baseTrajectory.controlSetup_[ControlLevel::Velocity] = false; + baseTrajectory.controlSetup_[ControlLevel::Acceleration] = false; + baseTrajectory.controlSetup_[ControlLevel::Effort] = false; + + for (const auto& point : message.trajectory.points) { + if (!point.transforms.empty()) baseTrajectory.controlSetup_[ControlLevel::Position] = true; + if (!point.velocities.empty()) baseTrajectory.controlSetup_[ControlLevel::Velocity] = true; + if (!point.accelerations.empty()) baseTrajectory.controlSetup_[ControlLevel::Acceleration] = true; + } + + if (baseTrajectory.controlSetup_[ControlLevel::Position]) { + baseTrajectory.values_[ControlLevel::Position] = std::vector(); + } + for (const auto& controlSetup : baseTrajectory.controlSetup_) { + if (controlSetup.first == ControlLevel::Position || !controlSetup.second) continue; + baseTrajectory.times_[controlSetup.first] = std::vector(); + baseTrajectory.derivatives_[controlSetup.first] = std::vector(); + } + + for (const auto& point : message.trajectory.points) { + if (!point.transforms.empty()) baseTrajectory.times_[ControlLevel::Position].push_back(ros::Duration(point.time_from_start).toSec()); + if (!point.velocities.empty()) baseTrajectory.times_[ControlLevel::Velocity].push_back(ros::Duration(point.time_from_start).toSec()); + if (!point.accelerations.empty()) baseTrajectory.times_[ControlLevel::Acceleration].push_back(ros::Duration(point.time_from_start).toSec()); + } + + for (const auto& controlSetup : baseTrajectory.controlSetup_) { + if (!controlSetup.second)continue; + for (const auto& point : message.trajectory.points) { + if (controlSetup.first == ControlLevel::Position && !point.transforms.empty()) { + BaseTrajectory::ValueType pose; + kindr_ros::convertFromRosGeometryMsg(point.transforms[0], pose); + baseTrajectory.values_[controlSetup.first].push_back(pose); + } else if (controlSetup.first == ControlLevel::Velocity && !point.velocities.empty()) { +// baseTrajectory.derivatives_[controlSetup.first][j].push_back(point.velocities[j]); + } else if (controlSetup.first == ControlLevel::Acceleration && !point.accelerations.empty()) { +// baseTrajectory.derivatives_[controlSetup.first][j].push_back(point.accelerations[j]); + } /*else if (controlSetup.first == ControlLevel::Effort && !point.effort.empty()) { + baseTrajectory.derivatives_[controlSetup.first][j].push_back(point.effort[j]); + }*/ + } + } + + return true; +} + +bool StepRosConverter::fromMessage(const free_gait_msgs::CustomCommand& message, CustomCommand& customCommand) +{ + customCommand.type_ = message.type; + customCommand.command_ = message.command; + customCommand.duration_ = ros::Duration(message.duration).toSec(); + return true; +} + +bool StepRosConverter::toMessage(const StepQueue& stepQueue, free_gait_msgs::ExecuteStepsGoal::_steps_type& message) +{ + for (const Step& step : stepQueue.getQueue()) { + free_gait_msgs::Step stepMessage; + if (!toMessage(step, stepMessage)) return false; + message.push_back(stepMessage); + } + + return true; +} + +bool StepRosConverter::toMessage(const Step& step, free_gait_msgs::Step& message) +{ + free_gait_msgs::Step& stepMessage = message; + + // ID. + message.id = step.getId(); + + // Leg motions. + for (const auto& legMotion : step.getLegMotions()) { + + // Foostep. + if (legMotion.second->getType() == LegMotionBase::Type::Footstep) { + const Footstep& footstep = dynamic_cast(*(legMotion.second)); + free_gait_msgs::Footstep message; + if (!toMessage(footstep, message)) return false; + stepMessage.footstep.push_back(message); + } + + // EndEffectorTajectory + if (legMotion.second->getType() == LegMotionBase::Type::EndEffectorTrajectory) { + const EndEffectorTrajectory& endEffectorTrajectory = dynamic_cast(*(legMotion.second)); + free_gait_msgs::EndEffectorTrajectory message; + if (!toMessage(endEffectorTrajectory, message)) return false; + stepMessage.end_effector_trajectory.push_back(message); + } + + // JointTrajectory + if (legMotion.second->getType() == LegMotionBase::Type::JointTrajectory) { + const JointTrajectory& jointTrajectory = dynamic_cast(*(legMotion.second)); + free_gait_msgs::JointTrajectory message; + if (!toMessage(jointTrajectory, message)) return false; + stepMessage.joint_trajectory.push_back(message); + } + } + + // Base motion. + if (step.hasBaseMotion()) { + + const auto& baseMotion = step.getBaseMotion(); + + // Base Auto. + if (baseMotion.getType() == BaseMotionBase::Type::Auto) { + const BaseAuto& baseAuto = dynamic_cast(baseMotion); + free_gait_msgs::BaseAuto message; + if (!toMessage(baseAuto, message)) return false; + stepMessage.base_auto.push_back(message); + } + + // Base Trajectory. + if (baseMotion.getType() == BaseMotionBase::Type::Trajectory) { + const BaseTrajectory& baseTrajectory = dynamic_cast(baseMotion); + free_gait_msgs::BaseTrajectory message; + if (!toMessage(baseTrajectory, message)) return false; + stepMessage.base_trajectory.push_back(message); + } + } + + // Custom command. + for (const auto& customCommand : step.getCustomCommands()) { + free_gait_msgs::CustomCommand message; + if (!toMessage(customCommand, message)) return false; + stepMessage.custom_command.push_back(message); + } + + return true; +} + +bool StepRosConverter::toMessage(const Footstep& footstep, free_gait_msgs::Footstep& message) +{ + // Limb. + message.name = adapter_.getLimbStringFromLimbEnum(footstep.limb_); + + // Target. + message.target.header.frame_id = footstep.frameId_; + kindr_ros::convertToRosGeometryMsg(footstep.target_, message.target.point); + + // Profile. + message.profile_height = footstep.profileHeight_; + message.profile_type = footstep.profileType_; + + // Average Velocity. + message.average_velocity = footstep.averageVelocity_; + + // Surface normal. + if (footstep.surfaceNormal_) { + kindr_ros::convertToRosGeometryMsg(*(footstep.surfaceNormal_), message.surface_normal.vector); + } + + // Ignore contact. + message.ignore_contact = footstep.ignoreContact_; + + // Ignore for pose adaptation. + message.ignore_for_pose_adaptation = footstep.ignoreForPoseAdaptation_; + + return true; +} + +bool StepRosConverter::toMessage(const EndEffectorTrajectory& endEffectorTrajectory, free_gait_msgs::EndEffectorTrajectory& message) +{ + // Limb. + message.name = adapter_.getLimbStringFromLimbEnum(endEffectorTrajectory.limb_); + + // Trajectory. + message.trajectory.header.frame_id = endEffectorTrajectory.frameIds_.at(ControlLevel::Position); + message.trajectory.points.resize(endEffectorTrajectory.times_.size()); + size_t i = 0; + for (auto& point : message.trajectory.points) { + point.time_from_start = ros::Duration(endEffectorTrajectory.times_[i]); + + if (endEffectorTrajectory.controlSetup_.at(ControlLevel::Position)) { + geometry_msgs::Transform transform; + Position position(endEffectorTrajectory.values_.at(ControlLevel::Position)[i]); + kindr_ros::convertToRosGeometryMsg(position, transform.translation); + point.transforms.push_back(transform); + } + + ++i; + } + + // Surface normal. + if (endEffectorTrajectory.surfaceNormal_) { + kindr_ros::convertToRosGeometryMsg(*(endEffectorTrajectory.surfaceNormal_), message.surface_normal.vector); + } + + // Ignore contact. + message.ignore_contact = endEffectorTrajectory.ignoreContact_; + + // Ignore for pose adaptation. + message.ignore_for_pose_adaptation = endEffectorTrajectory.ignoreForPoseAdaptation_; + + return true; +} + +bool StepRosConverter::toMessage(const JointTrajectory& jointTrajectory, free_gait_msgs::JointTrajectory& message) +{ + // Limb. + message.name = adapter_.getLimbStringFromLimbEnum(jointTrajectory.limb_); + + // Surface normal. + if (jointTrajectory.surfaceNormal_) { + kindr_ros::convertToRosGeometryMsg(*(jointTrajectory.surfaceNormal_), message.surface_normal.vector); + } + + // Trajectory. + for (const auto& jointNode : jointTrajectory.jointNodeEnums_) { + message.trajectory.joint_names.push_back(adapter_.getJointNodeStringFromJointNodeEnum(jointNode)); + } + for (const auto& controlLevel : jointTrajectory.controlSetup_) { + if (controlLevel.second) { + size_t i = 0; + for (const auto& time : jointTrajectory.times_.at(controlLevel.first)) { + std::vector::iterator it = + std::find_if(message.trajectory.points.begin(), message.trajectory.points.end(), + [&](const trajectory_msgs::JointTrajectoryPoint& point) -> bool {return point.time_from_start.toSec() >= time;}); + if (it == message.trajectory.points.end() || it->time_from_start.toSec() != time) { + it = message.trajectory.points.insert(it, trajectory_msgs::JointTrajectoryPoint()); + it->time_from_start = ros::Duration(time); + } + if (controlLevel.first == ControlLevel::Position) + it->positions = jointTrajectory.values_.at(controlLevel.first)[i]; + else if (controlLevel.first == ControlLevel::Velocity) + it->velocities = jointTrajectory.values_.at(controlLevel.first)[i]; + else if (controlLevel.first == ControlLevel::Acceleration) + it->accelerations = jointTrajectory.values_.at(controlLevel.first)[i]; + else if (controlLevel.first == ControlLevel::Effort) + it->effort = jointTrajectory.values_.at(controlLevel.first)[i]; + i++; + } + } + } + + // Ignore contact. + message.ignore_contact = jointTrajectory.ignoreContact_; + + return true; +} + +bool StepRosConverter::toMessage(const BaseAuto& baseAuto, free_gait_msgs::BaseAuto& message) +{ + if (baseAuto.height_) message.height = *(baseAuto.height_); + message.ignore_timing_of_leg_motion = baseAuto.ignoreTimingOfLegMotion_; + message.average_linear_velocity = baseAuto.averageLinearVelocity_; + message.average_angular_velocity = baseAuto.averageAngularVelocity_; + message.support_margin = baseAuto.supportMargin_; + return true; +} + +bool StepRosConverter::toMessage(const BaseTrajectory& baseTrajectory, free_gait_msgs::BaseTrajectory& message) +{ + message.trajectory.header.frame_id = baseTrajectory.frameIds_.at(ControlLevel::Position); + message.trajectory.points.resize(baseTrajectory.values_.at(ControlLevel::Position).size()); + size_t i = 0; + for (auto& point : message.trajectory.points) { + if (baseTrajectory.controlSetup_.at(ControlLevel::Position)) { + point.time_from_start = ros::Duration(baseTrajectory.times_.at(ControlLevel::Position)[i]); + geometry_msgs::Transform transform; + BaseTrajectory::ValueType pose(baseTrajectory.values_.at(ControlLevel::Position)[i]); + kindr_ros::convertToRosGeometryMsg(pose, transform); + point.transforms.push_back(transform); + } + ++i; + } + + return true; +} + +bool StepRosConverter::toMessage(const CustomCommand& customCommand, free_gait_msgs::CustomCommand& message) +{ + message.type = customCommand.type_; + message.command = customCommand.command_; + message.duration = ros::Duration(customCommand.duration_); + return true; +} + +} /* namespace */ diff --git a/free_gait_ros/test/AdapterDummy.cpp b/free_gait_ros/test/AdapterDummy.cpp new file mode 100644 index 0000000..22146d9 --- /dev/null +++ b/free_gait_ros/test/AdapterDummy.cpp @@ -0,0 +1,404 @@ +/* + * AdapterDummy.cpp + * + * Created on: Mar 23, 2017 + * Author: Péter Fankhauser + * Institute: ETH Zurich + */ + +#include "AdapterDummy.hpp" + +namespace free_gait { + +AdapterDummy::AdapterDummy() + : AdapterBase(), + worldFrameId_("odom"), + baseFrameId_("base_link") +{ + state_.reset(new State()); + state_->initialize(getLimbs(), getBranches()); +// state_->LoadRobotDescriptionFromFile("/home/hitstar/catkin_ws/src/quadruped_locomotion-dev/quadruped_model/urdf/simpledog.urdf"); +// std::cout<LF_Chain.getNrOfJoints()<getPositionWorldToBaseInWorldFrame(); + RotationQuaternion I_R_B = state_->getOrientationBaseToWorld(); +// BaseMotionBase baseMotion = stepQueue.getCurrentStep().getBaseMotion(); +// BaseAuto baseAuto; +// BaseTarget baseTarget; +// BaseTrajectory baseTrajectory; +// switch (baseMotion.getType()) { +// case BaseMotionBase::Type::Auto: +// { +// baseAuto = (dynamic_cast(baseMotion)); +// break; +// } +// case BaseMotionBase::Type::Target: +// { +// baseTarget = (dynamic_cast(baseMotion)); +// break; +// } +// case BaseMotionBase::Type::Trajectory: +// { +// baseTrajectory = (dynamic_cast(baseMotion)); +// break; +// } +// default: +// { +// baseAuto = (dynamic_cast(baseMotion)); +// break; +// } +// } + if(stepQueue.empty()) return true; + double time = stepQueue.getCurrentStep().getTime(); + std::cout<<"+++++++++++++TIME++++++++++++++++"<isSupportLeg(limb)) + { + if(!(time>0.01)) + { + footholdsInSupport_[limb] = state_->getPositionWorldToFootInWorldFrame(limb); + } +// I_r_IF = state_->getPositionWorldToFootInWorldFrame(limb); + I_r_IF = footholdsInSupport_.at(limb); +// baseMotion.getDuration(); +// std::cout<<"=========================="<getLimbJointPositionsFromPositionBaseToFootInBaseFrame(B_r_BF, limb, joint_limb); + state_->setJointPositionsForLimb(limb, joint_limb); + } else { + + } + } +// stepQueue.getCurrentStep().getLegMotion(LimbEnum::LF_LEG + return true; +} + +const std::string& AdapterDummy::getWorldFrameId() const +{ + return worldFrameId_; +} + +const std::string& AdapterDummy::getBaseFrameId() const +{ + return baseFrameId_; +} + +const std::vector& AdapterDummy::getLimbs() const +{ + return limbs_; +} + +const std::vector& AdapterDummy::getBranches() const +{ + return branches_; +} + +LimbEnum AdapterDummy::getLimbEnumFromLimbString(const std::string& limb) const +{ + if(limb == "LF_LEG") + return static_cast(0); + if(limb == "RF_LEG") + return static_cast(1); + if(limb == "LH_LEG") + return static_cast(2); + if(limb == "RH_LEG") + return static_cast(3); + +// throw std::runtime_error("AdapterDummy::getLimbEnumFromLimbString() is not implemented."); +} + +std::string AdapterDummy::getLimbStringFromLimbEnum(const LimbEnum& limb) const +{ + if(limb == LimbEnum::LF_LEG) + return "LF_LEG"; + if(limb == LimbEnum::RF_LEG) + return "RF_LEG"; + if(limb == LimbEnum::LH_LEG) + return "LH_LEG"; + if(limb == LimbEnum::RH_LEG) + return "RH_LEG"; +// throw std::runtime_error("AdapterDummy::getLimbStringFromLimbEnum() is not implemented."); +} + +std::string AdapterDummy::getBaseString() const +{ + throw std::runtime_error("AdapterDummy::getBaseString() is not implemented."); +} + +JointNodeEnum AdapterDummy::getJointNodeEnumFromJointNodeString(const std::string& jointNode) const +{ + throw std::runtime_error("AdapterDummy::getJointNodeEnumFromJointNodeString() is not implemented."); +} + +std::string AdapterDummy::getJointNodeStringFromJointNodeEnum(const JointNodeEnum& jointNode) const +{ + throw std::runtime_error("AdapterDummy::getJointNodeStringFromJointNodeEnum() is not implemented."); +} + +bool AdapterDummy::getLimbJointPositionsFromPositionBaseToFootInBaseFrame( + const Position& positionBaseToFootInBaseFrame, const LimbEnum& limb, + JointPositionsLeg& jointPositions) const +{ + // TODO(Shunyao): solve the single limb IK +// QuadrupedKinematics::FowardKinematicsSolve() +// state_->FowardKinematicsSolve() + std::cout<<"start"<getLimbJointPositionsFromPositionBaseToFootInBaseFrame(positionBaseToFootInBaseFrame, + limb,jointPositions)) + { + return true; + } + throw std::runtime_error("AdapterDummy::getLimbJointPositionsFromPositionBaseToFootInBaseFrame() is not implemented."); +} + +Position AdapterDummy::getPositionBaseToFootInBaseFrame( + const LimbEnum& limb, const JointPositionsLeg& jointPositions) const +{ + // TODO(Shunyao): solve the single limb Kinematics +// state_->FowardKinematicsSolve() + return state_->getPositionBaseToFootInBaseFrame(limb,jointPositions); + throw std::runtime_error("AdapterDummy::getPositionBaseToFootInBaseFrame() is not implemented."); +} + +Position AdapterDummy::getPositionBaseToHipInBaseFrame(const LimbEnum& limb) const +{ + switch (limb) { + case LimbEnum::LF_LEG: + return Position(0.3, 0.2, 0.0); + case LimbEnum::RF_LEG: + return Position(0.3, -0.2, 0.0); + case LimbEnum::LH_LEG: + return Position(-0.3, 0.2, 0.0); + case LimbEnum::RH_LEG: + return Position(-0.3, -0.2, 0.0); + default: + throw std::runtime_error("AdapterDummy::getPositionBaseToHipInBaseFrame() something went wrong."); + } +} + +bool AdapterDummy::isExecutionOk() const +{ + // TODO(Shunyao): How to confirm isOK? from State? + return true; + throw std::runtime_error("AdapterDummy::isExecutionOk() is not implemented."); +} + +bool AdapterDummy::isLegGrounded(const LimbEnum& limb) const +{ +// return state_->isSupportLeg(limb); + return true; + throw std::runtime_error("AdapterDummy::isLegGrounded() is not implemented."); +} + +JointPositionsLeg AdapterDummy::getJointPositionsForLimb(const LimbEnum& limb) const +{ + return state_->getJointPositionsForLimb(limb); +} + +JointPositions AdapterDummy::getAllJointPositions() const +{ + return state_->getJointPositions(); +} + +JointVelocitiesLeg AdapterDummy::getJointVelocitiesForLimb(const LimbEnum& limb) const +{ + return state_->getJointVelocitiesForLimb(limb); +} + +JointVelocities AdapterDummy::getAllJointVelocities() const +{ + return state_->getJointVelocities(); +} + +JointAccelerationsLeg AdapterDummy::getJointAccelerationsForLimb(const LimbEnum& limb) const +{ + throw std::runtime_error("AdapterDummy::getJointAccelerationsForLimb() is not implemented."); +} + +JointAccelerations AdapterDummy::getAllJointAccelerations() const +{ + throw std::runtime_error("AdapterDummy::getAllJointAccelerations() is not implemented."); +} + +JointEffortsLeg AdapterDummy::getJointEffortsForLimb(const LimbEnum& limb) const +{ + return state_->getJointEffortsForLimb(limb); +} + +JointEfforts AdapterDummy::getAllJointEfforts() const +{ + return state_->getAllJointEfforts(); +} + +Position AdapterDummy::getPositionWorldToBaseInWorldFrame() const +{ +// return Position(0,0,0); + return state_->getPositionWorldToBaseInWorldFrame(); +} + +RotationQuaternion AdapterDummy::getOrientationBaseToWorld() const +{ +// return RotationQuaternion(1,0,0,0); + return state_->getOrientationBaseToWorld(); +} + +LinearVelocity AdapterDummy::getLinearVelocityBaseInWorldFrame() const +{ + return state_->getLinearVelocityBaseInWorldFrame(); +} + +LocalAngularVelocity AdapterDummy::getAngularVelocityBaseInBaseFrame() const +{ + return state_->getAngularVelocityBaseInBaseFrame(); +} + +LinearAcceleration AdapterDummy::getLinearAccelerationBaseInWorldFrame() const +{ + throw std::runtime_error("AdapterDummy::getLinearAccelerationBaseInWorldFrame() is not implemented."); +} + +AngularAcceleration AdapterDummy::getAngularAccelerationBaseInBaseFrame() const +{ + throw std::runtime_error("AdapterDummy::getAngularAccelerationBaseInBaseFrame() is not implemented."); +} + +Position AdapterDummy::getPositionBaseToFootInBaseFrame(const LimbEnum& limb) const +{ + return state_->getPositionBaseToFootInBaseFrame(limb); + throw std::runtime_error("AdapterDummy::getPositionBaseToFootInBaseFrame() is not implemented."); +} + +Position AdapterDummy::getPositionWorldToFootInWorldFrame(const LimbEnum& limb) const +{ + return state_->getPositionWorldToFootInWorldFrame(limb); + throw std::runtime_error("AdapterDummy::getPositionWorldToFootInWorldFrame() is not implemented."); +} + +Position AdapterDummy::getCenterOfMassInWorldFrame() const +{ +// return Position(0,0,0); + return state_->getPositionWorldToBaseInWorldFrame(); +} + +void AdapterDummy::getAvailableFrameTransforms(std::vector& frameTransforms) const +{ + frameTransforms.push_back("/odom"); +// throw std::runtime_error("AdapterDummy::getAvailableFrameTransforms() is not implemented."); +} + +Pose AdapterDummy::getFrameTransform(const std::string& frameId) const +{ + //TODO(Shunyao): It's seems to feedback Odom To Map, amcl? + return Pose(Position(0,0,0),RotationQuaternion());//fixed odom with map temparently + throw std::runtime_error("AdapterDummy::getFrameTransform() is not implemented."); +} + +ControlSetup AdapterDummy::getControlSetup(const BranchEnum& branch) const +{ +// TODO(Shunyao): How to set up Control Level, In State class to set controlSetup + return ControlSetup({{ControlLevel::Position, true}, + {ControlLevel::Velocity, false}, + {ControlLevel::Acceleration, false}, + {ControlLevel::Effort, false}}); + throw std::runtime_error("AdapterDummy::getControlSetup() is not implemented."); +} + +ControlSetup AdapterDummy::getControlSetup(const LimbEnum& limb) const +{ + return ControlSetup({{ControlLevel::Position, true}, + {ControlLevel::Velocity, false}, + {ControlLevel::Acceleration, false}, + {ControlLevel::Effort, false}}); + throw std::runtime_error("AdapterDummy::getControlSetup() is not implemented."); +} + +//! State depending on real robot. +JointVelocitiesLeg AdapterDummy::getJointVelocitiesFromEndEffectorLinearVelocityInWorldFrame( + const LimbEnum& limb, const LinearVelocity& endEffectorLinearVelocityInWorldFrame) const +{ + throw std::runtime_error("AdapterDummy::getJointVelocitiesFromEndEffectorLinearVelocityInWorldFrame() is not implemented."); +} + +LinearVelocity AdapterDummy::getEndEffectorLinearVelocityFromJointVelocities(const LimbEnum& limb, + const JointVelocitiesLeg& jointVelocities, + const std::string& frameId) const +{ + //TODO(Shunyao): dp = Jacobian * dq +// if(frameId != getBaseFrameId()) +// { +// transformLinearVelocity(frameId, getBaseFrameId(),) +// } + return state_->getEndEffectorLinearVelocityFromJointVelocities(limb, jointVelocities); + throw std::runtime_error("AdapterDummy::getEndEffectorLinearVelocityFromJointVelocities() is not implemented."); +} + +JointAccelerationsLeg AdapterDummy::getJointAccelerationsFromEndEffectorLinearAccelerationInWorldFrame( + const LimbEnum& limb, const LinearAcceleration& endEffectorLinearAccelerationInWorldFrame) const +{ + throw std::runtime_error("AdapterDummy::getJointAccelerationsFromEndEffectorLinearAccelerationInWorldFrame() is not implemented."); +} + +bool AdapterDummy::setInternalDataFromState(const State& state, bool updateContacts, bool updatePosition, + bool updateVelocity, bool updateAcceleration) const +{ + *state_ = state; + return true; +} + +void AdapterDummy::createCopyOfState() const +{ + +} + +void AdapterDummy::resetToCopyOfState() const +{ + +} + +const State& AdapterDummy::getState() const +{ + return *state_; +} + +} /* namespace */ + diff --git a/free_gait_ros/test/AdapterDummy.hpp b/free_gait_ros/test/AdapterDummy.hpp new file mode 100644 index 0000000..86639b2 --- /dev/null +++ b/free_gait_ros/test/AdapterDummy.hpp @@ -0,0 +1,104 @@ +/* + * AdapterDummy.hpp + * + * Created on: Mar 23, 2017 + * Author: Péter Fankhauser + * Institute: ETH Zurich + */ + +#pragma once + +#include "free_gait_core/executor/AdapterBase.hpp" +#include "free_gait_core/free_gait_core.hpp" +#include + +namespace free_gait { + +class AdapterDummy : public AdapterBase +{ + public: + AdapterDummy(); + virtual ~AdapterDummy(); + + //! Copying data from real robot to free gait state. + virtual bool resetExtrasWithRobot(const StepQueue& stepQueue, State& state); + virtual bool updateExtrasBefore(const StepQueue& stepQueue, State& state); + virtual bool updateExtrasAfter(const StepQueue& stepQueue, State& state); + + //! State independent functions. + virtual const std::string& getWorldFrameId() const; + virtual const std::string& getBaseFrameId() const; + virtual const std::vector& getLimbs() const; + virtual const std::vector& getBranches() const; + virtual LimbEnum getLimbEnumFromLimbString(const std::string& limb) const; + virtual std::string getLimbStringFromLimbEnum(const LimbEnum& limb) const; + virtual std::string getBaseString() const; + virtual JointNodeEnum getJointNodeEnumFromJointNodeString(const std::string& jointNode) const; + virtual std::string getJointNodeStringFromJointNodeEnum(const JointNodeEnum& jointNode) const; + virtual bool getLimbJointPositionsFromPositionBaseToFootInBaseFrame( + const Position& positionBaseToFootInBaseFrame, const LimbEnum& limb, + JointPositionsLeg& jointPositions) const; + virtual Position getPositionBaseToFootInBaseFrame(const LimbEnum& limb, + const JointPositionsLeg& jointPositions) const; + virtual Position getPositionBaseToHipInBaseFrame(const LimbEnum& limb) const; + + //! Reading state of real robot. + virtual bool isExecutionOk() const; + virtual bool isLegGrounded(const LimbEnum& limb) const; + virtual JointPositionsLeg getJointPositionsForLimb(const LimbEnum& limb) const; + virtual JointPositions getAllJointPositions() const; + virtual JointVelocitiesLeg getJointVelocitiesForLimb(const LimbEnum& limb) const; + virtual JointVelocities getAllJointVelocities() const; + virtual JointAccelerationsLeg getJointAccelerationsForLimb(const LimbEnum& limb) const; + virtual JointAccelerations getAllJointAccelerations() const; + virtual JointEffortsLeg getJointEffortsForLimb(const LimbEnum& limb) const; + virtual JointEfforts getAllJointEfforts() const; + virtual Position getPositionWorldToBaseInWorldFrame() const; + virtual RotationQuaternion getOrientationBaseToWorld() const; + virtual LinearVelocity getLinearVelocityBaseInWorldFrame() const; + virtual LocalAngularVelocity getAngularVelocityBaseInBaseFrame() const; + virtual LinearAcceleration getLinearAccelerationBaseInWorldFrame() const; + virtual AngularAcceleration getAngularAccelerationBaseInBaseFrame() const; + virtual Position getPositionBaseToFootInBaseFrame(const LimbEnum& limb) const; + virtual Position getPositionWorldToFootInWorldFrame(const LimbEnum& limb) const; + virtual Position getCenterOfMassInWorldFrame() const; + + /*! + * Transform is frameId to world => C_IF. + * @param frameId + * @return + */ + virtual void getAvailableFrameTransforms(std::vector& frameTransforms) const; + virtual Pose getFrameTransform(const std::string& frameId) const; + virtual ControlSetup getControlSetup(const BranchEnum& branch) const; + virtual ControlSetup getControlSetup(const LimbEnum& limb) const; + + //! Depending on state of real robot. + virtual JointVelocitiesLeg getJointVelocitiesFromEndEffectorLinearVelocityInWorldFrame( + const LimbEnum& limb, const LinearVelocity& endEffectorLinearVelocityInWorldFrame) const; + virtual LinearVelocity getEndEffectorLinearVelocityFromJointVelocities(const LimbEnum& limb, + const JointVelocitiesLeg& jointVelocities, + const std::string& frameId) const; + virtual JointAccelerationsLeg getJointAccelerationsFromEndEffectorLinearAccelerationInWorldFrame( + const LimbEnum& limb, const LinearAcceleration& endEffectorLinearAccelerationInWorldFrame) const; + + //! Hook to write data to internal robot representation from state. +// virtual bool setInternalDataFromState(const State& state) const; + virtual bool setInternalDataFromState(const State& state, bool updateContacts = true, bool updatePosition = true, + bool updateVelocity = true, bool updateAcceleration = false) const; + + virtual void createCopyOfState() const; + virtual void resetToCopyOfState() const; + + const State& getState() const; + + private: + std::unique_ptr state_; + std::vector limbs_; + std::vector branches_; + const std::string worldFrameId_; + const std::string baseFrameId_; + Stance footholdsInSupport_; +}; + +} /* namespace free_gait */ diff --git a/free_gait_ros/test/StepTest.cpp b/free_gait_ros/test/StepTest.cpp new file mode 100644 index 0000000..4c9e0f7 --- /dev/null +++ b/free_gait_ros/test/StepTest.cpp @@ -0,0 +1,124 @@ +/* + * FootstepTest.cpp + * + * Created on: Feb 11, 2017 + * Author: Péter Fankhauser + * Institute: ETH Zurich, Robotic Systems Lab + */ + +#include "free_gait_core/TypeDefs.hpp" +#include "free_gait_core/leg_motion/Footstep.hpp" +#include "free_gait_core/free_gait_core.hpp" +#include "AdapterDummy.hpp" +#include "free_gait_ros/StepRosConverter.hpp" +#include "free_gait_msgs/Footstep.h" +// gtest +#include + +using namespace free_gait; + +//TEST(footstep, triangleLowLongStep) +//{ +// Footstep footstep(LimbEnum::LF_LEG); +// Position start(0.0, 0.0, 0.0); +// Position target(0.3, 0.0, 0.0); +// footstep.updateStartPosition(start); +// footstep.setTargetPosition("map", target); +// double height = 0.05; +// footstep.setProfileHeight(0.05); +// footstep.setProfileType("triangle"); +// footstep.setAverageVelocity(0.15); +// ASSERT_TRUE(footstep.compute(true)); + +// for (double time = 0.0; time < footstep.getDuration(); time += 0.001) { +// Position position = footstep.evaluatePosition(time); +// EXPECT_LT(position.z(), height + 0.001); +// EXPECT_GT(position.x(), start.x() - 0.001); +// EXPECT_LT(position.x(), target.x() + 0.001); +// } +//} + +//TEST(footstep, trianglehighLong) +//{ +// Footstep footstep(LimbEnum::LF_LEG); +// Position start(0.0, 0.0, 0.0); +// Position target(0.3, 0.0, 0.3); +// footstep.updateStartPosition(start); +// footstep.setTargetPosition("map", target); +// double height = 0.05; +// footstep.setProfileHeight(0.05); +// footstep.setProfileType("triangle"); +// footstep.setAverageVelocity(0.15); +// ASSERT_TRUE(footstep.compute(true)); + +// for (double time = 0.0; time < footstep.getDuration(); time += 0.001) { +// Position position = footstep.evaluatePosition(time); +// EXPECT_LT(position.z(), target.z() + height + 0.01); +// EXPECT_GT(position.x(), start.x() - 0.001); +// EXPECT_LT(position.x(), target.x() + 0.001); +// } +//} + +TEST(executor, footstepExecute) +{ + AdapterDummy adapter; + State state; + StepParameters parameters; + StepCompleter completer(parameters, adapter); + StepComputer computer; + Executor executor(completer, computer, adapter, state); + StepRosConverter converter(adapter); + std::vector steps; + Step step; + step.setId("01"); + // Footstep + free_gait_msgs::Footstep footstep_msg; + // Position start(0.0, 0.0, 0.0); +// Position target(0.3, 0.0, 0.0); +// Vector sufaceNormal(0, 0, 1); + geometry_msgs::PointStamped target; + geometry_msgs::Vector3Stamped surface_normal; + target.point.x = 0.3; + target.point.y = 0.0; + target.point.z = 0.0; + surface_normal.vector.x = 0.0; + surface_normal.vector.y = 0.0; + surface_normal.vector.z = 1.0; +// footstep.updateStartPosition(start); + footstep_msg.name = "LF_LEG"; + footstep_msg.target = target; + footstep_msg.average_velocity = 0.15; + footstep_msg.profile_height = 0.05; + footstep_msg.profile_type = "triangle"; + footstep_msg.ignore_contact = false; + footstep_msg.ignore_for_pose_adaptation = false; + footstep_msg.surface_normal = surface_normal; + + Footstep footstep(LimbEnum::LF_LEG); + converter.fromMessage(footstep_msg, footstep); + step.addLegMotion(footstep); + // Basemotion + free_gait_msgs::BaseAuto baseauto_msg; + baseauto_msg.height = 0.4; + baseauto_msg.average_angular_velocity = 0.0; + baseauto_msg.average_linear_velocity = 0.0; + baseauto_msg.support_margin = 0.0; + baseauto_msg.ignore_timing_of_leg_motion = false; + BaseAuto baseauto; + converter.fromMessage(baseauto_msg, baseauto); + step.addBaseMotion(baseauto); + // in to step queue + steps.push_back(step); + + executor.getQueue().add(steps); + executor.setPreemptionType(Executor::PreemptionType::PREEMPT_IMMEDIATE); + double dt = 0.001; + ASSERT_TRUE(executor.advance(dt, false)); +// footstep.setTargetPosition("map", target); +// footstep.setProfileHeight(0.05); +// footstep.setProfileType("triangle"); +// footstep.setAverageVelocity(0.15); + + + +} diff --git a/free_gait_ros/test/test.cpp b/free_gait_ros/test/test.cpp new file mode 100644 index 0000000..158fe41 --- /dev/null +++ b/free_gait_ros/test/test.cpp @@ -0,0 +1,192 @@ +/* + * test.cpp + * Descriotion: + * + * Created on: Aug 31, 2017 + * Author: Shunyao Wang + * Institute: Harbin Institute of Technology, Shenzhen + */ + +#include "free_gait_core/TypeDefs.hpp" +#include "free_gait_core/leg_motion/Footstep.hpp" +#include "free_gait_core/free_gait_core.hpp" +#include "AdapterDummy.hpp" +//#include "free_gait_ros/StepRosConverter.hpp" +#include "free_gait_msgs/Footstep.h" +#include "free_gait_ros/free_gait_ros.hpp" + + +using namespace free_gait; +using namespace std; + +int main(int argc, char **argv) +{ + ros::init(argc, argv, "step_test"); + ros::NodeHandle nh("~"); + AdapterDummy adapter; + JointPositionsLeg joints(0,0,0); +// Position end(0.4,0.25,-0.5); +// State testState; +// testState.getPositionBaseToFootInBaseFrame(LimbEnum::LF_LEG,joints); + Position end = adapter.getPositionBaseToFootInBaseFrame(LimbEnum::LF_LEG,joints); + cout< steps; + Step step; + // Basemotion + step.setId("00"); + free_gait_msgs::BaseAuto baseauto_msg; + baseauto_msg.height = 0.4; + baseauto_msg.average_angular_velocity = 0.0; + baseauto_msg.average_linear_velocity = 0.0; + baseauto_msg.support_margin = 0.0; + baseauto_msg.ignore_timing_of_leg_motion = false; + BaseAuto baseauto; + converter.fromMessage(baseauto_msg, baseauto); + step.addBaseMotion(baseauto); + steps.push_back(step); + + Step step1; + step1.setId("01"); + free_gait_msgs::BaseTarget base_target_msg; + base_target_msg.target.header.frame_id = "odom"; + base_target_msg.target.pose.position.x = 0.1; + base_target_msg.target.pose.position.y = 0; + base_target_msg.target.pose.position.z = 0.4; + base_target_msg.target.pose.orientation.w = 1; + base_target_msg.target.pose.orientation.x = 0; + base_target_msg.target.pose.orientation.y = 0; + base_target_msg.target.pose.orientation.z = 0; + base_target_msg.average_angular_velocity = 0.1; + base_target_msg.average_linear_velocity = 0.1; + base_target_msg.ignore_timing_of_leg_motion = false; + BaseTarget baseTarget; + converter.fromMessage(base_target_msg, baseTarget); + step1.addBaseMotion(baseTarget); + steps.push_back(step1); + + Step step2; + step2.setId("02"); + // Footstep + free_gait_msgs::Footstep footstep_msg; + // Position start(0.0, 0.0, 0.0); +// Position target(0.3, 0.0, 0.0); +// Vector sufaceNormal(0, 0, 1); + geometry_msgs::PointStamped target; + geometry_msgs::Vector3Stamped surface_normal; + target.point.x = 0.5; + target.point.y = 0.25; + target.point.z = 0.0; + target.header.frame_id = "odom"; + surface_normal.vector.x = 0.0; + surface_normal.vector.y = 0.0; + surface_normal.vector.z = 1.0; +// footstep.updateStartPosition(start); + footstep_msg.name = "LF_LEG"; + footstep_msg.target = target; + footstep_msg.average_velocity = 0.15; + footstep_msg.profile_height = 0.15; + footstep_msg.profile_type = "triangle"; + footstep_msg.ignore_contact = false; + footstep_msg.ignore_for_pose_adaptation = false; + footstep_msg.surface_normal = surface_normal; + + Footstep footstep(LimbEnum::LF_LEG); + converter.fromMessage(footstep_msg, footstep); + step2.addLegMotion(footstep); + // Basemotion +// free_gait_msgs::BaseAuto baseauto_msg; + baseauto_msg.height = 0.4; + baseauto_msg.average_angular_velocity = 0.0; + baseauto_msg.average_linear_velocity = 0.0; + baseauto_msg.support_margin = 0.0; + baseauto_msg.ignore_timing_of_leg_motion = false; +// BaseAuto baseauto; + converter.fromMessage(baseauto_msg, baseauto); + step2.addBaseMotion(baseauto); + // in to step queue + steps.push_back(step2); + + Step step3; + step3.setId("03"); + // Footstep +// free_gait_msgs::Footstep footstep_msg; + // Position start(0.0, 0.0, 0.0); +// Position target(0.3, 0.0, 0.0); +// Vector sufaceNormal(0, 0, 1); +// geometry_msgs::PointStamped target; +// geometry_msgs::Vector3Stamped surface_normal; + target.point.x = -0.2; + target.point.y = -0.25; + target.point.z = 0.0; + target.header.frame_id = "odom"; + surface_normal.vector.x = 0.0; + surface_normal.vector.y = 0.0; + surface_normal.vector.z = 1.0; +// footstep.updateStartPosition(start); + footstep_msg.name = "RH_LEG"; + footstep_msg.target = target; + footstep_msg.average_velocity = 0.15; + footstep_msg.profile_height = 0.15; + footstep_msg.profile_type = "triangle"; + footstep_msg.ignore_contact = false; + footstep_msg.ignore_for_pose_adaptation = false; + footstep_msg.surface_normal = surface_normal; + + Footstep footstep2(LimbEnum::RH_LEG); + converter.fromMessage(footstep_msg, footstep2); + step3.addLegMotion(footstep2); + // Basemotion +// free_gait_msgs::BaseAuto baseauto_msg; + baseauto_msg.height = 0.4; + baseauto_msg.average_angular_velocity = 0.0; + baseauto_msg.average_linear_velocity = 0.0; + baseauto_msg.support_margin = 0.0; + baseauto_msg.ignore_timing_of_leg_motion = false; +// BaseAuto baseauto; + converter.fromMessage(baseauto_msg, baseauto); + step3.addBaseMotion(baseauto); + steps.push_back(step3); + + free_gait_msgs::BaseTrajectory base_trajectory_msg; + + BaseTrajectory baseTrajectory; + converter.fromMessage(base_trajectory_msg, baseTrajectory); + + + executor.getQueue().add(steps); + executor.setPreemptionType(Executor::PreemptionType::PREEMPT_IMMEDIATE); + double dt = 0.01; + double time = 0.0; + ros::Time t_start = ros::Time::now(); + ros::Rate rate(100); + while (!executor.getQueue().empty()) { + executor.advance(dt, true); + time = time + dt; + rosPublisher.publish(adapter.getState()); + rate.sleep(); + } + ros::Time t_end = ros::Time::now(); +cout<<"end time : "< + +// Run all the tests that were declared with TEST() +int main(int argc, char **argv) +{ + testing::InitGoogleTest(&argc, argv); + srand((int)time(0)); + return RUN_ALL_TESTS(); +} diff --git a/free_gait_rviz_plugin/CHANGELOG.rst b/free_gait_rviz_plugin/CHANGELOG.rst new file mode 100644 index 0000000..836c620 --- /dev/null +++ b/free_gait_rviz_plugin/CHANGELOG.rst @@ -0,0 +1,11 @@ +^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Changelog for package free_gait_rviz_plugin +^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ + +Forthcoming +----------- + +0.1.1 (2017-01-31) +------------------ +* Initial release. +* Contributors: Peter Fankhauser diff --git a/free_gait_rviz_plugin/CMakeLists.txt b/free_gait_rviz_plugin/CMakeLists.txt new file mode 100644 index 0000000..c98d0b3 --- /dev/null +++ b/free_gait_rviz_plugin/CMakeLists.txt @@ -0,0 +1,132 @@ +cmake_minimum_required(VERSION 2.8.3) +project(free_gait_rviz_plugin) + +set(CMAKE_CXX_FLAGS "-std=c++11 ${CMAKE_CXX_FLAGS}") + +find_package(catkin REQUIRED COMPONENTS + rviz + free_gait_core + free_gait_ros + free_gait_msgs +) + +catkin_package( + INCLUDE_DIRS + include + src + LIBRARIES + ${PROJECT_NAME} + CATKIN_DEPENDS + rviz + free_gait_core + free_gait_ros + free_gait_msgs + DEPENDS +) + +include_directories( + include + src + ${catkin_INCLUDE_DIRS} +) + +link_directories(${catkin_LIBRARY_DIRS}) + +## This setting causes Qt's "MOC" generation to happen automatically. +set(CMAKE_AUTOMOC ON) + +set(INCLUDE_FILES + include/free_gait_rviz_plugin/FreeGaitPreviewDisplay.hpp +) + +## This plugin includes Qt widgets, so we must include Qt. +## We'll use the version that rviz used so they are compatible. +if(rviz_QT_VERSION VERSION_LESS "5") + message(STATUS "Using Qt4 based on the rviz_QT_VERSION: ${rviz_QT_VERSION}") + find_package(Qt4 ${rviz_QT_VERSION} EXACT REQUIRED QtCore QtGui) + ## pull in all required include dirs, define QT_LIBRARIES, etc. + include(${QT_USE_FILE}) + qt4_wrap_cpp(MOC_FILES + ${INCLUDE_FILES} + ) +else() + message(STATUS "Using Qt5 based on the rviz_QT_VERSION: ${rviz_QT_VERSION}") + find_package(Qt5 ${rviz_QT_VERSION} EXACT REQUIRED Core Widgets) + ## make target_link_libraries(${QT_LIBRARIES}) pull in all required dependencies + set(QT_LIBRARIES Qt5::Widgets) + QT5_WRAP_CPP(MOC_FILES + ${INCLUDE_FILES} + ) +endif() + +## Avoid Qt signals and slots defining "emit", "slots", etc. +add_definitions(-DQT_NO_KEYWORDS) + +## The list of source files. +## The generated MOC files are included automatically as headers. +set(SOURCE_FILES + src/FreeGaitPreviewDisplay.cpp + src/FreeGaitPreviewPlayback.cpp + src/FreeGaitPreviewVisual.cpp +) + +## An rviz plugin is just a shared library, so here we declare the +## library to be called ${PROJECT_NAME} and specify the list of +## source files we collected above in ${SOURCE_FILES}. + +add_library(rviz_free_gait_additions + src/rviz/properties/button_property.cpp + src/rviz/properties/button_toggle_property.cpp + src/rviz/properties/float_slider_property.cpp + src/rviz/properties/line_edit_with_slider.cpp +) + +add_library(${PROJECT_NAME} + ${SOURCE_FILES} + ${MOC_FILES} +) + +## Link the executable with whatever Qt libraries have been defined by +## the find_package(Qt4 ...) line above, or by the +## set(QT_LIBRARIES Qt5::Widgets), and with whatever libraries +## catkin has included. + +target_link_libraries(rviz_free_gait_additions + ${Boost_LIBRARIES} + ${catkin_LIBRARIES} + ${QT_LIBRARIES} + ${OGRE_OV_LIBRARIES_ABS} + ${OPENGL_LIBRARIES} + ${rviz_ADDITIONAL_LIBRARIES} + ${X11_X11_LIB} + assimp +) + +target_link_libraries(${PROJECT_NAME} + ${QT_LIBRARIES} + ${catkin_LIBRARIES} + rviz_free_gait_additions +) + +## Install rules +install( + DIRECTORY include/${PROJECT_NAME} + DESTINATION ${CATKIN_PACKAGE_INCLUDE_DESTINATION} +) + +install( +TARGETS ${PROJECT_NAME} rviz_free_gait_additions + ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} + LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} + RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} +) + +install( + FILES plugin_description.xml + DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION} +) + +install( + DIRECTORY icons + DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}/icons +) diff --git a/free_gait_rviz_plugin/icons/classes/FreeGaitPreview.png b/free_gait_rviz_plugin/icons/classes/FreeGaitPreview.png new file mode 100755 index 0000000..c5c707c Binary files /dev/null and b/free_gait_rviz_plugin/icons/classes/FreeGaitPreview.png differ diff --git a/free_gait_rviz_plugin/include/free_gait_rviz_plugin/FreeGaitPreviewDisplay.hpp b/free_gait_rviz_plugin/include/free_gait_rviz_plugin/FreeGaitPreviewDisplay.hpp new file mode 100644 index 0000000..0677442 --- /dev/null +++ b/free_gait_rviz_plugin/include/free_gait_rviz_plugin/FreeGaitPreviewDisplay.hpp @@ -0,0 +1,137 @@ +/* + * FreeGaitPanel.h + * + * Created on: Nov 29, 2016 + * Author: Péter Fankhauser + * Institute: ETH Zurich, Robotic Systems Lab + */ + +#pragma once + +#ifndef Q_MOC_RUN +#include "free_gait_rviz_plugin/FreeGaitPreviewPlayback.hpp" +#include "free_gait_rviz_plugin/FreeGaitPreviewVisual.hpp" + +#include +#include + +#include +#include +#endif + +namespace Ogre { +class SceneNode; +} + +namespace rviz { +class BoolProperty; +class ColorProperty; +class FloatProperty; +class IntProperty; +class EnumProperty; +class EditableEnumProperty; +class ButtonToggleProperty; +class FloatSliderProperty; +class RosTopicProperty; +class DisplayGroup; +} + +namespace free_gait_rviz_plugin { + +class FreeGaitPreviewVisual; +class FreeGaitPreviewDisplay : public rviz::Display +{ +Q_OBJECT + public: + // Constructor + FreeGaitPreviewDisplay(); + // Destructor + virtual ~FreeGaitPreviewDisplay(); + + void setTopic(const QString &topic, const QString &datatype); + void update(float wall_dt, float ros_dt); + void reset(); + + protected: + void onInitialize(); + void onEnable(); + void onDisable(); + + private Q_SLOTS: + void updateTopic(); + void changeTfPrefix(); + void changePreviewRate(); + void changeAutoPlay(); + void changeAutoEnableVisuals(); + void changeRobotModel(); + void changeStartStateMethod(); + void changeAutoHideVisuals(); + void changePlaybackSpeed(); + void startAndStopPlayback(); + void jumpToTime(); + void newGoalAvailable(); + void previewStateChanged(const ros::Time& time); + void previewReachedEnd(); + void changeShowAllVisuals(); + void changeShowEndEffectorTargets(); + void changeShowSurfaceNormal(); + void changeShowEndEffectorTrajectories(); + void changeShowStances(); + + private: + void subscribe(); + void unsubscribe(); + void processMessage(const free_gait_msgs::ExecuteStepsActionGoal::ConstPtr& message); + void feedbackCallback(const free_gait_msgs::ExecuteStepsActionFeedback::ConstPtr& message); + void resultCallback(const free_gait_msgs::ExecuteStepsActionResult::ConstPtr& message); + void setEnabledRobotModel(bool enable); + bool findAssociatedRobotModelPlugin(); + + enum StartStateMethod { + ResetToRealState = 0, + ContinuePreviewedState = 1 + }; + + enum AutoHideMode { + ReachedPreviewEnd = 0, + ReceivedResult = 1 + }; + + free_gait::AdapterRos adapterRos_; + + FreeGaitPreviewPlayback playback_; + FreeGaitPreviewVisual* visual_; + free_gait::StepRosConverter stepRosConverter_; + free_gait_msgs::ExecuteStepsActionFeedback feedbackMessage_; + ros::Subscriber goalSubscriber_; + ros::Subscriber feedbackSubscriber_; + ros::Subscriber resultSubscriber_; + + // Property variables + rviz::Property* settingsTree_; + rviz::RosTopicProperty* goalTopicProperty_; + rviz::RosTopicProperty* robotStateTopicProperty_; + rviz::StringProperty* tfPrefixProperty_; + rviz::FloatProperty* previewRateRoperty_; + rviz::BoolProperty* autoPlayProperty_; + rviz::BoolProperty* autoEnableVisualsProperty_; + rviz::EditableEnumProperty* robotModelProperty_; + rviz::EnumProperty* startStateMethodProperty_; + rviz::EnumProperty* autoHideVisualsProperty_; + rviz::RosTopicProperty* resultTopicProperty_; + rviz::RosTopicProperty* feedbackTopicProperty_; + rviz::Property* playbackTree_; + rviz::FloatSliderProperty* playbackSpeedProperty_; + rviz::ButtonToggleProperty* playButtonProperty_; + rviz::FloatSliderProperty* timelimeSliderProperty_; + rviz::BoolProperty* visualsTree_; + rviz::BoolProperty* showEndEffectorTargetsProperty_; + rviz::ColorProperty* endEffectorTargetsColorProperty_; + rviz::BoolProperty* showSurfaceNormalsProperty_; + rviz::BoolProperty* showEndEffectorTrajectoriesProperty_; + rviz::BoolProperty* showStancesProperty_; + + rviz::Display* robotModelRvizPlugin_; +}; + +} // end namespace grid_map_rviz_plugin diff --git a/free_gait_rviz_plugin/include/free_gait_rviz_plugin/FreeGaitPreviewPlayback.hpp b/free_gait_rviz_plugin/include/free_gait_rviz_plugin/FreeGaitPreviewPlayback.hpp new file mode 100644 index 0000000..b9eee37 --- /dev/null +++ b/free_gait_rviz_plugin/include/free_gait_rviz_plugin/FreeGaitPreviewPlayback.hpp @@ -0,0 +1,86 @@ +/* + * FreeGaitPreviewPlayback.hpp + * + * Created on: Dec 19, 2016 + * Author: Péter Fankhauser + * Institute: ETH Zurich, Autonomous Systems Lab + */ + +#pragma once + +// Free Gait +#include +#include + +// ROS +#include + +// STD +#include +#include +#include +#include + +namespace free_gait_rviz_plugin { + +class FreeGaitPreviewPlayback +{ + public: + enum class PlayMode { + STOPPED, + FORWARD, + BACKWARD, + ONHOLD + }; + + typedef std::lock_guard Lock; + + FreeGaitPreviewPlayback(ros::NodeHandle& nodeHandle, + free_gait::AdapterBase& adapter); + virtual ~FreeGaitPreviewPlayback(); + + void addNewGoalCallback(std::function callback); + void addStateChangedCallback(std::function callback); + void addReachedEndCallback(std::function callback); + + bool process(const std::vector& steps); + bool isProcessing(); + void cancelProcessing(); + + void run(); + void stop(); + void goToTime(const ros::Time& time); + void clear(); + + const free_gait::StateBatch& getStateBatch() const; + const ros::Time& getTime() const; + void update(double timeStep); + void setSpeedFactor(const double speedFactor); + void setRate(const double rate); + double getRate() const; + void setTfPrefix(const std::string tfPrefix); + + private: + void processingCallback(bool success); + void publish(const ros::Time& time); + + ros::NodeHandle& nodeHandle_; + std::function newGoalCallback_; + std::function stateChangedCallback_; + std::function reachedEndCallback_; + std::unique_ptr batchExecutor_; + std::unique_ptr executorState_; + std::unique_ptr parameters_; + std::unique_ptr completer_; + std::unique_ptr computer_; + std::unique_ptr executor_; + free_gait::StateBatch stateBatch_; + free_gait::StateBatchComputer stateBatchComputer_; + std::recursive_mutex dataMutex_; + free_gait::StateRosPublisher stateRosPublisher_; + PlayMode playMode_; + ros::Time time_; + double speedFactor_; +}; + +} /* namespace free_gait_rviz_plugin */ diff --git a/free_gait_rviz_plugin/include/free_gait_rviz_plugin/FreeGaitPreviewVisual.hpp b/free_gait_rviz_plugin/include/free_gait_rviz_plugin/FreeGaitPreviewVisual.hpp new file mode 100644 index 0000000..49b5abb --- /dev/null +++ b/free_gait_rviz_plugin/include/free_gait_rviz_plugin/FreeGaitPreviewVisual.hpp @@ -0,0 +1,78 @@ +/* + * FreeGaitPreviewVisual.hpp + * + * Created on: Dec 22, 2016 + * Author: Péter Fankhauser + * Institute: ETH Zurich, Autonomous Systems Lab + */ + +#pragma once + +#include + +#include +#include +#include +#include +#include +#include +#include + +#include +#include +#include + +namespace free_gait_rviz_plugin { + +class FreeGaitPreviewVisual +{ + public: + enum class Modul { + EndEffectorTargets, + SurfaceNormals, + EndEffectorTrajectories, + Stances + }; + + FreeGaitPreviewVisual(rviz::DisplayContext* context, Ogre::SceneNode* parentNode); + virtual ~FreeGaitPreviewVisual(); + + void clear(); + void setStateBatch(const free_gait::StateBatch& stateBatch); + void setEnabledModul(Modul modul, bool enable); + void showEnabled(); + void hideEnabled(); + void update(); + + protected: + void showEndEffectorTargets(const float diameter = 0.04, + const Ogre::ColourValue& color = Ogre::ColourValue(1, 0, 0, 1)); + void hideEndEffectorTargets(); + + void showSurfaceNormals(const float diameter = 0.003, const float length = 0.07, + const Ogre::ColourValue& color = Ogre::ColourValue(1, 0, 0, 1)); + void hideSurfaceNormals(); + + void showEndEffectorTrajectories(const float width = 0.01, + const Ogre::ColourValue& color = Ogre::ColourValue(1, 0, 0, 1)); + void hideEndEffectorTrajectories(); + + void showStances(const float alpha = 0.3); + void hideStances(); + + static void getRainbowColor(const double min, const double max, const double value, std_msgs::ColorRGBA& color); + + private: + rviz::DisplayContext* context_; + Ogre::SceneNode* frameNode_; + const free_gait::StateBatch* stateBatchPtr_; + std::list modulesToEnable_; + std::list modulesToDisable_; + std::list enabledModules_; + std::vector>> endEffectorTargets_; + std::vector>> surfaceNormals_; + std::vector> endEffectorTrajectories_; + std::vector> stancesMarker_; +}; + +} /* namespace free_gait_rviz_plugin */ diff --git a/free_gait_rviz_plugin/package.xml b/free_gait_rviz_plugin/package.xml new file mode 100644 index 0000000..9afc3b5 --- /dev/null +++ b/free_gait_rviz_plugin/package.xml @@ -0,0 +1,27 @@ + + + free_gait_rviz_plugin + 0.3.0 + RViz plugin to preview Free Gait actions. + Peter Fankhauser + Peter Fankhauser + BSD + https://github.com/leggedrobotics/free_gait + https://github.com/leggedrobotics/free_gait/issues + + catkin + + qtbase5-dev + rviz + free_gait_core + free_gait_ros + free_gait_msgs + + libqt5-core + libqt5-gui + libqt5-widgets + + + + + diff --git a/free_gait_rviz_plugin/plugin_description.xml b/free_gait_rviz_plugin/plugin_description.xml new file mode 100644 index 0000000..2d5e898 --- /dev/null +++ b/free_gait_rviz_plugin/plugin_description.xml @@ -0,0 +1,8 @@ + + + Display to preview Free Gait actions. + free_gait_msgs/ExecuteStepsActionGoal + + diff --git a/free_gait_rviz_plugin/src/FreeGaitPreviewDisplay.cpp b/free_gait_rviz_plugin/src/FreeGaitPreviewDisplay.cpp new file mode 100644 index 0000000..00bd0f2 --- /dev/null +++ b/free_gait_rviz_plugin/src/FreeGaitPreviewDisplay.cpp @@ -0,0 +1,493 @@ +/* + * FreeGaitPreviewDisplay.cpp + * + * Created on: Nov 29, 2016 + * Author: Péter Fankhauser + * Institute: ETH Zurich, Robotic Systems Lab + */ + +#include "free_gait_rviz_plugin/FreeGaitPreviewDisplay.hpp" + +// OGRE +#include +#include + +// ROS +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + +// STD +#include + +namespace free_gait_rviz_plugin { + +FreeGaitPreviewDisplay::FreeGaitPreviewDisplay() + : adapterRos_(update_nh_, free_gait::AdapterRos::AdapterType::Preview), + playback_(update_nh_, *adapterRos_.getAdapterPtr()), + stepRosConverter_(adapterRos_.getAdapter()), + visual_(NULL), + robotModelRvizPlugin_(NULL) +{ + playback_.addNewGoalCallback(std::bind(&FreeGaitPreviewDisplay::newGoalAvailable, this)); + playback_.addStateChangedCallback( + std::bind(&FreeGaitPreviewDisplay::previewStateChanged, this, std::placeholders::_1)); + playback_.addReachedEndCallback(std::bind(&FreeGaitPreviewDisplay::previewReachedEnd, this)); + + settingsTree_ = new Property("Settings", QVariant(), "", this); + + goalTopicProperty_ = new rviz::RosTopicProperty("Goal Topic", "", "", "", settingsTree_, + SLOT(updateTopic()), this); + QString goalMessageType = QString::fromStdString( + ros::message_traits::datatype()); + goalTopicProperty_->setMessageType(goalMessageType); + goalTopicProperty_->setDescription(goalMessageType + " topic to subscribe to."); + + robotStateTopicProperty_ = new rviz::RosTopicProperty("Robot State Topic", "", "", "", + settingsTree_, SLOT(updateTopic()), this); + QString robotStateMessageType = QString::fromStdString(adapterRos_.getRobotStateMessageType()); + robotStateTopicProperty_->setMessageType(robotStateMessageType); + robotStateTopicProperty_->setDescription(robotStateMessageType + " topic to subscribe to."); + + startStateMethodProperty_ = new rviz::EnumProperty( + "Start State", "Reset to Real Robot State", "Determine the robot start state for motion execution.", + settingsTree_, SLOT(changeStartStateMethod()), this); + startStateMethodProperty_->addOption("Reset to Real State", 0); + startStateMethodProperty_->addOption("Continue Previewed State", 1); + + feedbackTopicProperty_ = new rviz::RosTopicProperty("Feedback Topic", "", "", "", settingsTree_, + SLOT(updateTopic()), this); + QString feedbackMessageType = QString::fromStdString( + ros::message_traits::datatype()); + feedbackTopicProperty_->setMessageType(feedbackMessageType); + feedbackTopicProperty_->setDescription(feedbackMessageType + " topic to subscribe to."); + feedbackTopicProperty_->hide(); + + tfPrefixProperty_ = new rviz::StringProperty( + "TF Prefix", "", + "TF prefix used for the preview. Set the same value in the robot model plugin.", + settingsTree_, SLOT(changeTfPrefix()), this); + + previewRateRoperty_ = new rviz::FloatProperty( + "Preview Rate", playback_.getRate(), "Rate in Hz at which to simulate the motion preview.", + settingsTree_, SLOT(changePreviewRate()), this); + previewRateRoperty_->setMin(0.0); + + autoPlayProperty_ = new rviz::BoolProperty("Auto-Play", true, "Play motion once received.", + settingsTree_, SLOT(changeAutoPlay()), this); + + autoEnableVisualsProperty_ = new rviz::BoolProperty( + "Auto En/Disable Visuals", false, + "Automatically enable and disable visuals for the duration of the preview.", settingsTree_, + SLOT(changeAutoEnableVisuals()), this); + + robotModelProperty_ = new rviz::EditableEnumProperty( + "Robot Model", "", "Select the robot model used for preview. Robot model must be in the same RViz group.", settingsTree_, + SLOT(changeRobotModel()), this); + robotModelProperty_->hide(); + + autoHideVisualsProperty_ = new rviz::EnumProperty( + "Auto Hide When", "Reached End of Preview", "Select the event when to hide the visuals.", + settingsTree_, SLOT(changeAutoHideVisuals()), this); + autoHideVisualsProperty_->addOption("Reached End of Preview", 0); + autoHideVisualsProperty_->addOption("Received Action Result", 1); + autoHideVisualsProperty_->hide(); + + resultTopicProperty_ = new rviz::RosTopicProperty("Result Topic", "", "", "", settingsTree_, + SLOT(updateTopic()), this); + QString resultMessageType = QString::fromStdString( + ros::message_traits::datatype()); + resultTopicProperty_->setMessageType(resultMessageType); + resultTopicProperty_->setDescription(resultMessageType + " topic to subscribe to."); + resultTopicProperty_->hide(); + + playbackTree_ = new Property("Playback", QVariant(), "", this); + + playbackSpeedProperty_ = new rviz::FloatSliderProperty("Playback Speed", 1.0, + "Playback speed factor.", playbackTree_, + SLOT(changePlaybackSpeed()), this); + playbackSpeedProperty_->setMin(0.0); + playbackSpeedProperty_->setMax(10.0); + + playButtonProperty_ = new rviz::ButtonToggleProperty("Play", false, "Play back the motion.", + playbackTree_, SLOT(startAndStopPlayback()), + this); + playButtonProperty_->setLabels("Play", "Pause"); + playButtonProperty_->setReadOnly(true); + + timelimeSliderProperty_ = new rviz::FloatSliderProperty( + "Time", 0.0, "Determine the current time to visualize the motion.", playbackTree_, + SLOT(jumpToTime()), this); + timelimeSliderProperty_->setReadOnly(true); + + visualsTree_ = new rviz::BoolProperty("Visuals", true, "Show/hide all enabled visuals.", this, + SLOT(changeShowAllVisuals())); + visualsTree_->setDisableChildrenIfFalse(true); + + showEndEffectorTargetsProperty_ = new rviz::BoolProperty( + "End Effector Targets", true, "Show target position of end effector motions.", + visualsTree_, SLOT(changeShowEndEffectorTargets()), this); + showEndEffectorTargetsProperty_->setDisableChildrenIfFalse(true); + + endEffectorTargetsColorProperty_ = new rviz::ColorProperty( + "Color", true, "Set the color of the end effector targets.", + showEndEffectorTargetsProperty_, SLOT(changeShowAllVisuals()), this); + + showSurfaceNormalsProperty_ = new rviz::BoolProperty( + "Surface Normals", true, "Show surface normals for end effector motions.", + visualsTree_, SLOT(changeShowSurfaceNormal()), this); + showSurfaceNormalsProperty_->setDisableChildrenIfFalse(true); + + showEndEffectorTrajectoriesProperty_ = new rviz::BoolProperty( + "End Effector Trajectories", true, "Draw a trace for the end effector trajectory.", + visualsTree_, SLOT(changeShowEndEffectorTrajectories()), this); + showEndEffectorTrajectoriesProperty_->setDisableChildrenIfFalse(true); + + showStancesProperty_ = new rviz::BoolProperty( + "Stances", true, "Draw stances as support areas.", + visualsTree_, SLOT(changeShowStances()), this); + showStancesProperty_->setDisableChildrenIfFalse(true); +} + +FreeGaitPreviewDisplay::~FreeGaitPreviewDisplay() +{ + unsubscribe(); +} + +void FreeGaitPreviewDisplay::setTopic(const QString &topic, const QString &datatype) +{ + goalTopicProperty_->setString(topic); +} + +void FreeGaitPreviewDisplay::update(float wall_dt, float ros_dt) +{ + visual_->update(); + playback_.update(wall_dt); +} + +void FreeGaitPreviewDisplay::onInitialize() +{ + visual_ = new FreeGaitPreviewVisual(context_, scene_node_); +} + +void FreeGaitPreviewDisplay::onEnable() +{ + subscribe(); +} + +void FreeGaitPreviewDisplay::onDisable() +{ + unsubscribe(); +//reset(); +} + +void FreeGaitPreviewDisplay::reset() +{ +// MFDClass::reset(); +// visuals_.clear(); +// Display::reset(); +// tf_filter_->clear(); +// messages_received_ = 0; +} + +void FreeGaitPreviewDisplay::updateTopic() +{ + unsubscribe(); + reset(); + subscribe(); +} + +void FreeGaitPreviewDisplay::changeTfPrefix() +{ + ROS_DEBUG_STREAM("Setting tf prefix to " << tfPrefixProperty_->getStdString() << "."); + playback_.setTfPrefix(tfPrefixProperty_->getStdString()); +} + +void FreeGaitPreviewDisplay::changePreviewRate() +{ + ROS_DEBUG_STREAM("Setting preview rate to " << previewRateRoperty_->getFloat() << "."); + playback_.setRate(previewRateRoperty_->getFloat()); +} + +void FreeGaitPreviewDisplay::changeAutoPlay() +{ + ROS_DEBUG_STREAM("Setting auto-play to " << (autoPlayProperty_->getBool() ? "True" : "False") << "."); +} + +void FreeGaitPreviewDisplay::changeAutoEnableVisuals() +{ + ROS_DEBUG_STREAM("Setting auto en/disable visuals to " << (autoEnableVisualsProperty_->getBool() ? "True" : "False") << "."); + if (autoEnableVisualsProperty_->getBool()) { + robotModelProperty_->show(); + autoHideVisualsProperty_->show(); + if (autoHideVisualsProperty_->getOptionInt() == AutoHideMode::ReceivedResult) resultTopicProperty_->show(); + robotModelProperty_->clearOptions(); + for (size_t i = 0; i < getParent()->numChildren(); ++i) { + rviz::Display* display = dynamic_cast(getParent()->childAt(i)); + if (display == NULL) continue; + if (display->getClassId() != "rviz/RobotModel") continue; + robotModelProperty_->addOption(display->getName()); + } + } else { + robotModelProperty_->hide(); + autoHideVisualsProperty_->hide(); + resultTopicProperty_->hide(); + } +} + +void FreeGaitPreviewDisplay::changeRobotModel() +{ + ROS_DEBUG_STREAM("Setting robot model name to " << robotModelProperty_->getStdString() << "."); + findAssociatedRobotModelPlugin(); +} + +void FreeGaitPreviewDisplay::changeStartStateMethod() +{ + ROS_DEBUG_STREAM("Changed start state method to " << startStateMethodProperty_->getStdString() << "."); + switch (startStateMethodProperty_->getOptionInt()) { + case StartStateMethod::ResetToRealState: + feedbackTopicProperty_->hide(); + break; + case StartStateMethod::ContinuePreviewedState: + feedbackTopicProperty_->show(); + break; + default: + break; + } +} + +void FreeGaitPreviewDisplay::changeAutoHideVisuals() +{ + ROS_DEBUG_STREAM("Changed auto-hide visuals event to " << autoHideVisualsProperty_->getStdString() << "."); + switch (autoHideVisualsProperty_->getOptionInt()) { + case AutoHideMode::ReachedPreviewEnd: + resultTopicProperty_->hide(); + break; + case AutoHideMode::ReceivedResult: + resultTopicProperty_->show(); + break; + default: + break; + } +} + +void FreeGaitPreviewDisplay::changePlaybackSpeed() +{ + ROS_DEBUG_STREAM("Setting playback speed to " << playbackSpeedProperty_->getFloat() << "."); + playback_.setSpeedFactor(playbackSpeedProperty_->getFloat()); +} + +void FreeGaitPreviewDisplay::startAndStopPlayback() +{ + if (playButtonProperty_->getBool()) { + ROS_DEBUG("Pressed start."); + playback_.run(); + } else { + ROS_DEBUG("Pressed stop."); + playback_.stop(); + } +} + +void FreeGaitPreviewDisplay::jumpToTime() +{ + playback_.goToTime(ros::Time(timelimeSliderProperty_->getFloat())); +} + +void FreeGaitPreviewDisplay::newGoalAvailable() +{ + ROS_DEBUG("FreeGaitPreviewDisplay::newGoalAvailable: New preview available."); + + // Auto-enable visuals. + ROS_DEBUG("FreeGaitPreviewDisplay::newGoalAvailable: Drawing visualizations."); + visual_->setStateBatch(playback_.getStateBatch()); + if (autoEnableVisualsProperty_->getBool()) { + setEnabledRobotModel(true); + visualsTree_->setBool(true); + } else { + visual_->showEnabled(); + } + + // Playback. + ROS_DEBUG("FreeGaitPreviewDisplay::newGoalAvailable: Setting up control."); + playButtonProperty_->setReadOnly(false); + const double midTime = (playback_.getStateBatch().getEndTime() - playback_.getStateBatch().getStartTime()) / 2.0; + timelimeSliderProperty_->setValuePassive(midTime); // This is required for not triggering value change signal. + timelimeSliderProperty_->setMin(playback_.getStateBatch().getStartTime()); + timelimeSliderProperty_->setMax(playback_.getStateBatch().getEndTime()); + timelimeSliderProperty_->setValuePassive(playback_.getTime().toSec()); + timelimeSliderProperty_->setReadOnly(false); + ROS_DEBUG_STREAM("Setting slider min and max time to: " << timelimeSliderProperty_->getMin() + << " & " << timelimeSliderProperty_->getMax() << "."); + + // Play. + if (autoPlayProperty_->getBool()) { + ROS_DEBUG("FreeGaitPreviewDisplay::newGoalAvailable: Starting playback."); + playback_.run(); + playButtonProperty_->setLabel("Pause"); + } +} + +void FreeGaitPreviewDisplay::previewStateChanged(const ros::Time& time) +{ + timelimeSliderProperty_->setValuePassive(time.toSec()); +} + +void FreeGaitPreviewDisplay::previewReachedEnd() +{ + ROS_DEBUG("FreeGaitPreviewDisplay::previewReachedEnd: Reached end of preview."); + if (autoEnableVisualsProperty_->getBool() && (autoHideVisualsProperty_->getOptionInt() == AutoHideMode::ReachedPreviewEnd)) { + setEnabledRobotModel(false); + visualsTree_->setBool(false); + } +} + +void FreeGaitPreviewDisplay::changeShowAllVisuals() +{ + ROS_DEBUG_STREAM("Setting show all visuals to " << (visualsTree_->getBool() ? "True" : "False") << "."); + if (visualsTree_->getBool()) { + visual_->showEnabled(); + } else { + visual_->hideEnabled(); + } +} + +void FreeGaitPreviewDisplay::changeShowEndEffectorTargets() +{ + ROS_DEBUG_STREAM("Setting show end effector targets to " << (showEndEffectorTargetsProperty_->getBool() ? "True" : "False") << "."); + visual_->setEnabledModul(FreeGaitPreviewVisual::Modul::EndEffectorTargets, showEndEffectorTargetsProperty_->getBool()); +} + +void FreeGaitPreviewDisplay::changeShowSurfaceNormal() +{ + ROS_DEBUG_STREAM("Setting surface normal to " << (showSurfaceNormalsProperty_->getBool() ? "True" : "False") << "."); + visual_->setEnabledModul(FreeGaitPreviewVisual::Modul::SurfaceNormals, showSurfaceNormalsProperty_->getBool()); +} + +void FreeGaitPreviewDisplay::changeShowEndEffectorTrajectories() +{ + ROS_DEBUG_STREAM("Setting show end effector trajectories to " << (showEndEffectorTrajectoriesProperty_->getBool() ? "True" : "False") << "."); + visual_->setEnabledModul(FreeGaitPreviewVisual::Modul::EndEffectorTrajectories, showEndEffectorTrajectoriesProperty_->getBool()); +} + +void FreeGaitPreviewDisplay::changeShowStances() +{ + ROS_DEBUG_STREAM("Setting show stances to " << (showStancesProperty_->getBool() ? "True" : "False") << "."); + visual_->setEnabledModul(FreeGaitPreviewVisual::Modul::Stances, showStancesProperty_->getBool()); +} + +void FreeGaitPreviewDisplay::subscribe() +{ + if (!isEnabled()) { + return; + } + + if (!goalTopicProperty_->getTopicStd().empty()) { + try { + goalSubscriber_ = update_nh_.subscribe(goalTopicProperty_->getTopicStd(), 1, &FreeGaitPreviewDisplay::processMessage, this); + setStatus(rviz::StatusProperty::Ok, "Goal Topic", "OK"); + } catch (ros::Exception& e) { + setStatus(rviz::StatusProperty::Error, "Goal Topic", QString("Error subscribing: ") + e.what()); + } + } + + if (startStateMethodProperty_->getOptionInt() == StartStateMethod::ContinuePreviewedState && + !feedbackTopicProperty_->getTopicStd().empty()) { + try { + feedbackSubscriber_ = update_nh_.subscribe(feedbackTopicProperty_->getTopicStd(), 1, &FreeGaitPreviewDisplay::feedbackCallback, this); + setStatus(rviz::StatusProperty::Ok, "Feedback Topic", "OK"); + } catch (ros::Exception& e) { + setStatus(rviz::StatusProperty::Error, "Feedback Topic", QString("Error subscribing: ") + e.what()); + } + } + + if (autoHideVisualsProperty_->getOptionInt() == AutoHideMode::ReceivedResult && + !resultTopicProperty_->getTopicStd().empty()) { + try { + resultSubscriber_ = update_nh_.subscribe(resultTopicProperty_->getTopicStd(), 1, &FreeGaitPreviewDisplay::resultCallback, this); + setStatus(rviz::StatusProperty::Ok, "Result Topic", "OK"); + } catch (ros::Exception& e) { + setStatus(rviz::StatusProperty::Error, "Result Topic", QString("Error subscribing: ") + e.what()); + } + } + + try { + adapterRos_.subscribeToRobotState(robotStateTopicProperty_->getStdString()); + setStatus(rviz::StatusProperty::Ok, "Robot State Topic", "OK"); + } catch (ros::Exception& e) { + setStatus(rviz::StatusProperty::Error, "Robot State Topic", QString("Error subscribing: ") + e.what()); + } +} + +void FreeGaitPreviewDisplay::unsubscribe() +{ + goalSubscriber_.shutdown(); + feedbackSubscriber_.shutdown(); + resultSubscriber_.shutdown(); + adapterRos_.unsubscribeFromRobotState(); +} + +void FreeGaitPreviewDisplay::processMessage(const free_gait_msgs::ExecuteStepsActionGoal::ConstPtr& message) +{ + ROS_DEBUG("FreeGaitPreviewDisplay::processMessage: Starting to process new goal."); + std::vector steps; + stepRosConverter_.fromMessage(message->goal.steps, steps); + + if (startStateMethodProperty_->getOptionInt() == StartStateMethod::ResetToRealState) { + adapterRos_.updateAdapterWithState(); + } else if (startStateMethodProperty_->getOptionInt() == StartStateMethod::ContinuePreviewedState) { + double time; + bool success = playback_.getStateBatch().getEndTimeOfStep(feedbackMessage_.feedback.step_id, time); + if (success) { + adapterRos_.getAdapter().setInternalDataFromState(playback_.getStateBatch().getState(time)); + } else { + ROS_DEBUG("FreeGaitPreviewDisplay::processMessage: No corresponding step found, resetting real state."); + adapterRos_.updateAdapterWithState(); + } + } + + playback_.process(steps); +} + +void FreeGaitPreviewDisplay::feedbackCallback(const free_gait_msgs::ExecuteStepsActionFeedback::ConstPtr& message) +{ + ROS_DEBUG("FreeGaitPreviewDisplay::feedbackCallback: Received feedback callback."); + feedbackMessage_ = *message; +} + +void FreeGaitPreviewDisplay::resultCallback(const free_gait_msgs::ExecuteStepsActionResult::ConstPtr& message) +{ + ROS_DEBUG("FreeGaitPreviewDisplay::resultCallback: Received result callback."); + if (autoEnableVisualsProperty_->getBool() && (autoHideVisualsProperty_->getOptionInt() == AutoHideMode::ReceivedResult)) { + setEnabledRobotModel(false); + visualsTree_->setBool(false); + } +} + +void FreeGaitPreviewDisplay::setEnabledRobotModel(bool enable) +{ + if (robotModelRvizPlugin_ == NULL) return; + ROS_DEBUG_STREAM("Setting robot model plugin to enabled " << (enable ? "True" : "False") << "."); + robotModelRvizPlugin_->setEnabled(enable); +} + +bool FreeGaitPreviewDisplay::findAssociatedRobotModelPlugin() +{ + const QString name = robotModelProperty_->getString(); + ROS_DEBUG_STREAM("Trying to find RobotModel RViz plugin with name " << name.toStdString() << "."); + auto property = getParent()->subProp(name); + robotModelRvizPlugin_ = dynamic_cast(property); + return (robotModelRvizPlugin_ != NULL); +} + +} // end namespace + +#include +PLUGINLIB_EXPORT_CLASS(free_gait_rviz_plugin::FreeGaitPreviewDisplay, rviz::Display) diff --git a/free_gait_rviz_plugin/src/FreeGaitPreviewPlayback.cpp b/free_gait_rviz_plugin/src/FreeGaitPreviewPlayback.cpp new file mode 100644 index 0000000..0b3222f --- /dev/null +++ b/free_gait_rviz_plugin/src/FreeGaitPreviewPlayback.cpp @@ -0,0 +1,174 @@ +/* + * FreeGaitPreviewPlayback.cpp + * + * Created on: Dec 19, 2016 + * Author: Péter Fankhauser + * Institute: ETH Zurich, Autonomous Systems Lab + */ +#include "free_gait_rviz_plugin/FreeGaitPreviewPlayback.hpp" + +// STD +#include + +using namespace free_gait; + +namespace free_gait_rviz_plugin { + +FreeGaitPreviewPlayback::FreeGaitPreviewPlayback(ros::NodeHandle& nodeHandle, + free_gait::AdapterBase& adapter) + : nodeHandle_(nodeHandle), + playMode_(PlayMode::ONHOLD), + time_(0.0), + stateBatchComputer_(adapter), + stateRosPublisher_(nodeHandle, adapter), + speedFactor_(1.0) +{ + executorState_.reset(new State()); + + parameters_.reset(new StepParameters); + completer_.reset(new StepCompleter(*parameters_, adapter)); + computer_.reset(new StepComputer()); + + executor_.reset(new Executor(*completer_, *computer_, adapter, *executorState_)); + executor_->initialize(); + + batchExecutor_.reset(new BatchExecutor(*executor_)); + batchExecutor_->addProcessingCallback( + std::bind(&FreeGaitPreviewPlayback::processingCallback, this, std::placeholders::_1)); +} + +FreeGaitPreviewPlayback::~FreeGaitPreviewPlayback() +{ +} + +void FreeGaitPreviewPlayback::addNewGoalCallback(std::function callback) +{ + newGoalCallback_ = callback; +} + +void FreeGaitPreviewPlayback::addStateChangedCallback(std::function callback) +{ + stateChangedCallback_ = callback; +} + +void FreeGaitPreviewPlayback::addReachedEndCallback(std::function callback) +{ + reachedEndCallback_ = callback; +} + +bool FreeGaitPreviewPlayback::process(const std::vector& steps) +{ + return batchExecutor_->process(steps); +} + +void FreeGaitPreviewPlayback::run() +{ + ROS_DEBUG("Set mode to PlayMode::FORWARD."); + playMode_ = PlayMode::FORWARD; +} + +void FreeGaitPreviewPlayback::stop() +{ + ROS_DEBUG("Set mode to PlayMode::STOPPED."); + playMode_ = PlayMode::STOPPED; +} + +void FreeGaitPreviewPlayback::goToTime(const ros::Time& time) +{ + ROS_DEBUG_STREAM("Jumping to time " << time << "."); + time_ = time; + stop(); +} + +void FreeGaitPreviewPlayback::clear() +{ + Lock lock(dataMutex_); + playMode_ = PlayMode::ONHOLD; + time_.fromSec(0.0); + stateBatch_.clear(); +} + +const free_gait::StateBatch& FreeGaitPreviewPlayback::getStateBatch() const +{ + return stateBatch_; +} + +const ros::Time& FreeGaitPreviewPlayback::getTime() const +{ + return time_; +} + +void FreeGaitPreviewPlayback::setSpeedFactor(const double speedFactor) +{ + speedFactor_ = speedFactor; +} + +void FreeGaitPreviewPlayback::setRate(const double rate) +{ + batchExecutor_->setTimeStep(1.0/rate); +} + +double FreeGaitPreviewPlayback::getRate() const +{ + return (1.0/batchExecutor_->getTimeStep()); +} + +void FreeGaitPreviewPlayback::setTfPrefix(const std::string tfPrefix) +{ + stateRosPublisher_.setTfPrefix(tfPrefix); +} + +void FreeGaitPreviewPlayback::update(double timeStep) +{ + switch (playMode_) { + case PlayMode::FORWARD: { + Lock lock(dataMutex_); + time_ += ros::Duration(speedFactor_ * timeStep); + if (time_ > ros::Time(stateBatch_.getEndTime())) { + stop(); + reachedEndCallback_(); + } else { + publish(time_); + } + break; + } + case PlayMode::STOPPED: { + publish(time_); + ROS_DEBUG("Set mode to PlayMode::ONHOLD."); + playMode_ = PlayMode::ONHOLD; + break; + } + case PlayMode::ONHOLD: + default: + break; + } +} + +void FreeGaitPreviewPlayback::processingCallback(bool success) +{ + ROS_DEBUG("FreeGaitPreviewPlayback::processingCallback: Finished processing new goal, copying new data."); + if (!success) return; + Lock lock(dataMutex_); + clear(); + stateBatch_ = batchExecutor_->getStateBatch(); // Deep copy. + stateBatchComputer_.computeEndEffectorTrajectories(stateBatch_); + stateBatchComputer_.computeEndEffectorTargetsAndSurfaceNormals(stateBatch_); + stateBatchComputer_.computeStances(stateBatch_); + stateBatchComputer_.computeStepIds(stateBatch_); + time_.fromSec(stateBatch_.getStartTime()); + ROS_DEBUG_STREAM("Resetting time to " << time_ << "."); + newGoalCallback_(); +} + +void FreeGaitPreviewPlayback::publish(const ros::Time& time) +{ + // TODO Increase speed by smarter locking. + Lock lock(dataMutex_); + const double timeInDouble = time.toSec(); + if (!stateBatch_.isValidTime(timeInDouble)) return; + const State state = stateBatch_.getState(timeInDouble); + stateRosPublisher_.publish(state); + stateChangedCallback_(time); +} + +} /* namespace free_gait_rviz_plugin */ diff --git a/free_gait_rviz_plugin/src/FreeGaitPreviewVisual.cpp b/free_gait_rviz_plugin/src/FreeGaitPreviewVisual.cpp new file mode 100644 index 0000000..1bc2b0d --- /dev/null +++ b/free_gait_rviz_plugin/src/FreeGaitPreviewVisual.cpp @@ -0,0 +1,282 @@ +/* + * FreeGaitPreviewVisual.cpp + * + * Created on: Dec 22, 2016 + * Author: Péter Fankhauser + * Institute: ETH Zurich, Autonomous Systems Lab + */ +#include "free_gait_rviz_plugin/FreeGaitPreviewVisual.hpp" + +#include + +#include + +namespace free_gait_rviz_plugin { + +FreeGaitPreviewVisual::FreeGaitPreviewVisual(rviz::DisplayContext* context, Ogre::SceneNode* parentNode) + : context_(context), + frameNode_(parentNode->createChildSceneNode()), + stateBatchPtr_(NULL) +{ + // Visible by default. + setEnabledModul(Modul::EndEffectorTargets, true); + setEnabledModul(Modul::SurfaceNormals, true); + setEnabledModul(Modul::EndEffectorTrajectories, true); + setEnabledModul(Modul::Stances, true); +} + +FreeGaitPreviewVisual::~FreeGaitPreviewVisual() +{ + context_->getSceneManager()->destroySceneNode(frameNode_); +} + +void FreeGaitPreviewVisual::clear() +{ + stateBatchPtr_ = NULL; + + for (const auto& modul : enabledModules_) { + switch (modul) { + case Modul::EndEffectorTargets: + hideEndEffectorTargets(); + break; + case Modul::SurfaceNormals: + hideSurfaceNormals(); + break; + case Modul::EndEffectorTrajectories: + hideEndEffectorTrajectories(); + break; + case Modul::Stances: + hideStances(); + break; + default: + break; + } + } +} + +void FreeGaitPreviewVisual::setStateBatch(const free_gait::StateBatch& stateBatch) +{ + stateBatchPtr_ = &stateBatch; + modulesToEnable_ = enabledModules_; +} + +void FreeGaitPreviewVisual::setEnabledModul(Modul modul, bool enable) +{ + if (enable) { + modulesToEnable_.push_back(modul); + auto iterator = std::find(enabledModules_.begin(), enabledModules_.end(), modul); + if (iterator != enabledModules_.end()) return; + enabledModules_.push_back(modul); + } else { + modulesToDisable_.push_back(modul); + enabledModules_.remove(modul); + } +} + +void FreeGaitPreviewVisual::showEnabled() +{ + modulesToEnable_ = enabledModules_; +} + +void FreeGaitPreviewVisual::hideEnabled() +{ + modulesToDisable_ = enabledModules_; +} + +void FreeGaitPreviewVisual::update() +{ + for (const auto& modul : modulesToDisable_) { + switch (modul) { + case Modul::EndEffectorTargets: + hideEndEffectorTargets(); + break; + case Modul::SurfaceNormals: + hideSurfaceNormals(); + break; + case Modul::EndEffectorTrajectories: + hideEndEffectorTrajectories(); + break; + case Modul::Stances: + hideStances(); + break; + default: + break; + } + } + modulesToDisable_.clear(); + + for (const auto& modul : modulesToEnable_) { + switch (modul) { + case Modul::EndEffectorTargets: + showEndEffectorTargets(); + break; + case Modul::SurfaceNormals: + showSurfaceNormals(); + break; + case Modul::EndEffectorTrajectories: + showEndEffectorTrajectories(); + break; + case Modul::Stances: + showStances(); + break; + default: + break; + } + } + modulesToEnable_.clear(); +} + +void FreeGaitPreviewVisual::showEndEffectorTargets(const float diameter, const Ogre::ColourValue& color) +{ + ROS_DEBUG("Rendering end effector targets."); + if (!stateBatchPtr_) return; + + endEffectorTargets_.clear(); + const auto targets = stateBatchPtr_->getEndEffectorTargets(); + const size_t nEndEffectors(targets.size()); + + for (size_t i = 0; i < nEndEffectors; ++i) { + endEffectorTargets_.push_back(std::vector>()); + // For each limb. + for (const auto& target : targets[i]) { + // For each target. + const auto& targetPosition = target.second; + endEffectorTargets_[i].push_back( + std::unique_ptr(new rviz::Shape(rviz::Shape::Type::Sphere, context_->getSceneManager(), frameNode_))); + auto& shape = endEffectorTargets_[i].back(); + shape->setPosition(Ogre::Vector3(targetPosition.x(), targetPosition.y(), targetPosition.z())); + shape->setColor(color); + shape->setScale(Ogre::Vector3(diameter)); + } + } +} + +void FreeGaitPreviewVisual::hideEndEffectorTargets() +{ + endEffectorTargets_.clear(); +} + +void FreeGaitPreviewVisual::showSurfaceNormals(const float diameter, const float length, const Ogre::ColourValue& color) +{ + ROS_DEBUG("Rendering surface normals."); + if (!stateBatchPtr_) return; + surfaceNormals_.clear(); + + const auto surfaceNormals = stateBatchPtr_->getSurfaceNormals(); + const size_t nSurfaceNormals(surfaceNormals.size()); + + for (size_t i = 0; i < nSurfaceNormals; ++i) { + surfaceNormals_.push_back(std::vector>()); + // For each limb. + for (const auto& surfaceNormal : surfaceNormals[i]) { + // For each surface normal. + const auto& position = std::get<0>(surfaceNormal.second); + const auto& vector = std::get<1>(surfaceNormal.second); + surfaceNormals_[i].push_back( + std::unique_ptr(new rviz::Arrow(context_->getSceneManager(), frameNode_))); + auto& arrow = surfaceNormals_[i].back(); + arrow->setPosition(Ogre::Vector3(position.x(), position.y(), position.z())); + arrow->setColor(color); + arrow->setDirection(Ogre::Vector3(vector.x(), vector.y(), vector.z())); + arrow->set(length, diameter, 0.1 * length, 3.0 * diameter); + } + } + +} + +void FreeGaitPreviewVisual::hideSurfaceNormals() +{ + surfaceNormals_.clear(); +} + +void FreeGaitPreviewVisual::showEndEffectorTrajectories(const float width, const Ogre::ColourValue& color) +{ + ROS_DEBUG("Rendering end effector trajectories."); + if (!stateBatchPtr_) return; + + const auto positions = stateBatchPtr_->getEndEffectorPositions(); + + // Define size. + const size_t nEndEffectors(positions.size()); + const size_t nStates(stateBatchPtr_->getStates().size()); + + // Cleanup. + if (endEffectorTrajectories_.size() != nEndEffectors) { + endEffectorTrajectories_.clear(); + for (size_t i = 0; i < nEndEffectors; ++i) { + endEffectorTrajectories_.push_back(std::unique_ptr(new rviz::BillboardLine(context_->getSceneManager(), frameNode_))); + } + } + + // Render. + for (size_t i = 0; i < nEndEffectors; ++i) { + auto& trajectory = endEffectorTrajectories_[i]; + // For each foot trajectory. + trajectory->clear(); + trajectory->setLineWidth(width); + trajectory->setColor(color.r, color.g, color.b, color.a); + trajectory->setNumLines(1); + trajectory->setMaxPointsPerLine(nStates); + + free_gait::Position previousPosition(free_gait::Position::Zero()); + for (const auto& positionElement : positions[i]) { + const auto& position = positionElement.second; + if ((position - previousPosition).norm() < 0.01) continue; // TODO Set as parameter. + previousPosition = position; + const auto point = Ogre::Vector3(position.x(), position.y(), position.z()); + trajectory->addPoint(point); + } + } +} + +void FreeGaitPreviewVisual::hideEndEffectorTrajectories() +{ + for (auto& trajectory : endEffectorTrajectories_) { + trajectory->clear(); + } +} + +void FreeGaitPreviewVisual::showStances(const float alpha) +{ + ROS_DEBUG("Rendering stances."); + if (!stateBatchPtr_) return; + stancesMarker_.clear(); + + for (const auto& stance : stateBatchPtr_->getStances()) { + if (stance.second.empty()) continue; + std_msgs::ColorRGBA color; + getRainbowColor(stateBatchPtr_->getStartTime(), stateBatchPtr_->getEndTime(), stance.first, color); + color.a = alpha; + visualization_msgs::Marker marker = free_gait::RosVisualization::getStanceMarker(stance.second, "odom", // TODO Use adapter? + color); + stancesMarker_.push_back(std::unique_ptr(new rviz::TriangleListMarker(nullptr, context_, frameNode_))); + stancesMarker_.back()->setMessage(marker); + } +} + +void FreeGaitPreviewVisual::hideStances() +{ + stancesMarker_.clear(); +} + +void FreeGaitPreviewVisual::getRainbowColor(const double min, const double max, const double value, std_msgs::ColorRGBA& color) +{ + double adaptedValue = (value - min) / (max - min); + + // From rviz/src/rviz/default_plugin/point_cloud_transformers.cpp: + adaptedValue = std::min(adaptedValue, 1.0); + adaptedValue = std::max(adaptedValue, 0.0); + + float h = adaptedValue * 5.0 + 1.0; + int i = floor(h); + float f = h - i; + if (!(i & 1)) f = 1 - f; // If i is even. + float n = 1 - f; + if (i <= 1) color.r = n, color.g = 0, color.b = 1; + else if (i == 2) color.r = 0, color.g = n, color.b = 1; + else if (i == 3) color.r = 0, color.g = 1, color.b = n; + else if (i == 4) color.r = n, color.g = 1, color.b = 0; + else if (i >= 5) color.r = 1, color.g = n, color.b = 0; +} + +} /* namespace free_gait_rviz_plugin */ diff --git a/free_gait_rviz_plugin/src/rviz/properties/button_property.cpp b/free_gait_rviz_plugin/src/rviz/properties/button_property.cpp new file mode 100644 index 0000000..1d76a5e --- /dev/null +++ b/free_gait_rviz_plugin/src/rviz/properties/button_property.cpp @@ -0,0 +1,55 @@ +#include "rviz/properties/button_property.h" + +namespace rviz +{ + +ButtonProperty::ButtonProperty(const QString& name, const QString& default_value, + const QString& description, Property* parent, + const char *changed_slot, QObject* receiver) + : Property(name, default_value, description, parent, changed_slot, receiver), + label_(default_value) +{ + setShouldBeSaved(false); +} + +void ButtonProperty::setLabel(const std::string& label) +{ + label_ = QString::fromStdString(label); +} + +QVariant ButtonProperty::getViewData( int column, int role ) const +{ + if (column == 1) { + switch (role) { + case Qt::DisplayRole: + return label_; + case Qt::EditRole: + case Qt::CheckStateRole: + return QVariant(); + default: + return Property::getViewData(column, role); + } + } + return Property::getViewData(column, role); +} + +Qt::ItemFlags ButtonProperty::getViewFlags( int column ) const +{ + Qt::ItemFlags enabled_flag = ( getParent() && getParent()->getDisableChildren() ) ? Qt::NoItemFlags : Qt::ItemIsEnabled; + if (column == 0) return Property::getViewFlags(column); + return Qt::ItemIsEditable | enabled_flag | Qt::ItemIsSelectable; +} + +QWidget* ButtonProperty::createEditor(QWidget* parent, const QStyleOptionViewItem& option) +{ + QPushButton* button = new QPushButton(label_, parent); + connect(button, SIGNAL(released()), this, SLOT(buttonReleased())); + return button; +} + +void ButtonProperty::buttonReleased() +{ + Q_EMIT changed(); +} + +} // end namespace rviz diff --git a/free_gait_rviz_plugin/src/rviz/properties/button_property.h b/free_gait_rviz_plugin/src/rviz/properties/button_property.h new file mode 100644 index 0000000..9dc1d05 --- /dev/null +++ b/free_gait_rviz_plugin/src/rviz/properties/button_property.h @@ -0,0 +1,34 @@ +#pragma once + +#include +#include +#include "rviz/properties/property.h" +#include + +namespace rviz +{ + +class ButtonProperty: public Property +{ +Q_OBJECT +public: + ButtonProperty(const QString& name = QString(), const QString& default_value = QString(), + const QString& description = QString(), Property* parent = 0, + const char* changed_slot = 0, QObject* receiver = 0); + + void setLabel(const std::string& label); + QVariant getViewData( int column, int role ) const; + Qt::ItemFlags getViewFlags( int column ) const; + +public Q_SLOTS: + void buttonReleased(); + +protected: + virtual QWidget* createEditor(QWidget* parent, const QStyleOptionViewItem& option); + +private: + QString label_; +}; + +} // end namespace rviz + diff --git a/free_gait_rviz_plugin/src/rviz/properties/button_toggle_property.cpp b/free_gait_rviz_plugin/src/rviz/properties/button_toggle_property.cpp new file mode 100644 index 0000000..a18c104 --- /dev/null +++ b/free_gait_rviz_plugin/src/rviz/properties/button_toggle_property.cpp @@ -0,0 +1,48 @@ +#include "rviz/properties/button_toggle_property.h" + +namespace rviz +{ + +ButtonToggleProperty::ButtonToggleProperty(const QString& name, bool default_value, + const QString& description, Property* parent, + const char *changed_slot, QObject* receiver) + : ButtonProperty(name, "", description, parent, changed_slot, receiver), + labelForTrue_("True"), + labelForFalse_("False"), + button_(NULL) +{ +} + +void ButtonToggleProperty::setLabels(const std::string& labelForTrue, const std::string& labelForFalse) +{ + labelForTrue_ = QString::fromStdString(labelForTrue); + labelForFalse_ = QString::fromStdString(labelForFalse); +} + +const QString& ButtonToggleProperty::getLabel() +{ + return (getBool() ? labelForTrue_ : labelForFalse_); +} + +bool ButtonToggleProperty::getBool() const +{ + return getValue().toBool(); +} + +QWidget* ButtonToggleProperty::createEditor(QWidget* parent, const QStyleOptionViewItem& option) +{ + button_ = new QPushButton(getLabel(), parent); + button_->setCheckable(true); + button_->setChecked(true); + connect(button_, SIGNAL(toggled(bool)), this, SLOT(buttonToggled(bool))); + return button_; +} + +void ButtonToggleProperty::buttonToggled(bool state) +{ + button_->setText(getLabel()); + value_.setValue(state); + Q_EMIT changed(); +} + +} // end namespace rviz diff --git a/free_gait_rviz_plugin/src/rviz/properties/button_toggle_property.h b/free_gait_rviz_plugin/src/rviz/properties/button_toggle_property.h new file mode 100644 index 0000000..6ef07eb --- /dev/null +++ b/free_gait_rviz_plugin/src/rviz/properties/button_toggle_property.h @@ -0,0 +1,32 @@ +#pragma once + +#include "rviz/properties/button_property.h" + +namespace rviz +{ + +class ButtonToggleProperty: public ButtonProperty +{ +Q_OBJECT +public: + ButtonToggleProperty(const QString& name = QString(), bool default_value = false, + const QString& description = QString(), Property* parent = 0, + const char* changed_slot = 0, QObject* receiver = 0); + + void setLabels(const std::string& labelForTrue, const std::string& labelForFalse); + const QString& getLabel(); + bool getBool() const; + +public Q_SLOTS: + void buttonToggled(bool state); + +protected: + virtual QWidget* createEditor(QWidget* parent, const QStyleOptionViewItem& option); + + QString labelForTrue_; + QString labelForFalse_; + QPushButton* button_; +}; + +} // end namespace rviz + diff --git a/free_gait_rviz_plugin/src/rviz/properties/float_slider_property.cpp b/free_gait_rviz_plugin/src/rviz/properties/float_slider_property.cpp new file mode 100644 index 0000000..7c342fa --- /dev/null +++ b/free_gait_rviz_plugin/src/rviz/properties/float_slider_property.cpp @@ -0,0 +1,54 @@ +/* + * float_slider_property.cpp + * + * Created on: Dec 12, 2016 + * Author: Péter Fankhauser + * Institute: ETH Zurich, Robotic Systems Lab + */ + +#include "rviz/properties/float_slider_property.h" +#include "rviz/properties/line_edit_with_slider.h" + +#include +#include + +namespace rviz +{ + +FloatSliderProperty::FloatSliderProperty(const QString& name, float default_value, + const QString& description, Property* parent, + const char *changed_slot, QObject* receiver) + : FloatProperty(name, default_value, description, parent, changed_slot, receiver), + minIntValue_(0), + maxIntValue_(1000) +{ + setMin(0.0); + setMax(1.0); +} + +bool FloatSliderProperty::setValuePassive(const QVariant& value) +{ + value_ = qBound(getMin(), value.toFloat(), getMax()); + return true; +} + +void FloatSliderProperty::sliderValueChanged(int value) +{ + const float floatValue = getMin() + (getMax() - getMin()) + * (value - minIntValue_) / (maxIntValue_ - minIntValue_); + setValue(floatValue); +} + +QWidget* FloatSliderProperty::createEditor(QWidget* parent, const QStyleOptionViewItem& option) +{ + LineEditWithSlider* slider = new LineEditWithSlider(parent); + slider->slider()->setRange(minIntValue_, maxIntValue_); + slider->setValidator(new QDoubleValidator(slider->slider())); + const int intValue = minIntValue_ + (maxIntValue_ - minIntValue_) + * (getValue().toFloat() - getMin()) / (getMax() - getMin()); + slider->slider()->setValue(intValue); + connect(slider->slider(), SIGNAL(valueChanged(int)), this, SLOT(sliderValueChanged(int))); + return slider; +} + +} // end namespace rviz diff --git a/free_gait_rviz_plugin/src/rviz/properties/float_slider_property.h b/free_gait_rviz_plugin/src/rviz/properties/float_slider_property.h new file mode 100644 index 0000000..d227015 --- /dev/null +++ b/free_gait_rviz_plugin/src/rviz/properties/float_slider_property.h @@ -0,0 +1,38 @@ +/* + * float_slider_property.h + * + * Created on: Dec 12, 2016 + * Author: Péter Fankhauser + * Institute: ETH Zurich, Robotic Systems Lab + */ + +#pragma once + +#include +#include +#include + +namespace rviz { + +class FloatSliderProperty : public FloatProperty +{ + Q_OBJECT + public: + FloatSliderProperty(const QString& name = QString(), float default_value = 0.0, + const QString& description = QString(), Property* parent = 0, + const char* changed_slot = 0, QObject* receiver = 0); + + virtual bool setValuePassive(const QVariant& value); + +public Q_SLOTS: + virtual void sliderValueChanged(int value); + +protected: + virtual QWidget* createEditor(QWidget* parent, const QStyleOptionViewItem& option); + + private: + int minIntValue_, maxIntValue_; +}; + +} // end namespace rviz + diff --git a/free_gait_rviz_plugin/src/rviz/properties/line_edit_with_slider.cpp b/free_gait_rviz_plugin/src/rviz/properties/line_edit_with_slider.cpp new file mode 100644 index 0000000..a673b23 --- /dev/null +++ b/free_gait_rviz_plugin/src/rviz/properties/line_edit_with_slider.cpp @@ -0,0 +1,33 @@ +/* + * line_edit_with_slider.cpp + * + * Created on: Dec 12, 2016 + * Author: Péter Fankhauser + * Institute: ETH Zurich, Robotic Systems Lab + */ + +#include "rviz/properties/line_edit_with_slider.h" + +#include + +namespace rviz +{ + +LineEditWithSlider::LineEditWithSlider( QWidget* parent ) + : QLineEdit( parent ) +{ + setFrame(false); + slider_ = new QSlider(Qt::Orientation::Horizontal, this); +} + +void LineEditWithSlider::resizeEvent( QResizeEvent* event ) +{ + int padding = 0; + int textMinimalWidth = 45; + int sliderWidth = width() - textMinimalWidth - 2; + setTextMargins(padding, 0, sliderWidth, 0); + QLineEdit::resizeEvent(event); + slider_->setGeometry(textMinimalWidth, padding, sliderWidth, height()); +} + +} // end namespace rviz diff --git a/free_gait_rviz_plugin/src/rviz/properties/line_edit_with_slider.h b/free_gait_rviz_plugin/src/rviz/properties/line_edit_with_slider.h new file mode 100644 index 0000000..c42ca61 --- /dev/null +++ b/free_gait_rviz_plugin/src/rviz/properties/line_edit_with_slider.h @@ -0,0 +1,37 @@ +/* + * line_edit_with_slider.h + * + * Created on: Dec 12, 2016 + * Author: Péter Fankhauser + * Institute: ETH Zurich, Robotic Systems Lab + */ + +#pragma once + +#include + +class QSlider; + +namespace rviz { + +/** + * A QLineEdit for a numeric value with a slider to the right. + */ +class LineEditWithSlider : public QLineEdit +{ + Q_OBJECT + public: + LineEditWithSlider(QWidget* parent = 0); + + /** Returns the child slider. Use this to connect() something to a + * changed value. */ + QSlider* slider() { return slider_; } + + protected: + virtual void resizeEvent(QResizeEvent* event); + + private: + QSlider* slider_; +}; + +} // end namespace rviz diff --git a/qp_solver/CMakeLists.txt b/qp_solver/CMakeLists.txt index 129b064..67c34f4 100644 --- a/qp_solver/CMakeLists.txt +++ b/qp_solver/CMakeLists.txt @@ -1,8 +1,5 @@ cmake_minimum_required(VERSION 2.8.3) project(qp_solver) -#file(GLOB sources *.cc) -#file(GLOB headers *.hh) -#set(library_sources ${sources}) ## Compile as C++11, supported in ROS Kinetic and newer # add_compile_options(-std=c++11) @@ -14,6 +11,7 @@ find_package(catkin REQUIRED COMPONENTS roscpp grid_map_core std_utils + quadruped_model ) ## System dependencies are found with CMake's conventions @@ -109,8 +107,9 @@ catkin_package( INCLUDE_DIRS include ${EIGEN3_INCLUDE_DIR} -# LIBRARIES qp_sovler + LIBRARIES ${PROJECT_NAME} CATKIN_DEPENDS + quadruped_model roscpp grid_map_core std_utils @@ -132,31 +131,32 @@ include_directories( ) ## Declare a C++ library -add_library(quadprog +add_library(${PROJECT_NAME} src/Array.cc src/QuadProg++.cc src/quadraticproblemsolver.cpp src/sequencequadraticproblemsolver.cpp - src/pose_optimization/poseparameterization.cpp - src/pose_optimization/PoseOptimizationBase.cpp - src/pose_optimization/PoseConstraintsChecker.cpp - src/pose_optimization/PoseOptimizationGeometric.cpp - src/pose_optimization/PoseOptimizationQP.cpp - src/pose_optimization/PoseOptimizationSQP.cpp - src/pose_optimization/PoseOptimizationObjectiveFunction.cpp - src/pose_optimization/PoseOptimizationFunctionConstraints.cpp - src/pose_optimization/PoseOptimizationProblem.cpp +# src/pose_optimization/poseparameterization.cpp +# src/pose_optimization/PoseOptimizationBase.cpp +# src/pose_optimization/PoseConstraintsChecker.cpp +# src/pose_optimization/PoseOptimizationGeometric.cpp +# src/pose_optimization/PoseOptimizationQP.cpp +# src/pose_optimization/PoseOptimizationSQP.cpp +# src/pose_optimization/PoseOptimizationObjectiveFunction.cpp +# src/pose_optimization/PoseOptimizationFunctionConstraints.cpp +# src/pose_optimization/PoseOptimizationProblem.cpp + # src/poseparameterization.h # src/sequencequadraticproblemsolver.h # include/qp_solver/quadraticproblemsolver.h # include/qp_solver/Array.h # include/qp_solver/QuadProg++.h - src/TypeDefs.cpp - src/State.cpp - src/AdapterBase.cpp - test/AdapterDummy.cpp +# src/TypeDefs.cpp +# src/State.cpp +# src/AdapterBase.cpp +# test/AdapterDummy.cpp ) -target_link_libraries(quadprog ${catkin_LIBRARIES}) +target_link_libraries(${PROJECT_NAME} ${catkin_LIBRARIES}) ## Add cmake target dependencies of the library ## as an example, code may need to be generated before libraries ## either from message generation or dynamic reconfigure @@ -166,10 +166,12 @@ target_link_libraries(quadprog ${catkin_LIBRARIES}) ## With catkin_make all packages are built within a single CMake context ## The recommended prefix ensures that target names across packages don't collide # add_executable(${PROJECT_NAME}_node src/qp_sovler_node.cpp) -add_executable(main src/main.cc) -target_link_libraries(main ${catkin_LIBRARIES} quadprog) -add_executable(qp_solve_test src/qp_solve_test.cpp) -target_link_libraries(qp_solve_test ${catkin_LIBRARIES} quadprog) +#add_executable(main src/main.cc) +#target_link_libraries(main ${catkin_LIBRARIES} quadprog) + +#add_executable(qp_solve_test src/qp_solve_test.cpp) +#target_link_libraries(qp_solve_test ${catkin_LIBRARIES} ${PROJECT_NAME}) + ## Rename C++ executable without prefix ## The above recommended prefix causes long target names, the following renames the ## target back to the shorter version for ease of user use @@ -201,7 +203,7 @@ catkin_add_gtest(${PROJECT_NAME}-test ) if(TARGET ${PROJECT_NAME}-test) target_link_libraries(${PROJECT_NAME}-test - quadprog + ${PROJECT_NAME} ) endif() @@ -220,18 +222,18 @@ endif() # ) ## Mark executables and/or libraries for installation -# install(TARGETS ${PROJECT_NAME} ${PROJECT_NAME}_node -# ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} -# LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} -# RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} -# ) + install(TARGETS ${PROJECT_NAME} #${PROJECT_NAME}_node + ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} + LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} + RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} + ) ## Mark cpp header files for installation -# install(DIRECTORY include/${PROJECT_NAME}/ -# DESTINATION ${CATKIN_PACKAGE_INCLUDE_DESTINATION} + install(DIRECTORY include/${PROJECT_NAME}/ + DESTINATION ${CATKIN_PACKAGE_INCLUDE_DESTINATION} # FILES_MATCHING PATTERN "*.h" # PATTERN ".svn" EXCLUDE -# ) + ) ## Mark other files for installation (e.g. launch and bag files, etc.) # install(FILES diff --git a/qp_solver/include/qp_solver/QuadProg++.h b/qp_solver/include/qp_solver/QuadProg++.h index 612a762..cf2137d 100644 --- a/qp_solver/include/qp_solver/QuadProg++.h +++ b/qp_solver/include/qp_solver/QuadProg++.h @@ -1,59 +1,59 @@ /* - File $Id: QuadProg++.hh 232 2007-06-21 12:29:00Z digasper $ - - The quadprog_solve() function implements the algorithm of Goldfarb and Idnani + File $Id: QuadProg++.hh 232 2007-06-21 12:29:00Z digasper $ + + The quadprog_solve() function implements the algorithm of Goldfarb and Idnani for the solution of a (convex) Quadratic Programming problem by means of an active-set dual method. - + The problem is in the form: min 0.5 * x G x + g0 x s.t. CE^T x + ce0 = 0 CI^T x + ci0 >= 0 - + The matrix and vectors dimensions are as follows: G: n * n g0: n - + CE: n * p ce0: p - + CI: n * m ci0: m x: n - + The function will return the cost of the solution written in the x vector or std::numeric_limits::infinity() if the problem is infeasible. In the latter case the value of the x vector is not correct. - + References: D. Goldfarb, A. Idnani. A numerically stable dual method for solving strictly convex quadratic programs. Mathematical Programming 27 (1983) pp. 1-33. Notes: - 1. pay attention in setting up the vectors ce0 and ci0. - If the constraints of your problem are specified in the form + 1. pay attention in setting up the vectors ce0 and ci0. + If the constraints of your problem are specified in the form A^T x = b and C^T x >= d, then you should set ce0 = -b and ci0 = -d. - 2. The matrices have column dimension equal to MATRIX_DIM, - a constant set to 20 in this file (by means of a #define macro). + 2. The matrices have column dimension equal to MATRIX_DIM, + a constant set to 20 in this file (by means of a #define macro). If the matrices are bigger than 20 x 20 the limit could be increased by means of a -DMATRIX_DIM=n on the compiler command line. 3. The matrix G is modified within the function since it is used to compute - the G = L^T L cholesky factorization for further computations inside the function. + the G = L^T L cholesky factorization for further computations inside the function. If you need the original matrix G you should make a copy of it and pass the copy to the function. - + Author: Luca Di Gaspero DIEGM - University of Udine, Italy luca.digaspero@uniud.it http://www.diegm.uniud.it/digaspero/ - + The author will be grateful if the researchers using this software will acknowledge the contribution of this function in their research papers. - + Copyright (c) 2007-2016 Luca Di Gaspero - + This software may be modified and distributed under the terms of the MIT license. See the LICENSE file for details. */ @@ -62,13 +62,13 @@ s.t. #ifndef _QUADPROGPP #define _QUADPROGPP -#include "qp_solver/Array.h" +#include "qp_solver/qp_array.h" namespace quadprogpp { -double solve_quadprog(Matrix& G, Vector& g0, - const Matrix& CE, const Vector& ce0, - const Matrix& CI, const Vector& ci0, +double solve_quadprog(Matrix& G, Vector& g0, + const Matrix& CE, const Vector& ce0, + const Matrix& CI, const Vector& ci0, Vector& x); } // namespace quadprogpp diff --git a/qp_solver/include/qp_solver/Array.h b/qp_solver/include/qp_solver/qp_array.h similarity index 92% rename from qp_solver/include/qp_solver/Array.h rename to qp_solver/include/qp_solver/qp_array.h index 0ad5d91..ea47bc3 100644 --- a/qp_solver/include/qp_solver/Array.h +++ b/qp_solver/include/qp_solver/qp_array.h @@ -1,13 +1,13 @@ // $Id: Array.hh 249 2008-11-20 09:58:23Z schaerf $ // This file is part of EasyLocalpp: a C++ Object-Oriented framework // aimed at easing the development of Local Search algorithms. -// Copyright (C) 2001--2008 Andrea Schaerf, Luca Di Gaspero. +// Copyright (C) 2001--2008 Andrea Schaerf, Luca Di Gaspero. // // This software may be modified and distributed under the terms // of the MIT license. See the LICENSE file for details. -#if !defined(_ARRAY_HH) -#define _ARRAY_HH +#ifndef _QP_ARRAY_HH +#define _QP_ARRAY_HH #include #include @@ -23,25 +23,25 @@ enum MType { DIAG }; template class Vector { -public: - Vector(); - Vector(const unsigned int n); - Vector(const T& a, const unsigned int n); //initialize to constant value - Vector(const T* a, const unsigned int n); // Initialize to array - Vector(const Vector &rhs); // copy constructor +public: + Vector(); + Vector(const unsigned int n); + Vector(const T& a, const unsigned int n); //initialize to constant value + Vector(const T* a, const unsigned int n); // Initialize to array + Vector(const Vector &rhs); // copy constructor ~Vector(); // destructor - + inline void set(const T* a, const unsigned int n); Vector extract(const std::set& indexes) const; - inline T& operator[](const unsigned int& i); //i-th element - inline const T& operator[](const unsigned int& i) const; - + inline T& operator[](const unsigned int& i); //i-th element + inline const T& operator[](const unsigned int& i) const; + inline unsigned int size() const; inline void resize(const unsigned int n); inline void resize(const T& a, const unsigned int n); - - Vector& operator=(const Vector& rhs); //assignment - Vector& operator=(const T& a); //assign a to every element + + Vector& operator=(const Vector& rhs); //assignment + Vector& operator=(const T& a); //assign a to every element inline Vector& operator+=(const Vector& rhs); inline Vector& operator-=(const Vector& rhs); inline Vector& operator*=(const Vector& rhs); @@ -52,160 +52,160 @@ class Vector inline Vector& operator*=(const T& a); inline Vector& operator/=(const T& a); inline Vector& operator^=(const T& a); -private: - unsigned int n; // size of array. upper index is n-1 +private: + unsigned int n; // size of array. upper index is n-1 T* v; // storage for data -}; +}; -template -Vector::Vector() - : n(0), v(0) -{} +template +Vector::Vector() + : n(0), v(0) +{} -template -Vector::Vector(const unsigned int n) - : v(new T[n]) +template +Vector::Vector(const unsigned int n) + : v(new T[n]) { this->n = n; -} +} -template -Vector::Vector(const T& a, const unsigned int n) +template +Vector::Vector(const T& a, const unsigned int n) : v(new T[n]) -{ +{ this->n = n; - for (unsigned int i = 0; i < n; i++) - v[i] = a; -} + for (unsigned int i = 0; i < n; i++) + v[i] = a; +} -template -Vector::Vector(const T* a, const unsigned int n) +template +Vector::Vector(const T* a, const unsigned int n) : v(new T[n]) -{ +{ this->n = n; - for (unsigned int i = 0; i < n; i++) - v[i] = *a++; -} + for (unsigned int i = 0; i < n; i++) + v[i] = *a++; +} -template -Vector::Vector(const Vector& rhs) +template +Vector::Vector(const Vector& rhs) : v(new T[rhs.n]) -{ +{ this->n = rhs.n; - for (unsigned int i = 0; i < n; i++) - v[i] = rhs[i]; -} + for (unsigned int i = 0; i < n; i++) + v[i] = rhs[i]; +} -template -Vector::~Vector() -{ - if (v != 0) - delete[] (v); -} +template +Vector::~Vector() +{ + if (v != 0) + delete[] (v); +} -template -void Vector::resize(const unsigned int n) +template +void Vector::resize(const unsigned int n) { if (n == this->n) return; - if (v != 0) - delete[] (v); + if (v != 0) + delete[] (v); v = new T[n]; this->n = n; -} +} -template -void Vector::resize(const T& a, const unsigned int n) +template +void Vector::resize(const T& a, const unsigned int n) { resize(n); for (unsigned int i = 0; i < n; i++) v[i] = a; -} +} -template -inline Vector& Vector::operator=(const Vector& rhs) -// postcondition: normal assignment via copying has been performed; -// if vector and rhs were different sizes, vector -// has been resized to match the size of rhs -{ - if (this != &rhs) - { +template +inline Vector& Vector::operator=(const Vector& rhs) +// postcondition: normal assignment via copying has been performed; +// if vector and rhs were different sizes, vector +// has been resized to match the size of rhs +{ + if (this != &rhs) + { resize(rhs.n); - for (unsigned int i = 0; i < n; i++) - v[i] = rhs[i]; - } - return *this; -} + for (unsigned int i = 0; i < n; i++) + v[i] = rhs[i]; + } + return *this; +} -template -inline Vector & Vector::operator=(const T& a) //assign a to every element -{ - for (unsigned int i = 0; i < n; i++) - v[i] = a; - return *this; -} +template +inline Vector & Vector::operator=(const T& a) //assign a to every element +{ + for (unsigned int i = 0; i < n; i++) + v[i] = a; + return *this; +} -template -inline T & Vector::operator[](const unsigned int& i) //subscripting -{ - return v[i]; +template +inline T & Vector::operator[](const unsigned int& i) //subscripting +{ + return v[i]; } template -inline const T& Vector::operator[](const unsigned int& i) const //subscripting -{ - return v[i]; -} +inline const T& Vector::operator[](const unsigned int& i) const //subscripting +{ + return v[i]; +} -template -inline unsigned int Vector::size() const -{ - return n; +template +inline unsigned int Vector::size() const +{ + return n; } -template -inline void Vector::set(const T* a, unsigned int n) -{ +template +inline void Vector::set(const T* a, unsigned int n) +{ resize(n); - for (unsigned int i = 0; i < n; i++) - v[i] = a[i]; -} + for (unsigned int i = 0; i < n; i++) + v[i] = a[i]; +} -template +template inline Vector Vector::extract(const std::set& indexes) const { Vector tmp(indexes.size()); unsigned int i = 0; - + for (std::set::const_iterator el = indexes.begin(); el != indexes.end(); el++) { if (*el >= n) throw std::logic_error("Error extracting subvector: the indexes are out of vector bounds"); tmp[i++] = v[*el]; } - + return tmp; } -template +template inline Vector& Vector::operator+=(const Vector& rhs) { if (this->size() != rhs.size()) throw std::logic_error("Operator+=: vectors have different sizes"); for (unsigned int i = 0; i < n; i++) v[i] += rhs[i]; - + return *this; } -template +template inline Vector& Vector::operator+=(const T& a) { for (unsigned int i = 0; i < n; i++) v[i] += a; - + return *this; } @@ -223,7 +223,7 @@ inline Vector operator+(const Vector& lhs, const Vector& rhs) Vector tmp(lhs.size()); for (unsigned int i = 0; i < lhs.size(); i++) tmp[i] = lhs[i] + rhs[i]; - + return tmp; } @@ -233,7 +233,7 @@ inline Vector operator+(const Vector& lhs, const T& a) Vector tmp(lhs.size()); for (unsigned int i = 0; i < lhs.size(); i++) tmp[i] = lhs[i] + a; - + return tmp; } @@ -243,28 +243,28 @@ inline Vector operator+(const T& a, const Vector& rhs) Vector tmp(rhs.size()); for (unsigned int i = 0; i < rhs.size(); i++) tmp[i] = a + rhs[i]; - + return tmp; } -template +template inline Vector& Vector::operator-=(const Vector& rhs) { if (this->size() != rhs.size()) throw std::logic_error("Operator-=: vectors have different sizes"); for (unsigned int i = 0; i < n; i++) v[i] -= rhs[i]; - + return *this; } -template +template inline Vector& Vector::operator-=(const T& a) { for (unsigned int i = 0; i < n; i++) v[i] -= a; - + return *this; } @@ -282,7 +282,7 @@ inline Vector operator-(const Vector& lhs, const Vector& rhs) Vector tmp(lhs.size()); for (unsigned int i = 0; i < lhs.size(); i++) tmp[i] = lhs[i] - rhs[i]; - + return tmp; } @@ -292,7 +292,7 @@ inline Vector operator-(const Vector& lhs, const T& a) Vector tmp(lhs.size()); for (unsigned int i = 0; i < lhs.size(); i++) tmp[i] = lhs[i] - a; - + return tmp; } @@ -302,28 +302,28 @@ inline Vector operator-(const T& a, const Vector& rhs) Vector tmp(rhs.size()); for (unsigned int i = 0; i < rhs.size(); i++) tmp[i] = a - rhs[i]; - + return tmp; } -template +template inline Vector& Vector::operator*=(const Vector& rhs) { if (this->size() != rhs.size()) throw std::logic_error("Operator*=: vectors have different sizes"); for (unsigned int i = 0; i < n; i++) v[i] *= rhs[i]; - + return *this; } -template +template inline Vector& Vector::operator*=(const T& a) { for (unsigned int i = 0; i < n; i++) v[i] *= a; - + return *this; } @@ -335,7 +335,7 @@ inline Vector operator*(const Vector& lhs, const Vector& rhs) Vector tmp(lhs.size()); for (unsigned int i = 0; i < lhs.size(); i++) tmp[i] = lhs[i] * rhs[i]; - + return tmp; } @@ -345,7 +345,7 @@ inline Vector operator*(const Vector& lhs, const T& a) Vector tmp(lhs.size()); for (unsigned int i = 0; i < lhs.size(); i++) tmp[i] = lhs[i] * a; - + return tmp; } @@ -355,28 +355,28 @@ inline Vector operator*(const T& a, const Vector& rhs) Vector tmp(rhs.size()); for (unsigned int i = 0; i < rhs.size(); i++) tmp[i] = a * rhs[i]; - + return tmp; } -template +template inline Vector& Vector::operator/=(const Vector& rhs) { if (this->size() != rhs.size()) throw std::logic_error("Operator/=: vectors have different sizes"); for (unsigned int i = 0; i < n; i++) v[i] /= rhs[i]; - + return *this; } -template +template inline Vector& Vector::operator/=(const T& a) { for (unsigned int i = 0; i < n; i++) v[i] /= a; - + return *this; } @@ -388,7 +388,7 @@ inline Vector operator/(const Vector& lhs, const Vector& rhs) Vector tmp(lhs.size()); for (unsigned int i = 0; i < lhs.size(); i++) tmp[i] = lhs[i] / rhs[i]; - + return tmp; } @@ -398,7 +398,7 @@ inline Vector operator/(const Vector& lhs, const T& a) Vector tmp(lhs.size()); for (unsigned int i = 0; i < lhs.size(); i++) tmp[i] = lhs[i] / a; - + return tmp; } @@ -408,7 +408,7 @@ inline Vector operator/(const T& a, const Vector& rhs) Vector tmp(rhs.size()); for (unsigned int i = 0; i < rhs.size(); i++) tmp[i] = a / rhs[i]; - + return tmp; } @@ -420,7 +420,7 @@ inline Vector operator^(const Vector& lhs, const Vector& rhs) Vector tmp(lhs.size()); for (unsigned int i = 0; i < lhs.size(); i++) tmp[i] = pow(lhs[i], rhs[i]); - + return tmp; } @@ -430,7 +430,7 @@ inline Vector operator^(const Vector& lhs, const T& a) Vector tmp(lhs.size()); for (unsigned int i = 0; i < lhs.size(); i++) tmp[i] = pow(lhs[i], a); - + return tmp; } @@ -440,7 +440,7 @@ inline Vector operator^(const T& a, const Vector& rhs) Vector tmp(rhs.size()); for (unsigned int i = 0; i < rhs.size(); i++) tmp[i] = pow(a, rhs[i]); - + return tmp; } @@ -451,7 +451,7 @@ inline Vector& Vector::operator^=(const Vector& rhs) throw std::logic_error("Operator^=: vectors have different sizes"); for (unsigned int i = 0; i < n; i++) v[i] = pow(v[i], rhs[i]); - + return *this; } @@ -460,7 +460,7 @@ inline Vector& Vector::operator^=(const T& a) { for (unsigned int i = 0; i < n; i++) v[i] = pow(v[i], a); - + return *this; } @@ -531,7 +531,7 @@ inline bool operator>=(const Vector& v, const Vector& w) } /** - Input/Output + Input/Output */ template inline std::ostream& operator<<(std::ostream& os, const Vector& v) @@ -540,7 +540,7 @@ inline std::ostream& operator<<(std::ostream& os, const Vector& v) for (unsigned int i = 0; i < v.size() - 1; i++) os << std::setw(20) << std::setprecision(16) << v[i] << ", "; os << std::setw(20) << std::setprecision(16) << v[v.size() - 1] << std::endl; - + return os; } @@ -553,7 +553,7 @@ std::istream& operator>>(std::istream& is, Vector& v) v.resize(elements); for (unsigned int i = 0; i < elements; i++) is >> v[i] >> comma; - + return is; } @@ -582,9 +582,9 @@ CanonicalBaseVector::CanonicalBaseVector(unsigned int i, unsigned int n) template inline void CanonicalBaseVector::reset(unsigned int i) -{ - (*this)[e] = (T)0; - e = i; +{ + (*this)[e] = (T)0; + e = i; (*this)[e] = (T)1; } @@ -596,7 +596,7 @@ inline T sum(const Vector& v) T tmp = (T)0; for (unsigned int i = 0; i < v.size(); i++) tmp += v[i]; - + return tmp; } @@ -606,7 +606,7 @@ inline T prod(const Vector& v) T tmp = (T)1; for (unsigned int i = 0; i < v.size(); i++) tmp *= v[i]; - + return tmp; } @@ -641,7 +641,7 @@ inline T var(const Vector& v, bool sample_correction = false) T sum = (T)0, ssum = (T)0; unsigned int n = v.size(); for (unsigned int i = 0; i < n; i++) - { + { sum += v[i]; ssum += (v[i] * v[i]); } @@ -657,7 +657,7 @@ inline T max(const Vector& v) T value = v[0]; for (unsigned int i = 1; i < v.size(); i++) value = std::max(v[i], value); - + return value; } @@ -667,7 +667,7 @@ inline T min(const Vector& v) T value = v[0]; for (unsigned int i = 1; i < v.size(); i++) value = std::min(v[i], value); - + return value; } @@ -678,7 +678,7 @@ inline unsigned int index_max(const Vector& v) for (unsigned int i = 1; i < v.size(); i++) if (v[i] > v[max]) max = i; - + return max; } @@ -689,7 +689,7 @@ inline unsigned int index_min(const Vector& v) for (unsigned int i = 1; i < v.size(); i++) if (v[i] < v[min]) min = i; - + return min; } @@ -702,7 +702,7 @@ inline T dot_prod(const Vector& a, const Vector& b) throw std::logic_error("Dotprod error: the vectors are not the same size"); for (unsigned int i = 0; i < a.size(); i++) sum += a[i] * b[i]; - + return sum; } @@ -716,7 +716,7 @@ inline Vector exp(const Vector& v) Vector tmp(v.size()); for (unsigned int i = 0; i < v.size(); i++) tmp[i] = exp(v[i]); - + return tmp; } @@ -726,7 +726,7 @@ inline Vector log(const Vector& v) Vector tmp(v.size()); for (unsigned int i = 0; i < v.size(); i++) tmp[i] = log(v[i]); - + return tmp; } @@ -736,7 +736,7 @@ inline Vector vec_sqrt(const Vector& v) Vector tmp(v.size()); for (unsigned int i = 0; i < v.size(); i++) tmp[i] = sqrt(v[i]); - + return tmp; } @@ -746,7 +746,7 @@ inline Vector pow(const Vector& v, double a) Vector tmp(v.size()); for (unsigned int i = 0; i < v.size(); i++) tmp[i] = pow(v[i], a); - + return tmp; } @@ -756,7 +756,7 @@ inline Vector abs(const Vector& v) Vector tmp(v.size()); for (unsigned int i = 0; i < v.size(); i++) tmp[i] = (T)fabs(v[i]); - + return tmp; } @@ -766,7 +766,7 @@ inline Vector sign(const Vector& v) Vector tmp(v.size()); for (unsigned int i = 0; i < v.size(); i++) tmp[i] = v[i] > 0 ? +1 : v[i] == 0 ? 0 : -1; - + return tmp; } @@ -775,7 +775,7 @@ inline unsigned int partition(Vector& v, unsigned int begin, unsigned int end { unsigned int i = begin + 1, j = begin + 1; T pivot = v[begin]; - while (j <= end) + while (j <= end) { if (v[j] < pivot) { std::swap(v[i], v[j]); @@ -787,7 +787,7 @@ inline unsigned int partition(Vector& v, unsigned int begin, unsigned int end v[i - 1] = pivot; return i - 2; } - + template inline void quicksort(Vector& v, unsigned int begin, unsigned int end) @@ -804,9 +804,9 @@ template inline Vector sort(const Vector& v) { Vector tmp(v); - + quicksort(tmp, 0, tmp.size() - 1); - + return tmp; } @@ -814,8 +814,8 @@ template inline Vector rank(const Vector& v) { Vector tmp(v); - Vector tmp_rank(0.0, v.size()); - + Vector tmp_rank(0.0, v.size()); + for (unsigned int i = 0; i < tmp.size(); i++) { unsigned int smaller = 0, equal = 0; @@ -835,14 +835,14 @@ inline Vector rank(const Vector& v) tmp_rank[i] /= (double)(equal + 1); } } - + return tmp_rank; } //enum MType { DIAG }; template -class Matrix +class Matrix { public: Matrix(); // Default constructor @@ -850,24 +850,24 @@ class Matrix Matrix(const T& a, const unsigned int n, const unsigned int m); // Initialize the content to constant a Matrix(MType t, const T& a, const T& o, const unsigned int n, const unsigned int m); Matrix(MType t, const Vector& v, const T& o, const unsigned int n, const unsigned int m); - Matrix(const T* a, const unsigned int n, const unsigned int m); // Initialize to array + Matrix(const T* a, const unsigned int n, const unsigned int m); // Initialize to array Matrix(const Matrix& rhs); // Copy constructor ~Matrix(); // destructor - + inline T* operator[](const unsigned int& i) { return v[i]; } // Subscripting: row i inline const T* operator[](const unsigned int& i) const { return v[i]; }; // const subsctipting - + inline void resize(const unsigned int n, const unsigned int m); inline void resize(const T& a, const unsigned int n, const unsigned int m); - - - inline Vector extractRow(const unsigned int i) const; + + + inline Vector extractRow(const unsigned int i) const; inline Vector extractColumn(const unsigned int j) const; inline Vector extractDiag() const; inline Matrix extractRows(const std::set& indexes) const; inline Matrix extractColumns(const std::set& indexes) const; inline Matrix extract(const std::set& r_indexes, const std::set& c_indexes) const; - + inline void set(const T* a, unsigned int n, unsigned int m); inline void set(const std::set& r_indexes, const std::set& c_indexes, const Matrix& m); inline void setRow(const unsigned int index, const Vector& v); @@ -876,11 +876,11 @@ class Matrix inline void setColumn(const unsigned int index, const Vector& v); inline void setColumn(const unsigned int index, const Matrix& v); inline void setColumns(const std::set& indexes, const Matrix& m); - - + + inline unsigned int nrows() const { return n; } // number of rows inline unsigned int ncols() const { return m; } // number of columns - + inline Matrix& operator=(const Matrix& rhs); // Assignment operator inline Matrix& operator=(const T& a); // Assign to every element value a inline Matrix& operator+=(const Matrix& rhs); @@ -901,7 +901,7 @@ class Matrix }; template -Matrix::Matrix() +Matrix::Matrix() : n(0), m(0), v(0) {} @@ -928,71 +928,71 @@ Matrix::Matrix(const T& a, unsigned int n, unsigned int m) v[i][j] = a; } -template -Matrix::Matrix(const T* a, unsigned int n, unsigned int m) +template +Matrix::Matrix(const T* a, unsigned int n, unsigned int m) : v(new T*[n]) -{ +{ this->n = n; this->m = m; - v[0] = new T[m * n]; + v[0] = new T[m * n]; for (unsigned int i = 1; i < n; i++) - v[i] = v[i - 1] + m; + v[i] = v[i - 1] + m; for (unsigned int i = 0; i < n; i++) for (unsigned int j = 0; j < m; j++) - v[i][j] = *a++; -} + v[i][j] = *a++; +} -template -Matrix::Matrix(MType t, const T& a, const T& o, unsigned int n, unsigned int m) +template +Matrix::Matrix(MType t, const T& a, const T& o, unsigned int n, unsigned int m) : v(new T*[n]) -{ +{ this->n = n; this->m = m; - v[0] = new T[m * n]; + v[0] = new T[m * n]; for (unsigned int i = 1; i < n; i++) - v[i] = v[i - 1] + m; + v[i] = v[i - 1] + m; switch (t) { case DIAG: for (unsigned int i = 0; i < n; i++) for (unsigned int j = 0; j < m; j++) if (i != j) - v[i][j] = o; + v[i][j] = o; else v[i][j] = a; break; default: throw std::logic_error("Matrix type not supported"); } -} +} -template -Matrix::Matrix(MType t, const Vector& a, const T& o, unsigned int n, unsigned int m) +template +Matrix::Matrix(MType t, const Vector& a, const T& o, unsigned int n, unsigned int m) : v(new T*[n]) -{ +{ this->n = n; this->m = m; - v[0] = new T[m * n]; + v[0] = new T[m * n]; for (unsigned int i = 1; i < n; i++) - v[i] = v[i - 1] + m; + v[i] = v[i - 1] + m; switch (t) { case DIAG: for (unsigned int i = 0; i < n; i++) for (unsigned int j = 0; j < m; j++) if (i != j) - v[i][j] = o; + v[i][j] = o; else v[i][j] = a[i]; break; default: throw std::logic_error("Matrix type not supported"); } -} +} template Matrix::Matrix(const Matrix& rhs) : v(new T*[rhs.n]) { n = rhs.n; m = rhs.m; - v[0] = new T[m * n]; + v[0] = new T[m * n]; for (unsigned int i = 1; i < n; i++) v[i] = v[i - 1] + m; for (unsigned int i = 0; i < n; i++) @@ -1000,89 +1000,89 @@ Matrix::Matrix(const Matrix& rhs) v[i][j] = rhs[i][j]; } -template -Matrix::~Matrix() -{ - if (v != 0) { - delete[] (v[0]); - delete[] (v); - } -} - -template -inline Matrix& Matrix::operator=(const Matrix &rhs) -// postcondition: normal assignment via copying has been performed; -// if matrix and rhs were different sizes, matrix -// has been resized to match the size of rhs -{ - if (this != &rhs) +template +Matrix::~Matrix() +{ + if (v != 0) { + delete[] (v[0]); + delete[] (v); + } +} + +template +inline Matrix& Matrix::operator=(const Matrix &rhs) +// postcondition: normal assignment via copying has been performed; +// if matrix and rhs were different sizes, matrix +// has been resized to match the size of rhs +{ + if (this != &rhs) { resize(rhs.n, rhs.m); for (unsigned int i = 0; i < n; i++) for (unsigned int j = 0; j < m; j++) - v[i][j] = rhs[i][j]; - } - return *this; -} - -template -inline Matrix& Matrix::operator=(const T& a) // assign a to every element -{ + v[i][j] = rhs[i][j]; + } + return *this; +} + +template +inline Matrix& Matrix::operator=(const T& a) // assign a to every element +{ for (unsigned int i = 0; i < n; i++) for (unsigned int j = 0; j < m; j++) - v[i][j] = a; - return *this; -} + v[i][j] = a; + return *this; +} -template -inline void Matrix::resize(const unsigned int n, const unsigned int m) +template +inline void Matrix::resize(const unsigned int n, const unsigned int m) { if (n == this->n && m == this->m) return; - if (v != 0) - { - delete[] (v[0]); - delete[] (v); - } + if (v != 0) + { + delete[] (v[0]); + delete[] (v); + } this->n = n; this->m = m; - v = new T*[n]; - v[0] = new T[m * n]; + v = new T*[n]; + v[0] = new T[m * n]; for (unsigned int i = 1; i < n; i++) v[i] = v[i - 1] + m; -} +} -template -inline void Matrix::resize(const T& a, const unsigned int n, const unsigned int m) +template +inline void Matrix::resize(const T& a, const unsigned int n, const unsigned int m) { resize(n, m); for (unsigned int i = 0; i < n; i++) for (unsigned int j = 0; j < m; j++) v[i][j] = a; -} +} -template +template inline Vector Matrix::extractRow(const unsigned int i) const { if (i >= n) throw std::logic_error("Error in extractRow: trying to extract a row out of matrix bounds"); Vector tmp(v[i], m); - + return tmp; } -template +template inline Vector Matrix::extractColumn(const unsigned int j) const { if (j >= m) throw std::logic_error("Error in extractRow: trying to extract a row out of matrix bounds"); Vector tmp(n); - + for (unsigned int i = 0; i < n; i++) tmp[i] = v[i][j]; - + return tmp; } @@ -1090,22 +1090,22 @@ template inline Vector Matrix::extractDiag() const { unsigned int d = std::min(n, m); - + Vector tmp(d); - + for (unsigned int i = 0; i < d; i++) tmp[i] = v[i][i]; - + return tmp; - + } -template +template inline Matrix Matrix::extractRows(const std::set& indexes) const { Matrix tmp(indexes.size(), m); unsigned int i = 0; - + for (std::set::const_iterator el = indexes.begin(); el != indexes.end(); el++) { for (unsigned int j = 0; j < m; j++) @@ -1116,16 +1116,16 @@ inline Matrix Matrix::extractRows(const std::set& indexes) c } i++; } - + return tmp; } -template +template inline Matrix Matrix::extractColumns(const std::set& indexes) const { Matrix tmp(n, indexes.size()); unsigned int j = 0; - + for (std::set::const_iterator el = indexes.begin(); el != indexes.end(); el++) { for (unsigned int i = 0; i < n; i++) @@ -1136,16 +1136,16 @@ inline Matrix Matrix::extractColumns(const std::set& indexes } j++; } - + return tmp; } -template +template inline Matrix Matrix::extract(const std::set& r_indexes, const std::set& c_indexes) const { Matrix tmp(r_indexes.size(), c_indexes.size()); unsigned int i = 0, j; - + for (std::set::const_iterator r_el = r_indexes.begin(); r_el != r_indexes.end(); r_el++) { if (*r_el >= n) @@ -1160,13 +1160,13 @@ inline Matrix Matrix::extract(const std::set& r_indexes, con } i++; } - + return tmp; } -template +template inline void Matrix::setRow(unsigned int i, const Vector& a) -{ +{ if (i >= n) throw std::logic_error("Error in setRow: trying to set a row out of matrix bounds"); if (this->m != a.size()) @@ -1175,9 +1175,9 @@ inline void Matrix::setRow(unsigned int i, const Vector& a) v[i][j] = a[j]; } -template +template inline void Matrix::setRow(unsigned int i, const Matrix& a) -{ +{ if (i >= n) throw std::logic_error("Error in setRow: trying to set a row out of matrix bounds"); if (this->m != a.ncols()) @@ -1188,11 +1188,11 @@ inline void Matrix::setRow(unsigned int i, const Matrix& a) v[i][j] = a[0][j]; } -template +template inline void Matrix::setRows(const std::set& indexes, const Matrix& m) { unsigned int i = 0; - + if (indexes.size() != m.nrows() || this->m != m.ncols()) throw std::logic_error("Error setting matrix rows: ranges are not compatible"); for (std::set::const_iterator el = indexes.begin(); el != indexes.end(); el++) @@ -1207,9 +1207,9 @@ inline void Matrix::setRows(const std::set& indexes, const Matr } } -template +template inline void Matrix::setColumn(unsigned int j, const Vector& a) -{ +{ if (j >= m) throw std::logic_error("Error in setColumn: trying to set a column out of matrix bounds"); if (this->n != a.size()) @@ -1218,9 +1218,9 @@ inline void Matrix::setColumn(unsigned int j, const Vector& a) v[i][j] = a[i]; } -template +template inline void Matrix::setColumn(unsigned int j, const Matrix& a) -{ +{ if (j >= m) throw std::logic_error("Error in setColumn: trying to set a column out of matrix bounds"); if (this->n != a.nrows()) @@ -1232,11 +1232,11 @@ inline void Matrix::setColumn(unsigned int j, const Matrix& a) } -template +template inline void Matrix::setColumns(const std::set& indexes, const Matrix& a) { unsigned int j = 0; - + if (indexes.size() != a.ncols() || this->n != a.nrows()) throw std::logic_error("Error setting matrix columns: ranges are not compatible"); for (std::set::const_iterator el = indexes.begin(); el != indexes.end(); el++) @@ -1251,13 +1251,13 @@ inline void Matrix::setColumns(const std::set& indexes, const M } } -template +template inline void Matrix::set(const std::set& r_indexes, const std::set& c_indexes, const Matrix& a) { unsigned int i = 0, j; if (c_indexes.size() != a.ncols() || r_indexes.size() != a.nrows()) throw std::logic_error("Error setting matrix elements: ranges are not compatible"); - + for (std::set::const_iterator r_el = r_indexes.begin(); r_el != r_indexes.end(); r_el++) { if (*r_el >= n) @@ -1274,7 +1274,7 @@ inline void Matrix::set(const std::set& r_indexes, const std::s } } -template +template inline void Matrix::set(const T* a, unsigned int n, unsigned int m) { if (this->n != n || this->m != m) @@ -1301,7 +1301,7 @@ Matrix operator+(const Matrix& lhs, const Matrix& rhs) for (unsigned int i = 0; i < lhs.nrows(); i++) for (unsigned int j = 0; j < lhs.ncols(); j++) tmp[i][j] = lhs[i][j] + rhs[i][j]; - + return tmp; } @@ -1312,7 +1312,7 @@ Matrix operator+(const Matrix& lhs, const T& a) for (unsigned int i = 0; i < lhs.nrows(); i++) for (unsigned int j = 0; j < lhs.ncols(); j++) tmp[i][j] = lhs[i][j] + a; - + return tmp; } @@ -1323,7 +1323,7 @@ Matrix operator+(const T& a, const Matrix& rhs) for (unsigned int i = 0; i < rhs.nrows(); i++) for (unsigned int j = 0; j < rhs.ncols(); j++) tmp[i][j] = a + rhs[i][j]; - + return tmp; } @@ -1335,7 +1335,7 @@ inline Matrix& Matrix::operator+=(const Matrix& rhs) for (unsigned int i = 0; i < n; i++) for (unsigned int j = 0; j < m; j++) v[i][j] += rhs[i][j]; - + return *this; } @@ -1345,13 +1345,13 @@ inline Matrix& Matrix::operator+=(const T& a) for (unsigned int i = 0; i < n; i++) for (unsigned int j = 0; j < m; j++) v[i][j] += a; - + return *this; } template Matrix operator-(const Matrix& rhs) -{ +{ return (T)(-1) * rhs; } @@ -1364,7 +1364,7 @@ Matrix operator-(const Matrix& lhs, const Matrix& rhs) for (unsigned int i = 0; i < lhs.nrows(); i++) for (unsigned int j = 0; j < lhs.ncols(); j++) tmp[i][j] = lhs[i][j] - rhs[i][j]; - + return tmp; } @@ -1375,7 +1375,7 @@ Matrix operator-(const Matrix& lhs, const T& a) for (unsigned int i = 0; i < lhs.nrows(); i++) for (unsigned int j = 0; j < lhs.ncols(); j++) tmp[i][j] = lhs[i][j] - a; - + return tmp; } @@ -1386,7 +1386,7 @@ Matrix operator-(const T& a, const Matrix& rhs) for (unsigned int i = 0; i < rhs.nrows(); i++) for (unsigned int j = 0; j < rhs.ncols(); j++) tmp[i][j] = a - rhs[i][j]; - + return tmp; } @@ -1398,7 +1398,7 @@ inline Matrix& Matrix::operator-=(const Matrix& rhs) for (unsigned int i = 0; i < n; i++) for (unsigned int j = 0; j < m; j++) v[i][j] -= rhs[i][j]; - + return *this; } @@ -1408,7 +1408,7 @@ inline Matrix& Matrix::operator-=(const T& a) for (unsigned int i = 0; i < n; i++) for (unsigned int j = 0; j < m; j++) v[i][j] -= a; - + return *this; } @@ -1421,7 +1421,7 @@ Matrix operator*(const Matrix& lhs, const Matrix& rhs) for (unsigned int i = 0; i < lhs.nrows(); i++) for (unsigned int j = 0; j < lhs.ncols(); j++) tmp[i][j] = lhs[i][j] * rhs[i][j]; - + return tmp; } @@ -1432,7 +1432,7 @@ Matrix operator*(const Matrix& lhs, const T& a) for (unsigned int i = 0; i < lhs.nrows(); i++) for (unsigned int j = 0; j < lhs.ncols(); j++) tmp[i][j] = lhs[i][j] * a; - + return tmp; } @@ -1443,7 +1443,7 @@ Matrix operator*(const T& a, const Matrix& rhs) for (unsigned int i = 0; i < rhs.nrows(); i++) for (unsigned int j = 0; j < rhs.ncols(); j++) tmp[i][j] = a * rhs[i][j]; - + return tmp; } @@ -1455,7 +1455,7 @@ inline Matrix& Matrix::operator*=(const Matrix& rhs) for (unsigned int i = 0; i < n; i++) for (unsigned int j = 0; j < m; j++) v[i][j] *= rhs[i][j]; - + return *this; } @@ -1465,7 +1465,7 @@ inline Matrix& Matrix::operator*=(const T& a) for (unsigned int i = 0; i < n; i++) for (unsigned int j = 0; j < m; j++) v[i][j] *= a; - + return *this; } @@ -1478,7 +1478,7 @@ Matrix operator/(const Matrix& lhs, const Matrix& rhs) for (unsigned int i = 0; i < lhs.nrows(); i++) for (unsigned int j = 0; j < lhs.ncols(); j++) tmp[i][j] = lhs[i][j] / rhs[i][j]; - + return tmp; } @@ -1489,7 +1489,7 @@ Matrix operator/(const Matrix& lhs, const T& a) for (unsigned int i = 0; i < lhs.nrows(); i++) for (unsigned int j = 0; j < lhs.ncols(); j++) tmp[i][j] = lhs[i][j] / a; - + return tmp; } @@ -1500,7 +1500,7 @@ Matrix operator/(const T& a, const Matrix& rhs) for (unsigned int i = 0; i < rhs.nrows(); i++) for (unsigned int j = 0; j < rhs.ncols(); j++) tmp[i][j] = a / rhs[i][j]; - + return tmp; } @@ -1512,7 +1512,7 @@ inline Matrix& Matrix::operator/=(const Matrix& rhs) for (unsigned int i = 0; i < n; i++) for (unsigned int j = 0; j < m; j++) v[i][j] /= rhs[i][j]; - + return *this; } @@ -1522,7 +1522,7 @@ inline Matrix& Matrix::operator/=(const T& a) for (unsigned int i = 0; i < n; i++) for (unsigned int j = 0; j < m; j++) v[i][j] /= a; - + return *this; } @@ -1533,7 +1533,7 @@ Matrix operator^(const Matrix& lhs, const T& a) for (unsigned int i = 0; i < lhs.nrows(); i++) for (unsigned int j = 0; j < lhs.ncols(); j++) tmp[i][j] = pow(lhs[i][j], a); - + return tmp; } @@ -1545,7 +1545,7 @@ inline Matrix& Matrix::operator^=(const Matrix& rhs) for (unsigned int i = 0; i < n; i++) for (unsigned int j = 0; j < m; j++) v[i][j] = pow(v[i][j], rhs[i][j]); - + return *this; } @@ -1556,7 +1556,7 @@ inline Matrix& Matrix::operator^=(const T& a) for (unsigned int i = 0; i < n; i++) for (unsigned int j = 0; j < m; j++) v[i][j] = pow(v[i][j], a); - + return *this; } @@ -1598,7 +1598,7 @@ inline bool operator!=(const Matrix& a, const Matrix& b) /** - Input/Output + Input/Output */ template std::ostream& operator<<(std::ostream& os, const Matrix& m) @@ -1610,7 +1610,7 @@ std::ostream& operator<<(std::ostream& os, const Matrix& m) os << std::setw(20) << std::setprecision(16) << m[i][j] << ", "; os << std::setw(20) << std::setprecision(16) << m[i][m.ncols() - 1] << std::endl; } - + return os; } @@ -1624,7 +1624,7 @@ std::istream& operator>>(std::istream& is, Matrix& m) for (unsigned int i = 0; i < rows; i++) for (unsigned int j = 0; j < cols; j++) is >> m[i][j] >> comma; - + return is; } @@ -1659,7 +1659,7 @@ void svd(const Matrix& A, Matrix& U, Vector& W, Matrix& V) V.resize(n, n); T anorm, c, f, g, h, s, scale, x, y, z; g = scale = anorm = (T)0.0; - + // Householder reduction to bidiagonal form for (i = 0; i < n; i++) { @@ -1705,7 +1705,7 @@ void svd(const Matrix& A, Matrix& U, Vector& W, Matrix& V) for (k = l; k < n; k++) { U[i][k] /= scale; - s += U[i][k] * U[i][k]; + s += U[i][k] * U[i][k]; } f = U[i][l]; g = -sign(f) * sqrt(s); @@ -1730,7 +1730,7 @@ void svd(const Matrix& A, Matrix& U, Vector& W, Matrix& V) // Accumulation of right-hand transformations for (i = n - 1; i >= 0; i--) { - if (i < n - 1) + if (i < n - 1) { if (g != (T)0.0) { @@ -1867,7 +1867,7 @@ void svd(const Matrix& A, Matrix& U, Vector& W, Matrix& V) V[jj][i] = z * c - x * s; } z = dist(f, h); - W[j] = z; + W[j] = z; if (z != 0) // Rotation can be arbitrary if z = 0 { z = (T)1.0 / z; @@ -1888,7 +1888,7 @@ void svd(const Matrix& A, Matrix& U, Vector& W, Matrix& V) rv1[k] = f; W[k] = x; } - } + } } template @@ -1903,7 +1903,7 @@ Matrix pinv(const Matrix& A) e.reset(i); tmp.setColumn(i, dot_prod(dot_prod(dot_prod(V, Matrix(DIAG, 1.0 / W, 0.0, W.size(), W.size())), t(U)), e)); } - + return tmp; } @@ -1917,7 +1917,7 @@ int lu(const Matrix& A, Matrix& LU, Vector& index) Vector d(n); LU = A; index.resize(n); - + ex = 1; for (i = 0; i < n; i++) { @@ -1953,7 +1953,7 @@ int lu(const Matrix& A, Matrix& LU, Vector& index) for (j = 0; j < n; j++) std::swap(LU[k][j], LU[p][j]); } - + for (i = k + 1; i < n; i++) { LU[i][k] /= LU[k][k]; @@ -1963,7 +1963,7 @@ int lu(const Matrix& A, Matrix& LU, Vector& index) } if (LU[n - 1][n - 1] == (T)0.0) std::logic_error("Error in LU decomposition: matrix was singular"); - + return ex; } @@ -1978,10 +1978,10 @@ Vector lu_solve(const Matrix& LU, const Vector& b, Vector Vector x((T)0.0, n); int i, j, p; T sum; - + p = index[0]; x[0] = b[p]; - + for (i = 1; i < n; i++) { sum = (T)0.0; @@ -2011,11 +2011,11 @@ template Matrix lu_inverse(const Matrix& A) { if (A.ncols() != A.nrows()) - throw std::logic_error("Error in LU invert: matrix must be squared"); + throw std::logic_error("Error in LU invert: matrix must be squared"); unsigned int n = A.ncols(); Matrix A1(n, n), LU; Vector index; - + lu(A, LU, index); CanonicalBaseVector e(0, n); for (unsigned i = 0; i < n; i++) @@ -2023,7 +2023,7 @@ Matrix lu_inverse(const Matrix& A) e.reset(i); A1.setColumn(i, lu_solve(LU, e, index)); } - + return A1; } @@ -2031,25 +2031,25 @@ template T lu_det(const Matrix& A) { if (A.ncols() != A.nrows()) - throw std::logic_error("Error in LU determinant: matrix must be squared"); + throw std::logic_error("Error in LU determinant: matrix must be squared"); unsigned int d; Matrix LU; Vector index; - + d = lu(A, LU, index); - + return d * prod(LU.extractDiag()); } template -void cholesky(const Matrix A, Matrix& LL) +void cholesky(const Matrix A, Matrix& LL) { if (A.ncols() != A.nrows()) throw std::logic_error("Error in Cholesky decomposition: matrix must be squared"); int n = A.ncols(); double sum; LL = A; - + for (unsigned int i = 0; i < n; i++) { for (unsigned int j = i; j < n; j++) @@ -2057,7 +2057,7 @@ void cholesky(const Matrix A, Matrix& LL) sum = LL[i][j]; for (int k = i - 1; k >= 0; k--) sum -= LL[i][k] * LL[j][k]; - if (i == j) + if (i == j) { if (sum <= 0.0) throw std::logic_error("Error in Cholesky decomposition: matrix is not postive definite"); @@ -2068,15 +2068,15 @@ void cholesky(const Matrix A, Matrix& LL) } for (unsigned int k = i + 1; k < n; k++) LL[i][k] = LL[k][i]; - } + } } template -Matrix cholesky(const Matrix A) +Matrix cholesky(const Matrix A) { Matrix LL; cholesky(A, LL); - + return LL; } @@ -2089,12 +2089,12 @@ Vector cholesky_solve(const Matrix& LL, const Vector& b) if (b.size() != n) throw std::logic_error("Error in Cholesky decomposition: b vector must be of the same dimensions of LU matrix"); Vector x, y; - + /* Solve L * y = b */ forward_elimination(LL, y, b); /* Solve L^T * x = y */ backward_elimination(LL, x, y); - + return x; } @@ -2113,7 +2113,7 @@ void forward_elimination(const Matrix& L, Vector& y, const Vector b) throw std::logic_error("Error in Forward elimination: b vector must be of the same dimensions of L matrix"); unsigned int n = b.size(); y.resize(n); - + y[0] = b[0] / L[0][0]; for (unsigned int i = 1; i < n; i++) { @@ -2129,7 +2129,7 @@ Vector forward_elimination(const Matrix& L, const Vector b) { Vector y; forward_elimination(L, y, b); - + return y; } @@ -2142,7 +2142,7 @@ void backward_elimination(const Matrix& U, Vector& x, const Vector& y) throw std::logic_error("Error in Backward elimination: b vector must be of the same dimensions of U matrix"); int n = y.size(); x.resize(n); - + x[n - 1] = y[n - 1] / U[n - 1][n - 1]; for (int i = n - 2; i >= 0; i--) { @@ -2158,7 +2158,7 @@ Vector backward_elimination(const Matrix& U, const Vector y) { Vector x; forward_elimination(U, x, y); - + return x; } @@ -2166,7 +2166,7 @@ Vector backward_elimination(const Matrix& U, const Vector y) #define det lu_det #define inverse lu_inverse -#define solve lu_solve +//#define solve lu_solve /* Random */ @@ -2252,7 +2252,7 @@ Vector mean(const Matrix& m) res[j] += m[i][j]; res[j] /= m.nrows(); } - + return res; } @@ -2266,7 +2266,7 @@ Vector r_mean(const Matrix& m) res[i] += m[i][j]; res[i] /= m.nrows(); } - + return res; } @@ -2287,7 +2287,7 @@ Vector var(const Matrix& m, bool sample_correction = false) unsigned int n = m.nrows(); double sum, ssum; for (unsigned int j = 0; j < m.ncols(); j++) - { + { sum = (T)0.0; ssum = (T)0.0; for (unsigned int i = 0; i < m.nrows(); i++) { @@ -2297,9 +2297,9 @@ Vector var(const Matrix& m, bool sample_correction = false) if (!sample_correction) res[j] = (ssum / n) - (sum / n) * (sum / n); else - res[j] = n * ((ssum / n) - (sum / n) * (sum / n)) / (n - 1); + res[j] = n * ((ssum / n) - (sum / n) * (sum / n)) / (n - 1); } - + return res; } @@ -2316,7 +2316,7 @@ Vector r_var(const Matrix& m, bool sample_correction = false) double sum, ssum; unsigned int n = m.ncols(); for (unsigned int i = 0; i < m.nrows(); i++) - { + { sum = 0.0; ssum = 0.0; for (unsigned int j = 0; j < m.ncols(); j++) { @@ -2328,7 +2328,7 @@ Vector r_var(const Matrix& m, bool sample_correction = false) else res[i] = n * ((ssum / n) - (sum / n) * (sum / n)) / (n - 1); } - + return res; } @@ -2350,7 +2350,7 @@ Vector max(const Matrix& m) value = std::max(m[i][j], value); res[j] = value; } - + return res; } @@ -2366,7 +2366,7 @@ Vector r_max(const Matrix& m) value = std::max(m[i][j], value); res[i] = value; } - + return res; } @@ -2382,7 +2382,7 @@ Vector min(const Matrix& m) value = std::min(m[i][j], value); res[j] = value; } - + return res; } @@ -2398,7 +2398,7 @@ Vector r_min(const Matrix& m) value = std::min(m[i][j], value); res[i] = value; } - + return res; } @@ -2412,11 +2412,11 @@ template Matrix exp(const Matrix&m) { Matrix tmp(m.nrows(), m.ncols()); - + for (unsigned int i = 0; i < m.nrows(); i++) for (unsigned int j = 0; j < m.ncols(); j++) tmp[i][j] = exp(m[i][j]); - + return tmp; } @@ -2424,11 +2424,11 @@ template Matrix mat_sqrt(const Matrix&m) { Matrix tmp(m.nrows(), m.ncols()); - + for (unsigned int i = 0; i < m.nrows(); i++) for (unsigned int j = 0; j < m.ncols(); j++) tmp[i][j] = sqrt(m[i][j]); - + return tmp; } @@ -2443,7 +2443,7 @@ Matrix kron(const Vector& b, const Vector& a) for (unsigned int i = 0; i < b.size(); i++) for (unsigned int j = 0; j < a.size(); j++) tmp[i][j] = a[j] * b[i]; - + return tmp; } @@ -2454,7 +2454,7 @@ Matrix t(const Matrix& a) for (unsigned int i = 0; i < a.nrows(); i++) for (unsigned int j = 0; j < a.ncols(); j++) tmp[j][i] = a[i][j]; - + return tmp; } @@ -2471,7 +2471,7 @@ Matrix dot_prod(const Matrix& a, const Matrix& b) for (unsigned int k = 0; k < a.ncols(); k++) tmp[i][j] += a[i][k] * b[k][j]; } - + return tmp; } @@ -2487,7 +2487,7 @@ Matrix dot_prod(const Matrix& a, const Vector& b) for (unsigned int k = 0; k < a.ncols(); k++) tmp[i][0] += a[i][k] * b[k]; } - + return tmp; } @@ -2503,7 +2503,7 @@ Matrix dot_prod(const Vector& a, const Matrix& b) for (unsigned int k = 0; k < a.size(); k++) tmp[0][j] += a[k] * b[k][j]; } - + return tmp; } @@ -2513,8 +2513,8 @@ inline Matrix rank(const Matrix m) Matrix tmp(m.nrows(), m.ncols()); for (unsigned int j = 0; j < m.ncols(); j++) tmp.setColumn(j, rank(m.extractColumn(j))); - - return tmp; + + return tmp; } template @@ -2523,8 +2523,8 @@ inline Matrix r_rank(const Matrix m) Matrix tmp(m.nrows(), m.ncols()); for (unsigned int i = 0; i < m.nrows(); i++) tmp.setRow(i, rank(m.extractRow(i))); - - return tmp; + + return tmp; } } // namespace quadprogpp diff --git a/qp_solver/include/qp_solver/sequencequadraticproblemsolver.h b/qp_solver/include/qp_solver/sequencequadraticproblemsolver.h index ddb9d6c..c2fe9fc 100644 --- a/qp_solver/include/qp_solver/sequencequadraticproblemsolver.h +++ b/qp_solver/include/qp_solver/sequencequadraticproblemsolver.h @@ -1,10 +1,10 @@ #ifndef SEQUENCEQUADRATICPROBLEMSOLVER_H #define SEQUENCEQUADRATICPROBLEMSOLVER_H #include "qp_solver/quadraticproblemsolver.h" -#include "pose_optimization/PoseOptimizationProblem.hpp" -#include "qp_solver/pose_optimization/PoseOptimizationFunctionConstraints.hpp" -#include "qp_solver/pose_optimization/PoseOptimizationObjectiveFunction.hpp" -#include "qp_solver/pose_optimization/poseparameterization.h" +#include "free_gait_core/pose_optimization/PoseOptimizationProblem.hpp" +#include "free_gait_core/pose_optimization/PoseOptimizationFunctionConstraints.hpp" +#include "free_gait_core/pose_optimization/PoseOptimizationObjectiveFunction.hpp" +#include "free_gait_core/pose_optimization/poseparameterization.h" #include namespace sqp_solver { using namespace free_gait; diff --git a/qp_solver/package.xml b/qp_solver/package.xml index 1417fd2..1a1f328 100644 --- a/qp_solver/package.xml +++ b/qp_solver/package.xml @@ -52,11 +52,14 @@ roscpp grid_map_core std_utils + quadruped_model + roscpp roscpp grid_map_core std_utils - + quadruped_model + diff --git a/qp_solver/src/Array.cc b/qp_solver/src/Array.cc index 667213e..4c48dc3 100644 --- a/qp_solver/src/Array.cc +++ b/qp_solver/src/Array.cc @@ -1,11 +1,11 @@ // $Id: Array.cc 201 2008-05-18 19:47:38Z digasper $ -// This file is part of QuadProg++: -// Copyright (C) 2006--2009 Luca Di Gaspero. +// This file is part of QuadProg++: +// Copyright (C) 2006--2009 Luca Di Gaspero. // // This software may be modified and distributed under the terms // of the MIT license. See the LICENSE file for details. -#include "qp_solver/Array.h" +#include "qp_solver/qp_array.h" /** Index utilities @@ -18,7 +18,7 @@ std::set seq(unsigned int s, unsigned int e) std::set tmp; for (unsigned int i = s; i <= e; i++) tmp.insert(i); - + return tmp; } @@ -26,7 +26,7 @@ std::set singleton(unsigned int i) { std::set tmp; tmp.insert(i); - + return tmp; } diff --git a/qp_solver/src/qp_solve_test.cpp b/qp_solver/src/qp_solve_test.cpp index 2e5210b..809f74d 100644 --- a/qp_solver/src/qp_solve_test.cpp +++ b/qp_solver/src/qp_solve_test.cpp @@ -1,11 +1,11 @@ #include "qp_solver/quadraticproblemsolver.h" -#include "qp_solver/pose_optimization/PoseOptimizationQP.hpp" -#include "qp_solver/pose_optimization/PoseOptimizationGeometric.hpp" -#include "qp_solver/pose_optimization/PoseConstraintsChecker.hpp" -#include "qp_solver/pose_optimization/PoseOptimizationSQP.hpp" +#include "free_gait_core/pose_optimization/PoseOptimizationQP.hpp" +#include "free_gait_core/pose_optimization/PoseOptimizationGeometric.hpp" +#include "free_gait_core/pose_optimization/PoseConstraintsChecker.hpp" +#include "free_gait_core/pose_optimization/PoseOptimizationSQP.hpp" #include "free_gait_core/TypeDefs.hpp" #include "AdapterDummy.hpp" - +//#include "Eigen/QR" #include "iostream" #include //#include "nlopt.h" @@ -74,6 +74,13 @@ int main(int argc, char *argv[]) AdapterDummy adapter; Pose result; + LimbEnum le = LimbEnum::RF_LEG; +// cout<(le); + BranchEnum be = static_cast(le_int + 1); + BranchEnum be_map = quadruped_model::QuadrupedModel::QuadrupedDescription::mapEnums(le); +// cout< @@ -150,8 +150,8 @@ TEST(PoseOptimizationSQP, OptimizationSquareUp) kindr::assertNear(expectedPosition.vector(), result.getPosition().vector(), 1e-3, KINDR_SOURCE_FILE_POS); // Translation only. -// result.setIdentity(); - expectedPosition += Position(0.5, 0.25, 0.0); + result.setIdentity(); + expectedPosition += Position(0.3, 0.2, 0.0); result = Pose(expectedPosition, RotationQuaternion()); Stance translatedStance(stance); for (auto& stance : translatedStance) { diff --git a/quadruped_model/CMakeLists.txt b/quadruped_model/CMakeLists.txt new file mode 100644 index 0000000..30dbb45 --- /dev/null +++ b/quadruped_model/CMakeLists.txt @@ -0,0 +1,219 @@ +cmake_minimum_required(VERSION 2.8.3) +project(quadruped_model) + +## Compile as C++11, supported in ROS Kinetic and newer + add_compile_options(-std=c++11) + +## 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 + roscpp + kdl_parser +# orocos_kdl +) + +## System dependencies are found with CMake's conventions +# find_package(Boost REQUIRED COMPONENTS system) +find_package(Eigen3 REQUIRED) +#find_package(RBDL REQUIRED) + +# Attempt to find catkinized kindr +find_package(kindr QUIET) +if(NOT kindr_FOUND) + # Attempt to find package-based kindr + find_package(PkgConfig REQUIRED) + pkg_check_modules(kindr kindr REQUIRED) +endif() +## 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() + +################################################ +## Declare ROS messages, services and actions ## +################################################ + +## To declare and build messages, services or actions from within this +## package, follow these steps: +## * Let MSG_DEP_SET be the set of packages whose message types you use in +## your messages/services/actions (e.g. std_msgs, actionlib_msgs, ...). +## * In the file package.xml: +## * add a build_depend tag for "message_generation" +## * add a build_depend and a exec_depend tag for each package in MSG_DEP_SET +## * If MSG_DEP_SET isn't empty the following dependency has been pulled in +## but can be declared for certainty nonetheless: +## * add a exec_depend tag for "message_runtime" +## * In this file (CMakeLists.txt): +## * add "message_generation" and every package in MSG_DEP_SET to +## find_package(catkin REQUIRED COMPONENTS ...) +## * add "message_runtime" and every package in MSG_DEP_SET to +## catkin_package(CATKIN_DEPENDS ...) +## * uncomment the add_*_files sections below as needed +## and list every .msg/.srv/.action file to be processed +## * uncomment the generate_messages entry below +## * add every package in MSG_DEP_SET to generate_messages(DEPENDENCIES ...) + +## Generate messages in the 'msg' folder +# add_message_files( +# FILES +# Message1.msg +# Message2.msg +# ) + +## Generate services in the 'srv' folder +# add_service_files( +# FILES +# Service1.srv +# Service2.srv +# ) + +## Generate actions in the 'action' folder +# add_action_files( +# FILES +# Action1.action +# Action2.action +# ) + +## Generate added messages and services with any dependencies listed here +# generate_messages( +# DEPENDENCIES +# std_msgs # Or other packages containing msgs +# ) + +################################################ +## Declare ROS dynamic reconfigure parameters ## +################################################ + +## To declare and build dynamic reconfigure parameters within this +## package, follow these steps: +## * In the file package.xml: +## * add a build_depend and a exec_depend tag for "dynamic_reconfigure" +## * In this file (CMakeLists.txt): +## * add "dynamic_reconfigure" to +## find_package(catkin REQUIRED COMPONENTS ...) +## * uncomment the "generate_dynamic_reconfigure_options" section below +## and list every .cfg file to be processed + +## Generate dynamic reconfigure parameters in the 'cfg' folder +# generate_dynamic_reconfigure_options( +# cfg/DynReconf1.cfg +# cfg/DynReconf2.cfg +# ) + +################################### +## catkin specific configuration ## +################################### +## The catkin_package macro generates cmake config files for your package +## Declare things to be passed to dependent projects +## INCLUDE_DIRS: uncomment this if your package contains header files +## LIBRARIES: libraries you create in this project that dependent projects also need +## CATKIN_DEPENDS: catkin_packages dependent projects also need +## DEPENDS: system dependencies of this project that dependent projects also need +catkin_package( + INCLUDE_DIRS + include + ${EIGEN3_INCLUDE_DIR} + LIBRARIES quadruped_model + CATKIN_DEPENDS + roscpp + kdl_parser +# orocos_kdl +# DEPENDS system_lib +) + +########### +## Build ## +########### + +## Specify additional locations of header files +## Your package locations should be listed before other locations +include_directories( + include + ${catkin_INCLUDE_DIRS} + ${EIGEN3_INCLUDE_DIR} +# ${RBDL_INCLUDE_DIR} +) + +## Declare a C++ library + add_library(${PROJECT_NAME} + src/QuadrupedModel.cpp + src/quadrupedkinematics.cpp + src/quadruped_state.cpp + ) + +## Add cmake target dependencies of the library +## as an example, code may need to be generated before libraries +## either from message generation or dynamic reconfigure +# add_dependencies(${PROJECT_NAME} ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS}) + +## Declare a C++ executable +## With catkin_make all packages are built within a single CMake context +## The recommended prefix ensures that target names across packages don't collide +# add_executable(${PROJECT_NAME}_node src/quadruped_model_node.cpp) + +## Rename C++ executable without prefix +## The above recommended prefix causes long target names, the following renames the +## target back to the shorter version for ease of user use +## e.g. "rosrun someones_pkg node" instead of "rosrun someones_pkg someones_pkg_node" +# set_target_properties(${PROJECT_NAME}_node PROPERTIES OUTPUT_NAME node PREFIX "") + +## Add cmake target dependencies of the executable +## same as for the library above +# add_dependencies(${PROJECT_NAME}_node ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS}) + +## Specify libraries to link a library or executable target against +# target_link_libraries(${PROJECT_NAME}_node +# ${catkin_LIBRARIES} +# ) +target_link_libraries(${PROJECT_NAME} ${catkin_LIBRARIES}) + +add_executable(kinematics_test_node src/kinematicsTest.cpp) +target_link_libraries(kinematics_test_node ${PROJECT_NAME} ${catkin_LIBRARIES}) +############# +## 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 +# install(PROGRAMS +# scripts/my_python_script +# DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} +# ) + +## Mark executables and/or libraries for installation + install(TARGETS ${PROJECT_NAME} #${PROJECT_NAME}_node + ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} + LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} + RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} + ) + +## Mark cpp header files for installation + install(DIRECTORY include/${PROJECT_NAME}/ + DESTINATION ${CATKIN_PACKAGE_INCLUDE_DESTINATION} +# FILES_MATCHING PATTERN "*.h" +# PATTERN ".svn" EXCLUDE + ) + +## Mark other files for installation (e.g. launch and bag files, etc.) +# install(FILES +# # myfile1 +# # myfile2 +# DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION} +# ) + +############# +## Testing ## +############# + +## Add gtest based cpp test target and link libraries +# catkin_add_gtest(${PROJECT_NAME}-test test/test_quadruped_model.cpp) +# if(TARGET ${PROJECT_NAME}-test) +# target_link_libraries(${PROJECT_NAME}-test ${PROJECT_NAME}) +# endif() + +## Add folders to be run by python nosetests +# catkin_add_nosetests(test) diff --git a/quadruped_model/include/quadruped_model/QuadrupedModel.hpp b/quadruped_model/include/quadruped_model/QuadrupedModel.hpp new file mode 100644 index 0000000..4a40536 --- /dev/null +++ b/quadruped_model/include/quadruped_model/QuadrupedModel.hpp @@ -0,0 +1,149 @@ +#pragma once + +#include "kindr/Core" +#include +//#include "quadruped_model/quadrupedkinematics.h" + +struct EnumClassHash +{ + template + std::size_t operator()(T t) const + { + return static_cast(t); + } +}; + +using namespace romo; +namespace quadruped_model{ +typedef kindr::VectorTypeless GeneralizedCoordinates; +typedef kindr::VectorTypeless GeneralizedVelocities; +typedef kindr::VectorTypeless GeneralizedAccelerations; +typedef kindr::VectorTypeless JointPositionsLimb; +typedef kindr::VectorTypeless JointVelocitiesLimb; +typedef kindr::VectorTypeless JointAccelerationsLimb; +typedef kindr::VectorTypeless JointTorquesLimb; +typedef kindr::VectorTypeless JointPositions; +typedef kindr::VectorTypeless JointVelocities; +typedef kindr::VectorTypeless JointAccelerations; +typedef kindr::VectorTypeless JointTorques; + + +class QuadrupedModel +{ +private: + /* data */ +public: + QuadrupedModel(/* args */); + ~QuadrupedModel(); + + + class QuadrupedDescription + { + private: + /* data */ + public: + QuadrupedDescription(/* args */){}; + virtual ~QuadrupedDescription(); + enum class LimbEnum + { + LF_LEG, + RF_LEG, + LH_LEG, + RH_LEG, + }; + + enum class BranchEnum + { + BASE, + LF_LEG, + RF_LEG, + LH_LEG, + RH_LEG, + }; + enum class JointNodeEnum + { + LF_LEG_HAA, + LF_LEG_HFE, + LF_LEG_KFE, + RF_LEG_HAA, + RF_LEG_HFE, + RF_LEG_KFE, + LH_LEG_HAA, + LH_LEG_HFE, + LH_LEG_KFE, + RH_LEG_HAA, + RH_LEG_HFE, + RH_LEG_KFE, + }; + +// template +// static B mapEnums(L Enum) +// { +// int index = static_cast(Enum); +// index = index +1; +// return static_cast(index); +// } + static BranchEnum mapEnums(LimbEnum le) + { + int index = static_cast(le); + index = index +1; + return static_cast(index); + } + static int getNumDofLimb() + { + return 3; + } + + static int getLimbStartIndexInJ(LimbEnum limb) + { + switch (limb) { + case LimbEnum::LF_LEG: + return 0; + case LimbEnum::RF_LEG: + return 3; + case LimbEnum::RH_LEG: + return 6; + case LimbEnum::LH_LEG: + return 9; + } + } + }; + +}; + +//QuadrupedModel::QuadrupedModel(/* args */) +//{ +//} + +//QuadrupedModel::~QuadrupedModel() +//{ +//} +//class QuadrupedKinematics; +//class QuadrupedState //: public QuadrupedKinematics +//{ +//public: +// QuadrupedState(); +// ~QuadrupedState(); +// const Position getPositionWorldToBaseInWorldFrame() const; +// const RotationQuaternion getOrientationBaseToWorld() const; +// static JointPositions getJointPositions(); +// static JointVelocities getJointVelocities(); +// const LinearVelocity getLinearVelocityBaseInWorldFrame() const; +// const LocalAngularVelocity getAngularVelocityBaseInBaseFrame() const; + + +// bool setPoseBaseToWorld(const Pose pose); +// bool setPositionWorldToBaseInWorldFrame(const Position position); +// bool setOrientationBaseToWorld(const RotationQuaternion rotation); +// bool setLinearVelocityBaseInWorldFrame(const LinearVelocity linear_velocity); +// bool setAngularVelocityBaseInBaseFrame(const LocalAngularVelocity local_angular_velocity); +// bool setJointPositions(const JointPositions); +// bool setJointVelocities(const JointVelocities); +//private: +//// QuadrupedKinematics QK; +// Pose poseInWorldFrame_; +// static JointPositions joint_positions_; +// static JointVelocities joint_velocities_; +// Position positionWorldToBaseInWorldFrame_; +//}; +} diff --git a/free_gait_core/include/quadruped_model/common/typedefs.hpp b/quadruped_model/include/quadruped_model/common/typedefs.hpp similarity index 100% rename from free_gait_core/include/quadruped_model/common/typedefs.hpp rename to quadruped_model/include/quadruped_model/common/typedefs.hpp diff --git a/quadruped_model/include/quadruped_model/quadruped_state.h b/quadruped_model/include/quadruped_model/quadruped_state.h new file mode 100644 index 0000000..c21dd11 --- /dev/null +++ b/quadruped_model/include/quadruped_model/quadruped_state.h @@ -0,0 +1,62 @@ +#ifndef QUADRUPED_STATE_H +#define QUADRUPED_STATE_H +#include "quadruped_model/QuadrupedModel.hpp" +#include "quadruped_model/quadrupedkinematics.h" +namespace quadruped_model +{ +class QuadrupedState : public QuadrupedKinematics +{ +public: + typedef std::unordered_map FootPoseInBase; + typedef std::unordered_map limb_joints; + QuadrupedState(); + ~QuadrupedState(); + const Position getPositionWorldToBaseInWorldFrame() const; + const RotationQuaternion getOrientationBaseToWorld() const; + static JointPositions& getJointPositions(); + static JointVelocities& getJointVelocities(); + const LinearVelocity getLinearVelocityBaseInWorldFrame() const; + const LocalAngularVelocity getAngularVelocityBaseInBaseFrame() const; + + const Position getPositionWorldToFootInWorldFrame(const LimbEnum& limb); + const Position getPositionBaseToFootInBaseFrame(const LimbEnum& limb); + + Position getPositionBaseToFootInBaseFrame(const LimbEnum& limb, const JointPositionsLimb& jointPositions); + bool getLimbJointPositionsFromPositionBaseToFootInBaseFrame(const Position& positionBaseToFootInBaseFrame, + const LimbEnum& limb, + JointPositionsLimb& jointPositions); + LinearVelocity getEndEffectorLinearVelocityFromJointVelocities(const LimbEnum& limb, const JointVelocitiesLimb& jointVelocities); +// struct getJointPositions +// { +// Eigen::VectorXd joint_position_; +// Eigen::VectorXd vector() +// { +// return Eigen::VectorXd(joint_position_); +// } + +// void setSegment(Eigen::Index start, Eigen::Index n, JointPositionsLimb joint_position_limb) +// { +// joint_position_.segment(start, n) = joint_position_limb.vector(); +// } +// }; + void setCurrentLimbJoints(JointPositions all_joints_position); + bool setPoseBaseToWorld(const Pose pose); + bool setPositionWorldToBaseInWorldFrame(const Position position); + bool setOrientationBaseToWorld(const RotationQuaternion rotation); + bool setLinearVelocityBaseInWorldFrame(const LinearVelocity linear_velocity); + bool setAngularVelocityBaseInBaseFrame(const LocalAngularVelocity local_angular_velocity); + bool setJointPositions(const JointPositions joint_positions); + bool setJointVelocities(const JointVelocities joint_velocities); +private: +// QuadrupedKinematics QK; + limb_joints current_limb_joints_; + FootPoseInBase footPoseInBaseFrame_; + Pose poseInWorldFrame_; + static JointPositions joint_positions_, joint_positions_feedback_; + static JointVelocities joint_velocities_; + Position positionWorldToBaseInWorldFrame_; + RotationQuaternion orientationBaseToWorld_; +}; +}//namespace + +#endif // QUADRUPED_STATE_H diff --git a/quadruped_model/include/quadruped_model/quadrupedkinematics.h b/quadruped_model/include/quadruped_model/quadrupedkinematics.h new file mode 100644 index 0000000..4356518 --- /dev/null +++ b/quadruped_model/include/quadruped_model/quadrupedkinematics.h @@ -0,0 +1,86 @@ +#ifndef QUADRUPEDKINEMATICS_H +#define QUADRUPEDKINEMATICS_H + +#include "kdl_parser/kdl_parser.hpp" +#include "kdl/chainfksolver.hpp" +#include "kdl/chain.hpp" +#include "kdl/chainfksolverpos_recursive.hpp" +//#include "kdl/chainiksolver.hpp" +//#include "kdl/chainiksolverpos_nr.hpp" +//#include "kdl/chainiksolverpos_lma.hpp" +//#include "kdl/chainiksolverpos_nr_jl.hpp" +#include "kdl/chainjnttojacsolver.hpp" +#include "kdl/frames.hpp" +#include "kdl/frames_io.hpp" +#include "kdl/tree.hpp" +//#include "kdl/chainiksolvervel_wdls.hpp" + + +#include "kindr/Core" +#include "quadruped_model/QuadrupedModel.hpp" +#include "quadruped_model/common/typedefs.hpp" +//#include "free_gait_core/TypeDefs.hpp" + +#include "iostream" +#include "string.h" +#include "ros/ros.h" +#include "memory" +#include "unordered_map" +#include "utility" + +using namespace romo; +namespace quadruped_model +{ +//struct EnumClassHash +//{ +// template +// std::size_t operator()(T t) const +// { +// return static_cast(t); +// } +//}; + +using QD = quadruped_model::QuadrupedModel::QuadrupedDescription; + +using LimbEnum = QD::LimbEnum; +using BranchEnum = QD::BranchEnum; +using JointNodeEnum = QD::JointNodeEnum; +typedef std::unordered_map HipPoseInBase; +class QuadrupedKinematics +{ +public: +// typedef std::unordered_map HipPoseInBase; + + QuadrupedKinematics(); + ~QuadrupedKinematics(); + QuadrupedKinematics(const QuadrupedKinematics& other); +// QuadrupedKinematics& operator=(const QuadrupedKinematics& other); +// std::unique_ptr clone(s) + bool LoadRobotDescriptionFromFile(const std::string filename); + bool FowardKinematicsSolve(const JointPositionsLimb& joint_position, const LimbEnum& limb, Pose& cartisian_pose); +// bool InverseKinematicsSolve(Position foot_position, JointPositionsLimb& joint_positions); + bool InverseKinematicsSolve(const Position& foot_position, const LimbEnum& limb, + const JointPositionsLimb& joint_positions_last, + JointPositionsLimb& joint_positions); + bool AnalysticJacobian(const JointPositionsLimb& joint_positions, const LimbEnum& limb, Eigen::MatrixXd& jacobian); + double MapToPI(double q); + + bool setHipPoseInBase(const KDL::Chain& kdl_chain, const LimbEnum& limb); + Position getPositionFootToHipInHipFrame(const LimbEnum& limb, const Position& foot_position_in_base) const; + Position getPositionBaseToHipInBaseFrame(const LimbEnum& limb) const; + + KDL::Chain LF_Chain, RF_Chain,RH_Chain,LH_Chain; + HipPoseInBase hip_pose_in_base_; +private: + KDL::Tree tree_; + KDL::JntArray joint_positons_last_; +// HipPoseInBase hip_pose_in_base_; +// KDL::Chain LF_Chain, RF_Chain,RH_Chain,LH_Chain; +// KDL::TreeFkSolverPos_recursive tree_fk_solver_; +// std::unique_ptr lf_fk_solver_ptr, rf_fk_solver_ptr, rh_fk_solver_ptr, lh_fk_solver_ptr; +// std::unique_ptr lf_ik_solver_ptr; +// std::unique_ptr lf_ik_solver_nr_ptr; + +}; +}//namespace +#endif // QUADRUPEDKINEMATICS_H diff --git a/quadruped_model/package.xml b/quadruped_model/package.xml new file mode 100644 index 0000000..a068ede --- /dev/null +++ b/quadruped_model/package.xml @@ -0,0 +1,66 @@ + + + quadruped_model + 0.0.0 + The quadruped_model package + + + + + hitstar + + + + + + TODO + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + catkin + roscpp + roscpp + roscpp + kdl_parser + orocos_kdl + kdl_parser + orocos_kdl + + + + + + + + diff --git a/quadruped_model/src/QuadrupedModel.cpp b/quadruped_model/src/QuadrupedModel.cpp new file mode 100644 index 0000000..76cedce --- /dev/null +++ b/quadruped_model/src/QuadrupedModel.cpp @@ -0,0 +1,11 @@ +#include "quadruped_model/QuadrupedModel.hpp" +namespace quadruped_model { +QuadrupedModel::QuadrupedModel() +{ + +} +QuadrupedModel::~QuadrupedModel() +{ + +} +}; //namespace diff --git a/quadruped_model/src/kinematicsTest.cpp b/quadruped_model/src/kinematicsTest.cpp new file mode 100644 index 0000000..9154072 --- /dev/null +++ b/quadruped_model/src/kinematicsTest.cpp @@ -0,0 +1,37 @@ +#include +#include "quadruped_model/quadrupedkinematics.h" +//#include "geometry_msgs/Twist.h" +//#include "gazebo_msgs/ModelStates.h" +#include "rbdl/Model.h" +using namespace std; +using namespace quadruped_model; + +int main(int argc, char **argv) +{ + ros::init(argc, argv, "kinematicsTest"); + ros::NodeHandle nh; + Eigen::MatrixXd jacobian; + QuadrupedKinematics QK; +// QK.LoadRobotDescriptionFromFile("/home/hitstar/catkin_ws/src/quadruped_locomotion-dev/quadruped_model/urdf/simpledog.urdf"); + JointPositionsLimb joints(0,0,0); + Pose result; + HipPoseInBase test_enum; + test_enum[LimbEnum::RF_LEG] = Pose(Position(0,0,0), RotationQuaternion()); + if(QK.FowardKinematicsSolve(joints, LimbEnum::RF_LEG, result)) + { + EulerAnglesZyx eular_zyx(result.getRotation()); + cout<<"Kinematics solve result:"<(new KDL::ChainFkSolverPos_recursive(LF_Chain));//std::move(other.lf_fk_solver_ptr); +//// rf_fk_solver_ptr = std::unique_ptr(new KDL::ChainFkSolverPos_recursive(RF_Chain));//std::move(other.rf_fk_solver_ptr); +//// rh_fk_solver_ptr = std::unique_ptr(new KDL::ChainFkSolverPos_recursive(RH_Chain));//std::move(other.rh_fk_solver_ptr); +//// lh_fk_solver_ptr = std::unique_ptr(new KDL::ChainFkSolverPos_recursive(LH_Chain));//std::move(other.lh_fk_solver_ptr); +// return *this; +//} +QuadrupedKinematics::QuadrupedKinematics(const QuadrupedKinematics& other) + : tree_(other.tree_), + hip_pose_in_base_(other.hip_pose_in_base_), + LF_Chain(other.LF_Chain), + RF_Chain(other.RF_Chain), + RH_Chain(other.RH_Chain), + LH_Chain(other.LH_Chain) +{ + cout<<"QuadrupedKinematics Class has been Copied"<(new KDL::TreeFkSolverPos_recursive(tree_)); +// KDL::Chain chain; +// KDL::ChainFkSolverPos_recursive fk = KDL::ChainFkSolverPos_recursive(chain); +// KDL::TreeFkSolverPos_recursive fkt = KDL::TreeFkSolverPos_recursive(tree_); +// KDL::Chain LF_Chain, RF_Chain,RH_Chain,LH_Chain; + tree_.getChain("base_link", "lf_foot_Link", LF_Chain); + tree_.getChain("base_link", "rf_foot_Link", RF_Chain); + tree_.getChain("base_link", "lh_foot_Link", RH_Chain); + tree_.getChain("base_link", "rh_foot_Link", LH_Chain); + setHipPoseInBase(LF_Chain,LimbEnum::LF_LEG); + setHipPoseInBase(RF_Chain,LimbEnum::RF_LEG); + setHipPoseInBase(RH_Chain,LimbEnum::RH_LEG); + setHipPoseInBase(LH_Chain,LimbEnum::LH_LEG); +// cout<<"LF : "<(new KDL::ChainFkSolverPos_recursive(LF_Chain)); + + // LF_with_vitural_foot_joints_Chain = LF_Chain; +// tree_.getChain("base_link", "front_left_3_Link", LF_with_vitural_foot_joints_Chain); +// LF_with_vitural_foot_joints_Chain.addSegment(KDL::Segment("foot_roll_link",KDL::Joint("foot_roll_joint",KDL::Joint::RotX), KDL::Frame(KDL::Vector(0.25,0.0,-0.015)))); +// LF_with_vitural_foot_joints_Chain.addSegment(KDL::Segment("foot_pitch_link",KDL::Joint("foot_pitch_joint",KDL::Joint::RotY))); +// LF_with_vitural_foot_joints_Chain.addSegment(KDL::Segment("foot_yaw_link",KDL::Joint("foot_yaw_joint",KDL::Joint::RotZ))); +// double pose_error = 1E-3; +// double joint_error = 1E-5; +// int max_iterations = 500; +// lf_fk_solver_ptr = std::unique_ptr(new KDL::ChainFkSolverPos_recursive(LF_Chain)); +// rf_fk_solver_ptr = std::unique_ptr(new KDL::ChainFkSolverPos_recursive(RF_Chain)); +// lh_fk_solver_ptr = std::unique_ptr(new KDL::ChainFkSolverPos_recursive(LH_Chain)); +// rh_fk_solver_ptr = std::unique_ptr(new KDL::ChainFkSolverPos_recursive(RH_Chain)); + +// lf_ik_solver_ptr = std::unique_ptr(new KDL::ChainIkSolverPos_LMA(LF_with_vitural_foot_joints_Chain, pose_error,max_iterations,joint_error)); +// LF_with_vitural_foot_joints_Chain.getSegment(3).getFrameToTip(); +// cout<CartToJnt(q_init, foot_pose, joints_out)<0) +// { +// cout<<"Failed to solve Inverse kinematics problem"< + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/rqt_free_gait_action/CHANGELOG.rst b/rqt_free_gait_action/CHANGELOG.rst new file mode 100644 index 0000000..1a1fc28 --- /dev/null +++ b/rqt_free_gait_action/CHANGELOG.rst @@ -0,0 +1,11 @@ +^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Changelog for package rqt_free_gait_action +^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ + +Forthcoming +----------- + +0.1.1 (2017-01-31) +------------------ +* Initial release. +* Contributors: Samuel Bachmann diff --git a/rqt_free_gait_action/CMakeLists.txt b/rqt_free_gait_action/CMakeLists.txt new file mode 100644 index 0000000..f4e4151 --- /dev/null +++ b/rqt_free_gait_action/CMakeLists.txt @@ -0,0 +1,154 @@ +cmake_minimum_required(VERSION 2.8.3) +project(rqt_free_gait_action) + +# Flags +SET(CMAKE_CXX_FLAGS "-std=c++11 ${CMAKE_CXX_FLAGS}") + +# Load catkin and all dependencies required for this package +find_package(catkin REQUIRED COMPONENTS + free_gait_msgs + roscpp + rqt_gui + rqt_gui_cpp + std_srvs +) + +if ("${qt_gui_cpp_USE_QT_MAJOR_VERSION} " STREQUAL "5 ") + find_package(Qt5Widgets REQUIRED) +else() + find_package(Qt4 COMPONENTS QtCore QtGui REQUIRED) + include(${QT_USE_FILE}) +endif() + +set(${PROJECT_NAME}_SRCS + src/rqt_free_gait_action/Action.cpp + src/rqt_free_gait_action/ActionModel.cpp + src/rqt_free_gait_action/Collection.cpp + src/rqt_free_gait_action/CollectionModel.cpp + src/rqt_free_gait_action/FavoritePushButton.cpp + src/rqt_free_gait_action/FreeGaitActionPlugin.cpp + src/rqt_free_gait_action/WorkerThreadGetCollections.cpp + src/rqt_free_gait_action/WorkerThreadSendAction.cpp + src/rqt_free_gait_action/WorkerThreadSendActionSequence.cpp + src/rqt_free_gait_action/WorkerThreadUpdateTrigger.cpp +) + +set(${PROJECT_NAME}_HDRS_QT + include/rqt_free_gait_action/FavoritePushButton.h + include/rqt_free_gait_action/FreeGaitActionPlugin.h + include/rqt_free_gait_action/WorkerThreadGetCollections.h + include/rqt_free_gait_action/WorkerThreadSendAction.h + include/rqt_free_gait_action/WorkerThreadSendActionSequence.h + include/rqt_free_gait_action/WorkerThreadUpdateTrigger.h +) +set(${PROJECT_NAME}_HDRS + include/rqt_free_gait_action/Action.h + include/rqt_free_gait_action/ActionModel.h + include/rqt_free_gait_action/Collection.h + include/rqt_free_gait_action/CollectionModel.h +) + +set(${PROJECT_NAME}_UIS + resource/FreeGaitActionPlugin.ui +) + +set(${PROJECT_NAME}_QRC + resource/resources.qrc +) + +set(ui_INCLUDE_DIR + "${CATKIN_DEVEL_PREFIX}/${CATKIN_GLOBAL_INCLUDE_DESTINATION}/${PROJECT_NAME}" +) + +set(${PROJECT_NAME}_INCLUDE_DIRECTORIES + include + ${ui_INCLUDE_DIR}/.. +) + +if(NOT EXISTS ${ui_INCLUDE_DIR}) + file(MAKE_DIRECTORY ${ui_INCLUDE_DIR}) +endif() + +catkin_package( + INCLUDE_DIRS include +# LIBRARIES ${PROJECT_NAME} + CATKIN_DEPENDS + free_gait_msgs + roscpp + rqt_gui + rqt_gui_cpp +) + +if ("${qt_gui_cpp_USE_QT_MAJOR_VERSION} " STREQUAL "5 ") + qt5_wrap_cpp(${PROJECT_NAME}_MOCS ${${PROJECT_NAME}_HDRS_QT}) + qt5_add_resources(${PROJECT_NAME}_RCC ${${PROJECT_NAME}_QRC}) +else() + qt4_wrap_cpp(${PROJECT_NAME}_MOCS ${${PROJECT_NAME}_HDRS_QT}) + qt4_add_resources(${PROJECT_NAME}_RCC ${${PROJECT_NAME}_QRC}) +endif() + +# ensure generated header files are being created in the devel space +set(_cmake_current_binary_dir "${CMAKE_CURRENT_BINARY_DIR}") +set(CMAKE_CURRENT_BINARY_DIR ${ui_INCLUDE_DIR}) +if("${qt_gui_cpp_USE_QT_MAJOR_VERSION} " STREQUAL "5 ") + qt5_wrap_ui(${PROJECT_NAME}_UIS_H ${${PROJECT_NAME}_UIS}) +else() + qt4_wrap_ui(${PROJECT_NAME}_UIS_H ${${PROJECT_NAME}_UIS}) +endif() +set(CMAKE_CURRENT_BINARY_DIR "${_cmake_current_binary_dir}") + +include_directories(${${PROJECT_NAME}_INCLUDE_DIRECTORIES} + ${${PROJECT_NAME}_INCLUDE_DIRECTORIES} + ${catkin_INCLUDE_DIRS} +) + +add_library(${PROJECT_NAME} + ${${PROJECT_NAME}_SRCS} + ${${PROJECT_NAME}_HDRS_QT} + ${${PROJECT_NAME}_HDRS} + ${${PROJECT_NAME}_MOCS} + ${${PROJECT_NAME}_UIS_H} + ${${PROJECT_NAME}_RCC} +) + +target_link_libraries(${PROJECT_NAME} + ${catkin_LIBRARIES} +) + +if ("${qt_gui_cpp_USE_QT_MAJOR_VERSION} " STREQUAL "5 ") + target_link_libraries(${PROJECT_NAME} Qt5::Widgets) +else() + target_link_libraries(${PROJECT_NAME} ${QT_QTCORE_LIBRARY} ${QT_QTGUI_LIBRARY}) +endif() + +add_dependencies(${PROJECT_NAME} + ${catkin_EXPORTED_TARGETS} +) + +find_package(class_loader) +class_loader_hide_library_symbols(${PROJECT_NAME}) + +install(FILES plugin.xml + DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION} +) + +install(TARGETS ${PROJECT_NAME} + ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} + LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} + RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} +) + +install(PROGRAMS scripts/${PROJECT_NAME} + DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} +) + +install(DIRECTORY include/${PROJECT_NAME}/ + DESTINATION ${CATKIN_PACKAGE_INCLUDE_DESTINATION} +) + +install( + DIRECTORY resource + DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION} + FILES_MATCHING + PATTERN "*.svg" +) diff --git a/rqt_free_gait_action/include/rqt_free_gait_action/Action.h b/rqt_free_gait_action/include/rqt_free_gait_action/Action.h new file mode 100644 index 0000000..9e6b87a --- /dev/null +++ b/rqt_free_gait_action/include/rqt_free_gait_action/Action.h @@ -0,0 +1,73 @@ +/****************************************************************************** + * Copyright 2017 Samuel Bachmann * + * * + * Redistribution and use in source and binary forms, with or without * + * modification, are permitted provided that the following conditions are met:* + * * + * 1. Redistributions of source code must retain the above copyright notice, * + * this list of conditions and the following disclaimer. * + * * + * 2. Redistributions in binary form must reproduce the above copyright * + * notice, this list of conditions and the following disclaimer in the * + * documentation and/or other materials provided with the distribution. * + * * + * 3. Neither the name of the copyright holder nor the names of its * + * contributors may be used to endorse or promote products derived from this * + * software without specific prior written permission. * + * * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"* + * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE * + * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE * + * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE * + * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR * + * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF * + * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS * + * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN * + * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) * + * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF * + * THE POSSIBILITY OF SUCH DAMAGE. * + * * + * Author: Samuel Bachmann * + ******************************************************************************/ + +#pragma once + +#include + +namespace rqt_free_gait { + +class Action { +public: + + /***************************************************************************/ + /** Constructor/Destructor **/ + /***************************************************************************/ + + Action(); + + Action(QString id, QString name, QString description); + + ~Action(); + + /***************************************************************************/ + /** Accessors **/ + /***************************************************************************/ + + const QString& getId() const; + + const QString& getName() const; + + const QString& getDescription() const; + +private: + + /***************************************************************************/ + /** Variables **/ + /***************************************************************************/ + + QString id_; + QString name_; + QString description_; +}; + +} // namespace diff --git a/rqt_free_gait_action/include/rqt_free_gait_action/ActionModel.h b/rqt_free_gait_action/include/rqt_free_gait_action/ActionModel.h new file mode 100644 index 0000000..819c23c --- /dev/null +++ b/rqt_free_gait_action/include/rqt_free_gait_action/ActionModel.h @@ -0,0 +1,87 @@ +/****************************************************************************** + * Copyright 2017 Samuel Bachmann * + * * + * Redistribution and use in source and binary forms, with or without * + * modification, are permitted provided that the following conditions are met:* + * * + * 1. Redistributions of source code must retain the above copyright notice, * + * this list of conditions and the following disclaimer. * + * * + * 2. Redistributions in binary form must reproduce the above copyright * + * notice, this list of conditions and the following disclaimer in the * + * documentation and/or other materials provided with the distribution. * + * * + * 3. Neither the name of the copyright holder nor the names of its * + * contributors may be used to endorse or promote products derived from this * + * software without specific prior written permission. * + * * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"* + * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE * + * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE * + * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE * + * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR * + * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF * + * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS * + * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN * + * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) * + * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF * + * THE POSSIBILITY OF SUCH DAMAGE. * + * * + * Author: Samuel Bachmann * + ******************************************************************************/ + +#pragma once + +#include +#include + +#include + +#include "rqt_free_gait_action/Action.h" + +namespace rqt_free_gait { + +class ActionModel : public QAbstractListModel { +public: + + /***************************************************************************/ + /** Constructor/Destructor **/ + /***************************************************************************/ + + ActionModel(QObject *parent = 0); + + ~ActionModel(); + + /***************************************************************************/ + /** Accessors **/ + /***************************************************************************/ + + void addAction(const Action &action); + + int rowCount(const QModelIndex &parent) const; + + QVariant data(const QModelIndex &index, int role) const; + + Action getAction(const QModelIndex &index); + + void sortActions(); + + std::vector getActions(); + +private: + + /***************************************************************************/ + /** Variables **/ + /***************************************************************************/ + + std::vector actions_; + + /***************************************************************************/ + /** Methods **/ + /***************************************************************************/ + + static bool comparator(const Action &l, const Action &r); + +}; + +} // namespace diff --git a/rqt_free_gait_action/include/rqt_free_gait_action/Collection.h b/rqt_free_gait_action/include/rqt_free_gait_action/Collection.h new file mode 100644 index 0000000..c8cdee9 --- /dev/null +++ b/rqt_free_gait_action/include/rqt_free_gait_action/Collection.h @@ -0,0 +1,77 @@ +/****************************************************************************** + * Copyright 2017 Samuel Bachmann, Peter Fankhauser * + * * + * Redistribution and use in source and binary forms, with or without * + * modification, are permitted provided that the following conditions are met:* + * * + * 1. Redistributions of source code must retain the above copyright notice, * + * this list of conditions and the following disclaimer. * + * * + * 2. Redistributions in binary form must reproduce the above copyright * + * notice, this list of conditions and the following disclaimer in the * + * documentation and/or other materials provided with the distribution. * + * * + * 3. Neither the name of the copyright holder nor the names of its * + * contributors may be used to endorse or promote products derived from this * + * software without specific prior written permission. * + * * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"* + * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE * + * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE * + * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE * + * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR * + * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF * + * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS * + * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN * + * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) * + * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF * + * THE POSSIBILITY OF SUCH DAMAGE. * + * * + * Authors: Samuel Bachmann , * + * Peter Fankhauser * + ******************************************************************************/ + +#pragma once + +#include + +#include "rqt_free_gait_action/ActionModel.h" + +namespace rqt_free_gait { + +class Collection { +public: + + /***************************************************************************/ + /** Constructor/Destructor **/ + /***************************************************************************/ + + Collection(QString id, QString name, ActionModel *actionModel, bool isSequence); + + ~Collection(); + + /***************************************************************************/ + /** Accessors **/ + /***************************************************************************/ + + const QString& getId() const; + + const QString& getName() const; + + ActionModel *getActionModel(); + + bool isSequence() const; + +private: + + /***************************************************************************/ + /** Variables **/ + /***************************************************************************/ + + QString id_; + QString name_; + ActionModel *actionModel_ = nullptr; + bool isSequence_; +}; + +} // namespace diff --git a/rqt_free_gait_action/include/rqt_free_gait_action/CollectionModel.h b/rqt_free_gait_action/include/rqt_free_gait_action/CollectionModel.h new file mode 100644 index 0000000..a2cee38 --- /dev/null +++ b/rqt_free_gait_action/include/rqt_free_gait_action/CollectionModel.h @@ -0,0 +1,93 @@ +/****************************************************************************** + * Copyright 2017 Samuel Bachmann, Peter Fankhauser * + * * + * Redistribution and use in source and binary forms, with or without * + * modification, are permitted provided that the following conditions are met:* + * * + * 1. Redistributions of source code must retain the above copyright notice, * + * this list of conditions and the following disclaimer. * + * * + * 2. Redistributions in binary form must reproduce the above copyright * + * notice, this list of conditions and the following disclaimer in the * + * documentation and/or other materials provided with the distribution. * + * * + * 3. Neither the name of the copyright holder nor the names of its * + * contributors may be used to endorse or promote products derived from this * + * software without specific prior written permission. * + * * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"* + * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE * + * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE * + * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE * + * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR * + * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF * + * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS * + * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN * + * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) * + * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF * + * THE POSSIBILITY OF SUCH DAMAGE. * + * * + * Authors: Samuel Bachmann , * + * Peter Fankhauser * + ******************************************************************************/ + +#pragma once + +#include +#include + +#include +#include + +#include "rqt_free_gait_action/Collection.h" + +namespace rqt_free_gait { + +class CollectionModel : public QAbstractListModel { +public: + + /***************************************************************************/ + /** Constructor/Destructor **/ + /***************************************************************************/ + + CollectionModel(QObject *parent = 0); + + ~CollectionModel(); + + /***************************************************************************/ + /** Accessors **/ + /***************************************************************************/ + + void addCollection(const Collection &collection); + + int rowCount(const QModelIndex &parent) const; + + QVariant data(const QModelIndex &index, int role) const; + + ActionModel *getActionModel(const QModelIndex &index); + + QString getCollectionId(const QModelIndex &index); + + bool isSequence(const QModelIndex &index); + + void sortCollections(); + + std::vector getActions(QString collectionId); + +private: + + /***************************************************************************/ + /** Variables **/ + /***************************************************************************/ + + std::vector collections_; + + /***************************************************************************/ + /** Methods **/ + /***************************************************************************/ + + static bool comparator(const Collection &l, const Collection &r); + +}; + +} // namespace diff --git a/rqt_free_gait_action/include/rqt_free_gait_action/FavoritePushButton.h b/rqt_free_gait_action/include/rqt_free_gait_action/FavoritePushButton.h new file mode 100644 index 0000000..df888db --- /dev/null +++ b/rqt_free_gait_action/include/rqt_free_gait_action/FavoritePushButton.h @@ -0,0 +1,91 @@ +/****************************************************************************** + * Copyright 2017 Samuel Bachmann * + * * + * Redistribution and use in source and binary forms, with or without * + * modification, are permitted provided that the following conditions are met:* + * * + * 1. Redistributions of source code must retain the above copyright notice, * + * this list of conditions and the following disclaimer. * + * * + * 2. Redistributions in binary form must reproduce the above copyright * + * notice, this list of conditions and the following disclaimer in the * + * documentation and/or other materials provided with the distribution. * + * * + * 3. Neither the name of the copyright holder nor the names of its * + * contributors may be used to endorse or promote products derived from this * + * software without specific prior written permission. * + * * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"* + * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE * + * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE * + * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE * + * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR * + * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF * + * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS * + * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN * + * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) * + * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF * + * THE POSSIBILITY OF SUCH DAMAGE. * + * * + * Author: Samuel Bachmann * + ******************************************************************************/ + +#pragma once + +#include + +#include "rqt_free_gait_action/Action.h" + +namespace rqt_free_gait { + +class FavoritePushButton : public QPushButton { + Q_OBJECT +public: + + /***************************************************************************/ + /** Constructor/Destructor **/ + /***************************************************************************/ + + FavoritePushButton(QWidget *parent = 0); + + FavoritePushButton(Action action, QWidget *parent = 0); + + /***************************************************************************/ + /** Accessors **/ + /***************************************************************************/ + + void setAction(Action action); + +private: + + /***************************************************************************/ + /** Variables **/ + /***************************************************************************/ + + Action action_; + + /***************************************************************************/ + /** Methods **/ + /***************************************************************************/ + + void connectSignalsInternally(); + +protected slots: + + /***************************************************************************/ + /** Slots **/ + /***************************************************************************/ + + void clickedInternal(); + +signals: + + /***************************************************************************/ + /** Signals **/ + /***************************************************************************/ + + void clicked(Action action); + +}; + +} // namespace diff --git a/rqt_free_gait_action/include/rqt_free_gait_action/FreeGaitActionPlugin.h b/rqt_free_gait_action/include/rqt_free_gait_action/FreeGaitActionPlugin.h new file mode 100644 index 0000000..3ae6918 --- /dev/null +++ b/rqt_free_gait_action/include/rqt_free_gait_action/FreeGaitActionPlugin.h @@ -0,0 +1,263 @@ +/****************************************************************************** + * Copyright 2017 Samuel Bachmann, Peter Fankhauser * + * * + * Redistribution and use in source and binary forms, with or without * + * modification, are permitted provided that the following conditions are met:* + * * + * 1. Redistributions of source code must retain the above copyright notice, * + * this list of conditions and the following disclaimer. * + * * + * 2. Redistributions in binary form must reproduce the above copyright * + * notice, this list of conditions and the following disclaimer in the * + * documentation and/or other materials provided with the distribution. * + * * + * 3. Neither the name of the copyright holder nor the names of its * + * contributors may be used to endorse or promote products derived from this * + * software without specific prior written permission. * + * * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"* + * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE * + * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE * + * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE * + * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR * + * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF * + * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS * + * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN * + * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) * + * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF * + * THE POSSIBILITY OF SUCH DAMAGE. * + * * + * Authors: Samuel Bachmann , * + * Peter Fankhauser * + ******************************************************************************/ + +#pragma once + +#include +#include + +#include +#include + +#include +#include + +#include +#include +#include +#include +#include +#include + +#include "rqt_free_gait_action/ActionModel.h" +#include "rqt_free_gait_action/CollectionModel.h" +#include "rqt_free_gait_action/FavoritePushButton.h" +#include "rqt_free_gait_action/WorkerThreadGetCollections.h" +#include "rqt_free_gait_action/WorkerThreadSendAction.h" +#include "rqt_free_gait_action/WorkerThreadSendActionSequence.h" +#include "rqt_free_gait_action/WorkerThreadUpdateTrigger.h" + +namespace rqt_free_gait { + +class FreeGaitActionPlugin : public rqt_gui_cpp::Plugin { + Q_OBJECT +public: + + /***************************************************************************/ + /** Variables **/ + /***************************************************************************/ + + enum class MessageType { + ERROR, + WARNING, + SUCCESS, + STATUS + }; + + /***************************************************************************/ + /** Constructor/Destructor **/ + /***************************************************************************/ + + FreeGaitActionPlugin(); + + /***************************************************************************/ + /** Initialize/Shutdown **/ + /***************************************************************************/ + + virtual void initPlugin(qt_gui_cpp::PluginContext& context); + + virtual void shutdownPlugin(); + + /***************************************************************************/ + /** Settings **/ + /***************************************************************************/ + + virtual void saveSettings(qt_gui_cpp::Settings& plugin_settings, + qt_gui_cpp::Settings& instance_settings) const; + + virtual void restoreSettings(const qt_gui_cpp::Settings& plugin_settings, + const qt_gui_cpp::Settings& instance_settings); + +protected: + + /***************************************************************************/ + /** Constants **/ + /***************************************************************************/ + + const std::string TAG = "FreeGaitActionPlugin"; + + const QString FAVORITE_INFO = "No favorite collection selected. " + "Right click on a collection to set as favorite."; + + /***************************************************************************/ + /** Variables **/ + /***************************************************************************/ + + Ui::FreeGaitActionPluginWidget ui_; + QWidget* widget_; + QStatusBar* statusBar_; + + ros::ServiceClient actionsClient_; + ros::ServiceClient collectionsClient_; + ros::ServiceClient sendActionClient_; + ros::ServiceClient sendPreviewClient_; + ros::ServiceClient sendActionSequenceClient_; + ros::ServiceClient refreshCollectionsClient_; + + QString selectedAction_ = ""; + QString selectedCollection_ = ""; + QString favoriteCollectionId_ = ""; + + ActionModel *currentActionModel_ = nullptr; + CollectionModel *collectionModel_ = nullptr; + + bool isCollectionListViewConnected_ = false; + bool isActionListViewConnected_ = false; + bool isSendingFavoriteAction_ = false; + + std::vector favoritesPushButtons_; + std::vector favoritesPushButtonsLast_; + + int numberOfColumnsLast_ = 0; + + /***************************************************************************/ + /** Methods **/ + /***************************************************************************/ + + void getCollections(); + + void clearActionListView(); + + void clearCollectionListView(); + + void evaluateFreeGaitActionResponse( + free_gait_msgs::ExecuteActionResult result); + + void updateFavoritesInfo(QString info = ""); + + void setFavoriteActions(QString collectionId); + + void generateGridLayout(); + + void cleanGridLayout(bool deleteWidgets); + + /** + * @brief Helper function. Removes all layout items within the given @a layout + * which either span the given @a row or @a column. If @a deleteWidgets + * is true, all concerned child widgets become not only removed from the + * layout, but also deleted. + * https://goo.gl/dObpgP + * modified to detach widgets + */ + void remove(QGridLayout *layout, int row, int column, bool deleteWidgets); + + /** + * @brief Helper function. Deletes all child widgets of the given layout @a + * item. + * https://goo.gl/dObpgP + */ + void deleteChildWidgets(QLayoutItem *item); + + /** + * @brief Helper function. Detaches all child widgets of the given layout @a + * item by setting parent to 0. + */ + void detachChildWidgets(QLayoutItem *item); + + /** + * @brief Removes all layout items on the given @a row from the given grid + * @a layout. If @a deleteWidgets is true, all concerned child widgets + * become not only removed from the layout, but also deleted. Note that + * this function doesn't actually remove the row itself from the grid + * layout, as this isn't possible (i.e. the rowCount() and row indices + * will stay the same after this function has been called). + * https://goo.gl/dObpgP + */ + void removeRow(QGridLayout *layout, int row, bool deleteWidgets); + + /** + * @brief Removes all layout items on the given @a column from the given grid + * @a layout. If @a deleteWidgets is true, all concerned child widgets + * become not only removed from the layout, but also deleted. Note that + * this function doesn't actually remove the column itself from the grid + * layout, as this isn't possible (i.e. the columnCount() and column + * indices will stay the same after this function has been called). + * https://goo.gl/dObpgP + */ + void removeColumn(QGridLayout *layout, int column, bool deleteWidgets); + + /***************************************************************************/ + /** Events **/ + /***************************************************************************/ + + bool eventFilter(QObject *object, QEvent *event); + +protected slots: + + /***************************************************************************/ + /** Slots **/ + /***************************************************************************/ + + void listViewCollectionsContextMenu(const QPoint &pos); + + void onSendActionClicked(); + + void onSendPreviewClicked(); + + void onRefreshCollectionsClicked(); + + void onFavoriteButtonClicked(Action action); + + void onFavoriteButtonResult(bool isOk, + free_gait_msgs::ExecuteActionResult response); + + void onCollectionSelectionChanged(const QItemSelection &selection); + + void onActionSelectionChanged(const QItemSelection &selection); + + void onGetCollectionsResult(bool isOk, CollectionModel *collectionModel); + + void onSendActionResult(bool isOk, + free_gait_msgs::ExecuteActionResult response); + + void onSendPreviewResult(bool isOk, + free_gait_msgs::ExecuteActionResult response); + + void onRefreshCollectionsResult(bool isOk, + std_srvs::TriggerResponse response); + + void onStatusMessage(std::string message, MessageType type, + double displaySeconds); + +signals: + + /***************************************************************************/ + /** Signals **/ + /***************************************************************************/ + + void statusMessage(std::string message, MessageType type, + double displaySeconds); + +}; + +} // namespace diff --git a/rqt_free_gait_action/include/rqt_free_gait_action/WorkerThreadGetCollections.h b/rqt_free_gait_action/include/rqt_free_gait_action/WorkerThreadGetCollections.h new file mode 100644 index 0000000..f950658 --- /dev/null +++ b/rqt_free_gait_action/include/rqt_free_gait_action/WorkerThreadGetCollections.h @@ -0,0 +1,89 @@ +/****************************************************************************** + * Copyright 2017 Samuel Bachmann * + * * + * Redistribution and use in source and binary forms, with or without * + * modification, are permitted provided that the following conditions are met:* + * * + * 1. Redistributions of source code must retain the above copyright notice, * + * this list of conditions and the following disclaimer. * + * * + * 2. Redistributions in binary form must reproduce the above copyright * + * notice, this list of conditions and the following disclaimer in the * + * documentation and/or other materials provided with the distribution. * + * * + * 3. Neither the name of the copyright holder nor the names of its * + * contributors may be used to endorse or promote products derived from this * + * software without specific prior written permission. * + * * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"* + * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE * + * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE * + * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE * + * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR * + * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF * + * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS * + * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN * + * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) * + * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF * + * THE POSSIBILITY OF SUCH DAMAGE. * + * * + * Author: Samuel Bachmann * + ******************************************************************************/ + +#pragma once + +#include + +#include + +#include +#include + +#include "rqt_free_gait_action/CollectionModel.h" + +namespace rqt_free_gait { + +class WorkerThreadGetCollections : public QThread { +Q_OBJECT + + /***************************************************************************/ + /** Methods **/ + /***************************************************************************/ + + void run(); + +public: + + /***************************************************************************/ + /** Accessors **/ + /***************************************************************************/ + + void setActionsClient(ros::ServiceClient &client); + + void setCollectionsClient(ros::ServiceClient &client); + +private: + + /***************************************************************************/ + /** Constants **/ + /***************************************************************************/ + + const std::string TAG = "WorkerThreadGetCollections"; + + /***************************************************************************/ + /** Variables **/ + /***************************************************************************/ + + ros::ServiceClient actionsClient_; + ros::ServiceClient collectionsClient_; + +signals: + + /***************************************************************************/ + /** Signals **/ + /***************************************************************************/ + + void result(bool isOk, CollectionModel *collectionModel); +}; + +} // namespace diff --git a/rqt_free_gait_action/include/rqt_free_gait_action/WorkerThreadSendAction.h b/rqt_free_gait_action/include/rqt_free_gait_action/WorkerThreadSendAction.h new file mode 100644 index 0000000..0c7a400 --- /dev/null +++ b/rqt_free_gait_action/include/rqt_free_gait_action/WorkerThreadSendAction.h @@ -0,0 +1,81 @@ +/****************************************************************************** + * Copyright 2017 Samuel Bachmann * + * * + * Redistribution and use in source and binary forms, with or without * + * modification, are permitted provided that the following conditions are met:* + * * + * 1. Redistributions of source code must retain the above copyright notice, * + * this list of conditions and the following disclaimer. * + * * + * 2. Redistributions in binary form must reproduce the above copyright * + * notice, this list of conditions and the following disclaimer in the * + * documentation and/or other materials provided with the distribution. * + * * + * 3. Neither the name of the copyright holder nor the names of its * + * contributors may be used to endorse or promote products derived from this * + * software without specific prior written permission. * + * * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"* + * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE * + * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE * + * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE * + * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR * + * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF * + * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS * + * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN * + * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) * + * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF * + * THE POSSIBILITY OF SUCH DAMAGE. * + * * + * Author: Samuel Bachmann * + ******************************************************************************/ + +#pragma once + +#include + +#include + +#include + +namespace rqt_free_gait { + +class WorkerThreadSendAction : public QThread { +Q_OBJECT + + /***************************************************************************/ + /** Methods **/ + /***************************************************************************/ + + void run(); + +public: + + /***************************************************************************/ + /** Accessors **/ + /***************************************************************************/ + + void setClient(ros::ServiceClient &client); + + void setRequest(free_gait_msgs::SendActionRequest request); + +private: + + /***************************************************************************/ + /** Variables **/ + /***************************************************************************/ + + free_gait_msgs::SendActionRequest request_; + free_gait_msgs::SendActionResponse response_; + ros::ServiceClient client_; + +signals: + + /***************************************************************************/ + /** Signals **/ + /***************************************************************************/ + + void result(bool isOk, free_gait_msgs::ExecuteActionResult); +}; + +} // namespace diff --git a/rqt_free_gait_action/include/rqt_free_gait_action/WorkerThreadSendActionSequence.h b/rqt_free_gait_action/include/rqt_free_gait_action/WorkerThreadSendActionSequence.h new file mode 100644 index 0000000..eafad49 --- /dev/null +++ b/rqt_free_gait_action/include/rqt_free_gait_action/WorkerThreadSendActionSequence.h @@ -0,0 +1,82 @@ +/****************************************************************************** + * Copyright 2017 Samuel Bachmann, Peter Fankhauser * + * * + * Redistribution and use in source and binary forms, with or without * + * modification, are permitted provided that the following conditions are met:* + * * + * 1. Redistributions of source code must retain the above copyright notice, * + * this list of conditions and the following disclaimer. * + * * + * 2. Redistributions in binary form must reproduce the above copyright * + * notice, this list of conditions and the following disclaimer in the * + * documentation and/or other materials provided with the distribution. * + * * + * 3. Neither the name of the copyright holder nor the names of its * + * contributors may be used to endorse or promote products derived from this * + * software without specific prior written permission. * + * * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"* + * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE * + * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE * + * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE * + * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR * + * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF * + * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS * + * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN * + * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) * + * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF * + * THE POSSIBILITY OF SUCH DAMAGE. * + * * + * Authors: Samuel Bachmann , * + * Peter Fankhauser * + ******************************************************************************/ + +#pragma once + +#include + +#include + +#include + +namespace rqt_free_gait { + +class WorkerThreadSendActionSequence : public QThread { +Q_OBJECT + + /***************************************************************************/ + /** Methods **/ + /***************************************************************************/ + + void run(); + +public: + + /***************************************************************************/ + /** Accessors **/ + /***************************************************************************/ + + void setClient(ros::ServiceClient &client); + + void setRequest(free_gait_msgs::SendActionSequenceRequest request); + +private: + + /***************************************************************************/ + /** Variables **/ + /***************************************************************************/ + + free_gait_msgs::SendActionSequenceRequest request_; + free_gait_msgs::SendActionSequenceResponse response_; + ros::ServiceClient client_; + +signals: + + /***************************************************************************/ + /** Signals **/ + /***************************************************************************/ + + void result(bool isOk, free_gait_msgs::ExecuteActionResult); +}; + +} // namespace diff --git a/rqt_free_gait_action/include/rqt_free_gait_action/WorkerThreadUpdateTrigger.h b/rqt_free_gait_action/include/rqt_free_gait_action/WorkerThreadUpdateTrigger.h new file mode 100644 index 0000000..eb51f2a --- /dev/null +++ b/rqt_free_gait_action/include/rqt_free_gait_action/WorkerThreadUpdateTrigger.h @@ -0,0 +1,79 @@ +/****************************************************************************** + * Copyright 2017 Samuel Bachmann * + * * + * Redistribution and use in source and binary forms, with or without * + * modification, are permitted provided that the following conditions are met:* + * * + * 1. Redistributions of source code must retain the above copyright notice, * + * this list of conditions and the following disclaimer. * + * * + * 2. Redistributions in binary form must reproduce the above copyright * + * notice, this list of conditions and the following disclaimer in the * + * documentation and/or other materials provided with the distribution. * + * * + * 3. Neither the name of the copyright holder nor the names of its * + * contributors may be used to endorse or promote products derived from this * + * software without specific prior written permission. * + * * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"* + * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE * + * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE * + * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE * + * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR * + * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF * + * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS * + * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN * + * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) * + * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF * + * THE POSSIBILITY OF SUCH DAMAGE. * + * * + * Author: Samuel Bachmann * + ******************************************************************************/ + +#pragma once + +#include + +#include + +#include + +namespace rqt_free_gait { + +class WorkerThreadUpdateTrigger : public QThread { +Q_OBJECT + + /***************************************************************************/ + /** Methods **/ + /***************************************************************************/ + + void run(); + +public: + + /***************************************************************************/ + /** Accessors **/ + /***************************************************************************/ + + void setClient(ros::ServiceClient &client); + +private: + + /***************************************************************************/ + /** Variables **/ + /***************************************************************************/ + + std_srvs::TriggerRequest request_; + std_srvs::TriggerResponse response_; + ros::ServiceClient client_; + +signals: + + /***************************************************************************/ + /** Signals **/ + /***************************************************************************/ + + void result(bool isOk, std_srvs::TriggerResponse response); +}; + +} // namespace diff --git a/rqt_free_gait_action/launch/rqt_free_gait_action.launch b/rqt_free_gait_action/launch/rqt_free_gait_action.launch new file mode 100644 index 0000000..f5cc26d --- /dev/null +++ b/rqt_free_gait_action/launch/rqt_free_gait_action.launch @@ -0,0 +1,8 @@ + + + + + + + diff --git a/rqt_free_gait_action/package.xml b/rqt_free_gait_action/package.xml new file mode 100644 index 0000000..b9e9023 --- /dev/null +++ b/rqt_free_gait_action/package.xml @@ -0,0 +1,23 @@ + + + rqt_free_gait_action + 0.3.0 + A RQT plugin to select and execute Free Gait actions. + Samuel Bachmann + Samuel Bachmann + BSD + https://github.com/leggedrobotics/free_gait + https://github.com/leggedrobotics/free_gait/issues + + catkin + + free_gait_msgs + roscpp + rqt_gui + rqt_gui_cpp + std_srvs + + + + + diff --git a/rqt_free_gait_action/plugin.xml b/rqt_free_gait_action/plugin.xml new file mode 100644 index 0000000..903265a --- /dev/null +++ b/rqt_free_gait_action/plugin.xml @@ -0,0 +1,19 @@ + + + + A GUI plugin to select and send free gait actions. + + + + + folder + Plugins related to... + + + applications-graphics + A GUI plugin to select and send free gait actions. + + + diff --git a/rqt_free_gait_action/resource/FreeGaitActionPlugin.ui b/rqt_free_gait_action/resource/FreeGaitActionPlugin.ui new file mode 100644 index 0000000..6099f0c --- /dev/null +++ b/rqt_free_gait_action/resource/FreeGaitActionPlugin.ui @@ -0,0 +1,217 @@ + + + FreeGaitActionPluginWidget + + + true + + + + 0 + 0 + 496 + 499 + + + + + false + + + + Free Gait Actions + + + + + + <b>Send Action</b> + + + + + + + + 0 + 0 + + + + + 0 + + + 0 + + + 0 + + + 0 + + + + + Favorites + + + + + + + + true + + + + Favorites Info + + + true + + + + + + + + + + + + + + + Actions + + + + + + + + + + + + + + + Collections + + + + + + + Qt::Horizontal + + + + 40 + 20 + + + + + + + + Refresh collections + + + + + + + :/icons/16x16/refresh.svg:/icons/16x16/refresh.svg + + + true + + + + + + + + + + + + true + + + + ... action description ... + + + true + + + + + + + Qt::Vertical + + + QSizePolicy::Fixed + + + + 20 + 10 + + + + + + + + + + Send + + + + + + + Preview + + + + + + + Qt::Horizontal + + + + 40 + 20 + + + + + + + + + + + + + listViewCollections + listViewActions + pushButtonSend + pushButtonPreview + pushButtonRefreshCollections + + + + + + diff --git a/rqt_free_gait_action/resource/icons/16x16/refresh.svg b/rqt_free_gait_action/resource/icons/16x16/refresh.svg new file mode 100644 index 0000000..13bc233 --- /dev/null +++ b/rqt_free_gait_action/resource/icons/16x16/refresh.svg @@ -0,0 +1,201 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/rqt_free_gait_action/resource/icons/22x22/refresh.svg b/rqt_free_gait_action/resource/icons/22x22/refresh.svg new file mode 100644 index 0000000..13bc233 --- /dev/null +++ b/rqt_free_gait_action/resource/icons/22x22/refresh.svg @@ -0,0 +1,201 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/rqt_free_gait_action/resource/resources.qrc b/rqt_free_gait_action/resource/resources.qrc new file mode 100644 index 0000000..413388c --- /dev/null +++ b/rqt_free_gait_action/resource/resources.qrc @@ -0,0 +1,5 @@ + + + icons/16x16/refresh.svg + + \ No newline at end of file diff --git a/rqt_free_gait_action/scripts/rqt_free_gait_action b/rqt_free_gait_action/scripts/rqt_free_gait_action new file mode 100755 index 0000000..1babc28 --- /dev/null +++ b/rqt_free_gait_action/scripts/rqt_free_gait_action @@ -0,0 +1,8 @@ +#!/usr/bin/env python + +import sys + +from rqt_gui.main import Main + +main = Main() +sys.exit(main.main(sys.argv, standalone='rqt_free_gait/FreeGaitActionPlugin')) diff --git a/rqt_free_gait_action/setup.py b/rqt_free_gait_action/setup.py new file mode 100644 index 0000000..d34b487 --- /dev/null +++ b/rqt_free_gait_action/setup.py @@ -0,0 +1,11 @@ +#!/usr/bin/env python + +from distutils.core import setup +from catkin_pkg.python_setup import generate_distutils_setup + +d = generate_distutils_setup( + packages=['rqt_free_gait_action'], + package_dir={'': 'src'} +) + +setup(**d) diff --git a/rqt_free_gait_action/src/rqt_free_gait_action/Action.cpp b/rqt_free_gait_action/src/rqt_free_gait_action/Action.cpp new file mode 100644 index 0000000..cf3c406 --- /dev/null +++ b/rqt_free_gait_action/src/rqt_free_gait_action/Action.cpp @@ -0,0 +1,72 @@ +/****************************************************************************** + * Copyright 2017 Samuel Bachmann * + * * + * Redistribution and use in source and binary forms, with or without * + * modification, are permitted provided that the following conditions are met:* + * * + * 1. Redistributions of source code must retain the above copyright notice, * + * this list of conditions and the following disclaimer. * + * * + * 2. Redistributions in binary form must reproduce the above copyright * + * notice, this list of conditions and the following disclaimer in the * + * documentation and/or other materials provided with the distribution. * + * * + * 3. Neither the name of the copyright holder nor the names of its * + * contributors may be used to endorse or promote products derived from this * + * software without specific prior written permission. * + * * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"* + * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE * + * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE * + * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE * + * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR * + * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF * + * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS * + * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN * + * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) * + * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF * + * THE POSSIBILITY OF SUCH DAMAGE. * + * * + * Author: Samuel Bachmann * + ******************************************************************************/ + +#include "rqt_free_gait_action/Action.h" + +namespace rqt_free_gait { + +/*****************************************************************************/ +/** Constructor/Destructor **/ +/*****************************************************************************/ + +Action::Action() { + +} + +Action::Action(QString id, QString name, QString description) + : id_(id), + name_(name), + description_(description) { + +} + +Action::~Action() { + +} + +/*****************************************************************************/ +/** Accessors **/ +/*****************************************************************************/ + +const QString &Action::getId() const { + return id_; +} + +const QString &Action::getName() const { + return name_; +} + +const QString &Action::getDescription() const { + return description_; +} + +} // namespace \ No newline at end of file diff --git a/rqt_free_gait_action/src/rqt_free_gait_action/ActionModel.cpp b/rqt_free_gait_action/src/rqt_free_gait_action/ActionModel.cpp new file mode 100644 index 0000000..3b08ec0 --- /dev/null +++ b/rqt_free_gait_action/src/rqt_free_gait_action/ActionModel.cpp @@ -0,0 +1,104 @@ +/****************************************************************************** + * Copyright 2017 Samuel Bachmann * + * * + * Redistribution and use in source and binary forms, with or without * + * modification, are permitted provided that the following conditions are met:* + * * + * 1. Redistributions of source code must retain the above copyright notice, * + * this list of conditions and the following disclaimer. * + * * + * 2. Redistributions in binary form must reproduce the above copyright * + * notice, this list of conditions and the following disclaimer in the * + * documentation and/or other materials provided with the distribution. * + * * + * 3. Neither the name of the copyright holder nor the names of its * + * contributors may be used to endorse or promote products derived from this * + * software without specific prior written permission. * + * * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"* + * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE * + * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE * + * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE * + * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR * + * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF * + * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS * + * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN * + * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) * + * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF * + * THE POSSIBILITY OF SUCH DAMAGE. * + * * + * Author: Samuel Bachmann * + ******************************************************************************/ + +#include "rqt_free_gait_action/ActionModel.h" + +namespace rqt_free_gait { + +/*****************************************************************************/ +/** Constructor/Destructor **/ +/*****************************************************************************/ + +ActionModel::ActionModel(QObject *parent) : QAbstractListModel(parent) { + +} + +ActionModel::~ActionModel() { + +} + +/*****************************************************************************/ +/** Accessors **/ +/*****************************************************************************/ + +void ActionModel::addAction(const Action &action) { + actions_.push_back(action); + +#if QT_VERSION >= QT_VERSION_CHECK(5,0,0) + // TODO reset model? +// beginResetModel(); +// myData.clear(); +// endResetModel(); +#else + reset(); +#endif +} + +int ActionModel::rowCount(const QModelIndex &parent) const { + return (int)actions_.size(); +} + +QVariant ActionModel::data(const QModelIndex &index, int role) const { + if (!index.isValid()) { + return QVariant(); + } + + if (role == Qt::TextAlignmentRole) { + return int(Qt::AlignLeft | Qt::AlignVCenter); + } else if (role == Qt::DisplayRole) { + QString name = actions_.at((unsigned long)index.row()).getName(); + return name; + } + + return QVariant();} + +Action ActionModel::getAction(const QModelIndex &index) { + return actions_.at((unsigned long)index.row()); +} + +void ActionModel::sortActions() { + std::sort(actions_.begin(), actions_.end(), ActionModel::comparator); +} + +std::vector ActionModel::getActions() { + return actions_; +} + +/*****************************************************************************/ +/** Methods **/ +/*****************************************************************************/ + +bool ActionModel::comparator(const Action &l, const Action &r) { + return l.getName() < r.getName(); +} + +} // namespace diff --git a/rqt_free_gait_action/src/rqt_free_gait_action/Collection.cpp b/rqt_free_gait_action/src/rqt_free_gait_action/Collection.cpp new file mode 100644 index 0000000..f173eba --- /dev/null +++ b/rqt_free_gait_action/src/rqt_free_gait_action/Collection.cpp @@ -0,0 +1,76 @@ +/****************************************************************************** + * Copyright 2017 Samuel Bachmann, Peter Fankhauser * + * * + * Redistribution and use in source and binary forms, with or without * + * modification, are permitted provided that the following conditions are met:* + * * + * 1. Redistributions of source code must retain the above copyright notice, * + * this list of conditions and the following disclaimer. * + * * + * 2. Redistributions in binary form must reproduce the above copyright * + * notice, this list of conditions and the following disclaimer in the * + * documentation and/or other materials provided with the distribution. * + * * + * 3. Neither the name of the copyright holder nor the names of its * + * contributors may be used to endorse or promote products derived from this * + * software without specific prior written permission. * + * * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"* + * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE * + * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE * + * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE * + * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR * + * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF * + * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS * + * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN * + * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) * + * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF * + * THE POSSIBILITY OF SUCH DAMAGE. * + * * + * Authors: Samuel Bachmann , * + * Peter Fankhauser * + ******************************************************************************/ + +#include + +#include "rqt_free_gait_action/Collection.h" + +namespace rqt_free_gait { + +/*****************************************************************************/ +/** Constructor/Destructor **/ +/*****************************************************************************/ + +Collection::Collection(QString id, QString name, ActionModel *actionModel, bool isSequence) + : id_(id), + name_(name), + actionModel_(actionModel), + isSequence_(isSequence) { + +} + +Collection::~Collection() { + +} + +/*****************************************************************************/ +/** Accessors **/ +/*****************************************************************************/ + +const QString &Collection::getId() const { + return id_; +} + +const QString &Collection::getName() const { + return name_; +} + +ActionModel *Collection::getActionModel() { + return actionModel_; +} + +bool Collection::isSequence() const { + return isSequence_; +} + +} // namespace diff --git a/rqt_free_gait_action/src/rqt_free_gait_action/CollectionModel.cpp b/rqt_free_gait_action/src/rqt_free_gait_action/CollectionModel.cpp new file mode 100644 index 0000000..fe2032e --- /dev/null +++ b/rqt_free_gait_action/src/rqt_free_gait_action/CollectionModel.cpp @@ -0,0 +1,120 @@ +/****************************************************************************** + * Copyright 2017 Samuel Bachmann, Peter Fankhauser * + * * + * Redistribution and use in source and binary forms, with or without * + * modification, are permitted provided that the following conditions are met:* + * * + * 1. Redistributions of source code must retain the above copyright notice, * + * this list of conditions and the following disclaimer. * + * * + * 2. Redistributions in binary form must reproduce the above copyright * + * notice, this list of conditions and the following disclaimer in the * + * documentation and/or other materials provided with the distribution. * + * * + * 3. Neither the name of the copyright holder nor the names of its * + * contributors may be used to endorse or promote products derived from this * + * software without specific prior written permission. * + * * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"* + * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE * + * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE * + * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE * + * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR * + * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF * + * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS * + * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN * + * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) * + * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF * + * THE POSSIBILITY OF SUCH DAMAGE. * + * * + * Authors: Samuel Bachmann , * + * Peter Fankhauser * + ******************************************************************************/ + +#include "rqt_free_gait_action/CollectionModel.h" + +namespace rqt_free_gait { + +/*****************************************************************************/ +/** Constructor/Destructor **/ +/*****************************************************************************/ + +CollectionModel::CollectionModel(QObject *parent) : QAbstractListModel(parent) { + +} + +CollectionModel::~CollectionModel() { + +} + +/*****************************************************************************/ +/** Accessors **/ +/*****************************************************************************/ + +void CollectionModel::addCollection(const Collection &collection) { + collections_.push_back(collection); + +#if QT_VERSION >= QT_VERSION_CHECK(5,0,0) + // TODO reset model? +// beginResetModel(); +// myData.clear(); +// endResetModel(); +#else + reset(); +#endif +} + +int CollectionModel::rowCount(const QModelIndex &/*parent*/) const { + return (int)collections_.size(); +} + +QVariant CollectionModel::data(const QModelIndex &index, int role) const { + if (!index.isValid()) { + return QVariant(); + } + + if (role == Qt::TextAlignmentRole) { + return int(Qt::AlignLeft | Qt::AlignVCenter); + } else if (role == Qt::DisplayRole) { + return collections_.at((unsigned long)index.row()).getName(); + } + + return QVariant(); +} + +ActionModel *CollectionModel::getActionModel( + const QModelIndex &index) { + return collections_.at((unsigned long)index.row()).getActionModel(); +} + +void CollectionModel::sortCollections() { + std::sort(collections_.begin(), collections_.end(), + CollectionModel::comparator); +} + +std::vector CollectionModel::getActions(QString collectionId) { + for (int i = 0; i < collections_.size(); ++i) { + if (collections_[i].getId().compare(collectionId) == 0) { + return collections_[i].getActionModel()->getActions(); + } + } + return std::vector(); +} + +QString CollectionModel::getCollectionId(const QModelIndex &index) { + return collections_.at((unsigned long)index.row()).getId(); +} + +bool CollectionModel::isSequence(const QModelIndex &index) { + return collections_.at((unsigned long)index.row()).isSequence(); +} + +/*****************************************************************************/ +/** Methods **/ +/*****************************************************************************/ + +bool CollectionModel::comparator(const Collection &l, const Collection &r) { + return l.getName() < r.getName(); +} + +} // namespace diff --git a/rqt_free_gait_action/src/rqt_free_gait_action/FavoritePushButton.cpp b/rqt_free_gait_action/src/rqt_free_gait_action/FavoritePushButton.cpp new file mode 100644 index 0000000..8328865 --- /dev/null +++ b/rqt_free_gait_action/src/rqt_free_gait_action/FavoritePushButton.cpp @@ -0,0 +1,80 @@ +/****************************************************************************** + * Copyright 2017 Samuel Bachmann * + * * + * Redistribution and use in source and binary forms, with or without * + * modification, are permitted provided that the following conditions are met:* + * * + * 1. Redistributions of source code must retain the above copyright notice, * + * this list of conditions and the following disclaimer. * + * * + * 2. Redistributions in binary form must reproduce the above copyright * + * notice, this list of conditions and the following disclaimer in the * + * documentation and/or other materials provided with the distribution. * + * * + * 3. Neither the name of the copyright holder nor the names of its * + * contributors may be used to endorse or promote products derived from this * + * software without specific prior written permission. * + * * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"* + * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE * + * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE * + * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE * + * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR * + * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF * + * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS * + * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN * + * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) * + * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF * + * THE POSSIBILITY OF SUCH DAMAGE. * + * * + * Author: Samuel Bachmann * + ******************************************************************************/ + +#include "rqt_free_gait_action/FavoritePushButton.h" + +namespace rqt_free_gait { + +/*****************************************************************************/ +/** Constructor/Destructor **/ +/*****************************************************************************/ + +FavoritePushButton::FavoritePushButton(QWidget *parent) + : QPushButton(parent) { + connectSignalsInternally(); +} + +FavoritePushButton::FavoritePushButton(Action action, QWidget *parent) + : QPushButton(parent), + action_(action) { + connectSignalsInternally(); + + setText(action_.getName()); +} + +/*****************************************************************************/ +/** Accessors **/ +/*****************************************************************************/ + +void FavoritePushButton::setAction(Action action) { + action_ = action; + + setText(action_.getName()); +} + +/*****************************************************************************/ +/** Methods **/ +/*****************************************************************************/ + +void FavoritePushButton::connectSignalsInternally() { + connect(this, SIGNAL(clicked()), this, SLOT(clickedInternal())); +} + +/*****************************************************************************/ +/** Slots **/ +/*****************************************************************************/ + +void FavoritePushButton::clickedInternal() { + emit clicked(action_); +} + +} // namespace diff --git a/rqt_free_gait_action/src/rqt_free_gait_action/FreeGaitActionPlugin.cpp b/rqt_free_gait_action/src/rqt_free_gait_action/FreeGaitActionPlugin.cpp new file mode 100644 index 0000000..f290a6d --- /dev/null +++ b/rqt_free_gait_action/src/rqt_free_gait_action/FreeGaitActionPlugin.cpp @@ -0,0 +1,702 @@ +/****************************************************************************** + * Copyright 2017 Samuel Bachmann, Peter Fankhauser * + * * + * Redistribution and use in source and binary forms, with or without * + * modification, are permitted provided that the following conditions are met:* + * * + * 1. Redistributions of source code must retain the above copyright notice, * + * this list of conditions and the following disclaimer. * + * * + * 2. Redistributions in binary form must reproduce the above copyright * + * notice, this list of conditions and the following disclaimer in the * + * documentation and/or other materials provided with the distribution. * + * * + * 3. Neither the name of the copyright holder nor the names of its * + * contributors may be used to endorse or promote products derived from this * + * software without specific prior written permission. * + * * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"* + * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE * + * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE * + * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE * + * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR * + * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF * + * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS * + * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN * + * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) * + * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF * + * THE POSSIBILITY OF SUCH DAMAGE. * + * * + * Authors: Samuel Bachmann , * + * Peter Fankhauser * + ******************************************************************************/ + +#include "rqt_free_gait_action/FreeGaitActionPlugin.h" + +#include + +namespace rqt_free_gait { + +/*****************************************************************************/ +/** Constructor/Destructor **/ +/*****************************************************************************/ + +FreeGaitActionPlugin::FreeGaitActionPlugin() + : rqt_gui_cpp::Plugin() + , widget_(0) { + + qRegisterMetaType( + "free_gait_msgs::ExecuteActionResult"); + qRegisterMetaType( + "std_srvs::TriggerResponse"); + qRegisterMetaType("Action"); + + setObjectName("FreeGaitActionPlugin"); +} + +/*****************************************************************************/ +/** Initialize/Shutdown **/ +/*****************************************************************************/ + +void FreeGaitActionPlugin::initPlugin(qt_gui_cpp::PluginContext& context) { + widget_ = new QWidget(); + ui_.setupUi(widget_); + if (context.serialNumber() > 1) { + widget_->setWindowTitle(widget_->windowTitle() + + " (" + QString::number(context.serialNumber()) + ")"); + } + context.addWidget(widget_); + + // Install event filter. + ui_.widgetFavorites->installEventFilter(this); + + // Reset labels. + ui_.labelActionDescription->setText(""); + + // Set up custom context menu for collections list view. + ui_.listViewCollections->setContextMenuPolicy(Qt::CustomContextMenu); + + // Initialize status bar. + statusBar_ = new QStatusBar(widget_); + statusBar_->setSizeGripEnabled(false); + statusBar_->setStyleSheet("border: none"); + statusBar_->show(); + ui_.statusBarLayout->addWidget(statusBar_); + + // Initialize service clients. + // TODO(Shunyao): create ros service server to send action or like free gait marker, + // that means a do something with a callback function. + // !!! has been implemented in action_loasder.py + collectionsClient_ = getNodeHandle().serviceClient< + free_gait_msgs::GetCollections>( + "/free_gait_action_loader/list_collections", false); + actionsClient_ = getNodeHandle().serviceClient< + free_gait_msgs::GetActions>( + "/free_gait_action_loader/list_actions", false); + sendActionClient_ = getNodeHandle().serviceClient< + free_gait_msgs::SendAction>( + "/free_gait_action_loader/send_action", false); + sendPreviewClient_ = getNodeHandle().serviceClient< + free_gait_msgs::SendAction>( + "/free_gait_action_loader/preview_action", false); + sendActionSequenceClient_ = getNodeHandle().serviceClient< + free_gait_msgs::SendActionSequence>( + "/free_gait_action_loader/send_action_sequence", false); + refreshCollectionsClient_ = getNodeHandle().serviceClient< + std_srvs::Trigger>( + "/free_gait_action_loader/update", false); + + // Connect signals to slots. + connect(ui_.pushButtonSend, SIGNAL(clicked()), + this, SLOT(onSendActionClicked())); + connect(ui_.pushButtonPreview, SIGNAL(clicked()), + this, SLOT(onSendPreviewClicked())); + connect(ui_.pushButtonRefreshCollections, SIGNAL(clicked()), + this, SLOT(onRefreshCollectionsClicked())); + connect(this, SIGNAL(statusMessage(std::string, MessageType, double)), + this, SLOT(onStatusMessage(std::string, MessageType, double))); + connect(ui_.listViewCollections, + SIGNAL(customContextMenuRequested(const QPoint&)), + this, SLOT(listViewCollectionsContextMenu(const QPoint&))); + + // Initialize favorites info. + updateFavoritesInfo(FAVORITE_INFO); + + // Get collections. + getCollections(); +} + +void FreeGaitActionPlugin::shutdownPlugin() { + collectionsClient_.shutdown(); + actionsClient_.shutdown(); + sendActionClient_.shutdown(); + sendPreviewClient_.shutdown(); + sendActionSequenceClient_.shutdown(); + refreshCollectionsClient_.shutdown(); +} + +/*****************************************************************************/ +/** Settings **/ +/*****************************************************************************/ + +void FreeGaitActionPlugin::saveSettings( + qt_gui_cpp::Settings& plugin_settings, + qt_gui_cpp::Settings& instance_settings) const { + plugin_settings.setValue("favorite_collection_id", favoriteCollectionId_); +} + +void FreeGaitActionPlugin::restoreSettings( + const qt_gui_cpp::Settings& plugin_settings, + const qt_gui_cpp::Settings& instance_settings) { + favoriteCollectionId_ = + plugin_settings.value("favorite_collection_id", "").toString(); +} + +/*****************************************************************************/ +/** Methods **/ +/*****************************************************************************/ + +void FreeGaitActionPlugin::getCollections() { + WorkerThreadGetCollections *workerThreadGetCollections = + new WorkerThreadGetCollections; + connect(workerThreadGetCollections, SIGNAL(result(bool, CollectionModel*)), + this, SLOT(onGetCollectionsResult(bool, CollectionModel*))); + connect(workerThreadGetCollections, SIGNAL(finished()), + workerThreadGetCollections, SLOT(deleteLater())); + workerThreadGetCollections->setActionsClient(actionsClient_); + workerThreadGetCollections->setCollectionsClient(collectionsClient_); + workerThreadGetCollections->start(); +} + +void FreeGaitActionPlugin::clearActionListView() { + ActionModel *actionModel = new ActionModel(this); + ui_.listViewActions->setModel(actionModel); +} + +void FreeGaitActionPlugin::clearCollectionListView() { + if (collectionModel_ != nullptr) { + delete collectionModel_; + } + collectionModel_ = new CollectionModel(this); + ui_.listViewCollections->setModel(collectionModel_); +} + +void FreeGaitActionPlugin::evaluateFreeGaitActionResponse( + free_gait_msgs::ExecuteActionResult result) { + switch (result.status) { + case free_gait_msgs::ExecuteActionResult::RESULT_DONE: + emit statusMessage("Done", MessageType::SUCCESS, 2.0); + break; + case free_gait_msgs::ExecuteActionResult::RESULT_STARTED : + emit statusMessage("Started", MessageType::SUCCESS, 2.0); + break; + case free_gait_msgs::ExecuteActionResult::RESULT_FAILED: + emit statusMessage("Failed", MessageType::ERROR, 2.0); + break; + case free_gait_msgs::ExecuteActionResult::RESULT_NOT_FOUND: + emit statusMessage("Not found", MessageType::ERROR, 2.0); + break; + case free_gait_msgs::ExecuteActionResult::RESULT_UNKNOWN: + emit statusMessage("Unknown", MessageType::WARNING, 2.0); + break; + default: + break; + } +} + +void FreeGaitActionPlugin::updateFavoritesInfo(QString info) { + ui_.labelFavoritesInfo->setText(info); + if (ui_.gridLayoutFavorites->count() > 0) { + ui_.labelFavoritesInfo->hide(); + } else { + ui_.labelFavoritesInfo->show(); + } +} + +void FreeGaitActionPlugin::setFavoriteActions(QString collectionId) { + if (isSendingFavoriteAction_) { + statusMessage("Cannot change favorites while sending a favorite action.", + MessageType::WARNING, 2.0); + return; + } + cleanGridLayout(true); + favoritesPushButtons_.clear(); + if (collectionId.isEmpty()) { + updateFavoritesInfo(FAVORITE_INFO); + return; + } + // Set up favorites. + for (auto item : collectionModel_->getActions(collectionId)) { + favoritesPushButtons_.push_back(new FavoritePushButton(item)); + connect(favoritesPushButtons_.back(), SIGNAL(clicked(Action)), + this, SLOT(onFavoriteButtonClicked(Action))); + } + generateGridLayout(); + if (favoritesPushButtons_.empty()) { + favoriteCollectionId_ = ""; + updateFavoritesInfo(FAVORITE_INFO); + } else { + updateFavoritesInfo(); + } +} + +void FreeGaitActionPlugin::remove(QGridLayout *layout, int row, + int column, bool deleteWidgets) { + // We avoid usage of QGridLayout::itemAtPosition() here + // to improve performance. + for (int i = layout->count() - 1; i >= 0; i--) { + int r, c, rs, cs; + layout->getItemPosition(i, &r, &c, &rs, &cs); + if ((r <= row && r + rs - 1 >= row) || + (c <= column && c + cs - 1 >= column)) { + // This layout item is subject to deletion. + QLayoutItem *item = layout->takeAt(i); + if (deleteWidgets) { + deleteChildWidgets(item); + } else { + detachChildWidgets(item); + } + delete item; + } + } +} + +void FreeGaitActionPlugin::deleteChildWidgets(QLayoutItem *item) { + if (item->layout()) { + // Process all child items recursively. + for (int i = 0; i < item->layout()->count(); i++) { + deleteChildWidgets(item->layout()->itemAt(i)); + } + } + delete item->widget(); +} + +void FreeGaitActionPlugin::detachChildWidgets(QLayoutItem *item) { + if (item->layout()) { + // Process all child items recursively. + for (int i = 0; i < item->layout()->count(); i++) { + detachChildWidgets(item->layout()->itemAt(i)); + } + } else { + item->widget()->setParent(0); + } +} + +void FreeGaitActionPlugin::removeRow(QGridLayout *layout, + int row, bool deleteWidgets) { + remove(layout, row, -1, deleteWidgets); + layout->setRowMinimumHeight(row, 0); + layout->setRowStretch(row, 0); +} + +void FreeGaitActionPlugin::removeColumn(QGridLayout *layout, int column, + bool deleteWidgets) { + remove(layout, -1, column, deleteWidgets); + layout->setColumnMinimumWidth(column, 0); + layout->setColumnStretch(column, 0); +} + +void FreeGaitActionPlugin::cleanGridLayout(bool deleteWidgets) { + // Remove all widgets from grid layout. + if (ui_.gridLayoutFavorites->rowCount() > 0) { + for (int i = 0; i < ui_.gridLayoutFavorites->rowCount(); ++i) { + removeRow(ui_.gridLayoutFavorites, i, deleteWidgets); + } + ui_.gridLayoutFavorites->invalidate(); + } +} + +void FreeGaitActionPlugin::generateGridLayout() { + // Set the used font. + QFont myFont("Ubuntu", 11); + QFontMetrics fontMetrics(myFont); + + // Get available widget size. + int width = ui_.widgetFavorites->size().width(); + + int numberOfComponents = (int)favoritesPushButtons_.size(); + + // Check if it is necessary to regenerate the grid. + bool isNew = false; + if (numberOfComponents == favoritesPushButtonsLast_.size()) { + for (int i = 0; i < numberOfComponents; ++i) { + if (favoritesPushButtons_[i] != favoritesPushButtonsLast_[i]) { + isNew = true; + } + } + } else { + isNew = true; + } + + if (favoritesPushButtons_.empty()) { + // No buttons, clean layout and delete all buttons. + cleanGridLayout(true); + numberOfColumnsLast_ = 0; + } else { + // Compute the minimum width of the buttons, depending on the button text. + int minWidth = 0; + for (auto item : favoritesPushButtons_) { + int fontWidth = fontMetrics.width(item->text()) + 20; + if (fontWidth > minWidth) { + minWidth = fontWidth; + } + } + // Compute the number of columns that is possible with the given minimum + // width. + int numberOfColumns = 1; + while (true) { + // TODO 4 ??? + if (numberOfColumns * minWidth + + (numberOfColumns - 1) * ui_.gridLayoutFavorites->horizontalSpacing() + + 4 < width) { + numberOfColumns++; + } else { + numberOfColumns--; + break; + } + if (numberOfColumns == favoritesPushButtons_.size() && + numberOfColumns * minWidth + + (numberOfColumns - 1) * ui_.gridLayoutFavorites->horizontalSpacing() + + 4 < width) { + break; + } + } + if (numberOfColumns < 1) { + numberOfColumns = 1; + } + + // Check if and how to clean the grid layout (delete or not delete the + // buttons. + if (numberOfColumns != numberOfColumnsLast_ && !isNew) { + cleanGridLayout(false); + } else if (isNew) { + cleanGridLayout(true); + } + + // If new or the number of columns changed. + if (numberOfColumns != numberOfColumnsLast_ || isNew) { + // Add widgets to the grid layout. + int numberOfComponents = (int)favoritesPushButtons_.size(); + int row = 0; + unsigned long counter = 0; + while (numberOfComponents > 0) { + for (int i = 0; i < numberOfColumns; ++i) { + if (numberOfComponents > 0) { + // Add push button to grid layout. + ui_.gridLayoutFavorites->addWidget( + favoritesPushButtons_.at(counter), row, i); + counter++; + numberOfComponents--; + } + } + row++; + } + } + // Set current number of columns as last. + numberOfColumnsLast_ = numberOfColumns; + } + // Set current set of favorite buttons as last. + favoritesPushButtonsLast_ = favoritesPushButtons_; +} + +/*****************************************************************************/ +/** Events **/ +/*****************************************************************************/ + +bool FreeGaitActionPlugin::eventFilter(QObject *object, QEvent *event) { + if (event->type() == QEvent::Resize) { + QResizeEvent *resizeEvent = static_cast(event); + if (resizeEvent->size().width() != resizeEvent->oldSize().width()) { + generateGridLayout(); + } + } + return QObject::eventFilter(object, event); +} + +/*****************************************************************************/ +/** Slots **/ +/*****************************************************************************/ + +void +FreeGaitActionPlugin::onCollectionSelectionChanged( + const QItemSelection &selection) { + if (currentActionModel_ != nullptr && isActionListViewConnected_) { + disconnect(ui_.listViewActions->selectionModel(), + SIGNAL(selectionChanged(QItemSelection, QItemSelection)), + this, SLOT(onActionSelectionChanged(QItemSelection))); + isActionListViewConnected_ = false; + } + currentActionModel_ = nullptr; + if (selection.indexes().isEmpty()) { + clearActionListView(); + ui_.pushButtonSend->setText("Send Sequence"); + ui_.pushButtonSend->setEnabled(false); + ui_.pushButtonPreview->setEnabled(false); + } else { + currentActionModel_ = collectionModel_->getActionModel( + selection.indexes().first()); + ui_.listViewActions->setModel(currentActionModel_); + ui_.labelActionDescription->setText(""); + + connect(ui_.listViewActions->selectionModel(), + SIGNAL(selectionChanged(QItemSelection, QItemSelection)), + this, SLOT(onActionSelectionChanged(QItemSelection))); + isActionListViewConnected_ = true; + selectedAction_ = ""; + selectedCollection_ = ""; + if (collectionModel_->isSequence(selection.indexes().first())) { + selectedCollection_ = collectionModel_->getCollectionId(selection.indexes().first()); + ui_.pushButtonSend->setEnabled(true); + ui_.pushButtonSend->setText("Send Sequence"); + ui_.pushButtonPreview->setEnabled(false); + } else { + ui_.pushButtonSend->setEnabled(false); + ui_.pushButtonPreview->setEnabled(false); + } + } +} + +void FreeGaitActionPlugin::onActionSelectionChanged( + const QItemSelection &selection) { + if (selection.indexes().isEmpty()) { + ui_.labelActionDescription->setText(""); + ui_.pushButtonSend->setText("Send"); + ui_.pushButtonSend->setEnabled(false); + ui_.pushButtonPreview->setEnabled(false); + } else { + if (currentActionModel_ == nullptr) { + return; + } + Action action = currentActionModel_->getAction(selection.indexes().first()); + ui_.labelActionDescription->setText(action.getDescription()); + selectedAction_ = action.getId(); + selectedCollection_ = ""; + ui_.pushButtonSend->setEnabled(true); + ui_.pushButtonSend->setText("Send"); + ui_.pushButtonPreview->setEnabled(true); + } +} + +void FreeGaitActionPlugin::onGetCollectionsResult( + bool isOk, CollectionModel *collectionModel) { + if (isCollectionListViewConnected_) { + disconnect(ui_.listViewCollections->selectionModel(), + SIGNAL(selectionChanged(QItemSelection, QItemSelection)), + this, SLOT(onCollectionSelectionChanged(QItemSelection))); + isCollectionListViewConnected_ = false; + } + if (isOk) { + collectionModel_ = collectionModel; + ui_.listViewCollections->setModel(collectionModel_); + + connect(ui_.listViewCollections->selectionModel(), + SIGNAL(selectionChanged(QItemSelection, QItemSelection)), + this, SLOT(onCollectionSelectionChanged(QItemSelection))); + isCollectionListViewConnected_ = true; + + setFavoriteActions(favoriteCollectionId_); + } else { + // TODO clean collection and action list view? + } +} + +void FreeGaitActionPlugin::onSendActionResult( + bool isOk, free_gait_msgs::ExecuteActionResult response) { + if (!isOk) { + emit statusMessage("Could not send action.", MessageType::WARNING, 2.0); + } else { + evaluateFreeGaitActionResponse(response); + } + ui_.pushButtonSend->setEnabled(true); +} + +void FreeGaitActionPlugin::onSendActionClicked() { + if (selectedAction_.isEmpty() && selectedCollection_.isEmpty()) { + emit statusMessage("Nothing selected.", MessageType::WARNING, 2.0); + return; + } + + if (!selectedAction_.isEmpty()) { + free_gait_msgs::SendActionRequest request; + request.goal.action_id = selectedAction_.toStdString(); + + WorkerThreadSendAction *workerThreadSendAction = new WorkerThreadSendAction; + connect(workerThreadSendAction, SIGNAL(result(bool, free_gait_msgs::ExecuteActionResult)), this, + SLOT(onSendActionResult(bool, free_gait_msgs::ExecuteActionResult))); + connect(workerThreadSendAction, SIGNAL(finished()), workerThreadSendAction, SLOT(deleteLater())); + workerThreadSendAction->setClient(sendActionClient_); + workerThreadSendAction->setRequest(request); + workerThreadSendAction->start(); + } else if (!selectedCollection_.isEmpty()) { + free_gait_msgs::SendActionSequenceRequest request; + for (auto item : collectionModel_->getActions(selectedCollection_)) { + free_gait_msgs::SendActionRequest::_goal_type goal; + goal.action_id = item.getId().toStdString(); + request.goals.push_back(goal); + } + + WorkerThreadSendActionSequence *workerThreadSendActionSequence = new WorkerThreadSendActionSequence; + connect(workerThreadSendActionSequence, SIGNAL(result(bool, free_gait_msgs::ExecuteActionResult)), + this, SLOT(onSendActionResult(bool, free_gait_msgs::ExecuteActionResult))); + connect(workerThreadSendActionSequence, SIGNAL(finished()), + workerThreadSendActionSequence, SLOT(deleteLater())); + workerThreadSendActionSequence->setClient(sendActionSequenceClient_); + workerThreadSendActionSequence->setRequest(request); + workerThreadSendActionSequence->start(); + } + + ui_.pushButtonSend->setEnabled(false); +} + +void FreeGaitActionPlugin::onSendPreviewClicked() { + if (selectedAction_.isEmpty()) { + emit statusMessage("No action selected.", MessageType::WARNING, 2.0); + return; + } + + free_gait_msgs::SendActionRequest request; + request.goal.action_id = selectedAction_.toStdString(); + + WorkerThreadSendAction *workerThreadSendPreview = new WorkerThreadSendAction; + connect(workerThreadSendPreview, + SIGNAL(result(bool, free_gait_msgs::ExecuteActionResult)), + this, + SLOT(onSendPreviewResult(bool, free_gait_msgs::ExecuteActionResult))); + connect(workerThreadSendPreview, SIGNAL(finished()), + workerThreadSendPreview, SLOT(deleteLater())); + workerThreadSendPreview->setClient(sendPreviewClient_); + workerThreadSendPreview->setRequest(request); + workerThreadSendPreview->start(); + + ui_.pushButtonPreview->setEnabled(false); +} + +void FreeGaitActionPlugin::onSendPreviewResult( + bool isOk, free_gait_msgs::ExecuteActionResult response) { + if (!isOk) { + emit statusMessage("Could not send preview action.", + MessageType::WARNING, 2.0); + } else { + evaluateFreeGaitActionResponse(response); + } + ui_.pushButtonPreview->setEnabled(true); +} + +void FreeGaitActionPlugin::onRefreshCollectionsClicked() { + clearActionListView(); + clearCollectionListView(); + ui_.pushButtonRefreshCollections->setEnabled(false); + + WorkerThreadUpdateTrigger *workerThreadUpdateTrigger = + new WorkerThreadUpdateTrigger; + connect(workerThreadUpdateTrigger, + SIGNAL(result(bool, std_srvs::TriggerResponse)), + this, + SLOT(onRefreshCollectionsResult(bool, std_srvs::TriggerResponse))); + connect(workerThreadUpdateTrigger, SIGNAL(finished()), + workerThreadUpdateTrigger, SLOT(deleteLater())); + workerThreadUpdateTrigger->setClient(refreshCollectionsClient_); + workerThreadUpdateTrigger->start(); +} + +void FreeGaitActionPlugin::onRefreshCollectionsResult( + bool isOk, std_srvs::TriggerResponse response) { + if (!isOk) { + emit statusMessage("Could not refresh collections.", + MessageType::WARNING, 2.0); + } else { + emit statusMessage("Collections refreshed.", MessageType::SUCCESS, 2.0); + } + + ui_.pushButtonRefreshCollections->setEnabled(true); + + getCollections(); +} + +void FreeGaitActionPlugin::onStatusMessage(std::string message, + MessageType type, + double displaySeconds) { + switch(type) { + case MessageType::ERROR: + statusBar_->setStyleSheet("border: none; color: red"); + ROS_ERROR_STREAM_NAMED(TAG, TAG << " " << message.c_str()); + break; + case MessageType::WARNING: + statusBar_->setStyleSheet("border: none; color: orange"); + ROS_WARN_STREAM_NAMED(TAG, TAG << " " << message.c_str()); + break; + case MessageType::SUCCESS: + statusBar_->setStyleSheet("border: none; color: green"); + ROS_INFO_STREAM_NAMED(TAG, TAG << " " << message.c_str()); + break; + case MessageType::STATUS: + statusBar_->setStyleSheet("border: none; color: black"); + ROS_INFO_STREAM_NAMED(TAG, TAG << " " << message.c_str()); + break; + } + statusBar_->showMessage(QString::fromStdString(message), + (int)(displaySeconds * 1000)); +} + +void FreeGaitActionPlugin::listViewCollectionsContextMenu(const QPoint &pos) { + QPoint globalPos = ui_.listViewCollections->mapToGlobal(pos); + + QModelIndex index = ui_.listViewCollections->indexAt(pos); + + if (index.isValid()) { + QString collectionId = collectionModel_->getCollectionId(index); + + QMenu menuForItem; + QAction* actionSetAsFavorite = menuForItem.addAction("Set as favorite"); + QAction* actionResult; + actionResult = menuForItem.exec(globalPos); + if(actionResult) { + if(actionResult == actionSetAsFavorite) { + favoriteCollectionId_ = collectionId; + setFavoriteActions(favoriteCollectionId_); + } + } + } +} + +void FreeGaitActionPlugin::onFavoriteButtonClicked(Action action) { + isSendingFavoriteAction_ = true; + + free_gait_msgs::SendActionRequest request; + request.goal.action_id = action.getId().toStdString(); + + WorkerThreadSendAction *workerThreadSendAction = new WorkerThreadSendAction; + connect(workerThreadSendAction, + SIGNAL(result(bool, free_gait_msgs::ExecuteActionResult)), + this, + SLOT(onFavoriteButtonResult(bool, + free_gait_msgs::ExecuteActionResult))); + connect(workerThreadSendAction, SIGNAL(finished()), + workerThreadSendAction, SLOT(deleteLater())); + workerThreadSendAction->setClient(sendActionClient_); + workerThreadSendAction->setRequest(request); + workerThreadSendAction->start(); + + for (auto button : favoritesPushButtons_) { + button->setEnabled(false); + } +} + +void FreeGaitActionPlugin::onFavoriteButtonResult( + bool isOk, free_gait_msgs::ExecuteActionResult response) { + if (!isOk) { + emit statusMessage("Could not send action.", MessageType::WARNING, 2.0); + } else { + evaluateFreeGaitActionResponse(response); + } + for (auto button : favoritesPushButtons_) { + button->setEnabled(true); + } + + isSendingFavoriteAction_ = false; +} + +} // namespace + +PLUGINLIB_EXPORT_CLASS(rqt_free_gait::FreeGaitActionPlugin, rqt_gui_cpp::Plugin) + diff --git a/rqt_free_gait_action/src/rqt_free_gait_action/WorkerThreadGetCollections.cpp b/rqt_free_gait_action/src/rqt_free_gait_action/WorkerThreadGetCollections.cpp new file mode 100644 index 0000000..13cb275 --- /dev/null +++ b/rqt_free_gait_action/src/rqt_free_gait_action/WorkerThreadGetCollections.cpp @@ -0,0 +1,122 @@ +/****************************************************************************** + * Copyright 2017 Samuel Bachmann, Peter Fankhauser * + * * + * Redistribution and use in source and binary forms, with or without * + * modification, are permitted provided that the following conditions are met:* + * * + * 1. Redistributions of source code must retain the above copyright notice, * + * this list of conditions and the following disclaimer. * + * * + * 2. Redistributions in binary form must reproduce the above copyright * + * notice, this list of conditions and the following disclaimer in the * + * documentation and/or other materials provided with the distribution. * + * * + * 3. Neither the name of the copyright holder nor the names of its * + * contributors may be used to endorse or promote products derived from this * + * software without specific prior written permission. * + * * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"* + * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE * + * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE * + * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE * + * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR * + * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF * + * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS * + * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN * + * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) * + * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF * + * THE POSSIBILITY OF SUCH DAMAGE. * + * * + * Authors: Samuel Bachmann , * + * Peter Fankhauser * + ******************************************************************************/ + +#include "rqt_free_gait_action/WorkerThreadGetCollections.h" + +namespace rqt_free_gait { + +/*****************************************************************************/ +/** Methods **/ +/*****************************************************************************/ + +void WorkerThreadGetCollections::run() { + CollectionModel *collectionModel = new CollectionModel(); + free_gait_msgs::GetCollectionsRequest collectionsRequest; + free_gait_msgs::GetCollectionsResponse collectionsResponse; + + if (!ros::service::waitForService(collectionsClient_.getService(), + ros::Duration(600.0))) { + ROS_WARN_STREAM_NAMED(TAG, TAG + << " Service: " << collectionsClient_.getService() + << " is not available. (Timeout 600.0 seconds)"); + emit result(false, collectionModel); + return; + } + if (!collectionsClient_.call(collectionsRequest, collectionsResponse)) { + emit result(false, collectionModel); + return; + } + + // Add pseudo empty collection to get all available actions. + free_gait_msgs::CollectionDescription collectionDescription; + collectionDescription.id = ""; + collectionsResponse.collections.insert(collectionsResponse.collections.begin(), + collectionDescription); + + // Loop over the collections, get their actions and add them finally to the + // collection model. + for (auto collectionItem : collectionsResponse.collections) { + // Get the actions of the collection. + free_gait_msgs::GetActionsRequest actionsRequest; + free_gait_msgs::GetActionsResponse actionsResponse; + actionsRequest.collection_id = collectionItem.id; + if (!actionsClient_.call(actionsRequest, actionsResponse)) { + ROS_WARN_STREAM_NAMED(TAG, TAG + << " Could not get action descriptions for collection id: " + << collectionItem.id); + continue; + } + + // Loop over the actions and add them to the action model. + ActionModel *actionModel = new ActionModel(); + for (auto actionItem : actionsResponse.actions) { + Action action(QString::fromStdString(actionItem.id), + QString::fromStdString(actionItem.name), + QString::fromStdString(actionItem.description)); + actionModel->addAction(action); + } + + + // Add the action model to the collection. + QString collectionId = "all"; + QString collectionName = "All"; + bool collectionIsSequence = false; + if (collectionItem.id.length() != 0) { + collectionId = QString::fromStdString(collectionItem.id); + collectionName = QString::fromStdString(collectionItem.name); + collectionIsSequence = collectionItem.is_sequence; + } + Collection collection(collectionId, collectionName, actionModel, collectionIsSequence); + + // Add the collection to the collection model. + collectionModel->addCollection(collection); + } + + // Return the collection model. + emit result(true, collectionModel); +} + +/*****************************************************************************/ +/** Accessors **/ +/*****************************************************************************/ + +void WorkerThreadGetCollections::setActionsClient(ros::ServiceClient &client) { + actionsClient_ = client; +} + +void WorkerThreadGetCollections::setCollectionsClient( + ros::ServiceClient &client) { + collectionsClient_ = client; +} + +} // namespace diff --git a/rqt_free_gait_action/src/rqt_free_gait_action/WorkerThreadSendAction.cpp b/rqt_free_gait_action/src/rqt_free_gait_action/WorkerThreadSendAction.cpp new file mode 100644 index 0000000..e502012 --- /dev/null +++ b/rqt_free_gait_action/src/rqt_free_gait_action/WorkerThreadSendAction.cpp @@ -0,0 +1,59 @@ +/****************************************************************************** + * Copyright 2017 Samuel Bachmann * + * * + * Redistribution and use in source and binary forms, with or without * + * modification, are permitted provided that the following conditions are met:* + * * + * 1. Redistributions of source code must retain the above copyright notice, * + * this list of conditions and the following disclaimer. * + * * + * 2. Redistributions in binary form must reproduce the above copyright * + * notice, this list of conditions and the following disclaimer in the * + * documentation and/or other materials provided with the distribution. * + * * + * 3. Neither the name of the copyright holder nor the names of its * + * contributors may be used to endorse or promote products derived from this * + * software without specific prior written permission. * + * * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"* + * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE * + * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE * + * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE * + * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR * + * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF * + * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS * + * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN * + * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) * + * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF * + * THE POSSIBILITY OF SUCH DAMAGE. * + * * + * Author: Samuel Bachmann * + ******************************************************************************/ + +#include "rqt_free_gait_action/WorkerThreadSendAction.h" + +namespace rqt_free_gait { + +/*****************************************************************************/ +/** Methods **/ +/*****************************************************************************/ + +void WorkerThreadSendAction::run() { + bool isOk = client_.call(request_, response_); + emit result(isOk, response_.result); +} + +/*****************************************************************************/ +/** Accessors **/ +/*****************************************************************************/ + +void WorkerThreadSendAction::setClient(ros::ServiceClient &client) { + client_ = client; +} + +void +WorkerThreadSendAction::setRequest(free_gait_msgs::SendActionRequest request) { + request_ = request; +} + +} // namespace diff --git a/rqt_free_gait_action/src/rqt_free_gait_action/WorkerThreadSendActionSequence.cpp b/rqt_free_gait_action/src/rqt_free_gait_action/WorkerThreadSendActionSequence.cpp new file mode 100644 index 0000000..6596a42 --- /dev/null +++ b/rqt_free_gait_action/src/rqt_free_gait_action/WorkerThreadSendActionSequence.cpp @@ -0,0 +1,60 @@ +/****************************************************************************** + * Copyright 2017 Samuel Bachmann, Peter Fankhauser * + * * + * Redistribution and use in source and binary forms, with or without * + * modification, are permitted provided that the following conditions are met:* + * * + * 1. Redistributions of source code must retain the above copyright notice, * + * this list of conditions and the following disclaimer. * + * * + * 2. Redistributions in binary form must reproduce the above copyright * + * notice, this list of conditions and the following disclaimer in the * + * documentation and/or other materials provided with the distribution. * + * * + * 3. Neither the name of the copyright holder nor the names of its * + * contributors may be used to endorse or promote products derived from this * + * software without specific prior written permission. * + * * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"* + * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE * + * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE * + * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE * + * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR * + * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF * + * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS * + * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN * + * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) * + * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF * + * THE POSSIBILITY OF SUCH DAMAGE. * + * * + * Authors: Samuel Bachmann , * + * Peter Fankhauser * + ******************************************************************************/ + +#include "rqt_free_gait_action/WorkerThreadSendActionSequence.h" + +namespace rqt_free_gait { + +/*****************************************************************************/ +/** Methods **/ +/*****************************************************************************/ + +void WorkerThreadSendActionSequence::run() { + bool isOk = client_.call(request_, response_); + emit result(isOk, response_.result); +} + +/*****************************************************************************/ +/** Accessors **/ +/*****************************************************************************/ + +void WorkerThreadSendActionSequence::setClient(ros::ServiceClient &client) { + client_ = client; +} + +void +WorkerThreadSendActionSequence::setRequest(free_gait_msgs::SendActionSequenceRequest request) { + request_ = request; +} + +} // namespace diff --git a/rqt_free_gait_action/src/rqt_free_gait_action/WorkerThreadUpdateTrigger.cpp b/rqt_free_gait_action/src/rqt_free_gait_action/WorkerThreadUpdateTrigger.cpp new file mode 100644 index 0000000..ef32b09 --- /dev/null +++ b/rqt_free_gait_action/src/rqt_free_gait_action/WorkerThreadUpdateTrigger.cpp @@ -0,0 +1,54 @@ +/****************************************************************************** + * Copyright 2017 Samuel Bachmann * + * * + * Redistribution and use in source and binary forms, with or without * + * modification, are permitted provided that the following conditions are met:* + * * + * 1. Redistributions of source code must retain the above copyright notice, * + * this list of conditions and the following disclaimer. * + * * + * 2. Redistributions in binary form must reproduce the above copyright * + * notice, this list of conditions and the following disclaimer in the * + * documentation and/or other materials provided with the distribution. * + * * + * 3. Neither the name of the copyright holder nor the names of its * + * contributors may be used to endorse or promote products derived from this * + * software without specific prior written permission. * + * * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"* + * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE * + * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE * + * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE * + * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR * + * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF * + * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS * + * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN * + * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) * + * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF * + * THE POSSIBILITY OF SUCH DAMAGE. * + * * + * Author: Samuel Bachmann * + ******************************************************************************/ + +#include "rqt_free_gait_action/WorkerThreadUpdateTrigger.h" + +namespace rqt_free_gait { + +/*****************************************************************************/ +/** Methods **/ +/*****************************************************************************/ + +void WorkerThreadUpdateTrigger::run() { + bool isOk = client_.call(request_, response_); + emit result(isOk, response_); +} + +/*****************************************************************************/ +/** Accessors **/ +/*****************************************************************************/ + +void WorkerThreadUpdateTrigger::setClient(ros::ServiceClient &client) { + client_ = client; +} + +} // namespace diff --git a/rqt_free_gait_monitor/CHANGELOG.rst b/rqt_free_gait_monitor/CHANGELOG.rst new file mode 100644 index 0000000..ac1a4be --- /dev/null +++ b/rqt_free_gait_monitor/CHANGELOG.rst @@ -0,0 +1,11 @@ +^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Changelog for package rqt_free_gait_monitor +^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ + +Forthcoming +----------- + +0.1.1 (2017-01-31) +------------------ +* Initial release. +* Contributors: Samuel Bachmann diff --git a/rqt_free_gait_monitor/CMakeLists.txt b/rqt_free_gait_monitor/CMakeLists.txt new file mode 100644 index 0000000..8aaa5b3 --- /dev/null +++ b/rqt_free_gait_monitor/CMakeLists.txt @@ -0,0 +1,156 @@ +cmake_minimum_required(VERSION 2.8.3) +project(rqt_free_gait_monitor) + +# Flags +set(CMAKE_CXX_FLAGS "-std=c++11 ${CMAKE_CXX_FLAGS}") + +# Load catkin and all dependencies required for this package +find_package(catkin REQUIRED COMPONENTS + rqt_gui + rqt_gui_cpp + roscpp + rospy + std_msgs + std_srvs + free_gait_msgs +) + +find_package(Boost REQUIRED COMPONENTS date_time) + +if ("${qt_gui_cpp_USE_QT_MAJOR_VERSION} " STREQUAL "5 ") + find_package(Qt5Widgets REQUIRED) +else() + find_package(Qt4 COMPONENTS QtCore QtGui REQUIRED) + include(${QT_USE_FILE}) +endif() + +set(${PROJECT_NAME}_SRCS + src/${PROJECT_NAME}/FreeGaitMonitorPlugin.cpp + src/rqt_free_gait_monitor/CircularBuffer.cpp + src/rqt_free_gait_monitor/WorkerThreadPausePlay.cpp + src/rqt_free_gait_monitor/WorkerThreadStop.cpp + src/rqt_free_gait_monitor/ClickableLabel.cpp +) + +set(${PROJECT_NAME}_HDRS_QT + include/${PROJECT_NAME}/FreeGaitMonitorPlugin.h + include/rqt_free_gait_monitor/WorkerThreadPausePlay.h + include/rqt_free_gait_monitor/WorkerThreadStop.h + include/rqt_free_gait_monitor/ClickableLabel.h +) +set(${PROJECT_NAME}_HDRS + include/rqt_free_gait_monitor/CircularBuffer.h +) + +set(${PROJECT_NAME}_UIS + resource/FreeGaitMonitorPlugin.ui +) + +set(${PROJECT_NAME}_QRC + resource/resources.qrc +) + +set(${PROJECT_NAME}_INCLUDE_DIRECTORIES + include + ${CMAKE_CURRENT_BINARY_DIR} +) + +set(ui_INCLUDE_DIR + "${CATKIN_DEVEL_PREFIX}/${CATKIN_GLOBAL_INCLUDE_DESTINATION}/${PROJECT_NAME}" +) + +set(${PROJECT_NAME}_INCLUDE_DIRECTORIES + include + ${ui_INCLUDE_DIR}/.. +) + +if(NOT EXISTS ${ui_INCLUDE_DIR}) + file(MAKE_DIRECTORY ${ui_INCLUDE_DIR}) +endif() + +catkin_package( + INCLUDE_DIRS + include +# LIBRARIES ${PROJECT_NAME} + CATKIN_DEPENDS + rqt_gui + rqt_gui_cpp + roscpp rospy + std_msgs + std_srvs + free_gait_msgs + DEPENDS + Boost +) + +if ("${qt_gui_cpp_USE_QT_MAJOR_VERSION} " STREQUAL "5 ") + qt5_wrap_cpp(${PROJECT_NAME}_MOCS ${${PROJECT_NAME}_HDRS_QT}) + qt5_add_resources(${PROJECT_NAME}_RCC ${${PROJECT_NAME}_QRC}) +else() + qt4_wrap_cpp(${PROJECT_NAME}_MOCS ${${PROJECT_NAME}_HDRS_QT}) + qt4_add_resources(${PROJECT_NAME}_RCC ${${PROJECT_NAME}_QRC}) +endif() + +# ensure generated header files are being created in the devel space +set(_cmake_current_binary_dir "${CMAKE_CURRENT_BINARY_DIR}") +set(CMAKE_CURRENT_BINARY_DIR ${ui_INCLUDE_DIR}) +if("${qt_gui_cpp_USE_QT_MAJOR_VERSION} " STREQUAL "5 ") + qt5_wrap_ui(${PROJECT_NAME}_UIS_H ${${PROJECT_NAME}_UIS}) +else() + qt4_wrap_ui(${PROJECT_NAME}_UIS_H ${${PROJECT_NAME}_UIS}) +endif() +set(CMAKE_CURRENT_BINARY_DIR "${_cmake_current_binary_dir}") + +include_directories(${${PROJECT_NAME}_INCLUDE_DIRECTORIES} + ${${PROJECT_NAME}_INCLUDE_DIRECTORIES} + ${catkin_INCLUDE_DIRS} + ${Boost_INCLUDE_DIRS} +) + +add_library(${PROJECT_NAME} + ${${PROJECT_NAME}_SRCS} + ${${PROJECT_NAME}_HDRS_QT} + ${${PROJECT_NAME}_HDRS} + ${${PROJECT_NAME}_MOCS} + ${${PROJECT_NAME}_UIS_H} + ${${PROJECT_NAME}_RCC} +) + +target_link_libraries(${PROJECT_NAME} + ${catkin_LIBRARIES} + ${Boost_LIBRARIES} +) + +if ("${qt_gui_cpp_USE_QT_MAJOR_VERSION} " STREQUAL "5 ") + target_link_libraries(${PROJECT_NAME} Qt5::Widgets) +else() + target_link_libraries(${PROJECT_NAME} ${QT_QTCORE_LIBRARY} ${QT_QTGUI_LIBRARY}) +endif() + +find_package(class_loader) +class_loader_hide_library_symbols(${PROJECT_NAME}) + +install(FILES plugin.xml + DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION} +) + +install(TARGETS ${PROJECT_NAME} + ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} + LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} + RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} +) + +install(PROGRAMS scripts/${PROJECT_NAME} + DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} +) + +install(DIRECTORY include/${PROJECT_NAME}/ + DESTINATION ${CATKIN_PACKAGE_INCLUDE_DESTINATION} +) + +install( + DIRECTORY resource + DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION} + FILES_MATCHING + PATTERN "*.svg" +) diff --git a/rqt_free_gait_monitor/include/rqt_free_gait_monitor/CircularBuffer.h b/rqt_free_gait_monitor/include/rqt_free_gait_monitor/CircularBuffer.h new file mode 100644 index 0000000..ff42457 --- /dev/null +++ b/rqt_free_gait_monitor/include/rqt_free_gait_monitor/CircularBuffer.h @@ -0,0 +1,104 @@ +/****************************************************************************** + * Copyright 2017 Samuel Bachmann * + * * + * Redistribution and use in source and binary forms, with or without * + * modification, are permitted provided that the following conditions are met:* + * * + * 1. Redistributions of source code must retain the above copyright notice, * + * this list of conditions and the following disclaimer. * + * * + * 2. Redistributions in binary form must reproduce the above copyright * + * notice, this list of conditions and the following disclaimer in the * + * documentation and/or other materials provided with the distribution. * + * * + * 3. Neither the name of the copyright holder nor the names of its * + * contributors may be used to endorse or promote products derived from this * + * software without specific prior written permission. * + * * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"* + * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE * + * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE * + * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE * + * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR * + * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF * + * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS * + * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN * + * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) * + * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF * + * THE POSSIBILITY OF SUCH DAMAGE. * + * * + * Author: Samuel Bachmann * + ******************************************************************************/ + +#pragma once + +#include +#include +#include + +namespace rqt_free_gait_monitor { + +struct description_t { + QString message; + QString timestamp; +}; + +class CircularBuffer { +public: + + /***************************************************************************/ + /** Constructor/Destructor **/ + /***************************************************************************/ + + CircularBuffer(unsigned int length); + + ~CircularBuffer(); + + /***************************************************************************/ + /** Accessors **/ + /***************************************************************************/ + + void push_back(description_t description); + + unsigned long size(); + + description_t back(); + + description_t front(); + + description_t current(); + + QString backQString(); + + QString frontQString(); + + QString currentQString(); + + int moveIndex(int steps); + + int moveIndexBack(); + + int moveIndexFront(); + + int index(); + + void clear(); + +private: + + /***************************************************************************/ + /** Variables **/ + /***************************************************************************/ + + int index_ = 0; + unsigned int length_; + std::deque descriptions_; + + /***************************************************************************/ + /** Methods **/ + /***************************************************************************/ + + QString composeDescription(description_t description); +}; + +} // namespace diff --git a/rqt_free_gait_monitor/include/rqt_free_gait_monitor/ClickableLabel.h b/rqt_free_gait_monitor/include/rqt_free_gait_monitor/ClickableLabel.h new file mode 100644 index 0000000..351666e --- /dev/null +++ b/rqt_free_gait_monitor/include/rqt_free_gait_monitor/ClickableLabel.h @@ -0,0 +1,68 @@ +/****************************************************************************** + * Copyright 2017 Samuel Bachmann * + * * + * Redistribution and use in source and binary forms, with or without * + * modification, are permitted provided that the following conditions are met:* + * * + * 1. Redistributions of source code must retain the above copyright notice, * + * this list of conditions and the following disclaimer. * + * * + * 2. Redistributions in binary form must reproduce the above copyright * + * notice, this list of conditions and the following disclaimer in the * + * documentation and/or other materials provided with the distribution. * + * * + * 3. Neither the name of the copyright holder nor the names of its * + * contributors may be used to endorse or promote products derived from this * + * software without specific prior written permission. * + * * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"* + * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE * + * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE * + * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE * + * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR * + * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF * + * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS * + * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN * + * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) * + * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF * + * THE POSSIBILITY OF SUCH DAMAGE. * + * * + * Author: Samuel Bachmann * + ******************************************************************************/ + +#pragma once + +#include + +namespace rqt_free_gait_monitor { + +class ClickableLabel : public QLabel { +Q_OBJECT +public: + + /***************************************************************************/ + /** Constructor/Destructor **/ + /***************************************************************************/ + + explicit ClickableLabel(QWidget *parent = 0); + + ~ClickableLabel(); + +signals: + + /***************************************************************************/ + /** Signals **/ + /***************************************************************************/ + + void clicked(); + +protected: + + /***************************************************************************/ + /** Events **/ + /***************************************************************************/ + + void mouseReleaseEvent(QMouseEvent *event); +}; + +} // namespace diff --git a/rqt_free_gait_monitor/include/rqt_free_gait_monitor/FreeGaitMonitorPlugin.h b/rqt_free_gait_monitor/include/rqt_free_gait_monitor/FreeGaitMonitorPlugin.h new file mode 100644 index 0000000..771c00a --- /dev/null +++ b/rqt_free_gait_monitor/include/rqt_free_gait_monitor/FreeGaitMonitorPlugin.h @@ -0,0 +1,204 @@ +/****************************************************************************** + * Copyright 2017 Samuel Bachmann * + * * + * Redistribution and use in source and binary forms, with or without * + * modification, are permitted provided that the following conditions are met:* + * * + * 1. Redistributions of source code must retain the above copyright notice, * + * this list of conditions and the following disclaimer. * + * * + * 2. Redistributions in binary form must reproduce the above copyright * + * notice, this list of conditions and the following disclaimer in the * + * documentation and/or other materials provided with the distribution. * + * * + * 3. Neither the name of the copyright holder nor the names of its * + * contributors may be used to endorse or promote products derived from this * + * software without specific prior written permission. * + * * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"* + * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE * + * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE * + * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE * + * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR * + * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF * + * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS * + * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN * + * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) * + * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF * + * THE POSSIBILITY OF SUCH DAMAGE. * + * * + * Author: Samuel Bachmann * + ******************************************************************************/ + +#pragma once + +#include "rqt_free_gait_monitor/CircularBuffer.h" +#include "rqt_free_gait_monitor/WorkerThreadPausePlay.h" +#include "rqt_free_gait_monitor/WorkerThreadStop.h" + +#include +#include + +#include +#include + +#include + +#include +#include + +#include +#include +#include + +#include +#include + +#include +#include +#include +#include +#include +#include +#include + +namespace rqt_free_gait_monitor { + +class FreeGaitMonitorPlugin : public rqt_gui_cpp::Plugin { + Q_OBJECT +public: + + /***************************************************************************/ + /** Constructor/Destructor **/ + /***************************************************************************/ + + FreeGaitMonitorPlugin(); + + /***************************************************************************/ + /** Initialization/Shutdown **/ + /***************************************************************************/ + + virtual void initPlugin(qt_gui_cpp::PluginContext &context); + + virtual void shutdownPlugin(); + + /***************************************************************************/ + /** Settings **/ + /***************************************************************************/ + + virtual void saveSettings(qt_gui_cpp::Settings &plugin_settings, + qt_gui_cpp::Settings &instance_settings) const; + + virtual void restoreSettings(const qt_gui_cpp::Settings &plugin_settings, + const qt_gui_cpp::Settings &instance_settings); + +protected: + + /***************************************************************************/ + /** Constants **/ + /***************************************************************************/ + + const std::string TAG = "FreeGaitMonitorPlugin"; + + /***************************************************************************/ + /** Variables **/ + /***************************************************************************/ + + Ui::FreeGaitMonitorPluginWidget ui_; + QWidget *widget_; + + ros::Subscriber goalSubscriber_; + ros::Subscriber feedbackSubscriber_; + ros::Subscriber resultSubscriber_; + + ros::ServiceClient pauseClient_; + ros::ServiceClient stopClient_; + + const double progressBarMultiplicator_ = 1000.0; + + bool isOnBottom_ = true; + + CircularBuffer descriptions_; + + ros::Time currentGoalStamp_; + + /***************************************************************************/ + /** Callbacks **/ + /***************************************************************************/ + + void goalCallback(const free_gait_msgs::ExecuteStepsActionGoalConstPtr &goal); + + void feedbackCallback( + const free_gait_msgs::ExecuteStepsActionFeedbackConstPtr &feedback); + + void resultCallback( + const free_gait_msgs::ExecuteStepsActionResultConstPtr &result); + + /***************************************************************************/ + /** Methods **/ + /***************************************************************************/ + + void updateNavigationButtonStates(); + + void expandDebug(); + + void collapseDebug(); + + /***************************************************************************/ + /** Events **/ + /***************************************************************************/ + + bool eventFilter(QObject *object, QEvent *event); + +protected slots: + + /***************************************************************************/ + /** Slots **/ + /***************************************************************************/ + + void updateGoal(free_gait_msgs::ExecuteStepsActionGoal goal); + + void updateFeedback(free_gait_msgs::ExecuteStepsActionFeedback feedback); + + void updateResult(free_gait_msgs::ExecuteStepsActionResult result); + + void onClickableLabelExpandCollapse(); + + void onPushButtonDeleteHistory(); + + void onPushButtonGoTop(); + + void onPushButtonGoUp(); + + void onPushButtonGoDown(); + + void onPushButtonGoBottom(); + + void onPushButtonPlay(); + + void onPushButtonPause(); + + void onPushButtonStop(); + + void onPushButtonPlayResult(bool isOk, std_srvs::SetBoolResponse response); + + void onPushButtonPauseResult(bool isOk, std_srvs::SetBoolResponse response); + + void onPushButtonStopResult(bool isOk, std_srvs::TriggerResponse response); + +signals: + + /***************************************************************************/ + /** Signals **/ + /***************************************************************************/ + + void updateGoalSignal(free_gait_msgs::ExecuteStepsActionGoal goal); + + void updateFeedbackSignal( + free_gait_msgs::ExecuteStepsActionFeedback feedback); + + void updateResultSignal(free_gait_msgs::ExecuteStepsActionResult result); + +}; + +} // namespace diff --git a/rqt_free_gait_monitor/include/rqt_free_gait_monitor/WorkerThreadPausePlay.h b/rqt_free_gait_monitor/include/rqt_free_gait_monitor/WorkerThreadPausePlay.h new file mode 100644 index 0000000..d84ee92 --- /dev/null +++ b/rqt_free_gait_monitor/include/rqt_free_gait_monitor/WorkerThreadPausePlay.h @@ -0,0 +1,81 @@ +/****************************************************************************** + * Copyright 2017 Samuel Bachmann * + * * + * Redistribution and use in source and binary forms, with or without * + * modification, are permitted provided that the following conditions are met:* + * * + * 1. Redistributions of source code must retain the above copyright notice, * + * this list of conditions and the following disclaimer. * + * * + * 2. Redistributions in binary form must reproduce the above copyright * + * notice, this list of conditions and the following disclaimer in the * + * documentation and/or other materials provided with the distribution. * + * * + * 3. Neither the name of the copyright holder nor the names of its * + * contributors may be used to endorse or promote products derived from this * + * software without specific prior written permission. * + * * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"* + * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE * + * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE * + * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE * + * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR * + * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF * + * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS * + * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN * + * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) * + * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF * + * THE POSSIBILITY OF SUCH DAMAGE. * + * * + * Author: Samuel Bachmann * + ******************************************************************************/ + +#pragma once + +#include + +#include + +#include + +namespace rqt_free_gait_monitor { + +class WorkerThreadPausePlay : public QThread { +Q_OBJECT + + /***************************************************************************/ + /** Methods **/ + /***************************************************************************/ + + void run(); + +public: + + /***************************************************************************/ + /** Accessors **/ + /***************************************************************************/ + + void setClient(ros::ServiceClient &client); + + void setRequest(std_srvs::SetBoolRequest request); + +private: + + /***************************************************************************/ + /** Variables **/ + /***************************************************************************/ + + std_srvs::SetBoolRequest request_; + std_srvs::SetBoolResponse response_; + ros::ServiceClient client_; + +signals: + + /***************************************************************************/ + /** Signals **/ + /***************************************************************************/ + + void result(bool isOk, std_srvs::SetBoolResponse); +}; + +} // namespace diff --git a/rqt_free_gait_monitor/include/rqt_free_gait_monitor/WorkerThreadStop.h b/rqt_free_gait_monitor/include/rqt_free_gait_monitor/WorkerThreadStop.h new file mode 100644 index 0000000..6cbbfab --- /dev/null +++ b/rqt_free_gait_monitor/include/rqt_free_gait_monitor/WorkerThreadStop.h @@ -0,0 +1,78 @@ +/****************************************************************************** + * Copyright 2017 Samuel Bachmann * + * * + * Redistribution and use in source and binary forms, with or without * + * modification, are permitted provided that the following conditions are met:* + * * + * 1. Redistributions of source code must retain the above copyright notice, * + * this list of conditions and the following disclaimer. * + * * + * 2. Redistributions in binary form must reproduce the above copyright * + * notice, this list of conditions and the following disclaimer in the * + * documentation and/or other materials provided with the distribution. * + * * + * 3. Neither the name of the copyright holder nor the names of its * + * contributors may be used to endorse or promote products derived from this * + * software without specific prior written permission. * + * * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"* + * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE * + * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE * + * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE * + * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR * + * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF * + * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS * + * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN * + * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) * + * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF * + * THE POSSIBILITY OF SUCH DAMAGE. * + * * + * Author: Samuel Bachmann * + ******************************************************************************/ + +#pragma once + +#include + +#include + +#include + +namespace rqt_free_gait_monitor { + +class WorkerThreadStop : public QThread { +Q_OBJECT + + /***************************************************************************/ + /** Methods **/ + /***************************************************************************/ + + void run(); + +public: + + /***************************************************************************/ + /** Accessors **/ + /***************************************************************************/ + + void setClient(ros::ServiceClient &client); + +private: + + /***************************************************************************/ + /** Variables **/ + /***************************************************************************/ + + std_srvs::TriggerResponse response_; + ros::ServiceClient client_; + +signals: + + /***************************************************************************/ + /** Signals **/ + /***************************************************************************/ + + void result(bool isOk, std_srvs::TriggerResponse); +}; + +} // namespace diff --git a/rqt_free_gait_monitor/launch/rqt_free_gait_monitor.launch b/rqt_free_gait_monitor/launch/rqt_free_gait_monitor.launch new file mode 100644 index 0000000..542179f --- /dev/null +++ b/rqt_free_gait_monitor/launch/rqt_free_gait_monitor.launch @@ -0,0 +1,5 @@ + + + + + diff --git a/rqt_free_gait_monitor/package.xml b/rqt_free_gait_monitor/package.xml new file mode 100644 index 0000000..e03c643 --- /dev/null +++ b/rqt_free_gait_monitor/package.xml @@ -0,0 +1,26 @@ + + + rqt_free_gait_monitor + 0.3.0 + A RQT plugin to monitor, pause, and stop Free Gait actions. + Samuel Bachmann + Samuel Bachmann + BSD + https://github.com/leggedrobotics/free_gait + https://github.com/leggedrobotics/free_gait/issues + + catkin + + rqt_gui + rqt_gui_cpp + roscpp + rospy + std_msgs + std_srvs + free_gait_msgs + boost + + + + + diff --git a/rqt_free_gait_monitor/plugin.xml b/rqt_free_gait_monitor/plugin.xml new file mode 100644 index 0000000..8e974c4 --- /dev/null +++ b/rqt_free_gait_monitor/plugin.xml @@ -0,0 +1,17 @@ + + + + A GUI plugin to monitor free gait. + + + + + folder + Plugins related to... + + + applications-graphics + A GUI plugin to monitor free gait. + + + diff --git a/rqt_free_gait_monitor/resource/FreeGaitMonitorPlugin.ui b/rqt_free_gait_monitor/resource/FreeGaitMonitorPlugin.ui new file mode 100644 index 0000000..17114aa --- /dev/null +++ b/rqt_free_gait_monitor/resource/FreeGaitMonitorPlugin.ui @@ -0,0 +1,425 @@ + + + FreeGaitMonitorPluginWidget + + + true + + + + 0 + 0 + 725 + 475 + + + + Free Gait Monitor + + + + + + <html><head/><body><p><span style=" font-weight:600;">Feedback</span></p></body></html> + + + + + + + + + true + + + 24 + + + + + + + + + + 24 + + + %v/%m s + + + + + + + + 0 + 25 + + + + Step Progress + + + + + + + + 0 + 25 + + + + Progress + + + + + + + + 0 + 25 + + + + Status + + + + + + + + 0 + 25 + + + + Active Branches + + + + + + + + + + :/icons/16x16/done.svg + + + + + + + + + ... + + + + + + + Qt::Horizontal + + + + 40 + 20 + + + + + + + + + + + + + + + + + :/icons/16x16/expand.svg + + + + + + + Description + + + + + + + Qt::Horizontal + + + + 40 + 20 + + + + + + + + ... + + + + + + + / + + + + + + + ... + + + + + + + + + 0 + + + + + + 8 + + + + QTextEdit::AutoAll + + + true + + + + + + + + 0 + + + 0 + + + 0 + + + 0 + + + + + Show the first step + + + + + + + :/icons/16x16/go-top.svg:/icons/16x16/go-top.svg + + + true + + + + + + + Qt::Vertical + + + + 0 + 0 + + + + + + + + + + + + :/icons/16x16/go-down.svg:/icons/16x16/go-down.svg + + + true + + + + + + + Qt::Vertical + + + + 0 + 0 + + + + + + + + Show the latest step + + + + + + + :/icons/16x16/go-bottom.svg:/icons/16x16/go-bottom.svg + + + true + + + + + + + + + + + :/icons/16x16/go-up.svg:/icons/16x16/go-up.svg + + + true + + + + + + + Delete history + + + + + + + :/icons/16x16/delete.svg:/icons/16x16/delete.svg + + + true + + + + + + + + + + + + Qt::Vertical + + + + 20 + 0 + + + + + + + + + + + + + + :/icons/16x16/play.svg:/icons/16x16/play.svg + + + + + + + + + + + :/icons/16x16/pause.svg:/icons/16x16/pause.svg + + + + + + + + + + + :/icons/16x16/stop.svg:/icons/16x16/stop.svg + + + + + + + Qt::Horizontal + + + + 40 + 20 + + + + + + + + + + + rqt_free_gait_monitor::ClickableLabel + QLabel +
rqt_free_gait_monitor/ClickableLabel.h
+ + + + pushButtonGoTop + pushButtonGoUp + pushButtonGoDown + pushButtonGoBottom + pushButtonPlay + pushButtonPause + pushButtonStop + + + + + + diff --git a/rqt_free_gait_monitor/resource/icons/16x16/collapse.svg b/rqt_free_gait_monitor/resource/icons/16x16/collapse.svg new file mode 100644 index 0000000..61a5c5f --- /dev/null +++ b/rqt_free_gait_monitor/resource/icons/16x16/collapse.svg @@ -0,0 +1,147 @@ + + + + + + + + image/svg+xml + + + + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/rqt_free_gait_monitor/resource/icons/16x16/delete.svg b/rqt_free_gait_monitor/resource/icons/16x16/delete.svg new file mode 100644 index 0000000..02dd423 --- /dev/null +++ b/rqt_free_gait_monitor/resource/icons/16x16/delete.svg @@ -0,0 +1,124 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/rqt_free_gait_monitor/resource/icons/16x16/done.svg b/rqt_free_gait_monitor/resource/icons/16x16/done.svg new file mode 100644 index 0000000..f009b90 --- /dev/null +++ b/rqt_free_gait_monitor/resource/icons/16x16/done.svg @@ -0,0 +1,18 @@ + + + + + + + + + + + + + + + + + + diff --git a/rqt_free_gait_monitor/resource/icons/16x16/expand.svg b/rqt_free_gait_monitor/resource/icons/16x16/expand.svg new file mode 100644 index 0000000..a508267 --- /dev/null +++ b/rqt_free_gait_monitor/resource/icons/16x16/expand.svg @@ -0,0 +1,147 @@ + + + + + + + + image/svg+xml + + + + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/rqt_free_gait_monitor/resource/icons/16x16/failed.svg b/rqt_free_gait_monitor/resource/icons/16x16/failed.svg new file mode 100644 index 0000000..1c9162c --- /dev/null +++ b/rqt_free_gait_monitor/resource/icons/16x16/failed.svg @@ -0,0 +1,28 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/rqt_free_gait_monitor/resource/icons/16x16/go-bottom.svg b/rqt_free_gait_monitor/resource/icons/16x16/go-bottom.svg new file mode 100644 index 0000000..f4c45f5 --- /dev/null +++ b/rqt_free_gait_monitor/resource/icons/16x16/go-bottom.svg @@ -0,0 +1,43 @@ + + + + + + + image/svg+xml + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/rqt_free_gait_monitor/resource/icons/16x16/go-down.svg b/rqt_free_gait_monitor/resource/icons/16x16/go-down.svg new file mode 100644 index 0000000..5da1ba0 --- /dev/null +++ b/rqt_free_gait_monitor/resource/icons/16x16/go-down.svg @@ -0,0 +1,26 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/rqt_free_gait_monitor/resource/icons/16x16/go-top.svg b/rqt_free_gait_monitor/resource/icons/16x16/go-top.svg new file mode 100644 index 0000000..376a833 --- /dev/null +++ b/rqt_free_gait_monitor/resource/icons/16x16/go-top.svg @@ -0,0 +1,43 @@ + + + + + + + image/svg+xml + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/rqt_free_gait_monitor/resource/icons/16x16/go-up.svg b/rqt_free_gait_monitor/resource/icons/16x16/go-up.svg new file mode 100644 index 0000000..e5e5867 --- /dev/null +++ b/rqt_free_gait_monitor/resource/icons/16x16/go-up.svg @@ -0,0 +1,28 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/rqt_free_gait_monitor/resource/icons/16x16/pause.svg b/rqt_free_gait_monitor/resource/icons/16x16/pause.svg new file mode 100644 index 0000000..fde2760 --- /dev/null +++ b/rqt_free_gait_monitor/resource/icons/16x16/pause.svg @@ -0,0 +1,25 @@ + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/rqt_free_gait_monitor/resource/icons/16x16/play.svg b/rqt_free_gait_monitor/resource/icons/16x16/play.svg new file mode 100644 index 0000000..87c34d3 --- /dev/null +++ b/rqt_free_gait_monitor/resource/icons/16x16/play.svg @@ -0,0 +1,18 @@ + + + + + + + + + + + + + + + + + + diff --git a/rqt_free_gait_monitor/resource/icons/16x16/stop.svg b/rqt_free_gait_monitor/resource/icons/16x16/stop.svg new file mode 100644 index 0000000..acfdd24 --- /dev/null +++ b/rqt_free_gait_monitor/resource/icons/16x16/stop.svg @@ -0,0 +1,33 @@ + + + + + + + + + + + + + + + + + + + + + + + image/svg+xml + + + + + + + + + + diff --git a/rqt_free_gait_monitor/resource/icons/16x16/unknown.svg b/rqt_free_gait_monitor/resource/icons/16x16/unknown.svg new file mode 100644 index 0000000..e8e3a96 --- /dev/null +++ b/rqt_free_gait_monitor/resource/icons/16x16/unknown.svg @@ -0,0 +1,27 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/rqt_free_gait_monitor/resource/icons/16x16/warning.svg b/rqt_free_gait_monitor/resource/icons/16x16/warning.svg new file mode 100644 index 0000000..4e8e0be --- /dev/null +++ b/rqt_free_gait_monitor/resource/icons/16x16/warning.svg @@ -0,0 +1,27 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/rqt_free_gait_monitor/resource/icons/22x22/done.svg b/rqt_free_gait_monitor/resource/icons/22x22/done.svg new file mode 100644 index 0000000..9a53707 --- /dev/null +++ b/rqt_free_gait_monitor/resource/icons/22x22/done.svg @@ -0,0 +1,23 @@ + + + + + + + + + + + + + + + + + + + + + + + diff --git a/rqt_free_gait_monitor/resource/icons/22x22/failed.svg b/rqt_free_gait_monitor/resource/icons/22x22/failed.svg new file mode 100644 index 0000000..41d64fa --- /dev/null +++ b/rqt_free_gait_monitor/resource/icons/22x22/failed.svg @@ -0,0 +1,30 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/rqt_free_gait_monitor/resource/icons/22x22/pause.svg b/rqt_free_gait_monitor/resource/icons/22x22/pause.svg new file mode 100644 index 0000000..bd118d7 --- /dev/null +++ b/rqt_free_gait_monitor/resource/icons/22x22/pause.svg @@ -0,0 +1,23 @@ + + + + + + + + + + + + + + + + + + + + + + + diff --git a/rqt_free_gait_monitor/resource/icons/22x22/play.svg b/rqt_free_gait_monitor/resource/icons/22x22/play.svg new file mode 100644 index 0000000..634cff3 --- /dev/null +++ b/rqt_free_gait_monitor/resource/icons/22x22/play.svg @@ -0,0 +1,20 @@ + + + + + + + + + + + + + + + + + + + + diff --git a/rqt_free_gait_monitor/resource/icons/22x22/stop.svg b/rqt_free_gait_monitor/resource/icons/22x22/stop.svg new file mode 100644 index 0000000..7f923f9 --- /dev/null +++ b/rqt_free_gait_monitor/resource/icons/22x22/stop.svg @@ -0,0 +1,33 @@ + + + + + + + + + + + + + + + + + + + + + + + image/svg+xml + + + + + + + + + + diff --git a/rqt_free_gait_monitor/resource/icons/22x22/unknown.svg b/rqt_free_gait_monitor/resource/icons/22x22/unknown.svg new file mode 100644 index 0000000..6292b96 --- /dev/null +++ b/rqt_free_gait_monitor/resource/icons/22x22/unknown.svg @@ -0,0 +1,26 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/rqt_free_gait_monitor/resource/icons/22x22/warning.svg b/rqt_free_gait_monitor/resource/icons/22x22/warning.svg new file mode 100644 index 0000000..b0544af --- /dev/null +++ b/rqt_free_gait_monitor/resource/icons/22x22/warning.svg @@ -0,0 +1,30 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/rqt_free_gait_monitor/resource/resources.qrc b/rqt_free_gait_monitor/resource/resources.qrc new file mode 100644 index 0000000..173f5d1 --- /dev/null +++ b/rqt_free_gait_monitor/resource/resources.qrc @@ -0,0 +1,18 @@ + + + icons/16x16/done.svg + icons/16x16/failed.svg + icons/16x16/go-bottom.svg + icons/16x16/go-down.svg + icons/16x16/go-top.svg + icons/16x16/go-up.svg + icons/16x16/pause.svg + icons/16x16/play.svg + icons/16x16/stop.svg + icons/16x16/unknown.svg + icons/16x16/warning.svg + icons/16x16/expand.svg + icons/16x16/collapse.svg + icons/16x16/delete.svg + + \ No newline at end of file diff --git a/rqt_free_gait_monitor/scripts/rqt_free_gait_monitor b/rqt_free_gait_monitor/scripts/rqt_free_gait_monitor new file mode 100755 index 0000000..e211e23 --- /dev/null +++ b/rqt_free_gait_monitor/scripts/rqt_free_gait_monitor @@ -0,0 +1,8 @@ +#!/usr/bin/env python + +import sys + +from rqt_gui.main import Main + +main = Main() +sys.exit(main.main(sys.argv, standalone='rqt_free_gait_monitor/FreeGaitMonitorPlugin')) diff --git a/rqt_free_gait_monitor/setup.py b/rqt_free_gait_monitor/setup.py new file mode 100644 index 0000000..6dd4baa --- /dev/null +++ b/rqt_free_gait_monitor/setup.py @@ -0,0 +1,11 @@ +#!/usr/bin/env python + +from distutils.core import setup +from catkin_pkg.python_setup import generate_distutils_setup + +d = generate_distutils_setup( + packages=['rqt_free_gait_monitor'], + package_dir={'': 'src'} +) + +setup(**d) diff --git a/rqt_free_gait_monitor/src/rqt_free_gait_monitor/CircularBuffer.cpp b/rqt_free_gait_monitor/src/rqt_free_gait_monitor/CircularBuffer.cpp new file mode 100644 index 0000000..098c59b --- /dev/null +++ b/rqt_free_gait_monitor/src/rqt_free_gait_monitor/CircularBuffer.cpp @@ -0,0 +1,134 @@ +/****************************************************************************** + * Copyright 2017 Samuel Bachmann * + * * + * Redistribution and use in source and binary forms, with or without * + * modification, are permitted provided that the following conditions are met:* + * * + * 1. Redistributions of source code must retain the above copyright notice, * + * this list of conditions and the following disclaimer. * + * * + * 2. Redistributions in binary form must reproduce the above copyright * + * notice, this list of conditions and the following disclaimer in the * + * documentation and/or other materials provided with the distribution. * + * * + * 3. Neither the name of the copyright holder nor the names of its * + * contributors may be used to endorse or promote products derived from this * + * software without specific prior written permission. * + * * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"* + * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE * + * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE * + * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE * + * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR * + * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF * + * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS * + * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN * + * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) * + * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF * + * THE POSSIBILITY OF SUCH DAMAGE. * + * * + * Author: Samuel Bachmann * + ******************************************************************************/ + +#include "rqt_free_gait_monitor/CircularBuffer.h" + +namespace rqt_free_gait_monitor { + +/*****************************************************************************/ +/** Constructor/Destructor **/ +/*****************************************************************************/ + +CircularBuffer::CircularBuffer(unsigned int length) : length_(length) { + +} + +CircularBuffer::~CircularBuffer() { + +} + +/*****************************************************************************/ +/** Accessors **/ +/*****************************************************************************/ + +void CircularBuffer::push_back(description_t description) { + QRegExp regExp; + regExp.setPattern("\\n[-]{3,10}\\n"); + description.message.replace(regExp, "
"); + regExp.setPattern("\\n"); + description.message.replace(regExp, "
"); + descriptions_.push_back(description); + if (descriptions_.size() > length_) { + descriptions_.pop_front(); + index_--; + index_ = std::max(index_, 0); + } +} + +unsigned long CircularBuffer::size() { + return descriptions_.size(); +} + +int CircularBuffer::moveIndex(int steps) { + index_ += steps; + if (index_ < 0) { + index_ = 0; + } else if (index_ > size() - 1) { + index_ = (int)size() - 1; + } + return index_; +} + +description_t CircularBuffer::back() { + return descriptions_.back(); +} + +description_t CircularBuffer::front() { + return descriptions_.front(); +} + +description_t CircularBuffer::current() { + return descriptions_[index_]; +} + +QString CircularBuffer::backQString() { + return composeDescription(descriptions_.back()); +} + +QString CircularBuffer::frontQString() { + return composeDescription(descriptions_.front()); +} + +QString CircularBuffer::currentQString() { + return composeDescription(descriptions_[index_]); +} + +int CircularBuffer::index() { + return index_; +} + +int CircularBuffer::moveIndexBack() { + index_ = (int)size() - 1; + return index_; +} + +int CircularBuffer::moveIndexFront() { + index_ = 0; + return index_; +} + +void CircularBuffer::clear() { + index_ = 0; + descriptions_.clear(); +} + +/*****************************************************************************/ +/** Methods **/ +/*****************************************************************************/ + +QString CircularBuffer::composeDescription(description_t description) { + QString str = "" + description.timestamp + "" + "
" + + description.message + ""; + return str; +} + +} // namespace diff --git a/rqt_free_gait_monitor/src/rqt_free_gait_monitor/ClickableLabel.cpp b/rqt_free_gait_monitor/src/rqt_free_gait_monitor/ClickableLabel.cpp new file mode 100644 index 0000000..36949ad --- /dev/null +++ b/rqt_free_gait_monitor/src/rqt_free_gait_monitor/ClickableLabel.cpp @@ -0,0 +1,57 @@ +/****************************************************************************** + * Copyright 2017 Samuel Bachmann * + * * + * Redistribution and use in source and binary forms, with or without * + * modification, are permitted provided that the following conditions are met:* + * * + * 1. Redistributions of source code must retain the above copyright notice, * + * this list of conditions and the following disclaimer. * + * * + * 2. Redistributions in binary form must reproduce the above copyright * + * notice, this list of conditions and the following disclaimer in the * + * documentation and/or other materials provided with the distribution. * + * * + * 3. Neither the name of the copyright holder nor the names of its * + * contributors may be used to endorse or promote products derived from this * + * software without specific prior written permission. * + * * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"* + * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE * + * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE * + * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE * + * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR * + * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF * + * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS * + * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN * + * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) * + * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF * + * THE POSSIBILITY OF SUCH DAMAGE. * + * * + * Author: Samuel Bachmann * + ******************************************************************************/ + +#include "rqt_free_gait_monitor/ClickableLabel.h" +#include + +namespace rqt_free_gait_monitor { + +/*****************************************************************************/ +/** Constructor/Destructor **/ +/*****************************************************************************/ + +ClickableLabel::ClickableLabel(QWidget *parent) + : QLabel(parent) { +} + +ClickableLabel::~ClickableLabel() { +} + +/*****************************************************************************/ +/** Events **/ +/*****************************************************************************/ + +void ClickableLabel::mouseReleaseEvent(QMouseEvent *event) { + emit clicked(); +} + +} // namespace diff --git a/rqt_free_gait_monitor/src/rqt_free_gait_monitor/FreeGaitMonitorPlugin.cpp b/rqt_free_gait_monitor/src/rqt_free_gait_monitor/FreeGaitMonitorPlugin.cpp new file mode 100644 index 0000000..3f7a875 --- /dev/null +++ b/rqt_free_gait_monitor/src/rqt_free_gait_monitor/FreeGaitMonitorPlugin.cpp @@ -0,0 +1,567 @@ +/****************************************************************************** + * Copyright 2017 Samuel Bachmann * + * * + * Redistribution and use in source and binary forms, with or without * + * modification, are permitted provided that the following conditions are met:* + * * + * 1. Redistributions of source code must retain the above copyright notice, * + * this list of conditions and the following disclaimer. * + * * + * 2. Redistributions in binary form must reproduce the above copyright * + * notice, this list of conditions and the following disclaimer in the * + * documentation and/or other materials provided with the distribution. * + * * + * 3. Neither the name of the copyright holder nor the names of its * + * contributors may be used to endorse or promote products derived from this * + * software without specific prior written permission. * + * * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"* + * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE * + * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE * + * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE * + * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR * + * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF * + * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS * + * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN * + * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) * + * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF * + * THE POSSIBILITY OF SUCH DAMAGE. * + * * + * Author: Samuel Bachmann * + ******************************************************************************/ + +#include "rqt_free_gait_monitor/FreeGaitMonitorPlugin.h" + +#include + +namespace rqt_free_gait_monitor { + +/*****************************************************************************/ +/** Constructor/Destructor **/ +/*****************************************************************************/ + +FreeGaitMonitorPlugin::FreeGaitMonitorPlugin() + : rqt_gui_cpp::Plugin(), widget_(0), descriptions_(1000) { + + setObjectName("FreeGaitMonitorPlugin"); + + qRegisterMetaType + ("free_gait_msgs::ExecuteStepsActionGoal"); + qRegisterMetaType + ("free_gait_msgs::ExecuteStepsActionFeedback"); + qRegisterMetaType + ("free_gait_msgs::ExecuteStepsActionResult"); + qRegisterMetaType + ("std_srvs::SetBoolResponse"); + qRegisterMetaType + ("std_srvs::TriggerResponse"); +} + +/*****************************************************************************/ +/** Initialization/Shutdown **/ +/*****************************************************************************/ + +void FreeGaitMonitorPlugin::initPlugin(qt_gui_cpp::PluginContext &context) { + widget_ = new QWidget(); + ui_.setupUi(widget_); + if (context.serialNumber() > 1) { + widget_->setWindowTitle(widget_->windowTitle() + + " (" + QString::number(context.serialNumber()) + ")"); + } + context.addWidget(widget_); + + // Initialize subscribers. + goalSubscriber_ = getNodeHandle().subscribe< + free_gait_msgs::ExecuteStepsActionGoal>( + getNodeHandle().param("/free_gait/action_server", "") + + "/goal", 10, &FreeGaitMonitorPlugin::goalCallback, this); + feedbackSubscriber_ = getNodeHandle().subscribe< + free_gait_msgs::ExecuteStepsActionFeedback>( + getNodeHandle().param("/free_gait/action_server", "") + + "/feedback", 10, &FreeGaitMonitorPlugin::feedbackCallback, this); + resultSubscriber_ = getNodeHandle().subscribe< + free_gait_msgs::ExecuteStepsActionResult>( + getNodeHandle().param("/free_gait/action_server", "") + + "/result", 10, &FreeGaitMonitorPlugin::resultCallback, this); + + // Initialize service clients. + pauseClient_ = getNodeHandle().serviceClient( + getNodeHandle().param( + "/free_gait/pause_execution_service", ""), false); + + stopClient_ = getNodeHandle().serviceClient( + getNodeHandle().param( + "/free_gait/stop_execution_service", ""), false); + + // Initialize progress bar. + ui_.progressBarAll->setMinimum(0); + ui_.progressBarAll->setMaximum(1); + ui_.progressBarAll->setValue(1); + ui_.progressBarAll->setFormat(""); + + ui_.progressBarStep->setMinimum(0); + ui_.progressBarStep->setMaximum(1); + ui_.progressBarStep->setValue(1); + ui_.progressBarStep->setFormat(""); + + // Set icon. + ui_.labelStatus->setPixmap(QPixmap(":/icons/16x16/done.svg")); + + // Initialize buttons. + ui_.pushButtonPlay->setEnabled(false); + ui_.pushButtonPause->setEnabled(false); + ui_.pushButtonStop->setEnabled(false); + + // Initialize navigation buttons. + updateNavigationButtonStates(); + ui_.widgetScrollArea->installEventFilter(this); + ui_.pushButtonGoTop->installEventFilter(this); + ui_.pushButtonGoUp->installEventFilter(this); + ui_.pushButtonGoDown->installEventFilter(this); + ui_.pushButtonGoBottom->installEventFilter(this); + + // Initialize debug info. + collapseDebug(); + + // Connect signals and slots. + connect(this, + SIGNAL(updateGoalSignal(free_gait_msgs::ExecuteStepsActionGoal)), + this, + SLOT(updateGoal(free_gait_msgs::ExecuteStepsActionGoal))); + connect(this, + SIGNAL(updateFeedbackSignal( + free_gait_msgs::ExecuteStepsActionFeedback)), + this, + SLOT(updateFeedback(free_gait_msgs::ExecuteStepsActionFeedback))); + connect(this, + SIGNAL(updateResultSignal(free_gait_msgs::ExecuteStepsActionResult)), + this, + SLOT(updateResult(free_gait_msgs::ExecuteStepsActionResult))); + + connect(ui_.pushButtonGoTop, SIGNAL(clicked()), + this, SLOT(onPushButtonGoTop())); + connect(ui_.pushButtonGoUp, SIGNAL(clicked()), + this, SLOT(onPushButtonGoUp())); + connect(ui_.pushButtonGoDown, SIGNAL(clicked()), + this, SLOT(onPushButtonGoDown())); + connect(ui_.pushButtonGoBottom, SIGNAL(clicked()), + this, SLOT(onPushButtonGoBottom())); + + connect(ui_.pushButtonPlay, SIGNAL(clicked()), + this, SLOT(onPushButtonPlay())); + connect(ui_.pushButtonPause, SIGNAL(clicked()), + this, SLOT(onPushButtonPause())); + connect(ui_.pushButtonStop, SIGNAL(clicked()), + this, SLOT(onPushButtonStop())); + + connect(ui_.clickableLabelExpandCollapse, SIGNAL(clicked()), + this, SLOT(onClickableLabelExpandCollapse())); + connect(ui_.pushButtonDeleteHistory, SIGNAL(clicked()), + this, SLOT(onPushButtonDeleteHistory())); +} + +void FreeGaitMonitorPlugin::shutdownPlugin() { + goalSubscriber_.shutdown(); + feedbackSubscriber_.shutdown(); + resultSubscriber_.shutdown(); +} + +/*****************************************************************************/ +/** Settings **/ +/*****************************************************************************/ + +void FreeGaitMonitorPlugin::saveSettings( + qt_gui_cpp::Settings &plugin_settings, + qt_gui_cpp::Settings &instance_settings) const { +} + +void FreeGaitMonitorPlugin::restoreSettings( + const qt_gui_cpp::Settings &plugin_settings, + const qt_gui_cpp::Settings &instance_settings) { +} + +/*****************************************************************************/ +/** Callbacks **/ +/*****************************************************************************/ + +void FreeGaitMonitorPlugin::goalCallback( + const free_gait_msgs::ExecuteStepsActionGoalConstPtr &goal) { + emit updateGoalSignal(*goal); +} + +void FreeGaitMonitorPlugin::feedbackCallback( + const free_gait_msgs::ExecuteStepsActionFeedbackConstPtr &feedback) { + emit updateFeedbackSignal(*feedback); +} + +void FreeGaitMonitorPlugin::resultCallback( + const free_gait_msgs::ExecuteStepsActionResultConstPtr &result) { + emit updateResultSignal(*result); +} + +/*****************************************************************************/ +/** Methods **/ +/*****************************************************************************/ + +void FreeGaitMonitorPlugin::updateNavigationButtonStates() { + if (descriptions_.size() > 0) { + ui_.labelStepNumber->setText(QString::number(descriptions_.index() + 1)); + ui_.labelStepMax->setText(QString::number(descriptions_.size())); + + ui_.pushButtonDeleteHistory->setEnabled(true); + } else { + ui_.labelStepNumber->setText("..."); + ui_.labelStepMax->setText("..."); + + ui_.pushButtonDeleteHistory->setEnabled(false); + } + + if (isOnBottom_) { + ui_.pushButtonGoBottom->setEnabled(false); + ui_.pushButtonGoDown->setEnabled(false); + ui_.pushButtonGoUp->setEnabled(descriptions_.size() > 1); + ui_.pushButtonGoTop->setEnabled(descriptions_.size() > 1); + return; + } + + ui_.pushButtonGoBottom->setEnabled(true); + ui_.pushButtonGoDown->setEnabled( + descriptions_.size() > 1 && descriptions_.index() != + descriptions_.size()-1); + ui_.pushButtonGoUp->setEnabled( + descriptions_.size() > 1 && descriptions_.index() != 0); + ui_.pushButtonGoTop->setEnabled( + descriptions_.size() > 1 && descriptions_.index() != 0); +} + +void FreeGaitMonitorPlugin::expandDebug() { + ui_.plainTextEditDescription->show(); + ui_.widgetScrollArea->show(); +} + +void FreeGaitMonitorPlugin::collapseDebug() { + ui_.plainTextEditDescription->hide(); + ui_.widgetScrollArea->hide(); +} + +/*****************************************************************************/ +/** Events **/ +/*****************************************************************************/ + +bool FreeGaitMonitorPlugin::eventFilter(QObject *object, QEvent *event) { + if (event->type() == QEvent::Wheel) { + QWheelEvent *wheelEvent = static_cast(event); + int numDegrees = wheelEvent->delta() / 8; + int numSteps = numDegrees / 15; + + descriptions_.moveIndex(-numSteps); + ui_.plainTextEditDescription->setText(descriptions_.currentQString()); + isOnBottom_ = false; + updateNavigationButtonStates(); + + wheelEvent->accept(); + } + return QObject::eventFilter(object, event); +} + +/*****************************************************************************/ +/** Slots **/ +/*****************************************************************************/ + +void FreeGaitMonitorPlugin::updateGoal(free_gait_msgs::ExecuteStepsActionGoal goal) { + // get goal time stamp + currentGoalStamp_ = goal.goal_id.stamp; + + // init progress bar + int totalSteps = (int)goal.goal.steps.size(); + ui_.progressBarAll->setMinimum(0); + ui_.progressBarAll->setMaximum( + (int)(progressBarMultiplicator_ * totalSteps)); + ui_.progressBarAll->setValue(0); + std::stringstream progressBarTextFreeGait; + progressBarTextFreeGait << 0 << "/" << totalSteps << " steps"; + ui_.progressBarAll->setFormat(QString::fromStdString( + progressBarTextFreeGait.str())); + + ui_.progressBarStep->setMinimum(0); + ui_.progressBarStep->setMaximum(1); + ui_.progressBarStep->setValue(0); + ui_.progressBarStep->setFormat(""); + + // reset text + updateNavigationButtonStates(); + + ui_.plainTextEditDescription->setText(" "); + + // pause/play button + ui_.pushButtonStop->setEnabled(true); + ui_.pushButtonPause->setEnabled(true); + ui_.pushButtonPlay->setEnabled(false); + + // update status + ui_.labelStatus->setPixmap(QPixmap(":/icons/16x16/play.svg")); +} + +void FreeGaitMonitorPlugin::updateFeedback( + free_gait_msgs::ExecuteStepsActionFeedback feedback) { + // update progress bar + double totalSteps = (double)feedback.feedback.number_of_steps_in_goal; + double stepNumber = (double)feedback.feedback.step_number; + if (stepNumber < 0.0) { + stepNumber = 0.0; + } + double freeGaitProgress = stepNumber + feedback.feedback.phase; + ui_.progressBarAll->setMaximum((int)(progressBarMultiplicator_ * totalSteps)); + ui_.progressBarAll->setValue((int)( + progressBarMultiplicator_ * freeGaitProgress)); + std::stringstream progressBarTextFreeGait; + progressBarTextFreeGait << (int)stepNumber << "/" + << (int)totalSteps << " steps"; + ui_.progressBarAll->setFormat(QString::fromStdString( + progressBarTextFreeGait.str())); + + double stepProgress = feedback.feedback.phase * + feedback.feedback.duration.toSec(); + double stepMaximum = feedback.feedback.duration.toSec(); + ui_.progressBarStep->setMinimum(0); + ui_.progressBarStep->setMaximum((int)( + progressBarMultiplicator_ * stepMaximum)); + ui_.progressBarStep->setValue((int)( + progressBarMultiplicator_ * stepProgress)); + std::stringstream progressBarText; + progressBarText << std::fixed << std::setprecision(2); + progressBarText << stepProgress << "/" << stepMaximum << " s"; + ui_.progressBarStep->setFormat(QString::fromStdString(progressBarText.str())); + + // update text + if (!feedback.feedback.description.empty()) { + description_t description; + description.message = QString::fromStdString( + feedback.feedback.description); + boost::posix_time::ptime now = boost::posix_time::second_clock::local_time(); + std::string timestamp = boost::posix_time::to_simple_string(now); + description.timestamp = QString::fromStdString(timestamp); + descriptions_.push_back(description); + + if (isOnBottom_) { + descriptions_.moveIndexBack(); + ui_.plainTextEditDescription->setText(descriptions_.backQString()); + } else { + ui_.plainTextEditDescription->setText(descriptions_.currentQString()); + } + } + updateNavigationButtonStates(); + + // update legs + QString activeBranches = ""; + for (auto active_branch : feedback.feedback.active_branches) { + activeBranches += QString::fromStdString(active_branch) + " "; + } + ui_.labelActiveBranches->setText(activeBranches); + + // update status + switch (feedback.feedback.status) { + case free_gait_msgs::ExecuteStepsFeedback::PROGRESS_PAUSED: + ui_.labelStatus->setPixmap(QPixmap(":/icons/16x16/pause.svg")); + break; + case free_gait_msgs::ExecuteStepsFeedback::PROGRESS_EXECUTING: + ui_.labelStatus->setPixmap(QPixmap(":/icons/16x16/play.svg")); + break; + case free_gait_msgs::ExecuteStepsFeedback::PROGRESS_UNKNOWN: + ui_.labelStatus->setPixmap(QPixmap(":/icons/16x16/unknown.svg")); + break; + default: + ui_.labelStatus->setPixmap(QPixmap(":/icons/16x16/warning.svg")); + break; + } +} + +void FreeGaitMonitorPlugin::updateResult( + free_gait_msgs::ExecuteStepsActionResult result) { + // reset progress bar + ui_.progressBarAll->setMinimum(0); + ui_.progressBarAll->setMaximum(1); + ui_.progressBarAll->setValue(1); + ui_.progressBarAll->setFormat(""); + + ui_.progressBarStep->setMinimum(0); + ui_.progressBarStep->setMaximum(1); + ui_.progressBarStep->setValue(1); + ui_.progressBarStep->setFormat(""); + + // reset legs + ui_.labelActiveBranches->setText("..."); + + // reset status + bool setButtons = true; + switch (result.status.status) { + case actionlib_msgs::GoalStatus::SUCCEEDED: + case actionlib_msgs::GoalStatus::PREEMPTED: + case actionlib_msgs::GoalStatus::RECALLED: + ui_.labelStatus->setPixmap(QPixmap(":/icons/16x16/done.svg")); + break; + case actionlib_msgs::GoalStatus::ABORTED: + case actionlib_msgs::GoalStatus::REJECTED: + ui_.labelStatus->setPixmap(QPixmap(":/icons/16x16/failed.svg")); + break; + case actionlib_msgs::GoalStatus::LOST: + setButtons = false; + ui_.labelStatus->setPixmap(QPixmap(":/icons/16x16/unknown.svg")); + break; + default: + ui_.labelStatus->setPixmap(QPixmap(":/icons/16x16/warning.svg")); + break; + } + + // if result from the current goal + if (setButtons && result.status.goal_id.stamp == currentGoalStamp_) { + // pause/play button + ui_.pushButtonStop->setEnabled(false); + ui_.pushButtonPause->setEnabled(false); + ui_.pushButtonPlay->setEnabled(false); + } +} + +void FreeGaitMonitorPlugin::onPushButtonGoTop() { + descriptions_.moveIndexFront(); + ui_.plainTextEditDescription->setText(descriptions_.currentQString()); + isOnBottom_ = false; + + updateNavigationButtonStates(); +} + +void FreeGaitMonitorPlugin::onPushButtonGoUp() { + descriptions_.moveIndex(-1); + ui_.plainTextEditDescription->setText(descriptions_.currentQString()); + isOnBottom_ = false; + + updateNavigationButtonStates(); +} + +void FreeGaitMonitorPlugin::onPushButtonGoDown() { + descriptions_.moveIndex(1); + ui_.plainTextEditDescription->setText(descriptions_.currentQString()); + isOnBottom_ = false; + + updateNavigationButtonStates(); +} + +void FreeGaitMonitorPlugin::onPushButtonGoBottom() { + descriptions_.moveIndexBack(); + ui_.plainTextEditDescription->setText(descriptions_.backQString()); + isOnBottom_ = true; + + updateNavigationButtonStates(); +} + +void FreeGaitMonitorPlugin::onPushButtonPlay() { + ui_.pushButtonPause->setEnabled(false); + ui_.pushButtonPlay->setEnabled(false); + + std_srvs::SetBoolRequest request; + request.data = false; + + WorkerThreadPausePlay *workerThreadPausePlay = new WorkerThreadPausePlay; + connect(workerThreadPausePlay, + SIGNAL(result(bool, std_srvs::SetBoolResponse)), + this, + SLOT(onPushButtonPlayResult(bool, std_srvs::SetBoolResponse))); + connect(workerThreadPausePlay, SIGNAL(finished()), + workerThreadPausePlay, SLOT(deleteLater())); + workerThreadPausePlay->setClient(pauseClient_); + workerThreadPausePlay->setRequest(request); + workerThreadPausePlay->start(); +} + +void FreeGaitMonitorPlugin::onPushButtonPause() { + ui_.pushButtonPause->setEnabled(false); + ui_.pushButtonPlay->setEnabled(false); + + std_srvs::SetBoolRequest request; + request.data = true; + + WorkerThreadPausePlay *workerThreadPausePlay = new WorkerThreadPausePlay; + connect(workerThreadPausePlay, + SIGNAL(result(bool, std_srvs::SetBoolResponse)), + this, + SLOT(onPushButtonPauseResult(bool, std_srvs::SetBoolResponse))); + connect(workerThreadPausePlay, SIGNAL(finished()), + workerThreadPausePlay, SLOT(deleteLater())); + workerThreadPausePlay->setClient(pauseClient_); + workerThreadPausePlay->setRequest(request); + workerThreadPausePlay->start(); +} + +void FreeGaitMonitorPlugin::onPushButtonStop() { + ui_.pushButtonStop->setEnabled(false); + ui_.pushButtonPause->setEnabled(false); + ui_.pushButtonPlay->setEnabled(false); + + WorkerThreadStop *workerThreadStop = new WorkerThreadStop; + connect(workerThreadStop, + SIGNAL(result(bool, std_srvs::TriggerResponse)), + this, + SLOT(onPushButtonStopResult(bool, std_srvs::TriggerResponse))); + connect(workerThreadStop, SIGNAL(finished()), + workerThreadStop, SLOT(deleteLater())); + workerThreadStop->setClient(stopClient_); + workerThreadStop->start(); +} + +void FreeGaitMonitorPlugin::onPushButtonPlayResult( + bool isOk, std_srvs::SetBoolResponse response) { + if (isOk && response.success) { + ui_.pushButtonPause->setEnabled(true); + ui_.pushButtonPlay->setEnabled(false); + } else { + ui_.pushButtonPause->setEnabled(false); + ui_.pushButtonPlay->setEnabled(true); + } +} + +void FreeGaitMonitorPlugin::onPushButtonPauseResult( + bool isOk, std_srvs::SetBoolResponse response) { + if (isOk && response.success) { + ui_.pushButtonPause->setEnabled(false); + ui_.pushButtonPlay->setEnabled(true); + } else { + ui_.pushButtonPause->setEnabled(true); + ui_.pushButtonPlay->setEnabled(false); + } +} + +void FreeGaitMonitorPlugin::onPushButtonStopResult( + bool isOk, std_srvs::TriggerResponse response) { + if (isOk && response.success) { + ui_.pushButtonStop->setEnabled(false); + ui_.pushButtonPause->setEnabled(false); + ui_.pushButtonPlay->setEnabled(false); + } else { + ui_.pushButtonStop->setEnabled(true); + ui_.pushButtonPause->setEnabled(true); + ui_.pushButtonPlay->setEnabled(false); + } +} + +void FreeGaitMonitorPlugin::onClickableLabelExpandCollapse() { + if (ui_.plainTextEditDescription->isVisible()) { + collapseDebug(); + ui_.clickableLabelExpandCollapse->setPixmap( + QPixmap(":/icons/16x16/expand.svg")); + } else { + expandDebug(); + ui_.clickableLabelExpandCollapse->setPixmap( + QPixmap(":/icons/16x16/collapse.svg")); + } +} + +void FreeGaitMonitorPlugin::onPushButtonDeleteHistory() { + descriptions_.clear(); + ui_.plainTextEditDescription->setText(""); + + updateNavigationButtonStates(); +} + +} // namespace + +PLUGINLIB_EXPORT_CLASS(rqt_free_gait_monitor::FreeGaitMonitorPlugin, rqt_gui_cpp::Plugin) diff --git a/rqt_free_gait_monitor/src/rqt_free_gait_monitor/WorkerThreadPausePlay.cpp b/rqt_free_gait_monitor/src/rqt_free_gait_monitor/WorkerThreadPausePlay.cpp new file mode 100644 index 0000000..120a301 --- /dev/null +++ b/rqt_free_gait_monitor/src/rqt_free_gait_monitor/WorkerThreadPausePlay.cpp @@ -0,0 +1,59 @@ +/****************************************************************************** + * Copyright 2017 Samuel Bachmann * + * * + * Redistribution and use in source and binary forms, with or without * + * modification, are permitted provided that the following conditions are met:* + * * + * 1. Redistributions of source code must retain the above copyright notice, * + * this list of conditions and the following disclaimer. * + * * + * 2. Redistributions in binary form must reproduce the above copyright * + * notice, this list of conditions and the following disclaimer in the * + * documentation and/or other materials provided with the distribution. * + * * + * 3. Neither the name of the copyright holder nor the names of its * + * contributors may be used to endorse or promote products derived from this * + * software without specific prior written permission. * + * * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"* + * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE * + * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE * + * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE * + * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR * + * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF * + * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS * + * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN * + * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) * + * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF * + * THE POSSIBILITY OF SUCH DAMAGE. * + * * + * Author: Samuel Bachmann * + ******************************************************************************/ + +#include "rqt_free_gait_monitor/WorkerThreadPausePlay.h" + +namespace rqt_free_gait_monitor { + +/*****************************************************************************/ +/** Methods **/ +/*****************************************************************************/ + +void WorkerThreadPausePlay::run() { + bool isOk = client_.call(request_, response_); + emit result(isOk, response_); +} + +/*****************************************************************************/ +/** Accessors **/ +/*****************************************************************************/ + +void WorkerThreadPausePlay::setClient(ros::ServiceClient &client) { + client_ = client; +} + +void +WorkerThreadPausePlay::setRequest(std_srvs::SetBoolRequest request) { + request_ = request; +} + +} // namespace diff --git a/rqt_free_gait_monitor/src/rqt_free_gait_monitor/WorkerThreadStop.cpp b/rqt_free_gait_monitor/src/rqt_free_gait_monitor/WorkerThreadStop.cpp new file mode 100644 index 0000000..2228f9c --- /dev/null +++ b/rqt_free_gait_monitor/src/rqt_free_gait_monitor/WorkerThreadStop.cpp @@ -0,0 +1,55 @@ +/****************************************************************************** + * Copyright 2017 Samuel Bachmann * + * * + * Redistribution and use in source and binary forms, with or without * + * modification, are permitted provided that the following conditions are met:* + * * + * 1. Redistributions of source code must retain the above copyright notice, * + * this list of conditions and the following disclaimer. * + * * + * 2. Redistributions in binary form must reproduce the above copyright * + * notice, this list of conditions and the following disclaimer in the * + * documentation and/or other materials provided with the distribution. * + * * + * 3. Neither the name of the copyright holder nor the names of its * + * contributors may be used to endorse or promote products derived from this * + * software without specific prior written permission. * + * * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"* + * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE * + * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE * + * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE * + * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR * + * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF * + * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS * + * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN * + * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) * + * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF * + * THE POSSIBILITY OF SUCH DAMAGE. * + * * + * Author: Samuel Bachmann * + ******************************************************************************/ + +#include "rqt_free_gait_monitor/WorkerThreadStop.h" + +namespace rqt_free_gait_monitor { + +/*****************************************************************************/ +/** Methods **/ +/*****************************************************************************/ + +void WorkerThreadStop::run() { + std_srvs::TriggerRequest request; + bool isOk = client_.call(request, response_); + emit result(isOk, response_); +} + +/*****************************************************************************/ +/** Accessors **/ +/*****************************************************************************/ + +void WorkerThreadStop::setClient(ros::ServiceClient &client) { + client_ = client; +} + +} // namespace