diff --git a/src/interaction_msgs/srv/TaskEvaluation.srv b/src/interaction_msgs/srv/TaskEvaluation.srv index ea15f77..4e7f5b1 100644 --- a/src/interaction_msgs/srv/TaskEvaluation.srv +++ b/src/interaction_msgs/srv/TaskEvaluation.srv @@ -1,12 +1,14 @@ bool user_input uint16 number_of_population +float32 duration +uint16 number_of_time_steps # case if user_input is true float32[] user_parameters # Length: number_of_dimensions * number_of_parameters_per_dimension float32[] user_covariance_diag # Length: number_of_dimensions * number_of_parameters_per_dimension float32[] current_cma_mean # Length: number_of_dimensions * number_of_parameters_per_dimension float32[] conditional_points # Length: (number_of_dimensions + time_stamp[0,1]) * number_of_conditional_points -float32 weight_parameter # this parameter sets the weighted average 0 dont trust user 1 completly trust user (it is set by the user or it is decays over time i have to do some experiments on that) +float32[] weight_parameter # this parameter sets the weighted average 0 dont trust user 1 completly trust user (it is set by the user or it is decays over time i have to do some experiments on that) # case if user_input is false uint16 number_of_dimensions # this is the number of ProMPs * 2 (Position and Velocity) diff --git a/src/interaction_optimizers/interaction_optimizers/cmaes_optimization_node.py b/src/interaction_optimizers/interaction_optimizers/cmaes_optimization_node.py index fbb98ff..3808b18 100644 --- a/src/interaction_optimizers/interaction_optimizers/cmaes_optimization_node.py +++ b/src/interaction_optimizers/interaction_optimizers/cmaes_optimization_node.py @@ -48,21 +48,22 @@ class CMAESOptimizationNode(Node): self.heartbeat_timeout = 30 # secs self.heartbeat_timer = self.create_timer(1.0, self.check_heartbeats) - # MR Heartbeat + # Heartbeat self.last_mr_heartbeat_time = None self.mr_heartbeat_sub = self.create_subscription(Bool, 'interaction/mr_heartbeat', self.mr_heatbeat_callback) self.last_task_heartbeat_time = None self.task_heartbeat_sub = self.create_subscription(Bool, 'interaction/task_heartbeat', self.task_heartbeat_callback) - # Topics + # Topic # Service - self.parameter_srv = self.create_service(ParameterChange, 'cmaes_parameter_srv', self.parameter_callback) - self.query_srv = self.create_client(Query, 'query_srv') - self.task_srv = self.create_client(TaskEvaluation, 'task_srv') - self.user_interface_srv = self.create_client(UserInterface, 'user_interface_srv') + self.parameter_srv = self.create_service(ParameterChange, 'interaction/cmaes_parameter_srv', self.parameter_callback) + self.query_srv = self.create_client(Query, 'interaction/query_srv') + self.task_srv = self.create_client(TaskEvaluation, 'interaction/task_srv') + self.user_interface_srv = self.create_client(UserInterface, 'interaction/user_interface_srv') - # Define states + # State Machine + # States self.states = [ 'idle', 'initialization', @@ -338,3 +339,14 @@ class CMAESOptimizationNode(Node): self.get_logger().error(f'Task service call failed: {e}') self.error_trigger() # endregion + + +def main(args=None): + rclpy.init(args=args) + node = CMAESOptimizationNode() + rclpy.spin(node) + rclpy.shutdown() + + +if __name__ == '__main__': + main() diff --git a/src/interaction_optimizers/package.xml b/src/interaction_optimizers/package.xml index 404340c..2278d21 100644 --- a/src/interaction_optimizers/package.xml +++ b/src/interaction_optimizers/package.xml @@ -7,6 +7,9 @@ niko TODO: License declaration + interaction_msgs + rclpy + ament_copyright ament_flake8 ament_pep257 diff --git a/src/interaction_tasks/interaction_tasks/__init__.py b/src/interaction_tasks/interaction_tasks/__init__.py new file mode 100644 index 0000000..e69de29 diff --git a/src/interaction_tasks/interaction_tasks/task_node.py b/src/interaction_tasks/interaction_tasks/task_node.py new file mode 100644 index 0000000..8c3a518 --- /dev/null +++ b/src/interaction_tasks/interaction_tasks/task_node.py @@ -0,0 +1,63 @@ +import rclpy +from rclpy.node import Node +from rclpy.parameter import Parameter + +from transitions import Machine +import yaml +import numpy as np +from movement_primitives.promp import ProMP +from src.interaction_utils.serialization import flatten_population, unflatten_population + +from interaction_msgs.srv import TaskEvaluation +from std_msgs.msg import Bool + + +class TaskNode(Node): + def __init__(self): + super().__init__('task_node') + + # Task Attributes + + # ROS2 Interfaces + + # Heartbeat + self.heartbeat_pub = self.create_publisher(Bool, 'interaction/task_heartbeat', 10) + self.heartbeat_timer = self.create_timer(5.0, self.send_heartbeat) + # Topic + + # Service + + # Action + self._goal_handle = None + + # State Machine + # States + self.states = [ + 'waiting_for_task_specs', + 'processing_non_interactive_input', + 'processing_interactive_inpute', + 'waiting_for_robot_response', + 'waiting_for_objective_function_response', + 'error_recovery' + ] + + # initialize state machine + self.machine = Machine(self, states=self.states, initial='waiting_for_task_specs') + + self.machine.add_transition(trigger='non_interactive_specs_received', source='waiting_for_task_specs', dest='processing_non_interactive_input') + self.machine.add_transition(trigger='interactive_specs_received', source='waiting_for_task_specs', dest='processing_interactive_input') + self.machine.add_transition(trigger='non_interactive_to_robot', source='processing_non_interactive_input', dest='waiting_for_robot_response') + self.machine.add_transition(trigger='non_interactive_to_obj_fun', source='processing_non_interactive_input', dest='waiting_for_objective_function_response') + self.machine.add_transition(trigger='interactive_to_robot', source='processing_interactive_input', dest='waiting_for_robot_response') + self.machine.add_transition(trigger='interactive_to_obj_fun', source='processing_interactive_input', dest='waiting_for_objective_function_response') + self.machine.add_transition(trigger='sending_robot_results', source='waiting_for_robot_response', dest='waiting_for_task_specs') + self.machine.add_transition(trigger='sending_obj_fun_results', source='waiting_for_obj_fun_response', dest='waiting_for_task_specs') + self.machine.add_transition(trigger='error_trigger', source='*', dest='error_recovery') + self.machine.add_transition(trigger='recovery_complete', source='error_recovery', dest='waiting_for_task_specs') + + # State Methods + + # Callback functions + def send_heartbeat(self): + msg = Bool() + self.heartbeat_pub.publish(msg) diff --git a/src/interaction_tasks/package.xml b/src/interaction_tasks/package.xml new file mode 100644 index 0000000..c1bd463 --- /dev/null +++ b/src/interaction_tasks/package.xml @@ -0,0 +1,21 @@ + + + + interaction_tasks + 0.0.0 + TODO: Package description + niko + TODO: License declaration + + interaction_msgs + rclpy + + ament_copyright + ament_flake8 + ament_pep257 + python3-pytest + + + ament_python + + diff --git a/src/interaction_tasks/resource/interaction_tasks b/src/interaction_tasks/resource/interaction_tasks new file mode 100644 index 0000000..e69de29 diff --git a/src/interaction_tasks/setup.cfg b/src/interaction_tasks/setup.cfg new file mode 100644 index 0000000..b2c04c6 --- /dev/null +++ b/src/interaction_tasks/setup.cfg @@ -0,0 +1,4 @@ +[develop] +script_dir=$base/lib/interaction_tasks +[install] +install_scripts=$base/lib/interaction_tasks diff --git a/src/interaction_tasks/setup.py b/src/interaction_tasks/setup.py new file mode 100644 index 0000000..6bb0806 --- /dev/null +++ b/src/interaction_tasks/setup.py @@ -0,0 +1,25 @@ +from setuptools import find_packages, setup + +package_name = 'interaction_tasks' + +setup( + name=package_name, + version='0.0.0', + packages=find_packages(exclude=['test']), + data_files=[ + ('share/ament_index/resource_index/packages', + ['resource/' + package_name]), + ('share/' + package_name, ['package.xml']), + ], + install_requires=['setuptools'], + zip_safe=True, + maintainer='niko', + maintainer_email='nikolaus.feith@unileoben.ac.at', + description='TODO: Package description', + license='TODO: License declaration', + tests_require=['pytest'], + entry_points={ + 'console_scripts': [ + ], + }, +) diff --git a/src/interaction_tasks/test/test_copyright.py b/src/interaction_tasks/test/test_copyright.py new file mode 100644 index 0000000..97a3919 --- /dev/null +++ b/src/interaction_tasks/test/test_copyright.py @@ -0,0 +1,25 @@ +# Copyright 2015 Open Source Robotics Foundation, Inc. +# +# Licensed under the Apache License, Version 2.0 (the "License"); +# you may not use this file except in compliance with the License. +# You may obtain a copy of the License at +# +# http://www.apache.org/licenses/LICENSE-2.0 +# +# Unless required by applicable law or agreed to in writing, software +# distributed under the License is distributed on an "AS IS" BASIS, +# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +# See the License for the specific language governing permissions and +# limitations under the License. + +from ament_copyright.main import main +import pytest + + +# Remove the `skip` decorator once the source file(s) have a copyright header +@pytest.mark.skip(reason='No copyright header has been placed in the generated source file.') +@pytest.mark.copyright +@pytest.mark.linter +def test_copyright(): + rc = main(argv=['.', 'test']) + assert rc == 0, 'Found errors' diff --git a/src/interaction_tasks/test/test_flake8.py b/src/interaction_tasks/test/test_flake8.py new file mode 100644 index 0000000..27ee107 --- /dev/null +++ b/src/interaction_tasks/test/test_flake8.py @@ -0,0 +1,25 @@ +# Copyright 2017 Open Source Robotics Foundation, Inc. +# +# Licensed under the Apache License, Version 2.0 (the "License"); +# you may not use this file except in compliance with the License. +# You may obtain a copy of the License at +# +# http://www.apache.org/licenses/LICENSE-2.0 +# +# Unless required by applicable law or agreed to in writing, software +# distributed under the License is distributed on an "AS IS" BASIS, +# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +# See the License for the specific language governing permissions and +# limitations under the License. + +from ament_flake8.main import main_with_errors +import pytest + + +@pytest.mark.flake8 +@pytest.mark.linter +def test_flake8(): + rc, errors = main_with_errors(argv=[]) + assert rc == 0, \ + 'Found %d code style errors / warnings:\n' % len(errors) + \ + '\n'.join(errors) diff --git a/src/interaction_tasks/test/test_pep257.py b/src/interaction_tasks/test/test_pep257.py new file mode 100644 index 0000000..b234a38 --- /dev/null +++ b/src/interaction_tasks/test/test_pep257.py @@ -0,0 +1,23 @@ +# Copyright 2015 Open Source Robotics Foundation, Inc. +# +# Licensed under the Apache License, Version 2.0 (the "License"); +# you may not use this file except in compliance with the License. +# You may obtain a copy of the License at +# +# http://www.apache.org/licenses/LICENSE-2.0 +# +# Unless required by applicable law or agreed to in writing, software +# distributed under the License is distributed on an "AS IS" BASIS, +# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +# See the License for the specific language governing permissions and +# limitations under the License. + +from ament_pep257.main import main +import pytest + + +@pytest.mark.linter +@pytest.mark.pep257 +def test_pep257(): + rc = main(argv=['.', 'test']) + assert rc == 0, 'Found code style errors / warnings'