diff --git a/.LICENSE b/.LICENSE new file mode 100644 index 0000000..b22b1d1 --- /dev/null +++ b/.LICENSE @@ -0,0 +1,13 @@ +# Copyright 2023 Carologistics +# +# 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. diff --git a/.flake8 b/.flake8 deleted file mode 100644 index 7fc980c..0000000 --- a/.flake8 +++ /dev/null @@ -1,3 +0,0 @@ -[flake8] -exclude = .git -max-line-length = 130 diff --git a/CONTRIBUTING.md b/CONTRIBUTING.md deleted file mode 100644 index 0703ae9..0000000 --- a/CONTRIBUTING.md +++ /dev/null @@ -1,3 +0,0 @@ -Any new contributions that you make to this repository will -be under the Apache 2.0 License, as dictated by that -[license](http://www.apache.org/licenses/LICENSE-2.0). diff --git a/README.md b/README.md index 3a0fa17..c2586b9 100644 --- a/README.md +++ b/README.md @@ -1,101 +1,17 @@ -# FlexBE States and Behaviors for caro_skills +# Installing Flexbe -Generic template for a behaviors repository to be used for new projects +Follow the README in the [flexbe behavior engine repo](https://github.com/FlexBE/flexbe_behavior_engine) and the [flexbe webUI](https://github.com/FlexBE/flexbe_webui). -Modify this README as needed for your specific project details. +# Storing Behaviors -Below we provide basic details, but you are free to delete or modify this README as you wish. +Behaviors typically edited and generated by the FlexBE UI. +These generated files are stored in the +`install/caro_skills_flexbe_behaviors/lib/caro_skills_flexbe_behaviors/manifest` and +`install/caro_skills_flexbe_behaviors/python3.12/site-packages/caro_skills_flexbe_behaviors` folder. ----- +To store it properly, save these files in the `src/caro_flexbe_behavior/manifest` and `src/caro_skills_flexbe_behaviors/caro_skills_flexbe_behaviors` folders. -This raw repository has several folders and files with the generic name `caro_skills`. - - -This repository is used by the FlexBE widget -[`create_repo`](https://github.com/FlexBE/flexbe_behavior_engine/blob/ros2-devel/flexbe_widget/bin/create_repo) -script to create an example project that you can build off of to add your own states and behaviors. - -Using `ros2 run flexbe_widget create_repo ` will clone this repository, -and change the relevant `caro_skills` text to `my_new_project_name` as needed. - -It sets up the `package.xml` files with proper FlexBE export tags. -It is maintained at version `0.0.1` as the starting point for your work. - -We have provided a license file to conform to ROS guidelines; however, you are free to replace the -`LICENSE` file, and apply whatever license you choose to states and behaviors that you create. - -This repository contains an example behavior and examples for writing your own state implementations. - -## Example States in `caro_skills_flexbe_states` - -Packages providing FlexBE states are identified by an export tag in the `package.xml`: - -```xml - - - ament_cmake - -``` - -* `example_state.py ` - * Example state implementation with extra console logging to show the state life cycle. - -* `example_action_state.py` - -> Note: These example states are defined with extra console logging that is useful when learning FlexBE, -> but you will typically not include so much of the `Logger.info` commands as in these examples. - -> Note: You are free to copy and modify these files to create your own files and publish under your own license terms. -> As per the existing licenses, no warranty is implied. - -## Example Behaviors in `caro_skills_flexbe_behaviors` - -Packages providing FlexBE behaviors are identified by an export tag in the `package.xml`: - -```xml - - - ament_cmake - -``` - - * `example_behavior_sm.py` - * Most basic example state machine - - * `example_action_behavior_sm.py` - * Uses the `ExampleActionState` with the standard action tutorials - - [Understanding ROS2 Actions](https://docs.ros.org/en/iron/Tutorials/Beginner-CLI-Tools/Understanding-ROS2-Actions/Understanding-ROS2-Actions.html) - - [Introducing Turtlesim](https://docs.ros.org/en/iron/Tutorials/Beginner-CLI-Tools/Introducing-Turtlesim/Introducing-Turtlesim.html) - - To execute the associated behavior in FlexBE, you need to first run the turtlesim node that provdes the action server - - `ros2 run turtlesim turtlesim_node` - - To display the available actions: - - `ros2 action list` - - The action is defined by: - - `/turtle1/rotate_absolute:` [`turtlesim/action/RotateAbsolute`](https://docs.ros2.org/latest/api/turtlesim/action/RotateAbsolute.html) - -Behaviors typically edited and generated by the FlexBE UI. -These generated files are stored in the root workspace `install` folder. -Presuming a `WORKSPACE_ROOT` environment variable exists, we provide a simple -[`copy_behavior`](caro_skills_flexbe_behaviors/bin/copy_behavior) script to copy a saved behavior -— both the Python implementation and manifest `.xml` file — -to the project source folder for long term storage. -Use `ros2 run caro_skills_flexbe_behavior copy_behavior` to see the usage guide. -The script should be run from this repository's base folder. - -For a Quick-start and more comprehensive introduction to FlexBE, -see the [FlexBE Turtlesim Demonstrations](https://github.com/FlexBE/flexbe_turtlesim_demo). - -# Our instructions: - -### start WebUI: +# Start WebUI: ```ros2 launch flexbe_webui flexbe_ocs.launch.py headless:=True``` @@ -103,7 +19,7 @@ see the [FlexBE Turtlesim Demonstrations](https://github.com/FlexBE/flexbe_turtl ```ros2 launch flexbe_onboard behavior_onboard.launch.py``` -## start behavior without WebUI: +# Start behavior without WebUI: requires flexbe_behavior_engine to be on carologistic's fork on branch [mtschesche/action_server_jazzy](https://github.com/carologistics/flexbe_behavior_engine/tree/mtschesche/action_server_jazzy) @@ -111,8 +27,13 @@ requires flexbe_behavior_engine to be on carologistic's fork on branch [mtschesc ```ros2 run flexbe_widget be_action_server``` -example: +## Examples for starting behaviors via action server: +MoveTo: ``` ros2 action send_goal /flexbe/execute_behavior flexbe_msgs/action/BehaviorExecution "{behavior_name: 'MoveTo', input_keys: ['frame_id','target_x','target_y','target_yaw'], input_values: ['map','3.0','4.5','1.0']}" ``` +MotorMove: +``` +ros2 action send_goal /flexbe/execute_behavior flexbe_msgs/action/BehaviorExecution "{behavior_name: 'MotorMove', input_keys: ['frame_id','target_x','target_y','target_yaw'], input_values: ['robotinobase1/base_link','0.0','-10.0','0.0']}" +``` diff --git a/caro_flexbe_behavior/CHANGELOG.rst b/caro_flexbe_behavior/CHANGELOG.rst index e36c4a7..91e16dd 100644 --- a/caro_flexbe_behavior/CHANGELOG.rst +++ b/caro_flexbe_behavior/CHANGELOG.rst @@ -4,4 +4,4 @@ Changelog for package caro_flexbe_behavior 0.0.1 (2023-07-21) ------------------ -* Initial release of ROS 2 example FlexBE project \ No newline at end of file +* Initial release of ROS 2 example FlexBE project diff --git a/caro_skills_flexbe_behaviors/CHANGELOG.rst b/caro_skills_flexbe_behaviors/CHANGELOG.rst index 50c52b6..47685e4 100644 --- a/caro_skills_flexbe_behaviors/CHANGELOG.rst +++ b/caro_skills_flexbe_behaviors/CHANGELOG.rst @@ -4,4 +4,4 @@ Changelog for package caro_skills_flexbe_behaviors 0.0.1 (2023-07-21) ------------------ -* Initial release of ROS 2 example FlexBE behaviors \ No newline at end of file +* Initial release of ROS 2 example FlexBE behaviors diff --git a/caro_skills_flexbe_behaviors/bin/copy_behavior b/caro_skills_flexbe_behaviors/bin/copy_behavior index 3d54acd..538d32c 100755 --- a/caro_skills_flexbe_behaviors/bin/copy_behavior +++ b/caro_skills_flexbe_behaviors/bin/copy_behavior @@ -39,7 +39,7 @@ fi beh=$(echo "$1" | cut -f 1 -d '.') # include only the base file name is someone pastes .xml name pack="caro_skills_flexbe_behaviors" # Default name uses this behaviors package -if [ $# -eq 2 ]; then +if [ $# -eq 2 ]; then pack="$2" echo "Using specified package '${pack}'!" fi @@ -97,4 +97,4 @@ else echo "" echo -e "\e[91m Failed to copy behavior '${beh}'!\033[0m" exit -fi \ No newline at end of file +fi diff --git a/caro_skills_flexbe_behaviors/caro_skills_flexbe_behaviors/__init__.py b/caro_skills_flexbe_behaviors/caro_skills_flexbe_behaviors/__init__.py index e69de29..6ac8226 100644 --- a/caro_skills_flexbe_behaviors/caro_skills_flexbe_behaviors/__init__.py +++ b/caro_skills_flexbe_behaviors/caro_skills_flexbe_behaviors/__init__.py @@ -0,0 +1,13 @@ +# # Copyright 2026 Carologistics +# # +# # 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. diff --git a/caro_skills_flexbe_behaviors/caro_skills_flexbe_behaviors/example_action_state_behavior_sm.py b/caro_skills_flexbe_behaviors/caro_skills_flexbe_behaviors/example_action_state_behavior_sm.py old mode 100644 new mode 100755 index e845a39..f8e4774 --- a/caro_skills_flexbe_behaviors/caro_skills_flexbe_behaviors/example_action_state_behavior_sm.py +++ b/caro_skills_flexbe_behaviors/caro_skills_flexbe_behaviors/example_action_state_behavior_sm.py @@ -1,35 +1,29 @@ #!/usr/bin/env python -# -*- coding: utf-8 -*- - -# Copyright 2023 David Conner -# -# 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. - +# # Copyright 2026 Carologistics +# # +# # 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. ########################################################### # WARNING: Generated code! # # ************************** # # Manual changes may get lost if file is generated again. # # Only code inside the [MANUAL] tags will be kept. # ########################################################### - """ Define Example Action State Behavior. Created on July 21, 2023 @author: David Conner """ - - from flexbe_core import Autonomy from flexbe_core import Behavior from flexbe_core import ConcurrencyContainer @@ -58,7 +52,7 @@ class ExampleActionStateBehaviorSM(Behavior): def __init__(self, node): super().__init__() - self.name = 'Example Action State Behavior' + self.name = "Example Action State Behavior" # parameters of this behavior @@ -80,7 +74,7 @@ def __init__(self, node): def create(self): # x:1096 y:62, x:1095 y:363 - _state_machine = OperatableStateMachine(outcomes=['finished', 'failed']) + _state_machine = OperatableStateMachine(outcomes=["finished", "failed"]) _state_machine.userdata.angle_degrees = 90.0 # Additional creation code can be added inside the following tags @@ -89,44 +83,64 @@ def create(self): # [/MANUAL_CREATE] with _state_machine: # x:102 y:157 - OperatableStateMachine.add('LogRequest', - LogKeyState(text="Request to rotate {} degrees", severity=2), - transitions={'done': 'Rotate Turtle State'}, - autonomy={'done': Autonomy.Off}, - remapping={'data': 'angle_degrees'}) + OperatableStateMachine.add( + "LogRequest", + LogKeyState(text="Request to rotate {} degrees", severity=2), + transitions={"done": "Rotate Turtle State"}, + autonomy={"done": Autonomy.Off}, + remapping={"data": "angle_degrees"}, + ) # x:659 y:190 - OperatableStateMachine.add('Log Failed', - LogState(text="Failed", severity=Logger.REPORT_WARN), - transitions={'done': 'failed'}, - autonomy={'done': Autonomy.Off}) + OperatableStateMachine.add( + "Log Failed", + LogState(text="Failed", severity=Logger.REPORT_WARN), + transitions={"done": "failed"}, + autonomy={"done": Autonomy.Off}, + ) # x:658 y:57 - OperatableStateMachine.add('Log Success', - LogState(text="Success", severity=Logger.REPORT_HINT), - transitions={'done': 'finished'}, - autonomy={'done': Autonomy.Off}) + OperatableStateMachine.add( + "Log Success", + LogState(text="Success", severity=Logger.REPORT_HINT), + transitions={"done": "finished"}, + autonomy={"done": Autonomy.Off}, + ) # x:657 y:480 - OperatableStateMachine.add('Log Timeout', - LogState(text="Timeout", severity=Logger.REPORT_WARN), - transitions={'done': 'failed'}, - autonomy={'done': Autonomy.Off}) + OperatableStateMachine.add( + "Log Timeout", + LogState(text="Timeout", severity=Logger.REPORT_WARN), + transitions={"done": "failed"}, + autonomy={"done": Autonomy.Off}, + ) # x:244 y:294 - OperatableStateMachine.add('Rotate Turtle State', - RotateTurtleState(timeout=10, action_topic="/turtle1/rotate_absolute"), - transitions={'rotation_complete': 'Log Success', 'failed': 'Log Failed', - 'canceled': 'Log Canceled', 'timeout': 'Log Timeout'}, - autonomy={'rotation_complete': Autonomy.Off, 'failed': Autonomy.Off, - 'canceled': Autonomy.Off, 'timeout': Autonomy.Off}, - remapping={'angle': 'angle_degrees', 'duration': 'duration'}) + OperatableStateMachine.add( + "Rotate Turtle State", + RotateTurtleState(timeout=10, action_topic="/turtle1/rotate_absolute"), + transitions={ + "rotation_complete": "Log Success", + "failed": "Log Failed", + "canceled": "Log Canceled", + "timeout": "Log Timeout", + }, + autonomy={ + "rotation_complete": Autonomy.Off, + "failed": Autonomy.Off, + "canceled": Autonomy.Off, + "timeout": Autonomy.Off, + }, + remapping={"angle": "angle_degrees", "duration": "duration"}, + ) # x:656 y:337 - OperatableStateMachine.add('Log Canceled', - LogState(text="Canceled", severity=Logger.REPORT_WARN), - transitions={'done': 'failed'}, - autonomy={'done': Autonomy.Off}) + OperatableStateMachine.add( + "Log Canceled", + LogState(text="Canceled", severity=Logger.REPORT_WARN), + transitions={"done": "failed"}, + autonomy={"done": Autonomy.Off}, + ) return _state_machine diff --git a/caro_skills_flexbe_behaviors/caro_skills_flexbe_behaviors/example_behavior_sm.py b/caro_skills_flexbe_behaviors/caro_skills_flexbe_behaviors/example_behavior_sm.py old mode 100644 new mode 100755 index 385e2a1..8480740 --- a/caro_skills_flexbe_behaviors/caro_skills_flexbe_behaviors/example_behavior_sm.py +++ b/caro_skills_flexbe_behaviors/caro_skills_flexbe_behaviors/example_behavior_sm.py @@ -1,35 +1,29 @@ #!/usr/bin/env python -# -*- coding: utf-8 -*- - -# Copyright 2015 Philipp Schillinger -# -# 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. - +# # Copyright 2026 Carologistics +# # +# # 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. ########################################################### # WARNING: Generated code! # # ************************** # # Manual changes may get lost if file is generated again. # # Only code inside the [MANUAL] tags will be kept. # ########################################################### - """ Define Example Behavior. Created on Fri Aug 21 2015 @author: Philipp Schillinger """ - - from flexbe_core import Autonomy from flexbe_core import Behavior from flexbe_core import ConcurrencyContainer @@ -54,10 +48,10 @@ class ExampleBehaviorSM(Behavior): def __init__(self, node): super().__init__() - self.name = 'Example Behavior' + self.name = "Example Behavior" # parameters of this behavior - self.add_parameter('waiting_time', 3) + self.add_parameter("waiting_time", 3) # references to used behaviors OperatableStateMachine.initialize_ros(node) @@ -77,7 +71,7 @@ def __init__(self, node): def create(self): log_msg = "Hello World!" # x:83 y:390 - _state_machine = OperatableStateMachine(outcomes=['finished']) + _state_machine = OperatableStateMachine(outcomes=["finished"]) # Additional creation code can be added inside the following tags # [MANUAL_CREATE] @@ -85,16 +79,20 @@ def create(self): # [/MANUAL_CREATE] with _state_machine: # x:52 y:78 - OperatableStateMachine.add('Print_Message', - LogState(text=log_msg, severity=Logger.REPORT_HINT), - transitions={'done': 'Wait_After_Logging'}, - autonomy={'done': Autonomy.Low}) + OperatableStateMachine.add( + "Print_Message", + LogState(text=log_msg, severity=Logger.REPORT_HINT), + transitions={"done": "Wait_After_Logging"}, + autonomy={"done": Autonomy.Low}, + ) # x:40 y:228 - OperatableStateMachine.add('Wait_After_Logging', - WaitState(wait_time=self.waiting_time), - transitions={'done': 'finished'}, - autonomy={'done': Autonomy.Off}) + OperatableStateMachine.add( + "Wait_After_Logging", + WaitState(wait_time=self.waiting_time), + transitions={"done": "finished"}, + autonomy={"done": Autonomy.Off}, + ) return _state_machine diff --git a/caro_skills_flexbe_behaviors/manifest/estimateposebehavior.xml b/caro_skills_flexbe_behaviors/manifest/estimateposebehavior.xml new file mode 100644 index 0000000..0f10a6e --- /dev/null +++ b/caro_skills_flexbe_behaviors/manifest/estimateposebehavior.xml @@ -0,0 +1,27 @@ + + + + + + + Sam + Tue May 27 2025 + + bla bla + + + + + + + + + + + + + + + + + diff --git a/caro_skills_flexbe_behaviors/manifest/example_action_state_behavior.xml b/caro_skills_flexbe_behaviors/manifest/example_action_state_behavior.xml index a650dbc..98fe9af 100644 --- a/caro_skills_flexbe_behaviors/manifest/example_action_state_behavior.xml +++ b/caro_skills_flexbe_behaviors/manifest/example_action_state_behavior.xml @@ -18,4 +18,4 @@ - \ No newline at end of file + diff --git a/caro_skills_flexbe_behaviors/manifest/example_behavior.xml b/caro_skills_flexbe_behaviors/manifest/example_behavior.xml index 35b95b5..822dc58 100644 --- a/caro_skills_flexbe_behaviors/manifest/example_behavior.xml +++ b/caro_skills_flexbe_behaviors/manifest/example_behavior.xml @@ -25,4 +25,4 @@ - \ No newline at end of file + diff --git a/caro_skills_flexbe_behaviors/manifest/finde_tf_behavior.xml b/caro_skills_flexbe_behaviors/manifest/finde_tf_behavior.xml new file mode 100644 index 0000000..c78239d --- /dev/null +++ b/caro_skills_flexbe_behaviors/manifest/finde_tf_behavior.xml @@ -0,0 +1,18 @@ + + + + + + + Sam + Thu May 29 2025 + + find TF + + + + + + + + diff --git a/caro_skills_flexbe_behaviors/manifest/motormove.xml b/caro_skills_flexbe_behaviors/manifest/motormove.xml new file mode 100644 index 0000000..9b6b7c5 --- /dev/null +++ b/caro_skills_flexbe_behaviors/manifest/motormove.xml @@ -0,0 +1,30 @@ + + + + + + + Carologistics + Sat Feb 01 2025 + + This state moves the robot to the given pose using MotorMove messages. It does + not avoid collisions! + + + + + + + + + + + + + + + + + + + diff --git a/caro_skills_flexbe_behaviors/manifest/move_to.xml b/caro_skills_flexbe_behaviors/manifest/move_to.xml new file mode 100644 index 0000000..72b348b --- /dev/null +++ b/caro_skills_flexbe_behaviors/manifest/move_to.xml @@ -0,0 +1,29 @@ + + + + + + + Carologistics + Sat Feb 01 2025 + + This state navigates the robot to the given pose using NavigateToPose messages + + + + + + + + + + + + + + + + + + + diff --git a/caro_skills_flexbe_behaviors/manifest/move_to_machine.xml b/caro_skills_flexbe_behaviors/manifest/move_to_machine.xml new file mode 100644 index 0000000..a5464c4 --- /dev/null +++ b/caro_skills_flexbe_behaviors/manifest/move_to_machine.xml @@ -0,0 +1,18 @@ + + + + + + + Sam Koehler + Wed Apr 30 2025 + + Moves to a given TF + + + + + + + + diff --git a/caro_skills_flexbe_behaviors/setup.py b/caro_skills_flexbe_behaviors/setup.py old mode 100644 new mode 100755 index 3868c9f..57bbbcb --- a/caro_skills_flexbe_behaviors/setup.py +++ b/caro_skills_flexbe_behaviors/setup.py @@ -1,27 +1,39 @@ #!/usr/bin/env python +# # Copyright 2026 Carologistics +# # +# # 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 setuptools import setup -package_name = 'caro_skills_flexbe_behaviors' +package_name = "caro_skills_flexbe_behaviors" setup( name=package_name, - version='0.0.1', + version="0.0.1", packages=[package_name], data_files=[ - ('share/ament_index/resource_index/packages', - ['resource/' + package_name]), - ('share/' + package_name, ['package.xml']), + ("share/ament_index/resource_index/packages", ["resource/" + package_name]), + ("share/" + package_name, ["package.xml"]), ], - install_requires=['setuptools'], + install_requires=["setuptools"], zip_safe=True, - maintainer='phil', - maintainer_email='philsplus@gmail.com', - description='TODO: Package description', - license='TODO: License declaration', - tests_require=['pytest'], + maintainer="phil", + maintainer_email="philsplus@gmail.com", + description="TODO: Package description", + license="TODO: License declaration", + tests_require=["pytest"], entry_points={ - 'console_scripts': [ - 'example_behavior_sm = caro_skills_flexbe_behaviors.example_behavior_sm', + "console_scripts": [ + "example_behavior_sm = caro_skills_flexbe_behaviors.example_behavior_sm", ], }, ) diff --git a/caro_skills_flexbe_states/CHANGELOG.rst b/caro_skills_flexbe_states/CHANGELOG.rst index 4b4dd9c..d9d0bd6 100644 --- a/caro_skills_flexbe_states/CHANGELOG.rst +++ b/caro_skills_flexbe_states/CHANGELOG.rst @@ -4,4 +4,4 @@ Changelog for package caro_skills_flexbe_states 0.0.1 (2023-07-21) ------------------ -* Initial release of ROS 2 example FlexBE states \ No newline at end of file +* Initial release of ROS 2 example FlexBE states diff --git a/caro_skills_flexbe_states/caro_skills_flexbe_states/__init__.py b/caro_skills_flexbe_states/caro_skills_flexbe_states/__init__.py index e69de29..6ac8226 100644 --- a/caro_skills_flexbe_states/caro_skills_flexbe_states/__init__.py +++ b/caro_skills_flexbe_states/caro_skills_flexbe_states/__init__.py @@ -0,0 +1,13 @@ +# # Copyright 2026 Carologistics +# # +# # 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. diff --git a/caro_skills_flexbe_states/caro_skills_flexbe_states/estimate_pose.py b/caro_skills_flexbe_states/caro_skills_flexbe_states/estimate_pose.py new file mode 100755 index 0000000..80fd45f --- /dev/null +++ b/caro_skills_flexbe_states/caro_skills_flexbe_states/estimate_pose.py @@ -0,0 +1,142 @@ +#!/usr/bin/env python +# # Copyright 2026 Carologistics +# # +# # 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. +""" +This state applies an X and Y offset to a given TransformStamped +and outputs the resulting X, Y, Yaw, and Frame ID. +""" +import transforms3d.euler +import transforms3d.quaternions +from flexbe_core import EventState +from flexbe_core import Logger +from geometry_msgs.msg import TransformStamped + + +class EstimatedPoseState(EventState): + """ + Applies an X and Y offset to an input_transform and outputs the + resulting pose (x, y, yaw) and the frame_id. + + Input Keys: + ># transform_in TransformStamped The input transform. + ># offset_x float The offset to apply in the X-direction of the child frame. + ># offset_y float The offset to apply in the Y-direction of the child frame. + ># ns string Namespace (currently not used, but accepted as an input key). + + Output Keys: + #> target_x float The resulting X-coordinate. + #> target_y float The resulting Y-coordinate. + #> target_yaw float The resulting Yaw angle (in radians). + #> frame_id string The frame ID of the transform. + + Outcomes: + <= done The calculation was successful. + <= failed An error occurred during calculation. + """ + + def __init__(self): + super().__init__( + outcomes=["done", "failed"], + input_keys=["transform_in", "offset_x", "offset_y", "ns"], + output_keys=["target_x", "target_y", "target_yaw", "frame_id"], + ) + self._return_code = None # Store the outcome for execute() + + def on_enter(self, userdata): + self._return_code = None # Reset return code before processing + + try: + # 1. Validate inputs + if not all(key in userdata for key in ["transform_in", "offset_x", "offset_y"]): + missing_keys = [key for key in ["transform_in", "offset_x", "offset_y"] if key not in userdata] + Logger.logwarn(f"'{self.name}': Missing input keys: {', '.join(missing_keys)}.") + self._return_code = "failed" + return + + transform_in = userdata.transform_in + offset_x = userdata.offset_x + offset_y = userdata.offset_y + _ = userdata.get("ns", None) # Acknowledge 'ns' even if not actively used + + if not isinstance(transform_in, TransformStamped): + Logger.logwarn( + f"'{self.name}': Input 'transform_in' is not a TransformStamped. Received: {type(transform_in)}." + ) + self._return_code = "failed" + return + if not isinstance(offset_x, (float, int)): + Logger.logwarn( + f"'{self.name}': Input 'offset_x' is not a number (float/int). Received: {type(offset_x)}." + ) + self._return_code = "failed" + return + if not isinstance(offset_y, (float, int)): + Logger.logwarn( + f"'{self.name}': Input 'offset_y' is not a number (float/int). Received: {type(offset_y)}." + ) + self._return_code = "failed" + return + + # 2. Perform calculations + original_translation = transform_in.transform.translation + q_rotation = transform_in.transform.rotation + + # Prepare quaternion for transforms3d (w, x, y, z) + q_t3d_format = [q_rotation.w, q_rotation.x, q_rotation.y, q_rotation.z] + + # Create offset vector in the child frame's coordinate system + offset_vector_child = [float(offset_x), float(offset_y), 0.0] + + # Rotate the offset vector from the child frame to the parent frame + rotated_offset_parent = transforms3d.quaternions.rotate_vector(offset_vector_child, q_t3d_format) + + # Add the rotated offset to the original translation in the parent frame + output_x = original_translation.x + rotated_offset_parent[0] + output_y = original_translation.y + rotated_offset_parent[1] + # The Z component of the translation would be original_translation.z + rotated_offset_parent[2], + # but it's not used as an output key here, and the Z-offset is defined as 0.0 in offset_vector_child. + + # 3. Extract Yaw + _, _, output_yaw = transforms3d.euler.quat2euler(q_t3d_format, axes="sxyz") + + # 4. Extract Frame ID + output_frame_id = transform_in.header.frame_id + + # 5. Set output userdata + userdata.target_x = output_x + userdata.target_y = output_y + userdata.target_yaw = output_yaw + userdata.frame_id = output_frame_id + + Logger.loginfo( + f"'{self.name}': Offset applied successfully. New pose in frame '{output_frame_id}': " + f"x={output_x:.3f}, y={output_y:.3f}, yaw={output_yaw:.3f} rad." + ) + self._return_code = "done" + except KeyError as e: + Logger.logwarn(f"'{self.name}': Missing input key in userdata: {str(e)}.") + self._return_code = "failed" + except Exception as e: + Logger.logerr(f"'{self.name}': Error during processing: {str(e)}") + self._return_code = "failed" + # on_enter does not return an outcome, so execute() will be called next. + + def execute(self, userdata): + # All logic is in on_enter, as this state's operations are immediate. + # execute() simply returns the pre-calculated result. + return self._return_code + + def on_exit(self, userdata): + # Nothing to clean up for this state. + pass diff --git a/caro_skills_flexbe_states/caro_skills_flexbe_states/example_action_state.py b/caro_skills_flexbe_states/caro_skills_flexbe_states/example_action_state.py old mode 100644 new mode 100755 index 4c79643..7476aae --- a/caro_skills_flexbe_states/caro_skills_flexbe_states/example_action_state.py +++ b/caro_skills_flexbe_states/caro_skills_flexbe_states/example_action_state.py @@ -1,34 +1,31 @@ #!/usr/bin/env python - -# Copyright 2023 Christopher Newport University -# -# 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. - +# # Copyright 2026 Carologistics +# # +# # 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. """Example Action FlexBE State.""" - import math -from rclpy.duration import Duration - -from flexbe_core import EventState, Logger +from flexbe_core import EventState +from flexbe_core import Logger from flexbe_core.proxy import ProxyActionClient +from rclpy.duration import Duration +from turtlesim.action import RotateAbsolute # import of required action # This ExampleActionState is based on the standard action tutorials # https://docs.ros.org/en/iron/Tutorials/Beginner-CLI-Tools/Understanding-ROS2-Actions/Understanding-ROS2-Actions.html # https://docs.ros.org/en/iron/Tutorials/Beginner-CLI-Tools/Introducing-Turtlesim/Introducing-Turtlesim.html # https://docs.ros2.org/latest/api/turtlesim/action/RotateAbsolute.html -from turtlesim.action import RotateAbsolute class ExampleActionState(EventState): @@ -69,9 +66,11 @@ class ExampleActionState(EventState): def __init__(self, timeout, action_topic="/turtle1/rotate_absolute"): # See example_state.py for basic explanations. - super().__init__(outcomes=['rotation_complete', 'failed', 'canceled', 'timeout'], - input_keys=['angle'], - output_keys=['duration']) + super().__init__( + outcomes=["rotation_complete", "failed", "canceled", "timeout"], + input_keys=["angle"], + output_keys=["duration"], + ) self._timeout = Duration(seconds=timeout) self._timeout_sec = timeout @@ -82,8 +81,9 @@ def __init__(self, timeout, action_topic="/turtle1/rotate_absolute"): # and makes sure only one client is used, no matter how often this state is used in a behavior. ProxyActionClient.initialize(ExampleActionState._node) - self._client = ProxyActionClient({self._topic: RotateAbsolute}, - wait_duration=0.0) # pass required clients as dict (topic: type) + self._client = ProxyActionClient( + {self._topic: RotateAbsolute}, wait_duration=0.0 + ) # pass required clients as dict (topic: type) # It may happen that the action client fails to send the action goal. self._error = False @@ -95,7 +95,7 @@ def execute(self, userdata): # Check if the client failed to send the goal. if self._error: - return 'failed' + return "failed" if self._return is not None: # Return prior outcome in case transition is blocked by autonomy level @@ -105,14 +105,14 @@ def execute(self, userdata): if self._client.has_result(self._topic): _ = self._client.get_result(self._topic) # The delta result value is not useful here userdata.duration = self._node.get_clock().now() - self._start_time - Logger.loginfo('Rotation complete') - self._return = 'rotation_complete' + Logger.loginfo("Rotation complete") + self._return = "rotation_complete" return self._return if self._node.get_clock().now().nanoseconds - self._start_time.nanoseconds > self._timeout.nanoseconds: # Checking for timeout after we check for goal response - self._return = 'timeout' - return 'timeout' + self._return = "timeout" + return "timeout" # If the action has not yet finished, no outcome will be returned and the state stays active. return None @@ -123,7 +123,7 @@ def on_enter(self, userdata): self._error = False self._return = None - if 'angle' not in userdata: + if "angle" not in userdata: self._error = True Logger.logwarn("ExampleActionState requires userdata.angle key!") return @@ -151,8 +151,8 @@ def on_enter(self, userdata): def on_exit(self, userdata): # Make sure that the action is not running when leaving this state. - # A situation where the action would still be active is for example when the operator manually triggers an outcome. - + # A situation where the action would still be active is for example when the operator + # manually triggers an outcome. if not self._client.has_result(self._topic): self._client.cancel(self._topic) - Logger.loginfo('Cancelled active action goal.') + Logger.loginfo("Cancelled active action goal.") diff --git a/caro_skills_flexbe_states/caro_skills_flexbe_states/example_state.py b/caro_skills_flexbe_states/caro_skills_flexbe_states/example_state.py old mode 100644 new mode 100755 index 3e98ad4..31886ba --- a/caro_skills_flexbe_states/caro_skills_flexbe_states/example_state.py +++ b/caro_skills_flexbe_states/caro_skills_flexbe_states/example_state.py @@ -1,25 +1,23 @@ #!/usr/bin/env python - -# Copyright 2023 Christopher Newport University -# -# 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. - +# # Copyright 2026 Carologistics +# # +# # 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. """Demonstration state.""" +from flexbe_core import EventState +from flexbe_core import Logger from rclpy.constants import S_TO_NS from rclpy.duration import Duration -from flexbe_core import EventState, Logger - class ExampleState(EventState): """ @@ -46,8 +44,8 @@ class ExampleState(EventState): """ def __init__(self, target_time): - """Declare outcomes, input_keys, and output_keys by calling the super constructor with the corresponding arguments.""" - super().__init__(outcomes=['done', 'failed']) + """Declare outcomes, input_keys, and output_keys by calling the super constructor.""" + super().__init__(outcomes=["done", "failed"]) # Store state parameter for later use. self._target_wait_time = Duration(seconds=target_time) @@ -108,28 +106,36 @@ def execute(self, userdata): # Here we will just return the prior outcome and not recalculate # Local info is NOT sent to the UI, and only shown in logs and terminal - Logger.localinfo(f"execute blocked for '{self._name}' state ({self.path}) @ {self.clock_time} " - f"- use prior return code={self._return}") + Logger.localinfo( + f"execute blocked for '{self._name}' state ({self.path}) @ {self.clock_time} " + f"- use prior return code={self._return}" + ) return self._return # Normal calculation block try: self._elapsed_time = ExampleState._node.get_clock().now() - self._state_enter_time if self._elapsed_time >= self._target_wait_time: - Logger.loginfo(f"execute for '{self._name}' state ({self.path}) @ {self.clock_time} " - f"- done waiting at {self.elapsed_seconds} seconds.") - self._return = 'done' - return 'done' # One of the outcomes declared above. + Logger.loginfo( + f"execute for '{self._name}' state ({self.path}) @ {self.clock_time} " + f"- done waiting at {self.elapsed_seconds} seconds." + ) + self._return = "done" + return "done" # One of the outcomes declared above. except Exception: # pylint:disable=W0703 # Something went wrong - Logger.logerr(f"execute for '{self._name}' state ({self.path}) @ {self.clock_time} " - f"- something went wrong after {self.elapsed_seconds} seconds.") - self._return = 'failed' - return 'failed' + Logger.logerr( + f"execute for '{self._name}' state ({self.path}) @ {self.clock_time} " + f"- something went wrong after {self.elapsed_seconds} seconds." + ) + self._return = "failed" + return "failed" # Local info is NOT sent to the UI, and only shown in logs and terminal - Logger.localinfo(f"execute for '{self._name}' state ({self.path}) @ {self.clock_time} " - f"- {self.elapsed_seconds} seconds since start.") + Logger.localinfo( + f"execute for '{self._name}' state ({self.path}) @ {self.clock_time} " + f"- {self.elapsed_seconds} seconds since start." + ) return None # This is normal behavior for state to continue executing def on_enter(self, userdata): @@ -146,8 +152,10 @@ def on_enter(self, userdata): self._elapsed_time = Duration(seconds=0.0) self._return = None # Clear return code on entry - Logger.loginfo(f"on_enter for '{self._name}' state ({self.path}) @ {self.clock_time} " - f"- need to wait for {self.target_seconds} seconds.") + Logger.loginfo( + f"on_enter for '{self._name}' state ({self.path}) @ {self.clock_time} " + f"- need to wait for {self.target_seconds} seconds." + ) def on_exit(self, userdata): """ @@ -157,8 +165,10 @@ def on_exit(self, userdata): Nothing to do in this example. """ self._state_exit_time = ExampleState._node.get_clock().now() - Logger.loginfo(f"on_exit for '{self._name}' state ({self.path}) @ {self.clock_time} " - f"elapsed time = {self.elapsed_seconds} seconds.") + Logger.loginfo( + f"on_exit for '{self._name}' state ({self.path}) @ {self.clock_time} " + f"elapsed time = {self.elapsed_seconds} seconds." + ) def on_start(self): """ @@ -169,8 +179,10 @@ def on_start(self): In this example, we use this event to set the correct start time. """ self._state_start_time = ExampleState._node.get_clock().now() - Logger.loginfo(f"on_start for '{self._name}' state ({self.path}) @ {self.start_time} seconds " - f" time to wait = {self.target_seconds} seconds..") + Logger.loginfo( + f"on_start for '{self._name}' state ({self.path}) @ {self.start_time} seconds " + f" time to wait = {self.target_seconds} seconds.." + ) def on_stop(self): """ @@ -180,15 +192,20 @@ def on_stop(self): Nothing to do in this example. """ self._elapsed_time = ExampleState._node.get_clock().now() - self._state_start_time - Logger.loginfo(f"on_stop for '{self._name}' state ({self.path}) @ {self.clock_time} seconds " - f" total behavior instance elapsed time = {self.elapsed_seconds} seconds ") + Logger.loginfo( + f"on_stop for '{self._name}' state ({self.path}) @ {self.clock_time} seconds " + f" total behavior instance elapsed time = {self.elapsed_seconds} seconds " + ) if self._state_enter_time is None: - Logger.loginfo(f"on_stop for '{self._name}' state ({self.path}) @ {self.clock_time} seconds " - f" - never entered the state to execute! ") + Logger.loginfo( + f"on_stop for '{self._name}' state ({self.path}) @ {self.clock_time} seconds " + f" - never entered the state to execute! " + ) else: try: self._elapsed_time = self._state_exit_time - self._state_enter_time - Logger.loginfo(f" '{self._name}' state " - f"was active (enter-to-exit) for {self.elapsed_seconds} seconds.") + Logger.loginfo( + f" '{self._name}' state " f"was active (enter-to-exit) for {self.elapsed_seconds} seconds." + ) except Exception: # pylint: disable=W0703 Logger.logerr(f" entered at time={self.enter_time} seconds but never exited!") diff --git a/caro_skills_flexbe_states/caro_skills_flexbe_states/find_tf.py b/caro_skills_flexbe_states/caro_skills_flexbe_states/find_tf.py new file mode 100755 index 0000000..154a45c --- /dev/null +++ b/caro_skills_flexbe_states/caro_skills_flexbe_states/find_tf.py @@ -0,0 +1,161 @@ +#!/usr/bin/env python +# # Copyright 2026 Carologistics +# # +# # 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. +"""FlexBE State to find a TF transform between two frames.""" +from flexbe_core import EventState +from flexbe_core import Logger +from rclpy.duration import Duration +from rclpy.time import Time +from tf2_ros import TransformException +from tf2_ros.buffer import Buffer +from tf2_ros.transform_listener import TransformListener + + +class FindTF(EventState): + """ + This state attempts to find the TF (transform) from a specified child frame + (source_frame_id) to a specified parent frame (target_frame_id). + + Elements defined here for UI + Parameters + -- timeout float Maximum time (in seconds) to wait for the transform lookup. + + Outputs + <= frame_found The transform was successfully found (from source_frame_id to target_frame_id). + <= failed The transform could not be found (e.g., timeout, frames don't exist, or other error). + + User data + ># target_frame_id string The name of the target frame (e.g., 'map', 'odom'). + ># This is the frame into which the source_frame will be transformed. + ># source_frame_id string The name of the source frame (e.g., 'base_link', 'camera_link'). + ># This is the frame that will be transformed. + #> transform TransformStamped The found transform. + """ + + def __init__(self, timeout): + """Initialize the state with outcomes, input/output keys, and timeout.""" + super().__init__( + outcomes=["frame_found", "failed"], + input_keys=["source_frame_id", "target_frame_id"], + output_keys=["transform"], + ) + + # TF resources initialized in on_enter + self._tf_buffer = None + self._tf_listener = None + self._lookup_target_frame = None + self._lookup_source_frame = None + + # Parse timeout with fallback to default + try: + timeout_float = float(timeout) + self._lookup_timeout_duration = Duration(seconds=timeout_float) + except ValueError: + Logger.logwarn( + f"Invalid timeout value '{timeout}' for FindTF state. " f"It must be a number. Using default 1.0s." + ) + self._lookup_timeout_duration = Duration(seconds=1.0) + + def on_enter(self, userdata): + """Called when the state becomes active. Sets up TF listener and validates input.""" + super().on_enter(userdata) + + # Initialize TF listener + try: + self._tf_buffer = Buffer() + self._tf_listener = TransformListener(self._tf_buffer, self._node, spin_thread=False) + Logger.loginfo(f"TF Listener initialized for state '{self.name}'.") + except Exception as e: + Logger.logerr(f"Failed to initialize TF Buffer/Listener for state '{self.name}': {str(e)}") + # No outcome is returned here, execute will handle the error by returning 'failed' + # because _tf_buffer will be None. + return # Return here to prevent further processing in on_enter if listener fails + + if self._tf_buffer is None: + Logger.logerr(f"TF Buffer not initialized in state '{self.name}'.") + return # execute will return 'failed' + + # Validate and get frame_ids from userdata + if ( + "target_frame_id" not in userdata + or not isinstance(userdata.target_frame_id, str) + or not userdata.target_frame_id + ): + Logger.logwarn(f"'{self.name}': Invalid or missing 'target_frame_id' in userdata.") + self._lookup_target_frame = None # Mark as invalid + return + if ( + "source_frame_id" not in userdata + or not isinstance(userdata.source_frame_id, str) + or not userdata.source_frame_id + ): + Logger.logwarn(f"'{self.name}': Invalid or missing 'source_frame_id' in userdata.") + self._lookup_source_frame = None # Mark as invalid + return + + # lookup_transform(target, source, ...) gives the transform from source to target. + # The userdata.target_frame_id is the frame we want to transform *into*. + # The userdata.source_frame_id is the frame we want to transform *from*. + self._lookup_target_frame = userdata.target_frame_id + self._lookup_source_frame = userdata.source_frame_id + Logger.loginfo( + f"'{self.name}': Attempting to find transform from '{self._lookup_source_frame}' (source) " + f"to '{self._lookup_target_frame}' (target)." + ) + + def execute(self, userdata): + """Perform the TF lookup and return outcome based on success or failure.""" + if self._tf_buffer is None: + Logger.logerr(f"'{self.name}': TF Buffer not available in execute.") + return "failed" + + if self._lookup_target_frame is None or self._lookup_source_frame is None: + Logger.logwarn(f"'{self.name}': Frame IDs not properly set in on_enter.") + return "failed" + + try: + # Get the transform from source_frame_id to target_frame_id + transform_stamped = self._tf_buffer.lookup_transform( + self._lookup_target_frame, # Target frame + self._lookup_source_frame, # Source frame + Time(), + timeout=self._lookup_timeout_duration, + ) + userdata.transform = transform_stamped + Logger.loginfo( + f"'{self.name}': Successfully found transform " + f"from '{self._lookup_source_frame}' to '{self._lookup_target_frame}'." + ) + return "frame_found" + + except TransformException as ex: + Logger.logwarn( + f"'{self.name}': Could not transform '{self._lookup_source_frame}' to '{self._lookup_target_frame}' " + f"within {self._lookup_timeout_duration.nanoseconds / 1e9:.2f}s: {ex}" + ) + return "failed" + except Exception as e: + Logger.logerr(f"'{self.name}': An unexpected error occurred during TF lookup: {str(e)}") + return "failed" + + def on_exit(self, userdata): + """Clean up TF resources when leaving the state.""" + if self._tf_listener is not None: + # TransformListener doesn't have an explicit shutdown, relying on garbage collection. + # Setting to None helps the garbage collector and signals it's no longer in use. + self._tf_listener = None + if self._tf_buffer is not None: + self._tf_buffer.clear() # Clear the buffer if desired, though not strictly necessary for just one lookup. + self._tf_buffer = None + Logger.loginfo(f"'{self.name}': Cleaned up TF resources.") diff --git a/caro_skills_flexbe_states/caro_skills_flexbe_states/generic_service_toggle_state.py b/caro_skills_flexbe_states/caro_skills_flexbe_states/generic_service_toggle_state.py new file mode 100755 index 0000000..36aa01a --- /dev/null +++ b/caro_skills_flexbe_states/caro_skills_flexbe_states/generic_service_toggle_state.py @@ -0,0 +1,203 @@ +#!/usr/bin/env python3 +# # Copyright 2026 Carologistics +# # +# # 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. +""" +Generic FlexBE Service State for boolean toggle services. + +This state allows calling any ROS2 service that has a boolean field in its request, +making it reusable across different robots and service types. + +Author: Sam +License: BSD-3-Clause +""" +import importlib +import json + +from flexbe_core import EventState +from flexbe_core import Logger +from flexbe_core.proxy import ProxyServiceCaller + + +class GenericToggleServiceState(EventState): + """ + A generic FlexBE state for calling services with multiple parameters. + + This state dynamically loads the service type at runtime, allowing it to be + used with any service. Supports multiple fields via a JSON string or dict. + + -- namespace str The ROS2 namespace (e.g., 'robotinobase1') + -- service_name str Name of the service (e.g., 'toggle_segmentation') + -- service_type str Full service type path (e.g., 'laser_scan_integrator_msg.srv.ToggleSegmentation') + -- fields str/dict JSON string or dict with field name-value pairs + e.g., '{"enable": true, "threshold": 0.5}' or {"enable": True} + -- timeout float Timeout for service availability in seconds (default: 5.0) + + ># none No input keys required + + #> success bool True if service call was successful (optional output) + + <= done Service was called successfully + <= failed Service call failed due to an error + <= unavailable Service is not available within timeout + + Example usage: + ros2 service call /robotinobase1/toggle_segmentation + laser_scan_integrator_msg/srv/ToggleSegmentation "{enable_segmentation: true}" + + Would be configured as: + namespace='robotinobase1' + service_name='toggle_segmentation' + service_type='laser_scan_integrator_msg.srv.ToggleSegmentation' + fields='{"enable_segmentation": true}' + """ + + def __init__(self, namespace, service_name, service_type, fields, timeout=5.0): + """ + Initialize the state. + + Args: + namespace: ROS2 namespace for the service + service_name: Name of the service to call + service_type: Full Python import path for the service type + fields: JSON string or dict with field name-value pairs + timeout: Maximum time to wait for service availability + """ + super().__init__(outcomes=["done", "failed", "unavailable"], output_keys=["success"]) + + # Store parameters + self._namespace = namespace + self._service_name = service_name + self._timeout = timeout + + # Parse fields - accept both JSON string and dict + if isinstance(fields, str): + try: + self._fields = json.loads(fields) + except json.JSONDecodeError as e: + Logger.logerr(f"Failed to parse fields JSON: {str(e)}") + self._fields = {} + else: + self._fields = fields + + # Construct the full service topic + # Handle empty namespace case + if namespace: + self._service_topic = f"/{namespace}/{service_name}" + else: + self._service_topic = f"/{service_name}" + + # Dynamically load the service class + try: + module_name, class_name = service_type.rsplit(".", 1) + module = importlib.import_module(module_name) + self._service_class = getattr(module, class_name) + except (ValueError, ImportError, AttributeError) as e: + Logger.logerr(f'Failed to load service type "{service_type}": {str(e)}') + self._service_class = None + + # Runtime state variables + self._srv_result = None + self._error = False + self._unavailable = False + + def on_enter(self, userdata): + """ + Called when the state becomes active. + + Initializes the service proxy and sends the request. + """ + # Reset state variables + self._error = False + self._unavailable = False + self._srv_result = None + userdata.success = False + + # Check if service class was loaded successfully + if self._service_class is None: + Logger.logerr(f"[{self.name}] Service class not loaded - check service_type parameter") + self._error = True + return + + try: + # Create service proxy + srv_proxy = ProxyServiceCaller({self._service_topic: self._service_class}, wait_duration=self._timeout) + + # Check service availability + if not srv_proxy.is_available(self._service_topic): + Logger.logwarn(f'[{self.name}] Service "{self._service_topic}" is not available!') + self._unavailable = True + return + + # Create and populate the request + request = self._service_class.Request() + + # Set all fields from the fields dict + for field_name, field_value in self._fields.items(): + if not hasattr(request, field_name): + Logger.logerr(f'[{self.name}] Request has no field named "{field_name}"') + self._error = True + return + setattr(request, field_name, field_value) + + # Log the service call + Logger.loginfo(f"[{self.name}] Calling {self._service_topic} with {self._fields}") + + # Execute the service call (blocking) + self._srv_result = srv_proxy.call(self._service_topic, request) + userdata.success = True + + except Exception as e: + Logger.logerr(f"[{self.name}] Service call failed: {str(e)}") + self._error = True + + def execute(self, userdata): + """ + Called periodically while the state is active. + + Returns the appropriate outcome based on the service call result. + """ + if self._unavailable: + return "unavailable" + + if self._error: + return "failed" + + if self._srv_result is not None: + Logger.loginfo(f"[{self.name}] Service call completed successfully") + return "done" + + # No result yet - continue waiting + # Note: This should not happen with blocking calls, but included for safety + return None + + def on_exit(self, userdata): + """ + Called when the state is exited. + + Cleanup can be performed here if needed. + """ + + def on_start(self): + """ + Called when the behavior starts. + + Can be used for one-time initialization. + """ + + def on_stop(self): + """ + Called when the behavior stops. + + Can be used for cleanup when behavior is stopped prematurely. + """ diff --git a/caro_skills_flexbe_states/caro_skills_flexbe_states/gripper_command_state.py b/caro_skills_flexbe_states/caro_skills_flexbe_states/gripper_command_state.py old mode 100644 new mode 100755 index 5c5b0df..687b33b --- a/caro_skills_flexbe_states/caro_skills_flexbe_states/gripper_command_state.py +++ b/caro_skills_flexbe_states/caro_skills_flexbe_states/gripper_command_state.py @@ -1,40 +1,37 @@ #!/usr/bin/env python - -# Copyright 2023 Carologistics -# -# 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. - +# # Copyright 2026 Carologistics +# # +# # 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. """Use to move gripper to target pose.""" - import math -from rclpy.duration import Duration - -from flexbe_core import EventState, Logger +from flexbe_core import EventState +from flexbe_core import Logger from flexbe_core.proxy import ProxyActionClient +from rclpy.duration import Duration +from turtlesim.action import RotateAbsolute # import of required action # This ExampleActionState is based on the standard action tutorials # https://docs.ros.org/en/iron/Tutorials/Beginner-CLI-Tools/Understanding-ROS2-Actions/Understanding-ROS2-Actions.html # https://docs.ros.org/en/iron/Tutorials/Beginner-CLI-Tools/Introducing-Turtlesim/Introducing-Turtlesim.html # https://docs.ros2.org/latest/api/turtlesim/action/RotateAbsolute.html -# from turtlesim.action import RotateAbsolute class GripperCommandState(EventState): """ Use to move gripper to target pose - + Parameters -- timeout Maximum time allowed (seconds) -- action_topic Name of action to invoke @@ -53,9 +50,11 @@ class GripperCommandState(EventState): def __init__(self, timeout, action_topic="/turtle1/rotate_absolute"): # See example_state.py for basic explanations. - super().__init__(outcomes=['rotation_complete', 'failed', 'canceled', 'timeout'], - input_keys=['angle'], - output_keys=['duration']) + super().__init__( + outcomes=["rotation_complete", "failed", "canceled", "timeout"], + input_keys=["angle"], + output_keys=["duration"], + ) self._timeout = Duration(seconds=timeout) self._timeout_sec = timeout @@ -64,10 +63,11 @@ def __init__(self, timeout, action_topic="/turtle1/rotate_absolute"): # Create the action client when building the behavior. # Using the proxy client provides asynchronous access to the result and status # and makes sure only one client is used, no matter how often this state is used in a behavior. - ProxyActionClient.initialize(ExampleActionState._node) + ProxyActionClient.initialize(GripperCommandState._node) - self._client = ProxyActionClient({self._topic: RotateAbsolute}, - wait_duration=0.0) # pass required clients as dict (topic: type) + self._client = ProxyActionClient( + {self._topic: RotateAbsolute}, wait_duration=0.0 + ) # pass required clients as dict (topic: type) # It may happen that the action client fails to send the action goal. self._error = False @@ -79,7 +79,7 @@ def execute(self, userdata): # Check if the client failed to send the goal. if self._error: - return 'failed' + return "failed" if self._return is not None: # Return prior outcome in case transition is blocked by autonomy level @@ -89,14 +89,14 @@ def execute(self, userdata): if self._client.has_result(self._topic): _ = self._client.get_result(self._topic) # The delta result value is not useful here userdata.duration = self._node.get_clock().now() - self._start_time - Logger.loginfo('Rotation complete') - self._return = 'rotation_complete' + Logger.loginfo("Rotation complete") + self._return = "rotation_complete" return self._return if self._node.get_clock().now().nanoseconds - self._start_time.nanoseconds > self._timeout.nanoseconds: # Checking for timeout after we check for goal response - self._return = 'timeout' - return 'timeout' + self._return = "timeout" + return "timeout" # If the action has not yet finished, no outcome will be returned and the state stays active. return None @@ -107,7 +107,7 @@ def on_enter(self, userdata): self._error = False self._return = None - if 'angle' not in userdata: + if "angle" not in userdata: self._error = True Logger.logwarn("ExampleActionState requires userdata.angle key!") return @@ -135,8 +135,8 @@ def on_enter(self, userdata): def on_exit(self, userdata): # Make sure that the action is not running when leaving this state. - # A situation where the action would still be active is for example when the operator manually triggers an outcome. - + # A situation where the action would still be active is for example when the operator + # manually triggers an outcome. if not self._client.has_result(self._topic): self._client.cancel(self._topic) - Logger.loginfo('Cancelled active action goal.') + Logger.loginfo("Cancelled active action goal.") diff --git a/caro_skills_flexbe_states/caro_skills_flexbe_states/mapper_state.py b/caro_skills_flexbe_states/caro_skills_flexbe_states/mapper_state.py new file mode 100755 index 0000000..185cbfc --- /dev/null +++ b/caro_skills_flexbe_states/caro_skills_flexbe_states/mapper_state.py @@ -0,0 +1,291 @@ +#!/usr/bin/env python +# # Copyright 2026 Carologistics +# # +# # 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. +"""Example Action FlexBE State.""" +import math + +import transforms3d.euler +import transforms3d.quaternions +from flexbe_core import EventState +from flexbe_core import Logger +from flexbe_core.proxy import ProxyServiceCaller +from flexbe_core.proxy import ProxySubscriberCached +from geometry_msgs.msg import PointStamped +from geometry_msgs.msg import TransformStamped +from laser_scan_integrator_msg.msg import LineSegments +from laser_scan_integrator_msg.srv import ToggleSegmentation +from laser_scan_integrator_msg.srv import ToggleSegmentation_Request +from rclpy.duration import Duration +from rclpy.time import Time +from tf2_ros import TransformException +from tf2_ros.buffer import Buffer +from tf2_ros.transform_listener import TransformListener + + +class MapLaserToTF(EventState): + """FlexBE state that detects line segments from laser scans and outputs a TF when + a segment matches the robot's pose within configured tolerances.""" + + def __init__(self, timeout=5): + super().__init__( + outcomes=["frame_found", "failed"], + input_keys=["source_frame_id", "target_frame_id", "ns"], + output_keys=["transform"], + ) + + self._timeout_sec = timeout + self._service_client = ProxyServiceCaller() + self._subscriber = None + self._error = False + self._pose_condition_met = False + self._start_time = None + self._estimated_pose_output = None + try: + timeout_float = float(timeout) + self._lookup_timeout_duration = Duration(seconds=timeout_float) + except ValueError: + Logger.logerr( + f"Invalid timeout value '{timeout}' for EstimatePose state. " + f"It must be a number. Using default 1.0s." + ) + self._lookup_timeout_duration = Duration(seconds=5.0) + + self._execute_timeout_duration = Duration(seconds=2.0) # + self._tf_buffer = None + self._tf_listener = None + + self.position_tolerance = 0.3 + self.angle_tolerance = 0.5 + self.target_distance_to_segment = 0.15 + + def on_enter(self, userdata): + self._pose_condition_met = False + self._error = False + self._mapped_segemnt = None + + service_topic = "/" + userdata.ns + "/toggle_segmentation" + self._service_client = ProxyServiceCaller({service_topic: ToggleSegmentation}) + + if not self._service_client.is_available(service_topic): + Logger.logwarn(f"Service {service_topic} not available!") + self._error = True + return + + request = ToggleSegmentation_Request() + request.enable_segmentation = True + + try: + result = self._service_client.call(service_topic, request) + if result.success: + Logger.loginfo(f"Service responded: {result.message}") + else: + Logger.logwarn(f"Service reported failure: {result.message}") + self._error = True + except Exception as e: + Logger.logerr(f"Service call failed: {str(e)}") + self._error = True + return + + try: + self._tf_buffer = Buffer() + self._tf_listener = TransformListener(self._tf_buffer, self._node, spin_thread=False) + Logger.loginfo(f"TF Listener initialized for state '{self.name}'.") + except Exception as e: + Logger.logerr(f"Failed to initialize TF Buffer/Listener for state '{self.name}': {str(e)}") + self._error = True + return + + if self._tf_buffer is None: + Logger.logerr(f"TF Buffer not initialized in state '{self.name}'.") + return "failed" + + try: + target_frame = userdata.target_frame_id + self.target_frame = target_frame + source_frame = userdata.source_frame_id + self.source_frame = source_frame + + transform_stamped = self._tf_buffer.lookup_transform( + target_frame, source_frame, Time(), timeout=self._lookup_timeout_duration + ) + self.transform = transform_stamped + Logger.loginfo( + f"Initial pose (transform from '{source_frame}' to '{target_frame}') acquired:\n" + f"Translation: [x: {transform_stamped.transform.translation.x:.3f}, " + f"y: {transform_stamped.transform.translation.y:.3f}, " + f"z: {transform_stamped.transform.translation.z:.3f}]\n" + f"Rotation: [x: {transform_stamped.transform.rotation.x:.3f}, " + f"y: {transform_stamped.transform.rotation.y:.3f}, " + f"z: {transform_stamped.transform.rotation.z:.3f}, " + f"w: {transform_stamped.transform.rotation.w:.3f}]" + ) + + except TransformException as ex: + Logger.logwarn( + f"Could not transform '{source_frame}' to '{target_frame}' " + f"within {self._lookup_timeout_duration.nanoseconds / 1e9:.2f}s: {ex}" + ) + self._error = True + return "failed" + except Exception as e: + Logger.logerr(f"An unexpected error occurred during TF lookup in state '{self.name}': {str(e)}") + return "failed" + + self.topic_name = "/" + userdata.ns + "/line_segments" + self._subscriber = ProxySubscriberCached({self.topic_name: LineSegments}) + self._subscriber.subscribe( + self.topic_name, LineSegments, callback=self._line_segments_callback, buffered=True, inst_id=id(self) + ) + Logger.loginfo(f"Subscribed to topic: {self.topic_name} with callback _line_segments_callback.") + + # Start timeout timer + self._start_time = self._node.get_clock().now() + + def execute(self, userdata): + if self._error: + return "failed" + + # Check if the condition was met by the callback + if self._pose_condition_met: + Logger.loginfo(f"Pose estimation condition met (flag set by callback) in state '{self.name}'.") + if self._mapped_segemnt is not None: + userdata.transform = self._mapped_segemnt + return "frame_found" + else: + Logger.logwarn(f"Pose condition met but no output transform was generated in state '{self.name}'.") + return "failed" + + # Check for timeout + if self._start_time is not None: + elapsed_time = self._node.get_clock().now() - self._start_time + if elapsed_time >= self._execute_timeout_duration: + Logger.logwarn( + f"Timeout in state '{self.name}': Condition not met within " + f"{self._execute_timeout_duration.nanoseconds / 1e9:.1f} seconds." + ) + return "failed" + + return None + + def on_exit(self, userdata): + if self._tf_listener is not None: + Logger.loginfo(f"Cleaning up TF Listener for state '{self.name}'.") + self._tf_listener = None + self._tf_buffer = None + + # Unsubscribe and clear buffer + if self._subscriber and hasattr(self, "topic_name") and self.topic_name: + self._subscriber.unsubscribe_topic(self.topic_name, inst_id=id(self)) + Logger.loginfo(f"Unsubscribed from topic: {self.topic_name} for instance {id(self)}") + + def _line_segments_callback(self, msg: LineSegments): + """Process incoming line segments and check if any matches robot pose.""" + if self._error or self._pose_condition_met: + return + + if self.transform is None: # This is the robot's initial pose from on_enter + Logger.logwarn("Initial transform not available in _line_segments_callback.") + return + + # Helper: compute smallest angle difference (handles wraparound) + def angle_diff(a, b): + diff = a - b + while diff > math.pi: + diff -= 2 * math.pi + while diff < -math.pi: + diff += 2 * math.pi + return diff + + for segment in msg.segments: + if self._pose_condition_met: + break + # 1) Transform segment endpoints to map frame + pt1 = PointStamped() + pt1.header.stamp = Time().to_msg() + pt1.header.frame_id = segment.frame_id + pt1.point = segment.end_point1 + + pt2 = PointStamped() + pt2.header = pt1.header + pt2.point = segment.end_point2 + + try: + pt1_map = self._tf_buffer.transform(pt1, self.target_frame, timeout=self._lookup_timeout_duration) + pt2_map = self._tf_buffer.transform(pt2, self.target_frame, timeout=self._lookup_timeout_duration) + except TransformException as ex: + Logger.logwarn(f"Transform failed: {ex}") + continue + + # 2) Calculate segment midpoint and angle + mx = 0.5 * (pt1_map.point.x + pt2_map.point.x) + my = 0.5 * (pt1_map.point.y + pt2_map.point.y) + dx = pt2_map.point.x - pt1_map.point.x + dy = pt2_map.point.y - pt1_map.point.y + segment_angle = math.atan2(dy, dx) + + # 3) Extract robot pose from stored transform and convert to yaw + if self.transform is None: + Logger.logwarn("_ros_message_handler: self.transform is None, cannot compare.") + return + + t = self.transform.transform.translation + r = self.transform.transform.rotation + q_t3d = [r.w, r.x, r.y, r.z] # transforms3d expects [w, x, y, z] + rotation_matrix = transforms3d.quaternions.quat2mat(q_t3d) + _, _, ref_yaw = transforms3d.euler.mat2euler(rotation_matrix, axes="sxyz") + + # 4) Calculate distance and angle deviations + dist = math.hypot(mx - t.x, my - t.y) + actual_dist_offset = abs(dist - self.target_distance_to_segment) + + yaw_diff = abs(angle_diff(segment_angle, ref_yaw)) + diff_plus_90 = abs(angle_diff(segment_angle + math.pi / 2.0, ref_yaw)) + diff_minus_90 = abs(angle_diff(segment_angle - math.pi / 2.0, ref_yaw)) + yaw_diff_to_normal = min(diff_plus_90, diff_minus_90) # Robot should face perpendicular to segment + + # 5) Check if segment matches within tolerances + if actual_dist_offset <= self.position_tolerance and yaw_diff_to_normal <= self.angle_tolerance: + Logger.loginfo( + f"Pose condition met for segment. " + f"DistOffset: {actual_dist_offset:.3f} " + f"(Cur: {dist:.3f}, Tgt: {self.target_distance_to_segment:.2f}), " + f"YawDiffToNormal: {yaw_diff_to_normal:.3f}" + ) + + # Create output transform with segment midpoint and orientation + segment_pose_ts = TransformStamped() + segment_pose_ts.header.stamp = self._node.get_clock().now().to_msg() + segment_pose_ts.header.frame_id = self.target_frame # e.g., "map" + # Use the state's instance name to make the child_frame_id somewhat unique + segment_pose_ts.child_frame_id = f"{self.name}_detected_segment" + + segment_pose_ts.transform.translation.x = mx + segment_pose_ts.transform.translation.y = my + segment_pose_ts.transform.translation.z = (pt1_map.point.z + pt2_map.point.z) / 2.0 + + # Convert yaw to quaternion (euler2quat returns w, x, y, z) + q_w, q_x, q_y, q_z = transforms3d.euler.euler2quat(0.0, 0.0, segment_angle, axes="sxyz") + segment_pose_ts.transform.rotation.w = q_w + segment_pose_ts.transform.rotation.x = q_x + segment_pose_ts.transform.rotation.y = q_y + segment_pose_ts.transform.rotation.z = q_z + + self._mapped_segemnt = segment_pose_ts + self._pose_condition_met = True + return + Logger.logwarn( + f"Segment not met. DistOffset: {actual_dist_offset:.4f} " + f"(Cur: {dist:.4f}, Tgt: {self.target_distance_to_segment:.2f}), " + f"YawDiffToNormal: {yaw_diff_to_normal:.4f}, OriginalYawDiff: {yaw_diff:.4f}" + ) diff --git a/caro_skills_flexbe_states/caro_skills_flexbe_states/move_gripper_state.py b/caro_skills_flexbe_states/caro_skills_flexbe_states/move_gripper_state.py index 067ec18..acdde2f 100644 --- a/caro_skills_flexbe_states/caro_skills_flexbe_states/move_gripper_state.py +++ b/caro_skills_flexbe_states/caro_skills_flexbe_states/move_gripper_state.py @@ -1,7 +1,23 @@ -from flexbe_core import EventState, Logger +# # Copyright 2026 Carologistics +# # +# # 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 flexbe_core import EventState +from flexbe_core import Logger from flexbe_core.proxy import ProxyActionClient from gripper_msgs.action import Gripper from rclpy.duration import Duration + + class gripperMoveForward(EventState): """ This state navigates the robot to the given pose using NavigateToPose messages @@ -22,11 +38,14 @@ class gripperMoveForward(EventState): ># y_target Y value of goal pose ># z_target z value of goal pose """ - def __init__(self, timeout,action_topic): - super().__init__(outcomes=['pose_reached', 'failed', 'canceled', 'timeout'], - input_keys=['frame', 'x_target','y_target', 'z_target'], - output_keys=[]) + def __init__(self, timeout, action_topic): + + super().__init__( + outcomes=["pose_reached", "failed", "canceled", "timeout"], + input_keys=["frame", "x_target", "y_target", "z_target"], + output_keys=[], + ) self._timeout = Duration(seconds=timeout) self._timeout_sec = timeout self._topic = action_topic @@ -36,8 +55,9 @@ def __init__(self, timeout,action_topic): # and makes sure only one client is used, no matter how often this state is used in a behavior. ProxyActionClient.initialize(gripperMoveForward._node) - self._client = ProxyActionClient({self._topic: Gripper}, - wait_duration=0.0) # pass required clients as dict (topic: type) + self._client = ProxyActionClient( + {self._topic: Gripper}, wait_duration=0.0 + ) # pass required clients as dict (topic: type) # It may happen that the action client fails to send the action goal. self._error = False @@ -47,12 +67,12 @@ def __init__(self, timeout,action_topic): def execute(self, userdata): """ Call this method periodically while the state is active. - + If no outcome is returned, the state will stay active. """ # Check if the client failed to send the goal. if self._error: - return 'failed' + return "failed" if self._return is not None: # Return prior outcome in case transition is blocked by autonomy level @@ -62,8 +82,8 @@ def execute(self, userdata): # Normal completion, do not bother repeating the publish # We won't bother publishing a 0 command unless blocked (above) # so that we can chain multiple motions together - self._return = 'timeout' - return 'timeout' + self._return = "timeout" + return "timeout" # Normal operation if self._cmd_topic: @@ -80,17 +100,17 @@ def on_enter(self, userdata): """ self._error = False self._return = None # reset the completion flag - if 'x_target' not in userdata: + if "x_target" not in userdata: self._error = True Logger.logwarn("MoveTotState requires userdata.target_x key!") return - - if 'y_target' not in userdata: + + if "y_target" not in userdata: self._error = True Logger.logwarn("MoveTotState requires userdata.target_y key!") return - - if 'z_target' not in userdata: + + if "z_target" not in userdata: self._error = True Logger.logwarn("MoveTotState requires userdata.target_yaw key!") return @@ -115,8 +135,7 @@ def on_enter(self, userdata): Logger.logwarn(f"Invalid frame_id type: {type(userdata.frame).__name__}. Expected a string.") self._error = True return - - + if isinstance(userdata.x_target, float): goal.x_target = userdata.x_target else: @@ -150,8 +169,9 @@ def on_enter(self, userdata): def on_exit(self, userdata): # Make sure that the action is not running when leaving this state. - # A situation where the action would still be active is for example when the operator manually triggers an outcome. + # A situation where the action would still be active is for example when + # the operator manually triggers an outcome. if not self._client.has_result(self._topic): self._client.cancel(self._topic) - Logger.loginfo('Cancelled active action goal.') \ No newline at end of file + Logger.loginfo("Cancelled active action goal.") diff --git a/caro_skills_flexbe_states/caro_skills_flexbe_states/move_to_state.py b/caro_skills_flexbe_states/caro_skills_flexbe_states/move_to_state.py old mode 100644 new mode 100755 index 4e64e75..9ea4c6d --- a/caro_skills_flexbe_states/caro_skills_flexbe_states/move_to_state.py +++ b/caro_skills_flexbe_states/caro_skills_flexbe_states/move_to_state.py @@ -1,33 +1,30 @@ #!/usr/bin/env python - -# Copyright 2023 Carologistics -# -# 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. - +# # Copyright 2026 Carologistics +# # +# # 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. """ This state navigates the robot to the given pose using NavigateToPose messages. """ - # import math - +from flexbe_core import EventState +from flexbe_core import Logger +from flexbe_core.proxy import ProxyActionClient +from nav2_msgs.action import NavigateToPose from rclpy.duration import Duration -# from geometry_msgs.msg import Quaternion from transforms3d.euler import euler2quat -from flexbe_core import EventState, Logger -from flexbe_core.proxy import ProxyActionClient -from nav2_msgs.action import NavigateToPose +# from geometry_msgs.msg import Quaternion # import of required action # This ExampleActionState is based on the standard action tutorials @@ -60,9 +57,11 @@ class MoveToState(EventState): def __init__(self, timeout, action_topic): # See example_state.py for basic explanations. - super().__init__(outcomes=['pose_reached', 'failed', 'canceled', 'timeout'], - input_keys=['frame_id', 'target_x','target_y', 'target_yaw'], - output_keys=[]) + super().__init__( + outcomes=["pose_reached", "failed", "canceled", "timeout"], + input_keys=["frame_id", "target_x", "target_y", "target_yaw"], + output_keys=[], + ) self._timeout = Duration(seconds=timeout) self._timeout_sec = timeout @@ -73,8 +72,9 @@ def __init__(self, timeout, action_topic): # and makes sure only one client is used, no matter how often this state is used in a behavior. ProxyActionClient.initialize(MoveToState._node) - self._client = ProxyActionClient({self._topic: NavigateToPose}, - wait_duration=0.0) # pass required clients as dict (topic: type) + self._client = ProxyActionClient( + {self._topic: NavigateToPose}, wait_duration=0.0 + ) # pass required clients as dict (topic: type) # It may happen that the action client fails to send the action goal. self._error = False @@ -86,7 +86,7 @@ def execute(self, userdata): # Check if the client failed to send the goal. if self._error: - return 'failed' + return "failed" if self._return is not None: # Return prior outcome in case transition is blocked by autonomy level @@ -95,15 +95,15 @@ def execute(self, userdata): # Check if the action has been finished if self._client.has_result(self._topic): _ = self._client.get_result(self._topic) # The delta result value is not useful here - #userdata.duration = self._node.get_clock().now() - self._start_time - Logger.loginfo('Pose reached') - self._return = 'pose_reached' + # userdata.duration = self._node.get_clock().now() - self._start_time + Logger.loginfo("Pose reached") + self._return = "pose_reached" return self._return if self._node.get_clock().now().nanoseconds - self._start_time.nanoseconds > self._timeout.nanoseconds: # Checking for timeout after we check for goal response - self._return = 'timeout' - return 'timeout' + self._return = "timeout" + return "timeout" # If the action has not yet finished, no outcome will be returned and the state stays active. return None @@ -114,17 +114,17 @@ def on_enter(self, userdata): self._error = False self._return = None - if 'target_x' not in userdata: + if "target_x" not in userdata: self._error = True Logger.logwarn("MoveTotState requires userdata.target_x key!") return - - if 'target_y' not in userdata: + + if "target_y" not in userdata: self._error = True Logger.logwarn("MoveTotState requires userdata.target_y key!") return - - if 'target_yaw' not in userdata: + + if "target_yaw" not in userdata: self._error = True Logger.logwarn("MoveTotState requires userdata.target_yaw key!") return @@ -145,14 +145,13 @@ def on_enter(self, userdata): Logger.logwarn("target_yaw is %s. Expects an int or a float.", type(userdata.target_yaw).__name__) Logger.logwarn("target_yaw is %f.", userdata.target_yaw) - if isinstance(userdata.frame_id, str): goal.pose.header.frame_id = userdata.frame_id else: self._error = True Logger.logwarn("Input is %s. Expects a string.", type(userdata.frame_id).__name__) return - + if isinstance(userdata.target_x, (float, int)): goal.pose.pose.position.x = userdata.target_x else: @@ -173,7 +172,6 @@ def on_enter(self, userdata): goal.pose.pose.orientation.y = quat[2] goal.pose.pose.orientation.z = quat[3] goal.pose.pose.orientation.w = quat[0] - pass else: self._error = True Logger.logwarn("Input is %s. Expects an int or a float.", type(userdata.target_yaw).__name__) @@ -191,8 +189,9 @@ def on_enter(self, userdata): def on_exit(self, userdata): # Make sure that the action is not running when leaving this state. - # A situation where the action would still be active is for example when the operator manually triggers an outcome. + # A situation where the action would still be active is for example when + # the operator manually triggers an outcome. if not self._client.has_result(self._topic): self._client.cancel(self._topic) - Logger.loginfo('Cancelled active action goal.') + Logger.loginfo("Cancelled active action goal.") diff --git a/caro_skills_flexbe_states/package.xml b/caro_skills_flexbe_states/package.xml index 44dfb01..51ebfa5 100644 --- a/caro_skills_flexbe_states/package.xml +++ b/caro_skills_flexbe_states/package.xml @@ -16,12 +16,14 @@ rclpy flexbe_core nav2_msgs - python3-numpy - flexbe_core - gripper_msg + motor_move_msgs tf_transformations tf + transforms3d + python3-numpy + gripper_msg std_msgs + ament_copyright ament_flake8 ament_pep257 diff --git a/caro_skills_flexbe_states/setup.py b/caro_skills_flexbe_states/setup.py old mode 100644 new mode 100755 index 4ebe876..8a50945 --- a/caro_skills_flexbe_states/setup.py +++ b/caro_skills_flexbe_states/setup.py @@ -1,34 +1,45 @@ #!/usr/bin/env python - +# # Copyright 2026 Carologistics +# # +# # 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 glob import glob -from setuptools import setup + from setuptools import find_packages +from setuptools import setup -PACKAGE_NAME = 'caro_skills_flexbe_states' +PACKAGE_NAME = "caro_skills_flexbe_states" setup( name=PACKAGE_NAME, - version='0.0.1', + version="0.0.1", packages=find_packages(), data_files=[ - ('share/ament_index/resource_index/packages', - ['resource/' + PACKAGE_NAME]), - ('share/' + PACKAGE_NAME, ['package.xml']), - ('share/' + PACKAGE_NAME + "/tests", glob('tests/*.test')), - ('share/' + PACKAGE_NAME + "/launch", glob('tests/*.launch.py')), + ("share/ament_index/resource_index/packages", ["resource/" + PACKAGE_NAME]), + ("share/" + PACKAGE_NAME, ["package.xml"]), + ("share/" + PACKAGE_NAME + "/tests", glob("tests/*.test")), + ("share/" + PACKAGE_NAME + "/launch", glob("tests/*.launch.py")), ], - - install_requires=['setuptools'], + install_requires=["setuptools"], zip_safe=True, - maintainer='TODO', - maintainer_email='TODO@TODO.com', - description='TODO: Package description', - license='TODO: License declaration', - tests_require=['pytest'], + maintainer="TODO", + maintainer_email="TODO@TODO.com", + description="TODO: Package description", + license="TODO: License declaration", + tests_require=["pytest"], entry_points={ - 'console_scripts': [ - 'example_action_state = caro_skills_flexbe_states.example_action_state', - 'example_state = caro_skills_flexbe_states.example_state', + "console_scripts": [ + "example_action_state = caro_skills_flexbe_states.example_action_state", + "example_state = caro_skills_flexbe_states.example_state", ], }, ) diff --git a/caro_skills_flexbe_states/tests/run_colcon_test.py b/caro_skills_flexbe_states/tests/run_colcon_test.py index fcdeaa0..ed1de87 100644 --- a/caro_skills_flexbe_states/tests/run_colcon_test.py +++ b/caro_skills_flexbe_states/tests/run_colcon_test.py @@ -1,3 +1,16 @@ +# # Copyright 2026 Carologistics +# # +# # 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. # Copyright 2023 Christopher Newport University # # Redistribution and use in source and binary forms, with or without @@ -25,11 +38,7 @@ # 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. - - """Pytest testing for caro_skills_flexbe_states.""" - - from flexbe_testing.py_tester import PyTester diff --git a/caro_skills_flexbe_states/tests/run_tests.launch.py b/caro_skills_flexbe_states/tests/run_tests.launch.py index 5732b09..b29a1ef 100644 --- a/caro_skills_flexbe_states/tests/run_tests.launch.py +++ b/caro_skills_flexbe_states/tests/run_tests.launch.py @@ -1,3 +1,16 @@ +# # Copyright 2026 Carologistics +# # +# # 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. # Copyright 2023 Philipp Schillinger, Team ViGIR, Christopher Newport University # # Redistribution and use in source and binary forms, with or without @@ -25,22 +38,21 @@ # 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. - """caro_skills_flexbe_states testing.""" - from os.path import join +from ament_index_python.packages import get_package_share_directory from launch import LaunchDescription -from launch.actions import DeclareLaunchArgument, IncludeLaunchDescription -from launch.substitutions import LaunchConfiguration +from launch.actions import DeclareLaunchArgument +from launch.actions import IncludeLaunchDescription from launch.launch_description_sources import PythonLaunchDescriptionSource -from ament_index_python.packages import get_package_share_directory +from launch.substitutions import LaunchConfiguration def generate_launch_description(): """Flexbe_states testing.""" - flexbe_testing_dir = get_package_share_directory('flexbe_testing') - flexbe_states_test_dir = get_package_share_directory('caro_skills_flexbe_states') + flexbe_testing_dir = get_package_share_directory("flexbe_testing") + flexbe_states_test_dir = get_package_share_directory("caro_skills_flexbe_states") path = join(flexbe_states_test_dir, "tests") @@ -49,16 +61,18 @@ def generate_launch_description(): testcases += join(path, "example_state.test") + "\n" testcases += join(path, "example_action_state.test") + "\n" - return LaunchDescription([ - DeclareLaunchArgument("pkg", default_value="caro_skills_flexbe_states"), - DeclareLaunchArgument("testcases", default_value=testcases), - DeclareLaunchArgument("compact_format", default_value='true'), - IncludeLaunchDescription( - PythonLaunchDescriptionSource(join(flexbe_testing_dir, "launch", "flexbe_testing.launch.py")), - launch_arguments={ - 'package': LaunchConfiguration("pkg"), - 'compact_format': LaunchConfiguration("compact_format"), - 'testcases': LaunchConfiguration("testcases"), - }.items() - ) - ]) + return LaunchDescription( + [ + DeclareLaunchArgument("pkg", default_value="caro_skills_flexbe_states"), + DeclareLaunchArgument("testcases", default_value=testcases), + DeclareLaunchArgument("compact_format", default_value="true"), + IncludeLaunchDescription( + PythonLaunchDescriptionSource(join(flexbe_testing_dir, "launch", "flexbe_testing.launch.py")), + launch_arguments={ + "package": LaunchConfiguration("pkg"), + "compact_format": LaunchConfiguration("compact_format"), + "testcases": LaunchConfiguration("testcases"), + }.items(), + ), + ] + )