├── fkie_behavior_tree_manager ├── .gitignore ├── scripts │ └── run_groot.sh ├── config │ ├── dummy_nav_tree.xml │ └── bt_default.yaml ├── launch │ └── behavior_tree.launch ├── src │ ├── BehaviorTreeNodes.cpp │ ├── behavior_tree_manager_node.cpp │ ├── BehaviorTreeManager.h │ └── BehaviorTreeParameters.hpp ├── package.xml ├── cmake │ └── FindZMQ.cmake ├── include │ └── fkie_behavior_tree_manager │ │ ├── conditions │ │ ├── checkBooleanPort.hpp │ │ ├── ValidPose.hpp │ │ ├── checkCommand.hpp │ │ ├── CheckTimeoutAsync.hpp │ │ ├── GenericMsgReceived.hpp │ │ ├── BoolCondition.hpp │ │ ├── TFTransformCondition.hpp │ │ ├── PoseReceived.hpp │ │ ├── PoseStampedReceived.hpp │ │ ├── TFTransformAsyncCondition.hpp │ │ └── CheckPoseReachedAsync.hpp │ │ ├── actions │ │ ├── DelayAction.hpp │ │ ├── PoseStampedToPose2D.hpp │ │ ├── ServiceEmptyAction.hpp │ │ ├── PublishPathAction.hpp │ │ ├── PublishPoseAction.hpp │ │ ├── PublishPoseStampedAction.hpp │ │ ├── BoolAction.hpp │ │ ├── Relative3DPoseAction.hpp │ │ └── WaitForGenericMsgAsyncAction.hpp │ │ ├── loggers │ │ └── RosMsgLogger.hpp │ │ ├── decorators │ │ ├── RunOnlyOnce.hpp │ │ ├── ConditionalLoop.hpp │ │ ├── CommandTrigger.hpp │ │ ├── CommandSwitch.hpp │ │ └── RateController.hpp │ │ ├── utils.hpp │ │ └── BTPublisherRosSingleton.hpp └── CMakeLists.txt ├── fkie_bt_move_base_actions ├── .gitignore ├── msg │ └── NavigationResult.msg ├── config │ ├── tree_move_base_parallel.xml │ └── tree_move_base_simple.xml ├── src │ ├── MoveBaseActions.cpp │ ├── MoveBaseUtils.h │ ├── Recovery.h │ ├── NavigateToGoal.h │ ├── CancelExePathSync.h │ ├── NavigateToGoalSync.h │ ├── CancelExePathSync.cpp │ ├── GetPath.h │ ├── MoveBaseActions.h │ ├── ExePathAsync.h │ ├── ExePathSync.h │ ├── Recovery.cpp │ ├── NotifyNavigationResult.h │ ├── NavigateToGoalSync.cpp │ └── NavigateToGoal.cpp ├── package.xml └── CMakeLists.txt ├── fkie_behavior_tree_msgs ├── msg │ ├── BTLogging.msg │ ├── BTStatus.msg │ └── BTNodeStatus.msg ├── CMakeLists.txt └── package.xml ├── fkie_bt_moveit_actions ├── config │ ├── bt_moveit_demo.yaml │ ├── bt_moveit_default.yaml │ ├── trees │ │ ├── tree_moveit_simple.xml │ │ └── tree_moveit_demo.xml │ └── bt_default.yaml ├── src │ ├── MoveitActions.cpp │ └── MoveitActions.h ├── package.xml ├── CMakeLists.txt └── include │ └── fkie_bt_moveit_actions │ └── MoveitErrorCodeInterface.hpp ├── fkie_behavior_trees ├── CMakeLists.txt └── package.xml ├── fkie_bt_visualization ├── plugin_description.xml ├── package.xml ├── CMakeLists.txt ├── src │ ├── rviz_bt_logger.h │ ├── rviz_bt_status.h │ └── randomcolor │ │ └── randomcolor.h └── include │ └── fkie_bt_visualization │ └── TreeItem.hpp ├── fkie_bt_tutorials ├── config │ ├── hello_world_tree.xml │ └── hello_world_config.yaml ├── launch │ └── 1_hello_world_tutorial.launch ├── package.xml ├── src │ ├── TutorialActions.cpp │ └── TutorialActions.h ├── CMakeLists.txt └── include │ └── fkie_bt_tutorials │ └── actions │ └── HelloWorldAction.hpp └── README.md /fkie_behavior_tree_manager/.gitignore: -------------------------------------------------------------------------------- 1 | *.json 2 | -------------------------------------------------------------------------------- /fkie_bt_move_base_actions/.gitignore: -------------------------------------------------------------------------------- 1 | *.json 2 | -------------------------------------------------------------------------------- /fkie_behavior_tree_msgs/msg/BTLogging.msg: -------------------------------------------------------------------------------- 1 | Header header 2 | string source 3 | string level 4 | string data -------------------------------------------------------------------------------- /fkie_behavior_tree_manager/scripts/run_groot.sh: -------------------------------------------------------------------------------- 1 | #!/bin/bash 2 | ~/ros/external/Groot/build/Groot --mode monitor -------------------------------------------------------------------------------- /fkie_behavior_tree_msgs/msg/BTStatus.msg: -------------------------------------------------------------------------------- 1 | Header header 2 | string tree_file 3 | string tree_xml 4 | bool tree_is_running -------------------------------------------------------------------------------- /fkie_bt_move_base_actions/msg/NavigationResult.msg: -------------------------------------------------------------------------------- 1 | Header header 2 | uint16 nav_error_code 3 | string origin 4 | string description -------------------------------------------------------------------------------- /fkie_bt_moveit_actions/config/bt_moveit_demo.yaml: -------------------------------------------------------------------------------- 1 | MoveitActions: 2 | planning_group: "robot_arm" 3 | execute_trajectory: true 4 | -------------------------------------------------------------------------------- /fkie_behavior_tree_msgs/msg/BTNodeStatus.msg: -------------------------------------------------------------------------------- 1 | Header header 2 | string node_name 3 | string node_type 4 | string node_id 5 | string node_status -------------------------------------------------------------------------------- /fkie_bt_moveit_actions/config/bt_moveit_default.yaml: -------------------------------------------------------------------------------- 1 | MoveitActions: 2 | planning_group: "arm" 3 | execute_trajectory: false 4 | ExecuteWaypoints: 5 | radius: 0.5 6 | x_init: 0.4 7 | y_init: 0.0 8 | z_init: 1.0 -------------------------------------------------------------------------------- /fkie_bt_moveit_actions/config/trees/tree_moveit_simple.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | -------------------------------------------------------------------------------- /fkie_behavior_trees/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 3.0.2) 2 | project(fkie_behavior_trees) 3 | 4 | find_package(catkin REQUIRED COMPONENTS) 5 | 6 | catkin_package() 7 | 8 | ########### 9 | ## Build ## 10 | ########### 11 | 12 | include_directories( 13 | ${catkin_INCLUDE_DIRS} 14 | ) 15 | -------------------------------------------------------------------------------- /fkie_behavior_tree_manager/config/dummy_nav_tree.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | -------------------------------------------------------------------------------- /fkie_bt_moveit_actions/config/bt_default.yaml: -------------------------------------------------------------------------------- 1 | tree: 2 | static_xml: "" 3 | # static_file: "" 4 | rate: 10.0 5 | 6 | logging: 7 | #logger_type: "ZMQ_LOGGER" 8 | #logger_type: "MINITRACE_LOGGER" 9 | #logger_type: "STD_COUNT_LOGGER" 10 | file_logger_path: "bt_trace.fbl" 11 | minitrace_logger_path: "/home/garcia/Documents/logging/bt_trace.json" 12 | zmq_max_msg_per_second: 25 13 | 14 | action_plugins: 15 | ["libMoveitActions_dyn.so"] 16 | -------------------------------------------------------------------------------- /fkie_behavior_tree_manager/launch/behavior_tree.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | -------------------------------------------------------------------------------- /fkie_bt_visualization/plugin_description.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | A panel widget to visualize the current execution state of a Behavior Tree 6 | 7 | 8 | 9 | 10 | 11 | A panel widget to visualize logs from Behavior Tree running nodes 12 | 13 | 14 | 15 | 16 | -------------------------------------------------------------------------------- /fkie_behavior_tree_manager/config/bt_default.yaml: -------------------------------------------------------------------------------- 1 | tree: 2 | static_xml: "" 3 | # static_file: "" 4 | rate : 2.0 5 | start_tree_at_init: True 6 | 7 | logging: 8 | RosMsgLogger: 9 | enabled: true 10 | StdCoutLogger: 11 | enabled: false 12 | PublisherZMQ: 13 | enabled: true 14 | max_msg_per_second: 25 15 | publisher_port: 1666 16 | server_port: 1667 17 | FileLogger: 18 | enabled: false 19 | file_path: "bt_trace.fbl" 20 | MinitraceLogger: 21 | enabled: false 22 | file_path: "bt_trace.json" 23 | 24 | # logger_type: "ZMQ_LOGGER" # deprecated 25 | 26 | # Example of loading an action pluging 27 | #action_plugins: 28 | # ["libMoveBaseActions_dyn.so"] 29 | 30 | 31 | -------------------------------------------------------------------------------- /fkie_behavior_tree_msgs/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 3.0.2) 2 | project(fkie_behavior_tree_msgs) 3 | 4 | find_package(catkin REQUIRED COMPONENTS roscpp std_msgs geometry_msgs message_generation message_runtime) 5 | 6 | ## Generate messages in the 'msg' folder 7 | add_message_files( 8 | FILES 9 | BTNodeStatus.msg 10 | BTStatus.msg 11 | BTLogging.msg 12 | ) 13 | 14 | ## Generate added messages and services with any dependencies listed here 15 | generate_messages( 16 | DEPENDENCIES 17 | geometry_msgs 18 | std_msgs 19 | ) 20 | 21 | catkin_package( 22 | CATKIN_DEPENDS geometry_msgs std_msgs message_runtime roscpp 23 | ) 24 | 25 | include_directories( 26 | ${catkin_INCLUDE_DIRS} 27 | ) 28 | -------------------------------------------------------------------------------- /fkie_bt_visualization/package.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | fkie_bt_visualization 4 | 0.0.0 5 | The fkie_bt_visualization package 6 | 7 | Francisco Garcia 8 | Francisco Garcia 9 | 10 | Apache License 2.0 11 | 12 | catkin 13 | 14 | roscpp 15 | rviz 16 | fkie_behavior_tree_msgs 17 | 18 | 19 | 20 | 21 | 22 | 23 | -------------------------------------------------------------------------------- /fkie_bt_moveit_actions/config/trees/tree_moveit_demo.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | -------------------------------------------------------------------------------- /fkie_bt_tutorials/config/hello_world_tree.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | -------------------------------------------------------------------------------- /fkie_behavior_tree_msgs/package.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | fkie_behavior_tree_msgs 5 | 0.0.1 6 | Messages used for behavior tree interfaces 7 | 8 | Francisco Garcia 9 | Francisco Garcia 10 | 11 | Apache License 2.0 12 | 13 | catkin 14 | 15 | roscpp 16 | message_generation 17 | std_msgs 18 | geometry_msgs 19 | message_runtime 20 | 21 | 22 | 23 | 24 | 25 | -------------------------------------------------------------------------------- /fkie_bt_tutorials/launch/1_hello_world_tutorial.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | 16 | 17 | -------------------------------------------------------------------------------- /fkie_bt_move_base_actions/config/tree_move_base_parallel.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | 16 | 17 | -------------------------------------------------------------------------------- /fkie_bt_tutorials/package.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | fkie_bt_tutorials 4 | 0.0.0 5 | Tutorial package for fkie_behavior_tree_manager 6 | 7 | Francisco Garcia 8 | Francisco Garcia 9 | 10 | Apache License 2.0 11 | 12 | catkin 13 | 14 | fkie_behavior_tree_manager 15 | 16 | roscpp 17 | rospy 18 | std_msgs 19 | 20 | 21 | 22 | 23 | 24 | 25 | -------------------------------------------------------------------------------- /fkie_bt_tutorials/src/TutorialActions.cpp: -------------------------------------------------------------------------------- 1 | // Copyright 2022 Fraunhofer FKIE - All Rights Reserved 2 | // 3 | // Licensed under the Apache License, Version 2.0 (the "License"); 4 | // you may not use this file except in compliance with the License. 5 | // You may obtain a copy of the License at 6 | // 7 | // http://www.apache.org/licenses/LICENSE-2.0 8 | // 9 | // Unless required by applicable law or agreed to in writing, software 10 | // distributed under the License is distributed on an "AS IS" BASIS, 11 | // WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 12 | // See the License for the specific language governing permissions and 13 | // limitations under the License. 14 | 15 | #include "TutorialActions.h" 16 | 17 | // This function must be implemented in the .cpp file to create 18 | // a plugin that can be loaded at run-time 19 | BT_REGISTER_NODES(factory) 20 | { 21 | BTTutorialGroup::RegisterNodes(factory); 22 | } 23 | -------------------------------------------------------------------------------- /fkie_bt_moveit_actions/src/MoveitActions.cpp: -------------------------------------------------------------------------------- 1 | // Copyright 2022 Fraunhofer FKIE - All Rights Reserved 2 | // 3 | // Licensed under the Apache License, Version 2.0 (the "License"); 4 | // you may not use this file except in compliance with the License. 5 | // You may obtain a copy of the License at 6 | // 7 | // http://www.apache.org/licenses/LICENSE-2.0 8 | // 9 | // Unless required by applicable law or agreed to in writing, software 10 | // distributed under the License is distributed on an "AS IS" BASIS, 11 | // WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 12 | // See the License for the specific language governing permissions and 13 | // limitations under the License. 14 | 15 | #include "MoveitActions.h" 16 | 17 | // This function must be implemented in the .cpp file to create 18 | // a plugin that can be loaded at run-time 19 | BT_REGISTER_NODES(factory) 20 | { 21 | MoveitActionsGroup::RegisterNodes(factory); 22 | } 23 | -------------------------------------------------------------------------------- /fkie_behavior_tree_manager/src/BehaviorTreeNodes.cpp: -------------------------------------------------------------------------------- 1 | // Copyright 2022 Fraunhofer FKIE - All Rights Reserved 2 | // 3 | // Licensed under the Apache License, Version 2.0 (the "License"); 4 | // you may not use this file except in compliance with the License. 5 | // You may obtain a copy of the License at 6 | // 7 | // http://www.apache.org/licenses/LICENSE-2.0 8 | // 9 | // Unless required by applicable law or agreed to in writing, software 10 | // distributed under the License is distributed on an "AS IS" BASIS, 11 | // WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 12 | // See the License for the specific language governing permissions and 13 | // limitations under the License. 14 | 15 | #include "BehaviorTreeNodes.h" 16 | 17 | // This function must be implemented in the .cpp file to create 18 | // a plugin that can be loaded at run-time 19 | BT_REGISTER_NODES(factory) 20 | { 21 | BehaviorTreeNodes::RegisterNodes(factory); 22 | } -------------------------------------------------------------------------------- /fkie_bt_move_base_actions/src/MoveBaseActions.cpp: -------------------------------------------------------------------------------- 1 | // Copyright 2022 Fraunhofer FKIE - All Rights Reserved 2 | // 3 | // Licensed under the Apache License, Version 2.0 (the "License"); 4 | // you may not use this file except in compliance with the License. 5 | // You may obtain a copy of the License at 6 | // 7 | // http://www.apache.org/licenses/LICENSE-2.0 8 | // 9 | // Unless required by applicable law or agreed to in writing, software 10 | // distributed under the License is distributed on an "AS IS" BASIS, 11 | // WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 12 | // See the License for the specific language governing permissions and 13 | // limitations under the License. 14 | 15 | #include "MoveBaseActions.h" 16 | 17 | // This function must be implemented in the .cpp file to create 18 | // a plugin that can be loaded at run-time 19 | BT_REGISTER_NODES(factory) 20 | { 21 | MoveBaseActionsGroup::RegisterNodes(factory); 22 | } 23 | -------------------------------------------------------------------------------- /fkie_bt_tutorials/config/hello_world_config.yaml: -------------------------------------------------------------------------------- 1 | tree: 2 | rate: 5.0 # rate from main root tree 3 | start_tree_at_init: true # Defines if tree must run automatically or wait for start message 4 | 5 | # loggin examples 6 | logging: 7 | # RosMsgLogger: Send ros messages with current BT status 8 | RosMsgLogger: 9 | enabled: true 10 | 11 | # StdCoutLogger: print BT node transitions to the console 12 | StdCoutLogger: 13 | enabled: false 14 | 15 | # PublisherZMQ: allows us to use Groot as monitor tool 16 | PublisherZMQ: 17 | enabled: true 18 | max_msg_per_second: 25 19 | publisher_port: 1666 20 | server_port: 1667 21 | 22 | FileLogger: 23 | enabled: false 24 | file_path: "bt_trace.fbl" 25 | 26 | MinitraceLogger: 27 | enabled: false 28 | file_path: "bt_trace.json" 29 | 30 | # load required libraries: 31 | # Hint: we generate [TutorialActions_dyn] and we can use it as [libTutorialActions_dyn.so] 32 | action_plugins: ["libTutorialActions_dyn.so"] 33 | -------------------------------------------------------------------------------- /fkie_behavior_trees/package.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | fkie_behavior_trees 4 | 0.0.0 5 | fkie_behavior_trees meta package 6 | 7 | Francisco Garcia 8 | Francisco Garcia 9 | 10 | Apache License 2.0 11 | 12 | catkin 13 | 14 | fkie_behavior_tree_msgs 15 | fkie_behavior_tree_manager 16 | fkie_bt_move_base_actions 17 | fkie_bt_moveit_actions 18 | fkie_bt_tutorials 19 | fkie_bt_visualization 20 | 21 | 22 | 23 | 24 | 25 | 26 | -------------------------------------------------------------------------------- /fkie_behavior_tree_manager/package.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | fkie_behavior_tree_manager 4 | 0.0.0 5 | The fkie_behavior_tree_manager package 6 | 7 | Francisco Garcia 8 | Francisco Garcia 9 | 10 | Apache License 2.0 11 | 12 | catkin 13 | 14 | roscpp 15 | rospy 16 | std_msgs 17 | geometry_msgs 18 | nav_msgs 19 | behaviortree_cpp_v3 20 | tf 21 | topic_tools 22 | tf2_ros 23 | fkie_behavior_tree_msgs 24 | 25 | 26 | 27 | 28 | 29 | 30 | -------------------------------------------------------------------------------- /fkie_bt_move_base_actions/config/tree_move_base_simple.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | 16 | 17 | 18 | 19 | 20 | -------------------------------------------------------------------------------- /fkie_bt_tutorials/src/TutorialActions.h: -------------------------------------------------------------------------------- 1 | // Copyright 2022 Fraunhofer FKIE - All Rights Reserved 2 | // 3 | // Licensed under the Apache License, Version 2.0 (the "License"); 4 | // you may not use this file except in compliance with the License. 5 | // You may obtain a copy of the License at 6 | // 7 | // http://www.apache.org/licenses/LICENSE-2.0 8 | // 9 | // Unless required by applicable law or agreed to in writing, software 10 | // distributed under the License is distributed on an "AS IS" BASIS, 11 | // WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 12 | // See the License for the specific language governing permissions and 13 | // limitations under the License. 14 | 15 | #ifndef TUTORIAL_ACTIONS_H 16 | #define TUTORIAL_ACTIONS_H 17 | 18 | #include 19 | #include 20 | #include 21 | #include 22 | 23 | #include "fkie_bt_tutorials/actions/HelloWorldAction.hpp" 24 | 25 | namespace BTTutorialGroup 26 | { 27 | inline void RegisterNodes(BT::BehaviorTreeFactory& factory) 28 | { 29 | // Register the action [HelloWorldAction] with the same name 30 | factory.registerNodeType("HelloWorldAction"); 31 | } 32 | } // namespace BTTutorialGroup 33 | 34 | #endif // TUTORIAL_ACTIONS_H -------------------------------------------------------------------------------- /fkie_bt_moveit_actions/package.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | fkie_bt_moveit_actions 4 | 0.0.0 5 | The fkie_bt_moveit_actions package 6 | 7 | Francisco Garcia 8 | Francisco Garcia 9 | 10 | Apache License 2.0 11 | 12 | catkin 13 | 14 | roscpp 15 | rospy 16 | std_msgs 17 | actionlib 18 | actionlib_msgs 19 | fkie_behavior_tree_manager 20 | pluginlib 21 | eigen 22 | moveit_core 23 | moveit_ros_planning 24 | moveit_ros_planning_interface 25 | moveit_ros_perception 26 | geometric_shapes 27 | moveit_visual_tools 28 | tf2_ros 29 | tf2_eigen 30 | tf2_geometry_msgs 31 | tf_conversions 32 | tf 33 | eigen_conversions 34 | 35 | behaviortree_cpp_v3 36 | 37 | 38 | 39 | 40 | 41 | 42 | -------------------------------------------------------------------------------- /fkie_bt_tutorials/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 3.0.2) 2 | project(fkie_bt_tutorials) 3 | 4 | find_package(catkin REQUIRED COMPONENTS 5 | fkie_behavior_tree_manager 6 | ) 7 | 8 | find_package(BehaviorTreeV3) 9 | 10 | catkin_package( 11 | CATKIN_DEPENDS fkie_behavior_tree_manager 12 | ) 13 | 14 | include_directories( 15 | include 16 | ${catkin_INCLUDE_DIRS} 17 | ${CMAKE_CURRENT_SOURCE_DIR} 18 | ) 19 | 20 | ########### 21 | ## Build ## 22 | ########### 23 | 24 | # static 25 | add_library(TutorialActions STATIC src/TutorialActions.cpp ) 26 | target_link_libraries(TutorialActions PRIVATE ${catkin_LIBRARIES} BT::behaviortree_cpp_v3) 27 | set_target_properties(TutorialActions PROPERTIES ARCHIVE_OUTPUT_DIRECTORY ${CMAKE_LIBRARY_OUTPUT_DIRECTORY} ) 28 | target_compile_features(TutorialActions PRIVATE cxx_std_17) 29 | 30 | # plugin 31 | add_library(TutorialActions_dyn SHARED src/TutorialActions.cpp ) 32 | target_link_libraries(TutorialActions_dyn PRIVATE ${catkin_LIBRARIES} BT::behaviortree_cpp_v3) 33 | target_compile_definitions(TutorialActions_dyn PRIVATE BT_PLUGIN_EXPORT) 34 | set_target_properties(TutorialActions_dyn PROPERTIES LIBRARY_OUTPUT_DIRECTORY ${CMAKE_LIBRARY_OUTPUT_DIRECTORY} ) 35 | target_compile_features(TutorialActions_dyn PRIVATE cxx_std_17) 36 | 37 | ############# 38 | ## Install ## 39 | ############# 40 | 41 | install( 42 | TARGETS TutorialActions TutorialActions_dyn 43 | ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} 44 | LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} 45 | RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} 46 | ) -------------------------------------------------------------------------------- /fkie_behavior_tree_manager/cmake/FindZMQ.cmake: -------------------------------------------------------------------------------- 1 | # - Try to find ZMQ 2 | # Once done this will define 3 | # 4 | # ZMQ_FOUND - system has ZMQ 5 | # ZMQ_INCLUDE_DIRS - the ZMQ include directory 6 | # ZMQ_LIBRARIES - Link these to use ZMQ 7 | # ZMQ_DEFINITIONS - Compiler switches required for using ZMQ 8 | # 9 | # Copyright (c) 2011 Lee Hambley 10 | # 11 | # Redistribution and use is allowed according to the terms of the New 12 | # BSD license. 13 | # For details see the accompanying COPYING-CMAKE-SCRIPTS file. 14 | # 15 | 16 | if (ZMQ_LIBRARIES AND ZMQ_INCLUDE_DIRS) 17 | # in cache already 18 | set(ZMQ_FOUND TRUE) 19 | else (ZMQ_LIBRARIES AND ZMQ_INCLUDE_DIRS) 20 | 21 | find_path(ZMQ_INCLUDE_DIR 22 | NAMES 23 | zmq.h 24 | PATHS 25 | /usr/include 26 | /usr/local/include 27 | /opt/local/include 28 | ) 29 | 30 | find_library(ZMQ_LIBRARY 31 | NAMES 32 | zmq 33 | PATHS 34 | /usr/lib 35 | /usr/local/lib 36 | /opt/local/lib 37 | /sw/lib 38 | ) 39 | 40 | set(ZMQ_INCLUDE_DIRS 41 | ${ZMQ_INCLUDE_DIR} 42 | ) 43 | 44 | if (ZMQ_LIBRARY) 45 | set(ZMQ_LIBRARIES 46 | ${ZMQ_LIBRARIES} 47 | ${ZMQ_LIBRARY} 48 | ) 49 | endif (ZMQ_LIBRARY) 50 | 51 | include(FindPackageHandleStandardArgs) 52 | find_package_handle_standard_args(ZMQ DEFAULT_MSG ZMQ_LIBRARIES ZMQ_INCLUDE_DIRS) 53 | 54 | # show the ZMQ_INCLUDE_DIRS and ZMQ_LIBRARIES variables only in the advanced view 55 | mark_as_advanced(ZMQ_INCLUDE_DIRS ZMQ_LIBRARIES) 56 | 57 | endif (ZMQ_LIBRARIES AND ZMQ_INCLUDE_DIRS) 58 | 59 | -------------------------------------------------------------------------------- /fkie_behavior_tree_manager/include/fkie_behavior_tree_manager/conditions/checkBooleanPort.hpp: -------------------------------------------------------------------------------- 1 | // Copyright 2022 Fraunhofer FKIE - All Rights Reserved 2 | // 3 | // Licensed under the Apache License, Version 2.0 (the "License"); 4 | // you may not use this file except in compliance with the License. 5 | // You may obtain a copy of the License at 6 | // 7 | // http://www.apache.org/licenses/LICENSE-2.0 8 | // 9 | // Unless required by applicable law or agreed to in writing, software 10 | // distributed under the License is distributed on an "AS IS" BASIS, 11 | // WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 12 | // See the License for the specific language governing permissions and 13 | // limitations under the License. 14 | 15 | #ifndef CHECK_BOOLEAN_PORT_H 16 | #define CHECK_BOOLEAN_PORT_H 17 | 18 | #include 19 | 20 | #include "behaviortree_cpp_v3/behavior_tree.h" 21 | #include "behaviortree_cpp_v3/bt_factory.h" 22 | 23 | #include 24 | #include 25 | 26 | namespace BehaviorTreeNodes 27 | { 28 | BT::NodeStatus CheckBooleanPort(BT::TreeNode& self) 29 | { 30 | bool port_value = false; 31 | bool success_value = false; 32 | 33 | auto res = self.getInput("port_value", port_value); 34 | if (!res) 35 | { 36 | BT_ROS_ERROR_STREAM(self.name() << " Error at reading [port_value]: " << res.error()); 37 | return BT::NodeStatus::FAILURE; 38 | } 39 | 40 | res = self.getInput("success_value", success_value); 41 | if (!res) 42 | { 43 | BT_ROS_ERROR_STREAM(self.name() << " Error at reading [success_value]: " << res.error()); 44 | return BT::NodeStatus::FAILURE; 45 | } 46 | 47 | if (port_value == success_value) 48 | return BT::NodeStatus::SUCCESS; 49 | else 50 | return BT::NodeStatus::FAILURE; 51 | }; 52 | 53 | } // namespace BehaviorTreeNodes 54 | 55 | #endif // CHECK_BOOLEAN_PORT_H -------------------------------------------------------------------------------- /fkie_bt_move_base_actions/src/MoveBaseUtils.h: -------------------------------------------------------------------------------- 1 | // Copyright 2022 Fraunhofer FKIE - All Rights Reserved 2 | // 3 | // Licensed under the Apache License, Version 2.0 (the "License"); 4 | // you may not use this file except in compliance with the License. 5 | // You may obtain a copy of the License at 6 | // 7 | // http://www.apache.org/licenses/LICENSE-2.0 8 | // 9 | // Unless required by applicable law or agreed to in writing, software 10 | // distributed under the License is distributed on an "AS IS" BASIS, 11 | // WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 12 | // See the License for the specific language governing permissions and 13 | // limitations under the License. 14 | 15 | #ifndef MOVE_BASE_SIMPLE_ACTIONS_H 16 | #define MOVE_BASE_SIMPLE_ACTIONS_H 17 | 18 | #include 19 | #include 20 | #include 21 | #include 22 | 23 | #include 24 | #include 25 | #include 26 | 27 | #include "behaviortree_cpp_v3/behavior_tree.h" 28 | #include "behaviortree_cpp_v3/blackboard.h" 29 | #include "behaviortree_cpp_v3/bt_factory.h" 30 | #include "behaviortree_cpp_v3/xml_parsing.h" 31 | 32 | #include 33 | #include 34 | 35 | #include 36 | 37 | typedef actionlib::SimpleActionClient ClientUtilsExePathAction; 38 | 39 | namespace MoveBaseActionsGroup 40 | { 41 | BT::NodeStatus moveBaseHeartbeat(BT::TreeNode& self); 42 | 43 | BT::NodeStatus newTargetPoseReceived(BT::TreeNode& self); 44 | 45 | BT::NodeStatus CancelCurrentPoseAction(BT::TreeNode& self); 46 | 47 | BT::NodeStatus hasValidGoal(BT::TreeNode& self); 48 | 49 | } // namespace MoveBaseActionsGroup 50 | 51 | #endif // MOVE_BASE_SIMPLE_ACTIONS_H -------------------------------------------------------------------------------- /fkie_behavior_tree_manager/include/fkie_behavior_tree_manager/conditions/ValidPose.hpp: -------------------------------------------------------------------------------- 1 | // Copyright 2022 Fraunhofer FKIE - All Rights Reserved 2 | // 3 | // Licensed under the Apache License, Version 2.0 (the "License"); 4 | // you may not use this file except in compliance with the License. 5 | // You may obtain a copy of the License at 6 | // 7 | // http://www.apache.org/licenses/LICENSE-2.0 8 | // 9 | // Unless required by applicable law or agreed to in writing, software 10 | // distributed under the License is distributed on an "AS IS" BASIS, 11 | // WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 12 | // See the License for the specific language governing permissions and 13 | // limitations under the License. 14 | 15 | #ifndef VALID_POSE_H 16 | #define VALID_POSE_H 17 | 18 | #include 19 | #include 20 | 21 | #include "behaviortree_cpp_v3/behavior_tree.h" 22 | #include "behaviortree_cpp_v3/bt_factory.h" 23 | 24 | #include 25 | #include 26 | 27 | namespace BehaviorTreeNodes 28 | { 29 | BT::NodeStatus ValidPose(BT::TreeNode& self) 30 | { 31 | Pose2D target_pose; 32 | auto res = self.getInput("target_pose", target_pose); 33 | if (!res) 34 | { 35 | self.setOutput("target_pose", Pose2D()); 36 | return BT::NodeStatus::FAILURE; 37 | } 38 | 39 | if (target_pose.frame_id.empty()) 40 | { 41 | BT_ROS_WARN_STREAM(self.name() << ": Frame_id is empty, returning Failure"); 42 | return BT::NodeStatus::FAILURE; 43 | } 44 | 45 | if (!std::isfinite(target_pose.x) || !std::isfinite(target_pose.y)) 46 | { 47 | BT_ROS_WARN_STREAM(self.name() << ": invalid (x,y):" << target_pose.toString()); 48 | return BT::NodeStatus::FAILURE; 49 | } 50 | 51 | BT_ROS_DEBUG_STREAM(self.name() << ": pose is valid"); 52 | return BT::NodeStatus::SUCCESS; 53 | }; 54 | 55 | } // namespace BehaviorTreeNodes 56 | 57 | #endif // VALID_POSE_H -------------------------------------------------------------------------------- /fkie_behavior_tree_manager/src/behavior_tree_manager_node.cpp: -------------------------------------------------------------------------------- 1 | // Copyright 2022 Fraunhofer FKIE - All Rights Reserved 2 | // 3 | // Licensed under the Apache License, Version 2.0 (the "License"); 4 | // you may not use this file except in compliance with the License. 5 | // You may obtain a copy of the License at 6 | // 7 | // http://www.apache.org/licenses/LICENSE-2.0 8 | // 9 | // Unless required by applicable law or agreed to in writing, software 10 | // distributed under the License is distributed on an "AS IS" BASIS, 11 | // WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 12 | // See the License for the specific language governing permissions and 13 | // limitations under the License. 14 | 15 | #include "BehaviorTreeManager.h" 16 | 17 | int main(int argc, char** argv) 18 | { 19 | ros::init(argc, argv, "behavior_tree_manager"); 20 | 21 | // initializes ROS NodeHandles - Please do not remove! 22 | ros::NodeHandle public_node = ros::NodeHandle(""); 23 | ros::NodeHandle private_node = ros::NodeHandle("~"); 24 | 25 | BehaviorTreeManager bt_manager; 26 | 27 | // Update ros parameters 28 | bt_manager.params.readParameters(); 29 | 30 | // register available action plugins 31 | BT_ROS_INFO_STREAM("Registered plugins:"); 32 | for (std::string ap : bt_manager.params.action_plugins) 33 | BT_ROS_INFO_STREAM(" - " << ap); 34 | 35 | bt_manager.registerActionPlugins(); 36 | bt_manager.reportBTState(); 37 | 38 | // initialize static tree 39 | if (!bt_manager.params.tree_static_xml.empty()) 40 | { 41 | bt_manager.initializeTreeFromText(bt_manager.params.tree_static_xml); 42 | } 43 | else if (!bt_manager.params.tree_static_file.empty()) 44 | { 45 | bt_manager.initializeTreeFromFile(bt_manager.params.tree_static_file); 46 | } 47 | else 48 | { 49 | BT_ROS_ERROR("No tree is available. Check parameters [tree/static_xml] or [tree/static_file]"); 50 | return 0; 51 | } 52 | 53 | // initialize loggin system 54 | bt_manager.initializeLoggers(); 55 | 56 | bt_manager.spin(); 57 | 58 | return 0; 59 | } -------------------------------------------------------------------------------- /fkie_bt_moveit_actions/src/MoveitActions.h: -------------------------------------------------------------------------------- 1 | // Copyright 2022 Fraunhofer FKIE - All Rights Reserved 2 | // 3 | // Licensed under the Apache License, Version 2.0 (the "License"); 4 | // you may not use this file except in compliance with the License. 5 | // You may obtain a copy of the License at 6 | // 7 | // http://www.apache.org/licenses/LICENSE-2.0 8 | // 9 | // Unless required by applicable law or agreed to in writing, software 10 | // distributed under the License is distributed on an "AS IS" BASIS, 11 | // WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 12 | // See the License for the specific language governing permissions and 13 | // limitations under the License. 14 | 15 | #ifndef MOVEIT_ACTIONS_H 16 | #define MOVEIT_ACTIONS_H 17 | 18 | #include "behaviortree_cpp_v3/behavior_tree.h" 19 | #include "behaviortree_cpp_v3/bt_factory.h" 20 | #include "fkie_bt_moveit_actions/actions/MoveitCartesianPathPlanning.hpp" 21 | #include "fkie_bt_moveit_actions/actions/MoveitExecuteWaypoints.hpp" 22 | #include "fkie_bt_moveit_actions/actions/MoveitMoveToFixConfiguration.hpp" 23 | #include "fkie_bt_moveit_actions/actions/MoveitMoveToPose.hpp" 24 | #include "fkie_bt_moveit_actions/actions/MoveitPlanToPose.hpp" 25 | #include "fkie_bt_moveit_actions/actions/MoveitExecutePlan.hpp" 26 | #include 27 | #include 28 | 29 | namespace MoveitActionsGroup 30 | { 31 | inline void RegisterNodes(BT::BehaviorTreeFactory& factory) 32 | { 33 | // moveit actions 34 | factory.registerNodeType("MoveitCartesianPathPlanning"); 35 | factory.registerNodeType("MoveitExecuteWaypoints"); 36 | factory.registerNodeType("MoveitMoveToPose"); 37 | factory.registerNodeType("MoveitPlanToPose"); 38 | factory.registerNodeType("MoveitExecutePlan"); 39 | factory.registerNodeType("MoveitMoveToFixConfiguration"); 40 | } 41 | } // namespace MoveitActionsGroup 42 | 43 | #endif // MOVEIT_ACTIONS_H 44 | -------------------------------------------------------------------------------- /fkie_behavior_tree_manager/include/fkie_behavior_tree_manager/actions/DelayAction.hpp: -------------------------------------------------------------------------------- 1 | // Copyright 2022 Fraunhofer FKIE - All Rights Reserved 2 | // 3 | // Licensed under the Apache License, Version 2.0 (the "License"); 4 | // you may not use this file except in compliance with the License. 5 | // You may obtain a copy of the License at 6 | // 7 | // http://www.apache.org/licenses/LICENSE-2.0 8 | // 9 | // Unless required by applicable law or agreed to in writing, software 10 | // distributed under the License is distributed on an "AS IS" BASIS, 11 | // WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 12 | // See the License for the specific language governing permissions and 13 | // limitations under the License. 14 | 15 | #ifndef DELAY_ACTION_NODE_H 16 | #define DELAY_ACTION_NODE_H 17 | 18 | #include 19 | #include 20 | #include 21 | #include 22 | 23 | namespace BehaviorTreeNodes 24 | { 25 | class DelayAction : public BT::SyncActionNode 26 | { 27 | public: 28 | std::string name; 29 | 30 | static BT::PortsList providedPorts() 31 | { 32 | BT::PortsList ports = { BT::InputPort("delay_time") }; 33 | return ports; 34 | } 35 | 36 | DelayAction(const std::string& _name, const BT::NodeConfiguration& config) 37 | : BT::SyncActionNode(_name, config), name("[" + _name + "] ") 38 | { 39 | BT_ROS_INFO_STREAM(name << " initialized"); 40 | } 41 | 42 | BT::NodeStatus tick() override 43 | { 44 | setStatus(BT::NodeStatus::RUNNING); 45 | 46 | double delay_time = 0.0; 47 | auto res = BT::TreeNode::getInput("delay_time", delay_time); 48 | if (!res) 49 | { 50 | ROS_ERROR_STREAM("DelayAction: Could not load delay_time, resetting variable. Error: " << res.error()); 51 | BT::TreeNode::setOutput("delay_time", 0.0); 52 | return BT::NodeStatus::FAILURE; 53 | } 54 | 55 | BT_ROS_INFO_STREAM(name << ": Waiting for: " << delay_time << " (s)"); 56 | 57 | ros::Duration(delay_time).sleep(); 58 | return BT::NodeStatus::SUCCESS; 59 | } 60 | }; 61 | 62 | } // namespace BehaviorTreeNodes 63 | 64 | #endif // DELAY_ACTION_NODE_H -------------------------------------------------------------------------------- /fkie_behavior_tree_manager/include/fkie_behavior_tree_manager/loggers/RosMsgLogger.hpp: -------------------------------------------------------------------------------- 1 | // Copyright 2022 Fraunhofer FKIE - All Rights Reserved 2 | // 3 | // Licensed under the Apache License, Version 2.0 (the "License"); 4 | // you may not use this file except in compliance with the License. 5 | // You may obtain a copy of the License at 6 | // 7 | // http://www.apache.org/licenses/LICENSE-2.0 8 | // 9 | // Unless required by applicable law or agreed to in writing, software 10 | // distributed under the License is distributed on an "AS IS" BASIS, 11 | // WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 12 | // See the License for the specific language governing permissions and 13 | // limitations under the License. 14 | 15 | #ifndef BT_ROS_MSG_LOGGER_H 16 | #define BT_ROS_MSG_LOGGER_H 17 | 18 | #include 19 | 20 | #include "fkie_behavior_tree_msgs/BTNodeStatus.h" 21 | #include "fkie_behavior_tree_manager/logging.h" 22 | #include 23 | #include 24 | 25 | namespace BehaviorTreeNodes 26 | { 27 | class RosMsgLogger : public BT::StatusChangeLogger 28 | { 29 | protected: 30 | ros::Publisher pub_bt_status; 31 | fkie_behavior_tree_msgs::BTNodeStatus status_msg; 32 | 33 | public: 34 | RosMsgLogger(const BT::Tree& tree, const std::string topic) : BT::StatusChangeLogger(tree.rootNode()) 35 | { 36 | ros::NodeHandle nh("~"); 37 | pub_bt_status = nh.advertise(topic, 10, false); 38 | } 39 | 40 | void callback(BT::Duration timestamp, const BT::TreeNode& node, BT::NodeStatus prev_status, BT::NodeStatus status) 41 | { 42 | status_msg.header.stamp = ros::Time::now(); 43 | status_msg.node_name = node.name(); 44 | status_msg.node_id = node.registrationName(); 45 | status_msg.node_type = toStr(node.type()); 46 | status_msg.node_status = toStr(status, false); 47 | pub_bt_status.publish(status_msg); 48 | 49 | BT_ROS_DEBUG_STREAM("New callback: Node [" << node.name() << "]: " << toStr(prev_status, true) << " -> " 50 | << toStr(status, true)); 51 | } 52 | 53 | void flush() override 54 | { 55 | } 56 | }; 57 | 58 | } // namespace BehaviorTreeNodes 59 | 60 | #endif // BT_ROS_MSG_LOGGER_H 61 | -------------------------------------------------------------------------------- /fkie_behavior_tree_manager/include/fkie_behavior_tree_manager/decorators/RunOnlyOnce.hpp: -------------------------------------------------------------------------------- 1 | // Copyright 2022 Fraunhofer FKIE - All Rights Reserved 2 | // 3 | // Licensed under the Apache License, Version 2.0 (the "License"); 4 | // you may not use this file except in compliance with the License. 5 | // You may obtain a copy of the License at 6 | // 7 | // http://www.apache.org/licenses/LICENSE-2.0 8 | // 9 | // Unless required by applicable law or agreed to in writing, software 10 | // distributed under the License is distributed on an "AS IS" BASIS, 11 | // WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 12 | // See the License for the specific language governing permissions and 13 | // limitations under the License. 14 | 15 | #ifndef RUN_ONLY_ONCE_H 16 | #define RUN_ONLY_ONCE_H 17 | 18 | #include "behaviortree_cpp_v3/decorator_node.h" 19 | #include 20 | #include 21 | #include 22 | 23 | namespace BehaviorTreeNodes 24 | { 25 | /* 26 | RunOnlyOnce: This decorator ticks its children only once, useful for initializing states/poses. 27 | */ 28 | class RunOnlyOnce : public BT::DecoratorNode 29 | { 30 | public: 31 | std::string name; 32 | bool already_executed = false; 33 | 34 | static BT::PortsList providedPorts() 35 | { 36 | BT::PortsList ports; 37 | return ports; 38 | } 39 | 40 | RunOnlyOnce(const std::string& _name, const BT::NodeConfiguration& config) 41 | : BT::DecoratorNode(_name, config), name("[" + _name + "] ") 42 | { 43 | BT_ROS_INFO_STREAM(name << " initialized"); 44 | } 45 | 46 | inline BT::NodeStatus tick() override 47 | { 48 | setStatus(BT::NodeStatus::RUNNING); 49 | 50 | if (already_executed) 51 | return BT::NodeStatus::SUCCESS; 52 | 53 | BT_ROS_DEBUG_STREAM(name << "Ticking node [" << child_node_->name() << "]"); 54 | const BT::NodeStatus child_state = child_node_->executeTick(); 55 | 56 | if (child_state == BT::NodeStatus::FAILURE) 57 | { 58 | BT_ROS_WARN_STREAM(name << "Node [" << child_node_->name() << "] returns FAILURE, will be ticked again!"); 59 | } 60 | else 61 | { 62 | already_executed = true; 63 | } 64 | 65 | return child_state; 66 | } 67 | }; 68 | 69 | } // namespace BehaviorTreeNodes 70 | 71 | #endif // RUN_ONLY_ONCE_H -------------------------------------------------------------------------------- /fkie_bt_move_base_actions/src/Recovery.h: -------------------------------------------------------------------------------- 1 | // Copyright 2022 Fraunhofer FKIE - All Rights Reserved 2 | // 3 | // Licensed under the Apache License, Version 2.0 (the "License"); 4 | // you may not use this file except in compliance with the License. 5 | // You may obtain a copy of the License at 6 | // 7 | // http://www.apache.org/licenses/LICENSE-2.0 8 | // 9 | // Unless required by applicable law or agreed to in writing, software 10 | // distributed under the License is distributed on an "AS IS" BASIS, 11 | // WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 12 | // See the License for the specific language governing permissions and 13 | // limitations under the License. 14 | 15 | #ifndef MOVE_BASE_RECOVERY_H 16 | #define MOVE_BASE_RECOVERY_H 17 | 18 | #include 19 | 20 | #include 21 | 22 | #include 23 | #include 24 | #include 25 | 26 | #include 27 | #include 28 | 29 | #include "behaviortree_cpp_v3/behavior_tree.h" 30 | #include "behaviortree_cpp_v3/bt_factory.h" 31 | 32 | #include 33 | #include 34 | 35 | typedef actionlib::SimpleActionClient ClientRecoveryAction; 36 | 37 | namespace MoveBaseActionsGroup 38 | { 39 | class Recovery : public BT::ActionNodeBase 40 | { 41 | public: 42 | std::string name; 43 | std::string topic_recovery_action; 44 | bool init = false; 45 | 46 | Recovery(const std::string& name, const BT::NodeConfiguration& config); 47 | ~Recovery(); 48 | 49 | static BT::PortsList providedPorts() 50 | { 51 | static BT::PortsList ports = { BT::InputPort("strategy") }; 52 | return ports; 53 | } 54 | 55 | BT::NodeStatus tick() override; 56 | void halt() override; 57 | 58 | void doneCallback(const actionlib::SimpleClientGoalState& state, const mbf_msgs::RecoveryResultConstPtr& result); 59 | void activeCallback(); 60 | void feedbackCallback(); 61 | bool checkValidState(const int outcome); 62 | 63 | private: 64 | std::atomic_bool exe_result; 65 | std::unique_ptr ac; 66 | }; 67 | 68 | } // namespace MoveBaseActionsGroup 69 | 70 | #endif // MOVE_BASE_RECOVERY_H 71 | -------------------------------------------------------------------------------- /fkie_bt_move_base_actions/package.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | fkie_bt_move_base_actions 4 | 0.0.0 5 | The fkie_bt_move_base_actions package 6 | 7 | Francisco Garcia 8 | Francisco Garcia 9 | 10 | Apache License 2.0 11 | 12 | catkin 13 | 14 | roscpp 15 | rospy 16 | std_msgs 17 | mbf_msgs 18 | actionlib 19 | actionlib_msgss 20 | geometry_msgs 21 | nav_msgs 22 | fkie_behavior_tree_manager 23 | behaviortree_cpp_v3 24 | tf 25 | message_generation 26 | 27 | roscpp 28 | rospy 29 | std_msgs 30 | 31 | mbf_msgs 32 | actionlib 33 | actionlib_msgs 34 | geometry_msgs 35 | nav_msgs 36 | fkie_behavior_tree_manager 37 | tf 38 | 39 | roscpp 40 | rospy 41 | std_msgs 42 | mbf_msgs 43 | actionlib 44 | actionlib_msgs 45 | geometry_msgs 46 | nav_msgs 47 | fkie_behavior_tree_manager 48 | tf 49 | message_runtime 50 | 51 | 52 | 53 | 54 | 55 | 56 | 57 | -------------------------------------------------------------------------------- /fkie_behavior_tree_manager/include/fkie_behavior_tree_manager/conditions/checkCommand.hpp: -------------------------------------------------------------------------------- 1 | // Copyright 2022 Fraunhofer FKIE - All Rights Reserved 2 | // 3 | // Licensed under the Apache License, Version 2.0 (the "License"); 4 | // you may not use this file except in compliance with the License. 5 | // You may obtain a copy of the License at 6 | // 7 | // http://www.apache.org/licenses/LICENSE-2.0 8 | // 9 | // Unless required by applicable law or agreed to in writing, software 10 | // distributed under the License is distributed on an "AS IS" BASIS, 11 | // WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 12 | // See the License for the specific language governing permissions and 13 | // limitations under the License. 14 | 15 | #ifndef CHECK_COMMAND_H 16 | #define CHECK_COMMAND_H 17 | 18 | #include 19 | #include 20 | 21 | #include "behaviortree_cpp_v3/behavior_tree.h" 22 | #include "behaviortree_cpp_v3/bt_factory.h" 23 | 24 | #include 25 | #include 26 | 27 | namespace BehaviorTreeNodes 28 | { 29 | BT::NodeStatus checkCommand(BT::TreeNode& self) 30 | { 31 | std::string cmd; 32 | auto res = self.getInput("cmd", cmd); 33 | if (!res) 34 | { 35 | BT_ROS_ERROR_STREAM(self.name() << " Error at reading [cmd]: " << res.error()); 36 | return BT::NodeStatus::FAILURE; 37 | } 38 | 39 | ros::NodeHandle private_node("~"); 40 | std::string check_string_topic; 41 | float check_string_timeout = 2.0; 42 | private_node.param("checkCommand/check_string_topic", check_string_topic, std::string("")); 43 | private_node.param("checkCommand/check_string_timeout", check_string_timeout, 1.0f); 44 | 45 | if (check_string_topic.empty()) 46 | { 47 | BT_ROS_ERROR_STREAM(self.name() << " check_string_topic is empty"); 48 | return BT::NodeStatus::FAILURE; 49 | } 50 | 51 | // wait [timeout_target_pose] for first message 52 | std_msgs::StringConstPtr cmd_once = 53 | ros::topic::waitForMessage(check_string_topic, ros::Duration(check_string_timeout)); 54 | if (!cmd_once) 55 | { 56 | return BT::NodeStatus::FAILURE; 57 | } 58 | 59 | if (cmd == cmd_once->data) 60 | return BT::NodeStatus::SUCCESS; 61 | else 62 | return BT::NodeStatus::FAILURE; 63 | }; 64 | 65 | } // namespace BehaviorTreeNodes 66 | 67 | #endif // CHECK_COMMAND_H -------------------------------------------------------------------------------- /fkie_bt_visualization/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 3.0.2) 2 | project(fkie_bt_visualization) 3 | 4 | set(CMAKE_CXX_STANDARD 17) 5 | 6 | find_package(catkin REQUIRED COMPONENTS 7 | roscpp 8 | rviz 9 | fkie_behavior_tree_msgs 10 | ) 11 | 12 | catkin_package( 13 | LIBRARIES ${PROJECT_NAME} 14 | CATKIN_DEPENDS roscpp rviz fkie_behavior_tree_msgs 15 | ) 16 | 17 | include_directories(include ${catkin_INCLUDE_DIRS} ${Boost_INCLUDE_DIRS} ${CMAKE_CURRENT_SOURCE_DIR}) 18 | link_directories(${catkin_LIBRARY_DIRS} ${Boost_LIBRARY_DIRS}) 19 | 20 | ## This setting causes Qt's "MOC" generation to happen automatically. 21 | set(CMAKE_AUTOMOC ON) 22 | set(CMAKE_INCLUDE_CURRENT_DIR ON) 23 | 24 | ## This plugin includes Qt widgets, so we must include Qt. 25 | ## We'll use the version that rviz used so they are compatible. 26 | if(rviz_QT_VERSION VERSION_LESS "5") 27 | message(STATUS "Using Qt4 based on the rviz_QT_VERSION: ${rviz_QT_VERSION}") 28 | find_package(Qt4 ${rviz_QT_VERSION} EXACT REQUIRED QtCore QtGui QtXml) 29 | ## pull in all required include dirs, define QT_LIBRARIES, etc. 30 | include(${QT_USE_FILE}) 31 | else() 32 | message(STATUS "Using Qt5 based on the rviz_QT_VERSION: ${rviz_QT_VERSION}") 33 | find_package(Qt5 ${rviz_QT_VERSION} EXACT REQUIRED Core Widgets Xml) 34 | set(QT_LIBRARIES Qt5::Widgets Qt5::Xml) 35 | endif() 36 | 37 | add_definitions(-DQT_NO_KEYWORDS) 38 | 39 | ############# 40 | ## Build ## 41 | ############# 42 | 43 | set(SOURCE_FILES 44 | src/rviz_bt_status.cpp 45 | src/rviz_bt_logger.cpp 46 | include/${PROJECT_NAME}/TreeModel.hpp 47 | src/randomcolor/randomcolor.cpp 48 | ) 49 | 50 | add_library(${PROJECT_NAME} ${SOURCE_FILES}) 51 | target_link_libraries(${PROJECT_NAME} ${QT_LIBRARIES} ${catkin_LIBRARIES}) 52 | target_compile_features(${PROJECT_NAME} PRIVATE cxx_std_17) 53 | 54 | ############# 55 | ## Install ## 56 | ############# 57 | 58 | install(TARGETS 59 | ${PROJECT_NAME} 60 | ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} 61 | LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} 62 | RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} 63 | ) 64 | 65 | install(TARGETS 66 | ${PROJECT_NAME} 67 | ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} 68 | LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} 69 | RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} 70 | ) 71 | 72 | install(FILES 73 | plugin_description.xml 74 | DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}) 75 | -------------------------------------------------------------------------------- /fkie_bt_move_base_actions/src/NavigateToGoal.h: -------------------------------------------------------------------------------- 1 | // Copyright 2022 Fraunhofer FKIE - All Rights Reserved 2 | // 3 | // Licensed under the Apache License, Version 2.0 (the "License"); 4 | // you may not use this file except in compliance with the License. 5 | // You may obtain a copy of the License at 6 | // 7 | // http://www.apache.org/licenses/LICENSE-2.0 8 | // 9 | // Unless required by applicable law or agreed to in writing, software 10 | // distributed under the License is distributed on an "AS IS" BASIS, 11 | // WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 12 | // See the License for the specific language governing permissions and 13 | // limitations under the License. 14 | 15 | #ifndef MOVE_BASE_NAVIGATE_TO_GOAL_H 16 | #define MOVE_BASE_NAVIGATE_TO_GOAL_H 17 | 18 | #include 19 | 20 | #include "behaviortree_cpp_v3/behavior_tree.h" 21 | #include "behaviortree_cpp_v3/bt_factory.h" 22 | 23 | #include 24 | #include 25 | #include 26 | 27 | namespace MoveBaseActionsGroup 28 | { 29 | /* 30 | 31 | Publishes a goal to the bt_navigation, and wait for a [fkie_bt_move_base_actions/NavigationResult] message. 32 | Used to encapsulate the navigation from other applications 33 | It returns [nav_error_code] > 0 if navigation errors occurred 34 | 35 | */ 36 | class NavigateToGoal : public BT::CoroActionNode 37 | { 38 | public: 39 | bool is_initialized = false; 40 | bool new_message_received = false; 41 | std::string topic_goal_pose_out; 42 | std::string topic_notify_result; 43 | 44 | ros::Publisher pub_pose; 45 | 46 | NavigateToGoal(const std::string& name, const BT::NodeConfiguration& config); 47 | ~NavigateToGoal(); 48 | BT::NodeStatus tick() override; 49 | virtual void halt() override; 50 | 51 | static BT::PortsList providedPorts() 52 | { 53 | BT::PortsList ports = { BT::InputPort("target_pose"), BT::OutputPort("nav_error_code"), 54 | BT::OutputPort("navigation_result") }; 55 | return ports; 56 | } 57 | 58 | void callbackNavigationResult(const fkie_bt_move_base_actions::NavigationResult& msg); 59 | 60 | private: 61 | ros::Subscriber sub_notify; 62 | fkie_bt_move_base_actions::NavigationResult last_msg; 63 | }; 64 | } // namespace MoveBaseActionsGroup 65 | #endif // MOVE_BASE_NAVIGATE_TO_GOAL_H 66 | -------------------------------------------------------------------------------- /fkie_bt_move_base_actions/src/CancelExePathSync.h: -------------------------------------------------------------------------------- 1 | // Copyright 2022 Fraunhofer FKIE - All Rights Reserved 2 | // 3 | // Licensed under the Apache License, Version 2.0 (the "License"); 4 | // you may not use this file except in compliance with the License. 5 | // You may obtain a copy of the License at 6 | // 7 | // http://www.apache.org/licenses/LICENSE-2.0 8 | // 9 | // Unless required by applicable law or agreed to in writing, software 10 | // distributed under the License is distributed on an "AS IS" BASIS, 11 | // WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 12 | // See the License for the specific language governing permissions and 13 | // limitations under the License. 14 | 15 | #ifndef MOVE_BASE_CANCEL_EXE_PATH_SYNC_H 16 | #define MOVE_BASE_CANCEL_EXE_PATH_SYNC_H 17 | 18 | #include 19 | #include 20 | #include 21 | 22 | #include "behaviortree_cpp_v3/behavior_tree.h" 23 | #include "behaviortree_cpp_v3/bt_factory.h" 24 | 25 | #include 26 | #include 27 | 28 | #include 29 | #include 30 | #include 31 | 32 | #include 33 | #include 34 | 35 | typedef actionlib::SimpleActionClient ClientExePathSyncAction; 36 | 37 | namespace MoveBaseActionsGroup 38 | { 39 | class CancelExePathSync : public BT::ActionNodeBase 40 | { 41 | public: 42 | std::string topic_exe_path; 43 | bool is_initialized = false; 44 | 45 | CancelExePathSync(const std::string& name, const BT::NodeConfiguration& config); 46 | ~CancelExePathSync(); 47 | 48 | BT::NodeStatus tick() override; 49 | 50 | static BT::PortsList providedPorts() 51 | { 52 | static BT::PortsList ports = { BT::OutputPort("target_pose"), BT::OutputPort("path") }; 53 | 54 | return ports; 55 | } 56 | 57 | void doneCallback(const actionlib::SimpleClientGoalState& state, const mbf_msgs::ExePathResultConstPtr& result); 58 | void activeCallback(); 59 | void feedbackCallback(const mbf_msgs::ExePathFeedbackConstPtr& feedback); 60 | 61 | void cancelGoals(); 62 | 63 | void halt() override; 64 | 65 | private: 66 | std::unique_ptr ac; 67 | }; 68 | 69 | } // namespace MoveBaseActionsGroup 70 | 71 | #endif // MOVE_BASE_CANCEL_EXE_PATH_SYNC_H 72 | -------------------------------------------------------------------------------- /fkie_bt_move_base_actions/src/NavigateToGoalSync.h: -------------------------------------------------------------------------------- 1 | // Copyright 2022 Fraunhofer FKIE - All Rights Reserved 2 | // 3 | // Licensed under the Apache License, Version 2.0 (the "License"); 4 | // you may not use this file except in compliance with the License. 5 | // You may obtain a copy of the License at 6 | // 7 | // http://www.apache.org/licenses/LICENSE-2.0 8 | // 9 | // Unless required by applicable law or agreed to in writing, software 10 | // distributed under the License is distributed on an "AS IS" BASIS, 11 | // WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 12 | // See the License for the specific language governing permissions and 13 | // limitations under the License. 14 | 15 | #ifndef MOVE_BASE_NAVIGATE_TO_GOAL_SYNC_H 16 | #define MOVE_BASE_NAVIGATE_TO_GOAL_SYNC_H 17 | 18 | #include 19 | 20 | #include "behaviortree_cpp_v3/behavior_tree.h" 21 | #include "behaviortree_cpp_v3/bt_factory.h" 22 | 23 | #include 24 | #include 25 | #include 26 | 27 | namespace MoveBaseActionsGroup 28 | { 29 | /* 30 | 31 | Publishes a goal to the bt_navigation, and wait for a [fkie_bt_move_base_actions/NavigationResult] message. 32 | Used to encapsulate the navigation from other applications 33 | It returns [nav_error_code] > 0 if navigation errors occurred 34 | 35 | */ 36 | class NavigateToGoalSync : public BT::ActionNodeBase 37 | { 38 | public: 39 | bool is_initialized = false; 40 | bool new_message_received = false; 41 | std::string topic_goal_pose_out; 42 | std::string topic_notify_result; 43 | 44 | ros::Publisher pub_pose; 45 | 46 | NavigateToGoalSync(const std::string& name, const BT::NodeConfiguration& config); 47 | ~NavigateToGoalSync(); 48 | BT::NodeStatus tick() override; 49 | virtual void halt() override; 50 | 51 | static BT::PortsList providedPorts() 52 | { 53 | BT::PortsList ports = { BT::InputPort("target_pose"), BT::OutputPort("nav_error_code"), 54 | BT::OutputPort("navigation_result") }; 55 | return ports; 56 | } 57 | 58 | void callbackNavigationResult(const fkie_bt_move_base_actions::NavigationResult& msg); 59 | 60 | private: 61 | ros::Subscriber sub_notify; 62 | fkie_bt_move_base_actions::NavigationResult last_msg; 63 | }; 64 | } // namespace MoveBaseActionsGroup 65 | #endif // MOVE_BASE_NAVIGATE_TO_GOAL_SYNC_H 66 | -------------------------------------------------------------------------------- /fkie_behavior_tree_manager/include/fkie_behavior_tree_manager/utils.hpp: -------------------------------------------------------------------------------- 1 | // Copyright 2022 Fraunhofer FKIE - All Rights Reserved 2 | // 3 | // Licensed under the Apache License, Version 2.0 (the "License"); 4 | // you may not use this file except in compliance with the License. 5 | // You may obtain a copy of the License at 6 | // 7 | // http://www.apache.org/licenses/LICENSE-2.0 8 | // 9 | // Unless required by applicable law or agreed to in writing, software 10 | // distributed under the License is distributed on an "AS IS" BASIS, 11 | // WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 12 | // See the License for the specific language governing permissions and 13 | // limitations under the License. 14 | 15 | #ifndef UTILS_H 16 | #define UTILS_H 17 | 18 | #include 19 | #include 20 | #include 21 | #include 22 | 23 | /** 24 | * @brief Extract string content from a given [fileName] 25 | */ 26 | inline std::string readContentFile(const std::string& fileName) 27 | { 28 | std::ifstream ifs(fileName.c_str(), std::ios::in | std::ios::binary | std::ios::ate); 29 | 30 | std::ifstream::pos_type fileSize = ifs.tellg(); 31 | ifs.seekg(0, std::ios::beg); 32 | 33 | std::vector bytes(fileSize); 34 | ifs.read(bytes.data(), fileSize); 35 | 36 | return std::string(bytes.data(), fileSize); 37 | } 38 | 39 | /** 40 | * @brief Read a ROS parameter 41 | */ 42 | template 43 | bool getROSParameter(std::string name, T& param, bool print = true) 44 | { 45 | ros::NodeHandle private_node = ros::NodeHandle("~"); 46 | const T default_value = param; 47 | bool r = private_node.param(name, param, default_value); 48 | 49 | if (print) 50 | { 51 | BT_ROS_INFO_STREAM(name << ": [" << param << "]"); 52 | } 53 | 54 | return r; 55 | } 56 | 57 | /** 58 | * @brief Read ROS parameter (array-like) 59 | */ 60 | template 61 | bool getROSParameterVector(std::string name, T& param, bool print = true) 62 | { 63 | ros::NodeHandle private_node = ros::NodeHandle("~"); 64 | const T default_value = param; 65 | bool r = private_node.param(name, param, default_value); 66 | 67 | if (print) 68 | { 69 | std::string s = 70 | std::accumulate(param.begin(), param.end(), std::string{}, 71 | [](std::string& s, const std::string& piece) -> decltype(auto) { return s += piece + ", "; }); 72 | 73 | BT_ROS_INFO_STREAM(name << ": [" << s << "]"); 74 | } 75 | 76 | return r; 77 | } 78 | 79 | #endif // UTILS_H -------------------------------------------------------------------------------- /fkie_bt_tutorials/include/fkie_bt_tutorials/actions/HelloWorldAction.hpp: -------------------------------------------------------------------------------- 1 | // Copyright 2022 Fraunhofer FKIE - All Rights Reserved 2 | // 3 | // Licensed under the Apache License, Version 2.0 (the "License"); 4 | // you may not use this file except in compliance with the License. 5 | // You may obtain a copy of the License at 6 | // 7 | // http://www.apache.org/licenses/LICENSE-2.0 8 | // 9 | // Unless required by applicable law or agreed to in writing, software 10 | // distributed under the License is distributed on an "AS IS" BASIS, 11 | // WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 12 | // See the License for the specific language governing permissions and 13 | // limitations under the License. 14 | 15 | #ifndef HELLO_WORLD_ACTION_H 16 | #define HELLO_WORLD_ACTION_H 17 | 18 | #include 19 | #include 20 | #include 21 | #include 22 | #include 23 | 24 | namespace BTTutorialGroup 25 | { 26 | class HelloWorldAction : public BT::SyncActionNode 27 | { 28 | public: 29 | static BT::PortsList providedPorts() 30 | { 31 | BT::PortsList ports = { BT::InputPort("message_in"), BT::OutputPort("message_out") }; 32 | 33 | return ports; 34 | } 35 | 36 | HelloWorldAction(const std::string& name, const BT::NodeConfiguration& config) : BT::SyncActionNode(name, config) 37 | { 38 | } 39 | 40 | ~HelloWorldAction() = default; 41 | 42 | BT::NodeStatus tick() override 43 | { 44 | setStatus(BT::NodeStatus::RUNNING); 45 | 46 | std::string message_in; 47 | auto res = BT::TreeNode::getInput("message_in", message_in); 48 | if (!res) 49 | { 50 | // if no message available, greet the world again :) 51 | message_in = "Hello World"; 52 | 53 | // optionally, you can print error message and stop execution by returning [BT::NodeStatus::FAILURE] 54 | // BT_ROS_ERROR_STREAM("Could not load port [message_in]: " << res.error()); 55 | // return BT::NodeStatus::FAILURE; 56 | } 57 | 58 | // print the message to console log and wait 2 secs. 59 | BT_ROS_INFO_STREAM("message_in: " << message_in); 60 | 61 | // Write something to the [OutputPort] called [message_out]: 62 | BT::TreeNode::setOutput("message_out", std::string("BT Tutorials: HelloWorldAction")); 63 | 64 | ros::Duration(2.0).sleep(); 65 | 66 | return BT::NodeStatus::SUCCESS; 67 | } 68 | }; 69 | } // namespace BTTutorialGroup 70 | #endif // HELLO_WORLD_ACTION_H -------------------------------------------------------------------------------- /fkie_bt_move_base_actions/src/CancelExePathSync.cpp: -------------------------------------------------------------------------------- 1 | // Copyright 2022 Fraunhofer FKIE - All Rights Reserved 2 | // 3 | // Licensed under the Apache License, Version 2.0 (the "License"); 4 | // you may not use this file except in compliance with the License. 5 | // You may obtain a copy of the License at 6 | // 7 | // http://www.apache.org/licenses/LICENSE-2.0 8 | // 9 | // Unless required by applicable law or agreed to in writing, software 10 | // distributed under the License is distributed on an "AS IS" BASIS, 11 | // WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 12 | // See the License for the specific language governing permissions and 13 | // limitations under the License. 14 | 15 | #include "CancelExePathSync.h" 16 | 17 | namespace MoveBaseActionsGroup 18 | { 19 | CancelExePathSync::CancelExePathSync(const std::string& name, const BT::NodeConfiguration& config) 20 | : BT::ActionNodeBase(name, config) 21 | { 22 | // Get ros parameters 23 | ros::NodeHandle private_node("~"); 24 | private_node.param("MoveBaseActions/topic_exe_path", topic_exe_path, std::string("exe_path")); 25 | private_node.param("MoveBaseActions/topic_exe_path", topic_exe_path, std::string("")); 26 | 27 | if (topic_exe_path.empty()) 28 | { 29 | BT_ROS_ERROR("Missing param [MoveBaseActions/topic_exe_path]"); 30 | return; 31 | } 32 | 33 | bool spin_thread = false; 34 | ac = std::make_unique(topic_exe_path, spin_thread); 35 | BT_ROS_INFO_STREAM("Waiting for action server [" << topic_exe_path << "]"); 36 | 37 | while (ros::ok() && !ac->waitForServer(ros::Duration(0.1))) 38 | { 39 | ros::spinOnce(); 40 | } 41 | BT_ROS_INFO_STREAM("Action client started"); 42 | 43 | is_initialized = true; 44 | BT_ROS_INFO_STREAM("Successfully initialized"); 45 | } 46 | 47 | CancelExePathSync::~CancelExePathSync() 48 | { 49 | cancelGoals(); 50 | } 51 | 52 | BT::NodeStatus CancelExePathSync::tick() 53 | { 54 | setStatus(BT::NodeStatus::RUNNING); 55 | 56 | cancelGoals(); 57 | 58 | return BT::NodeStatus::SUCCESS; 59 | } 60 | 61 | void CancelExePathSync::halt() 62 | { 63 | cancelGoals(); 64 | 65 | setStatus(BT::NodeStatus::FAILURE); 66 | } 67 | 68 | void CancelExePathSync::cancelGoals() 69 | { 70 | if (ac->isServerConnected()) 71 | { 72 | ac->cancelAllGoals(); 73 | } 74 | 75 | BT::TreeNode::setOutput("path", nav_msgs::Path()); // reset current path 76 | BT::TreeNode::setOutput("target_pose", Pose2D()); // reset current target_pose 77 | BT_ROS_DEBUG_STREAM("CancelExePathSync: all goals were cancelled"); 78 | } 79 | 80 | void CancelExePathSync::activeCallback() 81 | { 82 | } 83 | 84 | void CancelExePathSync::feedbackCallback(const mbf_msgs::ExePathFeedbackConstPtr& feedback) 85 | { 86 | } 87 | 88 | } // namespace MoveBaseActionsGroup 89 | -------------------------------------------------------------------------------- /fkie_bt_move_base_actions/src/GetPath.h: -------------------------------------------------------------------------------- 1 | // Copyright 2022 Fraunhofer FKIE - All Rights Reserved 2 | // 3 | // Licensed under the Apache License, Version 2.0 (the "License"); 4 | // you may not use this file except in compliance with the License. 5 | // You may obtain a copy of the License at 6 | // 7 | // http://www.apache.org/licenses/LICENSE-2.0 8 | // 9 | // Unless required by applicable law or agreed to in writing, software 10 | // distributed under the License is distributed on an "AS IS" BASIS, 11 | // WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 12 | // See the License for the specific language governing permissions and 13 | // limitations under the License. 14 | 15 | #ifndef MOVE_BASE_GET_PATH_H 16 | #define MOVE_BASE_GET_PATH_H 17 | 18 | #include 19 | #include 20 | 21 | #include 22 | #include 23 | #include 24 | 25 | #include 26 | #include 27 | 28 | #include "behaviortree_cpp_v3/behavior_tree.h" 29 | #include "behaviortree_cpp_v3/bt_factory.h" 30 | 31 | #include 32 | #include 33 | 34 | typedef actionlib::SimpleActionClient ClientGetPathAction; 35 | typedef actionlib::SimpleActionClient ClientExePathAction; 36 | 37 | namespace MoveBaseActionsGroup 38 | { 39 | class GetPath : public BT::SyncActionNode 40 | { 41 | public: 42 | bool is_initialized = false; 43 | std::string topic_get_path, frame_id_get_path; 44 | std::string topic_exe_path; 45 | std::string frame_id_target_pose; 46 | 47 | tf::TransformListener mytf; 48 | 49 | GetPath(const std::string& name, const BT::NodeConfiguration& config); 50 | ~GetPath(); 51 | BT::NodeStatus tick() override; 52 | 53 | static BT::PortsList providedPorts() 54 | { 55 | BT::PortsList ports = { BT::InputPort("target_pose"), BT::InputPort("planner"), 56 | BT::OutputPort("path"), BT::OutputPort("nav_error_code") }; 57 | return ports; 58 | } 59 | 60 | void doneCallback(const actionlib::SimpleClientGoalState& state, const mbf_msgs::GetPathResultConstPtr& result); 61 | void activeCallback(); 62 | void feedbackCallback(const mbf_msgs::GetPathFeedbackConstPtr& feedback); 63 | bool getTfTransform(const std::string frame_to, const std::string frame_from, tf::StampedTransform& transform); 64 | 65 | private: 66 | std::atomic_bool exe_result; 67 | std::unique_ptr ac; 68 | std::unique_ptr ac_exec; 69 | }; 70 | } // namespace MoveBaseActionsGroup 71 | #endif // MOVE_BASE_GET_PATH_H 72 | -------------------------------------------------------------------------------- /fkie_behavior_tree_manager/include/fkie_behavior_tree_manager/conditions/CheckTimeoutAsync.hpp: -------------------------------------------------------------------------------- 1 | // Copyright 2022 Fraunhofer FKIE - All Rights Reserved 2 | // 3 | // Licensed under the Apache License, Version 2.0 (the "License"); 4 | // you may not use this file except in compliance with the License. 5 | // You may obtain a copy of the License at 6 | // 7 | // http://www.apache.org/licenses/LICENSE-2.0 8 | // 9 | // Unless required by applicable law or agreed to in writing, software 10 | // distributed under the License is distributed on an "AS IS" BASIS, 11 | // WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 12 | // See the License for the specific language governing permissions and 13 | // limitations under the License. 14 | 15 | #ifndef CHECK_TIMEOUT_NODE_H 16 | #define CHECK_TIMEOUT_NODE_H 17 | 18 | #include 19 | #include 20 | #include 21 | #include 22 | #include 23 | 24 | namespace BehaviorTreeNodes 25 | { 26 | class CheckTimeoutAsync : public BT::CoroActionNode 27 | { 28 | public: 29 | std::string name; 30 | 31 | double timeout = 0.0; 32 | 33 | ros::Time start; 34 | 35 | static BT::PortsList providedPorts() 36 | { 37 | BT::PortsList ports = { BT::InputPort("timeout") }; 38 | return ports; 39 | } 40 | 41 | CheckTimeoutAsync(const std::string& _name, const BT::NodeConfiguration& config) 42 | : BT::CoroActionNode(_name, config), name("[" + _name + "] ") 43 | { 44 | BT_ROS_INFO_STREAM(name << " initialized"); 45 | } 46 | 47 | inline BT::NodeStatus tick() override 48 | { 49 | BT::TreeNode::getInput("timeout", timeout); 50 | 51 | if (timeout <= 0) 52 | { 53 | // disable timeout, always running... 54 | return BT::NodeStatus::RUNNING; 55 | } 56 | 57 | // start new timer 58 | if (status() != BT::NodeStatus::RUNNING) 59 | { 60 | start = ros::Time::now(); 61 | BT_ROS_INFO_STREAM(name << " starting timer, timeout configured to: " << timeout << " s"); 62 | return BT::NodeStatus::RUNNING; 63 | } 64 | 65 | // Determine how long its been since we've started this iteration 66 | ros::Duration elapsed = ros::Time::now() - start; 67 | 68 | if (elapsed.toSec() < timeout) 69 | { 70 | // timer running, not timeout yet 71 | return BT::NodeStatus::RUNNING; 72 | } 73 | 74 | // timeout reached 75 | BT_ROS_INFO_STREAM(name << " timeout reached: " << elapsed.toSec()); 76 | return BT::NodeStatus::SUCCESS; 77 | } 78 | 79 | void halt() override 80 | { 81 | BT_ROS_DEBUG_STREAM(name << "node has been halted!"); 82 | setStatus(BT::NodeStatus::FAILURE); 83 | CoroActionNode::halt(); 84 | } 85 | }; 86 | 87 | } // namespace BehaviorTreeNodes 88 | 89 | #endif // CHECK_TIMEOUT_NODE_H -------------------------------------------------------------------------------- /fkie_bt_moveit_actions/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 3.0.2) 2 | project(fkie_bt_moveit_actions) 3 | 4 | find_package(catkin REQUIRED COMPONENTS 5 | roscpp 6 | rospy 7 | std_msgs 8 | actionlib 9 | actionlib_msgs 10 | fkie_behavior_tree_manager 11 | moveit_core 12 | moveit_visual_tools 13 | moveit_ros_planning 14 | moveit_ros_planning_interface 15 | moveit_ros_perception 16 | eigen_conversions 17 | tf_conversions 18 | pluginlib 19 | geometric_shapes 20 | tf 21 | tf2_ros 22 | tf2_eigen 23 | tf2_geometry_msgs 24 | ) 25 | 26 | # Look for system package 27 | find_package(BehaviorTreeV3) 28 | if(NOT TARGET BT::behaviortree_cpp_v3) 29 | # If not found, look for package in ROS workspace 30 | find_package(behaviortree_cpp_v3 REQUIRED) 31 | #catkin_lint: ignore_once order_violation 32 | add_library(BT::behaviortree_cpp_v3 INTERFACE IMPORTED) 33 | set_target_properties(BT::behaviortree_cpp_v3 PROPERTIES 34 | INTERFACE_INCLUDE_DIRECTORIES "${behaviortree_cpp_v3_INCLUDE_DIRS}" 35 | INTERFACE_LINK_LIBRARIES "${behaviortree_cpp_v3_LIBRARIES}" 36 | ) 37 | endif() 38 | 39 | find_package(Eigen3 REQUIRED) 40 | find_package(Boost REQUIRED system filesystem date_time thread) 41 | 42 | catkin_package( 43 | CATKIN_DEPENDS 44 | roscpp rospy std_msgs actionlib_msgs actionlib 45 | moveit_core 46 | moveit_visual_tools 47 | moveit_ros_planning_interface 48 | tf 49 | tf2_geometry_msgs 50 | fkie_behavior_tree_manager 51 | DEPENDS 52 | EIGEN3 53 | ) 54 | 55 | include_directories( 56 | include 57 | ${catkin_INCLUDE_DIRS} 58 | ${Boost_INCLUDE_DIRS} 59 | ${CMAKE_CURRENT_SOURCE_DIR} 60 | ${Eigen_INCLUDE_DIRS} 61 | ) 62 | 63 | ########### 64 | ## Build ## 65 | ########### 66 | 67 | # static 68 | add_library(MoveitActions STATIC src/MoveitActions.cpp ) 69 | target_link_libraries(MoveitActions PRIVATE ${catkin_LIBRARIES} ${Eigen_LIBRARIES} BT::behaviortree_cpp_v3) 70 | set_target_properties(MoveitActions PROPERTIES ARCHIVE_OUTPUT_DIRECTORY ${CMAKE_LIBRARY_OUTPUT_DIRECTORY} ) 71 | target_compile_features(MoveitActions PRIVATE cxx_std_17) 72 | 73 | # plugin 74 | add_library(MoveitActions_dyn SHARED src/MoveitActions.cpp ) 75 | target_link_libraries(MoveitActions_dyn PRIVATE ${catkin_LIBRARIES} ${Eigen_LIBRARIES} BT::behaviortree_cpp_v3) 76 | target_compile_definitions(MoveitActions_dyn PRIVATE BT_PLUGIN_EXPORT) 77 | set_target_properties(MoveitActions_dyn PROPERTIES LIBRARY_OUTPUT_DIRECTORY ${CMAKE_LIBRARY_OUTPUT_DIRECTORY} ) 78 | target_compile_features(MoveitActions_dyn PRIVATE cxx_std_17) 79 | 80 | ############# 81 | ## Install ## 82 | ############# 83 | 84 | install( 85 | TARGETS MoveitActions MoveitActions_dyn 86 | ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} 87 | LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} 88 | RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} 89 | ) 90 | -------------------------------------------------------------------------------- /fkie_bt_move_base_actions/src/MoveBaseActions.h: -------------------------------------------------------------------------------- 1 | // Copyright 2022 Fraunhofer FKIE - All Rights Reserved 2 | // 3 | // Licensed under the Apache License, Version 2.0 (the "License"); 4 | // you may not use this file except in compliance with the License. 5 | // You may obtain a copy of the License at 6 | // 7 | // http://www.apache.org/licenses/LICENSE-2.0 8 | // 9 | // Unless required by applicable law or agreed to in writing, software 10 | // distributed under the License is distributed on an "AS IS" BASIS, 11 | // WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 12 | // See the License for the specific language governing permissions and 13 | // limitations under the License. 14 | 15 | #ifndef MOVE_BASE_ACTIONS_H 16 | #define MOVE_BASE_ACTIONS_H 17 | 18 | #include "behaviortree_cpp_v3/behavior_tree.h" 19 | #include "behaviortree_cpp_v3/bt_factory.h" 20 | #include 21 | 22 | #include 23 | #include 24 | 25 | #include "CancelExePathSync.h" 26 | #include "ExePathAsync.h" 27 | #include "ExePathSync.h" 28 | #include "GetPath.h" 29 | #include "MoveBaseUtils.h" 30 | #include "NavigateToGoal.h" 31 | #include "NavigateToGoalSync.h" 32 | #include "NotifyNavigationResult.h" 33 | #include "Recovery.h" 34 | 35 | namespace MoveBaseActionsGroup 36 | { 37 | inline void RegisterNodes(BT::BehaviorTreeFactory& factory) 38 | { 39 | BT::PortsList newTargetPoseReceivedPorts = { BT::OutputPort("target_pose") }; 40 | factory.registerSimpleCondition("MoveBase_newTargetPoseReceived", newTargetPoseReceived, newTargetPoseReceivedPorts); 41 | 42 | BT::PortsList hasValidGoalPorts = { BT::BidirectionalPort("target_pose") }; 43 | factory.registerSimpleCondition("MoveBase_hasValidGoal", hasValidGoal, hasValidGoalPorts); 44 | 45 | BT::PortsList CancelCurrentPoseActionPorts = { BT::OutputPort("target_pose") }; 46 | factory.registerSimpleAction("CancelCurrentPoseAction", CancelCurrentPoseAction, CancelCurrentPoseActionPorts); 47 | 48 | // BT::PortsList moveBaseHeartbeatPorts = {BT::InputPort("message")}; 49 | // factory.registerSimpleAction("MoveBase_heartbeat", moveBaseHeartbeat, moveBaseHeartbeatPorts); 50 | 51 | factory.registerNodeType("MoveBase_ExePathSync"); 52 | factory.registerNodeType("MoveBase_ExePathAsync"); 53 | factory.registerNodeType("CancelExePathSync"); 54 | factory.registerNodeType("MoveBase_GetPath"); 55 | factory.registerNodeType("MoveBase_Recovery"); 56 | factory.registerNodeType("MoveBase_NotifyNavigationResult"); 57 | factory.registerNodeType("MoveBase_NavigateToGoal"); 58 | factory.registerNodeType("MoveBase_NavigateToGoalSync"); 59 | } 60 | 61 | } // namespace MoveBaseActionsGroup 62 | 63 | #endif // MOVE_BASE_ACTIONS_H -------------------------------------------------------------------------------- /fkie_behavior_tree_manager/include/fkie_behavior_tree_manager/actions/PoseStampedToPose2D.hpp: -------------------------------------------------------------------------------- 1 | // Copyright 2022 Fraunhofer FKIE - All Rights Reserved 2 | // 3 | // Licensed under the Apache License, Version 2.0 (the "License"); 4 | // you may not use this file except in compliance with the License. 5 | // You may obtain a copy of the License at 6 | // 7 | // http://www.apache.org/licenses/LICENSE-2.0 8 | // 9 | // Unless required by applicable law or agreed to in writing, software 10 | // distributed under the License is distributed on an "AS IS" BASIS, 11 | // WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 12 | // See the License for the specific language governing permissions and 13 | // limitations under the License. 14 | 15 | #ifndef POSE_3D_TO_POSE_2D_NODE_H 16 | #define POSE_3D_TO_POSE_2D_NODE_H 17 | 18 | #include 19 | #include 20 | #include 21 | #include 22 | #include 23 | 24 | namespace BehaviorTreeNodes 25 | { 26 | class PoseStampedToPose2D : public BT::SyncActionNode 27 | { 28 | public: 29 | std::string name; 30 | 31 | static BT::PortsList providedPorts() 32 | { 33 | BT::PortsList ports = { BT::InputPort("target_pose_3d"), BT::OutputPort("target" 34 | "_pose_" 35 | "2d") }; 36 | return ports; 37 | } 38 | 39 | PoseStampedToPose2D(const std::string& _name, const BT::NodeConfiguration& config) 40 | : BT::SyncActionNode(_name, config), name("[" + _name + "] ") 41 | { 42 | BT_ROS_INFO_STREAM(name << " initialized"); 43 | } 44 | 45 | inline BT::NodeStatus tick() override 46 | { 47 | setStatus(BT::NodeStatus::RUNNING); 48 | 49 | geometry_msgs::PoseStamped target_pose_3d; 50 | auto res = BT::TreeNode::getInput("target_pose_3d", target_pose_3d); 51 | if (!res) 52 | { 53 | BT_ROS_ERROR_STREAM(name << " Could not load target_pose_3d, resetting variable. Error: " << res.error()); 54 | BT::TreeNode::setOutput("target_pose_3d", geometry_msgs::PoseStamped()); 55 | return BT::NodeStatus::FAILURE; 56 | } 57 | 58 | Pose2D target_pose_2d(target_pose_3d); 59 | BT_ROS_DEBUG_STREAM(name << " target_pose_2d: " << target_pose_2d.toString()); 60 | 61 | // save current pose to blackboard variable 62 | res = BT::TreeNode::setOutput("target_pose_2d", target_pose_2d); 63 | if (!res) 64 | { 65 | BT_ROS_ERROR_STREAM(name << " Could not set output target_pose_2d: " << res.error()); 66 | return BT::NodeStatus::FAILURE; 67 | } 68 | else 69 | { 70 | return BT::NodeStatus::SUCCESS; 71 | } 72 | } 73 | }; 74 | 75 | } // namespace BehaviorTreeNodes 76 | #endif // POSE_3D_TO_POSE_2D_NODE_H -------------------------------------------------------------------------------- /README.md: -------------------------------------------------------------------------------- 1 | # FKIE Behavior Trees 2 | 3 | This repository contains packages for executing Behavior Trees in ROS. It is powered by [BehaviorTree.CPP](https://github.com/BehaviorTree/BehaviorTree.CPP) and supports navigation and manipulation tasks using [Move Base Flex](http://wiki.ros.org/move_base_flex) and [MoveIt](https://moveit.ros.org/). It also provides RVIZ visualization and debugging tools. 4 | 5 | ![packbot_arm_nav](https://user-images.githubusercontent.com/748097/169791484-5d4ef6a2-efcc-4a56-afaa-941c340163cb.gif) 6 | 7 | ## Available Packages 8 | 9 | - ```fkie_behavior_tree_manager``` interfaces with behavior tree engine 10 | - ```fkie_bt_move_base_actions``` contains actions for interfacing with [Move Base Flex](http://wiki.ros.org/move_base_flex) 11 | - ```fkie_bt_moveit_actions``` contains actions for interfacing with [MoveIt](https://moveit.ros.org/) 12 | - ```fkie_bt_visualization``` contains RVIZ plugins for BT visualization 13 | - ```fkie_bt_tutorials``` contains tutorials for creating new BT actions 14 | 15 | ## Documentation 16 | 17 | Check our [Wiki](https://github.com/fkie/fkie_behavior_trees/wiki) for tutorials and examples. 18 | 19 | ## How to Compile 20 | 21 | - Install dependencies: 22 | 23 | ``` 24 | sudo apt install ros-noetic-behaviortree-cpp-v3 ros-noetic-mbf-msgs qt5-default ros-noetic-moveit-ros ros-noetic-moveit-visual-tools 25 | ``` 26 | 27 | If you want to use all packages, you can compile the meta-package `fkie_behavior_trees`, using [catkin tools](https://catkin-tools.readthedocs.io/en/latest/installing.html) 28 | 29 | ``` 30 | cd ros/src 31 | git clone https://github.com/fkie/fkie_behavior_trees 32 | cd fkie_behavior_trees/fkie_behavior_trees/ 33 | catkin build --this 34 | ``` 35 | 36 | ## Try it out 37 | 38 | - If you want to run a very simple example of Behavior Tree Manager: 39 | 40 | ``` 41 | roslaunch fkie_behavior_tree_manager behavior_tree.launch 42 | ``` 43 | - You can also try a demo using a simulated husky robot and navigation, available in [fkie_husky_manipulation_simulation](https://github.com/fkie/fkie_husky_manipulation_simulation) 44 | 45 | ![navigation_demo](https://user-images.githubusercontent.com/748097/169975809-2f0e1f83-6910-488f-a3f4-27b0a1148389.gif) 46 | 47 | ## Authors 48 | 49 | - Francisco J. Garcia R. [E-Mail](francisco.garcia.rosas@fkie.fraunhofer.de) 50 | - Menaka Naazare - [E-Mail](menaka.naazare@fkie.fraunhofer.de) 51 | 52 | ## License 53 | 54 | ``` 55 | Copyright 2022 Fraunhofer FKIE 56 | 57 | Licensed under the Apache License, Version 2.0 (the "License"); 58 | you may not use this file except in compliance with the License. 59 | You may obtain a copy of the License at 60 | 61 | http://www.apache.org/licenses/LICENSE-2.0 62 | 63 | Unless required by applicable law or agreed to in writing, software 64 | distributed under the License is distributed on an "AS IS" BASIS, 65 | WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 66 | See the License for the specific language governing permissions and 67 | limitations under the License. 68 | ``` 69 | -------------------------------------------------------------------------------- /fkie_bt_move_base_actions/src/ExePathAsync.h: -------------------------------------------------------------------------------- 1 | // Copyright 2022 Fraunhofer FKIE - All Rights Reserved 2 | // 3 | // Licensed under the Apache License, Version 2.0 (the "License"); 4 | // you may not use this file except in compliance with the License. 5 | // You may obtain a copy of the License at 6 | // 7 | // http://www.apache.org/licenses/LICENSE-2.0 8 | // 9 | // Unless required by applicable law or agreed to in writing, software 10 | // distributed under the License is distributed on an "AS IS" BASIS, 11 | // WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 12 | // See the License for the specific language governing permissions and 13 | // limitations under the License. 14 | 15 | #ifndef MOVE_BASE_EXE_PATH_ASYNC_H 16 | #define MOVE_BASE_EXE_PATH_ASYNC_H 17 | 18 | #include 19 | #include 20 | #include 21 | 22 | #include "behaviortree_cpp_v3/behavior_tree.h" 23 | #include "behaviortree_cpp_v3/bt_factory.h" 24 | 25 | #include 26 | #include 27 | 28 | #include 29 | #include 30 | #include 31 | 32 | #include 33 | #include 34 | 35 | typedef actionlib::SimpleActionClient ClientExePathAsyncAction; 36 | 37 | namespace MoveBaseActionsGroup 38 | { 39 | class ExePathAsync : public BT::CoroActionNode 40 | { 41 | public: 42 | std::string topic_exe_path; 43 | ros::Subscriber sub_pose, sub_result; 44 | std::string topic_pose_reset, frame_id_target_pose; 45 | 46 | bool clear_pose_and_path_after_execution = true; 47 | bool cancel_goals_on_stop = true; 48 | 49 | bool is_initialized = false; 50 | 51 | ExePathAsync(const std::string& name, const BT::NodeConfiguration& config); 52 | ~ExePathAsync(); 53 | 54 | BT::NodeStatus tick() override; 55 | 56 | static BT::PortsList providedPorts() 57 | { 58 | static BT::PortsList ports = { BT::BidirectionalPort("target_pose"), 59 | BT::BidirectionalPort("path"), 60 | BT::InputPort("controller"), 61 | BT::OutputPort("nav_error_code") }; 62 | 63 | return ports; 64 | } 65 | 66 | bool sendNewGoal(); 67 | void doneCallback(const actionlib::SimpleClientGoalState& state, const mbf_msgs::ExePathResultConstPtr& result); 68 | void activeCallback(); 69 | void feedbackCallback(const mbf_msgs::ExePathFeedbackConstPtr& feedback); 70 | void callbackPose(const geometry_msgs::PoseStamped& msg); 71 | bool checkValidState(const int outcome); 72 | 73 | void halt() override; 74 | 75 | private: 76 | std::atomic_bool exe_result, navigation_finished, must_restart, tracking_goal; 77 | std::unique_ptr ac; 78 | }; 79 | 80 | } // namespace MoveBaseActionsGroup 81 | 82 | #endif // MOVE_BASE_EXE_PATH_ASYNC_H 83 | -------------------------------------------------------------------------------- /fkie_behavior_tree_manager/include/fkie_behavior_tree_manager/actions/ServiceEmptyAction.hpp: -------------------------------------------------------------------------------- 1 | // Copyright 2022 Fraunhofer FKIE - All Rights Reserved 2 | // 3 | // Licensed under the Apache License, Version 2.0 (the "License"); 4 | // you may not use this file except in compliance with the License. 5 | // You may obtain a copy of the License at 6 | // 7 | // http://www.apache.org/licenses/LICENSE-2.0 8 | // 9 | // Unless required by applicable law or agreed to in writing, software 10 | // distributed under the License is distributed on an "AS IS" BASIS, 11 | // WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 12 | // See the License for the specific language governing permissions and 13 | // limitations under the License. 14 | 15 | #ifndef SERVICE_EMPTY_NODE_H 16 | #define SERVICE_EMPTY_NODE_H 17 | 18 | #include 19 | #include 20 | #include 21 | #include 22 | 23 | namespace BehaviorTreeNodes 24 | { 25 | class ServiceEmptyAction : public BT::SyncActionNode 26 | { 27 | public: 28 | std::string name; 29 | ros::NodeHandle public_node; 30 | std::string topic_service; 31 | double delay = 0.0; 32 | bool valid_init = false; 33 | 34 | static BT::PortsList providedPorts() 35 | { 36 | BT::PortsList ports; 37 | return ports; 38 | } 39 | 40 | ServiceEmptyAction(const std::string& _name, const BT::NodeConfiguration& config) 41 | : BT::SyncActionNode(_name, config), name("[" + _name + "] ") 42 | { 43 | ros::NodeHandle private_node("~"); 44 | 45 | std::string parameter_topic_service = "ServiceEmptyAction/" + _name + "_topic_service"; 46 | std::string parameter_delay = "ServiceEmptyAction/" + _name + "_delay"; 47 | 48 | private_node.param(parameter_topic_service, topic_service, std::string("")); 49 | private_node.param(parameter_delay, delay, 0.0); 50 | 51 | if (topic_service.empty()) 52 | { 53 | BT_ROS_ERROR_STREAM(name << " topic_service is empty. Check parameter [" << parameter_topic_service << "]"); 54 | return; 55 | } 56 | 57 | valid_init = true; 58 | 59 | BT_ROS_INFO_STREAM(name << " initialized"); 60 | } 61 | 62 | inline BT::NodeStatus tick() override 63 | { 64 | setStatus(BT::NodeStatus::RUNNING); 65 | 66 | if (!valid_init) 67 | return BT::NodeStatus::FAILURE; 68 | 69 | BT_ROS_DEBUG_STREAM(name << " Calling service: " << topic_service); 70 | ros::ServiceClient client = public_node.serviceClient(topic_service); 71 | std_srvs::Empty srv; 72 | if (client.call(srv)) 73 | { 74 | BT_ROS_DEBUG_STREAM(name << " Service executed successfully"); 75 | ros::Duration(delay).sleep(); 76 | return BT::NodeStatus::SUCCESS; 77 | } 78 | else 79 | { 80 | BT_ROS_ERROR_STREAM(name << " Service call to Octomap failed. Is Octomap server running?"); 81 | return BT::NodeStatus::FAILURE; 82 | } 83 | } 84 | }; 85 | 86 | } // namespace BehaviorTreeNodes 87 | 88 | #endif // SERVICE_EMPTY_NODE_H 89 | -------------------------------------------------------------------------------- /fkie_bt_visualization/src/rviz_bt_logger.h: -------------------------------------------------------------------------------- 1 | // Copyright 2022 Fraunhofer FKIE - All Rights Reserved 2 | // 3 | // Licensed under the Apache License, Version 2.0 (the "License"); 4 | // you may not use this file except in compliance with the License. 5 | // You may obtain a copy of the License at 6 | // 7 | // http://www.apache.org/licenses/LICENSE-2.0 8 | // 9 | // Unless required by applicable law or agreed to in writing, software 10 | // distributed under the License is distributed on an "AS IS" BASIS, 11 | // WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 12 | // See the License for the specific language governing permissions and 13 | // limitations under the License. 14 | 15 | #ifndef BT_LOGGER_PANEL_H 16 | #define BT_LOGGER_PANEL_H 17 | 18 | #include "boost/date_time/posix_time/posix_time.hpp" 19 | #include 20 | #include 21 | #include 22 | #include 23 | #include 24 | #include 25 | #include 26 | #include 27 | #include 28 | #include 29 | #include 30 | #include 31 | #include 32 | #include 33 | #include 34 | #include 35 | #include 36 | #include 37 | #include 38 | #include 39 | 40 | #ifndef Q_MOC_RUN 41 | #include "fkie_bt_visualization/TreeModel.hpp" 42 | #include 43 | #include 44 | #endif 45 | 46 | #include "randomcolor/randomcolor.h" 47 | #include 48 | 49 | class QLineEdit; 50 | 51 | namespace fkie_bt_visualization 52 | { 53 | class BTLoggerPanel : public rviz::Panel 54 | { 55 | Q_OBJECT 56 | 57 | public: 58 | BTLoggerPanel(QWidget* parent = 0); 59 | 60 | void load(const rviz::Config& config) override; 61 | void save(rviz::Config config) const override; 62 | 63 | public Q_SLOTS: 64 | 65 | void updateTopicName(); 66 | void setTopicName(const QString& bt_name); 67 | 68 | void clearButton(); 69 | void updateScrollToEnd(bool state); 70 | 71 | protected Q_SLOTS: 72 | 73 | // ROS Callbacks 74 | void callbackBTlogging(const fkie_behavior_tree_msgs::BTLoggingConstPtr& msg); 75 | void rosSpin(); 76 | 77 | protected: 78 | ros::Subscriber sub_bt_log; 79 | QString bt_current_logging_topic_; 80 | 81 | // GUI Elements 82 | QTextEdit* q_text; 83 | QLineEdit* output_bt_topic_editor_; 84 | QString bt_topic_logging; 85 | 86 | QCheckBox* q_check_info; 87 | QCheckBox* q_check_warn; 88 | QCheckBox* q_check_error; 89 | QCheckBox* q_check_fatal; 90 | QCheckBox* q_check_scroll_to_end; 91 | bool scroll_to_end = true; 92 | 93 | QPushButton* q_button_clear; 94 | 95 | // ROS Elements 96 | ros::Subscriber sub_bt_status; 97 | ros::Subscriber sub_bt_node_status; 98 | 99 | ros::NodeHandle nh_; 100 | 101 | std::map map_color_sources; 102 | std::string generateRandomColor(); 103 | int counter = 0; 104 | }; 105 | } // namespace fkie_bt_visualization 106 | 107 | #endif // BT_LOGGER_PANEL_H 108 | -------------------------------------------------------------------------------- /fkie_bt_move_base_actions/src/ExePathSync.h: -------------------------------------------------------------------------------- 1 | // Copyright 2022 Fraunhofer FKIE - All Rights Reserved 2 | // 3 | // Licensed under the Apache License, Version 2.0 (the "License"); 4 | // you may not use this file except in compliance with the License. 5 | // You may obtain a copy of the License at 6 | // 7 | // http://www.apache.org/licenses/LICENSE-2.0 8 | // 9 | // Unless required by applicable law or agreed to in writing, software 10 | // distributed under the License is distributed on an "AS IS" BASIS, 11 | // WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 12 | // See the License for the specific language governing permissions and 13 | // limitations under the License. 14 | 15 | #ifndef MOVE_BASE_EXE_PATH_SYNC_H 16 | #define MOVE_BASE_EXE_PATH_SYNC_H 17 | 18 | #include 19 | #include 20 | #include 21 | 22 | #include "behaviortree_cpp_v3/behavior_tree.h" 23 | #include "behaviortree_cpp_v3/bt_factory.h" 24 | 25 | #include 26 | #include 27 | 28 | #include 29 | #include 30 | #include 31 | 32 | #include 33 | #include 34 | 35 | typedef actionlib::SimpleActionClient ClientExePathSyncAction; 36 | 37 | namespace MoveBaseActionsGroup 38 | { 39 | class ExePathSync : public BT::ActionNodeBase 40 | { 41 | public: 42 | std::string topic_exe_path; 43 | ros::Subscriber sub_pose, sub_result; 44 | ros::Publisher pub_bool; 45 | std::string topic_pose_reset, frame_id_target_pose; 46 | 47 | bool is_initialized = false; 48 | bool wait_for_reach_goal = true; 49 | bool clear_pose_and_path_after_execution = true; 50 | bool publish_confirmation = false; 51 | std::string topic_confirmation; 52 | 53 | ExePathSync(const std::string& name, const BT::NodeConfiguration& config); 54 | ~ExePathSync(); 55 | 56 | BT::NodeStatus tick() override; 57 | 58 | static BT::PortsList providedPorts() 59 | { 60 | static BT::PortsList ports = { BT::BidirectionalPort("target_pose"), 61 | BT::BidirectionalPort("path"), 62 | BT::InputPort("controller"), 63 | BT::OutputPort("nav_error_code") }; 64 | 65 | return ports; 66 | } 67 | 68 | bool sendNewGoal(); 69 | void doneCallback(const actionlib::SimpleClientGoalState& state, const mbf_msgs::ExePathResultConstPtr& result); 70 | void activeCallback(); 71 | void feedbackCallback(const mbf_msgs::ExePathFeedbackConstPtr& feedback); 72 | void callbackPose(const geometry_msgs::PoseStamped& msg); 73 | void callbackNavigationResult(const mbf_msgs::ExePathActionResult& msg); 74 | bool checkValidState(const int outcome); 75 | 76 | void halt() override; 77 | 78 | private: 79 | std::atomic_bool exe_result, exploration_finished, must_restart, tracking_goal; 80 | std::unique_ptr ac; 81 | }; 82 | 83 | } // namespace MoveBaseActionsGroup 84 | 85 | #endif // MOVE_BASE_EXE_PATH_SYNC_H 86 | -------------------------------------------------------------------------------- /fkie_bt_visualization/include/fkie_bt_visualization/TreeItem.hpp: -------------------------------------------------------------------------------- 1 | // Copyright 2022 Fraunhofer FKIE - All Rights Reserved 2 | // 3 | // Licensed under the Apache License, Version 2.0 (the "License"); 4 | // you may not use this file except in compliance with the License. 5 | // You may obtain a copy of the License at 6 | // 7 | // http://www.apache.org/licenses/LICENSE-2.0 8 | // 9 | // Unless required by applicable law or agreed to in writing, software 10 | // distributed under the License is distributed on an "AS IS" BASIS, 11 | // WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 12 | // See the License for the specific language governing permissions and 13 | // limitations under the License. 14 | 15 | #ifndef TREE_ITEM_H 16 | #define TREE_ITEM_H 17 | 18 | #include 19 | #include 20 | #include 21 | 22 | #include 23 | #include 24 | #include 25 | #include 26 | #include 27 | #include 28 | #include 29 | 30 | class TreeItem 31 | { 32 | public: 33 | std::string node_type; 34 | std::string node_type_view; 35 | std::string node_ID; 36 | std::string node_name; 37 | std::string node_name_view; 38 | 39 | TreeItem(std::string node_type, std::string node_ID, std::string node_name) 40 | : node_type(node_type), node_type_view(node_type), node_ID(node_ID), node_name(node_name), node_name_view(node_name) 41 | { 42 | clearName(); 43 | } 44 | 45 | TreeItem(QString node_type, QString node_ID, QString node_name) 46 | : TreeItem(node_type.toStdString(), node_ID.toStdString(), node_name.toStdString()) 47 | { 48 | clearName(); 49 | } 50 | 51 | TreeItem(QDomNode node) 52 | : TreeItem(node.toElement().tagName(), node.toElement().attribute(QString("ID")), 53 | node.toElement().attribute(QString("name"))) 54 | { 55 | clearName(); 56 | } 57 | 58 | bool isValid() const 59 | { 60 | return !node_type.empty() || !node_ID.empty() || !node_name.empty(); 61 | } 62 | 63 | void clearName() 64 | { 65 | if (node_type == "Action") 66 | { 67 | node_type_view = "|"; 68 | node_name_view = "[A] " + node_name; 69 | } 70 | 71 | if (node_type == "Condition") 72 | { 73 | node_type_view = "|"; 74 | node_name_view = "[C] " + node_name; 75 | } 76 | } 77 | 78 | /** 79 | * @brief Computes an "unique" string from properties. It might not be unique if the XML tree is not proper configured 80 | */ 81 | std::string getStrID() const 82 | { 83 | // return node_type + "_" + node_name + "_" + node_ID; 84 | return node_name; // so far, only name is consistent 85 | } 86 | 87 | QString toQString() const 88 | { 89 | return QString("Node:%0, ID:%1, Name:%2") 90 | .arg(QString(node_type.c_str())) 91 | .arg(QString(node_ID.c_str())) 92 | .arg(QString(node_name.c_str())); 93 | } 94 | 95 | std::string toString() const 96 | { 97 | std::string r; 98 | r += " Type: " + node_type; 99 | r += " Name: " + node_name; 100 | r += " ID: " + node_ID; 101 | return r; 102 | } 103 | }; 104 | 105 | #endif // TREE_ITEM_H 106 | -------------------------------------------------------------------------------- /fkie_behavior_tree_manager/include/fkie_behavior_tree_manager/decorators/ConditionalLoop.hpp: -------------------------------------------------------------------------------- 1 | // Copyright 2022 Fraunhofer FKIE - All Rights Reserved 2 | // 3 | // Licensed under the Apache License, Version 2.0 (the "License"); 4 | // you may not use this file except in compliance with the License. 5 | // You may obtain a copy of the License at 6 | // 7 | // http://www.apache.org/licenses/LICENSE-2.0 8 | // 9 | // Unless required by applicable law or agreed to in writing, software 10 | // distributed under the License is distributed on an "AS IS" BASIS, 11 | // WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 12 | // See the License for the specific language governing permissions and 13 | // limitations under the License. 14 | 15 | #ifndef CONDITIONAL_LOOP_H 16 | #define CONDITIONAL_LOOP_H 17 | 18 | #include "behaviortree_cpp_v3/decorator_node.h" 19 | #include 20 | #include 21 | 22 | namespace BehaviorTreeNodes 23 | { 24 | class ConditionalLoop : public BT::DecoratorNode 25 | { 26 | private: 27 | bool condition = false; 28 | 29 | public: 30 | std::string name; 31 | 32 | static BT::PortsList providedPorts() 33 | { 34 | BT::PortsList ports = { BT::InputPort("condition") }; 35 | return ports; 36 | } 37 | 38 | ConditionalLoop(const std::string& _name, const BT::NodeConfiguration& config) 39 | : BT::DecoratorNode(_name, config), name("[" + _name + "] ") 40 | { 41 | BT_ROS_INFO_STREAM(name << " initialized"); 42 | } 43 | 44 | inline BT::NodeStatus tick() override 45 | { 46 | setStatus(BT::NodeStatus::RUNNING); 47 | 48 | auto res = BT::TreeNode::getInput("condition", condition); 49 | if (!res) 50 | { 51 | BT_ROS_ERROR_STREAM(name << " Error reading [condition]: " << res.error()); 52 | return BT::NodeStatus::FAILURE; 53 | } 54 | 55 | BT::NodeStatus loop_status = BT::NodeStatus::IDLE; 56 | 57 | while (ros::ok() && loop_status != BT::NodeStatus::FAILURE && condition) 58 | { 59 | // tick node and wait 60 | 61 | ros::spinOnce(); 62 | 63 | BT_ROS_DEBUG_STREAM(name << "Ticking node [" << child_node_->name() << "]"); 64 | const BT::NodeStatus child_state = child_node_->executeTick(); 65 | 66 | switch (child_state) 67 | { 68 | case BT::NodeStatus::SUCCESS: 69 | loop_status = BT::NodeStatus::SUCCESS; 70 | break; 71 | 72 | case BT::NodeStatus::RUNNING: 73 | loop_status = BT::NodeStatus::RUNNING; 74 | break; 75 | 76 | case BT::NodeStatus::FAILURE: 77 | loop_status = BT::NodeStatus::FAILURE; 78 | break; 79 | 80 | default: 81 | loop_status = BT::NodeStatus::FAILURE; 82 | break; 83 | } 84 | 85 | res = BT::TreeNode::getInput("condition", condition); 86 | if (!res) 87 | { 88 | BT_ROS_ERROR_STREAM(name << " Error reading [condition]: " << res.error()); 89 | return BT::NodeStatus::FAILURE; 90 | } 91 | 92 | if (condition) 93 | { 94 | BT_ROS_DEBUG_STREAM(name << "condition is true, ticking again the node."); 95 | } 96 | } 97 | 98 | return loop_status; 99 | } 100 | }; 101 | 102 | } // namespace BehaviorTreeNodes 103 | 104 | #endif // CONDITIONAL_LOOP_H -------------------------------------------------------------------------------- /fkie_behavior_tree_manager/include/fkie_behavior_tree_manager/actions/PublishPathAction.hpp: -------------------------------------------------------------------------------- 1 | // Copyright 2022 Fraunhofer FKIE - All Rights Reserved 2 | // 3 | // Licensed under the Apache License, Version 2.0 (the "License"); 4 | // you may not use this file except in compliance with the License. 5 | // You may obtain a copy of the License at 6 | // 7 | // http://www.apache.org/licenses/LICENSE-2.0 8 | // 9 | // Unless required by applicable law or agreed to in writing, software 10 | // distributed under the License is distributed on an "AS IS" BASIS, 11 | // WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 12 | // See the License for the specific language governing permissions and 13 | // limitations under the License. 14 | 15 | #ifndef PUBLISH_PATH_ACTION_NODE_H 16 | #define PUBLISH_PATH_ACTION_NODE_H 17 | 18 | #include 19 | #include 20 | #include 21 | 22 | #include 23 | #include 24 | #include 25 | 26 | namespace BehaviorTreeNodes 27 | { 28 | class PublishPathAction : public BT::SyncActionNode 29 | { 30 | public: 31 | std::string name; 32 | ros::NodeHandle public_node; 33 | std::string topic_path_out; 34 | ros::Publisher pub_path; 35 | 36 | bool valid_init = false; 37 | 38 | static BT::PortsList providedPorts() 39 | { 40 | BT::PortsList ports = { BT::BidirectionalPort("path") }; 41 | return ports; 42 | } 43 | 44 | PublishPathAction(const std::string& _name, const BT::NodeConfiguration& config) 45 | : BT::SyncActionNode(_name, config), name("[" + _name + "] ") 46 | { 47 | ros::NodeHandle private_node("~"); 48 | 49 | std::string parameter_topic_path_out = "PublishPathAction/" + _name + "_topic_path_out"; 50 | private_node.param(parameter_topic_path_out, topic_path_out, std::string("")); 51 | 52 | if (topic_path_out.empty()) 53 | { 54 | BT_ROS_ERROR_STREAM(name << " topic_path_out is empty. Check parameter [" << parameter_topic_path_out << "]"); 55 | return; 56 | } 57 | 58 | pub_path = public_node.advertise(topic_path_out, 10); 59 | valid_init = true; 60 | 61 | BT_ROS_INFO_STREAM(name << " initialized"); 62 | } 63 | 64 | inline BT::NodeStatus tick() override 65 | { 66 | setStatus(BT::NodeStatus::RUNNING); 67 | 68 | if (!valid_init) 69 | return BT::NodeStatus::FAILURE; 70 | 71 | // get target pose 72 | nav_msgs::Path path; 73 | auto res = BT::TreeNode::getInput("path", path); 74 | if (!res) 75 | { 76 | BT_ROS_ERROR_STREAM(name << " Could not load path, resetting variable. Error: " << res.error()); 77 | BT::TreeNode::setOutput("path", nav_msgs::Path()); // reset current path 78 | return BT::NodeStatus::FAILURE; 79 | } 80 | 81 | if (path.poses.size() == 0) 82 | { 83 | BT_ROS_ERROR_STREAM(name << "Invalid path with 0 poses. "); 84 | return BT::NodeStatus::FAILURE; 85 | } 86 | 87 | if (path.header.frame_id.empty()) 88 | { 89 | BT_ROS_ERROR_STREAM(name << "Empty frame id."); 90 | return BT::NodeStatus::FAILURE; 91 | } 92 | 93 | pub_path.publish(path); 94 | return BT::NodeStatus::SUCCESS; 95 | } 96 | }; 97 | 98 | } // namespace BehaviorTreeNodes 99 | 100 | #endif // PUBLISH_PATH_ACTION_NODE_H -------------------------------------------------------------------------------- /fkie_bt_visualization/src/rviz_bt_status.h: -------------------------------------------------------------------------------- 1 | // Copyright 2022 Fraunhofer FKIE - All Rights Reserved 2 | // 3 | // Licensed under the Apache License, Version 2.0 (the "License"); 4 | // you may not use this file except in compliance with the License. 5 | // You may obtain a copy of the License at 6 | // 7 | // http://www.apache.org/licenses/LICENSE-2.0 8 | // 9 | // Unless required by applicable law or agreed to in writing, software 10 | // distributed under the License is distributed on an "AS IS" BASIS, 11 | // WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 12 | // See the License for the specific language governing permissions and 13 | // limitations under the License. 14 | 15 | #ifndef BT_STATUS_PANEL_H 16 | #define BT_STATUS_PANEL_H 17 | 18 | #include "boost/date_time/posix_time/posix_time.hpp" 19 | #include 20 | #include 21 | #include 22 | #include 23 | #include 24 | #include 25 | #include 26 | #include 27 | #include 28 | #include 29 | #include 30 | #include 31 | #include 32 | #include 33 | #include 34 | 35 | #include 36 | #include 37 | 38 | #ifndef Q_MOC_RUN 39 | #include "fkie_bt_visualization/TreeModel.hpp" 40 | #include 41 | #include 42 | #endif 43 | 44 | #include 45 | #include 46 | 47 | class QLineEdit; 48 | 49 | namespace fkie_bt_visualization 50 | { 51 | class BTStatusPanel : public rviz::Panel 52 | { 53 | Q_OBJECT 54 | 55 | public: 56 | BTStatusPanel(QWidget* parent = 0); 57 | 58 | virtual void load(const rviz::Config& config); 59 | virtual void save(rviz::Config config) const; 60 | 61 | public Q_SLOTS: 62 | 63 | void setBTName(const QString& bt_name); 64 | 65 | protected Q_SLOTS: 66 | 67 | // ROS Callbacks 68 | void callbackBTStatus(const fkie_behavior_tree_msgs::BTStatusConstPtr& msg); 69 | void callbackBTNodeStatus(const fkie_behavior_tree_msgs::BTNodeStatusConstPtr& msg); 70 | void rosSpin(); 71 | 72 | void updateBTName(); 73 | 74 | // Button callbacks 75 | void startButton(); 76 | void stopButton(); 77 | void cbStateChanged(bool); 78 | void treeButton(); 79 | 80 | bool loadModels(const fkie_behavior_tree_msgs::BTStatus& bt_status); 81 | void applyViewMode(const bool use_compact_mode); 82 | 83 | protected: 84 | // GUI Elements 85 | QLineEdit* output_bt_name_editor_; 86 | QString bt_status_bt_name_; 87 | 88 | QLabel* qlabel_bt_status; 89 | QLabel* qlabel_action; 90 | QLabel* q_bt_status; 91 | 92 | QPushButton* m_start_button; 93 | QPushButton* m_stop_button; 94 | QPushButton* m_tree_button; 95 | 96 | QCheckBox* cb_mode; 97 | 98 | QTreeView* q_tree_view_compact; 99 | 100 | // ROS Elements 101 | ros::Subscriber sub_bt_status; 102 | ros::Subscriber sub_bt_node_status; 103 | 104 | ros::NodeHandle nh_; 105 | 106 | ros::Publisher pub_start; 107 | ros::Publisher pub_stop; 108 | 109 | fkie_behavior_tree_msgs::BTStatus current_bt_status; 110 | fkie_behavior_tree_msgs::BTNodeStatus current_node_status; 111 | 112 | std::unique_ptr q_tree_view; 113 | TreeModel tree_model; 114 | 115 | bool use_compact_mode = true; 116 | }; 117 | } // namespace fkie_bt_visualization 118 | 119 | #endif // BT_STATUS_PANEL_H 120 | -------------------------------------------------------------------------------- /fkie_behavior_tree_manager/include/fkie_behavior_tree_manager/conditions/GenericMsgReceived.hpp: -------------------------------------------------------------------------------- 1 | // Copyright 2022 Fraunhofer FKIE - All Rights Reserved 2 | // 3 | // Licensed under the Apache License, Version 2.0 (the "License"); 4 | // you may not use this file except in compliance with the License. 5 | // You may obtain a copy of the License at 6 | // 7 | // http://www.apache.org/licenses/LICENSE-2.0 8 | // 9 | // Unless required by applicable law or agreed to in writing, software 10 | // distributed under the License is distributed on an "AS IS" BASIS, 11 | // WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 12 | // See the License for the specific language governing permissions and 13 | // limitations under the License. 14 | 15 | #ifndef GENERIC_MSG_CONDITION_H 16 | #define GENERIC_MSG_CONDITION_H 17 | 18 | #include 19 | #include 20 | #include 21 | #include 22 | #include 23 | #include 24 | #include 25 | #include 26 | 27 | namespace BehaviorTreeNodes 28 | { 29 | class GenericMsgReceived : public BT::ConditionNode 30 | { 31 | public: 32 | std::string name; 33 | ros::Subscriber sub; 34 | std::string parameter_topic; 35 | 36 | bool message_received; 37 | bool valid_init = false; 38 | 39 | GenericMsgReceived(const std::string& name, const BT::NodeConfiguration& config); 40 | 41 | static BT::PortsList providedPorts() 42 | { 43 | BT::PortsList ports = {}; 44 | return ports; 45 | } 46 | 47 | BT::NodeStatus tick() override; 48 | 49 | void topicCallback(const topic_tools::ShapeShifter::ConstPtr& msg, const std::string& topic_name); 50 | }; 51 | 52 | inline BT::NodeStatus GenericMsgReceived::tick() 53 | { 54 | setStatus(BT::NodeStatus::RUNNING); 55 | 56 | if (!valid_init) 57 | return BT::NodeStatus::FAILURE; 58 | 59 | if (message_received) 60 | { 61 | BT_ROS_INFO_STREAM(name << " New message received"); 62 | message_received = false; 63 | return BT::NodeStatus::SUCCESS; 64 | } 65 | else 66 | return BT::NodeStatus::FAILURE; 67 | } 68 | 69 | GenericMsgReceived::GenericMsgReceived(const std::string& _name, const BT::NodeConfiguration& config) 70 | : BT::ConditionNode(_name, config), name("[" + _name + "] ") 71 | { 72 | ros::NodeHandle private_node("~"); 73 | ros::NodeHandle public_node; 74 | 75 | std::string topic; 76 | 77 | parameter_topic = "GenericMsgReceived/" + _name + "_topic"; 78 | 79 | private_node.param(parameter_topic, topic, "msg_in"); 80 | 81 | if (topic.empty()) 82 | { 83 | BT_ROS_ERROR_STREAM(name << " [topic] is empty, check parameter [" << parameter_topic << "]"); 84 | return; 85 | } 86 | 87 | // who is afraid of lambdas and boost::functions ? 88 | boost::function callback; 89 | callback = [&](const topic_tools::ShapeShifter::ConstPtr& msg) -> void { topicCallback(msg, topic); }; 90 | sub = public_node.subscribe(topic, 10, callback); 91 | 92 | valid_init = true; 93 | 94 | BT_ROS_INFO_STREAM(name << " initialized"); 95 | } 96 | 97 | void GenericMsgReceived::topicCallback(const topic_tools::ShapeShifter::ConstPtr& msg, const std::string& topic_name) 98 | { 99 | // BT_ROS_DEBUG_STREAM(name << " new message received in topic: [" << topic_name << "]"); 100 | message_received = true; 101 | } 102 | 103 | } // namespace BehaviorTreeNodes 104 | 105 | #endif // GENERIC_MSG_CONDITION_H -------------------------------------------------------------------------------- /fkie_bt_move_base_actions/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 3.0.2) 2 | project(fkie_bt_move_base_actions) 3 | 4 | find_package(catkin REQUIRED COMPONENTS 5 | roscpp 6 | rospy 7 | std_msgs 8 | mbf_msgs 9 | actionlib 10 | actionlib_msgs 11 | geometry_msgs 12 | nav_msgs 13 | fkie_behavior_tree_manager 14 | tf 15 | message_generation 16 | ) 17 | 18 | # Look for system package 19 | find_package(BehaviorTreeV3) 20 | if(NOT TARGET BT::behaviortree_cpp_v3) 21 | # If not found, look for package in ROS workspace 22 | find_package(behaviortree_cpp_v3 REQUIRED) 23 | #catkin_lint: ignore_once order_violation 24 | add_library(BT::behaviortree_cpp_v3 INTERFACE IMPORTED) 25 | set_target_properties(BT::behaviortree_cpp_v3 PROPERTIES 26 | INTERFACE_INCLUDE_DIRECTORIES "${behaviortree_cpp_v3_INCLUDE_DIRS}" 27 | INTERFACE_LINK_LIBRARIES "${behaviortree_cpp_v3_LIBRARIES}" 28 | ) 29 | endif() 30 | 31 | add_message_files( 32 | FILES 33 | NavigationResult.msg 34 | ) 35 | 36 | generate_messages( 37 | DEPENDENCIES 38 | std_msgs 39 | ) 40 | 41 | catkin_package( 42 | CATKIN_DEPENDS 43 | roscpp 44 | rospy 45 | std_msgs 46 | mbf_msgs 47 | actionlib_msgs 48 | actionlib 49 | geometry_msgs 50 | nav_msgs 51 | fkie_behavior_tree_manager 52 | tf 53 | message_runtime 54 | ) 55 | 56 | include_directories( 57 | ${catkin_INCLUDE_DIRS} 58 | ${Boost_INCLUDE_DIRS} 59 | ${CMAKE_CURRENT_SOURCE_DIR} 60 | ) 61 | 62 | ########### 63 | ## Build ## 64 | ########### 65 | 66 | # static 67 | add_library(MoveBaseActions STATIC 68 | src/MoveBaseActions.cpp 69 | src/MoveBaseUtils.cpp 70 | src/GetPath.cpp 71 | src/ExePathAsync.cpp 72 | src/ExePathSync.cpp 73 | src/CancelExePathSync.cpp 74 | src/Recovery.cpp 75 | src/NotifyNavigationResult.cpp 76 | src/NavigateToGoal.cpp 77 | src/NavigateToGoalSync.cpp 78 | ) 79 | target_link_libraries(MoveBaseActions PRIVATE ${catkin_LIBRARIES} BT::behaviortree_cpp_v3) 80 | set_target_properties(MoveBaseActions PROPERTIES ARCHIVE_OUTPUT_DIRECTORY ${CMAKE_LIBRARY_OUTPUT_DIRECTORY} ) 81 | add_dependencies(MoveBaseActions 82 | ${${PROJECT_NAME}_generate_messages_cpp} 83 | ${${PROJECT_NAME}_EXPORTED_TARGETS} 84 | ${catkin_EXPORTED_TARGETS} 85 | ) 86 | target_compile_features(MoveBaseActions PRIVATE cxx_std_17) 87 | 88 | # plugin 89 | add_library(MoveBaseActions_dyn SHARED 90 | src/MoveBaseActions.cpp 91 | src/MoveBaseUtils.cpp 92 | src/GetPath.cpp 93 | src/ExePathAsync.cpp 94 | src/ExePathSync.cpp 95 | src/CancelExePathSync.cpp 96 | src/Recovery.cpp 97 | src/NotifyNavigationResult.cpp 98 | src/NavigateToGoal.cpp 99 | src/NavigateToGoalSync.cpp 100 | ) 101 | target_link_libraries(MoveBaseActions_dyn PRIVATE ${catkin_LIBRARIES} BT::behaviortree_cpp_v3) 102 | target_compile_definitions(MoveBaseActions_dyn PRIVATE BT_PLUGIN_EXPORT) 103 | set_target_properties(MoveBaseActions_dyn PROPERTIES LIBRARY_OUTPUT_DIRECTORY ${CMAKE_LIBRARY_OUTPUT_DIRECTORY} ) 104 | add_dependencies(MoveBaseActions_dyn 105 | ${${PROJECT_NAME}_generate_messages_cpp} 106 | ${${PROJECT_NAME}_EXPORTED_TARGETS} 107 | ${catkin_EXPORTED_TARGETS} 108 | ) 109 | target_compile_features(MoveBaseActions_dyn PRIVATE cxx_std_17) 110 | 111 | 112 | ############# 113 | ## Install ## 114 | ############# 115 | 116 | install( 117 | TARGETS MoveBaseActions MoveBaseActions_dyn 118 | ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} 119 | LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} 120 | RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} 121 | ) 122 | -------------------------------------------------------------------------------- /fkie_behavior_tree_manager/include/fkie_behavior_tree_manager/conditions/BoolCondition.hpp: -------------------------------------------------------------------------------- 1 | // Copyright 2022 Fraunhofer FKIE - All Rights Reserved 2 | // 3 | // Licensed under the Apache License, Version 2.0 (the "License"); 4 | // you may not use this file except in compliance with the License. 5 | // You may obtain a copy of the License at 6 | // 7 | // http://www.apache.org/licenses/LICENSE-2.0 8 | // 9 | // Unless required by applicable law or agreed to in writing, software 10 | // distributed under the License is distributed on an "AS IS" BASIS, 11 | // WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 12 | // See the License for the specific language governing permissions and 13 | // limitations under the License. 14 | 15 | #ifndef BOOL_CONDITION_H 16 | #define BOOL_CONDITION_H 17 | 18 | #include 19 | #include 20 | #include 21 | #include 22 | #include 23 | #include 24 | 25 | #include 26 | 27 | namespace BehaviorTreeNodes 28 | { 29 | class BoolCondition : public BT::ConditionNode 30 | { 31 | public: 32 | std::string name; 33 | ros::Subscriber sub_bool; 34 | std::string topic_bool; 35 | std_msgs::Bool current_bool; 36 | 37 | std::string parameter_topic_bool; 38 | std::string parameter_latch_mode; 39 | bool latch_mode; 40 | 41 | bool valid_init = false; 42 | 43 | BoolCondition(const std::string& name, const BT::NodeConfiguration& config); 44 | 45 | static BT::PortsList providedPorts() 46 | { 47 | BT::PortsList ports = {}; 48 | return ports; 49 | } 50 | 51 | BT::NodeStatus tick() override; 52 | void callbackBool(const std_msgs::Bool& msg); 53 | }; 54 | 55 | inline BT::NodeStatus BoolCondition::tick() 56 | { 57 | setStatus(BT::NodeStatus::RUNNING); 58 | 59 | if (!valid_init) 60 | return BT::NodeStatus::FAILURE; 61 | 62 | BT_ROS_DEBUG_STREAM(name << " current_bool: " << (current_bool.data ? "true" : "false")); 63 | if (current_bool.data) 64 | { 65 | if (!latch_mode) 66 | { 67 | BT_ROS_DEBUG_STREAM(name << " setting current_bool to false. (latch_mode disabled)"); 68 | current_bool.data = false; 69 | } 70 | return BT::NodeStatus::SUCCESS; 71 | } 72 | else 73 | return BT::NodeStatus::FAILURE; 74 | } 75 | 76 | BoolCondition::BoolCondition(const std::string& _name, const BT::NodeConfiguration& config) 77 | : BT::ConditionNode(_name, config), name("[" + _name + "] ") 78 | { 79 | ros::NodeHandle private_node("~"); 80 | ros::NodeHandle public_node; 81 | 82 | parameter_topic_bool = "BoolCondition/" + _name + "_topic_bool"; 83 | parameter_latch_mode = "BoolCondition/" + _name + "_latch_mode"; 84 | 85 | private_node.param(parameter_topic_bool, topic_bool, "bool_in"); 86 | private_node.param(parameter_latch_mode, latch_mode, false); 87 | 88 | if (topic_bool.empty()) 89 | { 90 | BT_ROS_ERROR_STREAM(name << " [topic_bool] is empty, check parameter [" << parameter_topic_bool << "]"); 91 | return; 92 | } 93 | 94 | sub_bool = public_node.subscribe(topic_bool, 1, &BoolCondition::callbackBool, this); 95 | valid_init = true; 96 | 97 | BT_ROS_INFO_STREAM(name << " initialized"); 98 | } 99 | 100 | void BoolCondition::callbackBool(const std_msgs::Bool& msg) 101 | { 102 | current_bool = msg; 103 | BT_ROS_DEBUG_STREAM(name << " New bool message received: " << current_bool.data); 104 | } 105 | 106 | } // namespace BehaviorTreeNodes 107 | 108 | #endif // BOOL_CONDITION_H 109 | -------------------------------------------------------------------------------- /fkie_bt_visualization/src/randomcolor/randomcolor.h: -------------------------------------------------------------------------------- 1 | // source: https://github.com/xuboying/randomcolor-cpp 2 | // License: BSD-3-Clause license 3 | 4 | #ifndef RANDOMCOLOR_H 5 | #define RANDOMCOLOR_H 6 | #include 7 | #include 8 | #include 9 | #include 10 | #include 11 | namespace RandomColor { 12 | enum LUMINOSITY { 13 | RANDOM, 14 | BRIGHT, 15 | LIGHT, 16 | DARK 17 | }; 18 | enum HUENAMES { 19 | UNSET, 20 | MONOCHROME, 21 | RED, 22 | ORANGE, 23 | YELLOW, 24 | GREEN, 25 | BLUE, 26 | PURPLE, 27 | PINK 28 | }; 29 | struct Options { 30 | int hue = 0; 31 | HUENAMES hue_name = HUENAMES::UNSET; 32 | size_t seed = 0; 33 | LUMINOSITY luminosity = LUMINOSITY::RANDOM; 34 | }; 35 | using rangetype = std::tuple; 36 | class RandomColorGenerator { 37 | private: 38 | enum COLORDICT { 39 | HEU_RANGE, 40 | LOW_BOUNDS, 41 | STATURATION_RANGE, 42 | BRIGHTNESS_RANGE 43 | }; 44 | using Colormap = std::map>>; 45 | static Colormap colorDictionary; 46 | //-------------------------------------------------------------------------- 47 | private: 48 | //-------------------------------------------------------------------------- 49 | static double Random() { 50 | return ((double) rand() / (RAND_MAX)); 51 | } 52 | //-------------------------------------------------------------------------- 53 | rangetype getSaturationRange(int hue) { 54 | return getColorInfo(hue)[COLORDICT::STATURATION_RANGE][0]; 55 | } 56 | //-------------------------------------------------------------------------- 57 | static std::tuple getHueRange(int colorInput); 58 | //-------------------------------------------------------------------------- 59 | static std::tuple getHueRange_s(HUENAMES colorInput); 60 | //-------------------------------------------------------------------------- 61 | int randomWithin(const std::tuple & range); 62 | //-------------------------------------------------------------------------- 63 | int pickHue(const Options & options); 64 | //-------------------------------------------------------------------------- 65 | std::map> getColorInfo(int hue); 66 | //-------------------------------------------------------------------------- 67 | int pickSaturation(int hue, const Options & options); 68 | //-------------------------------------------------------------------------- 69 | int getMinimumBrightness(int H, int S); 70 | //-------------------------------------------------------------------------- 71 | int pickBrightness(int H, int S, const Options & options); 72 | //-------------------------------------------------------------------------- 73 | std::tuple HSVtoRGB(std::tuple hsv); 74 | 75 | public: 76 | Options options; 77 | static Colormap loadColorBounds(); 78 | //-------------------------------------------------------------------------- 79 | RandomColorGenerator() : options() { 80 | } 81 | RandomColorGenerator(const Options & opt) : options(opt) { 82 | } 83 | //-------------------------------------------------------------------------- 84 | std::vector> randomColors(int count = 1); 85 | //-------------------------------------------------------------------------- 86 | std::tuple randomColorRGB(); 87 | //-------------------------------------------------------------------------- 88 | }; 89 | }; 90 | #endif -------------------------------------------------------------------------------- /fkie_behavior_tree_manager/include/fkie_behavior_tree_manager/decorators/CommandTrigger.hpp: -------------------------------------------------------------------------------- 1 | // Copyright 2022 Fraunhofer FKIE - All Rights Reserved 2 | // 3 | // Licensed under the Apache License, Version 2.0 (the "License"); 4 | // you may not use this file except in compliance with the License. 5 | // You may obtain a copy of the License at 6 | // 7 | // http://www.apache.org/licenses/LICENSE-2.0 8 | // 9 | // Unless required by applicable law or agreed to in writing, software 10 | // distributed under the License is distributed on an "AS IS" BASIS, 11 | // WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 12 | // See the License for the specific language governing permissions and 13 | // limitations under the License. 14 | 15 | #ifndef SWITCH_ONCE_NODE_H 16 | #define SWITCH_ONCE_NODE_H 17 | 18 | #include "behaviortree_cpp_v3/decorator_node.h" 19 | #include 20 | #include 21 | #include 22 | 23 | namespace BehaviorTreeNodes 24 | { 25 | class CommandTrigger : public BT::DecoratorNode 26 | { 27 | public: 28 | std::string name; 29 | ros::Subscriber sub_cmd; 30 | std::string cmd_on, topic_cmd; 31 | bool valid_init = false; 32 | bool msg_enable = false; 33 | bool normally_open = false; 34 | 35 | static BT::PortsList providedPorts() 36 | { 37 | BT::PortsList ports; 38 | return ports; 39 | } 40 | 41 | CommandTrigger(const std::string& _name, const BT::NodeConfiguration& config) 42 | : BT::DecoratorNode(_name, config), name("[" + _name + "] ") 43 | { 44 | ros::NodeHandle private_node("~"); 45 | ros::NodeHandle public_node; 46 | 47 | std::string parameter_topic_cmd = "CommandTrigger/" + _name + "_topic_cmd"; 48 | std::string parameter_cmd_on = "CommandTrigger/" + _name + "_cmd_on"; 49 | std::string parameter_normally_open = "CommandTrigger/" + _name + "_normally_open"; 50 | 51 | private_node.param(parameter_topic_cmd, topic_cmd, std::string("")); 52 | private_node.param(parameter_cmd_on, cmd_on, std::string("")); 53 | private_node.param(parameter_normally_open, normally_open, true); 54 | 55 | if (topic_cmd.empty()) 56 | { 57 | BT_ROS_ERROR_STREAM(name << "Parameter [" << parameter_topic_cmd << "] is empty."); 58 | return; 59 | } 60 | 61 | if (cmd_on.empty()) 62 | { 63 | BT_ROS_ERROR_STREAM(name << "Parameter [" << parameter_cmd_on << "] is empty."); 64 | return; 65 | } 66 | 67 | sub_cmd = public_node.subscribe(topic_cmd, 2, &CommandTrigger::callbackCmd, this); 68 | valid_init = true; 69 | 70 | BT_ROS_INFO_STREAM(name << " initialized"); 71 | } 72 | 73 | inline BT::NodeStatus tick() override 74 | { 75 | setStatus(BT::NodeStatus::RUNNING); 76 | 77 | if (!valid_init) 78 | return BT::NodeStatus::FAILURE; 79 | 80 | if ((normally_open && msg_enable) || (!normally_open && !msg_enable)) 81 | { 82 | const BT::NodeStatus child_state = child_node_->executeTick(); 83 | BT_ROS_DEBUG_STREAM(name << "Ticking node [" << child_node_->name() << "]"); 84 | msg_enable = false; // prevent following executions 85 | return child_state; 86 | } 87 | else 88 | { 89 | msg_enable = false; // prevent following executions 90 | return BT::NodeStatus::FAILURE; 91 | } 92 | } 93 | 94 | void callbackCmd(const std_msgs::String& msg) 95 | { 96 | if (msg.data == cmd_on) 97 | msg_enable = true; 98 | 99 | BT_ROS_DEBUG_STREAM(name << "new message received msg_enable: " << msg_enable); 100 | } 101 | }; 102 | 103 | } // namespace BehaviorTreeNodes 104 | 105 | #endif // SWITCH_ONCE_NODE_H -------------------------------------------------------------------------------- /fkie_behavior_tree_manager/include/fkie_behavior_tree_manager/decorators/CommandSwitch.hpp: -------------------------------------------------------------------------------- 1 | // Copyright 2022 Fraunhofer FKIE - All Rights Reserved 2 | // 3 | // Licensed under the Apache License, Version 2.0 (the "License"); 4 | // you may not use this file except in compliance with the License. 5 | // You may obtain a copy of the License at 6 | // 7 | // http://www.apache.org/licenses/LICENSE-2.0 8 | // 9 | // Unless required by applicable law or agreed to in writing, software 10 | // distributed under the License is distributed on an "AS IS" BASIS, 11 | // WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 12 | // See the License for the specific language governing permissions and 13 | // limitations under the License. 14 | 15 | #ifndef COMMAND_SWITCH_NODE_H 16 | #define COMMAND_SWITCH_NODE_H 17 | 18 | #include 19 | #include 20 | #include 21 | #include 22 | 23 | namespace BehaviorTreeNodes 24 | { 25 | class CommandSwitch : public BT::DecoratorNode 26 | { 27 | public: 28 | std::string name; 29 | ros::Subscriber sub_cmd; 30 | std::string cmd_on, cmd_off, topic_cmd; 31 | bool valid_init = false; 32 | bool gate_enable = false; 33 | 34 | static BT::PortsList providedPorts() 35 | { 36 | BT::PortsList ports; 37 | return ports; 38 | } 39 | 40 | CommandSwitch(const std::string& _name, const BT::NodeConfiguration& config) 41 | : BT::DecoratorNode(_name, config), name("[" + _name + "] ") 42 | { 43 | ros::NodeHandle private_node("~"); 44 | ros::NodeHandle public_node; 45 | 46 | std::string parameter_topic_cmd = "CommandSwitch/" + _name + "_topic_cmd"; 47 | std::string parameter_cmd_on = "CommandSwitch/" + _name + "_cmd_on"; 48 | std::string parameter_cmd_off = "CommandSwitch/" + _name + "_cmd_off"; 49 | 50 | private_node.param(parameter_topic_cmd, topic_cmd, std::string("")); 51 | private_node.param(parameter_cmd_on, cmd_on, std::string("")); 52 | private_node.param(parameter_cmd_off, cmd_off, std::string("")); 53 | 54 | if (topic_cmd.empty()) 55 | { 56 | BT_ROS_ERROR_STREAM(name << "Parameter [" << parameter_topic_cmd << "] is empty."); 57 | return; 58 | } 59 | 60 | if (cmd_on.empty()) 61 | { 62 | ROS_ERROR_STREAM(name << "Parameter [" << parameter_cmd_on << "] is empty."); 63 | return; 64 | } 65 | 66 | if (cmd_off.empty()) 67 | { 68 | ROS_ERROR_STREAM(name << "Parameter [" << parameter_cmd_off << "] is empty."); 69 | return; 70 | } 71 | 72 | sub_cmd = public_node.subscribe(topic_cmd, 2, &CommandSwitch::callbackCmd, this); 73 | valid_init = true; 74 | 75 | BT_ROS_INFO_STREAM(name << " initialized"); 76 | } 77 | 78 | BT::NodeStatus tick() override 79 | { 80 | setStatus(BT::NodeStatus::RUNNING); 81 | 82 | if (!valid_init) 83 | return BT::NodeStatus::FAILURE; 84 | 85 | if (gate_enable) 86 | { 87 | const BT::NodeStatus child_state = child_node_->executeTick(); 88 | BT_ROS_DEBUG_STREAM(name << "Ticking node [" << child_node_->name() << "]"); 89 | return child_state; 90 | } 91 | else 92 | return BT::NodeStatus::FAILURE; 93 | } 94 | 95 | void callbackCmd(const std_msgs::String& msg) 96 | { 97 | if (msg.data == cmd_on) 98 | gate_enable = true; 99 | 100 | if (msg.data == cmd_off) 101 | gate_enable = false; 102 | 103 | BT_ROS_DEBUG_STREAM(name << " New message received, gate_enable: " << gate_enable); 104 | } 105 | }; 106 | 107 | } // namespace BehaviorTreeNodes 108 | 109 | #endif // COMMAND_SWITCH_NODE_H -------------------------------------------------------------------------------- /fkie_behavior_tree_manager/include/fkie_behavior_tree_manager/actions/PublishPoseAction.hpp: -------------------------------------------------------------------------------- 1 | // Copyright 2022 Fraunhofer FKIE - All Rights Reserved 2 | // 3 | // Licensed under the Apache License, Version 2.0 (the "License"); 4 | // you may not use this file except in compliance with the License. 5 | // You may obtain a copy of the License at 6 | // 7 | // http://www.apache.org/licenses/LICENSE-2.0 8 | // 9 | // Unless required by applicable law or agreed to in writing, software 10 | // distributed under the License is distributed on an "AS IS" BASIS, 11 | // WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 12 | // See the License for the specific language governing permissions and 13 | // limitations under the License. 14 | 15 | #ifndef PUBLISH_POSE_ACTION_NODE_H 16 | #define PUBLISH_POSE_ACTION_NODE_H 17 | 18 | #include 19 | #include 20 | #include 21 | 22 | #include 23 | #include 24 | #include 25 | 26 | namespace BehaviorTreeNodes 27 | { 28 | class PublishPoseAction : public BT::SyncActionNode 29 | { 30 | public: 31 | std::string name; 32 | ros::NodeHandle public_node; 33 | std::string topic_pose_out; 34 | ros::Publisher pub_pose; 35 | 36 | bool valid_init = false; 37 | 38 | static BT::PortsList providedPorts() 39 | { 40 | BT::PortsList ports = { BT::BidirectionalPort("target_pose") }; 41 | return ports; 42 | } 43 | 44 | PublishPoseAction(const std::string& _name, const BT::NodeConfiguration& config) 45 | : BT::SyncActionNode(_name, config), name("[" + _name + "] ") 46 | { 47 | ros::NodeHandle private_node("~"); 48 | 49 | std::string parameter_topic_pose_out = "PublishPoseAction/" + _name + "_topic_pose_out"; 50 | private_node.param(parameter_topic_pose_out, topic_pose_out, std::string("")); 51 | 52 | if (topic_pose_out.empty()) 53 | { 54 | BT_ROS_ERROR_STREAM(name << " topic_pose_out is empty. Check parameter [" << parameter_topic_pose_out << "]"); 55 | return; 56 | } 57 | 58 | pub_pose = public_node.advertise(topic_pose_out, 10); 59 | valid_init = true; 60 | 61 | BT_ROS_INFO_STREAM(name << " initialized"); 62 | } 63 | 64 | inline BT::NodeStatus tick() override 65 | { 66 | setStatus(BT::NodeStatus::RUNNING); 67 | 68 | if (!valid_init) 69 | return BT::NodeStatus::FAILURE; 70 | 71 | // get target pose 72 | Pose2D target_pose; 73 | auto res = BT::TreeNode::getInput("target_pose", target_pose); 74 | if (!res) 75 | { 76 | BT_ROS_ERROR_STREAM(name << " Could not load target_pose, resetting variable. Error: " << res.error()); 77 | BT::TreeNode::setOutput("target_pose", Pose2D()); // reset current target_pose 78 | return BT::NodeStatus::FAILURE; 79 | } 80 | 81 | if (std::isnan(target_pose.x) || std::isnan(target_pose.y) || std::isnan(target_pose.theta)) 82 | { 83 | BT_ROS_ERROR_STREAM(name << "Invalid pose: " << target_pose.toString()); 84 | return BT::NodeStatus::FAILURE; 85 | } 86 | 87 | if (target_pose.frame_id.empty()) 88 | { 89 | BT_ROS_ERROR_STREAM(name << "Empty frame id: " << target_pose.toString()); 90 | return BT::NodeStatus::FAILURE; 91 | } 92 | 93 | // publish target pose 94 | geometry_msgs::PoseStamped pose; 95 | pose = target_pose.toPoseStamped(); 96 | pub_pose.publish(pose); 97 | return BT::NodeStatus::SUCCESS; 98 | } 99 | }; 100 | 101 | } // namespace BehaviorTreeNodes 102 | 103 | #endif // PUBLISH_POSE_ACTION_NODE_H -------------------------------------------------------------------------------- /fkie_behavior_tree_manager/include/fkie_behavior_tree_manager/decorators/RateController.hpp: -------------------------------------------------------------------------------- 1 | // Copyright 2022 Fraunhofer FKIE - All Rights Reserved 2 | // 3 | // Licensed under the Apache License, Version 2.0 (the "License"); 4 | // you may not use this file except in compliance with the License. 5 | // You may obtain a copy of the License at 6 | // 7 | // http://www.apache.org/licenses/LICENSE-2.0 8 | // 9 | // Unless required by applicable law or agreed to in writing, software 10 | // distributed under the License is distributed on an "AS IS" BASIS, 11 | // WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 12 | // See the License for the specific language governing permissions and 13 | // limitations under the License. 14 | 15 | #ifndef RATE_CONTROLLER_NODE_H 16 | #define RATE_CONTROLLER_NODE_H 17 | 18 | #include "behaviortree_cpp_v3/decorator_node.h" 19 | #include 20 | #include 21 | #include 22 | 23 | namespace BehaviorTreeNodes 24 | { 25 | class RateController : public BT::DecoratorNode 26 | { 27 | private: 28 | std::chrono::time_point start_; 29 | double period = 1.0; 30 | double hz = 1.0; 31 | bool valid_init = false; 32 | 33 | public: 34 | std::string name; 35 | 36 | static BT::PortsList providedPorts() 37 | { 38 | BT::PortsList ports = { BT::InputPort("hz") }; 39 | return ports; 40 | } 41 | 42 | RateController(const std::string& _name, const BT::NodeConfiguration& config) 43 | : BT::DecoratorNode(_name, config), name("[" + _name + "] ") 44 | { 45 | auto res = BT::TreeNode::getInput("hz", hz); 46 | if (!res) 47 | { 48 | BT_ROS_ERROR_STREAM(name << " error reading [hz]: " << res.error()); 49 | valid_init = false; 50 | return; 51 | } 52 | 53 | period = 1.0 / hz; 54 | valid_init = true; 55 | 56 | BT_ROS_INFO_STREAM(name << " initialized"); 57 | } 58 | 59 | inline BT::NodeStatus tick() override 60 | { 61 | if (!valid_init) 62 | return BT::NodeStatus::FAILURE; 63 | 64 | if (status() == BT::NodeStatus::IDLE) 65 | { 66 | // Reset the starting point (moving from IDLE to RUNNING) 67 | start_ = std::chrono::high_resolution_clock::now(); 68 | } 69 | 70 | setStatus(BT::NodeStatus::RUNNING); 71 | 72 | // Determine how long its been since we've started this iteration 73 | auto now = std::chrono::high_resolution_clock::now(); 74 | auto elapsed = now - start_; 75 | 76 | // Now, get that in seconds 77 | typedef std::chrono::duration float_seconds; 78 | auto seconds = std::chrono::duration_cast(elapsed); 79 | 80 | // If we've exceed the specified period, execute the child node 81 | if (seconds.count() >= period) 82 | { 83 | const BT::NodeStatus child_state = child_node_->executeTick(); 84 | BT_ROS_DEBUG_STREAM(name << "Ticking node [" << child_node_->name() << "] - [" << seconds.count() << "]"); 85 | 86 | switch (child_state) 87 | { 88 | case BT::NodeStatus::SUCCESS: 89 | // Reset the timer 90 | start_ = std::chrono::high_resolution_clock::now(); 91 | return BT::NodeStatus::SUCCESS; 92 | 93 | case BT::NodeStatus::RUNNING: 94 | return BT::NodeStatus::RUNNING; 95 | 96 | case BT::NodeStatus::FAILURE: 97 | // Reset the timer 98 | start_ = std::chrono::high_resolution_clock::now(); 99 | return BT::NodeStatus::FAILURE; 100 | 101 | default: 102 | // We'll try again next time 103 | return BT::NodeStatus::RUNNING; 104 | } 105 | } 106 | 107 | return status(); 108 | } 109 | }; 110 | 111 | } // namespace BehaviorTreeNodes 112 | 113 | #endif // RATE_CONTROLLER_NODE_H -------------------------------------------------------------------------------- /fkie_behavior_tree_manager/include/fkie_behavior_tree_manager/actions/PublishPoseStampedAction.hpp: -------------------------------------------------------------------------------- 1 | // Copyright 2022 Fraunhofer FKIE - All Rights Reserved 2 | // 3 | // Licensed under the Apache License, Version 2.0 (the "License"); 4 | // you may not use this file except in compliance with the License. 5 | // You may obtain a copy of the License at 6 | // 7 | // http://www.apache.org/licenses/LICENSE-2.0 8 | // 9 | // Unless required by applicable law or agreed to in writing, software 10 | // distributed under the License is distributed on an "AS IS" BASIS, 11 | // WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 12 | // See the License for the specific language governing permissions and 13 | // limitations under the License. 14 | 15 | #ifndef PUBLISH_POSE_STAMPED_ACTION_NODE_H 16 | #define PUBLISH_POSE_STAMPED_ACTION_NODE_H 17 | 18 | #include 19 | #include 20 | #include 21 | 22 | #include 23 | #include 24 | #include 25 | 26 | namespace BehaviorTreeNodes 27 | { 28 | class PublishPoseStampedAction : public BT::SyncActionNode 29 | { 30 | public: 31 | std::string name; 32 | ros::NodeHandle public_node; 33 | std::string topic_pose_out; 34 | ros::Publisher pub_pose; 35 | 36 | bool valid_init = false; 37 | 38 | static BT::PortsList providedPorts() 39 | { 40 | BT::PortsList ports = { BT::BidirectionalPort("target_pose") }; 41 | return ports; 42 | } 43 | 44 | PublishPoseStampedAction(const std::string& _name, const BT::NodeConfiguration& config) 45 | : BT::SyncActionNode(_name, config), name("[" + _name + "] ") 46 | { 47 | ros::NodeHandle private_node("~"); 48 | 49 | std::string parameter_topic_pose_out = _name + "/topic_pose_out"; 50 | private_node.param(parameter_topic_pose_out, topic_pose_out, std::string("")); 51 | 52 | if (topic_pose_out.empty()) 53 | { 54 | BT_ROS_ERROR_STREAM(name << " topic_pose_out is empty. Check parameter [" << parameter_topic_pose_out << "]"); 55 | return; 56 | } 57 | 58 | pub_pose = public_node.advertise(topic_pose_out, 10); 59 | valid_init = true; 60 | 61 | BT_ROS_INFO_STREAM(name << " initialized"); 62 | } 63 | 64 | inline BT::NodeStatus tick() override 65 | { 66 | setStatus(BT::NodeStatus::RUNNING); 67 | 68 | if (!valid_init) 69 | return BT::NodeStatus::FAILURE; 70 | 71 | // get target pose 72 | geometry_msgs::PoseStamped target_pose; 73 | auto res = BT::TreeNode::getInput("target_pose", target_pose); 74 | if (!res) 75 | { 76 | BT_ROS_ERROR_STREAM(name << " Could not load target_pose, resetting variable. Error: " << res.error()); 77 | BT::TreeNode::setOutput("target_pose", geometry_msgs::PoseStamped()); // reset current target_pose 78 | return BT::NodeStatus::FAILURE; 79 | } 80 | 81 | if (std::isnan(target_pose.pose.position.x) || std::isnan(target_pose.pose.position.y)) 82 | { 83 | BT_ROS_ERROR_STREAM(name << "Invalid pose"); 84 | return BT::NodeStatus::FAILURE; 85 | } 86 | 87 | if (target_pose.header.frame_id.empty()) 88 | { 89 | BT_ROS_ERROR_STREAM(name << "Empty header frame_id: "); 90 | return BT::NodeStatus::FAILURE; 91 | } 92 | 93 | if (pub_pose.getNumSubscribers() < 1) 94 | { 95 | BT_ROS_ERROR_STREAM(name << "Publisher: topic does not have any subcribers."); 96 | return BT::NodeStatus::FAILURE; 97 | } 98 | 99 | // publish target pose 100 | pub_pose.publish(target_pose); 101 | 102 | return BT::NodeStatus::SUCCESS; 103 | } 104 | }; 105 | 106 | } // namespace BehaviorTreeNodes 107 | 108 | #endif // PUBLISH_POSE_STAMPED_ACTION_NODE_H -------------------------------------------------------------------------------- /fkie_behavior_tree_manager/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 3.0.2) 2 | project(fkie_behavior_tree_manager) 3 | 4 | set(CMAKE_CXX_STANDARD 17) 5 | set(CMAKE_CXX_STANDARD_REQUIRED ON) 6 | 7 | find_package(catkin REQUIRED COMPONENTS 8 | roscpp 9 | rospy 10 | std_msgs 11 | geometry_msgs 12 | nav_msgs 13 | tf 14 | topic_tools 15 | tf2_ros 16 | fkie_behavior_tree_msgs 17 | ) 18 | 19 | # Look for system package 20 | find_package(BehaviorTreeV3) 21 | if(NOT TARGET BT::behaviortree_cpp_v3) 22 | # If not found, look for package in ROS workspace 23 | find_package(behaviortree_cpp_v3 REQUIRED) 24 | #catkin_lint: ignore_once order_violation 25 | add_library(BT::behaviortree_cpp_v3 INTERFACE IMPORTED) 26 | set_target_properties(BT::behaviortree_cpp_v3 PROPERTIES 27 | INTERFACE_INCLUDE_DIRECTORIES "${behaviortree_cpp_v3_INCLUDE_DIRS}" 28 | INTERFACE_LINK_LIBRARIES "${behaviortree_cpp_v3_LIBRARIES}" 29 | ) 30 | endif() 31 | 32 | 33 | set(CMAKE_CONFIG_PATH ${CMAKE_MODULE_PATH} "${CMAKE_CURRENT_LIST_DIR}/cmake") 34 | list(APPEND CMAKE_MODULE_PATH "${CMAKE_CONFIG_PATH}") 35 | 36 | find_package(ZMQ) 37 | 38 | catkin_package( 39 | CATKIN_DEPENDS roscpp rospy std_msgs geometry_msgs nav_msgs tf topic_tools fkie_behavior_tree_msgs 40 | INCLUDE_DIRS include 41 | ) 42 | 43 | include_directories( 44 | include 45 | ${catkin_INCLUDE_DIRS} 46 | ${Boost_INCLUDE_DIRS} 47 | ${CMAKE_CURRENT_SOURCE_DIR} 48 | ${ZeroMQ_INCLUDE_DIR} 49 | ) 50 | 51 | ########### 52 | ## Build ## 53 | ########### 54 | 55 | # static library 56 | add_library(BehaviorTreeNodes STATIC src/BehaviorTreeNodes.cpp ) 57 | target_link_libraries(BehaviorTreeNodes PRIVATE ${catkin_LIBRARIES} BT::behaviortree_cpp_v3) 58 | set_target_properties(BehaviorTreeNodes PROPERTIES ARCHIVE_OUTPUT_DIRECTORY ${CMAKE_LIBRARY_OUTPUT_DIRECTORY} ) 59 | target_compile_features(BehaviorTreeNodes PRIVATE cxx_std_17) 60 | set_target_properties(BehaviorTreeNodes PROPERTIES 61 | CXX_STANDARD 17 62 | CXX_EXTENSIONS OFF) 63 | 64 | # plugin 65 | add_library(BehaviorTreeNodes_dyn SHARED src/BehaviorTreeNodes.cpp ) 66 | target_link_libraries(BehaviorTreeNodes_dyn PRIVATE ${catkin_LIBRARIES} BT::behaviortree_cpp_v3) 67 | target_compile_definitions(BehaviorTreeNodes_dyn PRIVATE BT_PLUGIN_EXPORT) 68 | set_target_properties(BehaviorTreeNodes_dyn PROPERTIES LIBRARY_OUTPUT_DIRECTORY ${CMAKE_LIBRARY_OUTPUT_DIRECTORY} ) 69 | target_compile_features(BehaviorTreeNodes_dyn PRIVATE cxx_std_17) 70 | set_target_properties(BehaviorTreeNodes_dyn PROPERTIES 71 | CXX_STANDARD 17 72 | CXX_EXTENSIONS OFF) 73 | 74 | # Node 75 | add_executable(behavior_tree_manager_node 76 | src/behavior_tree_manager_node.cpp 77 | src/BehaviorTreeManager.cpp 78 | ) 79 | add_dependencies(behavior_tree_manager_node 80 | ${catkin_EXPORTED_TARGETS} 81 | ${${PROJECT_NAME}_generate_messages_cpp} 82 | ${${PROJECT_NAME}_EXPORTED_TARGETS}) 83 | target_link_libraries(behavior_tree_manager_node ${catkin_LIBRARIES} ${ZeroMQ_LIBRARY} BT::behaviortree_cpp_v3) 84 | target_compile_features(behavior_tree_manager_node PRIVATE cxx_std_17) 85 | set_target_properties(behavior_tree_manager_node PROPERTIES 86 | CXX_STANDARD 17 87 | CXX_EXTENSIONS OFF) 88 | 89 | 90 | ############# 91 | ## Install ## 92 | ############# 93 | 94 | # Install all the launch files 95 | install(DIRECTORY launch/ 96 | DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}/launch) 97 | 98 | # Install all the include files 99 | install(DIRECTORY include/${PROJECT_NAME}/ 100 | DESTINATION ${CATKIN_PACKAGE_INCLUDE_DESTINATION}) 101 | 102 | # Mark executables and/or libraries for installation 103 | install(TARGETS 104 | BehaviorTreeNodes 105 | BehaviorTreeNodes_dyn 106 | behavior_tree_manager_node 107 | EXPORT BehaviorTreeNodes 108 | ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} 109 | LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} 110 | RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} 111 | PUBLIC_HEADER DESTINATION ${CATKIN_PACKAGE_INCLUDE_DESTINATION} 112 | ) 113 | -------------------------------------------------------------------------------- /fkie_bt_moveit_actions/include/fkie_bt_moveit_actions/MoveitErrorCodeInterface.hpp: -------------------------------------------------------------------------------- 1 | // Copyright 2022 Fraunhofer FKIE - All Rights Reserved 2 | // 3 | // Licensed under the Apache License, Version 2.0 (the "License"); 4 | // you may not use this file except in compliance with the License. 5 | // You may obtain a copy of the License at 6 | // 7 | // http://www.apache.org/licenses/LICENSE-2.0 8 | // 9 | // Unless required by applicable law or agreed to in writing, software 10 | // distributed under the License is distributed on an "AS IS" BASIS, 11 | // WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 12 | // See the License for the specific language governing permissions and 13 | // limitations under the License. 14 | 15 | #ifndef MOVEIT_ERROR_CODE_INTERFACE_H 16 | #define MOVEIT_ERROR_CODE_INTERFACE_H 17 | 18 | #include 19 | #include 20 | #include 21 | 22 | inline std::string getMoveitErrorCodeDescription(moveit::core::MoveItErrorCode ec) 23 | { 24 | switch (ec.val) 25 | { 26 | case moveit_msgs::MoveItErrorCodes::SUCCESS: 27 | return std::string("SUCCESS"); 28 | case moveit_msgs::MoveItErrorCodes::FAILURE: 29 | return std::string("FAILURE"); 30 | case moveit_msgs::MoveItErrorCodes::PLANNING_FAILED: 31 | return std::string("PLANNING_FAILED"); 32 | case moveit_msgs::MoveItErrorCodes::INVALID_MOTION_PLAN: 33 | return std::string("INVALID_MOTION_PLAN"); 34 | case moveit_msgs::MoveItErrorCodes::MOTION_PLAN_INVALIDATED_BY_ENVIRONMENT_CHANGE: 35 | return std::string("MOTION_PLAN_INVALIDATED_BY_ENVIRONMENT_CHANGE"); 36 | case moveit_msgs::MoveItErrorCodes::CONTROL_FAILED: 37 | return std::string("CONTROL_FAILED"); 38 | case moveit_msgs::MoveItErrorCodes::UNABLE_TO_AQUIRE_SENSOR_DATA: 39 | return std::string("UNABLE_TO_AQUIRE_SENSOR_DATA"); 40 | case moveit_msgs::MoveItErrorCodes::TIMED_OUT: 41 | return std::string("TIMED_OUT"); 42 | case moveit_msgs::MoveItErrorCodes::PREEMPTED: 43 | return std::string("PREEMPTED"); 44 | case moveit_msgs::MoveItErrorCodes::START_STATE_IN_COLLISION: 45 | return std::string("START_STATE_IN_COLLISION"); 46 | case moveit_msgs::MoveItErrorCodes::START_STATE_VIOLATES_PATH_CONSTRAINTS: 47 | return std::string("START_STATE_VIOLATES_PATH_CONSTRAINTS"); 48 | case moveit_msgs::MoveItErrorCodes::GOAL_IN_COLLISION: 49 | return std::string("GOAL_IN_COLLISION"); 50 | case moveit_msgs::MoveItErrorCodes::GOAL_VIOLATES_PATH_CONSTRAINTS: 51 | return std::string("GOAL_VIOLATES_PATH_CONSTRAINTS"); 52 | case moveit_msgs::MoveItErrorCodes::GOAL_CONSTRAINTS_VIOLATED: 53 | return std::string("GOAL_CONSTRAINTS_VIOLATED"); 54 | case moveit_msgs::MoveItErrorCodes::INVALID_GROUP_NAME: 55 | return std::string("INVALID_GROUP_NAME"); 56 | case moveit_msgs::MoveItErrorCodes::INVALID_GOAL_CONSTRAINTS: 57 | return std::string("INVALID_GOAL_CONSTRAINTS"); 58 | case moveit_msgs::MoveItErrorCodes::INVALID_ROBOT_STATE: 59 | return std::string("INVALID_ROBOT_STATE"); 60 | case moveit_msgs::MoveItErrorCodes::INVALID_LINK_NAME: 61 | return std::string("INVALID_LINK_NAME"); 62 | case moveit_msgs::MoveItErrorCodes::INVALID_OBJECT_NAME: 63 | return std::string("INVALID_OBJECT_NAME"); 64 | case moveit_msgs::MoveItErrorCodes::FRAME_TRANSFORM_FAILURE: 65 | return std::string("FRAME_TRANSFORM_FAILURE"); 66 | case moveit_msgs::MoveItErrorCodes::COLLISION_CHECKING_UNAVAILABLE: 67 | return std::string("COLLISION_CHECKING_UNAVAILABLE"); 68 | case moveit_msgs::MoveItErrorCodes::ROBOT_STATE_STALE: 69 | return std::string("ROBOT_STATE_STALE"); 70 | case moveit_msgs::MoveItErrorCodes::SENSOR_INFO_STALE: 71 | return std::string("SENSOR_INFO_STALE"); 72 | case moveit_msgs::MoveItErrorCodes::NO_IK_SOLUTION: 73 | return std::string("NO_IK_SOLUTION"); 74 | default: 75 | return std::string("INVALID MOVEIT CODE"); 76 | } 77 | }; 78 | 79 | #endif // MOVEIT_ERROR_CODE_INTERFACE_H -------------------------------------------------------------------------------- /fkie_behavior_tree_manager/include/fkie_behavior_tree_manager/conditions/TFTransformCondition.hpp: -------------------------------------------------------------------------------- 1 | // Copyright 2022 Fraunhofer FKIE - All Rights Reserved 2 | // 3 | // Licensed under the Apache License, Version 2.0 (the "License"); 4 | // you may not use this file except in compliance with the License. 5 | // You may obtain a copy of the License at 6 | // 7 | // http://www.apache.org/licenses/LICENSE-2.0 8 | // 9 | // Unless required by applicable law or agreed to in writing, software 10 | // distributed under the License is distributed on an "AS IS" BASIS, 11 | // WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 12 | // See the License for the specific language governing permissions and 13 | // limitations under the License. 14 | 15 | #ifndef TF_TRANSFORM_CONDITION_H 16 | #define TF_TRANSFORM_CONDITION_H 17 | 18 | #include 19 | #include 20 | #include 21 | #include 22 | #include 23 | 24 | #include 25 | 26 | namespace BehaviorTreeNodes 27 | { 28 | /** 29 | * @brief Check if a valid TF between [from_frame] and [to_frame] is given. 30 | * return SUCCESS if the TF is valid, FAILURE if it can not transform. 31 | * ROS Params: 32 | * [name]/from_frame: source TF frame 33 | * [name]/to_frame: target TF frame 34 | * [name]/timeout: TF Timeout 35 | */ 36 | class TFTransformCondition : public BT::ConditionNode 37 | { 38 | public: 39 | std::string name; 40 | 41 | std::string from_frame; 42 | std::string to_frame; 43 | double timeout = 0.0; 44 | 45 | tf2_ros::Buffer tf_buffer; 46 | std::unique_ptr tf_listener; 47 | 48 | bool valid_init = false; 49 | 50 | static BT::PortsList providedPorts() 51 | { 52 | BT::PortsList ports = {}; 53 | return ports; 54 | } 55 | 56 | TFTransformCondition(const std::string& _name, const BT::NodeConfiguration& config) 57 | : BT::ConditionNode(_name, config), name("[" + _name + "] ") 58 | { 59 | ros::NodeHandle private_node("~"); 60 | ros::NodeHandle public_node; 61 | 62 | tf_listener = std::make_unique(tf_buffer); 63 | 64 | private_node.param(_name + "/timeout", timeout, 1.0); 65 | private_node.param(_name + "/from_frame", from_frame, ""); 66 | private_node.param(_name + "/to_frame", to_frame, ""); 67 | 68 | BT_ROS_INFO_STREAM(name << " from_frame: " << from_frame); 69 | BT_ROS_INFO_STREAM(name << " to_frame: " << to_frame); 70 | BT_ROS_INFO_STREAM(name << " timeout: " << timeout); 71 | 72 | if (from_frame.empty()) 73 | { 74 | BT_ROS_ERROR_STREAM(name << " [from_frame] is empty, check parameter:" << _name + "/from_frame"); 75 | return; 76 | } 77 | 78 | if (to_frame.empty()) 79 | { 80 | BT_ROS_ERROR_STREAM(name << " [to_frame] is empty, check parameter:" << _name + "/to_frame"); 81 | return; 82 | } 83 | 84 | valid_init = true; 85 | BT_ROS_INFO_STREAM(name << " initialized"); 86 | } 87 | 88 | BT::NodeStatus tick() 89 | { 90 | setStatus(BT::NodeStatus::RUNNING); 91 | 92 | if (!valid_init) 93 | return BT::NodeStatus::FAILURE; 94 | 95 | bool can_transform = false; 96 | 97 | if (tf_buffer.canTransform(to_frame, from_frame, ros::Time(0), ros::Duration(timeout))) 98 | { 99 | try 100 | { 101 | geometry_msgs::TransformStamped ts; 102 | ts = tf_buffer.lookupTransform(to_frame, from_frame, ros::Time(0), ros::Duration(timeout)); 103 | can_transform = true; 104 | } 105 | catch (std::exception& e) 106 | { 107 | } 108 | } 109 | 110 | if (!can_transform) 111 | { 112 | BT_ROS_ERROR_STREAM(name << " no transform between [" << from_frame << "] and [" << to_frame << "]"); 113 | } 114 | 115 | return can_transform ? BT::NodeStatus::SUCCESS : BT::NodeStatus::FAILURE; 116 | } 117 | }; 118 | 119 | } // namespace BehaviorTreeNodes 120 | 121 | #endif // TF_TRANSFORM_CONDITION_H 122 | -------------------------------------------------------------------------------- /fkie_behavior_tree_manager/src/BehaviorTreeManager.h: -------------------------------------------------------------------------------- 1 | // Copyright 2022 Fraunhofer FKIE - All Rights Reserved 2 | // 3 | // Licensed under the Apache License, Version 2.0 (the "License"); 4 | // you may not use this file except in compliance with the License. 5 | // You may obtain a copy of the License at 6 | // 7 | // http://www.apache.org/licenses/LICENSE-2.0 8 | // 9 | // Unless required by applicable law or agreed to in writing, software 10 | // distributed under the License is distributed on an "AS IS" BASIS, 11 | // WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 12 | // See the License for the specific language governing permissions and 13 | // limitations under the License. 14 | 15 | #ifndef ROBOT_BEHAVIOR_TREE_MANAGER_H 16 | #define ROBOT_BEHAVIOR_TREE_MANAGER_H 17 | 18 | #include 19 | #include 20 | #include 21 | 22 | // loggers 23 | #include 24 | #include 25 | #include 26 | #include 27 | #include 28 | 29 | #include 30 | #include 31 | #include 32 | #include 33 | 34 | #include 35 | 36 | #include "BehaviorTreeParameters.hpp" 37 | 38 | class BehaviorTreeManager 39 | { 40 | public: 41 | BT::BehaviorTreeFactory bt_factory; 42 | std::unique_ptr tree; 43 | BehaviorTreeParameters params; 44 | 45 | bool is_tree_enabled = true; 46 | bool must_delete_tree = false; 47 | std::string file_tree_content; 48 | 49 | // logger definitions 50 | std::unique_ptr std_cout_logger; 51 | std::unique_ptr file_logger; 52 | std::unique_ptr minitrace_logger; 53 | std::unique_ptr zmq_logger; 54 | std::unique_ptr ros_msg_logger; 55 | 56 | // control interface 57 | ros::Subscriber sub_start; 58 | ros::Subscriber sub_stop; 59 | ros::Publisher pub_bt_status; 60 | 61 | private: 62 | BTPublisherRosSingleton* s_bt_publisher = BTPublisherRosSingleton::GetInstance(); 63 | 64 | std::string current_static_tree; 65 | std::string current_path_file; 66 | 67 | std::mutex mutex_tree; 68 | 69 | public: 70 | BehaviorTreeManager(); 71 | ~BehaviorTreeManager(); 72 | 73 | /** 74 | * @brief Callback requesting to start the execution of current Tree 75 | */ 76 | void callbackStart(const topic_tools::ShapeShifter::ConstPtr& msg); 77 | 78 | /** 79 | * @brief Callback requesting to stop the execution of current Tree 80 | */ 81 | void callbackStop(const topic_tools::ShapeShifter::ConstPtr& msg); 82 | 83 | /** 84 | * @brief Publish the current execution state of the tree 85 | */ 86 | void reportBTState(); 87 | 88 | /** 89 | * @brief Main loop, here will be called the root node of the tree continuously. 90 | */ 91 | void spin(); 92 | 93 | /** 94 | * @brief Use [registerFromPlugin] from BT Factory to register plugins from ROS parameter [action_plugins] 95 | */ 96 | void registerActionPlugins(); 97 | 98 | /** 99 | * @brief Initializes the tree using an XML text string 100 | */ 101 | void initializeTreeFromText(const std::string static_tree); 102 | 103 | /** 104 | * @brief Initializes the tree using a file 105 | */ 106 | void initializeTreeFromFile(const std::string path_file); 107 | 108 | /** 109 | * @brief Reinitialize the tree, using [current_static_tree] or [current_path_file] 110 | */ 111 | bool reinitializeTree(); 112 | 113 | /** 114 | * @brief Initializes the loggers according to ROS parameter [logging/...] 115 | */ 116 | void initializeLoggers(); 117 | 118 | /** 119 | * @brief Reset/Destroy all loggers 120 | */ 121 | void resetLoggers(); 122 | }; 123 | 124 | #endif // ROBOT_BEHAVIOR_TREE_MANAGER_H -------------------------------------------------------------------------------- /fkie_behavior_tree_manager/include/fkie_behavior_tree_manager/conditions/PoseReceived.hpp: -------------------------------------------------------------------------------- 1 | // Copyright 2022 Fraunhofer FKIE - All Rights Reserved 2 | // 3 | // Licensed under the Apache License, Version 2.0 (the "License"); 4 | // you may not use this file except in compliance with the License. 5 | // You may obtain a copy of the License at 6 | // 7 | // http://www.apache.org/licenses/LICENSE-2.0 8 | // 9 | // Unless required by applicable law or agreed to in writing, software 10 | // distributed under the License is distributed on an "AS IS" BASIS, 11 | // WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 12 | // See the License for the specific language governing permissions and 13 | // limitations under the License. 14 | 15 | #ifndef POSE_RECEIVED_NODE_H 16 | #define POSE_RECEIVED_NODE_H 17 | 18 | #include 19 | #include 20 | #include 21 | #include 22 | #include 23 | #include 24 | 25 | #include 26 | 27 | namespace BehaviorTreeNodes 28 | { 29 | class PoseReceived : public BT::ConditionNode 30 | { 31 | public: 32 | std::string name; 33 | ros::Subscriber sub_pose; 34 | std::string topic_pose; 35 | 36 | geometry_msgs::PoseStamped current_pose; 37 | bool new_pose_arrived = false; 38 | bool invalid_pose_arrived = false; 39 | 40 | bool valid_init = false; 41 | 42 | PoseReceived(const std::string& name, const BT::NodeConfiguration& config); 43 | 44 | static BT::PortsList providedPorts() 45 | { 46 | BT::PortsList ports = { BT::OutputPort("target_pose") }; 47 | return ports; 48 | } 49 | 50 | BT::NodeStatus tick() override; 51 | void callbackPose(const geometry_msgs::PoseStamped& msg); 52 | }; 53 | 54 | inline BT::NodeStatus PoseReceived::tick() 55 | { 56 | setStatus(BT::NodeStatus::RUNNING); 57 | 58 | if (!valid_init) 59 | return BT::NodeStatus::FAILURE; 60 | 61 | // return failure if invalid pose received 62 | if (invalid_pose_arrived) 63 | { 64 | invalid_pose_arrived = false; 65 | return BT::NodeStatus::FAILURE; 66 | } 67 | 68 | if (new_pose_arrived) 69 | { 70 | Pose2D pose(current_pose); 71 | BT::TreeNode::setOutput("target_pose", pose); 72 | 73 | new_pose_arrived = false; 74 | invalid_pose_arrived = false; 75 | 76 | BT_ROS_DEBUG_STREAM(name << "New goal pose was received."); 77 | return BT::NodeStatus::SUCCESS; 78 | } 79 | else 80 | return BT::NodeStatus::FAILURE; 81 | } 82 | 83 | PoseReceived::PoseReceived(const std::string& _name, const BT::NodeConfiguration& config) 84 | : BT::ConditionNode(_name, config), name("[" + _name + "] ") 85 | { 86 | ros::NodeHandle private_node("~"); 87 | ros::NodeHandle public_node; 88 | 89 | std::string parameter_topic_pose = "PoseReceived/" + _name + "_topic_pose"; 90 | 91 | private_node.param(parameter_topic_pose, topic_pose, "pose_in"); 92 | 93 | if (topic_pose.empty()) 94 | { 95 | BT_ROS_ERROR_STREAM(name << " [topic_pose] is empty, check parameter [" << parameter_topic_pose << "]"); 96 | return; 97 | } 98 | 99 | sub_pose = public_node.subscribe(topic_pose, 1, &PoseReceived::callbackPose, this); 100 | valid_init = true; 101 | 102 | BT_ROS_INFO_STREAM(name << " initialized"); 103 | } 104 | 105 | void PoseReceived::callbackPose(const geometry_msgs::PoseStamped& msg) 106 | { 107 | if (msg.header.frame_id.empty()) 108 | { 109 | BT_ROS_WARN_STREAM(name << "pose message with empty frame_id"); 110 | new_pose_arrived = false; 111 | invalid_pose_arrived = true; 112 | return; 113 | } 114 | 115 | if (std::isnan(msg.pose.position.x) || std::isnan(msg.pose.position.y) || std::isnan(msg.pose.position.z)) 116 | { 117 | BT_ROS_WARN_STREAM(name << "invalid pose received"); 118 | new_pose_arrived = false; 119 | invalid_pose_arrived = true; 120 | return; 121 | } 122 | 123 | current_pose = msg; 124 | new_pose_arrived = true; 125 | BT_ROS_DEBUG_STREAM(name << " New message received with frame [" << msg.header.frame_id << "]"); 126 | } 127 | 128 | } // namespace BehaviorTreeNodes 129 | 130 | #endif // POSE_RECEIVED_NODE_H -------------------------------------------------------------------------------- /fkie_behavior_tree_manager/include/fkie_behavior_tree_manager/conditions/PoseStampedReceived.hpp: -------------------------------------------------------------------------------- 1 | // Copyright 2022 Fraunhofer FKIE - All Rights Reserved 2 | // 3 | // Licensed under the Apache License, Version 2.0 (the "License"); 4 | // you may not use this file except in compliance with the License. 5 | // You may obtain a copy of the License at 6 | // 7 | // http://www.apache.org/licenses/LICENSE-2.0 8 | // 9 | // Unless required by applicable law or agreed to in writing, software 10 | // distributed under the License is distributed on an "AS IS" BASIS, 11 | // WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 12 | // See the License for the specific language governing permissions and 13 | // limitations under the License. 14 | 15 | #ifndef POSE_STAMPED_RECEIVED_NODE_H 16 | #define POSE_STAMPED_RECEIVED_NODE_H 17 | 18 | #include 19 | #include 20 | #include 21 | #include 22 | #include 23 | #include 24 | 25 | #include "behaviortree_cpp_v3/behavior_tree.h" 26 | #include "behaviortree_cpp_v3/bt_factory.h" 27 | #include 28 | 29 | namespace BehaviorTreeNodes 30 | { 31 | class PoseStampedReceived : public BT::ConditionNode 32 | { 33 | public: 34 | std::string name; 35 | ros::Subscriber sub_pose; 36 | std::string topic_pose; 37 | 38 | geometry_msgs::PoseStamped current_pose; 39 | bool new_pose_arrived = false; 40 | 41 | bool valid_init = false; 42 | 43 | PoseStampedReceived(const std::string& name, const BT::NodeConfiguration& config); 44 | 45 | static BT::PortsList providedPorts() 46 | { 47 | BT::PortsList ports = { BT::OutputPort("target_pose_3d") }; 48 | return ports; 49 | } 50 | 51 | BT::NodeStatus tick() override; 52 | void callbackPose(const geometry_msgs::PoseStamped& msg); 53 | }; 54 | 55 | inline BT::NodeStatus PoseStampedReceived::tick() 56 | { 57 | setStatus(BT::NodeStatus::RUNNING); 58 | 59 | if (!valid_init) 60 | return BT::NodeStatus::FAILURE; 61 | 62 | if (new_pose_arrived) 63 | { 64 | auto res = BT::TreeNode::setOutput("target_pose_3d", current_pose); 65 | if (!res) 66 | { 67 | BT_ROS_ERROR_STREAM(name << "Could not set output target_pose: " << res.error()); 68 | return BT::NodeStatus::FAILURE; 69 | } 70 | 71 | new_pose_arrived = false; 72 | 73 | std::cout << "New pose_stamped was received." << std::endl; 74 | BT_ROS_DEBUG_STREAM(name << "New goal pose was received."); 75 | return BT::NodeStatus::SUCCESS; 76 | } 77 | else 78 | return BT::NodeStatus::FAILURE; 79 | } 80 | 81 | PoseStampedReceived::PoseStampedReceived(const std::string& _name, const BT::NodeConfiguration& config) 82 | : BT::ConditionNode(_name, config), name("[" + _name + "] ") 83 | { 84 | ros::NodeHandle private_node("~"); 85 | ros::NodeHandle public_node; 86 | 87 | std::string parameter_topic_pose = "PoseStampedReceived/" + _name + "_topic_pose"; 88 | 89 | private_node.param(parameter_topic_pose, topic_pose, "pose_in"); 90 | 91 | if (topic_pose.empty()) 92 | { 93 | BT_ROS_ERROR_STREAM(name << " [topic_pose] is empty, check parameter [" << parameter_topic_pose << "]"); 94 | return; 95 | } 96 | 97 | sub_pose = public_node.subscribe(topic_pose, 1, &PoseStampedReceived::callbackPose, this); 98 | valid_init = true; 99 | 100 | BT_ROS_INFO_STREAM(name << " initialized"); 101 | } 102 | 103 | void PoseStampedReceived::callbackPose(const geometry_msgs::PoseStamped& msg) 104 | { 105 | if (msg.header.frame_id.empty()) 106 | { 107 | BT_ROS_WARN_STREAM(name << "ignoring message with empty frame_id"); 108 | return; 109 | } 110 | 111 | if (std::isnan(msg.pose.position.x) || std::isnan(msg.pose.position.y) || std::isnan(msg.pose.position.z)) 112 | { 113 | BT_ROS_WARN_STREAM(name << "invalid pose received, ignoring."); 114 | return; 115 | } 116 | 117 | current_pose = msg; 118 | new_pose_arrived = true; 119 | BT_ROS_DEBUG_STREAM(name << " New message received with frame [" << msg.header.frame_id << "]"); 120 | } 121 | 122 | } // namespace BehaviorTreeNodes 123 | 124 | #endif // POSE_STAMPED_RECEIVED_NODE_H 125 | -------------------------------------------------------------------------------- /fkie_behavior_tree_manager/include/fkie_behavior_tree_manager/actions/BoolAction.hpp: -------------------------------------------------------------------------------- 1 | // Copyright 2022 Fraunhofer FKIE - All Rights Reserved 2 | // 3 | // Licensed under the Apache License, Version 2.0 (the "License"); 4 | // you may not use this file except in compliance with the License. 5 | // You may obtain a copy of the License at 6 | // 7 | // http://www.apache.org/licenses/LICENSE-2.0 8 | // 9 | // Unless required by applicable law or agreed to in writing, software 10 | // distributed under the License is distributed on an "AS IS" BASIS, 11 | // WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 12 | // See the License for the specific language governing permissions and 13 | // limitations under the License. 14 | 15 | #ifndef BOOL_ACTION_NODE_H 16 | #define BOOL_ACTION_NODE_H 17 | 18 | #include 19 | #include 20 | #include 21 | #include 22 | #include 23 | 24 | namespace BehaviorTreeNodes 25 | { 26 | class BoolAction : public BT::ActionNodeBase 27 | { 28 | public: 29 | std::string name; 30 | ros::Subscriber sub_bool; 31 | ros::Publisher pub_sub; 32 | std::string topic_in, topic_out; 33 | float timeout; 34 | bool valid_init = false; 35 | bool message_received = false; 36 | bool result_action = false; 37 | 38 | static BT::PortsList providedPorts() 39 | { 40 | BT::PortsList ports; 41 | return ports; 42 | } 43 | 44 | BoolAction(const std::string& _name, const BT::NodeConfiguration& config) 45 | : BT::ActionNodeBase(_name, config), name("[" + _name + "] ") 46 | { 47 | ros::NodeHandle private_node("~"); 48 | ros::NodeHandle public_node; 49 | 50 | std::string parameter_topic_in = "BoolAction/" + _name + "_topic_in"; 51 | std::string parameter_topic_out = "BoolAction/" + _name + "_topic_out"; 52 | std::string parameter_timeout = "BoolAction/" + _name + "_timeout"; 53 | 54 | private_node.param(parameter_topic_in, topic_in, std::string("")); 55 | private_node.param(parameter_topic_out, topic_out, std::string("")); 56 | private_node.param(parameter_timeout, timeout, -1.0f); 57 | 58 | if (topic_in.empty() || topic_out.empty()) 59 | { 60 | BT_ROS_ERROR_STREAM(name << " (in/out) topic are empty. Check parameters: [" << topic_in << "] and [" << topic_out 61 | << "]"); 62 | return; 63 | } 64 | 65 | pub_sub = public_node.advertise(topic_out, 10, false); 66 | sub_bool = public_node.subscribe(topic_in, 5, &BoolAction::callbackBoolAction, this); 67 | valid_init = true; 68 | 69 | BT_ROS_INFO_STREAM(name << " initialized"); 70 | } 71 | 72 | BT::NodeStatus tick() override 73 | { 74 | setStatus(BT::NodeStatus::RUNNING); 75 | 76 | if (!valid_init) 77 | return BT::NodeStatus::FAILURE; 78 | 79 | message_received = false; 80 | result_action = false; 81 | 82 | std_msgs::Bool msg; 83 | msg.data = true; 84 | pub_sub.publish(msg); 85 | ros::spinOnce(); 86 | 87 | ros::Time begin = ros::Time::now(); 88 | while (ros::ok() && !message_received) 89 | { 90 | ros::spinOnce(); 91 | 92 | // timeout was defined 93 | if (timeout > 0) 94 | { 95 | // waiting for message or timeout 96 | ros::Duration waiting_time = ros::Time::now() - begin; 97 | if (waiting_time.toSec() >= timeout) 98 | { 99 | BT_ROS_WARN_STREAM("BoolAction: Timeout reached, assume the action has failed!"); 100 | result_action = false; 101 | break; 102 | } 103 | } 104 | 105 | ros::Duration(0.05).sleep(); 106 | } 107 | 108 | if (result_action) 109 | return BT::NodeStatus::SUCCESS; 110 | else 111 | return BT::NodeStatus::FAILURE; 112 | } 113 | 114 | void callbackBoolAction(const std_msgs::Bool& msg) 115 | { 116 | message_received = true; 117 | result_action = msg.data; 118 | } 119 | 120 | void halt() override 121 | { 122 | message_received = true; 123 | result_action = false; 124 | setStatus(BT::NodeStatus::IDLE); 125 | } 126 | }; 127 | 128 | } // namespace BehaviorTreeNodes 129 | 130 | #endif // BOOL_ACTION_NODE_H -------------------------------------------------------------------------------- /fkie_behavior_tree_manager/include/fkie_behavior_tree_manager/BTPublisherRosSingleton.hpp: -------------------------------------------------------------------------------- 1 | // Copyright 2022 Fraunhofer FKIE - All Rights Reserved 2 | // 3 | // Licensed under the Apache License, Version 2.0 (the "License"); 4 | // you may not use this file except in compliance with the License. 5 | // You may obtain a copy of the License at 6 | // 7 | // http://www.apache.org/licenses/LICENSE-2.0 8 | // 9 | // Unless required by applicable law or agreed to in writing, software 10 | // distributed under the License is distributed on an "AS IS" BASIS, 11 | // WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 12 | // See the License for the specific language governing permissions and 13 | // limitations under the License. 14 | 15 | #ifndef BT_PUBLISHER_ROS_SINGLETONE_H_ 16 | #define BT_PUBLISHER_ROS_SINGLETONE_H_ 17 | 18 | #include 19 | #include 20 | #include 21 | #include 22 | 23 | #include 24 | #include 25 | #include 26 | #include 27 | 28 | #include 29 | 30 | class BTPublisherRosSingleton 31 | { 32 | private: 33 | inline static BTPublisherRosSingleton* pinstance_{ nullptr }; 34 | inline static std::mutex mutex_; 35 | 36 | ros::NodeHandle public_node; 37 | ros::Publisher pub; 38 | bool enable_publisher = false; 39 | 40 | std::mutex mutex_pub; 41 | 42 | protected: 43 | BTPublisherRosSingleton() 44 | { 45 | enablePublisher(); 46 | } 47 | ~BTPublisherRosSingleton() 48 | { 49 | disablePublisher(); 50 | } 51 | 52 | public: 53 | BTPublisherRosSingleton(BTPublisherRosSingleton& other) = delete; // should not be cloneable. 54 | void operator=(const BTPublisherRosSingleton&) = delete; // should not be assignable. 55 | 56 | static BTPublisherRosSingleton* GetInstance() 57 | { 58 | std::lock_guard lock(mutex_); 59 | if (pinstance_ == nullptr) 60 | { 61 | pinstance_ = new BTPublisherRosSingleton(); 62 | } 63 | return pinstance_; 64 | } 65 | 66 | /** 67 | * @brief Enable ROS publisher for fixed topic [bt_logging] from type [std_msgs::String] 68 | */ 69 | void enablePublisher() 70 | { 71 | pub = public_node.advertise("bt_logging", 5); 72 | enable_publisher = true; 73 | } 74 | 75 | /** 76 | * @brief Enable ROS publisher for fixed topic [bt_logging] from type [std_msgs::String] 77 | */ 78 | void disablePublisher() 79 | { 80 | pub.shutdown(); 81 | enable_publisher = false; 82 | } 83 | 84 | /** 85 | * @brief Publish [text] to an [std_msgs::String] message in ROS 86 | */ 87 | void publish(const ros::console::levels::Level level, const std::string& text) 88 | { 89 | if (!enable_publisher) 90 | return; 91 | 92 | if (pub.getNumSubscribers() == 0) 93 | return; 94 | 95 | // TODO: Check if required 96 | if (level == ros::console::levels::Level::Debug) 97 | return; 98 | 99 | { 100 | const std::lock_guard lock(mutex_pub); 101 | fkie_behavior_tree_msgs::BTLogging msg; 102 | msg.header.stamp = ros::Time::now(); 103 | msg.source = public_node.getNamespace(); 104 | msg.level = levelToString(level); 105 | msg.data = text; 106 | pub.publish(msg); 107 | } 108 | } 109 | 110 | /** 111 | * @brief Publish [stream] to an [std_msgs::String] message in ROS 112 | */ 113 | void publishStream(const ros::console::levels::Level level, const std::stringstream& stream) 114 | { 115 | publish(level, stream.str()); 116 | } 117 | 118 | std::string levelToString(const ros::console::levels::Level& level) const 119 | { 120 | switch (level) 121 | { 122 | case ros::console::levels::Level::Debug: 123 | return "DEBUG"; 124 | break; 125 | 126 | case ros::console::levels::Level::Info: 127 | return "INFO"; 128 | break; 129 | 130 | case ros::console::levels::Level::Warn: 131 | return "WARN"; 132 | break; 133 | 134 | case ros::console::levels::Level::Error: 135 | return "ERROR"; 136 | break; 137 | 138 | case ros::console::levels::Level::Fatal: 139 | return "FATAL"; 140 | break; 141 | 142 | default: 143 | return "UNKNOWN"; 144 | break; 145 | } 146 | } 147 | }; 148 | 149 | #endif /* BT_PUBLISHER_ROS_SINGLETONE_H_ */ 150 | -------------------------------------------------------------------------------- /fkie_bt_move_base_actions/src/Recovery.cpp: -------------------------------------------------------------------------------- 1 | // Copyright 2022 Fraunhofer FKIE - All Rights Reserved 2 | // 3 | // Licensed under the Apache License, Version 2.0 (the "License"); 4 | // you may not use this file except in compliance with the License. 5 | // You may obtain a copy of the License at 6 | // 7 | // http://www.apache.org/licenses/LICENSE-2.0 8 | // 9 | // Unless required by applicable law or agreed to in writing, software 10 | // distributed under the License is distributed on an "AS IS" BASIS, 11 | // WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 12 | // See the License for the specific language governing permissions and 13 | // limitations under the License. 14 | 15 | #include "Recovery.h" 16 | 17 | namespace MoveBaseActionsGroup 18 | { 19 | Recovery::Recovery(const std::string& _name, const BT::NodeConfiguration& config) 20 | : BT::ActionNodeBase(_name, config), name("[" + _name + "] ") 21 | { 22 | ros::NodeHandle private_node("~"); 23 | 24 | private_node.param("MoveBaseActions/topic_recovery_action", topic_recovery_action, std::string("")); 25 | if (topic_recovery_action.empty()) 26 | { 27 | BT_ROS_ERROR_STREAM(name << "Missing param: [MoveBaseActions/topic_recovery_action]"); 28 | return; 29 | } 30 | 31 | ac = std::make_unique(topic_recovery_action, true); 32 | BT_ROS_INFO_STREAM(name << "Waiting for action server [" << topic_recovery_action << "]"); 33 | ac->waitForServer(); 34 | BT_ROS_INFO_STREAM(name << "Action client started"); 35 | 36 | init = true; 37 | } 38 | 39 | Recovery::~Recovery() 40 | { 41 | if (ac && ac->isServerConnected()) 42 | ac->cancelAllGoals(); 43 | } 44 | 45 | BT::NodeStatus Recovery::tick() 46 | { 47 | setStatus(BT::NodeStatus::RUNNING); 48 | 49 | if (!init) 50 | { 51 | BT_ROS_ERROR_STREAM(name << "not initialized!"); 52 | return BT::NodeStatus::FAILURE; 53 | } 54 | 55 | if (!ac->isServerConnected()) 56 | { 57 | BT_ROS_ERROR("Action server is not connected"); 58 | return BT::NodeStatus::FAILURE; 59 | } 60 | 61 | std::string strategy; 62 | 63 | auto res = BT::TreeNode::getInput("strategy", strategy); 64 | if (!res) 65 | { 66 | BT_ROS_ERROR_STREAM(name << "Could not load [strategy]: " << res.error()); 67 | return BT::NodeStatus::FAILURE; 68 | } 69 | 70 | if (strategy.empty()) 71 | { 72 | BT_ROS_INFO_STREAM(name << "No valid strategy was found"); 73 | return BT::NodeStatus::FAILURE; 74 | } 75 | 76 | exe_result.store(false); 77 | 78 | BT_ROS_INFO_STREAM(name << "starting recovery: [" << strategy << "]"); 79 | mbf_msgs::RecoveryGoal goal_action; 80 | goal_action.behavior = strategy; 81 | 82 | if (ac) 83 | { 84 | ac->sendGoal(goal_action, boost::bind(&Recovery::doneCallback, this, _1, _2), 85 | boost::bind(&Recovery::activeCallback, this), boost::bind(&Recovery::feedbackCallback, this)); 86 | 87 | ac->waitForResult(); 88 | } 89 | 90 | BT_ROS_DEBUG_STREAM(name << "Finishing with exe_result: [" << exe_result << "]"); 91 | return exe_result ? BT::NodeStatus::SUCCESS : BT::NodeStatus::FAILURE; 92 | } 93 | 94 | void Recovery::halt() 95 | { 96 | exe_result.store(false); 97 | if (ac) 98 | ac->cancelAllGoals(); 99 | setStatus(BT::NodeStatus::FAILURE); 100 | } 101 | 102 | void Recovery::doneCallback(const actionlib::SimpleClientGoalState& state, 103 | const mbf_msgs::RecoveryResultConstPtr& result) 104 | { 105 | mbf_msgs::RecoveryResult r = *result; 106 | if (state != actionlib::SimpleClientGoalState::LOST) 107 | { 108 | if (r.outcome == mbf_msgs::RecoveryResult::SUCCESS) 109 | { 110 | exe_result.store(true); 111 | BT_ROS_INFO_STREAM(name << "Recovery successfully executed: [" << r.used_plugin << "] message: " << r.message); 112 | } 113 | else 114 | { 115 | exe_result.store(false); 116 | BT_ROS_INFO_STREAM(name << "Recovery failed: [" << r.used_plugin << "] message: " << r.message); 117 | } 118 | } 119 | else 120 | { 121 | BT_ROS_ERROR_STREAM("Transition to LOST. Check if Action server [" << topic_recovery_action << "] is running."); 122 | } 123 | } 124 | 125 | void Recovery::activeCallback() 126 | { 127 | // ROS_INFO("Recovery::activeCallback"); 128 | } 129 | 130 | void Recovery::feedbackCallback() 131 | { 132 | BT_ROS_DEBUG_STREAM("Not Implemented: Recovery::feedbackCallback"); 133 | } 134 | } // namespace MoveBaseActionsGroup 135 | -------------------------------------------------------------------------------- /fkie_behavior_tree_manager/include/fkie_behavior_tree_manager/actions/Relative3DPoseAction.hpp: -------------------------------------------------------------------------------- 1 | // Copyright 2022 Fraunhofer FKIE - All Rights Reserved 2 | // 3 | // Licensed under the Apache License, Version 2.0 (the "License"); 4 | // you may not use this file except in compliance with the License. 5 | // You may obtain a copy of the License at 6 | // 7 | // http://www.apache.org/licenses/LICENSE-2.0 8 | // 9 | // Unless required by applicable law or agreed to in writing, software 10 | // distributed under the License is distributed on an "AS IS" BASIS, 11 | // WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 12 | // See the License for the specific language governing permissions and 13 | // limitations under the License. 14 | 15 | #ifndef RELATIVE_3D_POSE_ACTION_H 16 | #define RELATIVE_3D_POSE_ACTION_H 17 | 18 | #include 19 | #include 20 | #include 21 | #include 22 | #include 23 | #include 24 | #include 25 | #include 26 | 27 | namespace BehaviorTreeNodes 28 | { 29 | class Relative3DPoseAction : public BT::SyncActionNode 30 | { 31 | public: 32 | std::string name; 33 | ros::Publisher pub_3d; 34 | std::string relative_frame; 35 | 36 | std::unique_ptr tf2_listener; 37 | tf2_ros::Buffer tf2_buffer; 38 | 39 | public: 40 | static BT::PortsList providedPorts() 41 | { 42 | BT::PortsList ports = { BT::BidirectionalPort("target_pose_3d") }; 43 | return ports; 44 | } 45 | 46 | Relative3DPoseAction(const std::string& _name, const BT::NodeConfiguration& config) 47 | : BT::SyncActionNode(_name, config), name("[" + _name + "] ") 48 | { 49 | ros::NodeHandle public_node; 50 | ros::NodeHandle private_node("~"); 51 | 52 | // Publish relative 3d pose 53 | pub_3d = public_node.advertise("relative_pose_3d", 10, false); 54 | 55 | // Get the relative frame 56 | std::string parameter_relative_frame = "Relative3DPoseAction/" + _name + "_relative_frame"; 57 | private_node.param(parameter_relative_frame, relative_frame, std::string("")); 58 | 59 | tf2_listener = std::make_unique(tf2_buffer); 60 | 61 | BT_ROS_INFO_STREAM(name << " initialized"); 62 | } 63 | 64 | inline BT::NodeStatus tick() override 65 | { 66 | setStatus(BT::NodeStatus::RUNNING); 67 | 68 | // Validate the relative frame 69 | if (relative_frame.empty()) 70 | { 71 | BT_ROS_ERROR_STREAM(name << " relative_frame is empty"); 72 | return BT::NodeStatus::FAILURE; 73 | } 74 | 75 | // Validate 3D goal 76 | geometry_msgs::PoseStamped target_pose_3d; 77 | auto res = BT::TreeNode::getInput("target_pose_3d", target_pose_3d); 78 | if (!res) 79 | { 80 | BT_ROS_ERROR_STREAM(name << " Could not load target_pose_3d, resetting variable. Error: " << res.error()); 81 | BT::TreeNode::setOutput("target_pose_3d", geometry_msgs::PoseStamped()); 82 | return BT::NodeStatus::FAILURE; 83 | } 84 | 85 | target_pose_3d.header.stamp = ros::Time::now(); 86 | 87 | // compute relative pose 88 | geometry_msgs::PoseStamped relative_pose; 89 | tf2_buffer.transform(target_pose_3d, relative_pose, relative_frame, ros::Duration(0.5)); 90 | 91 | BT_ROS_WARN_STREAM(name << " Frame id for relative pose: " << relative_pose.header.frame_id); 92 | BT_ROS_WARN_STREAM(name << " Received GOAL 3D pose: " << target_pose_3d.pose.position.x << ", " 93 | << target_pose_3d.pose.position.y << ", " << target_pose_3d.pose.position.z); 94 | BT_ROS_WARN_STREAM(name << " Transformed Relative 3D pose: " << relative_pose.pose.position.x << ", " 95 | << relative_pose.pose.position.y << ", " << relative_pose.pose.position.z); 96 | 97 | pub_3d.publish(relative_pose); 98 | 99 | // save relative_pose to blackboard variable 100 | res = BT::TreeNode::setOutput("target_pose_3d", relative_pose); 101 | if (!res) 102 | { 103 | BT_ROS_ERROR_STREAM(name << " Could not set output target_pose_2d: " << res.error()); 104 | return BT::NodeStatus::FAILURE; 105 | } 106 | 107 | return BT::NodeStatus::SUCCESS; 108 | } 109 | }; 110 | 111 | } // namespace BehaviorTreeNodes 112 | #endif // RELATIVE_3D_POSE_ACTION_H -------------------------------------------------------------------------------- /fkie_behavior_tree_manager/include/fkie_behavior_tree_manager/actions/WaitForGenericMsgAsyncAction.hpp: -------------------------------------------------------------------------------- 1 | // Copyright 2022 Fraunhofer FKIE - All Rights Reserved 2 | // 3 | // Licensed under the Apache License, Version 2.0 (the "License"); 4 | // you may not use this file except in compliance with the License. 5 | // You may obtain a copy of the License at 6 | // 7 | // http://www.apache.org/licenses/LICENSE-2.0 8 | // 9 | // Unless required by applicable law or agreed to in writing, software 10 | // distributed under the License is distributed on an "AS IS" BASIS, 11 | // WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 12 | // See the License for the specific language governing permissions and 13 | // limitations under the License. 14 | 15 | #ifndef WAIT_FOR_GENERIC_MSG_ASYNC_ACTION_NODE_H 16 | #define WAIT_FOR_GENERIC_MSG_ASYNC_ACTION_NODE_H 17 | 18 | #include 19 | #include 20 | 21 | #include 22 | #include 23 | 24 | #include 25 | 26 | namespace BehaviorTreeNodes 27 | { 28 | /** 29 | * @brief Asynchronously wait until a generic message is received, return RUNNING while waiting 30 | * ROS Params: 31 | * WaitForGenericMsgAsyncAction/[name]_topic_in: Input topic for generic message (any) 32 | * WaitForGenericMsgAsyncAction/[name]_timeout: Timeout to return FAILURE, (-1.0 disable) 33 | */ 34 | class WaitForGenericMsgAsyncAction : public BT::CoroActionNode 35 | { 36 | public: 37 | std::string name; 38 | ros::Subscriber sub; 39 | std::string topic_in; 40 | float timeout = -1.0f; 41 | bool valid_init = false; 42 | bool message_received = false; 43 | 44 | static BT::PortsList providedPorts() 45 | { 46 | BT::PortsList ports; 47 | return ports; 48 | } 49 | 50 | WaitForGenericMsgAsyncAction(const std::string& _name, const BT::NodeConfiguration& config) 51 | : BT::CoroActionNode(_name, config), name("[" + _name + "] ") 52 | { 53 | ros::NodeHandle private_node("~"); 54 | ros::NodeHandle public_node; 55 | 56 | std::string parameter_topic_in = "WaitForGenericMsgAsyncAction/" + _name + "_topic_in"; 57 | std::string parameter_timeout = "WaitForGenericMsgAsyncAction/" + _name + "_timeout"; 58 | 59 | private_node.param(parameter_topic_in, topic_in, topic_in); 60 | private_node.param(parameter_timeout, timeout, timeout); 61 | 62 | if (topic_in.empty()) 63 | { 64 | BT_ROS_ERROR_STREAM(name << " (in) topic is empty. Check parameter: [" << topic_in << "]"); 65 | return; 66 | } 67 | 68 | boost::function callback; 69 | callback = [&](const topic_tools::ShapeShifter::ConstPtr& msg) -> void { topicCallback(msg, topic_in); }; 70 | sub = public_node.subscribe(topic_in, 10, callback); 71 | 72 | valid_init = true; 73 | 74 | BT_ROS_INFO_STREAM(name << " initialized"); 75 | } 76 | 77 | BT::NodeStatus tick() override 78 | { 79 | setStatus(BT::NodeStatus::RUNNING); 80 | 81 | if (!valid_init) 82 | return BT::NodeStatus::FAILURE; 83 | 84 | message_received = false; 85 | 86 | ros::spinOnce(); 87 | 88 | ros::Time begin = ros::Time::now(); 89 | while (ros::ok() && !message_received) 90 | { 91 | ros::spinOnce(); 92 | ros::Duration(0.05).sleep(); 93 | 94 | // timeout was defined 95 | if (timeout > 0) 96 | { 97 | // waiting for message or timeout 98 | ros::Duration waiting_time = ros::Time::now() - begin; 99 | if (waiting_time.toSec() >= timeout) 100 | { 101 | BT_ROS_WARN_STREAM(name << "Timeout reached, assume the action has failed!"); 102 | return BT::NodeStatus::FAILURE; 103 | } 104 | } 105 | 106 | // set status to RUNNING and "pause/sleep" 107 | // If halt() is called, we will not resume execution (stack destroyed) 108 | setStatusRunningAndYield(); 109 | } 110 | 111 | message_received = false; 112 | return BT::NodeStatus::SUCCESS; 113 | } 114 | 115 | void topicCallback(const topic_tools::ShapeShifter::ConstPtr& msg, const std::string& topic_name) 116 | { 117 | message_received = true; 118 | } 119 | 120 | void halt() override 121 | { 122 | BT_ROS_DEBUG_STREAM(name << "node has been halted!"); 123 | message_received = true; 124 | setStatus(BT::NodeStatus::FAILURE); 125 | 126 | CoroActionNode::halt(); 127 | } 128 | }; 129 | 130 | } // namespace BehaviorTreeNodes 131 | 132 | #endif // WAIT_FOR_GENERIC_MSG_ASYNC_ACTION_NODE_H -------------------------------------------------------------------------------- /fkie_bt_move_base_actions/src/NotifyNavigationResult.h: -------------------------------------------------------------------------------- 1 | // Copyright 2022 Fraunhofer FKIE - All Rights Reserved 2 | // 3 | // Licensed under the Apache License, Version 2.0 (the "License"); 4 | // you may not use this file except in compliance with the License. 5 | // You may obtain a copy of the License at 6 | // 7 | // http://www.apache.org/licenses/LICENSE-2.0 8 | // 9 | // Unless required by applicable law or agreed to in writing, software 10 | // distributed under the License is distributed on an "AS IS" BASIS, 11 | // WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 12 | // See the License for the specific language governing permissions and 13 | // limitations under the License. 14 | 15 | #ifndef MOVE_BASE_NOTIFY_NAVIGATION_RESULT_H 16 | #define MOVE_BASE_NOTIFY_NAVIGATION_RESULT_H 17 | 18 | #include 19 | 20 | #include "behaviortree_cpp_v3/behavior_tree.h" 21 | #include "behaviortree_cpp_v3/bt_factory.h" 22 | 23 | #include 24 | #include 25 | #include 26 | 27 | namespace MoveBaseActionsGroup 28 | { 29 | /* Error codes defined by Move Base Flex messages 30 | uint8 SUCCESS = 0 31 | 32 | Global path planning errors: 33 | uint8 FAILURE = 50 # Unspecified failure, only used for old, non-mfb_core based plugins 34 | uint8 CANCELED = 51 # The action has been canceled by a action client 35 | uint8 INVALID_START = 52 # 36 | uint8 INVALID_GOAL = 53 37 | uint8 NO_PATH_FOUND = 54 38 | uint8 PAT_EXCEEDED = 55 39 | uint8 EMPTY_PATH = 56 40 | uint8 TF_ERROR = 57 41 | uint8 NOT_INITIALIZED = 58 42 | uint8 INVALID_PLUGIN = 59 43 | uint8 INTERNAL_ERROR = 60 44 | uint8 OUT_OF_MAP = 61 45 | uint8 MAP_ERROR = 62 46 | uint8 STOPPED = 63 # The planner execution has been stopped rigorously. 47 | # 71..99 are reserved as plugin specific errors 48 | 49 | Local navigation errors: 50 | uint8 FAILURE = 100 # Unspecified failure, only used for old, non-mfb_core based plugins 51 | uint8 CANCELED = 101 52 | uint8 NO_VALID_CMD = 102 53 | uint8 PAT_EXCEEDED = 103 54 | uint8 COLLISION = 104 55 | uint8 OSCILLATION = 105 56 | uint8 ROBOT_STUCK = 106 57 | uint8 MISSED_GOAL = 107 58 | uint8 MISSED_PATH = 108 59 | uint8 BLOCKED_PATH = 109 60 | uint8 INVALID_PATH = 110 61 | uint8 TF_ERROR = 111 62 | uint8 NOT_INITIALIZED = 112 63 | uint8 INVALID_PLUGIN = 113 64 | uint8 INTERNAL_ERROR = 114 65 | uint8 OUT_OF_MAP = 115 # The start and / or the goal are outside the map 66 | uint8 MAP_ERROR = 116 # The map is not running properly 67 | uint8 STOPPED = 117 # The controller execution has been stopped rigorously. 68 | # 121..149 are reserved as plugin specific errors 69 | 70 | Recovery errors: 71 | uint8 FAILURE = 150 72 | uint8 CANCELED = 151 73 | uint8 PAT_EXCEEDED = 152 74 | uint8 TF_ERROR = 153 75 | uint8 NOT_INITIALIZED = 154 76 | uint8 INVALID_PLUGIN = 155 77 | uint8 INTERNAL_ERROR = 156 78 | uint8 STOPPED = 157 # The recovery behaviour execution has been stopped rigorously. 79 | uint8 IMPASSABLE = 158 # Further execution would lead to a collision 80 | # 171..199 are reserved as plugin specific errors 81 | 82 | Move base interface errors: 83 | uint8 FAILURE = 10 84 | uint8 CANCELED = 11 85 | uint8 COLLISION = 12 86 | uint8 OSCILLATION = 13 87 | uint8 START_BLOCKED = 14 88 | uint8 GOAL_BLOCKED = 15 89 | uint8 TF_ERROR = 16 90 | uint8 INTERNAL_ERROR = 17 91 | # 21..49 are reserved for future general error codes 92 | */ 93 | class NotifyNavigationResult : public BT::SyncActionNode 94 | { 95 | public: 96 | bool is_initialized = false; 97 | std::string topic_notify_result; 98 | 99 | NotifyNavigationResult(const std::string& name, const BT::NodeConfiguration& config); 100 | ~NotifyNavigationResult(); 101 | BT::NodeStatus tick() override; 102 | 103 | static BT::PortsList providedPorts() 104 | { 105 | BT::PortsList ports = { BT::BidirectionalPort("nav_error_code") }; 106 | return ports; 107 | } 108 | 109 | void updateErrorDescriptionAndOrigin(fkie_bt_move_base_actions::NavigationResult& nr) const; 110 | void set(fkie_bt_move_base_actions::NavigationResult& nr, std::string origin, std::string description) const; 111 | 112 | private: 113 | ros::Publisher pub_notify; 114 | }; 115 | } // namespace MoveBaseActionsGroup 116 | #endif // MOVE_BASE_NOTIFY_NAVIGATION_RESULT_H 117 | -------------------------------------------------------------------------------- /fkie_bt_move_base_actions/src/NavigateToGoalSync.cpp: -------------------------------------------------------------------------------- 1 | // Copyright 2022 Fraunhofer FKIE - All Rights Reserved 2 | // 3 | // Licensed under the Apache License, Version 2.0 (the "License"); 4 | // you may not use this file except in compliance with the License. 5 | // You may obtain a copy of the License at 6 | // 7 | // http://www.apache.org/licenses/LICENSE-2.0 8 | // 9 | // Unless required by applicable law or agreed to in writing, software 10 | // distributed under the License is distributed on an "AS IS" BASIS, 11 | // WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 12 | // See the License for the specific language governing permissions and 13 | // limitations under the License. 14 | 15 | #include "NavigateToGoalSync.h" 16 | 17 | namespace MoveBaseActionsGroup 18 | { 19 | NavigateToGoalSync::NavigateToGoalSync(const std::string& name, const BT::NodeConfiguration& config) 20 | : BT::ActionNodeBase(name, config) 21 | { 22 | ros::NodeHandle public_node(""); 23 | ros::NodeHandle private_node("~"); 24 | 25 | private_node.param("MoveBaseActions/topic_notify_result", topic_notify_result, std::string("navigation_result")); 26 | private_node.param("MoveBaseActions/topic_goal_pose_out", topic_goal_pose_out, std::string("goal_pose")); 27 | 28 | if (topic_notify_result.empty()) 29 | { 30 | BT_ROS_ERROR("Missing param [MoveBaseActions/topic_notify_result]"); 31 | return; 32 | } 33 | 34 | if (topic_goal_pose_out.empty()) 35 | { 36 | BT_ROS_ERROR_STREAM("Missing param [MoveBaseActions/topic_goal_pos_out]"); 37 | return; 38 | } 39 | 40 | pub_pose = public_node.advertise(topic_goal_pose_out, 10, false); 41 | sub_notify = public_node.subscribe(topic_notify_result, 1, &NavigateToGoalSync::callbackNavigationResult, this); 42 | 43 | is_initialized = true; 44 | BT_ROS_INFO_STREAM("Successfully initialized"); 45 | } 46 | 47 | NavigateToGoalSync::~NavigateToGoalSync() 48 | { 49 | } 50 | 51 | void NavigateToGoalSync::halt() 52 | { 53 | new_message_received = true; 54 | // force failure 55 | last_msg.nav_error_code = -1; 56 | // TODO: Shall we cancel all goals for global and local planner? 57 | } 58 | 59 | BT::NodeStatus NavigateToGoalSync::tick() 60 | { 61 | setStatus(BT::NodeStatus::RUNNING); 62 | 63 | if (!is_initialized) 64 | { 65 | BT_ROS_ERROR("NavigateToGoalSync is not initialized"); 66 | return BT::NodeStatus::FAILURE; 67 | } 68 | 69 | BT_ROS_DEBUG_STREAM("NavigateToGoalSync::tick()"); 70 | 71 | // send pose to navigation tree -------------------------------------------------------------------------------------- 72 | // get target pose 73 | Pose2D target_pose; 74 | auto res = BT::TreeNode::getInput("target_pose", target_pose); 75 | if (!res) 76 | { 77 | BT_ROS_ERROR_STREAM(" Could not load target_pose, resetting variable. Error: " << res.error()); 78 | BT::TreeNode::setOutput("target_pose", Pose2D()); // reset current target_pose 79 | return BT::NodeStatus::FAILURE; 80 | } 81 | 82 | if (std::isnan(target_pose.x) || std::isnan(target_pose.y) || std::isnan(target_pose.theta)) 83 | { 84 | BT_ROS_ERROR_STREAM("Invalid pose: " << target_pose.toString()); 85 | return BT::NodeStatus::FAILURE; 86 | } 87 | 88 | if (target_pose.frame_id.empty()) 89 | { 90 | BT_ROS_ERROR_STREAM("Empty frame id: " << target_pose.toString()); 91 | return BT::NodeStatus::FAILURE; 92 | } 93 | 94 | // publish target pose 95 | geometry_msgs::PoseStamped pose; 96 | pose = target_pose.toPoseStamped(); 97 | pub_pose.publish(pose); 98 | 99 | new_message_received = false; 100 | 101 | // wait for feedback -------------------------------------------------------------------------------------- 102 | while (ros::ok() && !new_message_received) 103 | { 104 | ros::spinOnce(); 105 | ros::Duration(0.1).sleep(); 106 | } 107 | 108 | new_message_received = false; 109 | 110 | BT_ROS_DEBUG_STREAM("NavigateToGoalSync: Navigation finished"); 111 | 112 | BT::TreeNode::setOutput("nav_error_code", (unsigned int)last_msg.nav_error_code); 113 | BT::TreeNode::setOutput("navigation_result", last_msg); 114 | BT::TreeNode::setOutput("target_pose", Pose2D()); // reset current target_pose 115 | 116 | if (last_msg.nav_error_code == 0) 117 | return BT::NodeStatus::SUCCESS; 118 | else 119 | return BT::NodeStatus::FAILURE; 120 | } 121 | 122 | void NavigateToGoalSync::callbackNavigationResult(const fkie_bt_move_base_actions::NavigationResult& msg) 123 | { 124 | new_message_received = true; 125 | last_msg = msg; 126 | 127 | BT_ROS_DEBUG_STREAM("New callback navigation result: (" << msg.nav_error_code << ") - " << msg.origin << ": " 128 | << msg.description); 129 | } 130 | 131 | } // namespace MoveBaseActionsGroup 132 | -------------------------------------------------------------------------------- /fkie_bt_move_base_actions/src/NavigateToGoal.cpp: -------------------------------------------------------------------------------- 1 | // Copyright 2022 Fraunhofer FKIE - All Rights Reserved 2 | // 3 | // Licensed under the Apache License, Version 2.0 (the "License"); 4 | // you may not use this file except in compliance with the License. 5 | // You may obtain a copy of the License at 6 | // 7 | // http://www.apache.org/licenses/LICENSE-2.0 8 | // 9 | // Unless required by applicable law or agreed to in writing, software 10 | // distributed under the License is distributed on an "AS IS" BASIS, 11 | // WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 12 | // See the License for the specific language governing permissions and 13 | // limitations under the License. 14 | 15 | #include "NavigateToGoal.h" 16 | 17 | namespace MoveBaseActionsGroup 18 | { 19 | NavigateToGoal::NavigateToGoal(const std::string& name, const BT::NodeConfiguration& config) 20 | : BT::CoroActionNode(name, config) 21 | { 22 | ros::NodeHandle public_node(""); 23 | ros::NodeHandle private_node("~"); 24 | 25 | private_node.param("MoveBaseActions/topic_notify_result", topic_notify_result, std::string("navigation_result")); 26 | private_node.param("MoveBaseActions/topic_goal_pose_out", topic_goal_pose_out, std::string("goal_pose")); 27 | 28 | if (topic_notify_result.empty()) 29 | { 30 | BT_ROS_ERROR("Missing param [MoveBaseActions/topic_notify_result]"); 31 | return; 32 | } 33 | 34 | if (topic_goal_pose_out.empty()) 35 | { 36 | BT_ROS_ERROR_STREAM("Missing param [MoveBaseActions/topic_goal_pos_out]"); 37 | return; 38 | } 39 | 40 | pub_pose = public_node.advertise(topic_goal_pose_out, 10, false); 41 | sub_notify = public_node.subscribe(topic_notify_result, 1, &NavigateToGoal::callbackNavigationResult, this); 42 | 43 | is_initialized = true; 44 | BT_ROS_INFO_STREAM("Successfully initialized"); 45 | } 46 | 47 | NavigateToGoal::~NavigateToGoal() 48 | { 49 | } 50 | 51 | void NavigateToGoal::halt() 52 | { 53 | new_message_received = true; 54 | // force failure 55 | last_msg.nav_error_code = -1; 56 | // TODO: Shall we cancel all goals for global and local planner? 57 | BT::CoroActionNode::halt(); 58 | } 59 | 60 | BT::NodeStatus NavigateToGoal::tick() 61 | { 62 | setStatus(BT::NodeStatus::RUNNING); 63 | 64 | if (!is_initialized) 65 | { 66 | BT_ROS_ERROR("NavigateToGoal is not initialized"); 67 | return BT::NodeStatus::FAILURE; 68 | } 69 | 70 | BT_ROS_DEBUG_STREAM("NavigateToGoal::tick()"); 71 | 72 | // send pose to navigation tree -------------------------------------------------------------------------------------- 73 | // get target pose 74 | Pose2D target_pose; 75 | auto res = BT::TreeNode::getInput("target_pose", target_pose); 76 | if (!res) 77 | { 78 | BT_ROS_ERROR_STREAM(" Could not load target_pose, resetting variable. Error: " << res.error()); 79 | BT::TreeNode::setOutput("target_pose", Pose2D()); // reset current target_pose 80 | return BT::NodeStatus::FAILURE; 81 | } 82 | 83 | if (std::isnan(target_pose.x) || std::isnan(target_pose.y) || std::isnan(target_pose.theta)) 84 | { 85 | BT_ROS_ERROR_STREAM("Invalid pose: " << target_pose.toString()); 86 | return BT::NodeStatus::FAILURE; 87 | } 88 | 89 | if (target_pose.frame_id.empty()) 90 | { 91 | BT_ROS_ERROR_STREAM("Empty frame id: " << target_pose.toString()); 92 | return BT::NodeStatus::FAILURE; 93 | } 94 | 95 | // publish target pose 96 | geometry_msgs::PoseStamped pose; 97 | pose = target_pose.toPoseStamped(); 98 | pub_pose.publish(pose); 99 | 100 | new_message_received = false; 101 | 102 | // wait for feedback -------------------------------------------------------------------------------------- 103 | while (ros::ok() && !new_message_received) 104 | { 105 | if (!new_message_received) 106 | { 107 | // set status to RUNNING and "pause/sleep" 108 | // If halt() is called, we will not resume execution (stack destroyed) 109 | setStatusRunningAndYield(); 110 | } 111 | } 112 | 113 | new_message_received = false; 114 | 115 | BT_ROS_DEBUG_STREAM("NavigateToGoal: Navigation finished"); 116 | 117 | BT::TreeNode::setOutput("nav_error_code", (unsigned int)last_msg.nav_error_code); 118 | BT::TreeNode::setOutput("navigation_result", last_msg); 119 | BT::TreeNode::setOutput("target_pose", Pose2D()); // reset current target_pose 120 | 121 | if (last_msg.nav_error_code == 0) 122 | return BT::NodeStatus::SUCCESS; 123 | else 124 | return BT::NodeStatus::FAILURE; 125 | } 126 | 127 | void NavigateToGoal::callbackNavigationResult(const fkie_bt_move_base_actions::NavigationResult& msg) 128 | { 129 | new_message_received = true; 130 | last_msg = msg; 131 | 132 | BT_ROS_DEBUG_STREAM("New callback navigation result: (" << msg.nav_error_code << ") - " << msg.origin << ": " 133 | << msg.description); 134 | } 135 | 136 | } // namespace MoveBaseActionsGroup 137 | -------------------------------------------------------------------------------- /fkie_behavior_tree_manager/include/fkie_behavior_tree_manager/conditions/TFTransformAsyncCondition.hpp: -------------------------------------------------------------------------------- 1 | // Copyright 2022 Fraunhofer FKIE - All Rights Reserved 2 | // 3 | // Licensed under the Apache License, Version 2.0 (the "License"); 4 | // you may not use this file except in compliance with the License. 5 | // You may obtain a copy of the License at 6 | // 7 | // http://www.apache.org/licenses/LICENSE-2.0 8 | // 9 | // Unless required by applicable law or agreed to in writing, software 10 | // distributed under the License is distributed on an "AS IS" BASIS, 11 | // WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 12 | // See the License for the specific language governing permissions and 13 | // limitations under the License. 14 | 15 | #ifndef TF_TRANSFORM_ASYNC_CONDITION_H 16 | #define TF_TRANSFORM_ASYNC_CONDITION_H 17 | 18 | #include 19 | #include 20 | #include 21 | #include 22 | #include 23 | 24 | #include 25 | 26 | namespace BehaviorTreeNodes 27 | { 28 | /** 29 | * @brief Asynchronously wait and check if a valid TF between [from_frame] and [to_frame] is given. 30 | * return RUNNING while the TF is valid, FAILURE if it can not transform 31 | * ROS Params: 32 | * [name]/from_frame: source TF frame 33 | * [name]/to_frame: target TF frame 34 | * [name]/timeout: TF Timeout 35 | */ 36 | class TFTransformAsyncCondition : public BT::CoroActionNode 37 | { 38 | public: 39 | std::string name; 40 | 41 | std::string from_frame; 42 | std::string to_frame; 43 | double timeout = 0.0; 44 | 45 | bool can_transform = false; 46 | 47 | tf2_ros::Buffer tf_buffer; 48 | std::unique_ptr tf_listener; 49 | 50 | bool valid_init = false; 51 | 52 | static BT::PortsList providedPorts() 53 | { 54 | BT::PortsList ports = {}; 55 | return ports; 56 | } 57 | 58 | TFTransformAsyncCondition(const std::string& _name, const BT::NodeConfiguration& config) 59 | : BT::CoroActionNode(_name, config), name("[" + _name + "] ") 60 | { 61 | ros::NodeHandle private_node("~"); 62 | ros::NodeHandle public_node; 63 | 64 | tf_listener = std::make_unique(tf_buffer); 65 | 66 | private_node.param(_name + "/timeout", timeout, 1.0); 67 | private_node.param(_name + "/from_frame", from_frame, ""); 68 | private_node.param(_name + "/to_frame", to_frame, ""); 69 | 70 | BT_ROS_INFO_STREAM(name << " from_frame: " << from_frame); 71 | BT_ROS_INFO_STREAM(name << " to_frame: " << to_frame); 72 | BT_ROS_INFO_STREAM(name << " timeout: " << timeout); 73 | 74 | if (from_frame.empty()) 75 | { 76 | BT_ROS_ERROR_STREAM(name << " [from_frame] is empty, check parameter:" << _name + "/from_frame"); 77 | return; 78 | } 79 | 80 | if (to_frame.empty()) 81 | { 82 | BT_ROS_ERROR_STREAM(name << " [to_frame] is empty, check parameter:" << _name + "/to_frame"); 83 | return; 84 | } 85 | 86 | valid_init = true; 87 | BT_ROS_INFO_STREAM(name << " initialized"); 88 | } 89 | 90 | BT::NodeStatus tick() override 91 | { 92 | setStatus(BT::NodeStatus::RUNNING); 93 | 94 | if (!valid_init) 95 | return BT::NodeStatus::FAILURE; 96 | 97 | ros::spinOnce(); 98 | ros::Duration(0.1).sleep(); 99 | 100 | can_transform = false; 101 | while (ros::ok()) 102 | { 103 | if (tf_buffer.canTransform(to_frame, from_frame, ros::Time(0), ros::Duration(timeout))) 104 | { 105 | try 106 | { 107 | geometry_msgs::TransformStamped ts; 108 | ts = tf_buffer.lookupTransform(to_frame, from_frame, ros::Time(0), ros::Duration(timeout)); 109 | can_transform = true; 110 | } 111 | catch (std::exception& e) 112 | { 113 | can_transform = false; 114 | } 115 | } 116 | else 117 | { 118 | can_transform = false; 119 | } 120 | 121 | if (!can_transform) 122 | { 123 | BT_ROS_ERROR_STREAM(name << " no transform between [" << from_frame << "] and [" << to_frame << "]"); 124 | break; 125 | } 126 | else 127 | { 128 | // BT_ROS_WARN_STREAM(name << " tf ready: [" << from_frame << "] and [" << to_frame << "]"); 129 | 130 | // set status to RUNNING and "pause/sleep" 131 | // If halt() is called, we will not resume execution (stack destroyed) 132 | setStatusRunningAndYield(); 133 | } 134 | } 135 | 136 | return can_transform ? BT::NodeStatus::SUCCESS : BT::NodeStatus::FAILURE; 137 | } 138 | 139 | void halt() override 140 | { 141 | BT_ROS_DEBUG_STREAM(name << "node has been halted!"); 142 | setStatus(BT::NodeStatus::FAILURE); 143 | CoroActionNode::halt(); 144 | } 145 | }; 146 | 147 | } // namespace BehaviorTreeNodes 148 | 149 | #endif // TF_TRANSFORM_ASYNC_CONDITION_H 150 | -------------------------------------------------------------------------------- /fkie_behavior_tree_manager/src/BehaviorTreeParameters.hpp: -------------------------------------------------------------------------------- 1 | // Copyright 2022 Fraunhofer FKIE - All Rights Reserved 2 | // 3 | // Licensed under the Apache License, Version 2.0 (the "License"); 4 | // you may not use this file except in compliance with the License. 5 | // You may obtain a copy of the License at 6 | // 7 | // http://www.apache.org/licenses/LICENSE-2.0 8 | // 9 | // Unless required by applicable law or agreed to in writing, software 10 | // distributed under the License is distributed on an "AS IS" BASIS, 11 | // WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 12 | // See the License for the specific language governing permissions and 13 | // limitations under the License. 14 | 15 | #ifndef BEHAVIOR_TREE_PARAMETERS_H_ 16 | #define BEHAVIOR_TREE_PARAMETERS_H_ 17 | 18 | #include 19 | #include 20 | 21 | class BehaviorTreeParameters 22 | { 23 | public: 24 | double rate; 25 | double seconds_wait_after_stop; 26 | bool start_tree_at_init; 27 | 28 | std::string tree_static_xml; 29 | std::string tree_static_file; 30 | 31 | // action plugins 32 | std::vector action_plugins; 33 | 34 | // logging 35 | bool enable_global_log_publisher; 36 | std::string logger_type; 37 | 38 | bool RosMsgLogger_enabled; 39 | 40 | bool StdCoutLogger_enabled; 41 | 42 | bool PublisherZMQ_enabled; 43 | int PublisherZMQ_max_msg_per_second; 44 | int PublisherZMQ_publisher_port; 45 | int PublisherZMQ_server_port; 46 | 47 | bool FileLogger_enabled; 48 | std::string FileLogger_file_path; 49 | 50 | bool MinitraceLogger_enabled; 51 | std::string MinitraceLogger_file_path; 52 | 53 | BehaviorTreeParameters(){}; 54 | 55 | void readParameters() 56 | { 57 | readParam("tree/rate", rate, 5.0); 58 | readParam("tree/start_tree_at_init", start_tree_at_init, true); 59 | readParam("tree/static_xml", tree_static_xml, std::string("")); 60 | readParam("tree/static_file", tree_static_file, std::string("tree.xml")); 61 | readParam("tree/seconds_wait_after_stop", seconds_wait_after_stop, 3.0); 62 | 63 | readParam("logging/enable_global_log_publisher", enable_global_log_publisher, true); 64 | readParam("logging/logger_type", logger_type, std::string("")); 65 | readParam("logging/RosMsgLogger/enabled", RosMsgLogger_enabled, false); 66 | readParam("logging/StdCoutLogger/enabled", StdCoutLogger_enabled, false); 67 | readParam("logging/PublisherZMQ/enabled", PublisherZMQ_enabled, false); 68 | readParam("logging/PublisherZMQ/max_msg_per_second", PublisherZMQ_max_msg_per_second, 20); 69 | readParam("logging/PublisherZMQ/publisher_port", PublisherZMQ_publisher_port, 1666); 70 | readParam("logging/PublisherZMQ/server_port", PublisherZMQ_server_port, 1667); 71 | readParam("logging/FileLogger/enabled", FileLogger_enabled, false); 72 | readParam("logging/FileLogger/file_path", FileLogger_file_path, std::string("bt_trace.fbl")); 73 | readParam("logging/MinitraceLogger/enabled", MinitraceLogger_enabled, false); 74 | readParam("logging/MinitraceLogger/file_path", MinitraceLogger_file_path, std::string("bt_trace.json")); 75 | 76 | readParamNoPrint("action_plugins", action_plugins, std::vector()); 77 | 78 | // validate parameters 79 | if (PublisherZMQ_enabled && 80 | (PublisherZMQ_publisher_port <= 0 || PublisherZMQ_server_port <= 0 || PublisherZMQ_max_msg_per_second <= 0)) 81 | { 82 | BT_ROS_WARN_STREAM( 83 | "PublisherZMQ enabled but has invalid config. Check parameters [logging/PublisherZMQ/...]. Disabling " 84 | "logger."); 85 | PublisherZMQ_enabled = false; 86 | } 87 | 88 | if (FileLogger_enabled && FileLogger_file_path.empty()) 89 | { 90 | BT_ROS_WARN_STREAM( 91 | "FileLogger enabled but file_path is empty. Check parameter [logging/FileLogger/file_path]. Disabling " 92 | "logger."); 93 | FileLogger_enabled = false; 94 | } 95 | 96 | if (MinitraceLogger_enabled && MinitraceLogger_file_path.empty()) 97 | { 98 | BT_ROS_WARN_STREAM( 99 | "MinitraceLogger enabled but file_path is empty. Check parameter [logging/MinitraceLogger/file_path]. " 100 | "Disabling logger."); 101 | MinitraceLogger_enabled = false; 102 | } 103 | 104 | if (!logger_type.empty()) 105 | { 106 | BT_ROS_WARN_STREAM( 107 | "[logging/logger_type] parameter is deprecated. Check BT Wiki for more details about logging."); 108 | 109 | if (logger_type == "ZMQ_LOGGER") 110 | { 111 | BT_ROS_WARN_STREAM( 112 | "Overwriting [ZMQ_LOGGER] config because it was defined on [logging/logger_type] (deprecated)."); 113 | PublisherZMQ_enabled = true; 114 | } 115 | } 116 | } 117 | 118 | protected: 119 | ros::NodeHandle private_node = ros::NodeHandle("~"); 120 | 121 | template 122 | void readParam(const std::string& name, T& param, T default_value) const 123 | { 124 | readParamNoPrint(name, param, default_value); 125 | BT_ROS_INFO_STREAM(name << ": " << param); 126 | } 127 | 128 | template 129 | void readParamNoPrint(const std::string& name, T& param, T default_value) const 130 | { 131 | private_node.param(name, param, default_value); 132 | } 133 | }; 134 | 135 | #endif /* BEHAVIOR_TREE_PARAMETERS_H_ */ 136 | -------------------------------------------------------------------------------- /fkie_behavior_tree_manager/include/fkie_behavior_tree_manager/conditions/CheckPoseReachedAsync.hpp: -------------------------------------------------------------------------------- 1 | // Copyright 2022 Fraunhofer FKIE - All Rights Reserved 2 | // 3 | // Licensed under the Apache License, Version 2.0 (the "License"); 4 | // you may not use this file except in compliance with the License. 5 | // You may obtain a copy of the License at 6 | // 7 | // http://www.apache.org/licenses/LICENSE-2.0 8 | // 9 | // Unless required by applicable law or agreed to in writing, software 10 | // distributed under the License is distributed on an "AS IS" BASIS, 11 | // WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 12 | // See the License for the specific language governing permissions and 13 | // limitations under the License. 14 | 15 | #ifndef CHECK_POSE_REACHED_NODE_H 16 | #define CHECK_POSE_REACHED_NODE_H 17 | 18 | #include 19 | #include 20 | #include 21 | #include 22 | #include 23 | 24 | namespace BehaviorTreeNodes 25 | { 26 | class CheckPoseReachedAsync : public BT::CoroActionNode 27 | { 28 | public: 29 | std::string name; 30 | 31 | tf2_ros::Buffer tf_buffer; 32 | std::unique_ptr tf_listener; 33 | std::string frame_robot, frame_global; 34 | 35 | Pose2D target_pose; 36 | double distance; // Threshold distance 37 | double angle; // Threshold angle 38 | bool valid_init = false; 39 | 40 | static BT::PortsList providedPorts() 41 | { 42 | BT::PortsList ports = { BT::BidirectionalPort("target_pose"), BT::InputPort("distance"), 43 | BT::InputPort("angle") }; 44 | return ports; 45 | } 46 | 47 | CheckPoseReachedAsync(const std::string& _name, const BT::NodeConfiguration& config) 48 | : BT::CoroActionNode(_name, config), name("[" + _name + "] ") 49 | { 50 | ros::NodeHandle private_node("~"); 51 | ros::NodeHandle public_node; 52 | 53 | tf_listener = std::make_unique(tf_buffer); 54 | 55 | std::string parameter_frame_robot = "CheckPoseReachedAsync/" + _name + "_frame_robot"; 56 | std::string parameter_frame_global = "CheckPoseReachedAsync/" + _name + "_frame_global"; 57 | 58 | private_node.param(parameter_frame_robot, frame_robot, std::string("")); 59 | private_node.param(parameter_frame_global, frame_global, std::string("")); 60 | 61 | if (frame_robot.empty() || frame_global.empty()) 62 | { 63 | BT_ROS_ERROR_STREAM(name << " [frame_robot] or [frame_global] is empty."); 64 | return; 65 | } 66 | 67 | valid_init = true; 68 | BT_ROS_INFO_STREAM(name << " initialized"); 69 | } 70 | 71 | inline BT::NodeStatus tick() override 72 | { 73 | setStatus(BT::NodeStatus::RUNNING); 74 | 75 | if (!valid_init) 76 | return BT::NodeStatus::FAILURE; 77 | 78 | // get parameters 79 | BT::TreeNode::getInput("target_pose", target_pose); 80 | BT::TreeNode::getInput("distance", distance); 81 | BT::TreeNode::getInput("angle", angle); 82 | 83 | if (distance <= 0) 84 | { 85 | // disable distance threshold, always running... 86 | return BT::NodeStatus::RUNNING; 87 | } 88 | 89 | if (checkPoseReached()) 90 | { 91 | return BT::NodeStatus::SUCCESS; 92 | } 93 | else 94 | { 95 | return BT::NodeStatus::RUNNING; 96 | } 97 | 98 | return BT::NodeStatus::SUCCESS; 99 | } 100 | 101 | void halt() override 102 | { 103 | BT_ROS_DEBUG_STREAM(name << "node has been halted!"); 104 | setStatus(BT::NodeStatus::FAILURE); 105 | CoroActionNode::halt(); 106 | } 107 | 108 | bool checkPoseReached() 109 | { 110 | if (tf_buffer.canTransform(frame_global, frame_robot, ros::Time(0), ros::Duration(2.0))) 111 | { 112 | try 113 | { 114 | geometry_msgs::TransformStamped ts; 115 | ts = tf_buffer.lookupTransform(frame_global, frame_robot, ros::Time(0), ros::Duration(2.0)); 116 | 117 | double current_x = ts.transform.translation.x; 118 | double current_y = ts.transform.translation.y; 119 | double euclidean_distance = 120 | std::sqrt(std::pow(current_x - target_pose.x, 2) + std::pow(current_y - target_pose.y, 2)); 121 | 122 | double current_angle = tf2::getYaw(ts.transform.rotation); 123 | 124 | // compute the absolute distance between two angles 125 | 126 | // option 1: 127 | double angle_distance = 128 | M_PI - std::fabs(std::fmod(std::fabs(current_angle - target_pose.theta), 2 * M_PI) - M_PI); 129 | 130 | // option 2: 131 | // double angle_distance = std::fabs(std::atan2(std::sin(current_angle - target_pose.theta), 132 | // std::cos(current_angle - target_pose.theta))); 133 | 134 | BT_ROS_DEBUG_STREAM(name << "distance: " << euclidean_distance << " (" << distance 135 | << "), angle: " << angle_distance << " (" << angle << ")"); 136 | 137 | return (euclidean_distance <= distance && angle_distance <= angle); 138 | } 139 | catch (std::exception& e) 140 | { 141 | BT_ROS_ERROR_STREAM(name << "can't transform between [" << frame_global << "] and [" << frame_robot 142 | << "]: " << e.what()); 143 | return false; 144 | } 145 | } 146 | else 147 | { 148 | BT_ROS_ERROR_STREAM(name << "can't transform between [" << frame_global << "] and [" << frame_robot << "]"); 149 | return false; 150 | } 151 | } 152 | }; 153 | 154 | } // namespace BehaviorTreeNodes 155 | 156 | #endif // CHECK_POSE_REACHED_NODE_H --------------------------------------------------------------------------------