diff --git a/plansys2_auction_example/.vscode/c_cpp_properties.json b/plansys2_auction_example/.vscode/c_cpp_properties.json new file mode 100644 index 0000000..fb0b0ac --- /dev/null +++ b/plansys2_auction_example/.vscode/c_cpp_properties.json @@ -0,0 +1,156 @@ +{ + "configurations": [ + { + "browse": { + "databaseFilename": "${default}", + "limitSymbolsToIncludedHeaders": false + }, + "includePath": [ + "/home/kalman/projects/research_ws/install/plansys2_bt_actions/include/**", + "/home/kalman/projects/research_ws/install/plansys2_executor/include/**", + "/home/kalman/projects/research_ws/install/plansys2_planner/include/**", + "/home/kalman/projects/research_ws/install/plansys2_problem_expert/include/**", + "/home/kalman/projects/research_ws/install/plansys2_domain_expert/include/**", + "/home/kalman/projects/research_ws/install/plansys2_popf_plan_solver/include/**", + "/home/kalman/projects/research_ws/install/popf/include/**", + "/home/kalman/projects/research_ws/install/plansys2_tfd_plan_solver/include/**", + "/home/kalman/projects/research_ws/install/plansys2_core/include/**", + "/home/kalman/projects/research_ws/install/plansys2_pddl_parser/include/**", + "/home/kalman/projects/research_ws/install/plansys2_msgs/include/**", + "/home/kalman/projects/research_ws/install/plansys2_lifecycle_manager/include/**", + "/home/kalman/projects/research_ws/install/nav2_smoother/include/**", + "/home/kalman/projects/research_ws/install/nav2_graceful_controller/include/**", + "/home/kalman/projects/research_ws/install/nav2_controller/include/**", + "/home/kalman/projects/research_ws/install/dwb_plugins/include/**", + "/home/kalman/projects/research_ws/install/dwb_critics/include/**", + "/home/kalman/projects/research_ws/install/dwb_core/include/**", + "/home/kalman/projects/research_ws/install/nav_2d_utils/include/**", + "/home/kalman/projects/research_ws/install/dwb_msgs/include/**", + "/home/kalman/projects/research_ws/install/nav_2d_msgs/include/**", + "/home/kalman/projects/research_ws/install/nav2_waypoint_follower/include/**", + "/home/kalman/projects/research_ws/install/nav2_theta_star_planner/include/**", + "/home/kalman/projects/research_ws/install/nav2_smac_planner/include/**", + "/home/kalman/projects/research_ws/install/nav2_rotation_shim_controller/include/**", + "/home/kalman/projects/research_ws/install/nav2_regulated_pure_pursuit_controller/include/**", + "/home/kalman/projects/research_ws/install/nav2_planner/include/**", + "/home/kalman/projects/research_ws/install/nav2_navfn_planner/include/**", + "/home/kalman/projects/research_ws/install/nav2_mppi_controller/include/**", + "/home/kalman/projects/research_ws/install/nav2_constrained_smoother/include/**", + "/home/kalman/projects/research_ws/install/nav2_bt_navigator/include/**", + "/home/kalman/projects/research_ws/install/nav2_behaviors/include/**", + "/home/kalman/projects/research_ws/install/nav2_core/include/**", + "/home/kalman/projects/research_ws/install/nav2_collision_monitor/include/**", + "/home/kalman/projects/research_ws/install/costmap_queue/include/**", + "/home/kalman/projects/research_ws/install/nav2_costmap_2d/include/**", + "/home/kalman/projects/research_ws/install/nav2_voxel_grid/include/**", + "/home/kalman/projects/research_ws/install/nav2_velocity_smoother/include/**", + "/home/kalman/projects/research_ws/install/nav2_rviz_plugins/include/**", + "/home/kalman/projects/research_ws/install/nav2_map_server/include/**", + "/home/kalman/projects/research_ws/install/nav2_lifecycle_manager/include/**", + "/home/kalman/projects/research_ws/install/nav2_behavior_tree/include/**", + "/home/kalman/projects/research_ws/install/nav2_amcl/include/**", + "/home/kalman/projects/research_ws/install/nav2_util/include/**", + "/home/kalman/projects/research_ws/install/nav2_msgs/include/**", + "/home/kalman/projects/research_ws/install/behaviortree_cpp_v3/include/**", + "/home/kalman/projects/rolling_personal_ws/install/plansys2_bt_actions/include/**", + "/home/kalman/projects/rolling_personal_ws/install/plansys2_executor/include/**", + "/home/kalman/projects/rolling_personal_ws/install/plansys2_planner/include/**", + "/home/kalman/projects/rolling_personal_ws/install/plansys2_problem_expert/include/**", + "/home/kalman/projects/rolling_personal_ws/install/plansys2_domain_expert/include/**", + "/home/kalman/projects/rolling_personal_ws/install/plansys2_popf_plan_solver/include/**", + "/home/kalman/projects/rolling_personal_ws/install/popf/include/**", + "/home/kalman/projects/rolling_personal_ws/install/plansys2_tfd_plan_solver/include/**", + "/home/kalman/projects/rolling_personal_ws/install/plansys2_core/include/**", + "/home/kalman/projects/rolling_personal_ws/install/plansys2_pddl_parser/include/**", + "/home/kalman/projects/rolling_personal_ws/install/plansys2_msgs/include/**", + "/home/kalman/projects/rolling_personal_ws/install/plansys2_lifecycle_manager/include/**", + "/home/kalman/projects/rolling_personal_ws/install/nav2_smoother/include/**", + "/home/kalman/projects/rolling_personal_ws/install/nav2_controller/include/**", + "/home/kalman/projects/rolling_personal_ws/install/dwb_plugins/include/**", + "/home/kalman/projects/rolling_personal_ws/install/dwb_critics/include/**", + "/home/kalman/projects/rolling_personal_ws/install/dwb_core/include/**", + "/home/kalman/projects/rolling_personal_ws/install/nav_2d_utils/include/**", + "/home/kalman/projects/rolling_personal_ws/install/dwb_msgs/include/**", + "/home/kalman/projects/rolling_personal_ws/install/nav_2d_msgs/include/**", + "/home/kalman/projects/rolling_personal_ws/install/nav2_waypoint_follower/include/**", + "/home/kalman/projects/rolling_personal_ws/install/nav2_theta_star_planner/include/**", + "/home/kalman/projects/rolling_personal_ws/install/nav2_smac_planner/include/**", + "/home/kalman/projects/rolling_personal_ws/install/nav2_rotation_shim_controller/include/**", + "/home/kalman/projects/rolling_personal_ws/install/nav2_regulated_pure_pursuit_controller/include/**", + "/home/kalman/projects/rolling_personal_ws/install/nav2_planner/include/**", + "/home/kalman/projects/rolling_personal_ws/install/nav2_navfn_planner/include/**", + "/home/kalman/projects/rolling_personal_ws/install/nav2_mppi_controller/include/**", + "/home/kalman/projects/rolling_personal_ws/install/nav2_constrained_smoother/include/**", + "/home/kalman/projects/rolling_personal_ws/install/nav2_bt_navigator/include/**", + "/home/kalman/projects/rolling_personal_ws/install/nav2_behaviors/include/**", + "/home/kalman/projects/rolling_personal_ws/install/nav2_core/include/**", + "/home/kalman/projects/rolling_personal_ws/install/nav2_collision_monitor/include/**", + "/home/kalman/projects/rolling_personal_ws/install/costmap_queue/include/**", + "/home/kalman/projects/rolling_personal_ws/install/nav2_costmap_2d/include/**", + "/home/kalman/projects/rolling_personal_ws/install/nav2_voxel_grid/include/**", + "/home/kalman/projects/rolling_personal_ws/install/nav2_velocity_smoother/include/**", + "/home/kalman/projects/rolling_personal_ws/install/nav2_rviz_plugins/include/**", + "/home/kalman/projects/rolling_personal_ws/install/nav2_map_server/include/**", + "/home/kalman/projects/rolling_personal_ws/install/nav2_lifecycle_manager/include/**", + "/home/kalman/projects/rolling_personal_ws/install/nav2_behavior_tree/include/**", + "/home/kalman/projects/rolling_personal_ws/install/nav2_amcl/include/**", + "/home/kalman/projects/rolling_personal_ws/install/nav2_util/include/**", + "/home/kalman/projects/rolling_personal_ws/install/nav2_msgs/include/**", + "/home/kalman/projects/rolling_personal_ws/install/behaviortree_cpp_v3/include/**", + "/opt/ros/rolling/include/**", + "/home/kalman/projects/research_ws/src/BehaviorTree.CPP/include/**", + "/home/kalman/projects/research_ws/src/navigation2/nav2_amcl/include/**", + "/home/kalman/projects/research_ws/src/navigation2/nav2_behavior_tree/include/**", + "/home/kalman/projects/research_ws/src/navigation2/nav2_behaviors/include/**", + "/home/kalman/projects/research_ws/src/navigation2/nav2_bt_navigator/include/**", + "/home/kalman/projects/research_ws/src/navigation2/nav2_collision_monitor/include/**", + "/home/kalman/projects/research_ws/src/navigation2/nav2_constrained_smoother/include/**", + "/home/kalman/projects/research_ws/src/navigation2/nav2_controller/include/**", + "/home/kalman/projects/research_ws/src/navigation2/nav2_core/include/**", + "/home/kalman/projects/research_ws/src/navigation2/nav2_costmap_2d/include/**", + "/home/kalman/projects/research_ws/src/navigation2/nav2_dwb_controller/costmap_queue/include/**", + "/home/kalman/projects/research_ws/src/navigation2/nav2_dwb_controller/dwb_core/include/**", + "/home/kalman/projects/research_ws/src/navigation2/nav2_dwb_controller/dwb_critics/include/**", + "/home/kalman/projects/research_ws/src/navigation2/nav2_dwb_controller/dwb_plugins/include/**", + "/home/kalman/projects/research_ws/src/navigation2/nav2_dwb_controller/nav_2d_utils/include/**", + "/home/kalman/projects/research_ws/src/navigation2/nav2_graceful_controller/include/**", + "/home/kalman/projects/research_ws/src/navigation2/nav2_lifecycle_manager/include/**", + "/home/kalman/projects/research_ws/src/navigation2/nav2_map_server/include/**", + "/home/kalman/projects/research_ws/src/navigation2/nav2_mppi_controller/include/**", + "/home/kalman/projects/research_ws/src/navigation2/nav2_navfn_planner/include/**", + "/home/kalman/projects/research_ws/src/navigation2/nav2_planner/include/**", + "/home/kalman/projects/research_ws/src/navigation2/nav2_regulated_pure_pursuit_controller/include/**", + "/home/kalman/projects/research_ws/src/navigation2/nav2_rotation_shim_controller/include/**", + "/home/kalman/projects/research_ws/src/navigation2/nav2_rviz_plugins/include/**", + "/home/kalman/projects/research_ws/src/navigation2/nav2_smac_planner/include/**", + "/home/kalman/projects/research_ws/src/navigation2/nav2_smoother/include/**", + "/home/kalman/projects/research_ws/src/navigation2/nav2_theta_star_planner/include/**", + "/home/kalman/projects/research_ws/src/navigation2/nav2_util/include/**", + "/home/kalman/projects/research_ws/src/navigation2/nav2_velocity_smoother/include/**", + "/home/kalman/projects/research_ws/src/navigation2/nav2_voxel_grid/include/**", + "/home/kalman/projects/research_ws/src/navigation2/nav2_waypoint_follower/include/**", + "/home/kalman/projects/research_ws/src/plansys2_tfd_plan_solver/include/**", + "/home/kalman/projects/research_ws/src/ros2_planning_system/plansys2_bt_actions/include/**", + "/home/kalman/projects/research_ws/src/ros2_planning_system/plansys2_core/include/**", + "/home/kalman/projects/research_ws/src/ros2_planning_system/plansys2_domain_expert/include/**", + "/home/kalman/projects/research_ws/src/ros2_planning_system/plansys2_executor/include/**", + "/home/kalman/projects/research_ws/src/ros2_planning_system/plansys2_lifecycle_manager/include/**", + "/home/kalman/projects/research_ws/src/ros2_planning_system/plansys2_pddl_parser/include/**", + "/home/kalman/projects/research_ws/src/ros2_planning_system/plansys2_planner/include/**", + "/home/kalman/projects/research_ws/src/ros2_planning_system/plansys2_popf_plan_solver/include/**", + "/home/kalman/projects/research_ws/src/ros2_planning_system/plansys2_problem_expert/include/**", + "/home/kalman/projects/research_ws/src/ros2_planning_system/plansys2_terminal/include/**", + "/home/kalman/projects/research_ws/src/ros2_planning_system/plansys2_tests/include/**", + "/home/kalman/projects/research_ws/src/ros2_planning_system/plansys2_tools/include/**", + "/home/kalman/projects/research_ws/src/ros2_planning_system_examples/plansys2_bt_example/include/**", + "/usr/include/**" + ], + "name": "ROS", + "intelliSenseMode": "gcc-x64", + "compilerPath": "/usr/bin/gcc", + "cStandard": "gnu11", + "cppStandard": "c++14" + } + ], + "version": 4 +} \ No newline at end of file diff --git a/plansys2_auction_example/.vscode/settings.json b/plansys2_auction_example/.vscode/settings.json new file mode 100644 index 0000000..9f777a6 --- /dev/null +++ b/plansys2_auction_example/.vscode/settings.json @@ -0,0 +1,121 @@ +{ + "python.autoComplete.extraPaths": [ + "/home/kalman/projects/research_ws/install/plansys2_simple_example_py/lib/python3.10/site-packages", + "/home/kalman/projects/research_ws/install/plansys2_support_py/lib/python3.10/site-packages", + "/home/kalman/projects/research_ws/install/plansys2_msgs/lib/python3.10/site-packages", + "/home/kalman/projects/research_ws/install/dwb_msgs/lib/python3.10/site-packages", + "/home/kalman/projects/research_ws/install/nav_2d_msgs/lib/python3.10/site-packages", + "/home/kalman/projects/research_ws/build/nav2_simple_commander", + "/home/kalman/projects/research_ws/install/nav2_simple_commander/lib/python3.10/site-packages", + "/home/kalman/projects/research_ws/install/nav2_msgs/lib/python3.10/site-packages", + "/home/kalman/projects/research_ws/install/nav2_common/lib/python3.10/site-packages", + "/home/kalman/projects/rolling_personal_ws/install/plansys2_simple_example_py/lib/python3.10/site-packages", + "/home/kalman/projects/rolling_personal_ws/install/plansys2_support_py/lib/python3.10/site-packages", + "/home/kalman/projects/rolling_personal_ws/install/plansys2_msgs/lib/python3.10/site-packages", + "/home/kalman/projects/rolling_personal_ws/install/dwb_msgs/lib/python3.10/site-packages", + "/home/kalman/projects/rolling_personal_ws/install/nav_2d_msgs/lib/python3.10/site-packages", + "/home/kalman/projects/rolling_personal_ws/build/nav2_simple_commander", + "/home/kalman/projects/rolling_personal_ws/install/nav2_simple_commander/lib/python3.10/site-packages", + "/home/kalman/projects/rolling_personal_ws/install/nav2_msgs/lib/python3.10/site-packages", + "/home/kalman/projects/rolling_personal_ws/install/nav2_common/lib/python3.10/site-packages", + "/opt/ros/rolling/lib/python3.10/site-packages" + ], + "python.analysis.extraPaths": [ + "/home/kalman/projects/research_ws/install/plansys2_simple_example_py/lib/python3.10/site-packages", + "/home/kalman/projects/research_ws/install/plansys2_support_py/lib/python3.10/site-packages", + "/home/kalman/projects/research_ws/install/plansys2_msgs/lib/python3.10/site-packages", + "/home/kalman/projects/research_ws/install/dwb_msgs/lib/python3.10/site-packages", + "/home/kalman/projects/research_ws/install/nav_2d_msgs/lib/python3.10/site-packages", + "/home/kalman/projects/research_ws/build/nav2_simple_commander", + "/home/kalman/projects/research_ws/install/nav2_simple_commander/lib/python3.10/site-packages", + "/home/kalman/projects/research_ws/install/nav2_msgs/lib/python3.10/site-packages", + "/home/kalman/projects/research_ws/install/nav2_common/lib/python3.10/site-packages", + "/home/kalman/projects/rolling_personal_ws/install/plansys2_simple_example_py/lib/python3.10/site-packages", + "/home/kalman/projects/rolling_personal_ws/install/plansys2_support_py/lib/python3.10/site-packages", + "/home/kalman/projects/rolling_personal_ws/install/plansys2_msgs/lib/python3.10/site-packages", + "/home/kalman/projects/rolling_personal_ws/install/dwb_msgs/lib/python3.10/site-packages", + "/home/kalman/projects/rolling_personal_ws/install/nav_2d_msgs/lib/python3.10/site-packages", + "/home/kalman/projects/rolling_personal_ws/build/nav2_simple_commander", + "/home/kalman/projects/rolling_personal_ws/install/nav2_simple_commander/lib/python3.10/site-packages", + "/home/kalman/projects/rolling_personal_ws/install/nav2_msgs/lib/python3.10/site-packages", + "/home/kalman/projects/rolling_personal_ws/install/nav2_common/lib/python3.10/site-packages", + "/opt/ros/rolling/lib/python3.10/site-packages" + ], + "files.associations": { + "chrono": "cpp", + "cctype": "cpp", + "clocale": "cpp", + "cmath": "cpp", + "csignal": "cpp", + "cstdarg": "cpp", + "cstddef": "cpp", + "cstdio": "cpp", + "cstdlib": "cpp", + "cstring": "cpp", + "ctime": "cpp", + "cwchar": "cpp", + "cwctype": "cpp", + "any": "cpp", + "array": "cpp", + "atomic": "cpp", + "strstream": "cpp", + "bit": "cpp", + "*.tcc": "cpp", + "bitset": "cpp", + "codecvt": "cpp", + "compare": "cpp", + "complex": "cpp", + "concepts": "cpp", + "condition_variable": "cpp", + "cstdint": "cpp", + "deque": "cpp", + "forward_list": "cpp", + "list": "cpp", + "map": "cpp", + "set": "cpp", + "string": "cpp", + "unordered_map": "cpp", + "unordered_set": "cpp", + "vector": "cpp", + "exception": "cpp", + "algorithm": "cpp", + "functional": "cpp", + "iterator": "cpp", + "memory": "cpp", + "memory_resource": "cpp", + "numeric": "cpp", + "optional": "cpp", + "random": "cpp", + "ratio": "cpp", + "regex": "cpp", + "string_view": "cpp", + "system_error": "cpp", + "tuple": "cpp", + "type_traits": "cpp", + "utility": "cpp", + "fstream": "cpp", + "future": "cpp", + "initializer_list": "cpp", + "iomanip": "cpp", + "iosfwd": "cpp", + "iostream": "cpp", + "istream": "cpp", + "limits": "cpp", + "mutex": "cpp", + "new": "cpp", + "numbers": "cpp", + "ostream": "cpp", + "semaphore": "cpp", + "shared_mutex": "cpp", + "sstream": "cpp", + "stdexcept": "cpp", + "stop_token": "cpp", + "streambuf": "cpp", + "thread": "cpp", + "cfenv": "cpp", + "cinttypes": "cpp", + "typeindex": "cpp", + "typeinfo": "cpp", + "variant": "cpp" + } +} \ No newline at end of file diff --git a/plansys2_auction_example/CMakeLists.txt b/plansys2_auction_example/CMakeLists.txt new file mode 100644 index 0000000..e6d5d83 --- /dev/null +++ b/plansys2_auction_example/CMakeLists.txt @@ -0,0 +1,65 @@ +cmake_minimum_required(VERSION 3.5) +project(plansys2_auction_example) + +set(CMAKE_BUILD_TYPE Debug) + +find_package(ament_cmake REQUIRED) +find_package(rclcpp REQUIRED) +find_package(rclcpp_action REQUIRED) +find_package(plansys2_msgs REQUIRED) +find_package(geometry_msgs REQUIRED) +find_package(nav2_msgs REQUIRED) +find_package(nav2_util REQUIRED) +find_package(plansys2_domain_expert REQUIRED) +find_package(plansys2_executor REQUIRED) +find_package(plansys2_planner REQUIRED) +find_package(plansys2_problem_expert REQUIRED) +find_package(plansys2_pddl_parser REQUIRED) +find_package(yaml-cpp REQUIRED) + +set(CMAKE_CXX_STANDARD 17) + +set(dependencies + rclcpp + rclcpp_action + plansys2_msgs + nav2_msgs + nav2_util + plansys2_domain_expert + plansys2_executor + plansys2_planner + plansys2_problem_expert + plansys2_pddl_parser + yaml-cpp +) + +add_executable(move_action_node src/move_action_node.cpp) +ament_target_dependencies(move_action_node ${dependencies}) + +add_executable(patrol_action_node src/patrol_action_node.cpp) +ament_target_dependencies(patrol_action_node ${dependencies}) + +add_executable(patrolling_controller_node src/patrolling_controller_node.cpp) +ament_target_dependencies(patrolling_controller_node ${dependencies}) + +install(DIRECTORY launch pddl params DESTINATION share/${PROJECT_NAME}) + +install(TARGETS + move_action_node + patrol_action_node + patrolling_controller_node + ARCHIVE DESTINATION lib + LIBRARY DESTINATION lib + RUNTIME DESTINATION lib/${PROJECT_NAME} +) + +if(BUILD_TESTING) + find_package(ament_lint_auto REQUIRED) + ament_lint_auto_find_test_dependencies() + + find_package(ament_cmake_gtest REQUIRED) +endif() + +ament_export_dependencies(${dependencies}) + +ament_package() diff --git a/plansys2_auction_example/README.md b/plansys2_auction_example/README.md new file mode 100644 index 0000000..f6f80d0 --- /dev/null +++ b/plansys2_auction_example/README.md @@ -0,0 +1,25 @@ +# PlanSys2 Patrol Navigation Example + +## Description + +(**Requires Navigation2**) + +This example shows: +- How to use an application to initiate knowledge (the PDDL problem) and control its execution flow. +- How to interact with ROS2 Navigation + +The patrolling_controller_node node must be run separately. Use the PlanSys2 API to populate instances, predicates, and goals. Implement a finite state machine to establish as a goal which waypoint to patrol. Patrolling a waypoint is going to that point and turning around for a few seconds. + +## How to run + +In terminal 1: + +``` +ros2 launch plansys2_patrol_navigation_example patrol_example_launch.py +``` + +In terminal 2: + +``` +ros2 run plansys2_patrol_navigation_example patrolling_controller_node +``` diff --git a/plansys2_auction_example/launch/patrol_example_fakesim_launch.py b/plansys2_auction_example/launch/patrol_example_fakesim_launch.py new file mode 100644 index 0000000..42e9ac5 --- /dev/null +++ b/plansys2_auction_example/launch/patrol_example_fakesim_launch.py @@ -0,0 +1,87 @@ +# Copyright 2019 Intelligent Robotics Lab +# +# Licensed under the Apache License, Version 2.0 (the "License"); +# you may not use this file except in compliance with the License. +# You may obtain a copy of the License at +# +# http://www.apache.org/licenses/LICENSE-2.0 +# +# Unless required by applicable law or agreed to in writing, software +# distributed under the License is distributed on an "AS IS" BASIS, +# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +# See the License for the specific language governing permissions and +# limitations under the License. + +import os +import yaml + +from ament_index_python.packages import get_package_share_directory + +from launch import LaunchDescription +from launch.actions import IncludeLaunchDescription +from launch.launch_description_sources import PythonLaunchDescriptionSource +from launch_ros.actions import Node + + +def generate_launch_description(): + # Get the launch directory + example_dir = get_package_share_directory('plansys2_auction_example') + + plansys2_cmd = IncludeLaunchDescription( + PythonLaunchDescriptionSource(os.path.join( + get_package_share_directory('plansys2_bringup'), + 'launch', + 'plansys2_bringup_launch_monolithic.py')), + launch_arguments={'model_file': example_dir + '/pddl/patrol.pddl'}.items() + ) + + fake_nav2_cmd = Node( + package='plansys2_bt_example', + executable='nav2_sim_node', + name='nav2_sim_node', + output='screen', + parameters=[]) + + action_param_path = os.path.join( + example_dir, + 'params', + 'actions_params.yaml' + ) + with open(action_param_path, 'r') as file: + configFile = yaml.safe_load(file) + configParams = configFile['move_action_node']['ros__parameters'] + configParams1 = configFile['move_action_node1']['ros__parameters'] + + # Specify the actions + move_cmd = Node( + package='plansys2_auction_example', + executable='move_action_node', + name='move_action_node', + output='screen', + parameters=[configParams]) + move_cmd2 = Node( + package='plansys2_auction_example', + executable='move_action_node', + name='move_action_node1', + output='screen', + parameters=[configParams1]) + + patrol_cmd = Node( + package='plansys2_auction_example', + executable='patrol_action_node', + name='patrol_action_node', + output='screen', + parameters=[]) + + # Create the launch description and populate + ld = LaunchDescription() + + # Declare the launch options + ld.add_action(plansys2_cmd) + ld.add_action(fake_nav2_cmd) + + ld.add_action(move_cmd) + ld.add_action(move_cmd2) + ld.add_action(patrol_cmd) + + return ld diff --git a/plansys2_auction_example/launch/patrol_example_launch.py b/plansys2_auction_example/launch/patrol_example_launch.py new file mode 100644 index 0000000..1c276fd --- /dev/null +++ b/plansys2_auction_example/launch/patrol_example_launch.py @@ -0,0 +1,105 @@ +# Copyright 2019 Intelligent Robotics Lab +# +# Licensed under the Apache License, Version 2.0 (the "License"); +# you may not use this file except in compliance with the License. +# You may obtain a copy of the License at +# +# http://www.apache.org/licenses/LICENSE-2.0 +# +# Unless required by applicable law or agreed to in writing, software +# distributed under the License is distributed on an "AS IS" BASIS, +# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +# See the License for the specific language governing permissions and +# limitations under the License. + +import os +import yaml + +from ament_index_python.packages import get_package_share_directory + +from launch import LaunchDescription +from launch.actions import IncludeLaunchDescription +from launch.launch_description_sources import PythonLaunchDescriptionSource +from launch_ros.actions import Node + + +def generate_launch_description(): + # Get the launch directory + example_dir = get_package_share_directory('plansys2_auction_example') + + plansys2_cmd = IncludeLaunchDescription( + PythonLaunchDescriptionSource(os.path.join( + get_package_share_directory('plansys2_bringup'), + 'launch', + 'plansys2_bringup_launch_monolithic.py')), + launch_arguments={'model_file': example_dir + '/pddl/patrol.pddl'}.items() + ) + + nav2_cmd = IncludeLaunchDescription( + PythonLaunchDescriptionSource(os.path.join( + get_package_share_directory('nav2_bringup'), + 'launch', + 'tb3_simulation_launch.py')), + launch_arguments={ + 'autostart': 'true', + 'params_file': os.path.join(example_dir, 'params', 'nav2_params.yaml') + }.items()) + + + action_param_path = os.path.join( + example_dir, + 'params', + 'actions_params.yaml' + ) + with open(action_param_path, 'r') as file: + configFile = yaml.safe_load(file) + configParams = configFile['move_action_node']['ros__parameters'] + configParams1 = configFile['move_action_node1']['ros__parameters'] + # configParams2 = configFile['move_action_node2']['ros__parameters'] + + print(configParams) + move_cmd = Node( + package='plansys2_auction_example', + executable='move_action_node', + name='move_action_node', + output='screen', + parameters=[configParams]) + move_cmd1 = Node( + package='plansys2_auction_example', + executable='move_action_node', + name='move_action_node1', + output='screen', + parameters=[configParams1]) + # move_cmd2 = Node( + # package='plansys2_auction_example', + # executable='move_action_node', + # name='move_action_node2', + # output='screen', + # parameters=[configParams2]) + # move_cmd2 = Node( + # package='plansys2_auction_example', + # executable='move_action_node', + # name='move_action_node', + # output='screen', + # parameters=[]) + patrol_cmd = Node( + package='plansys2_auction_example', + executable='patrol_action_node', + name='patrol_action_node', + output='screen', + parameters=[]) + + # Create the launch description and populate + ld = LaunchDescription() + + # Declare the launch options + ld.add_action(plansys2_cmd) + # ld.add_action(nav2_cmd) + + ld.add_action(move_cmd) + ld.add_action(move_cmd1) + # ld.add_action(move_cmd2) + # ld.add_action(move_cmd2) + ld.add_action(patrol_cmd) + + return ld diff --git a/plansys2_auction_example/package.xml b/plansys2_auction_example/package.xml new file mode 100644 index 0000000..e1e299f --- /dev/null +++ b/plansys2_auction_example/package.xml @@ -0,0 +1,37 @@ + + + + plansys2_auction_example + 0.0.4 + + An example including Navigation + + Francisco Martin Rico + + Apache License, Version 2.0 + + ament_cmake + + rclcpp + rclcpp_action + plansys2_msgs + geometry_msgs + nav2_msgs + nav2_util + plansys2_domain_expert + plansys2_executor + plansys2_planner + plansys2_problem_expert + plansys2_pddl_parser + + plansys2_bringup + plansys2_terminal + + ament_lint_common + ament_lint_auto + ament_cmake_gtest + + + ament_cmake + + diff --git a/plansys2_auction_example/params/actions_params.yaml b/plansys2_auction_example/params/actions_params.yaml new file mode 100644 index 0000000..b8bba00 --- /dev/null +++ b/plansys2_auction_example/params/actions_params.yaml @@ -0,0 +1,53 @@ +move_action_node: + ros__parameters: + waypoints: + - "wp1" + - "wp2" + - "wp3" + - "wp4" + - "wp_control" + wp1: + x: 0.0 + y: -2.0 + wp2: + x: 1.8 + y: 0.0 + wp3: + x: 0.0 + y: 2.0 + wp4: + x: -0.5 + y: -0.5 + wp_control: + x: -2.0 + y: -0.4 + +move_action_node1: + ros__parameters: + waypoints: + - "wp1" + - "wp2" + - "wp3" + - "wp4" + - "wp_control" + wp1: + x: -1.95 + y: 0.29 + wp2: + x: -1.77 + y: 0.54 + wp3: + x: -1.91 + y: -0.021 + wp4: + x: -0.5 + y: -0.5 + wp_control: + x: -2.0 + y: -0.4 + + +# move_action_node: +# ros__parameters: +# waypoints: +# - {name: "wp1", x: 1.1, y: 2.1} diff --git a/plansys2_auction_example/params/nav2_params.yaml b/plansys2_auction_example/params/nav2_params.yaml new file mode 100644 index 0000000..0b4446a --- /dev/null +++ b/plansys2_auction_example/params/nav2_params.yaml @@ -0,0 +1,354 @@ +amcl: + ros__parameters: + use_sim_time: True + alpha1: 0.2 + alpha2: 0.2 + alpha3: 0.2 + alpha4: 0.2 + alpha5: 0.2 + base_frame_id: "base_footprint" + beam_skip_distance: 0.5 + beam_skip_error_threshold: 0.9 + beam_skip_threshold: 0.3 + do_beamskip: false + global_frame_id: "map" + lambda_short: 0.1 + laser_likelihood_max_dist: 2.0 + laser_max_range: 100.0 + laser_min_range: -1.0 + laser_model_type: "likelihood_field" + max_beams: 60 + max_particles: 2000 + min_particles: 500 + odom_frame_id: "odom" + pf_err: 0.05 + pf_z: 0.99 + recovery_alpha_fast: 0.0 + recovery_alpha_slow: 0.0 + resample_interval: 1 + robot_model_type: "nav2_amcl::DifferentialMotionModel" + save_pose_rate: 0.5 + sigma_hit: 0.2 + tf_broadcast: true + transform_tolerance: 1.0 + update_min_a: 0.2 + update_min_d: 0.25 + z_hit: 0.5 + z_max: 0.05 + z_rand: 0.5 + z_short: 0.05 + scan_topic: scan + set_initial_pose: True + initial_pose: + x: -2.0 + y: -0.4 + z: 0.0 + yaw: 0.0 + +amcl_map_client: + ros__parameters: + use_sim_time: True + +amcl_rclcpp_node: + ros__parameters: + use_sim_time: True + +bt_navigator: + ros__parameters: + use_sim_time: True + global_frame: map + robot_base_frame: base_link + odom_topic: /odom + bt_loop_duration: 10 + default_server_timeout: 20 + # 'default_nav_through_poses_bt_xml' and 'default_nav_to_pose_bt_xml' are use defaults: + # nav2_bt_navigator/navigate_to_pose_w_replanning_and_recovery.xml + # nav2_bt_navigator/navigate_through_poses_w_replanning_and_recovery.xml + # They can be set here or via a RewrittenYaml remap from a parent launch file to Nav2. + plugin_lib_names: + - nav2_compute_path_to_pose_action_bt_node + - nav2_compute_path_through_poses_action_bt_node + - nav2_smooth_path_action_bt_node + - nav2_follow_path_action_bt_node + - nav2_spin_action_bt_node + - nav2_wait_action_bt_node + - nav2_back_up_action_bt_node + - nav2_drive_on_heading_bt_node + - nav2_clear_costmap_service_bt_node + - nav2_is_stuck_condition_bt_node + - nav2_goal_reached_condition_bt_node + - nav2_goal_updated_condition_bt_node + - nav2_globally_updated_goal_condition_bt_node + - nav2_is_path_valid_condition_bt_node + - nav2_initial_pose_received_condition_bt_node + - nav2_reinitialize_global_localization_service_bt_node + - nav2_rate_controller_bt_node + - nav2_distance_controller_bt_node + - nav2_speed_controller_bt_node + - nav2_truncate_path_action_bt_node + - nav2_truncate_path_local_action_bt_node + - nav2_goal_updater_node_bt_node + - nav2_recovery_node_bt_node + - nav2_pipeline_sequence_bt_node + - nav2_round_robin_node_bt_node + - nav2_transform_available_condition_bt_node + - nav2_time_expired_condition_bt_node + - nav2_path_expiring_timer_condition + - nav2_distance_traveled_condition_bt_node + - nav2_single_trigger_bt_node + - nav2_is_battery_low_condition_bt_node + - nav2_navigate_through_poses_action_bt_node + - nav2_navigate_to_pose_action_bt_node + - nav2_remove_passed_goals_action_bt_node + - nav2_planner_selector_bt_node + - nav2_controller_selector_bt_node + - nav2_goal_checker_selector_bt_node + - nav2_controller_cancel_bt_node + - nav2_path_longer_on_approach_bt_node + - nav2_wait_cancel_bt_node + - nav2_spin_cancel_bt_node + - nav2_back_up_cancel_bt_node + - nav2_drive_on_heading_cancel_bt_node + +bt_navigator_rclcpp_node: + ros__parameters: + use_sim_time: True + +controller_server: + ros__parameters: + use_sim_time: True + controller_frequency: 20.0 + min_x_velocity_threshold: 0.001 + min_y_velocity_threshold: 0.5 + min_theta_velocity_threshold: 0.001 + failure_tolerance: 0.3 + progress_checker_plugin: "progress_checker" + goal_checker_plugins: ["general_goal_checker"] # "precise_goal_checker" + controller_plugins: ["FollowPath"] + + # Progress checker parameters + progress_checker: + plugin: "nav2_controller::SimpleProgressChecker" + required_movement_radius: 0.5 + movement_time_allowance: 10.0 + # Goal checker parameters + #precise_goal_checker: + # plugin: "nav2_controller::SimpleGoalChecker" + # xy_goal_tolerance: 0.25 + # yaw_goal_tolerance: 0.25 + # stateful: True + general_goal_checker: + stateful: True + plugin: "nav2_controller::SimpleGoalChecker" + xy_goal_tolerance: 0.25 + yaw_goal_tolerance: 0.25 + # DWB parameters + FollowPath: + plugin: "dwb_core::DWBLocalPlanner" + debug_trajectory_details: True + min_vel_x: 0.0 + min_vel_y: 0.0 + max_vel_x: 0.26 + max_vel_y: 0.0 + max_vel_theta: 1.0 + min_speed_xy: 0.0 + max_speed_xy: 0.26 + min_speed_theta: 0.0 + # Add high threshold velocity for turtlebot 3 issue. + # https://github.com/ROBOTIS-GIT/turtlebot3_simulations/issues/75 + acc_lim_x: 2.5 + acc_lim_y: 0.0 + acc_lim_theta: 3.2 + decel_lim_x: -2.5 + decel_lim_y: 0.0 + decel_lim_theta: -3.2 + vx_samples: 20 + vy_samples: 5 + vtheta_samples: 20 + sim_time: 1.7 + linear_granularity: 0.05 + angular_granularity: 0.025 + transform_tolerance: 0.2 + xy_goal_tolerance: 0.25 + trans_stopped_velocity: 0.25 + short_circuit_trajectory_evaluation: True + stateful: True + critics: ["RotateToGoal", "Oscillation", "BaseObstacle", "GoalAlign", "PathAlign", "PathDist", "GoalDist"] + BaseObstacle.scale: 0.02 + PathAlign.scale: 32.0 + PathAlign.forward_point_distance: 0.1 + GoalAlign.scale: 24.0 + GoalAlign.forward_point_distance: 0.1 + PathDist.scale: 32.0 + GoalDist.scale: 24.0 + RotateToGoal.scale: 32.0 + RotateToGoal.slowing_factor: 5.0 + RotateToGoal.lookahead_time: -1.0 + +controller_server_rclcpp_node: + ros__parameters: + use_sim_time: True + +local_costmap: + local_costmap: + ros__parameters: + update_frequency: 5.0 + publish_frequency: 2.0 + global_frame: odom + robot_base_frame: base_link + use_sim_time: True + rolling_window: true + width: 3 + height: 3 + resolution: 0.05 + robot_radius: 0.22 + plugins: ["voxel_layer", "inflation_layer"] + inflation_layer: + plugin: "nav2_costmap_2d::InflationLayer" + cost_scaling_factor: 3.0 + inflation_radius: 0.55 + voxel_layer: + plugin: "nav2_costmap_2d::VoxelLayer" + enabled: True + publish_voxel_map: True + origin_z: 0.0 + z_resolution: 0.05 + z_voxels: 16 + max_obstacle_height: 2.0 + mark_threshold: 0 + observation_sources: scan + scan: + topic: /scan + max_obstacle_height: 2.0 + clearing: True + marking: True + data_type: "LaserScan" + raytrace_max_range: 3.0 + raytrace_min_range: 0.0 + obstacle_max_range: 2.5 + obstacle_min_range: 0.0 + static_layer: + map_subscribe_transient_local: True + always_send_full_costmap: True + local_costmap_client: + ros__parameters: + use_sim_time: True + local_costmap_rclcpp_node: + ros__parameters: + use_sim_time: True + +global_costmap: + global_costmap: + ros__parameters: + update_frequency: 1.0 + publish_frequency: 1.0 + global_frame: map + robot_base_frame: base_link + use_sim_time: True + robot_radius: 0.22 + resolution: 0.05 + track_unknown_space: true + plugins: ["static_layer", "obstacle_layer", "inflation_layer"] + obstacle_layer: + plugin: "nav2_costmap_2d::ObstacleLayer" + enabled: True + observation_sources: scan + scan: + topic: /scan + max_obstacle_height: 2.0 + clearing: True + marking: True + data_type: "LaserScan" + raytrace_max_range: 3.0 + raytrace_min_range: 0.0 + obstacle_max_range: 2.5 + obstacle_min_range: 0.0 + static_layer: + plugin: "nav2_costmap_2d::StaticLayer" + map_subscribe_transient_local: True + inflation_layer: + plugin: "nav2_costmap_2d::InflationLayer" + cost_scaling_factor: 3.0 + inflation_radius: 0.55 + always_send_full_costmap: True + global_costmap_client: + ros__parameters: + use_sim_time: True + global_costmap_rclcpp_node: + ros__parameters: + use_sim_time: True + +map_server: + ros__parameters: + use_sim_time: True + yaml_filename: "turtlebot3_world.yaml" + +map_saver: + ros__parameters: + use_sim_time: True + save_map_timeout: 5.0 + free_thresh_default: 0.25 + occupied_thresh_default: 0.65 + map_subscribe_transient_local: True + +planner_server: + ros__parameters: + expected_planner_frequency: 20.0 + use_sim_time: True + planner_plugins: ["GridBased"] + GridBased: + plugin: "nav2_navfn_planner/NavfnPlanner" + tolerance: 0.5 + use_astar: false + allow_unknown: true + +planner_server_rclcpp_node: + ros__parameters: + use_sim_time: True + +smoother_server: + ros__parameters: + use_sim_time: True + smoother_plugins: ["simple_smoother"] + simple_smoother: + plugin: "nav2_smoother::SimpleSmoother" + tolerance: 1.0e-10 + max_its: 1000 + do_refinement: True + +behavior_server: + ros__parameters: + costmap_topic: local_costmap/costmap_raw + footprint_topic: local_costmap/published_footprint + cycle_frequency: 10.0 + behavior_plugins: ["spin", "backup", "drive_on_heading", "wait"] + spin: + plugin: "nav2_behaviors/Spin" + backup: + plugin: "nav2_behaviors/BackUp" + drive_on_heading: + plugin: "nav2_behaviors/DriveOnHeading" + wait: + plugin: "nav2_behaviors/Wait" + global_frame: odom + robot_base_frame: base_link + transform_tolerance: 0.1 + use_sim_time: true + simulate_ahead_time: 2.0 + max_rotational_vel: 1.0 + min_rotational_vel: 0.4 + rotational_acc_lim: 3.2 + +robot_state_publisher: + ros__parameters: + use_sim_time: True + +waypoint_follower: + ros__parameters: + loop_rate: 20 + stop_on_failure: false + waypoint_task_executor_plugin: "wait_at_waypoint" + wait_at_waypoint: + plugin: "nav2_waypoint_follower::WaitAtWaypoint" + enabled: True + waypoint_pause_duration: 200 diff --git a/plansys2_auction_example/pddl/patrol.pddl b/plansys2_auction_example/pddl/patrol.pddl new file mode 100644 index 0000000..e45ca2b --- /dev/null +++ b/plansys2_auction_example/pddl/patrol.pddl @@ -0,0 +1,47 @@ +(define (domain patrol) +(:requirements :strips :typing :adl :fluents :durative-actions) + +;; Types ;;;;;;;;;;;;;;;;;;;;;;;;;;;;;; +(:types +robot +waypoint +);; end Types ;;;;;;;;;;;;;;;;;;;;;;;;; + +;; Predicates ;;;;;;;;;;;;;;;;;;;;;;;;; +(:predicates + +(robot_at ?r - robot ?wp - waypoint) +(connected ?wp1 ?wp2 - waypoint) +(patrolled ?wp - waypoint) + +);; end Predicates ;;;;;;;;;;;;;;;;;;;; +;; Functions ;;;;;;;;;;;;;;;;;;;;;;;;; +(:functions + +);; end Functions ;;;;;;;;;;;;;;;;;;;; +;; Actions ;;;;;;;;;;;;;;;;;;;;;;;;;;;; +(:durative-action move + :parameters (?r - robot ?wp1 ?wp2 - waypoint) + :duration ( = ?duration 5) + :condition (and + (at start(connected ?wp1 ?wp2)) + (at start(robot_at ?r ?wp1)) + ) + :effect (and + (at start(not(robot_at ?r ?wp1))) + (at end(robot_at ?r ?wp2)) + ) +) + +(:durative-action patrol + :parameters (?r - robot ?wp - waypoint) + :duration ( = ?duration 5) + :condition (and + (at start(robot_at ?r ?wp)) + ) + :effect (and + (at end(patrolled ?wp)) + ) +) + +);; end Domain ;;;;;;;;;;;;;;;;;;;;;;;; diff --git a/plansys2_auction_example/src/move_action_node.cpp b/plansys2_auction_example/src/move_action_node.cpp new file mode 100644 index 0000000..c34b0f1 --- /dev/null +++ b/plansys2_auction_example/src/move_action_node.cpp @@ -0,0 +1,292 @@ +// Copyright 2019 Intelligent Robotics Lab +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#include + +#include +#include +#include +#include +#include + +#include "geometry_msgs/msg/pose_stamped.hpp" +#include "geometry_msgs/msg/pose.hpp" +#include "geometry_msgs/msg/pose_with_covariance_stamped.hpp" +#include "nav2_msgs/action/navigate_to_pose.hpp" +#include "nav2_msgs/action/compute_path_to_pose.hpp" +#include "nav2_util/geometry_utils.hpp" + +#include "plansys2_executor/ActionExecutorClient.hpp" + +#include "rclcpp/rclcpp.hpp" +#include "rclcpp_action/rclcpp_action.hpp" + +using namespace std::chrono_literals; + +class MoveAction : public plansys2::ActionExecutorClient +{ +public: + MoveAction() + : plansys2::ActionExecutorClient("move", 500ms) + { + + declare_parameter>("waypoints", std::vector()); + auto waypoint_names = get_parameter("waypoints").as_string_array(); + for (const auto& name : waypoint_names) { + geometry_msgs::msg::PoseStamped wp; + wp.header.frame_id = "map"; + wp.header.stamp = now(); + declare_parameter(name + ".x", 0.0); + declare_parameter(name + ".y", 0.0); + + wp.pose.position.x = get_parameter(name + ".x").as_double(); + wp.pose.position.y = get_parameter(name + ".y").as_double(); + wp.pose.position.z = 0.0; + wp.pose.orientation.x = 0.0; + wp.pose.orientation.y = 0.0; + wp.pose.orientation.z = 0.0; + wp.pose.orientation.w = 1.0; + + waypoints_[name] = wp; + RCLCPP_INFO(get_logger(), "Waypoint: %s, x: %f, y: %f", name.c_str(), wp.pose.position.x, wp.pose.position.y); + } + + using namespace std::placeholders; + pos_sub_ = create_subscription( + "/amcl_pose", + 10, + std::bind(&MoveAction::current_pos_callback, this, _1)); + path_pub_ = create_publisher("path_topic", 10); + } + + void current_pos_callback(const geometry_msgs::msg::PoseWithCovarianceStamped::SharedPtr msg) + { + current_pos_.header = msg->header; + current_pos_.pose = msg->pose.pose; + } + + CallbackReturnT + on_configure(const rclcpp_lifecycle::State & state) + { + CallbackReturnT result = plansys2::ActionExecutorClient::on_configure(state); + if(result != CallbackReturnT::SUCCESS) + { + return result; + } + compute_path_action_client_ = + rclcpp_action::create_client( + shared_from_this(), + "compute_path_to_pose"); + + bool is_action_server_ready; + do { + RCLCPP_INFO(get_logger(), "Waiting for compute path action server..."); + is_action_server_ready = + compute_path_action_client_->wait_for_action_server(std::chrono::seconds(5)); + } while (!is_action_server_ready); + RCLCPP_INFO(get_logger(), "Compute path action server ready"); + + // path_pub_->on_activate(); + + return CallbackReturnT::SUCCESS; + } + + rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn + on_activate(const rclcpp_lifecycle::State & previous_state) + { + send_feedback(0.0, "Move starting"); + + navigation_action_client_ = + rclcpp_action::create_client( + shared_from_this(), + "navigate_to_pose"); + + bool is_action_server_ready = false; + do { + RCLCPP_INFO(get_logger(), "Waiting for navigation action server..."); + + is_action_server_ready = + navigation_action_client_->wait_for_action_server(std::chrono::seconds(5)); + } while (!is_action_server_ready); + + RCLCPP_INFO(get_logger(), "Navigation action server ready"); + + auto wp_to_navigate = get_arguments()[2]; // The goal is in the 3rd argument of the action + RCLCPP_INFO(get_logger(), "Start navigation to [%s]", wp_to_navigate.c_str()); + + goal_pos_ = waypoints_[wp_to_navigate]; + navigation_goal_.pose = goal_pos_; + + dist_to_move = getDistance(goal_pos_.pose, current_pos_.pose); + + auto send_goal_options = + rclcpp_action::Client::SendGoalOptions(); + + send_goal_options.feedback_callback = [this]( + NavigationGoalHandle::SharedPtr, + NavigationFeedback feedback) { + set_action_cost(feedback->distance_remaining, 0.0); + send_feedback( + std::min(1.0, std::max(0.0, 1.0 - (feedback->distance_remaining / dist_to_move))), + "Move running"); + }; + + send_goal_options.result_callback = [this](auto) { + finish(true, 1.0, "Move completed"); + }; + + future_navigation_goal_handle_ = + navigation_action_client_->async_send_goal(navigation_goal_, send_goal_options); + + return ActionExecutorClient::on_activate(previous_state); + } + +private: + using ComputePathGoalHandle = + rclcpp_action::ClientGoalHandle; + using ComputePathFeedback = + const std::shared_ptr; + using ComputePathResult = + const ComputePathGoalHandle::WrappedResult; + // const std::shared_ptr; + + double getDistance(const geometry_msgs::msg::Pose & pos1, const geometry_msgs::msg::Pose & pos2) + { + return sqrt( + (pos1.position.x - pos2.position.x) * (pos1.position.x - pos2.position.x) + + (pos1.position.y - pos2.position.y) * (pos1.position.y - pos2.position.y)); + } + + void do_work() + { + } + + void compute_action_cost(const plansys2_msgs::msg::ActionExecution::SharedPtr msg) + { + RCLCPP_INFO(get_logger(), "Computing action cost"); + auto send_goal_options = rclcpp_action::Client::SendGoalOptions(); + + send_goal_options.feedback_callback = [this]( + ComputePathGoalHandle::SharedPtr goal_handle, + ComputePathFeedback feedback) + { + RCLCPP_INFO(get_logger(), "Feedback received from action cost computation (I am computing)"); + }; + + send_goal_options.result_callback = [this, msg](ComputePathResult &result) + { + ActionCostPtr action_cost = std::make_shared(); + + if (result.code != rclcpp_action::ResultCode::SUCCEEDED) + { + RCLCPP_INFO(get_logger(), "Compute path to cost. Goal failed with error code %d", static_cast(result.code)); + + action_cost->nominal_cost = std::numeric_limits::infinity(); + action_cost->std_dev_cost = 0.0; + this->set_action_cost(action_cost); + this->send_response(msg); + return; + } + + auto path = result.result->path; + double path_length = nav2_util::geometry_utils::calculate_path_length(path); + action_cost->nominal_cost = path_length; + action_cost->std_dev_cost = 0.0; + this->set_action_cost(action_cost); + this->send_response(msg); + + + path_pub_->publish(path); + + RCLCPP_INFO(get_logger(), "Action Cost (Length of path): %f", path_length); + + }; + + send_goal_options.goal_response_callback = [this, msg](const ComputePathGoalHandle::SharedPtr & goal_handle) + { + if (!goal_handle) { + RCLCPP_ERROR(this->get_logger(), "Goal was rejected by server"); + ActionCostPtr action_cost = std::make_shared(); + action_cost->nominal_cost = std::numeric_limits::infinity(); + action_cost->std_dev_cost = 0.0; + this->set_action_cost(action_cost); + this->send_response(msg); + } + else { + RCLCPP_INFO(this->get_logger(), "Goal accepted by server, waiting for result"); + } + + }; + + nav2_msgs::action::ComputePathToPose::Goal goal = nav2_msgs::action::ComputePathToPose::Goal(); + + auto wp_to_navigate = get_arguments()[2]; // The goal is in the 3rd argument of the action + + RCLCPP_INFO(get_logger(), "Computing cost to [%s]", wp_to_navigate.c_str()); //.c_str()); + if(waypoints_.find(wp_to_navigate) == waypoints_.end()) + { + RCLCPP_ERROR(get_logger(), "Waypoint not found, has to be managed by specialized arguments"); + set_action_cost(std::numeric_limits::infinity(), 0.0); + } + goal.start = current_pos_; + goal.goal = waypoints_[wp_to_navigate]; + + goal.planner_id = "GridBased"; + RCLCPP_INFO(get_logger(), "Pre send goal"); + + auto future_goal_handle = compute_path_action_client_->async_send_goal(goal, send_goal_options); + RCLCPP_INFO(get_logger(), "Post send goal"); + + } + + std::map waypoints_; + + using NavigationGoalHandle = + rclcpp_action::ClientGoalHandle; + using NavigationFeedback = + const std::shared_ptr; + + + rclcpp_action::Client::SharedPtr navigation_action_client_; + std::shared_future future_navigation_goal_handle_; + NavigationGoalHandle::SharedPtr navigation_goal_handle_; + + rclcpp_action::Client::SharedPtr compute_path_action_client_; + + rclcpp::Subscription::SharedPtr pos_sub_; + rclcpp::Publisher::SharedPtr path_pub_; + + geometry_msgs::msg::PoseStamped current_pos_; + geometry_msgs::msg::PoseStamped goal_pos_; + nav2_msgs::action::NavigateToPose::Goal navigation_goal_; + + double dist_to_move; + + bool test_; +}; + +int main(int argc, char ** argv) +{ + rclcpp::init(argc, argv); + auto node = std::make_shared(); + + node->set_parameter(rclcpp::Parameter("action_name", "move")); + node->trigger_transition(lifecycle_msgs::msg::Transition::TRANSITION_CONFIGURE); + + rclcpp::spin(node->get_node_base_interface()); + + rclcpp::shutdown(); + + return 0; +} diff --git a/plansys2_auction_example/src/patrol_action_node.cpp b/plansys2_auction_example/src/patrol_action_node.cpp new file mode 100644 index 0000000..5541f8d --- /dev/null +++ b/plansys2_auction_example/src/patrol_action_node.cpp @@ -0,0 +1,104 @@ +// Copyright 2019 Intelligent Robotics Lab +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#include + +#include "geometry_msgs/msg/twist.hpp" + +#include "plansys2_executor/ActionExecutorClient.hpp" + +#include "rclcpp/rclcpp.hpp" +#include "rclcpp_action/rclcpp_action.hpp" +#include "lifecycle_msgs/msg/state.hpp" + +using namespace std::chrono_literals; + +class Patrol : public plansys2::ActionExecutorClient +{ +public: + Patrol() + : plansys2::ActionExecutorClient("patrol", 1s) + { + } + + rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn + on_activate(const rclcpp_lifecycle::State & previous_state) + { + progress_ = 0.0; + + cmd_vel_pub_ = this->create_publisher("/cmd_vel", 10); + cmd_vel_pub_->on_activate(); + + return ActionExecutorClient::on_activate(previous_state); + } + + rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn + on_deactivate(const rclcpp_lifecycle::State & previous_state) + { + cmd_vel_pub_->on_deactivate(); + + return ActionExecutorClient::on_deactivate(previous_state); + } + +private: + void do_work() + { + if (progress_ < 1.0) { + progress_ += 0.1; + + send_feedback(progress_, "Patrol running"); + + geometry_msgs::msg::Twist cmd; + cmd.linear.x = 0.0; + cmd.linear.y = 0.0; + cmd.linear.z = 0.0; + cmd.angular.x = 0.0; + cmd.angular.y = 0.0; + cmd.angular.z = 0.5; + + cmd_vel_pub_->publish(cmd); + } else { + geometry_msgs::msg::Twist cmd; + cmd.linear.x = 0.0; + cmd.linear.y = 0.0; + cmd.linear.z = 0.0; + cmd.angular.x = 0.0; + cmd.angular.y = 0.0; + cmd.angular.z = 0.0; + + cmd_vel_pub_->publish(cmd); + + finish(true, 1.0, "Patrol completed"); + } + } + + float progress_; + + rclcpp_lifecycle::LifecyclePublisher::SharedPtr cmd_vel_pub_; +}; + +int main(int argc, char ** argv) +{ + rclcpp::init(argc, argv); + auto node = std::make_shared(); + + node->set_parameter(rclcpp::Parameter("action_name", "patrol")); + node->trigger_transition(lifecycle_msgs::msg::Transition::TRANSITION_CONFIGURE); + + rclcpp::spin(node->get_node_base_interface()); + + rclcpp::shutdown(); + + return 0; +} diff --git a/plansys2_auction_example/src/patrolling_controller_node.cpp b/plansys2_auction_example/src/patrolling_controller_node.cpp new file mode 100644 index 0000000..de33694 --- /dev/null +++ b/plansys2_auction_example/src/patrolling_controller_node.cpp @@ -0,0 +1,366 @@ +// Copyright 2019 Intelligent Robotics Lab +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#include + +#include + +#include "plansys2_msgs/msg/action_execution_info.hpp" +#include "plansys2_msgs/msg/plan.hpp" + +#include "plansys2_domain_expert/DomainExpertClient.hpp" +#include "plansys2_executor/ExecutorClient.hpp" +#include "plansys2_planner/PlannerClient.hpp" +#include "plansys2_problem_expert/ProblemExpertClient.hpp" + +#include "rclcpp/rclcpp.hpp" +#include "rclcpp_action/rclcpp_action.hpp" + +class PatrollingController : public rclcpp::Node +{ +public: + PatrollingController() + : rclcpp::Node("patrolling_controller"), state_(STARTING) + { + } + + void init() + { + domain_expert_ = std::make_shared(); + planner_client_ = std::make_shared(); + problem_expert_ = std::make_shared(); + executor_client_ = std::make_shared(); + init_knowledge(); + } + + void init_knowledge() + { + problem_expert_->addInstance(plansys2::Instance{"r2d2", "robot"}); + problem_expert_->addInstance(plansys2::Instance{"wp_control", "waypoint"}); + problem_expert_->addInstance(plansys2::Instance{"wp1", "waypoint"}); + problem_expert_->addInstance(plansys2::Instance{"wp2", "waypoint"}); + problem_expert_->addInstance(plansys2::Instance{"wp3", "waypoint"}); + problem_expert_->addInstance(plansys2::Instance{"wp4", "waypoint"}); + + problem_expert_->addPredicate(plansys2::Predicate("(robot_at r2d2 wp_control)")); + problem_expert_->addPredicate(plansys2::Predicate("(connected wp_control wp1)")); + problem_expert_->addPredicate(plansys2::Predicate("(connected wp1 wp_control)")); + problem_expert_->addPredicate(plansys2::Predicate("(connected wp_control wp2)")); + problem_expert_->addPredicate(plansys2::Predicate("(connected wp2 wp_control)")); + problem_expert_->addPredicate(plansys2::Predicate("(connected wp_control wp3)")); + problem_expert_->addPredicate(plansys2::Predicate("(connected wp3 wp_control)")); + problem_expert_->addPredicate(plansys2::Predicate("(connected wp_control wp4)")); + problem_expert_->addPredicate(plansys2::Predicate("(connected wp4 wp_control)")); + } + + void step() + { + switch (state_) { + case STARTING: + { + // Set the goal for next state + problem_expert_->setGoal(plansys2::Goal("(and(patrolled wp1))")); + + // Compute the plan + auto domain = domain_expert_->getDomain(); + auto problem = problem_expert_->getProblem(); + auto plan = planner_client_->getPlan(domain, problem); + + if (!plan.has_value()) { + std::cout << "Could not find plan to reach goal " << + parser::pddl::toString(problem_expert_->getGoal()) << std::endl; + break; + } + + // Execute the plan + if (executor_client_->start_plan_execution(plan.value())) { + state_ = PATROL_WP1; + } + } + break; + case PATROL_WP1: + { + auto feedback = executor_client_->getFeedBack(); + + for (const auto & action_feedback : feedback.action_execution_status) { + std::cout << "[" << action_feedback.action << " " << + action_feedback.completion * 100.0 << "%]"; + } + std::cout << std::endl; + + if (!executor_client_->execute_and_check_plan() && executor_client_->getResult()) { + if (executor_client_->getResult().value().success) { + std::cout << "Successful finished " << std::endl; + + // Cleanning up + problem_expert_->removePredicate(plansys2::Predicate("(patrolled wp1)")); + + // Set the goal for next state + problem_expert_->setGoal(plansys2::Goal("(and(patrolled wp2))")); + + // Compute the plan + auto domain = domain_expert_->getDomain(); + auto problem = problem_expert_->getProblem(); + auto plan = planner_client_->getPlan(domain, problem); + + if (!plan.has_value()) { + std::cout << "Could not find plan to reach goal " << + parser::pddl::toString(problem_expert_->getGoal()) << std::endl; + break; + } + + // Execute the plan + if (executor_client_->start_plan_execution(plan.value())) { + state_ = PATROL_WP2; + } + } else { + for (const auto & action_feedback : feedback.action_execution_status) { + if (action_feedback.status == plansys2_msgs::msg::ActionExecutionInfo::FAILED) { + std::cout << "[" << action_feedback.action << "] finished with error: " << + action_feedback.message_status << std::endl; + } + } + + // Replan + auto domain = domain_expert_->getDomain(); + auto problem = problem_expert_->getProblem(); + auto plan = planner_client_->getPlan(domain, problem); + + if (!plan.has_value()) { + std::cout << "Unsuccessful replan attempt to reach goal " << + parser::pddl::toString(problem_expert_->getGoal()) << std::endl; + break; + } + + // Execute the plan + executor_client_->start_plan_execution(plan.value()); + } + } + } + break; + case PATROL_WP2: + { + auto feedback = executor_client_->getFeedBack(); + + for (const auto & action_feedback : feedback.action_execution_status) { + std::cout << "[" << action_feedback.action << " " << + action_feedback.completion * 100.0 << "%]"; + } + std::cout << std::endl; + + if (!executor_client_->execute_and_check_plan() && executor_client_->getResult()) { + if (executor_client_->getResult().value().success) { + std::cout << "Successful finished " << std::endl; + + // Cleanning up + problem_expert_->removePredicate(plansys2::Predicate("(patrolled wp2)")); + + // Set the goal for next state + problem_expert_->setGoal(plansys2::Goal("(and(patrolled wp3))")); + + // Compute the plan + auto domain = domain_expert_->getDomain(); + auto problem = problem_expert_->getProblem(); + auto plan = planner_client_->getPlan(domain, problem); + + if (!plan.has_value()) { + std::cout << "Could not find plan to reach goal " << + parser::pddl::toString(problem_expert_->getGoal()) << std::endl; + break; + } + + // Execute the plan + if (executor_client_->start_plan_execution(plan.value())) { + state_ = PATROL_WP3; + } + } else { + for (const auto & action_feedback : feedback.action_execution_status) { + if (action_feedback.status == plansys2_msgs::msg::ActionExecutionInfo::FAILED) { + std::cout << "[" << action_feedback.action << "] finished with error: " << + action_feedback.message_status << std::endl; + } + } + + // Replan + auto domain = domain_expert_->getDomain(); + auto problem = problem_expert_->getProblem(); + auto plan = planner_client_->getPlan(domain, problem); + + if (!plan.has_value()) { + std::cout << "Unsuccessful replan attempt to reach goal " << + parser::pddl::toString(problem_expert_->getGoal()) << std::endl; + break; + } + + // Execute the plan + executor_client_->start_plan_execution(plan.value()); + } + } + } + break; + case PATROL_WP3: + { + auto feedback = executor_client_->getFeedBack(); + + for (const auto & action_feedback : feedback.action_execution_status) { + std::cout << "[" << action_feedback.action << " " << + action_feedback.completion * 100.0 << "%]"; + } + std::cout << std::endl; + + if (!executor_client_->execute_and_check_plan() && executor_client_->getResult()) { + if (executor_client_->getResult().value().success) { + std::cout << "Successful finished " << std::endl; + + // Cleanning up + problem_expert_->removePredicate(plansys2::Predicate("(patrolled wp3)")); + + // Set the goal for next state + problem_expert_->setGoal(plansys2::Goal("(and(patrolled wp4))")); + + // Compute the plan + auto domain = domain_expert_->getDomain(); + auto problem = problem_expert_->getProblem(); + auto plan = planner_client_->getPlan(domain, problem); + + if (!plan.has_value()) { + std::cout << "Could not find plan to reach goal " << + parser::pddl::toString(problem_expert_->getGoal()) << std::endl; + break; + } + + // Execute the plan + if (executor_client_->start_plan_execution(plan.value())) { + state_ = PATROL_WP4; + } + } else { + for (const auto & action_feedback : feedback.action_execution_status) { + if (action_feedback.status == plansys2_msgs::msg::ActionExecutionInfo::FAILED) { + std::cout << "[" << action_feedback.action << "] finished with error: " << + action_feedback.message_status << std::endl; + } + } + + // Replan + auto domain = domain_expert_->getDomain(); + auto problem = problem_expert_->getProblem(); + auto plan = planner_client_->getPlan(domain, problem); + + if (!plan.has_value()) { + std::cout << "Unsuccessful replan attempt to reach goal " << + parser::pddl::toString(problem_expert_->getGoal()) << std::endl; + break; + } + + // Execute the plan + executor_client_->start_plan_execution(plan.value()); + } + } + } + break; + case PATROL_WP4: + { + auto feedback = executor_client_->getFeedBack(); + + for (const auto & action_feedback : feedback.action_execution_status) { + std::cout << "[" << action_feedback.action << " " << + action_feedback.completion * 100.0 << "%]"; + } + std::cout << std::endl; + + if (!executor_client_->execute_and_check_plan() && executor_client_->getResult()) { + if (executor_client_->getResult().value().success) { + std::cout << "Successful finished " << std::endl; + + // Cleanning up + problem_expert_->removePredicate(plansys2::Predicate("(patrolled wp4)")); + + // Set the goal for next state + problem_expert_->setGoal(plansys2::Goal("(and(patrolled wp1))")); + + // Compute the plan + auto domain = domain_expert_->getDomain(); + auto problem = problem_expert_->getProblem(); + auto plan = planner_client_->getPlan(domain, problem); + + if (!plan.has_value()) { + std::cout << "Could not find plan to reach goal " << + parser::pddl::toString(problem_expert_->getGoal()) << std::endl; + break; + } + + // Execute the plan + if (executor_client_->start_plan_execution(plan.value())) { + // Loop to WP1 + state_ = PATROL_WP1; + } + } else { + for (const auto & action_feedback : feedback.action_execution_status) { + if (action_feedback.status == plansys2_msgs::msg::ActionExecutionInfo::FAILED) { + std::cout << "[" << action_feedback.action << "] finished with error: " << + action_feedback.message_status << std::endl; + } + } + + // Replan + auto domain = domain_expert_->getDomain(); + auto problem = problem_expert_->getProblem(); + auto plan = planner_client_->getPlan(domain, problem); + + if (!plan.has_value()) { + std::cout << "Unsuccessful replan attempt to reach goal " << + parser::pddl::toString(problem_expert_->getGoal()) << std::endl; + break; + } + + // Execute the plan + executor_client_->start_plan_execution(plan.value()); + } + } + } + break; + default: + break; + } + } + +private: + typedef enum {STARTING, PATROL_WP1, PATROL_WP2, PATROL_WP3, PATROL_WP4} StateType; + StateType state_; + + std::shared_ptr domain_expert_; + std::shared_ptr planner_client_; + std::shared_ptr problem_expert_; + std::shared_ptr executor_client_; +}; + +int main(int argc, char ** argv) +{ + rclcpp::init(argc, argv); + auto node = std::make_shared(); + + node->init(); + + rclcpp::Rate rate(5); + while (rclcpp::ok()) { + node->step(); + + rate.sleep(); + rclcpp::spin_some(node->get_node_base_interface()); + } + + rclcpp::shutdown(); + + return 0; +} diff --git a/plansys2_bt_example/include/plansys2_bt_example/behavior_tree_nodes/ApproachObject.hpp b/plansys2_bt_example/include/plansys2_bt_example/behavior_tree_nodes/ApproachObject.hpp index ac91336..2c7d8b8 100644 --- a/plansys2_bt_example/include/plansys2_bt_example/behavior_tree_nodes/ApproachObject.hpp +++ b/plansys2_bt_example/include/plansys2_bt_example/behavior_tree_nodes/ApproachObject.hpp @@ -28,7 +28,7 @@ class ApproachObject : public BT::ActionNodeBase public: explicit ApproachObject( const std::string & xml_tag_name, - const BT::NodeConfiguration & conf); + const BT::NodeConfig & conf); void halt(); BT::NodeStatus tick(); diff --git a/plansys2_bt_example/include/plansys2_bt_example/behavior_tree_nodes/CloseGripper.hpp b/plansys2_bt_example/include/plansys2_bt_example/behavior_tree_nodes/CloseGripper.hpp index 0bc43e8..7429a40 100644 --- a/plansys2_bt_example/include/plansys2_bt_example/behavior_tree_nodes/CloseGripper.hpp +++ b/plansys2_bt_example/include/plansys2_bt_example/behavior_tree_nodes/CloseGripper.hpp @@ -28,7 +28,7 @@ class CloseGripper : public BT::ActionNodeBase public: explicit CloseGripper( const std::string & xml_tag_name, - const BT::NodeConfiguration & conf); + const BT::NodeConfig & conf); void halt(); BT::NodeStatus tick(); diff --git a/plansys2_bt_example/include/plansys2_bt_example/behavior_tree_nodes/Move.hpp b/plansys2_bt_example/include/plansys2_bt_example/behavior_tree_nodes/Move.hpp index 201cd4f..ab8d458 100644 --- a/plansys2_bt_example/include/plansys2_bt_example/behavior_tree_nodes/Move.hpp +++ b/plansys2_bt_example/include/plansys2_bt_example/behavior_tree_nodes/Move.hpp @@ -35,7 +35,7 @@ class Move : public plansys2::BtActionNode< explicit Move( const std::string & xml_tag_name, const std::string & action_name, - const BT::NodeConfiguration & conf); + const BT::NodeConfig & conf); BT::NodeStatus on_tick() override; BT::NodeStatus on_success() override; diff --git a/plansys2_bt_example/include/plansys2_bt_example/behavior_tree_nodes/OpenGripper.hpp b/plansys2_bt_example/include/plansys2_bt_example/behavior_tree_nodes/OpenGripper.hpp index 6f0033d..fd35868 100644 --- a/plansys2_bt_example/include/plansys2_bt_example/behavior_tree_nodes/OpenGripper.hpp +++ b/plansys2_bt_example/include/plansys2_bt_example/behavior_tree_nodes/OpenGripper.hpp @@ -28,7 +28,7 @@ class OpenGripper : public BT::ActionNodeBase public: explicit OpenGripper( const std::string & xml_tag_name, - const BT::NodeConfiguration & conf); + const BT::NodeConfig & conf); void halt(); BT::NodeStatus tick(); diff --git a/plansys2_bt_example/include/plansys2_bt_example/behavior_tree_nodes/Recharge.hpp b/plansys2_bt_example/include/plansys2_bt_example/behavior_tree_nodes/Recharge.hpp index d5d5d41..5a6e95b 100644 --- a/plansys2_bt_example/include/plansys2_bt_example/behavior_tree_nodes/Recharge.hpp +++ b/plansys2_bt_example/include/plansys2_bt_example/behavior_tree_nodes/Recharge.hpp @@ -28,7 +28,7 @@ class Recharge : public BT::ActionNodeBase public: explicit Recharge( const std::string & xml_tag_name, - const BT::NodeConfiguration & conf); + const BT::NodeConfig & conf); void halt(); BT::NodeStatus tick(); diff --git a/plansys2_bt_example/src/behavior_tree_nodes/ApproachObject.cpp b/plansys2_bt_example/src/behavior_tree_nodes/ApproachObject.cpp index fd5649b..f34bd2d 100644 --- a/plansys2_bt_example/src/behavior_tree_nodes/ApproachObject.cpp +++ b/plansys2_bt_example/src/behavior_tree_nodes/ApproachObject.cpp @@ -24,7 +24,7 @@ namespace plansys2_bt_example ApproachObject::ApproachObject( const std::string & xml_tag_name, - const BT::NodeConfiguration & conf) + const BT::NodeConfig & conf) : BT::ActionNodeBase(xml_tag_name, conf), counter_(0) { } diff --git a/plansys2_bt_example/src/behavior_tree_nodes/CloseGripper.cpp b/plansys2_bt_example/src/behavior_tree_nodes/CloseGripper.cpp index 4fa8948..da4af70 100644 --- a/plansys2_bt_example/src/behavior_tree_nodes/CloseGripper.cpp +++ b/plansys2_bt_example/src/behavior_tree_nodes/CloseGripper.cpp @@ -24,7 +24,7 @@ namespace plansys2_bt_example CloseGripper::CloseGripper( const std::string & xml_tag_name, - const BT::NodeConfiguration & conf) + const BT::NodeConfig & conf) : BT::ActionNodeBase(xml_tag_name, conf), counter_(0) { } diff --git a/plansys2_bt_example/src/behavior_tree_nodes/Move.cpp b/plansys2_bt_example/src/behavior_tree_nodes/Move.cpp index 3cb4390..5248cf3 100644 --- a/plansys2_bt_example/src/behavior_tree_nodes/Move.cpp +++ b/plansys2_bt_example/src/behavior_tree_nodes/Move.cpp @@ -32,7 +32,7 @@ namespace plansys2_bt_tests Move::Move( const std::string & xml_tag_name, const std::string & action_name, - const BT::NodeConfiguration & conf) + const BT::NodeConfig & conf) : plansys2::BtActionNode(xml_tag_name, action_name, conf) { rclcpp_lifecycle::LifecycleNode::SharedPtr node; @@ -120,7 +120,7 @@ Move::on_success() BT_REGISTER_NODES(factory) { BT::NodeBuilder builder = - [](const std::string & name, const BT::NodeConfiguration & config) + [](const std::string & name, const BT::NodeConfig & config) { return std::make_unique( name, "navigate_to_pose", config); diff --git a/plansys2_bt_example/src/behavior_tree_nodes/OpenGripper.cpp b/plansys2_bt_example/src/behavior_tree_nodes/OpenGripper.cpp index 1c21991..5f63c94 100644 --- a/plansys2_bt_example/src/behavior_tree_nodes/OpenGripper.cpp +++ b/plansys2_bt_example/src/behavior_tree_nodes/OpenGripper.cpp @@ -24,7 +24,7 @@ namespace plansys2_bt_example OpenGripper::OpenGripper( const std::string & xml_tag_name, - const BT::NodeConfiguration & conf) + const BT::NodeConfig & conf) : BT::ActionNodeBase(xml_tag_name, conf), counter_(0) { } diff --git a/plansys2_bt_example/src/behavior_tree_nodes/Recharge.cpp b/plansys2_bt_example/src/behavior_tree_nodes/Recharge.cpp index 0502114..d8967c9 100644 --- a/plansys2_bt_example/src/behavior_tree_nodes/Recharge.cpp +++ b/plansys2_bt_example/src/behavior_tree_nodes/Recharge.cpp @@ -24,7 +24,7 @@ namespace plansys2_bt_example Recharge::Recharge( const std::string & xml_tag_name, - const BT::NodeConfiguration & conf) + const BT::NodeConfig & conf) : BT::ActionNodeBase(xml_tag_name, conf), counter_(0) { } diff --git a/plansys2_bt_example/src/nav2_sim_node.cpp b/plansys2_bt_example/src/nav2_sim_node.cpp index 1d3c649..e9cd798 100644 --- a/plansys2_bt_example/src/nav2_sim_node.cpp +++ b/plansys2_bt_example/src/nav2_sim_node.cpp @@ -17,7 +17,9 @@ #include "rclcpp/rclcpp.hpp" #include "rclcpp_action/rclcpp_action.hpp" #include "nav2_msgs/action/navigate_to_pose.hpp" +#include "nav2_msgs/action/compute_path_to_pose.hpp" #include "tf2_geometry_msgs/tf2_geometry_msgs.hpp" +#include using std::placeholders::_1; using std::placeholders::_2; @@ -26,7 +28,9 @@ class Nav2Sim : public rclcpp::Node { public: using NavigateToPose = nav2_msgs::action::NavigateToPose; + using ComputePathToPose = nav2_msgs::action::ComputePathToPose; using GoalHandleNavigateToPose = rclcpp_action::ServerGoalHandle; + using GoalHandleComputePathToPose = rclcpp_action::ServerGoalHandle; Nav2Sim() : Node("navigate_to_pose_server") @@ -43,12 +47,21 @@ class Nav2Sim : public rclcpp::Node std::bind(&Nav2Sim::handle_goal, this, _1, _2), std::bind(&Nav2Sim::handle_cancel, this, _1), std::bind(&Nav2Sim::handle_accepted, this, _1)); + + compute_path_to_pose_server_ = rclcpp_action::create_server( + shared_from_this(), + "compute_path_to_pose", + std::bind(&Nav2Sim::handle_goal_compute_path, this, _1, _2), + std::bind(&Nav2Sim::handle_cancel_compute_path, this, _1), + std::bind(&Nav2Sim::handle_accepted_compute_path, this, _1)); RCLCPP_INFO(get_logger(), "Ready."); } private: rclcpp_action::Server::SharedPtr repeat_sentence_action_server_; + rclcpp_action::Server::SharedPtr compute_path_to_pose_server_; + NavigateToPose::Goal current_goal_; rclcpp_action::GoalResponse handle_goal( @@ -58,6 +71,14 @@ class Nav2Sim : public rclcpp::Node return rclcpp_action::GoalResponse::ACCEPT_AND_EXECUTE; } + rclcpp_action::GoalResponse handle_goal_compute_path( + const rclcpp_action::GoalUUID & uuid, + std::shared_ptr goal) + { + return rclcpp_action::GoalResponse::ACCEPT_AND_EXECUTE; + } + + rclcpp_action::CancelResponse handle_cancel( const std::shared_ptr goal_handle) { @@ -65,6 +86,73 @@ class Nav2Sim : public rclcpp::Node return rclcpp_action::CancelResponse::ACCEPT; } + rclcpp_action::CancelResponse handle_cancel_compute_path( + const std::shared_ptr goal_handle) + { + RCLCPP_INFO(this->get_logger(), "Received request to cancel goal"); + return rclcpp_action::CancelResponse::ACCEPT; + } + + void execute_compute_path(const std::shared_ptr goal_handle) + { + rclcpp::Rate loop_rate(1); + auto feedback = std::make_shared(); + auto result = std::make_shared(); + + // auto pose_cmd = goal_handle->get_goal()->pose.pose; + auto start_pose = goal_handle->get_goal()->start.pose; + auto goal_pose = goal_handle->get_goal()->goal.pose; + + tf2::Quaternion q; + tf2::fromMsg(start_pose.orientation, q); + + RCLCPP_INFO( + this->get_logger(), "Starting compute path to %lf, %lf, %lf", + start_pose.position.x, start_pose.position.y, q.getAngle()); + + auto start = now(); + int current_times = 0; + + std::random_device rd; + std::mt19937 gen(rd()); + std::uniform_int_distribution<> dis(1, 10); + + while (rclcpp::ok() && current_times++ < dis(gen)) { + RCLCPP_INFO(this->get_logger(), "Computing path %d ", current_times); + + if (goal_handle->is_canceling()) { + goal_handle->canceled(result); + + RCLCPP_INFO(this->get_logger(), "Action Canceled"); + + return; + } + + loop_rate.sleep(); + } + + // Fake Path + nav_msgs::msg::Path fake_path; + fake_path.header.frame_id = "map"; + + geometry_msgs::msg::PoseStamped start_pose_msg; + start_pose_msg.pose = start_pose; + fake_path.poses.push_back(start_pose_msg); + + geometry_msgs::msg::PoseStamped goal_pose_msg; + goal_pose_msg.pose = goal_pose; + fake_path.poses.push_back(goal_pose_msg); + + result->path = fake_path; + + + if (rclcpp::ok()) { + goal_handle->succeed(result); + + RCLCPP_INFO(this->get_logger(), "Compute Path Succeeded"); + } + } + void execute(const std::shared_ptr goal_handle) { rclcpp::Rate loop_rate(1); @@ -107,6 +195,11 @@ class Nav2Sim : public rclcpp::Node using namespace std::placeholders; std::thread{std::bind(&Nav2Sim::execute, this, _1), goal_handle}.detach(); } + void handle_accepted_compute_path(const std::shared_ptr goal_handle) + { + using namespace std::placeholders; + std::thread{std::bind(&Nav2Sim::execute_compute_path, this, _1), goal_handle}.detach(); + } }; diff --git a/plansys2_patrol_navigation_example/launch/patrol_example_launch.py b/plansys2_patrol_navigation_example/launch/patrol_example_launch.py index 31f9dde..7199de4 100644 --- a/plansys2_patrol_navigation_example/launch/patrol_example_launch.py +++ b/plansys2_patrol_navigation_example/launch/patrol_example_launch.py @@ -64,7 +64,7 @@ def generate_launch_description(): # Declare the launch options ld.add_action(plansys2_cmd) - ld.add_action(nav2_cmd) + # ld.add_action(nav2_cmd) ld.add_action(move_cmd) ld.add_action(patrol_cmd)