From 184d95b214e3c149671c872ce7371c675976482f Mon Sep 17 00:00:00 2001 From: brettpac Date: Thu, 26 Sep 2024 07:39:39 -0700 Subject: [PATCH] nitros client creation (#560) * nitros client creation * precommit * sm_panda_inventory keyboard addition * Adding keyboard server to arm launch files * sm_panda_1 tune up --------- Co-authored-by: brettpac --- .../nitrosz_client/CHANGELOG.rst | 13097 ++++++++++++++++ .../nitrosz_client/CMakeLists.txt | 119 + .../nitrosz_client/client_behaviors.hpp | 45 + .../client_behaviors/cb_abort_navigation.hpp | 48 + .../client_behaviors/cb_absolute_rotate.hpp | 51 + .../client_behaviors/cb_active_stop.hpp | 40 + .../cb_load_waypoints_file.hpp | 46 + .../cb_navigate_backwards.hpp | 50 + .../client_behaviors/cb_navigate_forward.hpp | 79 + .../cb_navigate_global_position.hpp | 68 + .../cb_navigate_named_waypoint.hpp | 45 + .../cb_navigate_next_waypoint.hpp | 47 + .../cb_navigate_next_waypoint_free.hpp | 44 + ...b_navigate_next_waypoint_until_reached.hpp | 103 + .../cb_nitrosz_client_behavior_base.hpp | 67 + .../client_behaviors/cb_pause_slam.hpp | 37 + .../cb_position_control_free_space.hpp | 64 + .../client_behaviors/cb_pure_spinning.hpp | 48 + .../client_behaviors/cb_resume_slam.hpp | 37 + .../client_behaviors/cb_retry_behavior.hpp | 51 + .../client_behaviors/cb_rotate.hpp | 47 + .../client_behaviors/cb_rotate_look_at.hpp | 43 + .../client_behaviors/cb_save_slam_map.hpp | 46 + .../client_behaviors/cb_seek_waypoint.hpp | 48 + .../client_behaviors/cb_stop_navigation.hpp | 38 + .../cb_undo_path_backwards.hpp | 56 + .../client_behaviors/cb_wait_nitros_nodes.hpp | 67 + .../client_behaviors/cb_wait_pose.hpp | 59 + .../client_behaviors/cb_wait_transform.hpp | 60 + .../include/nitrosz_client/common.hpp | 34 + .../components/amcl/cp_amcl.hpp | 43 + .../costmap_switch/cp_costmap_switch.hpp | 92 + .../cp_goal_checker_switcher.hpp | 47 + .../odom_tracker/cp_odom_tracker.hpp | 189 + .../planner_switcher/cp_planner_switcher.hpp | 70 + .../components/pose/cp_pose.hpp | 95 + .../slam_toolbox/cp_slam_toolbox.hpp | 50 + .../cp_waypoints_event_dispatcher.hpp | 1864 +++ .../cp_waypoints_navigator.hpp | 85 + .../cp_waypoints_navigator_base.hpp | 102 + .../cp_waypoints_visualizer.hpp | 58 + .../include/nitrosz_client/nitrosz_client.hpp | 44 + .../nitrosz_client/nitrosz_client/package.xml | 37 + .../scripts/lidar_completion.py | 105 + .../client_behaviors/cb_abort_navigation.cpp | 44 + .../client_behaviors/cb_absolute_rotate.cpp | 270 + .../client_behaviors/cb_active_stop.cpp | 53 + .../cb_load_waypoints_file.cpp | 57 + .../client_behaviors/cb_navigate_backward.cpp | 100 + .../client_behaviors/cb_navigate_forward.cpp | 158 + .../cb_navigate_global_position.cpp | 109 + .../cb_navigate_named_waypoint.cpp | 59 + .../cb_navigate_next_waypoint.cpp | 61 + .../cb_navigate_next_waypoint_free.cpp | 54 + ...b_navigate_next_waypoint_until_reached.cpp | 37 + .../cb_nitrosz_client_behavior_base.cpp | 127 + .../client_behaviors/cb_pause_slam.cpp | 51 + .../cb_position_control_free_space.cpp | 199 + .../client_behaviors/cb_pure_spinning.cpp | 102 + .../client_behaviors/cb_resume_slam.cpp | 51 + .../client_behaviors/cb_rotate.cpp | 87 + .../client_behaviors/cb_rotate_look_at.cpp | 58 + .../client_behaviors/cb_save_slam_map.cpp | 85 + .../client_behaviors/cb_seek_waypoint.cpp | 52 + .../client_behaviors/cb_stop_navigation.cpp | 52 + .../cb_undo_path_backwards.cpp | 124 + .../client_behaviors/cb_wait_nitros_nodes.cpp | 133 + .../client_behaviors/cb_wait_pose.cpp | 52 + .../client_behaviors/cb_wait_transform.cpp | 63 + .../src/nitrosz_client/common.cpp | 64 + .../components/amcl/cp_amcl.cpp | 43 + .../costmap_switch/cp_costmap_switch.cpp | 162 + .../cp_goal_checker_switcher.cpp | 75 + .../odom_tracker/cp_odom_tracker.cpp | 543 + .../odom_tracker/cp_odom_tracker_node.cpp | 123 + .../planner_switcher/cp_planner_switcher.cpp | 109 + .../components/pose/cp_pose.cpp | 153 + .../slam_toolbox/cp_slam_toolbox.cpp | 40 + .../cp_waypoints_event_dispatcher.cpp | 30 + .../cp_waypoints_navigator.cpp | 566 + .../cp_waypoints_visualizer.cpp | 156 + .../src/nitrosz_client/nitrosz_client.cpp | 39 + .../sm_multi_panda_sim/README.md | 56 + .../sm_multi_panda_sim/sm_multi_panda_sim.hpp | 55 +- .../states/st_acquire_sensors.hpp | 10 +- .../states/st_both_arms_move.hpp | 15 +- .../states/st_left_arm_moves.hpp | 16 +- .../states/st_right_arm_moves.hpp | 15 +- .../CMakeLists.txt | 11 + .../sm_panda_moveit2z_cb_inventory/README.md | 2 +- .../orthogonals/or_keyboard.hpp | 36 + .../sm_panda_moveit2z_cb_inventory.hpp | 30 +- .../states/st_acquire_sensors.hpp | 11 +- .../states/st_acquire_sensors_2.hpp | 82 + .../states/st_acquire_sensors_3.hpp | 68 + .../states/st_attach_object.hpp | 9 +- .../states/st_circular_pivot_motion.hpp | 6 + .../states/st_detach_object.hpp | 9 +- .../states/st_end_effector_rotate.hpp | 17 +- .../states/st_execute_last_trajectory.hpp | 11 +- .../states/st_move_cartesian_relative.hpp | 10 +- .../states/st_move_cartesian_relative2.hpp | 10 +- .../states/st_move_end_effector.hpp | 11 +- .../states/st_move_joints.hpp | 13 +- .../states/st_move_joints_2.hpp | 83 + .../states/st_move_joints_3.hpp | 83 + .../states/st_move_known_state.hpp | 11 +- .../st_move_last_trajectory_initial_state.hpp | 9 +- .../states/st_move_named_target.hpp | 5 + .../states/st_pouring_motion.hpp | 15 +- .../states/st_undo_last_trajectory.hpp | 12 +- .../sm_panda_moveit2z_cb_inventory.launch.py | 10 + .../package.xml | 3 + 113 files changed, 22553 insertions(+), 87 deletions(-) create mode 100644 smacc2_client_library/nitrosz_client/nitrosz_client/CHANGELOG.rst create mode 100644 smacc2_client_library/nitrosz_client/nitrosz_client/CMakeLists.txt create mode 100644 smacc2_client_library/nitrosz_client/nitrosz_client/include/nitrosz_client/client_behaviors.hpp create mode 100644 smacc2_client_library/nitrosz_client/nitrosz_client/include/nitrosz_client/client_behaviors/cb_abort_navigation.hpp create mode 100644 smacc2_client_library/nitrosz_client/nitrosz_client/include/nitrosz_client/client_behaviors/cb_absolute_rotate.hpp create mode 100644 smacc2_client_library/nitrosz_client/nitrosz_client/include/nitrosz_client/client_behaviors/cb_active_stop.hpp create mode 100644 smacc2_client_library/nitrosz_client/nitrosz_client/include/nitrosz_client/client_behaviors/cb_load_waypoints_file.hpp create mode 100644 smacc2_client_library/nitrosz_client/nitrosz_client/include/nitrosz_client/client_behaviors/cb_navigate_backwards.hpp create mode 100644 smacc2_client_library/nitrosz_client/nitrosz_client/include/nitrosz_client/client_behaviors/cb_navigate_forward.hpp create mode 100644 smacc2_client_library/nitrosz_client/nitrosz_client/include/nitrosz_client/client_behaviors/cb_navigate_global_position.hpp create mode 100644 smacc2_client_library/nitrosz_client/nitrosz_client/include/nitrosz_client/client_behaviors/cb_navigate_named_waypoint.hpp create mode 100644 smacc2_client_library/nitrosz_client/nitrosz_client/include/nitrosz_client/client_behaviors/cb_navigate_next_waypoint.hpp create mode 100644 smacc2_client_library/nitrosz_client/nitrosz_client/include/nitrosz_client/client_behaviors/cb_navigate_next_waypoint_free.hpp create mode 100644 smacc2_client_library/nitrosz_client/nitrosz_client/include/nitrosz_client/client_behaviors/cb_navigate_next_waypoint_until_reached.hpp create mode 100644 smacc2_client_library/nitrosz_client/nitrosz_client/include/nitrosz_client/client_behaviors/cb_nitrosz_client_behavior_base.hpp create mode 100644 smacc2_client_library/nitrosz_client/nitrosz_client/include/nitrosz_client/client_behaviors/cb_pause_slam.hpp create mode 100644 smacc2_client_library/nitrosz_client/nitrosz_client/include/nitrosz_client/client_behaviors/cb_position_control_free_space.hpp create mode 100644 smacc2_client_library/nitrosz_client/nitrosz_client/include/nitrosz_client/client_behaviors/cb_pure_spinning.hpp create mode 100644 smacc2_client_library/nitrosz_client/nitrosz_client/include/nitrosz_client/client_behaviors/cb_resume_slam.hpp create mode 100644 smacc2_client_library/nitrosz_client/nitrosz_client/include/nitrosz_client/client_behaviors/cb_retry_behavior.hpp create mode 100644 smacc2_client_library/nitrosz_client/nitrosz_client/include/nitrosz_client/client_behaviors/cb_rotate.hpp create mode 100644 smacc2_client_library/nitrosz_client/nitrosz_client/include/nitrosz_client/client_behaviors/cb_rotate_look_at.hpp create mode 100644 smacc2_client_library/nitrosz_client/nitrosz_client/include/nitrosz_client/client_behaviors/cb_save_slam_map.hpp create mode 100644 smacc2_client_library/nitrosz_client/nitrosz_client/include/nitrosz_client/client_behaviors/cb_seek_waypoint.hpp create mode 100644 smacc2_client_library/nitrosz_client/nitrosz_client/include/nitrosz_client/client_behaviors/cb_stop_navigation.hpp create mode 100644 smacc2_client_library/nitrosz_client/nitrosz_client/include/nitrosz_client/client_behaviors/cb_undo_path_backwards.hpp create mode 100644 smacc2_client_library/nitrosz_client/nitrosz_client/include/nitrosz_client/client_behaviors/cb_wait_nitros_nodes.hpp create mode 100644 smacc2_client_library/nitrosz_client/nitrosz_client/include/nitrosz_client/client_behaviors/cb_wait_pose.hpp create mode 100644 smacc2_client_library/nitrosz_client/nitrosz_client/include/nitrosz_client/client_behaviors/cb_wait_transform.hpp create mode 100644 smacc2_client_library/nitrosz_client/nitrosz_client/include/nitrosz_client/common.hpp create mode 100644 smacc2_client_library/nitrosz_client/nitrosz_client/include/nitrosz_client/components/amcl/cp_amcl.hpp create mode 100644 smacc2_client_library/nitrosz_client/nitrosz_client/include/nitrosz_client/components/costmap_switch/cp_costmap_switch.hpp create mode 100644 smacc2_client_library/nitrosz_client/nitrosz_client/include/nitrosz_client/components/goal_checker_switcher/cp_goal_checker_switcher.hpp create mode 100644 smacc2_client_library/nitrosz_client/nitrosz_client/include/nitrosz_client/components/odom_tracker/cp_odom_tracker.hpp create mode 100644 smacc2_client_library/nitrosz_client/nitrosz_client/include/nitrosz_client/components/planner_switcher/cp_planner_switcher.hpp create mode 100644 smacc2_client_library/nitrosz_client/nitrosz_client/include/nitrosz_client/components/pose/cp_pose.hpp create mode 100644 smacc2_client_library/nitrosz_client/nitrosz_client/include/nitrosz_client/components/slam_toolbox/cp_slam_toolbox.hpp create mode 100644 smacc2_client_library/nitrosz_client/nitrosz_client/include/nitrosz_client/components/waypoints_navigator/cp_waypoints_event_dispatcher.hpp create mode 100644 smacc2_client_library/nitrosz_client/nitrosz_client/include/nitrosz_client/components/waypoints_navigator/cp_waypoints_navigator.hpp create mode 100644 smacc2_client_library/nitrosz_client/nitrosz_client/include/nitrosz_client/components/waypoints_navigator/cp_waypoints_navigator_base.hpp create mode 100644 smacc2_client_library/nitrosz_client/nitrosz_client/include/nitrosz_client/components/waypoints_navigator/cp_waypoints_visualizer.hpp create mode 100644 smacc2_client_library/nitrosz_client/nitrosz_client/include/nitrosz_client/nitrosz_client.hpp create mode 100644 smacc2_client_library/nitrosz_client/nitrosz_client/package.xml create mode 100755 smacc2_client_library/nitrosz_client/nitrosz_client/scripts/lidar_completion.py create mode 100644 smacc2_client_library/nitrosz_client/nitrosz_client/src/nitrosz_client/client_behaviors/cb_abort_navigation.cpp create mode 100644 smacc2_client_library/nitrosz_client/nitrosz_client/src/nitrosz_client/client_behaviors/cb_absolute_rotate.cpp create mode 100644 smacc2_client_library/nitrosz_client/nitrosz_client/src/nitrosz_client/client_behaviors/cb_active_stop.cpp create mode 100644 smacc2_client_library/nitrosz_client/nitrosz_client/src/nitrosz_client/client_behaviors/cb_load_waypoints_file.cpp create mode 100644 smacc2_client_library/nitrosz_client/nitrosz_client/src/nitrosz_client/client_behaviors/cb_navigate_backward.cpp create mode 100644 smacc2_client_library/nitrosz_client/nitrosz_client/src/nitrosz_client/client_behaviors/cb_navigate_forward.cpp create mode 100644 smacc2_client_library/nitrosz_client/nitrosz_client/src/nitrosz_client/client_behaviors/cb_navigate_global_position.cpp create mode 100644 smacc2_client_library/nitrosz_client/nitrosz_client/src/nitrosz_client/client_behaviors/cb_navigate_named_waypoint.cpp create mode 100644 smacc2_client_library/nitrosz_client/nitrosz_client/src/nitrosz_client/client_behaviors/cb_navigate_next_waypoint.cpp create mode 100644 smacc2_client_library/nitrosz_client/nitrosz_client/src/nitrosz_client/client_behaviors/cb_navigate_next_waypoint_free.cpp create mode 100644 smacc2_client_library/nitrosz_client/nitrosz_client/src/nitrosz_client/client_behaviors/cb_navigate_next_waypoint_until_reached.cpp create mode 100644 smacc2_client_library/nitrosz_client/nitrosz_client/src/nitrosz_client/client_behaviors/cb_nitrosz_client_behavior_base.cpp create mode 100644 smacc2_client_library/nitrosz_client/nitrosz_client/src/nitrosz_client/client_behaviors/cb_pause_slam.cpp create mode 100644 smacc2_client_library/nitrosz_client/nitrosz_client/src/nitrosz_client/client_behaviors/cb_position_control_free_space.cpp create mode 100644 smacc2_client_library/nitrosz_client/nitrosz_client/src/nitrosz_client/client_behaviors/cb_pure_spinning.cpp create mode 100644 smacc2_client_library/nitrosz_client/nitrosz_client/src/nitrosz_client/client_behaviors/cb_resume_slam.cpp create mode 100644 smacc2_client_library/nitrosz_client/nitrosz_client/src/nitrosz_client/client_behaviors/cb_rotate.cpp create mode 100644 smacc2_client_library/nitrosz_client/nitrosz_client/src/nitrosz_client/client_behaviors/cb_rotate_look_at.cpp create mode 100644 smacc2_client_library/nitrosz_client/nitrosz_client/src/nitrosz_client/client_behaviors/cb_save_slam_map.cpp create mode 100644 smacc2_client_library/nitrosz_client/nitrosz_client/src/nitrosz_client/client_behaviors/cb_seek_waypoint.cpp create mode 100644 smacc2_client_library/nitrosz_client/nitrosz_client/src/nitrosz_client/client_behaviors/cb_stop_navigation.cpp create mode 100644 smacc2_client_library/nitrosz_client/nitrosz_client/src/nitrosz_client/client_behaviors/cb_undo_path_backwards.cpp create mode 100644 smacc2_client_library/nitrosz_client/nitrosz_client/src/nitrosz_client/client_behaviors/cb_wait_nitros_nodes.cpp create mode 100644 smacc2_client_library/nitrosz_client/nitrosz_client/src/nitrosz_client/client_behaviors/cb_wait_pose.cpp create mode 100644 smacc2_client_library/nitrosz_client/nitrosz_client/src/nitrosz_client/client_behaviors/cb_wait_transform.cpp create mode 100644 smacc2_client_library/nitrosz_client/nitrosz_client/src/nitrosz_client/common.cpp create mode 100644 smacc2_client_library/nitrosz_client/nitrosz_client/src/nitrosz_client/components/amcl/cp_amcl.cpp create mode 100644 smacc2_client_library/nitrosz_client/nitrosz_client/src/nitrosz_client/components/costmap_switch/cp_costmap_switch.cpp create mode 100644 smacc2_client_library/nitrosz_client/nitrosz_client/src/nitrosz_client/components/goal_checker_switcher/cp_goal_checker_switcher.cpp create mode 100644 smacc2_client_library/nitrosz_client/nitrosz_client/src/nitrosz_client/components/odom_tracker/cp_odom_tracker.cpp create mode 100644 smacc2_client_library/nitrosz_client/nitrosz_client/src/nitrosz_client/components/odom_tracker/cp_odom_tracker_node.cpp create mode 100644 smacc2_client_library/nitrosz_client/nitrosz_client/src/nitrosz_client/components/planner_switcher/cp_planner_switcher.cpp create mode 100644 smacc2_client_library/nitrosz_client/nitrosz_client/src/nitrosz_client/components/pose/cp_pose.cpp create mode 100644 smacc2_client_library/nitrosz_client/nitrosz_client/src/nitrosz_client/components/slam_toolbox/cp_slam_toolbox.cpp create mode 100644 smacc2_client_library/nitrosz_client/nitrosz_client/src/nitrosz_client/components/waypoints_navigator/cp_waypoints_event_dispatcher.cpp create mode 100644 smacc2_client_library/nitrosz_client/nitrosz_client/src/nitrosz_client/components/waypoints_navigator/cp_waypoints_navigator.cpp create mode 100644 smacc2_client_library/nitrosz_client/nitrosz_client/src/nitrosz_client/components/waypoints_navigator/cp_waypoints_visualizer.cpp create mode 100644 smacc2_client_library/nitrosz_client/nitrosz_client/src/nitrosz_client/nitrosz_client.cpp create mode 100644 smacc2_sm_reference_library/sm_multi_panda_sim/sm_multi_panda_sim/README.md create mode 100644 smacc2_sm_reference_library/sm_panda_moveit2z_cb_inventory/include/sm_panda_moveit2z_cb_inventory/orthogonals/or_keyboard.hpp create mode 100644 smacc2_sm_reference_library/sm_panda_moveit2z_cb_inventory/include/sm_panda_moveit2z_cb_inventory/states/st_acquire_sensors_2.hpp create mode 100644 smacc2_sm_reference_library/sm_panda_moveit2z_cb_inventory/include/sm_panda_moveit2z_cb_inventory/states/st_acquire_sensors_3.hpp create mode 100644 smacc2_sm_reference_library/sm_panda_moveit2z_cb_inventory/include/sm_panda_moveit2z_cb_inventory/states/st_move_joints_2.hpp create mode 100644 smacc2_sm_reference_library/sm_panda_moveit2z_cb_inventory/include/sm_panda_moveit2z_cb_inventory/states/st_move_joints_3.hpp diff --git a/smacc2_client_library/nitrosz_client/nitrosz_client/CHANGELOG.rst b/smacc2_client_library/nitrosz_client/nitrosz_client/CHANGELOG.rst new file mode 100644 index 000000000..df463a5a6 --- /dev/null +++ b/smacc2_client_library/nitrosz_client/nitrosz_client/CHANGELOG.rst @@ -0,0 +1,13097 @@ +^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Changelog for package nitrosz_client +^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ + +2.3.16 (2023-07-16) +------------------- +* Merge branch 'humble' of https://github.com/robosoft-ai/SMACC2 into humble +* Brettpac branch (`#518 `_) + * Attempt to fix weird issue with ros buildfarm + * More on this buildfarm issue + --------- + Co-authored-by: brettpac +* Contributors: brettpac, pabloinigoblasco + +2.3.6 (2023-03-12) +------------------ + +1.22.1 (2022-11-09) +------------------- +* pre-release +* Contributors: pabloinigoblasco + +* pre-release +* Contributors: pabloinigoblasco + +* more progress in humble SMACC2 deb generation +* humble check +* publisher +* progress in migration to humble +* Feature/fix mutex galactic (#319) + * bug fix galactic mutex + * testing undo motion and also improving action client + * important refactoring smacc action client + * fix + * progress in smacc action client + * progress in the smacc action client fork based on signals + * more changes + * more testing + * more testing + * more testing on abort + * minor + * adding smaccServiceerver client to galactic + * Update cb_default_keyboard_behavior.hpp + * testing more husky robot + * progress in tests husky demo + * testing abort forward and undo + * finishing cancel and undo behavior tests +* Undo motion in stEvasion after detecting enemy - in testing (#315) + * Undo motion in stEvasion after detecting enemy - in testing + * minor format + * putting in green SMACC2 +* Feature/husky barrel improvements (#314) + * improvements in navigation client behaviors and husky barrel demo + * many improvements in action client and cb sequence for hysky barrel search + * more and better navigation behaviors on husky barrel search demo + * functionality improvements in navigation and improvements of warehouse 3, format + * functionality improvements in navigation and improvements of warehouse 3 and husky + * format + * warehouse 3 improvements + * merge galactic + * merge fix + * minor + * dead branch for husky barrel sm and opencv funcionalities + * final s-pattern and final attack + * format and other + * refining final attack state and also retry states for s-pattern + * fixing PR green + * minor changes +* improvements in navigation client behaviors and husky barrel demo (#311) + * improvements in navigation client behaviors and husky barrel demo + * many improvements in action client and cb sequence for hysky barrel search + * more and better navigation behaviors on husky barrel search demo + * functionality improvements in navigation and improvements of warehouse 3, format + * functionality improvements in navigation and improvements of warehouse 3 and husky + * format +* husky_improvements (#299) + * husky_improvements + * different planners profiles for navigation + * getting changes from galactic + * planner switcher + * using galactic branch files + * fixing breaking changes + * minor fix + * removing nav from source files + * merge +* Feature/barrel husky improvements (#293) + * Rename to smacc2 and smacc2_msgs + * Correct GitHub branch reference. + * Update name of package and package.xml to pass liter. + * Execute on master update + * Reset all versions to 0.0.0 + * Ignore all packages except smacc2 and smacc2_msgs + * Update changelogs + * 0.1.0 + * Revert "Ignore all packages except smacc2 and smacc2_msgs" + This reverts commit f603166a4b3ccdfe96c64d9f9fb9d8b49fbf0e61. + * Update description table. + * Update table + * Copy initial docs + * Dockerfile w/ ROS distro as argument + use this command "sudo docker build --build-arg ROS2_DISTRO=(desiredRosTag) (directoryHoldingDockerfile)/" + * Opened new folder for additional tracing contents + * Delete tracing directory + * Moved tracing.md to tracing directory + * added setupTracing.sh + Installs necessary packages and configures tracing group. + * Removed manual installation of ros-rolling-ros2trace + This is now automated in setupTracing.sh + location of sh file assumed if user follows README.md under "Getting started" + * Created alternative ManualTracing + * added new sm markdowns + * added a dockerfile for Rolling and Galactic + * Update smacc2_ci/docker/ros_rollingAndGalactic_ubuntu_20.04/buildGalactic.sh + Co-authored-by: Denis Štogl + * Update tracing/ManualTracing.md + Co-authored-by: Denis Štogl + * changed wording "smacc application" to "SMACC2 library" + Co-authored-by: Denis Štogl + * Update smacc_sm_reference_library/sm_atomic/README.md + edit from html to markdown syntax + Co-authored-by: Denis Štogl + * reactivating smacc2 nav clients for rolling via submodules + * renamed tracing events after + * bug in smacc2 component + * reverted markdowns to html + * added README tutorial for Dockerfile + * additional cleanup + * cleanup + * cleanup + * edited tracing.md to reflect new tracing event names + * Enable build of missing rolling repositories. + * Enable Navigation2 for semi-binary build. + * Remove galactic builds from master and kepp only rolling. Remove submodules and use .repos file + * updated mentions of SMACC/ROS to SMACC2/ROS2 + * some progress on navigation rolling + * renamed folders, deleted tracing.md, edited README.md + * added smacc2_performance_tools + * performance tests improvements + * more on performance and other issues + * sm_respira_1 format cleanup + * sm_respira_1 format cleanup pre-commit + * sm_respira_test_2 + * sm_respira_test_2 + * more changes on performance tests + * Do not execute clang-format on smacc2_sm_reference_library package. + * sm_reference_library reformatting + * Correct trailing spaces. + * sm_atomic_24hr + * sm_atomic_performance_trace_1 + * Update smacc2_rta command across readmes + * Clean up of sm_atomic_24hr + * more sm_atomic_24hr cleanup + * Optimized deps in move_base_z_planners_common. + * Renaming of event generator library + * minor formatting + * Add galactic CI setup and rename rolling files. (#58) + * Fix source CI and correct README overview. (#62) + * Update c_cpp_properties.json + * changed launch command to ros2 launch sm_respira_1 sm_respira_1.launch (#69) + also noticed a note I had made while producing these that was not removed + * update doxygen links (#70) + Co-authored-by: Ubuntu 20-04-02-amd64 + * More Readme Updates (#72) + Co-authored-by: Ubuntu 20-04-02-amd64 + * More Readme (#74) + Co-authored-by: Ubuntu 20-04-02-amd64 + * created new sm from sm_respira_1 (#76) + * Feature/core and navigation fixes (#78) + * base for the sm_aws_aarehouse navigation + * progressing in aws navigation + * minor + * several core improvements during navigation testing + * formatting improvements + * progress in aws navigation demo + * format improvements + * Feature/aws demo progress (#80) + * base for the sm_aws_aarehouse navigation + * progressing in aws navigation + * minor + * several core improvements during navigation testing + * formatting improvements + * progress in aws navigation demo + * format improvements + * format improvements + * more on navigation + * sm_advanced_recovery_1 reworked (#83) + * sm_advanced_recovery_1 reworked + * fix pre-commit + * Trying to fix Pre-Commit + Co-authored-by: Ubuntu 20-04-02-amd64 + * more sm_advanced_recovery_1 (#84) + Co-authored-by: Ubuntu 20-04-02-amd64 + * More sm_advanced_recovery_1 work (#85) + Co-authored-by: Ubuntu 20-04-02-amd64 + * sm_advanced_recovery_1 round 4 (#86) + Co-authored-by: Ubuntu 20-04-02-amd64 + * Brettpac branch (#87) + * sm_atomic_performance_test_a_2 + * sm_atomic_performance_test_a_1 + Co-authored-by: Ubuntu 20-04-02-amd64 + * sm_atomic_performance_test_c_1 (#88) + Co-authored-by: Ubuntu 20-04-02-amd64 + * modifying sm_atomic_performance_test_a_2 (#89) + Co-authored-by: Ubuntu 20-04-02-amd64 + * sm_multi_stage_1 (#90) + * sm_multi_stage_1 + * fixing precommit + Co-authored-by: Ubuntu 20-04-02-amd64 + * more sm_multi_stage_1 (#91) + Co-authored-by: Ubuntu 20-04-02-amd64 + * Update README.md + updated launch command + * Wait topic message client behavior (#81) + * base for the sm_aws_aarehouse navigation + * progressing in aws navigation + * minor + * several core improvements during navigation testing + * formatting improvements + * progress in aws navigation demo + * format improvements + * format improvements + * more on navigation + * new feature, cb_wait_topic_message: asynchronous client behavior that waits a topic message and optionally checks its contents to success + * formatting + * attempting precommit fixes + Co-authored-by: Ubuntu 20-04-02-amd64 + Co-authored-by: Denis Štogl + * Feature/wait nav2 nodes client behavior (#82) + * base for the sm_aws_aarehouse navigation + * progressing in aws navigation + * minor + * several core improvements during navigation testing + * formatting improvements + * progress in aws navigation demo + * format improvements + * format improvements + * more on navigation + * new feature, cb_wait_topic_message: asynchronous client behavior that waits a topic message and optionally checks its contents to success + * formatting + * adding new client behavior add for nav2, wait nav2 nodes subscribing to the /bond topic and waiting they are alive. you optionally can select the nodes to wait + * Correct all linters and formaters. + Co-authored-by: Denis Štogl + Co-authored-by: Denis Štogl + * Feature/aws demo progress (#92) + * base for the sm_aws_aarehouse navigation + * progressing in aws navigation + * minor + * several core improvements during navigation testing + * formatting improvements + * progress in aws navigation demo + * format improvements + * format improvements + * more on navigation + * new feature, cb_wait_topic_message: asynchronous client behavior that waits a topic message and optionally checks its contents to success + * formatting + * adding new client behavior add for nav2, wait nav2 nodes subscribing to the /bond topic and waiting they are alive. you optionally can select the nodes to wait + * progress in aws navigation demo + * minor format + * Feature/sm dance bot fixes (#93) + * base for the sm_aws_aarehouse navigation + * progressing in aws navigation + * minor + * several core improvements during navigation testing + * formatting improvements + * progress in aws navigation demo + * format improvements + * format improvements + * more on navigation + * new feature, cb_wait_topic_message: asynchronous client behavior that waits a topic message and optionally checks its contents to success + * formatting + * adding new client behavior add for nav2, wait nav2 nodes subscribing to the /bond topic and waiting they are alive. you optionally can select the nodes to wait + * progress in aws navigation demo + * minor format + * navigation parameters fixes on sm_dance_bot + * minor format + * Feature/sm aws warehouse (#94) + * base for the sm_aws_aarehouse navigation + * progressing in aws navigation + * minor + * several core improvements during navigation testing + * formatting improvements + * progress in aws navigation demo + * format improvements + * format improvements + * more on navigation + * new feature, cb_wait_topic_message: asynchronous client behavior that waits a topic message and optionally checks its contents to success + * formatting + * adding new client behavior add for nav2, wait nav2 nodes subscribing to the /bond topic and waiting they are alive. you optionally can select the nodes to wait + * progress in aws navigation demo + * minor format + * merge and progress + * fix format + * Feature/sm dance bot fixes (#95) + * base for the sm_aws_aarehouse navigation + * progressing in aws navigation + * minor + * several core improvements during navigation testing + * formatting improvements + * progress in aws navigation demo + * format improvements + * format improvements + * more on navigation + * new feature, cb_wait_topic_message: asynchronous client behavior that waits a topic message and optionally checks its contents to success + * formatting + * adding new client behavior add for nav2, wait nav2 nodes subscribing to the /bond topic and waiting they are alive. you optionally can select the nodes to wait + * progress in aws navigation demo + * minor format + * navigation parameters fixes on sm_dance_bot + * minor format + * minor + * minor format + * Remove some compile warnings. (#96) + * Feature/cb pause slam (#98) + * base for the sm_aws_aarehouse navigation + * progressing in aws navigation + * minor + * several core improvements during navigation testing + * formatting improvements + * progress in aws navigation demo + * format improvements + * format improvements + * more on navigation + * new feature, cb_wait_topic_message: asynchronous client behavior that waits a topic message and optionally checks its contents to success + * formatting + * adding new client behavior add for nav2, wait nav2 nodes subscribing to the /bond topic and waiting they are alive. you optionally can select the nodes to wait + * progress in aws navigation demo + * minor format + * navigation parameters fixes on sm_dance_bot + * minor format + * minor + * formatting + * cb pause slam client behavior + * sm_dance_bot_lite (#99) + * sm_dance_bot_lite + * precommit + * Updates yaml + Co-authored-by: Ubuntu 20-04-02-amd64 + * Rename doxygen deployment workflow (#100) + * minor hotfix + * sm_dance_bot visualizing turtlebot3 (#101) + * Feature/dance bot launch gz lidar choice (#102) + * sm_dance_bot visualizing turtlebot3 + * cleaning and lidar show/hide option + * cleaning files and making formatting work + * more fixes + * Feature/sm dance bot lite gazebo fixes (#104) + * sm_dance_bot visualizing turtlebot3 + * cleaning and lidar show/hide option + * cleaning files and making formatting work + * more fixes + * gazebo fixes, to show the robot and the lidar + * format fixes + * sm_multi_stage_1 doubling (#103) + Co-authored-by: Ubuntu 20-04-02-amd64 + * Feature/sm dance bot strikes back gazebo fixes (#105) + * sm_dance_bot visualizing turtlebot3 + * cleaning and lidar show/hide option + * cleaning files and making formatting work + * more fixes + * gazebo fixes, to show the robot and the lidar + * format fixes + * gazebo fixes for sm_dance_bot_strikes_back + * precommit cleanup run (#106) + Co-authored-by: Ubuntu 20-04-02-amd64 + * aws demo (#108) + * aws demo + * format + * got sm_multi_stage_1 working (barely) (#109) + Co-authored-by: Ubuntu 20-04-02-amd64 + * Brettpac branch (#110) + * got sm_multi_stage_1 working (barely) + * gaining traction sm_multi_stage_1 + * more + * don't remember + * making progress + * More + * keep hammering + * two stages + * 3 part + * 4th stage + Co-authored-by: Ubuntu 20-04-02-amd64 + * Brettpac branch (#111) + * got sm_multi_stage_1 working (barely) + * gaining traction sm_multi_stage_1 + * more + * don't remember + * making progress + * More + * keep hammering + * two stages + * 3 part + * 4th stage + * 5th stage + Co-authored-by: Ubuntu 20-04-02-amd64 + * a3 (#113) + Co-authored-by: Ubuntu 20-04-02-amd64 + * Remove neo_simulation2 package. (#112) + * Remove neo_simulation2 package. + * Correct formatting. + * Enable source build on PR for testing. + * Adjust build packages of source CI + * more sm_multi_stage_1 (#114) + Co-authored-by: Ubuntu 20-04-02-amd64 + * mm (#115) + Co-authored-by: Ubuntu 20-04-02-amd64 + * diverse improvements navigation and performance (#116) + * diverse improvements navigation and performance + * minor + Co-authored-by: pabloinigoblasco + * Feature/diverse improvemets navigation performance (#117) + * diverse improvements navigation and performance + * minor + * additional linting and formatting + * Remove merge markers from a python file. (#119) + * Feature/slam toggle and smacc deep history (#122) + * progress in navigation, slam toggle client behaviors and slam_toolbox components. Also smacc2::deep_history syntax + * going forward in testing sm_dance_bot introducing slam pausing/resuming funcionality + * feature/more_sm_dance_bot_fixes + * minor format + * minor (#124) + Co-authored-by: Ubuntu 20-04-02-amd64 + * more changes in sm_dance_bot (#125) + * Move method after the method it calls. Otherwise recursion could happen. (#126) + * Feature/dance bot s pattern (#128) + * more changes in sm_dance_bot + * polishing sm_dance_bot and s-pattern + * noticed typo + Finnaly > Finally + * Feature/dance bot s pattern (#129) + * more changes in sm_dance_bot + * polishing sm_dance_bot and s-pattern + * more refinement in sm_dance_bot + * First working version of sm template and template generator. (#127) + * minor tweaks (#130) + Co-authored-by: Ubuntu 20-04-02-amd64 + * Feature/sm dance bot refine (#131) + * more changes in sm_dance_bot + * minor + * Feature/sm dance bot refine 2 (#132) + * more changes in sm_dance_bot + * minor + * build fix + * waypoints navigator bug (#133) + * minor tuning to mitigate overshot issue cases + * progress in the sm_dance_bot tests (#135) + * some more progress on markers cleanup + * minor format issues (#134) + * sm_dance_bot_lite (#136) + Co-authored-by: Ubuntu 20-04-02-amd64 + * Resolve compile wanings (#137) + * Add SM core test (#138) + * minor navigation improvements (#141) + * using local action msgs (#139) + * using local action msgs + * removing sm_dance_bot_msgs + * pending references + * Feature/nitrosz renaming (#144) + * using local action msgs + * removing sm_dance_bot_msgs + * pending references + * navigation 2 stack renaming + * formatting + * added SVGs to READMEs of atomic, dance_bot, and others (#140) + * added remaining SVGs to READMEs (#145) + * added remaining SVGs to READMEs + * precommit cleanup + * Update package list. (#142) + * removing parameters smacc (#147) + * removing parameters smacc + * workflows update + * workflow + * Noticed launch command was incorrect in README.md + fixed launch command for sm_dance_bot_strikes_back and removed some comments I had made in the past. + * Fix CI: format fix python version (#148) + * Add SM Atomic SM generator. (#143) + * Remove node creation and create only a logger. (#149) + * Rolling Docker environment to be executed from any environment (#154) + * Feature/sm dance bot strikes back refactoring (#152) + Co-authored-by: DecDury + Co-authored-by: Denis Štogl + * slight waypoint 4 and iterations changes so robot can complete course (#155) + * Feature/migration moveit client (#151) + * initial migration to smacc2 + * fixing some errors introduced on formatting + * missing dependency + * fixing some more linting warnings + * minor + * removing test from main moveit cmake + * test ur5 + * progressing in the moveit migration testing + * updating format + * adding .reps dependencies and also fixing some build errors + * repos dependency + * adding dependency to ur5 client + * docker refactoring + * minor + * progress on move_it PR + * minor dockerfile test workaround + * improving dockerfile for building local tests + * minor + * fixing compiling issues + * update readme (#164) + * update readme + * more readme updates + * more + Co-authored-by: Ubuntu 20-04-02-amd64 + * initial state machine transition timestamp (#165) + * moved reference library SMs to smacc2_performance_tools (#166) + * moved reference library SMs to smacc2_performance_tools + * pre-commit cleanup + * Add QOS durability to SmaccPublisherClient (#163) + * feat: add qos durability to SmaccPublisherClient + * fix: add a missing colon + * refactor: remove line + * feat: add reliability qos config + * Feature/testing moveit behaviors (#167) + * more testing on moveit + * progress on moveit + * more testing on moveit behaviors + * minor configuration + * fixing pipeline error + * fixing broken master build + * sm_pubsub_1 (#169) + Co-authored-by: Ubuntu 20-04-02-amd64 + * sm_pubsub_1 part 2 (#170) + * sm_pubsub_1 part 2 + * sm_pubsub_1 part 2 + Co-authored-by: Ubuntu 20-04-02-amd64 + * sm_advanced_recovery_1 renaming (#171) + Co-authored-by: Ubuntu 20-04-02-amd64 + * sm_multi_stage_1 reworking (#172) + * multistage modes + * sm_multi_stage sequences + * sm_multi_state_1 steps + * sm_multi_stage_1 sequence d + * sm_multi_stage_1 c sequence + * mode_5_sequence_b + * mode_4_sequence_b + * sm_multi_stage_1 most + * finishing touches 1 + * readme + Co-authored-by: Ubuntu 20-04-02-amd64 + * Feature/aws navigation sm dance bot (#174) + * repo dependency + * husky launch file in sm_dance_bot + * Add dependencies for husky simulation. + * Fix formatting. + * Update dependencies for husky in rolling and galactic. + * minor + * progress on aws navigation and some other refactorings on navigation clients and behaviors + * more on aws demo + * fixing broken build + * minor + Co-authored-by: Denis Štogl + Co-authored-by: Denis Štogl + * minor changes (#175) + * warehouse2 (#177) + * Waypoint Inputs (#178) + Co-authored-by: Ubuntu 20-04-02-amd64 + * wharehouse2 progress (#179) + * format (#180) + * sm_dance_bot_warehouse_3 (#181) + Co-authored-by: Ubuntu 20-04-02-amd64 + * Feature/sm warehouse 2 13 dec 2 (#182) + * format + * more changes and headless + * merge + * headless and other fixes + * default values + * Brettpac branch (#184) + * sm_dance_bot_warehouse_3 + * Redoing sm_dance_bot_warehouse_3 waypoints + * More Waypoints + Co-authored-by: Ubuntu 20-04-02-amd64 + * SrConditional fixes and formatting (#168) + * fix: some formatting and templating on SrConditional + * fix: move trigger logic into headers + * fix: lint + * Feature/wharehouse2 dec 14 (#185) + * warehouse2 + * minor + * Feature/sm warehouse 2 13 dec 2 (#186) + * format + * more changes and headless + * merge + * headless and other fixes + * default values + * minor + * finetuning waypoints (#187) + Co-authored-by: Ubuntu 20-04-02-amd64 + * Feature/cb pure spinning (#188) + * format + * more changes and headless + * merge + * headless and other fixes + * default values + * minor + * Feature/cb pure spinning (#189) + * format + * more changes and headless + * merge + * headless and other fixes + * default values + * minor + * pure spinning behavior missing files + * minor changes (#190) + * Feature/planner changes 16 12 (#191) + * minor changes + * more fixes + * minor + * minor + * Feature/replanning 16 dec (#193) + * minor changes + * replanning for all our examples + * several fixes (#194) + * minor changes (#195) + * Feature/undo motion 20 12 (#196) + * minor changes + * replanning for all our examples + * improving undo motion navigation warehouse2 + * minor + * tuning warehouse3 (#197) + * Feature/undo motion 20 12 (#198) + * minor changes + * replanning for all our examples + * improving undo motion navigation warehouse2 + * minor + * undo tuning and errors + * format + * Feature/sync 21 12 (#199) + * minor changes + * replanning for all our examples + * format issues + * Feature/warehouse2 22 12 (#200) + * minor changes + * replanning for all our examples + * format issues + * finishing warehouse2 + * Feature/warehouse2 23 12 (#201) + * minor changes + * replanning for all our examples + * tuning and fixes (#202) + * Feature/minor tune (#203) + * tuning and fixes + * minor tune + * fixing warehouse 3 problems, and other core improvements (#204) + * fixing warehouse 3 problems, and other core improvements to remove dead lock, also making continuous integration green + * weird moveit not downloaded repo + * added missing file from warehouse2 (#205) + * Update cb_navigate_global_position.hpp + * Merging code from backport foxy and updates about autoware (#208) + * minor changes + * replanning for all our examples + * backport to foxy + * minor format + * minor linking errors foxy + * Foxy backport (#206) + * minor formatting fixes + * Fix trailing spaces. + * Correct codespell. + * Correct python linters warnings. + * Add galactic CI build because Navigation2 is broken in rolling. + * Add partial changes for ament_cpplint. + * Add tf2_ros as dependency to find include. + * Disable ament_cpplint. + * Disable some packages and update workflows. + * Bump ccache version. + * Ignore further packages + * Satisfy ament_lint_cmake + * Add missing licences. + * Disable cpplint and cppcheck linters. + * Correct formatters. + * branching example + * Disable disabled packages + * Update ci-build-source.yml + * Change extension + * Change extension of imports. + * Enable cppcheck + * Correct formatting of python file. + * Included necessary package and edited Threesome launch + Changed... + ros2 launch sm_three_some sm_three_some + to + ros2 launch sm_three_some sm_three_some.launch + Added: + First ensure you have the necessary package installed. + ``` + sudo apt-get install ros-rolling-ros2trace + ``` + Then run this command. + * Rename header files and correct format. + * Add workflow for checking doc build. + * Update doxygen-check-build.yml + * Create doxygen-deploy.yml + * Use manual deployment for now. + * Create workflow for testing prerelease builds + * Use docs/ as source folder for documentation + * Use docs/ as output directory. + * Rename to smacc2 and smacc2_msgs + * Correct GitHub branch reference. + * Update name of package and package.xml to pass liter. + * Execute on master update + * Reset all versions to 0.0.0 + * Ignore all packages except smacc2 and smacc2_msgs + * Update changelogs + * 0.1.0 + * Revert "Ignore all packages except smacc2 and smacc2_msgs" + This reverts commit f603166a4b3ccdfe96c64d9f9fb9d8b49fbf0e61. + * Update description table. + * Update table + * Copy initial docs + * Dockerfile w/ ROS distro as argument + use this command "sudo docker build --build-arg ROS2_DISTRO=(desiredRosTag) (directoryHoldingDockerfile)/" + * Opened new folder for additional tracing contents + * Delete tracing directory + * Moved tracing.md to tracing directory + * added setupTracing.sh + Installs necessary packages and configures tracing group. + * Removed manual installation of ros-rolling-ros2trace + This is now automated in setupTracing.sh + location of sh file assumed if user follows README.md under "Getting started" + * Created alternative ManualTracing + * added new sm markdowns + * added a dockerfile for Rolling and Galactic + * Update smacc2_ci/docker/ros_rollingAndGalactic_ubuntu_20.04/buildGalactic.sh + Co-authored-by: Denis Štogl + * Update tracing/ManualTracing.md + Co-authored-by: Denis Štogl + * changed wording "smacc application" to "SMACC2 library" + Co-authored-by: Denis Štogl + * Update smacc_sm_reference_library/sm_atomic/README.md + edit from html to markdown syntax + Co-authored-by: Denis Štogl + * reactivating smacc2 nav clients for rolling via submodules + * renamed tracing events after + * bug in smacc2 component + * reverted markdowns to html + * added README tutorial for Dockerfile + * additional cleanup + * cleanup + * cleanup + * edited tracing.md to reflect new tracing event names + * Enable build of missing rolling repositories. + * Enable Navigation2 for semi-binary build. + * Remove galactic builds from master and kepp only rolling. Remove submodules and use .repos file + * updated mentions of SMACC/ROS to SMACC2/ROS2 + * some progress on navigation rolling + * renamed folders, deleted tracing.md, edited README.md + * added smacc2_performance_tools + * performance tests improvements + * more on performance and other issues + * sm_respira_1 format cleanup + * sm_respira_1 format cleanup pre-commit + * sm_respira_test_2 + * sm_respira_test_2 + * more changes on performance tests + * Do not execute clang-format on smacc2_sm_reference_library package. + * sm_reference_library reformatting + * Correct trailing spaces. + * sm_atomic_24hr + * sm_atomic_performance_trace_1 + * Update smacc2_rta command across readmes + * Clean up of sm_atomic_24hr + * more sm_atomic_24hr cleanup + * Optimized deps in move_base_z_planners_common. + * Renaming of event generator library + * minor formatting + * Add galactic CI setup and rename rolling files. (#58) + * Fix source CI and correct README overview. (#62) + * Update c_cpp_properties.json + * changed launch command to ros2 launch sm_respira_1 sm_respira_1.launch (#69) + also noticed a note I had made while producing these that was not removed + * update doxygen links (#70) + Co-authored-by: Ubuntu 20-04-02-amd64 + * More Readme Updates (#72) + Co-authored-by: Ubuntu 20-04-02-amd64 + * More Readme (#74) + Co-authored-by: Ubuntu 20-04-02-amd64 + * created new sm from sm_respira_1 (#76) + * Feature/core and navigation fixes (#78) + * base for the sm_aws_aarehouse navigation + * progressing in aws navigation + * minor + * several core improvements during navigation testing + * formatting improvements + * progress in aws navigation demo + * format improvements + * Feature/aws demo progress (#80) + * base for the sm_aws_aarehouse navigation + * progressing in aws navigation + * minor + * several core improvements during navigation testing + * formatting improvements + * progress in aws navigation demo + * format improvements + * format improvements + * more on navigation + * sm_advanced_recovery_1 reworked (#83) + * sm_advanced_recovery_1 reworked + * fix pre-commit + * Trying to fix Pre-Commit + Co-authored-by: Ubuntu 20-04-02-amd64 + * more sm_advanced_recovery_1 (#84) + Co-authored-by: Ubuntu 20-04-02-amd64 + * More sm_advanced_recovery_1 work (#85) + Co-authored-by: Ubuntu 20-04-02-amd64 + * sm_advanced_recovery_1 round 4 (#86) + Co-authored-by: Ubuntu 20-04-02-amd64 + * Brettpac branch (#87) + * sm_atomic_performance_test_a_2 + * sm_atomic_performance_test_a_1 + Co-authored-by: Ubuntu 20-04-02-amd64 + * sm_atomic_performance_test_c_1 (#88) + Co-authored-by: Ubuntu 20-04-02-amd64 + * modifying sm_atomic_performance_test_a_2 (#89) + Co-authored-by: Ubuntu 20-04-02-amd64 + * sm_multi_stage_1 (#90) + * sm_multi_stage_1 + * fixing precommit + Co-authored-by: Ubuntu 20-04-02-amd64 + * more sm_multi_stage_1 (#91) + Co-authored-by: Ubuntu 20-04-02-amd64 + * Update README.md + updated launch command + * Wait topic message client behavior (#81) + * base for the sm_aws_aarehouse navigation + * progressing in aws navigation + * minor + * several core improvements during navigation testing + * formatting improvements + * progress in aws navigation demo + * format improvements + * format improvements + * more on navigation + * new feature, cb_wait_topic_message: asynchronous client behavior that waits a topic message and optionally checks its contents to success + * formatting + * attempting precommit fixes + Co-authored-by: Ubuntu 20-04-02-amd64 + Co-authored-by: Denis Štogl + * Feature/wait nav2 nodes client behavior (#82) + * base for the sm_aws_aarehouse navigation + * progressing in aws navigation + * minor + * several core improvements during navigation testing + * formatting improvements + * progress in aws navigation demo + * format improvements + * format improvements + * more on navigation + * new feature, cb_wait_topic_message: asynchronous client behavior that waits a topic message and optionally checks its contents to success + * formatting + * adding new client behavior add for nav2, wait nav2 nodes subscribing to the /bond topic and waiting they are alive. you optionally can select the nodes to wait + * Correct all linters and formaters. + Co-authored-by: Denis Štogl + Co-authored-by: Denis Štogl + * Feature/aws demo progress (#92) + * base for the sm_aws_aarehouse navigation + * progressing in aws navigation + * minor + * several core improvements during navigation testing + * formatting improvements + * progress in aws navigation demo + * format improvements + * format improvements + * more on navigation + * new feature, cb_wait_topic_message: asynchronous client behavior that waits a topic message and optionally checks its contents to success + * formatting + * adding new client behavior add for nav2, wait nav2 nodes subscribing to the /bond topic and waiting they are alive. you optionally can select the nodes to wait + * progress in aws navigation demo + * minor format + * Feature/sm dance bot fixes (#93) + * base for the sm_aws_aarehouse navigation + * progressing in aws navigation + * minor + * several core improvements during navigation testing + * formatting improvements + * progress in aws navigation demo + * format improvements + * format improvements + * more on navigation + * new feature, cb_wait_topic_message: asynchronous client behavior that waits a topic message and optionally checks its contents to success + * formatting + * adding new client behavior add for nav2, wait nav2 nodes subscribing to the /bond topic and waiting they are alive. you optionally can select the nodes to wait + * progress in aws navigation demo + * minor format + * navigation parameters fixes on sm_dance_bot + * minor format + * Feature/sm aws warehouse (#94) + * base for the sm_aws_aarehouse navigation + * progressing in aws navigation + * minor + * several core improvements during navigation testing + * formatting improvements + * progress in aws navigation demo + * format improvements + * format improvements + * more on navigation + * new feature, cb_wait_topic_message: asynchronous client behavior that waits a topic message and optionally checks its contents to success + * formatting + * adding new client behavior add for nav2, wait nav2 nodes subscribing to the /bond topic and waiting they are alive. you optionally can select the nodes to wait + * progress in aws navigation demo + * minor format + * merge and progress + * fix format + * Feature/sm dance bot fixes (#95) + * base for the sm_aws_aarehouse navigation + * progressing in aws navigation + * minor + * several core improvements during navigation testing + * formatting improvements + * progress in aws navigation demo + * format improvements + * format improvements + * more on navigation + * new feature, cb_wait_topic_message: asynchronous client behavior that waits a topic message and optionally checks its contents to success + * formatting + * adding new client behavior add for nav2, wait nav2 nodes subscribing to the /bond topic and waiting they are alive. you optionally can select the nodes to wait + * progress in aws navigation demo + * minor format + * navigation parameters fixes on sm_dance_bot + * minor format + * minor + * minor format + * Remove some compile warnings. (#96) + * Feature/cb pause slam (#98) + * base for the sm_aws_aarehouse navigation + * progressing in aws navigation + * minor + * several core improvements during navigation testing + * formatting improvements + * progress in aws navigation demo + * format improvements + * format improvements + * more on navigation + * new feature, cb_wait_topic_message: asynchronous client behavior that waits a topic message and optionally checks its contents to success + * formatting + * adding new client behavior add for nav2, wait nav2 nodes subscribing to the /bond topic and waiting they are alive. you optionally can select the nodes to wait + * progress in aws navigation demo + * minor format + * navigation parameters fixes on sm_dance_bot + * minor format + * minor + * formatting + * cb pause slam client behavior + * sm_dance_bot_lite (#99) + * sm_dance_bot_lite + * precommit + * Updates yaml + Co-authored-by: Ubuntu 20-04-02-amd64 + * Rename doxygen deployment workflow (#100) + * minor hotfix + * sm_dance_bot visualizing turtlebot3 (#101) + * Feature/dance bot launch gz lidar choice (#102) + * sm_dance_bot visualizing turtlebot3 + * cleaning and lidar show/hide option + * cleaning files and making formatting work + * more fixes + * Feature/sm dance bot lite gazebo fixes (#104) + * sm_dance_bot visualizing turtlebot3 + * cleaning and lidar show/hide option + * cleaning files and making formatting work + * more fixes + * gazebo fixes, to show the robot and the lidar + * format fixes + * sm_multi_stage_1 doubling (#103) + Co-authored-by: Ubuntu 20-04-02-amd64 + * Feature/sm dance bot strikes back gazebo fixes (#105) + * sm_dance_bot visualizing turtlebot3 + * cleaning and lidar show/hide option + * cleaning files and making formatting work + * more fixes + * gazebo fixes, to show the robot and the lidar + * format fixes + * gazebo fixes for sm_dance_bot_strikes_back + * precommit cleanup run (#106) + Co-authored-by: Ubuntu 20-04-02-amd64 + * aws demo (#108) + * aws demo + * format + * got sm_multi_stage_1 working (barely) (#109) + Co-authored-by: Ubuntu 20-04-02-amd64 + * Brettpac branch (#110) + * got sm_multi_stage_1 working (barely) + * gaining traction sm_multi_stage_1 + * more + * don't remember + * making progress + * More + * keep hammering + * two stages + * 3 part + * 4th stage + Co-authored-by: Ubuntu 20-04-02-amd64 + * Brettpac branch (#111) + * got sm_multi_stage_1 working (barely) + * gaining traction sm_multi_stage_1 + * more + * don't remember + * making progress + * More + * keep hammering + * two stages + * 3 part + * 4th stage + * 5th stage + Co-authored-by: Ubuntu 20-04-02-amd64 + * a3 (#113) + Co-authored-by: Ubuntu 20-04-02-amd64 + * Remove neo_simulation2 package. (#112) + * Remove neo_simulation2 package. + * Correct formatting. + * Enable source build on PR for testing. + * Adjust build packages of source CI + * more sm_multi_stage_1 (#114) + Co-authored-by: Ubuntu 20-04-02-amd64 + * mm (#115) + Co-authored-by: Ubuntu 20-04-02-amd64 + * diverse improvements navigation and performance (#116) + * diverse improvements navigation and performance + * minor + Co-authored-by: pabloinigoblasco + * Feature/diverse improvemets navigation performance (#117) + * diverse improvements navigation and performance + * minor + * additional linting and formatting + * Remove merge markers from a python file. (#119) + * Feature/slam toggle and smacc deep history (#122) + * progress in navigation, slam toggle client behaviors and slam_toolbox components. Also smacc2::deep_history syntax + * going forward in testing sm_dance_bot introducing slam pausing/resuming funcionality + * feature/more_sm_dance_bot_fixes + * minor format + * minor (#124) + Co-authored-by: Ubuntu 20-04-02-amd64 + * more changes in sm_dance_bot (#125) + * Move method after the method it calls. Otherwise recursion could happen. (#126) + * Feature/dance bot s pattern (#128) + * more changes in sm_dance_bot + * polishing sm_dance_bot and s-pattern + * noticed typo + Finnaly > Finally + * Feature/dance bot s pattern (#129) + * more changes in sm_dance_bot + * polishing sm_dance_bot and s-pattern + * more refinement in sm_dance_bot + * First working version of sm template and template generator. (#127) + * minor tweaks (#130) + Co-authored-by: Ubuntu 20-04-02-amd64 + * Feature/sm dance bot refine (#131) + * more changes in sm_dance_bot + * minor + * Feature/sm dance bot refine 2 (#132) + * more changes in sm_dance_bot + * minor + * build fix + * waypoints navigator bug (#133) + * minor tuning to mitigate overshot issue cases + * progress in the sm_dance_bot tests (#135) + * some more progress on markers cleanup + * minor format issues (#134) + * sm_dance_bot_lite (#136) + Co-authored-by: Ubuntu 20-04-02-amd64 + * Resolve compile wanings (#137) + * Add SM core test (#138) + * minor navigation improvements (#141) + * using local action msgs (#139) + * using local action msgs + * removing sm_dance_bot_msgs + * pending references + * Feature/nitrosz renaming (#144) + * using local action msgs + * removing sm_dance_bot_msgs + * pending references + * navigation 2 stack renaming + * formatting + * added SVGs to READMEs of atomic, dance_bot, and others (#140) + * added remaining SVGs to READMEs (#145) + * added remaining SVGs to READMEs + * precommit cleanup + * Update package list. (#142) + * removing parameters smacc (#147) + * removing parameters smacc + * workflows update + * workflow + * Noticed launch command was incorrect in README.md + fixed launch command for sm_dance_bot_strikes_back and removed some comments I had made in the past. + * Fix CI: format fix python version (#148) + * Add SM Atomic SM generator. (#143) + * Remove node creation and create only a logger. (#149) + * Rolling Docker environment to be executed from any environment (#154) + * Feature/sm dance bot strikes back refactoring (#152) + Co-authored-by: DecDury + Co-authored-by: Denis Štogl + * slight waypoint 4 and iterations changes so robot can complete course (#155) + * Feature/migration moveit client (#151) + * initial migration to smacc2 + * fixing some errors introduced on formatting + * missing dependency + * fixing some more linting warnings + * minor + * removing test from main moveit cmake + * test ur5 + * progressing in the moveit migration testing + * updating format + * adding .reps dependencies and also fixing some build errors + * repos dependency + * adding dependency to ur5 client + * docker refactoring + * minor + * progress on move_it PR + * minor dockerfile test workaround + * improving dockerfile for building local tests + * minor + * fixing compiling issues + * update readme (#164) + * update readme + * more readme updates + * more + Co-authored-by: Ubuntu 20-04-02-amd64 + * initial state machine transition timestamp (#165) + * moved reference library SMs to smacc2_performance_tools (#166) + * moved reference library SMs to smacc2_performance_tools + * pre-commit cleanup + * Add QOS durability to SmaccPublisherClient (#163) + * feat: add qos durability to SmaccPublisherClient + * fix: add a missing colon + * refactor: remove line + * feat: add reliability qos config + * Feature/testing moveit behaviors (#167) + * more testing on moveit + * progress on moveit + * more testing on moveit behaviors + * minor configuration + * fixing pipeline error + * fixing broken master build + * sm_pubsub_1 (#169) + Co-authored-by: Ubuntu 20-04-02-amd64 + * sm_pubsub_1 part 2 (#170) + * sm_pubsub_1 part 2 + * sm_pubsub_1 part 2 + Co-authored-by: Ubuntu 20-04-02-amd64 + * sm_advanced_recovery_1 renaming (#171) + Co-authored-by: Ubuntu 20-04-02-amd64 + * sm_multi_stage_1 reworking (#172) + * multistage modes + * sm_multi_stage sequences + * sm_multi_state_1 steps + * sm_multi_stage_1 sequence d + * sm_multi_stage_1 c sequence + * mode_5_sequence_b + * mode_4_sequence_b + * sm_multi_stage_1 most + * finishing touches 1 + * readme + Co-authored-by: Ubuntu 20-04-02-amd64 + * Feature/aws navigation sm dance bot (#174) + * repo dependency + * husky launch file in sm_dance_bot + * Add dependencies for husky simulation. + * Fix formatting. + * Update dependencies for husky in rolling and galactic. + * minor + * progress on aws navigation and some other refactorings on navigation clients and behaviors + * more on aws demo + * fixing broken build + * minor + Co-authored-by: Denis Štogl + Co-authored-by: Denis Štogl + * minor changes (#175) + * warehouse2 (#177) + * Waypoint Inputs (#178) + Co-authored-by: Ubuntu 20-04-02-amd64 + * wharehouse2 progress (#179) + * format (#180) + * sm_dance_bot_warehouse_3 (#181) + Co-authored-by: Ubuntu 20-04-02-amd64 + * Feature/sm warehouse 2 13 dec 2 (#182) + * format + * more changes and headless + * merge + * headless and other fixes + * default values + * Brettpac branch (#184) + * sm_dance_bot_warehouse_3 + * Redoing sm_dance_bot_warehouse_3 waypoints + * More Waypoints + Co-authored-by: Ubuntu 20-04-02-amd64 + * SrConditional fixes and formatting (#168) + * fix: some formatting and templating on SrConditional + * fix: move trigger logic into headers + * fix: lint + * Feature/wharehouse2 dec 14 (#185) + * warehouse2 + * minor + * Feature/sm warehouse 2 13 dec 2 (#186) + * format + * more changes and headless + * merge + * headless and other fixes + * default values + * minor + * finetuning waypoints (#187) + Co-authored-by: Ubuntu 20-04-02-amd64 + * Feature/cb pure spinning (#188) + * format + * more changes and headless + * merge + * headless and other fixes + * default values + * minor + * Feature/cb pure spinning (#189) + * format + * more changes and headless + * merge + * headless and other fixes + * default values + * minor + * pure spinning behavior missing files + * minor changes (#190) + * Feature/planner changes 16 12 (#191) + * minor changes + * more fixes + * minor + * minor + * Feature/replanning 16 dec (#193) + * minor changes + * replanning for all our examples + * several fixes (#194) + * minor changes (#195) + * Feature/undo motion 20 12 (#196) + * minor changes + * replanning for all our examples + * improving undo motion navigation warehouse2 + * minor + * tuning warehouse3 (#197) + * Feature/undo motion 20 12 (#198) + * minor changes + * replanning for all our examples + * improving undo motion navigation warehouse2 + * minor + * undo tuning and errors + * format + * Feature/sync 21 12 (#199) + * minor changes + * replanning for all our examples + * format issues + * Feature/warehouse2 22 12 (#200) + * minor changes + * replanning for all our examples + * format issues + * finishing warehouse2 + * Feature/warehouse2 23 12 (#201) + * minor changes + * replanning for all our examples + * tuning and fixes (#202) + * Feature/minor tune (#203) + * tuning and fixes + * minor tune + * fixing warehouse 3 problems, and other core improvements (#204) + * fixing warehouse 3 problems, and other core improvements to remove dead lock, also making continuous integration green + * weird moveit not downloaded repo + * added missing file from warehouse2 (#205) + * backport to foxy + * minor format + * minor linking errors foxy + Co-authored-by: Denis Štogl + Co-authored-by: Denis Štogl + Co-authored-by: Declan Dury <44791484+DecDury@users.noreply.github.com> + Co-authored-by: DecDury + Co-authored-by: reelrbtx + Co-authored-by: brettpac + Co-authored-by: David Revay + * missing + * missing sm + * updating subscriber publisher components + * progress in autowarrrrrrrrrrrrrrrrrrrrrrrrrrrrrrrrrre machine + * refining cp subscriber cp publisher + * improvements in smacc core adding more components mostly developed for autoware demo + * autoware demo + * missing + * foxy ci + * fix + * minor broken build + Co-authored-by: Denis Štogl + Co-authored-by: Denis Štogl + Co-authored-by: Declan Dury <44791484+DecDury@users.noreply.github.com> + Co-authored-by: DecDury + Co-authored-by: reelrbtx + Co-authored-by: brettpac + Co-authored-by: David Revay + Co-authored-by: pabloinigoblasco + * Add mergify rules file. + * Try fixing CI for rolling. (#209) + Merging to get backport working. + * Remove example things from Foxy CI setup. (#214) + * Add Autoware Auto Msgs into not-released dependencies. (#220) + * Fix rolling builds (#222) + * do not merge yet - Feature/odom tracker improvements and retry motion (#223) + * odom tracker improvements + * adding forward behavior retry funcionality + * removing warnings (#213) + * minor changes + * replanning for all our examples + * backport to foxy + * minor format + * minor linking errors foxy + * Foxy backport (#206) + * minor formatting fixes + * Fix trailing spaces. + * Correct codespell. + * Correct python linters warnings. + * Add galactic CI build because Navigation2 is broken in rolling. + * Add partial changes for ament_cpplint. + * Add tf2_ros as dependency to find include. + * Disable ament_cpplint. + * Disable some packages and update workflows. + * Bump ccache version. + * Ignore further packages + * Satisfy ament_lint_cmake + * Add missing licences. + * Disable cpplint and cppcheck linters. + * Correct formatters. + * branching example + * Disable disabled packages + * Update ci-build-source.yml + * Change extension + * Change extension of imports. + * Enable cppcheck + * Correct formatting of python file. + * Included necessary package and edited Threesome launch + Changed... + ros2 launch sm_three_some sm_three_some + to + ros2 launch sm_three_some sm_three_some.launch + Added: + First ensure you have the necessary package installed. + ``` + sudo apt-get install ros-rolling-ros2trace + ``` + Then run this command. + * Rename header files and correct format. + * Add workflow for checking doc build. + * Update doxygen-check-build.yml + * Create doxygen-deploy.yml + * Use manual deployment for now. + * Create workflow for testing prerelease builds + * Use docs/ as source folder for documentation + * Use docs/ as output directory. + * Rename to smacc2 and smacc2_msgs + * Correct GitHub branch reference. + * Update name of package and package.xml to pass liter. + * Execute on master update + * Reset all versions to 0.0.0 + * Ignore all packages except smacc2 and smacc2_msgs + * Update changelogs + * 0.1.0 + * Revert "Ignore all packages except smacc2 and smacc2_msgs" + This reverts commit f603166a4b3ccdfe96c64d9f9fb9d8b49fbf0e61. + * Update description table. + * Update table + * Copy initial docs + * Dockerfile w/ ROS distro as argument + use this command "sudo docker build --build-arg ROS2_DISTRO=(desiredRosTag) (directoryHoldingDockerfile)/" + * Opened new folder for additional tracing contents + * Delete tracing directory + * Moved tracing.md to tracing directory + * added setupTracing.sh + Installs necessary packages and configures tracing group. + * Removed manual installation of ros-rolling-ros2trace + This is now automated in setupTracing.sh + location of sh file assumed if user follows README.md under "Getting started" + * Created alternative ManualTracing + * added new sm markdowns + * added a dockerfile for Rolling and Galactic + * Update smacc2_ci/docker/ros_rollingAndGalactic_ubuntu_20.04/buildGalactic.sh + Co-authored-by: Denis Štogl + * Update tracing/ManualTracing.md + Co-authored-by: Denis Štogl + * changed wording "smacc application" to "SMACC2 library" + Co-authored-by: Denis Štogl + * Update smacc_sm_reference_library/sm_atomic/README.md + edit from html to markdown syntax + Co-authored-by: Denis Štogl + * reactivating smacc2 nav clients for rolling via submodules + * renamed tracing events after + * bug in smacc2 component + * reverted markdowns to html + * added README tutorial for Dockerfile + * additional cleanup + * cleanup + * cleanup + * edited tracing.md to reflect new tracing event names + * Enable build of missing rolling repositories. + * Enable Navigation2 for semi-binary build. + * Remove galactic builds from master and kepp only rolling. Remove submodules and use .repos file + * updated mentions of SMACC/ROS to SMACC2/ROS2 + * some progress on navigation rolling + * renamed folders, deleted tracing.md, edited README.md + * added smacc2_performance_tools + * performance tests improvements + * more on performance and other issues + * sm_respira_1 format cleanup + * sm_respira_1 format cleanup pre-commit + * sm_respira_test_2 + * sm_respira_test_2 + * more changes on performance tests + * Do not execute clang-format on smacc2_sm_reference_library package. + * sm_reference_library reformatting + * Correct trailing spaces. + * sm_atomic_24hr + * sm_atomic_performance_trace_1 + * Update smacc2_rta command across readmes + * Clean up of sm_atomic_24hr + * more sm_atomic_24hr cleanup + * Optimized deps in move_base_z_planners_common. + * Renaming of event generator library + * minor formatting + * Add galactic CI setup and rename rolling files. (#58) + * Fix source CI and correct README overview. (#62) + * Update c_cpp_properties.json + * changed launch command to ros2 launch sm_respira_1 sm_respira_1.launch (#69) + also noticed a note I had made while producing these that was not removed + * update doxygen links (#70) + Co-authored-by: Ubuntu 20-04-02-amd64 + * More Readme Updates (#72) + Co-authored-by: Ubuntu 20-04-02-amd64 + * More Readme (#74) + Co-authored-by: Ubuntu 20-04-02-amd64 + * created new sm from sm_respira_1 (#76) + * Feature/core and navigation fixes (#78) + * base for the sm_aws_aarehouse navigation + * progressing in aws navigation + * minor + * several core improvements during navigation testing + * formatting improvements + * progress in aws navigation demo + * format improvements + * Feature/aws demo progress (#80) + * base for the sm_aws_aarehouse navigation + * progressing in aws navigation + * minor + * several core improvements during navigation testing + * formatting improvements + * progress in aws navigation demo + * format improvements + * format improvements + * more on navigation + * sm_advanced_recovery_1 reworked (#83) + * sm_advanced_recovery_1 reworked + * fix pre-commit + * Trying to fix Pre-Commit + Co-authored-by: Ubuntu 20-04-02-amd64 + * more sm_advanced_recovery_1 (#84) + Co-authored-by: Ubuntu 20-04-02-amd64 + * More sm_advanced_recovery_1 work (#85) + Co-authored-by: Ubuntu 20-04-02-amd64 + * sm_advanced_recovery_1 round 4 (#86) + Co-authored-by: Ubuntu 20-04-02-amd64 + * Brettpac branch (#87) + * sm_atomic_performance_test_a_2 + * sm_atomic_performance_test_a_1 + Co-authored-by: Ubuntu 20-04-02-amd64 + * sm_atomic_performance_test_c_1 (#88) + Co-authored-by: Ubuntu 20-04-02-amd64 + * modifying sm_atomic_performance_test_a_2 (#89) + Co-authored-by: Ubuntu 20-04-02-amd64 + * sm_multi_stage_1 (#90) + * sm_multi_stage_1 + * fixing precommit + Co-authored-by: Ubuntu 20-04-02-amd64 + * more sm_multi_stage_1 (#91) + Co-authored-by: Ubuntu 20-04-02-amd64 + * Update README.md + updated launch command + * Wait topic message client behavior (#81) + * base for the sm_aws_aarehouse navigation + * progressing in aws navigation + * minor + * several core improvements during navigation testing + * formatting improvements + * progress in aws navigation demo + * format improvements + * format improvements + * more on navigation + * new feature, cb_wait_topic_message: asynchronous client behavior that waits a topic message and optionally checks its contents to success + * formatting + * attempting precommit fixes + Co-authored-by: Ubuntu 20-04-02-amd64 + Co-authored-by: Denis Štogl + * Feature/wait nav2 nodes client behavior (#82) + * base for the sm_aws_aarehouse navigation + * progressing in aws navigation + * minor + * several core improvements during navigation testing + * formatting improvements + * progress in aws navigation demo + * format improvements + * format improvements + * more on navigation + * new feature, cb_wait_topic_message: asynchronous client behavior that waits a topic message and optionally checks its contents to success + * formatting + * adding new client behavior add for nav2, wait nav2 nodes subscribing to the /bond topic and waiting they are alive. you optionally can select the nodes to wait + * Correct all linters and formaters. + Co-authored-by: Denis Štogl + Co-authored-by: Denis Štogl + * Feature/aws demo progress (#92) + * base for the sm_aws_aarehouse navigation + * progressing in aws navigation + * minor + * several core improvements during navigation testing + * formatting improvements + * progress in aws navigation demo + * format improvements + * format improvements + * more on navigation + * new feature, cb_wait_topic_message: asynchronous client behavior that waits a topic message and optionally checks its contents to success + * formatting + * adding new client behavior add for nav2, wait nav2 nodes subscribing to the /bond topic and waiting they are alive. you optionally can select the nodes to wait + * progress in aws navigation demo + * minor format + * Feature/sm dance bot fixes (#93) + * base for the sm_aws_aarehouse navigation + * progressing in aws navigation + * minor + * several core improvements during navigation testing + * formatting improvements + * progress in aws navigation demo + * format improvements + * format improvements + * more on navigation + * new feature, cb_wait_topic_message: asynchronous client behavior that waits a topic message and optionally checks its contents to success + * formatting + * adding new client behavior add for nav2, wait nav2 nodes subscribing to the /bond topic and waiting they are alive. you optionally can select the nodes to wait + * progress in aws navigation demo + * minor format + * navigation parameters fixes on sm_dance_bot + * minor format + * Feature/sm aws warehouse (#94) + * base for the sm_aws_aarehouse navigation + * progressing in aws navigation + * minor + * several core improvements during navigation testing + * formatting improvements + * progress in aws navigation demo + * format improvements + * format improvements + * more on navigation + * new feature, cb_wait_topic_message: asynchronous client behavior that waits a topic message and optionally checks its contents to success + * formatting + * adding new client behavior add for nav2, wait nav2 nodes subscribing to the /bond topic and waiting they are alive. you optionally can select the nodes to wait + * progress in aws navigation demo + * minor format + * merge and progress + * fix format + * Feature/sm dance bot fixes (#95) + * base for the sm_aws_aarehouse navigation + * progressing in aws navigation + * minor + * several core improvements during navigation testing + * formatting improvements + * progress in aws navigation demo + * format improvements + * format improvements + * more on navigation + * new feature, cb_wait_topic_message: asynchronous client behavior that waits a topic message and optionally checks its contents to success + * formatting + * adding new client behavior add for nav2, wait nav2 nodes subscribing to the /bond topic and waiting they are alive. you optionally can select the nodes to wait + * progress in aws navigation demo + * minor format + * navigation parameters fixes on sm_dance_bot + * minor format + * minor + * minor format + * Remove some compile warnings. (#96) + * Feature/cb pause slam (#98) + * base for the sm_aws_aarehouse navigation + * progressing in aws navigation + * minor + * several core improvements during navigation testing + * formatting improvements + * progress in aws navigation demo + * format improvements + * format improvements + * more on navigation + * new feature, cb_wait_topic_message: asynchronous client behavior that waits a topic message and optionally checks its contents to success + * formatting + * adding new client behavior add for nav2, wait nav2 nodes subscribing to the /bond topic and waiting they are alive. you optionally can select the nodes to wait + * progress in aws navigation demo + * minor format + * navigation parameters fixes on sm_dance_bot + * minor format + * minor + * formatting + * cb pause slam client behavior + * sm_dance_bot_lite (#99) + * sm_dance_bot_lite + * precommit + * Updates yaml + Co-authored-by: Ubuntu 20-04-02-amd64 + * Rename doxygen deployment workflow (#100) + * minor hotfix + * sm_dance_bot visualizing turtlebot3 (#101) + * Feature/dance bot launch gz lidar choice (#102) + * sm_dance_bot visualizing turtlebot3 + * cleaning and lidar show/hide option + * cleaning files and making formatting work + * more fixes + * Feature/sm dance bot lite gazebo fixes (#104) + * sm_dance_bot visualizing turtlebot3 + * cleaning and lidar show/hide option + * cleaning files and making formatting work + * more fixes + * gazebo fixes, to show the robot and the lidar + * format fixes + * sm_multi_stage_1 doubling (#103) + Co-authored-by: Ubuntu 20-04-02-amd64 + * Feature/sm dance bot strikes back gazebo fixes (#105) + * sm_dance_bot visualizing turtlebot3 + * cleaning and lidar show/hide option + * cleaning files and making formatting work + * more fixes + * gazebo fixes, to show the robot and the lidar + * format fixes + * gazebo fixes for sm_dance_bot_strikes_back + * precommit cleanup run (#106) + Co-authored-by: Ubuntu 20-04-02-amd64 + * aws demo (#108) + * aws demo + * format + * got sm_multi_stage_1 working (barely) (#109) + Co-authored-by: Ubuntu 20-04-02-amd64 + * Brettpac branch (#110) + * got sm_multi_stage_1 working (barely) + * gaining traction sm_multi_stage_1 + * more + * don't remember + * making progress + * More + * keep hammering + * two stages + * 3 part + * 4th stage + Co-authored-by: Ubuntu 20-04-02-amd64 + * Brettpac branch (#111) + * got sm_multi_stage_1 working (barely) + * gaining traction sm_multi_stage_1 + * more + * don't remember + * making progress + * More + * keep hammering + * two stages + * 3 part + * 4th stage + * 5th stage + Co-authored-by: Ubuntu 20-04-02-amd64 + * a3 (#113) + Co-authored-by: Ubuntu 20-04-02-amd64 + * Remove neo_simulation2 package. (#112) + * Remove neo_simulation2 package. + * Correct formatting. + * Enable source build on PR for testing. + * Adjust build packages of source CI + * more sm_multi_stage_1 (#114) + Co-authored-by: Ubuntu 20-04-02-amd64 + * mm (#115) + Co-authored-by: Ubuntu 20-04-02-amd64 + * diverse improvements navigation and performance (#116) + * diverse improvements navigation and performance + * minor + Co-authored-by: pabloinigoblasco + * Feature/diverse improvemets navigation performance (#117) + * diverse improvements navigation and performance + * minor + * additional linting and formatting + * Remove merge markers from a python file. (#119) + * Feature/slam toggle and smacc deep history (#122) + * progress in navigation, slam toggle client behaviors and slam_toolbox components. Also smacc2::deep_history syntax + * going forward in testing sm_dance_bot introducing slam pausing/resuming funcionality + * feature/more_sm_dance_bot_fixes + * minor format + * minor (#124) + Co-authored-by: Ubuntu 20-04-02-amd64 + * more changes in sm_dance_bot (#125) + * Move method after the method it calls. Otherwise recursion could happen. (#126) + * Feature/dance bot s pattern (#128) + * more changes in sm_dance_bot + * polishing sm_dance_bot and s-pattern + * noticed typo + Finnaly > Finally + * Feature/dance bot s pattern (#129) + * more changes in sm_dance_bot + * polishing sm_dance_bot and s-pattern + * more refinement in sm_dance_bot + * First working version of sm template and template generator. (#127) + * minor tweaks (#130) + Co-authored-by: Ubuntu 20-04-02-amd64 + * Feature/sm dance bot refine (#131) + * more changes in sm_dance_bot + * minor + * Feature/sm dance bot refine 2 (#132) + * more changes in sm_dance_bot + * minor + * build fix + * waypoints navigator bug (#133) + * minor tuning to mitigate overshot issue cases + * progress in the sm_dance_bot tests (#135) + * some more progress on markers cleanup + * minor format issues (#134) + * sm_dance_bot_lite (#136) + Co-authored-by: Ubuntu 20-04-02-amd64 + * Resolve compile wanings (#137) + * Add SM core test (#138) + * minor navigation improvements (#141) + * using local action msgs (#139) + * using local action msgs + * removing sm_dance_bot_msgs + * pending references + * Feature/nitrosz renaming (#144) + * using local action msgs + * removing sm_dance_bot_msgs + * pending references + * navigation 2 stack renaming + * formatting + * added SVGs to READMEs of atomic, dance_bot, and others (#140) + * added remaining SVGs to READMEs (#145) + * added remaining SVGs to READMEs + * precommit cleanup + * Update package list. (#142) + * removing parameters smacc (#147) + * removing parameters smacc + * workflows update + * workflow + * Noticed launch command was incorrect in README.md + fixed launch command for sm_dance_bot_strikes_back and removed some comments I had made in the past. + * Fix CI: format fix python version (#148) + * Add SM Atomic SM generator. (#143) + * Remove node creation and create only a logger. (#149) + * Rolling Docker environment to be executed from any environment (#154) + * Feature/sm dance bot strikes back refactoring (#152) + Co-authored-by: DecDury + Co-authored-by: Denis Štogl + * slight waypoint 4 and iterations changes so robot can complete course (#155) + * Feature/migration moveit client (#151) + * initial migration to smacc2 + * fixing some errors introduced on formatting + * missing dependency + * fixing some more linting warnings + * minor + * removing test from main moveit cmake + * test ur5 + * progressing in the moveit migration testing + * updating format + * adding .reps dependencies and also fixing some build errors + * repos dependency + * adding dependency to ur5 client + * docker refactoring + * minor + * progress on move_it PR + * minor dockerfile test workaround + * improving dockerfile for building local tests + * minor + * fixing compiling issues + * update readme (#164) + * update readme + * more readme updates + * more + Co-authored-by: Ubuntu 20-04-02-amd64 + * initial state machine transition timestamp (#165) + * moved reference library SMs to smacc2_performance_tools (#166) + * moved reference library SMs to smacc2_performance_tools + * pre-commit cleanup + * Add QOS durability to SmaccPublisherClient (#163) + * feat: add qos durability to SmaccPublisherClient + * fix: add a missing colon + * refactor: remove line + * feat: add reliability qos config + * Feature/testing moveit behaviors (#167) + * more testing on moveit + * progress on moveit + * more testing on moveit behaviors + * minor configuration + * fixing pipeline error + * fixing broken master build + * sm_pubsub_1 (#169) + Co-authored-by: Ubuntu 20-04-02-amd64 + * sm_pubsub_1 part 2 (#170) + * sm_pubsub_1 part 2 + * sm_pubsub_1 part 2 + Co-authored-by: Ubuntu 20-04-02-amd64 + * sm_advanced_recovery_1 renaming (#171) + Co-authored-by: Ubuntu 20-04-02-amd64 + * sm_multi_stage_1 reworking (#172) + * multistage modes + * sm_multi_stage sequences + * sm_multi_state_1 steps + * sm_multi_stage_1 sequence d + * sm_multi_stage_1 c sequence + * mode_5_sequence_b + * mode_4_sequence_b + * sm_multi_stage_1 most + * finishing touches 1 + * readme + Co-authored-by: Ubuntu 20-04-02-amd64 + * Feature/aws navigation sm dance bot (#174) + * repo dependency + * husky launch file in sm_dance_bot + * Add dependencies for husky simulation. + * Fix formatting. + * Update dependencies for husky in rolling and galactic. + * minor + * progress on aws navigation and some other refactorings on navigation clients and behaviors + * more on aws demo + * fixing broken build + * minor + Co-authored-by: Denis Štogl + Co-authored-by: Denis Štogl + * minor changes (#175) + * warehouse2 (#177) + * Waypoint Inputs (#178) + Co-authored-by: Ubuntu 20-04-02-amd64 + * wharehouse2 progress (#179) + * format (#180) + * sm_dance_bot_warehouse_3 (#181) + Co-authored-by: Ubuntu 20-04-02-amd64 + * Feature/sm warehouse 2 13 dec 2 (#182) + * format + * more changes and headless + * merge + * headless and other fixes + * default values + * Brettpac branch (#184) + * sm_dance_bot_warehouse_3 + * Redoing sm_dance_bot_warehouse_3 waypoints + * More Waypoints + Co-authored-by: Ubuntu 20-04-02-amd64 + * SrConditional fixes and formatting (#168) + * fix: some formatting and templating on SrConditional + * fix: move trigger logic into headers + * fix: lint + * Feature/wharehouse2 dec 14 (#185) + * warehouse2 + * minor + * Feature/sm warehouse 2 13 dec 2 (#186) + * format + * more changes and headless + * merge + * headless and other fixes + * default values + * minor + * finetuning waypoints (#187) + Co-authored-by: Ubuntu 20-04-02-amd64 + * Feature/cb pure spinning (#188) + * format + * more changes and headless + * merge + * headless and other fixes + * default values + * minor + * Feature/cb pure spinning (#189) + * format + * more changes and headless + * merge + * headless and other fixes + * default values + * minor + * pure spinning behavior missing files + * minor changes (#190) + * Feature/planner changes 16 12 (#191) + * minor changes + * more fixes + * minor + * minor + * Feature/replanning 16 dec (#193) + * minor changes + * replanning for all our examples + * several fixes (#194) + * minor changes (#195) + * Feature/undo motion 20 12 (#196) + * minor changes + * replanning for all our examples + * improving undo motion navigation warehouse2 + * minor + * tuning warehouse3 (#197) + * Feature/undo motion 20 12 (#198) + * minor changes + * replanning for all our examples + * improving undo motion navigation warehouse2 + * minor + * undo tuning and errors + * format + * Feature/sync 21 12 (#199) + * minor changes + * replanning for all our examples + * format issues + * Feature/warehouse2 22 12 (#200) + * minor changes + * replanning for all our examples + * format issues + * finishing warehouse2 + * Feature/warehouse2 23 12 (#201) + * minor changes + * replanning for all our examples + * tuning and fixes (#202) + * Feature/minor tune (#203) + * tuning and fixes + * minor tune + * fixing warehouse 3 problems, and other core improvements (#204) + * fixing warehouse 3 problems, and other core improvements to remove dead lock, also making continuous integration green + * weird moveit not downloaded repo + * added missing file from warehouse2 (#205) + * backport to foxy + * minor format + * minor linking errors foxy + Co-authored-by: Denis Štogl + Co-authored-by: Denis Štogl + Co-authored-by: Declan Dury <44791484+DecDury@users.noreply.github.com> + Co-authored-by: DecDury + Co-authored-by: reelrbtx + Co-authored-by: brettpac + Co-authored-by: David Revay + * missing + * missing sm + * updating subscriber publisher components + * progress in autowarrrrrrrrrrrrrrrrrrrrrrrrrrrrrrrrrre machine + * refining cp subscriber cp publisher + * improvements in smacc core adding more components mostly developed for autoware demo + * autoware demo + * missing + * foxy ci + * fix + * minor broken build + * some reordering fixes + * minor + * docker files for different revisions, warnings removval and more testing on navigation + * fixing docker for foxy and galactic + Co-authored-by: Denis Štogl + Co-authored-by: Denis Štogl + Co-authored-by: Declan Dury <44791484+DecDury@users.noreply.github.com> + Co-authored-by: DecDury + Co-authored-by: reelrbtx + Co-authored-by: brettpac + Co-authored-by: David Revay + Co-authored-by: pabloinigoblasco + * dockerfiles (#225) + * Fix code generators (#221) + * Fix other build issues. + * Update SM template and make example code clearly visible. + * Remove use of node in the sm performance template. + * Updated templated to use Blackboard storage. + * Update template to resolve the global data correctly. + * Update sm_name.hpp + Co-authored-by: Pablo Iñigo Blasco + * Feature/retry behavior warehouse 1 (#226) + * minor changes + * replanning for all our examples + * backport to foxy + * minor format + * minor linking errors foxy + * Foxy backport (#206) + * minor formatting fixes + * Fix trailing spaces. + * Correct codespell. + * Correct python linters warnings. + * Add galactic CI build because Navigation2 is broken in rolling. + * Add partial changes for ament_cpplint. + * Add tf2_ros as dependency to find include. + * Disable ament_cpplint. + * Disable some packages and update workflows. + * Bump ccache version. + * Ignore further packages + * Satisfy ament_lint_cmake + * Add missing licences. + * Disable cpplint and cppcheck linters. + * Correct formatters. + * branching example + * Disable disabled packages + * Update ci-build-source.yml + * Change extension + * Change extension of imports. + * Enable cppcheck + * Correct formatting of python file. + * Included necessary package and edited Threesome launch + Changed... + ros2 launch sm_three_some sm_three_some + to + ros2 launch sm_three_some sm_three_some.launch + Added: + First ensure you have the necessary package installed. + ``` + sudo apt-get install ros-rolling-ros2trace + ``` + Then run this command. + * Rename header files and correct format. + * Add workflow for checking doc build. + * Update doxygen-check-build.yml + * Create doxygen-deploy.yml + * Use manual deployment for now. + * Create workflow for testing prerelease builds + * Use docs/ as source folder for documentation + * Use docs/ as output directory. + * Rename to smacc2 and smacc2_msgs + * Correct GitHub branch reference. + * Update name of package and package.xml to pass liter. + * Execute on master update + * Reset all versions to 0.0.0 + * Ignore all packages except smacc2 and smacc2_msgs + * Update changelogs + * 0.1.0 + * Revert "Ignore all packages except smacc2 and smacc2_msgs" + This reverts commit f603166a4b3ccdfe96c64d9f9fb9d8b49fbf0e61. + * Update description table. + * Update table + * Copy initial docs + * Dockerfile w/ ROS distro as argument + use this command "sudo docker build --build-arg ROS2_DISTRO=(desiredRosTag) (directoryHoldingDockerfile)/" + * Opened new folder for additional tracing contents + * Delete tracing directory + * Moved tracing.md to tracing directory + * added setupTracing.sh + Installs necessary packages and configures tracing group. + * Removed manual installation of ros-rolling-ros2trace + This is now automated in setupTracing.sh + location of sh file assumed if user follows README.md under "Getting started" + * Created alternative ManualTracing + * added new sm markdowns + * added a dockerfile for Rolling and Galactic + * Update smacc2_ci/docker/ros_rollingAndGalactic_ubuntu_20.04/buildGalactic.sh + Co-authored-by: Denis Štogl + * Update tracing/ManualTracing.md + Co-authored-by: Denis Štogl + * changed wording "smacc application" to "SMACC2 library" + Co-authored-by: Denis Štogl + * Update smacc_sm_reference_library/sm_atomic/README.md + edit from html to markdown syntax + Co-authored-by: Denis Štogl + * reactivating smacc2 nav clients for rolling via submodules + * renamed tracing events after + * bug in smacc2 component + * reverted markdowns to html + * added README tutorial for Dockerfile + * additional cleanup + * cleanup + * cleanup + * edited tracing.md to reflect new tracing event names + * Enable build of missing rolling repositories. + * Enable Navigation2 for semi-binary build. + * Remove galactic builds from master and kepp only rolling. Remove submodules and use .repos file + * updated mentions of SMACC/ROS to SMACC2/ROS2 + * some progress on navigation rolling + * renamed folders, deleted tracing.md, edited README.md + * added smacc2_performance_tools + * performance tests improvements + * more on performance and other issues + * sm_respira_1 format cleanup + * sm_respira_1 format cleanup pre-commit + * sm_respira_test_2 + * sm_respira_test_2 + * more changes on performance tests + * Do not execute clang-format on smacc2_sm_reference_library package. + * sm_reference_library reformatting + * Correct trailing spaces. + * sm_atomic_24hr + * sm_atomic_performance_trace_1 + * Update smacc2_rta command across readmes + * Clean up of sm_atomic_24hr + * more sm_atomic_24hr cleanup + * Optimized deps in move_base_z_planners_common. + * Renaming of event generator library + * minor formatting + * Add galactic CI setup and rename rolling files. (#58) + * Fix source CI and correct README overview. (#62) + * Update c_cpp_properties.json + * changed launch command to ros2 launch sm_respira_1 sm_respira_1.launch (#69) + also noticed a note I had made while producing these that was not removed + * update doxygen links (#70) + Co-authored-by: Ubuntu 20-04-02-amd64 + * More Readme Updates (#72) + Co-authored-by: Ubuntu 20-04-02-amd64 + * More Readme (#74) + Co-authored-by: Ubuntu 20-04-02-amd64 + * created new sm from sm_respira_1 (#76) + * Feature/core and navigation fixes (#78) + * base for the sm_aws_aarehouse navigation + * progressing in aws navigation + * minor + * several core improvements during navigation testing + * formatting improvements + * progress in aws navigation demo + * format improvements + * Feature/aws demo progress (#80) + * base for the sm_aws_aarehouse navigation + * progressing in aws navigation + * minor + * several core improvements during navigation testing + * formatting improvements + * progress in aws navigation demo + * format improvements + * format improvements + * more on navigation + * sm_advanced_recovery_1 reworked (#83) + * sm_advanced_recovery_1 reworked + * fix pre-commit + * Trying to fix Pre-Commit + Co-authored-by: Ubuntu 20-04-02-amd64 + * more sm_advanced_recovery_1 (#84) + Co-authored-by: Ubuntu 20-04-02-amd64 + * More sm_advanced_recovery_1 work (#85) + Co-authored-by: Ubuntu 20-04-02-amd64 + * sm_advanced_recovery_1 round 4 (#86) + Co-authored-by: Ubuntu 20-04-02-amd64 + * Brettpac branch (#87) + * sm_atomic_performance_test_a_2 + * sm_atomic_performance_test_a_1 + Co-authored-by: Ubuntu 20-04-02-amd64 + * sm_atomic_performance_test_c_1 (#88) + Co-authored-by: Ubuntu 20-04-02-amd64 + * modifying sm_atomic_performance_test_a_2 (#89) + Co-authored-by: Ubuntu 20-04-02-amd64 + * sm_multi_stage_1 (#90) + * sm_multi_stage_1 + * fixing precommit + Co-authored-by: Ubuntu 20-04-02-amd64 + * more sm_multi_stage_1 (#91) + Co-authored-by: Ubuntu 20-04-02-amd64 + * Update README.md + updated launch command + * Wait topic message client behavior (#81) + * base for the sm_aws_aarehouse navigation + * progressing in aws navigation + * minor + * several core improvements during navigation testing + * formatting improvements + * progress in aws navigation demo + * format improvements + * format improvements + * more on navigation + * new feature, cb_wait_topic_message: asynchronous client behavior that waits a topic message and optionally checks its contents to success + * formatting + * attempting precommit fixes + Co-authored-by: Ubuntu 20-04-02-amd64 + Co-authored-by: Denis Štogl + * Feature/wait nav2 nodes client behavior (#82) + * base for the sm_aws_aarehouse navigation + * progressing in aws navigation + * minor + * several core improvements during navigation testing + * formatting improvements + * progress in aws navigation demo + * format improvements + * format improvements + * more on navigation + * new feature, cb_wait_topic_message: asynchronous client behavior that waits a topic message and optionally checks its contents to success + * formatting + * adding new client behavior add for nav2, wait nav2 nodes subscribing to the /bond topic and waiting they are alive. you optionally can select the nodes to wait + * Correct all linters and formaters. + Co-authored-by: Denis Štogl + Co-authored-by: Denis Štogl + * Feature/aws demo progress (#92) + * base for the sm_aws_aarehouse navigation + * progressing in aws navigation + * minor + * several core improvements during navigation testing + * formatting improvements + * progress in aws navigation demo + * format improvements + * format improvements + * more on navigation + * new feature, cb_wait_topic_message: asynchronous client behavior that waits a topic message and optionally checks its contents to success + * formatting + * adding new client behavior add for nav2, wait nav2 nodes subscribing to the /bond topic and waiting they are alive. you optionally can select the nodes to wait + * progress in aws navigation demo + * minor format + * Feature/sm dance bot fixes (#93) + * base for the sm_aws_aarehouse navigation + * progressing in aws navigation + * minor + * several core improvements during navigation testing + * formatting improvements + * progress in aws navigation demo + * format improvements + * format improvements + * more on navigation + * new feature, cb_wait_topic_message: asynchronous client behavior that waits a topic message and optionally checks its contents to success + * formatting + * adding new client behavior add for nav2, wait nav2 nodes subscribing to the /bond topic and waiting they are alive. you optionally can select the nodes to wait + * progress in aws navigation demo + * minor format + * navigation parameters fixes on sm_dance_bot + * minor format + * Feature/sm aws warehouse (#94) + * base for the sm_aws_aarehouse navigation + * progressing in aws navigation + * minor + * several core improvements during navigation testing + * formatting improvements + * progress in aws navigation demo + * format improvements + * format improvements + * more on navigation + * new feature, cb_wait_topic_message: asynchronous client behavior that waits a topic message and optionally checks its contents to success + * formatting + * adding new client behavior add for nav2, wait nav2 nodes subscribing to the /bond topic and waiting they are alive. you optionally can select the nodes to wait + * progress in aws navigation demo + * minor format + * merge and progress + * fix format + * Feature/sm dance bot fixes (#95) + * base for the sm_aws_aarehouse navigation + * progressing in aws navigation + * minor + * several core improvements during navigation testing + * formatting improvements + * progress in aws navigation demo + * format improvements + * format improvements + * more on navigation + * new feature, cb_wait_topic_message: asynchronous client behavior that waits a topic message and optionally checks its contents to success + * formatting + * adding new client behavior add for nav2, wait nav2 nodes subscribing to the /bond topic and waiting they are alive. you optionally can select the nodes to wait + * progress in aws navigation demo + * minor format + * navigation parameters fixes on sm_dance_bot + * minor format + * minor + * minor format + * Remove some compile warnings. (#96) + * Feature/cb pause slam (#98) + * base for the sm_aws_aarehouse navigation + * progressing in aws navigation + * minor + * several core improvements during navigation testing + * formatting improvements + * progress in aws navigation demo + * format improvements + * format improvements + * more on navigation + * new feature, cb_wait_topic_message: asynchronous client behavior that waits a topic message and optionally checks its contents to success + * formatting + * adding new client behavior add for nav2, wait nav2 nodes subscribing to the /bond topic and waiting they are alive. you optionally can select the nodes to wait + * progress in aws navigation demo + * minor format + * navigation parameters fixes on sm_dance_bot + * minor format + * minor + * formatting + * cb pause slam client behavior + * sm_dance_bot_lite (#99) + * sm_dance_bot_lite + * precommit + * Updates yaml + Co-authored-by: Ubuntu 20-04-02-amd64 + * Rename doxygen deployment workflow (#100) + * minor hotfix + * sm_dance_bot visualizing turtlebot3 (#101) + * Feature/dance bot launch gz lidar choice (#102) + * sm_dance_bot visualizing turtlebot3 + * cleaning and lidar show/hide option + * cleaning files and making formatting work + * more fixes + * Feature/sm dance bot lite gazebo fixes (#104) + * sm_dance_bot visualizing turtlebot3 + * cleaning and lidar show/hide option + * cleaning files and making formatting work + * more fixes + * gazebo fixes, to show the robot and the lidar + * format fixes + * sm_multi_stage_1 doubling (#103) + Co-authored-by: Ubuntu 20-04-02-amd64 + * Feature/sm dance bot strikes back gazebo fixes (#105) + * sm_dance_bot visualizing turtlebot3 + * cleaning and lidar show/hide option + * cleaning files and making formatting work + * more fixes + * gazebo fixes, to show the robot and the lidar + * format fixes + * gazebo fixes for sm_dance_bot_strikes_back + * precommit cleanup run (#106) + Co-authored-by: Ubuntu 20-04-02-amd64 + * aws demo (#108) + * aws demo + * format + * got sm_multi_stage_1 working (barely) (#109) + Co-authored-by: Ubuntu 20-04-02-amd64 + * Brettpac branch (#110) + * got sm_multi_stage_1 working (barely) + * gaining traction sm_multi_stage_1 + * more + * don't remember + * making progress + * More + * keep hammering + * two stages + * 3 part + * 4th stage + Co-authored-by: Ubuntu 20-04-02-amd64 + * Brettpac branch (#111) + * got sm_multi_stage_1 working (barely) + * gaining traction sm_multi_stage_1 + * more + * don't remember + * making progress + * More + * keep hammering + * two stages + * 3 part + * 4th stage + * 5th stage + Co-authored-by: Ubuntu 20-04-02-amd64 + * a3 (#113) + Co-authored-by: Ubuntu 20-04-02-amd64 + * Remove neo_simulation2 package. (#112) + * Remove neo_simulation2 package. + * Correct formatting. + * Enable source build on PR for testing. + * Adjust build packages of source CI + * more sm_multi_stage_1 (#114) + Co-authored-by: Ubuntu 20-04-02-amd64 + * mm (#115) + Co-authored-by: Ubuntu 20-04-02-amd64 + * diverse improvements navigation and performance (#116) + * diverse improvements navigation and performance + * minor + Co-authored-by: pabloinigoblasco + * Feature/diverse improvemets navigation performance (#117) + * diverse improvements navigation and performance + * minor + * additional linting and formatting + * Remove merge markers from a python file. (#119) + * Feature/slam toggle and smacc deep history (#122) + * progress in navigation, slam toggle client behaviors and slam_toolbox components. Also smacc2::deep_history syntax + * going forward in testing sm_dance_bot introducing slam pausing/resuming funcionality + * feature/more_sm_dance_bot_fixes + * minor format + * minor (#124) + Co-authored-by: Ubuntu 20-04-02-amd64 + * more changes in sm_dance_bot (#125) + * Move method after the method it calls. Otherwise recursion could happen. (#126) + * Feature/dance bot s pattern (#128) + * more changes in sm_dance_bot + * polishing sm_dance_bot and s-pattern + * noticed typo + Finnaly > Finally + * Feature/dance bot s pattern (#129) + * more changes in sm_dance_bot + * polishing sm_dance_bot and s-pattern + * more refinement in sm_dance_bot + * First working version of sm template and template generator. (#127) + * minor tweaks (#130) + Co-authored-by: Ubuntu 20-04-02-amd64 + * Feature/sm dance bot refine (#131) + * more changes in sm_dance_bot + * minor + * Feature/sm dance bot refine 2 (#132) + * more changes in sm_dance_bot + * minor + * build fix + * waypoints navigator bug (#133) + * minor tuning to mitigate overshot issue cases + * progress in the sm_dance_bot tests (#135) + * some more progress on markers cleanup + * minor format issues (#134) + * sm_dance_bot_lite (#136) + Co-authored-by: Ubuntu 20-04-02-amd64 + * Resolve compile wanings (#137) + * Add SM core test (#138) + * minor navigation improvements (#141) + * using local action msgs (#139) + * using local action msgs + * removing sm_dance_bot_msgs + * pending references + * Feature/nitrosz renaming (#144) + * using local action msgs + * removing sm_dance_bot_msgs + * pending references + * navigation 2 stack renaming + * formatting + * added SVGs to READMEs of atomic, dance_bot, and others (#140) + * added remaining SVGs to READMEs (#145) + * added remaining SVGs to READMEs + * precommit cleanup + * Update package list. (#142) + * removing parameters smacc (#147) + * removing parameters smacc + * workflows update + * workflow + * Noticed launch command was incorrect in README.md + fixed launch command for sm_dance_bot_strikes_back and removed some comments I had made in the past. + * Fix CI: format fix python version (#148) + * Add SM Atomic SM generator. (#143) + * Remove node creation and create only a logger. (#149) + * Rolling Docker environment to be executed from any environment (#154) + * Feature/sm dance bot strikes back refactoring (#152) + Co-authored-by: DecDury + Co-authored-by: Denis Štogl + * slight waypoint 4 and iterations changes so robot can complete course (#155) + * Feature/migration moveit client (#151) + * initial migration to smacc2 + * fixing some errors introduced on formatting + * missing dependency + * fixing some more linting warnings + * minor + * removing test from main moveit cmake + * test ur5 + * progressing in the moveit migration testing + * updating format + * adding .reps dependencies and also fixing some build errors + * repos dependency + * adding dependency to ur5 client + * docker refactoring + * minor + * progress on move_it PR + * minor dockerfile test workaround + * improving dockerfile for building local tests + * minor + * fixing compiling issues + * update readme (#164) + * update readme + * more readme updates + * more + Co-authored-by: Ubuntu 20-04-02-amd64 + * initial state machine transition timestamp (#165) + * moved reference library SMs to smacc2_performance_tools (#166) + * moved reference library SMs to smacc2_performance_tools + * pre-commit cleanup + * Add QOS durability to SmaccPublisherClient (#163) + * feat: add qos durability to SmaccPublisherClient + * fix: add a missing colon + * refactor: remove line + * feat: add reliability qos config + * Feature/testing moveit behaviors (#167) + * more testing on moveit + * progress on moveit + * more testing on moveit behaviors + * minor configuration + * fixing pipeline error + * fixing broken master build + * sm_pubsub_1 (#169) + Co-authored-by: Ubuntu 20-04-02-amd64 + * sm_pubsub_1 part 2 (#170) + * sm_pubsub_1 part 2 + * sm_pubsub_1 part 2 + Co-authored-by: Ubuntu 20-04-02-amd64 + * sm_advanced_recovery_1 renaming (#171) + Co-authored-by: Ubuntu 20-04-02-amd64 + * sm_multi_stage_1 reworking (#172) + * multistage modes + * sm_multi_stage sequences + * sm_multi_state_1 steps + * sm_multi_stage_1 sequence d + * sm_multi_stage_1 c sequence + * mode_5_sequence_b + * mode_4_sequence_b + * sm_multi_stage_1 most + * finishing touches 1 + * readme + Co-authored-by: Ubuntu 20-04-02-amd64 + * Feature/aws navigation sm dance bot (#174) + * repo dependency + * husky launch file in sm_dance_bot + * Add dependencies for husky simulation. + * Fix formatting. + * Update dependencies for husky in rolling and galactic. + * minor + * progress on aws navigation and some other refactorings on navigation clients and behaviors + * more on aws demo + * fixing broken build + * minor + Co-authored-by: Denis Štogl + Co-authored-by: Denis Štogl + * minor changes (#175) + * warehouse2 (#177) + * Waypoint Inputs (#178) + Co-authored-by: Ubuntu 20-04-02-amd64 + * wharehouse2 progress (#179) + * format (#180) + * sm_dance_bot_warehouse_3 (#181) + Co-authored-by: Ubuntu 20-04-02-amd64 + * Feature/sm warehouse 2 13 dec 2 (#182) + * format + * more changes and headless + * merge + * headless and other fixes + * default values + * Brettpac branch (#184) + * sm_dance_bot_warehouse_3 + * Redoing sm_dance_bot_warehouse_3 waypoints + * More Waypoints + Co-authored-by: Ubuntu 20-04-02-amd64 + * SrConditional fixes and formatting (#168) + * fix: some formatting and templating on SrConditional + * fix: move trigger logic into headers + * fix: lint + * Feature/wharehouse2 dec 14 (#185) + * warehouse2 + * minor + * Feature/sm warehouse 2 13 dec 2 (#186) + * format + * more changes and headless + * merge + * headless and other fixes + * default values + * minor + * finetuning waypoints (#187) + Co-authored-by: Ubuntu 20-04-02-amd64 + * Feature/cb pure spinning (#188) + * format + * more changes and headless + * merge + * headless and other fixes + * default values + * minor + * Feature/cb pure spinning (#189) + * format + * more changes and headless + * merge + * headless and other fixes + * default values + * minor + * pure spinning behavior missing files + * minor changes (#190) + * Feature/planner changes 16 12 (#191) + * minor changes + * more fixes + * minor + * minor + * Feature/replanning 16 dec (#193) + * minor changes + * replanning for all our examples + * several fixes (#194) + * minor changes (#195) + * Feature/undo motion 20 12 (#196) + * minor changes + * replanning for all our examples + * improving undo motion navigation warehouse2 + * minor + * tuning warehouse3 (#197) + * Feature/undo motion 20 12 (#198) + * minor changes + * replanning for all our examples + * improving undo motion navigation warehouse2 + * minor + * undo tuning and errors + * format + * Feature/sync 21 12 (#199) + * minor changes + * replanning for all our examples + * format issues + * Feature/warehouse2 22 12 (#200) + * minor changes + * replanning for all our examples + * format issues + * finishing warehouse2 + * Feature/warehouse2 23 12 (#201) + * minor changes + * replanning for all our examples + * tuning and fixes (#202) + * Feature/minor tune (#203) + * tuning and fixes + * minor tune + * fixing warehouse 3 problems, and other core improvements (#204) + * fixing warehouse 3 problems, and other core improvements to remove dead lock, also making continuous integration green + * weird moveit not downloaded repo + * added missing file from warehouse2 (#205) + * backport to foxy + * minor format + * minor linking errors foxy + Co-authored-by: Denis Štogl + Co-authored-by: Denis Štogl + Co-authored-by: Declan Dury <44791484+DecDury@users.noreply.github.com> + Co-authored-by: DecDury + Co-authored-by: reelrbtx + Co-authored-by: brettpac + Co-authored-by: David Revay + * missing + * missing sm + * updating subscriber publisher components + * progress in autowarrrrrrrrrrrrrrrrrrrrrrrrrrrrrrrrrre machine + * refining cp subscriber cp publisher + * improvements in smacc core adding more components mostly developed for autoware demo + * autoware demo + * missing + * foxy ci + * fix + * minor broken build + * some reordering fixes + * minor + * docker files for different revisions, warnings removval and more testing on navigation + * fixing docker for foxy and galactic + * Update file for fake hardware simulation and add file for gazebo simulation. + * docker build files for all versions + * retry behavior warehouse 1 + * missing file + * minor format fix + * other minor changes + Co-authored-by: Denis Štogl + Co-authored-by: Denis Štogl + Co-authored-by: Declan Dury <44791484+DecDury@users.noreply.github.com> + Co-authored-by: DecDury + Co-authored-by: reelrbtx + Co-authored-by: brettpac + Co-authored-by: David Revay + Co-authored-by: pabloinigoblasco + * fix broken source build (#227) + * Only rolling version should be pre-released on on master. (#230) + * Correct Focal-Rolling builds by fixing the version of rosdep yaml (#234) + * Update file for fake hardware simulation and add file for gazebo simulation. (#224) + * Update file for fake hardware simulation and add file for gazebo simulation. + * Add ignition file and update repos files. + * Feature/improvements warehouse3 (#228) + * minor changes + * replanning for all our examples + * backport to foxy + * minor format + * minor linking errors foxy + * Foxy backport (#206) + * minor formatting fixes + * Fix trailing spaces. + * Correct codespell. + * Correct python linters warnings. + * Add galactic CI build because Navigation2 is broken in rolling. + * Add partial changes for ament_cpplint. + * Add tf2_ros as dependency to find include. + * Disable ament_cpplint. + * Disable some packages and update workflows. + * Bump ccache version. + * Ignore further packages + * Satisfy ament_lint_cmake + * Add missing licences. + * Disable cpplint and cppcheck linters. + * Correct formatters. + * branching example + * Disable disabled packages + * Update ci-build-source.yml + * Change extension + * Change extension of imports. + * Enable cppcheck + * Correct formatting of python file. + * Included necessary package and edited Threesome launch + Changed... + ros2 launch sm_three_some sm_three_some + to + ros2 launch sm_three_some sm_three_some.launch + Added: + First ensure you have the necessary package installed. + ``` + sudo apt-get install ros-rolling-ros2trace + ``` + Then run this command. + * Rename header files and correct format. + * Add workflow for checking doc build. + * Update doxygen-check-build.yml + * Create doxygen-deploy.yml + * Use manual deployment for now. + * Create workflow for testing prerelease builds + * Use docs/ as source folder for documentation + * Use docs/ as output directory. + * Rename to smacc2 and smacc2_msgs + * Correct GitHub branch reference. + * Update name of package and package.xml to pass liter. + * Execute on master update + * Reset all versions to 0.0.0 + * Ignore all packages except smacc2 and smacc2_msgs + * Update changelogs + * 0.1.0 + * Revert "Ignore all packages except smacc2 and smacc2_msgs" + This reverts commit f603166a4b3ccdfe96c64d9f9fb9d8b49fbf0e61. + * Update description table. + * Update table + * Copy initial docs + * Dockerfile w/ ROS distro as argument + use this command "sudo docker build --build-arg ROS2_DISTRO=(desiredRosTag) (directoryHoldingDockerfile)/" + * Opened new folder for additional tracing contents + * Delete tracing directory + * Moved tracing.md to tracing directory + * added setupTracing.sh + Installs necessary packages and configures tracing group. + * Removed manual installation of ros-rolling-ros2trace + This is now automated in setupTracing.sh + location of sh file assumed if user follows README.md under "Getting started" + * Created alternative ManualTracing + * added new sm markdowns + * added a dockerfile for Rolling and Galactic + * Update smacc2_ci/docker/ros_rollingAndGalactic_ubuntu_20.04/buildGalactic.sh + Co-authored-by: Denis Štogl + * Update tracing/ManualTracing.md + Co-authored-by: Denis Štogl + * changed wording "smacc application" to "SMACC2 library" + Co-authored-by: Denis Štogl + * Update smacc_sm_reference_library/sm_atomic/README.md + edit from html to markdown syntax + Co-authored-by: Denis Štogl + * reactivating smacc2 nav clients for rolling via submodules + * renamed tracing events after + * bug in smacc2 component + * reverted markdowns to html + * added README tutorial for Dockerfile + * additional cleanup + * cleanup + * cleanup + * edited tracing.md to reflect new tracing event names + * Enable build of missing rolling repositories. + * Enable Navigation2 for semi-binary build. + * Remove galactic builds from master and kepp only rolling. Remove submodules and use .repos file + * updated mentions of SMACC/ROS to SMACC2/ROS2 + * some progress on navigation rolling + * renamed folders, deleted tracing.md, edited README.md + * added smacc2_performance_tools + * performance tests improvements + * more on performance and other issues + * sm_respira_1 format cleanup + * sm_respira_1 format cleanup pre-commit + * sm_respira_test_2 + * sm_respira_test_2 + * more changes on performance tests + * Do not execute clang-format on smacc2_sm_reference_library package. + * sm_reference_library reformatting + * Correct trailing spaces. + * sm_atomic_24hr + * sm_atomic_performance_trace_1 + * Update smacc2_rta command across readmes + * Clean up of sm_atomic_24hr + * more sm_atomic_24hr cleanup + * Optimized deps in move_base_z_planners_common. + * Renaming of event generator library + * minor formatting + * Add galactic CI setup and rename rolling files. (#58) + * Fix source CI and correct README overview. (#62) + * Update c_cpp_properties.json + * changed launch command to ros2 launch sm_respira_1 sm_respira_1.launch (#69) + also noticed a note I had made while producing these that was not removed + * update doxygen links (#70) + Co-authored-by: Ubuntu 20-04-02-amd64 + * More Readme Updates (#72) + Co-authored-by: Ubuntu 20-04-02-amd64 + * More Readme (#74) + Co-authored-by: Ubuntu 20-04-02-amd64 + * created new sm from sm_respira_1 (#76) + * Feature/core and navigation fixes (#78) + * base for the sm_aws_aarehouse navigation + * progressing in aws navigation + * minor + * several core improvements during navigation testing + * formatting improvements + * progress in aws navigation demo + * format improvements + * Feature/aws demo progress (#80) + * base for the sm_aws_aarehouse navigation + * progressing in aws navigation + * minor + * several core improvements during navigation testing + * formatting improvements + * progress in aws navigation demo + * format improvements + * format improvements + * more on navigation + * sm_advanced_recovery_1 reworked (#83) + * sm_advanced_recovery_1 reworked + * fix pre-commit + * Trying to fix Pre-Commit + Co-authored-by: Ubuntu 20-04-02-amd64 + * more sm_advanced_recovery_1 (#84) + Co-authored-by: Ubuntu 20-04-02-amd64 + * More sm_advanced_recovery_1 work (#85) + Co-authored-by: Ubuntu 20-04-02-amd64 + * sm_advanced_recovery_1 round 4 (#86) + Co-authored-by: Ubuntu 20-04-02-amd64 + * Brettpac branch (#87) + * sm_atomic_performance_test_a_2 + * sm_atomic_performance_test_a_1 + Co-authored-by: Ubuntu 20-04-02-amd64 + * sm_atomic_performance_test_c_1 (#88) + Co-authored-by: Ubuntu 20-04-02-amd64 + * modifying sm_atomic_performance_test_a_2 (#89) + Co-authored-by: Ubuntu 20-04-02-amd64 + * sm_multi_stage_1 (#90) + * sm_multi_stage_1 + * fixing precommit + Co-authored-by: Ubuntu 20-04-02-amd64 + * more sm_multi_stage_1 (#91) + Co-authored-by: Ubuntu 20-04-02-amd64 + * Update README.md + updated launch command + * Wait topic message client behavior (#81) + * base for the sm_aws_aarehouse navigation + * progressing in aws navigation + * minor + * several core improvements during navigation testing + * formatting improvements + * progress in aws navigation demo + * format improvements + * format improvements + * more on navigation + * new feature, cb_wait_topic_message: asynchronous client behavior that waits a topic message and optionally checks its contents to success + * formatting + * attempting precommit fixes + Co-authored-by: Ubuntu 20-04-02-amd64 + Co-authored-by: Denis Štogl + * Feature/wait nav2 nodes client behavior (#82) + * base for the sm_aws_aarehouse navigation + * progressing in aws navigation + * minor + * several core improvements during navigation testing + * formatting improvements + * progress in aws navigation demo + * format improvements + * format improvements + * more on navigation + * new feature, cb_wait_topic_message: asynchronous client behavior that waits a topic message and optionally checks its contents to success + * formatting + * adding new client behavior add for nav2, wait nav2 nodes subscribing to the /bond topic and waiting they are alive. you optionally can select the nodes to wait + * Correct all linters and formaters. + Co-authored-by: Denis Štogl + Co-authored-by: Denis Štogl + * Feature/aws demo progress (#92) + * base for the sm_aws_aarehouse navigation + * progressing in aws navigation + * minor + * several core improvements during navigation testing + * formatting improvements + * progress in aws navigation demo + * format improvements + * format improvements + * more on navigation + * new feature, cb_wait_topic_message: asynchronous client behavior that waits a topic message and optionally checks its contents to success + * formatting + * adding new client behavior add for nav2, wait nav2 nodes subscribing to the /bond topic and waiting they are alive. you optionally can select the nodes to wait + * progress in aws navigation demo + * minor format + * Feature/sm dance bot fixes (#93) + * base for the sm_aws_aarehouse navigation + * progressing in aws navigation + * minor + * several core improvements during navigation testing + * formatting improvements + * progress in aws navigation demo + * format improvements + * format improvements + * more on navigation + * new feature, cb_wait_topic_message: asynchronous client behavior that waits a topic message and optionally checks its contents to success + * formatting + * adding new client behavior add for nav2, wait nav2 nodes subscribing to the /bond topic and waiting they are alive. you optionally can select the nodes to wait + * progress in aws navigation demo + * minor format + * navigation parameters fixes on sm_dance_bot + * minor format + * Feature/sm aws warehouse (#94) + * base for the sm_aws_aarehouse navigation + * progressing in aws navigation + * minor + * several core improvements during navigation testing + * formatting improvements + * progress in aws navigation demo + * format improvements + * format improvements + * more on navigation + * new feature, cb_wait_topic_message: asynchronous client behavior that waits a topic message and optionally checks its contents to success + * formatting + * adding new client behavior add for nav2, wait nav2 nodes subscribing to the /bond topic and waiting they are alive. you optionally can select the nodes to wait + * progress in aws navigation demo + * minor format + * merge and progress + * fix format + * Feature/sm dance bot fixes (#95) + * base for the sm_aws_aarehouse navigation + * progressing in aws navigation + * minor + * several core improvements during navigation testing + * formatting improvements + * progress in aws navigation demo + * format improvements + * format improvements + * more on navigation + * new feature, cb_wait_topic_message: asynchronous client behavior that waits a topic message and optionally checks its contents to success + * formatting + * adding new client behavior add for nav2, wait nav2 nodes subscribing to the /bond topic and waiting they are alive. you optionally can select the nodes to wait + * progress in aws navigation demo + * minor format + * navigation parameters fixes on sm_dance_bot + * minor format + * minor + * minor format + * Remove some compile warnings. (#96) + * Feature/cb pause slam (#98) + * base for the sm_aws_aarehouse navigation + * progressing in aws navigation + * minor + * several core improvements during navigation testing + * formatting improvements + * progress in aws navigation demo + * format improvements + * format improvements + * more on navigation + * new feature, cb_wait_topic_message: asynchronous client behavior that waits a topic message and optionally checks its contents to success + * formatting + * adding new client behavior add for nav2, wait nav2 nodes subscribing to the /bond topic and waiting they are alive. you optionally can select the nodes to wait + * progress in aws navigation demo + * minor format + * navigation parameters fixes on sm_dance_bot + * minor format + * minor + * formatting + * cb pause slam client behavior + * sm_dance_bot_lite (#99) + * sm_dance_bot_lite + * precommit + * Updates yaml + Co-authored-by: Ubuntu 20-04-02-amd64 + * Rename doxygen deployment workflow (#100) + * minor hotfix + * sm_dance_bot visualizing turtlebot3 (#101) + * Feature/dance bot launch gz lidar choice (#102) + * sm_dance_bot visualizing turtlebot3 + * cleaning and lidar show/hide option + * cleaning files and making formatting work + * more fixes + * Feature/sm dance bot lite gazebo fixes (#104) + * sm_dance_bot visualizing turtlebot3 + * cleaning and lidar show/hide option + * cleaning files and making formatting work + * more fixes + * gazebo fixes, to show the robot and the lidar + * format fixes + * sm_multi_stage_1 doubling (#103) + Co-authored-by: Ubuntu 20-04-02-amd64 + * Feature/sm dance bot strikes back gazebo fixes (#105) + * sm_dance_bot visualizing turtlebot3 + * cleaning and lidar show/hide option + * cleaning files and making formatting work + * more fixes + * gazebo fixes, to show the robot and the lidar + * format fixes + * gazebo fixes for sm_dance_bot_strikes_back + * precommit cleanup run (#106) + Co-authored-by: Ubuntu 20-04-02-amd64 + * aws demo (#108) + * aws demo + * format + * got sm_multi_stage_1 working (barely) (#109) + Co-authored-by: Ubuntu 20-04-02-amd64 + * Brettpac branch (#110) + * got sm_multi_stage_1 working (barely) + * gaining traction sm_multi_stage_1 + * more + * don't remember + * making progress + * More + * keep hammering + * two stages + * 3 part + * 4th stage + Co-authored-by: Ubuntu 20-04-02-amd64 + * Brettpac branch (#111) + * got sm_multi_stage_1 working (barely) + * gaining traction sm_multi_stage_1 + * more + * don't remember + * making progress + * More + * keep hammering + * two stages + * 3 part + * 4th stage + * 5th stage + Co-authored-by: Ubuntu 20-04-02-amd64 + * a3 (#113) + Co-authored-by: Ubuntu 20-04-02-amd64 + * Remove neo_simulation2 package. (#112) + * Remove neo_simulation2 package. + * Correct formatting. + * Enable source build on PR for testing. + * Adjust build packages of source CI + * more sm_multi_stage_1 (#114) + Co-authored-by: Ubuntu 20-04-02-amd64 + * mm (#115) + Co-authored-by: Ubuntu 20-04-02-amd64 + * diverse improvements navigation and performance (#116) + * diverse improvements navigation and performance + * minor + Co-authored-by: pabloinigoblasco + * Feature/diverse improvemets navigation performance (#117) + * diverse improvements navigation and performance + * minor + * additional linting and formatting + * Remove merge markers from a python file. (#119) + * Feature/slam toggle and smacc deep history (#122) + * progress in navigation, slam toggle client behaviors and slam_toolbox components. Also smacc2::deep_history syntax + * going forward in testing sm_dance_bot introducing slam pausing/resuming funcionality + * feature/more_sm_dance_bot_fixes + * minor format + * minor (#124) + Co-authored-by: Ubuntu 20-04-02-amd64 + * more changes in sm_dance_bot (#125) + * Move method after the method it calls. Otherwise recursion could happen. (#126) + * Feature/dance bot s pattern (#128) + * more changes in sm_dance_bot + * polishing sm_dance_bot and s-pattern + * noticed typo + Finnaly > Finally + * Feature/dance bot s pattern (#129) + * more changes in sm_dance_bot + * polishing sm_dance_bot and s-pattern + * more refinement in sm_dance_bot + * First working version of sm template and template generator. (#127) + * minor tweaks (#130) + Co-authored-by: Ubuntu 20-04-02-amd64 + * Feature/sm dance bot refine (#131) + * more changes in sm_dance_bot + * minor + * Feature/sm dance bot refine 2 (#132) + * more changes in sm_dance_bot + * minor + * build fix + * waypoints navigator bug (#133) + * minor tuning to mitigate overshot issue cases + * progress in the sm_dance_bot tests (#135) + * some more progress on markers cleanup + * minor format issues (#134) + * sm_dance_bot_lite (#136) + Co-authored-by: Ubuntu 20-04-02-amd64 + * Resolve compile wanings (#137) + * Add SM core test (#138) + * minor navigation improvements (#141) + * using local action msgs (#139) + * using local action msgs + * removing sm_dance_bot_msgs + * pending references + * Feature/nitrosz renaming (#144) + * using local action msgs + * removing sm_dance_bot_msgs + * pending references + * navigation 2 stack renaming + * formatting + * added SVGs to READMEs of atomic, dance_bot, and others (#140) + * added remaining SVGs to READMEs (#145) + * added remaining SVGs to READMEs + * precommit cleanup + * Update package list. (#142) + * removing parameters smacc (#147) + * removing parameters smacc + * workflows update + * workflow + * Noticed launch command was incorrect in README.md + fixed launch command for sm_dance_bot_strikes_back and removed some comments I had made in the past. + * Fix CI: format fix python version (#148) + * Add SM Atomic SM generator. (#143) + * Remove node creation and create only a logger. (#149) + * Rolling Docker environment to be executed from any environment (#154) + * Feature/sm dance bot strikes back refactoring (#152) + Co-authored-by: DecDury + Co-authored-by: Denis Štogl + * slight waypoint 4 and iterations changes so robot can complete course (#155) + * Feature/migration moveit client (#151) + * initial migration to smacc2 + * fixing some errors introduced on formatting + * missing dependency + * fixing some more linting warnings + * minor + * removing test from main moveit cmake + * test ur5 + * progressing in the moveit migration testing + * updating format + * adding .reps dependencies and also fixing some build errors + * repos dependency + * adding dependency to ur5 client + * docker refactoring + * minor + * progress on move_it PR + * minor dockerfile test workaround + * improving dockerfile for building local tests + * minor + * fixing compiling issues + * update readme (#164) + * update readme + * more readme updates + * more + Co-authored-by: Ubuntu 20-04-02-amd64 + * initial state machine transition timestamp (#165) + * moved reference library SMs to smacc2_performance_tools (#166) + * moved reference library SMs to smacc2_performance_tools + * pre-commit cleanup + * Add QOS durability to SmaccPublisherClient (#163) + * feat: add qos durability to SmaccPublisherClient + * fix: add a missing colon + * refactor: remove line + * feat: add reliability qos config + * Feature/testing moveit behaviors (#167) + * more testing on moveit + * progress on moveit + * more testing on moveit behaviors + * minor configuration + * fixing pipeline error + * fixing broken master build + * sm_pubsub_1 (#169) + Co-authored-by: Ubuntu 20-04-02-amd64 + * sm_pubsub_1 part 2 (#170) + * sm_pubsub_1 part 2 + * sm_pubsub_1 part 2 + Co-authored-by: Ubuntu 20-04-02-amd64 + * sm_advanced_recovery_1 renaming (#171) + Co-authored-by: Ubuntu 20-04-02-amd64 + * sm_multi_stage_1 reworking (#172) + * multistage modes + * sm_multi_stage sequences + * sm_multi_state_1 steps + * sm_multi_stage_1 sequence d + * sm_multi_stage_1 c sequence + * mode_5_sequence_b + * mode_4_sequence_b + * sm_multi_stage_1 most + * finishing touches 1 + * readme + Co-authored-by: Ubuntu 20-04-02-amd64 + * Feature/aws navigation sm dance bot (#174) + * repo dependency + * husky launch file in sm_dance_bot + * Add dependencies for husky simulation. + * Fix formatting. + * Update dependencies for husky in rolling and galactic. + * minor + * progress on aws navigation and some other refactorings on navigation clients and behaviors + * more on aws demo + * fixing broken build + * minor + Co-authored-by: Denis Štogl + Co-authored-by: Denis Štogl + * minor changes (#175) + * warehouse2 (#177) + * Waypoint Inputs (#178) + Co-authored-by: Ubuntu 20-04-02-amd64 + * wharehouse2 progress (#179) + * format (#180) + * sm_dance_bot_warehouse_3 (#181) + Co-authored-by: Ubuntu 20-04-02-amd64 + * Feature/sm warehouse 2 13 dec 2 (#182) + * format + * more changes and headless + * merge + * headless and other fixes + * default values + * Brettpac branch (#184) + * sm_dance_bot_warehouse_3 + * Redoing sm_dance_bot_warehouse_3 waypoints + * More Waypoints + Co-authored-by: Ubuntu 20-04-02-amd64 + * SrConditional fixes and formatting (#168) + * fix: some formatting and templating on SrConditional + * fix: move trigger logic into headers + * fix: lint + * Feature/wharehouse2 dec 14 (#185) + * warehouse2 + * minor + * Feature/sm warehouse 2 13 dec 2 (#186) + * format + * more changes and headless + * merge + * headless and other fixes + * default values + * minor + * finetuning waypoints (#187) + Co-authored-by: Ubuntu 20-04-02-amd64 + * Feature/cb pure spinning (#188) + * format + * more changes and headless + * merge + * headless and other fixes + * default values + * minor + * Feature/cb pure spinning (#189) + * format + * more changes and headless + * merge + * headless and other fixes + * default values + * minor + * pure spinning behavior missing files + * minor changes (#190) + * Feature/planner changes 16 12 (#191) + * minor changes + * more fixes + * minor + * minor + * Feature/replanning 16 dec (#193) + * minor changes + * replanning for all our examples + * several fixes (#194) + * minor changes (#195) + * Feature/undo motion 20 12 (#196) + * minor changes + * replanning for all our examples + * improving undo motion navigation warehouse2 + * minor + * tuning warehouse3 (#197) + * Feature/undo motion 20 12 (#198) + * minor changes + * replanning for all our examples + * improving undo motion navigation warehouse2 + * minor + * undo tuning and errors + * format + * Feature/sync 21 12 (#199) + * minor changes + * replanning for all our examples + * format issues + * Feature/warehouse2 22 12 (#200) + * minor changes + * replanning for all our examples + * format issues + * finishing warehouse2 + * Feature/warehouse2 23 12 (#201) + * minor changes + * replanning for all our examples + * tuning and fixes (#202) + * Feature/minor tune (#203) + * tuning and fixes + * minor tune + * fixing warehouse 3 problems, and other core improvements (#204) + * fixing warehouse 3 problems, and other core improvements to remove dead lock, also making continuous integration green + * weird moveit not downloaded repo + * added missing file from warehouse2 (#205) + * backport to foxy + * minor format + * minor linking errors foxy + Co-authored-by: Denis Štogl + Co-authored-by: Denis Štogl + Co-authored-by: Declan Dury <44791484+DecDury@users.noreply.github.com> + Co-authored-by: DecDury + Co-authored-by: reelrbtx + Co-authored-by: brettpac + Co-authored-by: David Revay + * missing + * missing sm + * updating subscriber publisher components + * progress in autowarrrrrrrrrrrrrrrrrrrrrrrrrrrrrrrrrre machine + * refining cp subscriber cp publisher + * improvements in smacc core adding more components mostly developed for autoware demo + * autoware demo + * missing + * foxy ci + * fix + * minor broken build + * some reordering fixes + * minor + * docker files for different revisions, warnings removval and more testing on navigation + * fixing docker for foxy and galactic + * docker build files for all versions + * barrel demo + * barrel search build fix and warehouse3 + * fixing startup problems in warehouse 3 + * fixing format and minor + * minor + * progress in barrel husky + * minor + * barrel demo + * progress + * fixing broken build + Co-authored-by: Denis Štogl + Co-authored-by: Denis Štogl + Co-authored-by: Declan Dury <44791484+DecDury@users.noreply.github.com> + Co-authored-by: DecDury + Co-authored-by: reelrbtx + Co-authored-by: brettpac + Co-authored-by: David Revay + Co-authored-by: pabloinigoblasco + * Feature/barrel - do not merge yet (#233) + * minor changes + * replanning for all our examples + * backport to foxy + * minor format + * minor linking errors foxy + * Foxy backport (#206) + * minor formatting fixes + * Fix trailing spaces. + * Correct codespell. + * Correct python linters warnings. + * Add galactic CI build because Navigation2 is broken in rolling. + * Add partial changes for ament_cpplint. + * Add tf2_ros as dependency to find include. + * Disable ament_cpplint. + * Disable some packages and update workflows. + * Bump ccache version. + * Ignore further packages + * Satisfy ament_lint_cmake + * Add missing licences. + * Disable cpplint and cppcheck linters. + * Correct formatters. + * branching example + * Disable disabled packages + * Update ci-build-source.yml + * Change extension + * Change extension of imports. + * Enable cppcheck + * Correct formatting of python file. + * Included necessary package and edited Threesome launch + Changed... + ros2 launch sm_three_some sm_three_some + to + ros2 launch sm_three_some sm_three_some.launch + Added: + First ensure you have the necessary package installed. + ``` + sudo apt-get install ros-rolling-ros2trace + ``` + Then run this command. + * Rename header files and correct format. + * Add workflow for checking doc build. + * Update doxygen-check-build.yml + * Create doxygen-deploy.yml + * Use manual deployment for now. + * Create workflow for testing prerelease builds + * Use docs/ as source folder for documentation + * Use docs/ as output directory. + * Rename to smacc2 and smacc2_msgs + * Correct GitHub branch reference. + * Update name of package and package.xml to pass liter. + * Execute on master update + * Reset all versions to 0.0.0 + * Ignore all packages except smacc2 and smacc2_msgs + * Update changelogs + * 0.1.0 + * Revert "Ignore all packages except smacc2 and smacc2_msgs" + This reverts commit f603166a4b3ccdfe96c64d9f9fb9d8b49fbf0e61. + * Update description table. + * Update table + * Copy initial docs + * Dockerfile w/ ROS distro as argument + use this command "sudo docker build --build-arg ROS2_DISTRO=(desiredRosTag) (directoryHoldingDockerfile)/" + * Opened new folder for additional tracing contents + * Delete tracing directory + * Moved tracing.md to tracing directory + * added setupTracing.sh + Installs necessary packages and configures tracing group. + * Removed manual installation of ros-rolling-ros2trace + This is now automated in setupTracing.sh + location of sh file assumed if user follows README.md under "Getting started" + * Created alternative ManualTracing + * added new sm markdowns + * added a dockerfile for Rolling and Galactic + * Update smacc2_ci/docker/ros_rollingAndGalactic_ubuntu_20.04/buildGalactic.sh + Co-authored-by: Denis Štogl + * Update tracing/ManualTracing.md + Co-authored-by: Denis Štogl + * changed wording "smacc application" to "SMACC2 library" + Co-authored-by: Denis Štogl + * Update smacc_sm_reference_library/sm_atomic/README.md + edit from html to markdown syntax + Co-authored-by: Denis Štogl + * reactivating smacc2 nav clients for rolling via submodules + * renamed tracing events after + * bug in smacc2 component + * reverted markdowns to html + * added README tutorial for Dockerfile + * additional cleanup + * cleanup + * cleanup + * edited tracing.md to reflect new tracing event names + * Enable build of missing rolling repositories. + * Enable Navigation2 for semi-binary build. + * Remove galactic builds from master and kepp only rolling. Remove submodules and use .repos file + * updated mentions of SMACC/ROS to SMACC2/ROS2 + * some progress on navigation rolling + * renamed folders, deleted tracing.md, edited README.md + * added smacc2_performance_tools + * performance tests improvements + * more on performance and other issues + * sm_respira_1 format cleanup + * sm_respira_1 format cleanup pre-commit + * sm_respira_test_2 + * sm_respira_test_2 + * more changes on performance tests + * Do not execute clang-format on smacc2_sm_reference_library package. + * sm_reference_library reformatting + * Correct trailing spaces. + * sm_atomic_24hr + * sm_atomic_performance_trace_1 + * Update smacc2_rta command across readmes + * Clean up of sm_atomic_24hr + * more sm_atomic_24hr cleanup + * Optimized deps in move_base_z_planners_common. + * Renaming of event generator library + * minor formatting + * Add galactic CI setup and rename rolling files. (#58) + * Fix source CI and correct README overview. (#62) + * Update c_cpp_properties.json + * changed launch command to ros2 launch sm_respira_1 sm_respira_1.launch (#69) + also noticed a note I had made while producing these that was not removed + * update doxygen links (#70) + Co-authored-by: Ubuntu 20-04-02-amd64 + * More Readme Updates (#72) + Co-authored-by: Ubuntu 20-04-02-amd64 + * More Readme (#74) + Co-authored-by: Ubuntu 20-04-02-amd64 + * created new sm from sm_respira_1 (#76) + * Feature/core and navigation fixes (#78) + * base for the sm_aws_aarehouse navigation + * progressing in aws navigation + * minor + * several core improvements during navigation testing + * formatting improvements + * progress in aws navigation demo + * format improvements + * Feature/aws demo progress (#80) + * base for the sm_aws_aarehouse navigation + * progressing in aws navigation + * minor + * several core improvements during navigation testing + * formatting improvements + * progress in aws navigation demo + * format improvements + * format improvements + * more on navigation + * sm_advanced_recovery_1 reworked (#83) + * sm_advanced_recovery_1 reworked + * fix pre-commit + * Trying to fix Pre-Commit + Co-authored-by: Ubuntu 20-04-02-amd64 + * more sm_advanced_recovery_1 (#84) + Co-authored-by: Ubuntu 20-04-02-amd64 + * More sm_advanced_recovery_1 work (#85) + Co-authored-by: Ubuntu 20-04-02-amd64 + * sm_advanced_recovery_1 round 4 (#86) + Co-authored-by: Ubuntu 20-04-02-amd64 + * Brettpac branch (#87) + * sm_atomic_performance_test_a_2 + * sm_atomic_performance_test_a_1 + Co-authored-by: Ubuntu 20-04-02-amd64 + * sm_atomic_performance_test_c_1 (#88) + Co-authored-by: Ubuntu 20-04-02-amd64 + * modifying sm_atomic_performance_test_a_2 (#89) + Co-authored-by: Ubuntu 20-04-02-amd64 + * sm_multi_stage_1 (#90) + * sm_multi_stage_1 + * fixing precommit + Co-authored-by: Ubuntu 20-04-02-amd64 + * more sm_multi_stage_1 (#91) + Co-authored-by: Ubuntu 20-04-02-amd64 + * Update README.md + updated launch command + * Wait topic message client behavior (#81) + * base for the sm_aws_aarehouse navigation + * progressing in aws navigation + * minor + * several core improvements during navigation testing + * formatting improvements + * progress in aws navigation demo + * format improvements + * format improvements + * more on navigation + * new feature, cb_wait_topic_message: asynchronous client behavior that waits a topic message and optionally checks its contents to success + * formatting + * attempting precommit fixes + Co-authored-by: Ubuntu 20-04-02-amd64 + Co-authored-by: Denis Štogl + * Feature/wait nav2 nodes client behavior (#82) + * base for the sm_aws_aarehouse navigation + * progressing in aws navigation + * minor + * several core improvements during navigation testing + * formatting improvements + * progress in aws navigation demo + * format improvements + * format improvements + * more on navigation + * new feature, cb_wait_topic_message: asynchronous client behavior that waits a topic message and optionally checks its contents to success + * formatting + * adding new client behavior add for nav2, wait nav2 nodes subscribing to the /bond topic and waiting they are alive. you optionally can select the nodes to wait + * Correct all linters and formaters. + Co-authored-by: Denis Štogl + Co-authored-by: Denis Štogl + * Feature/aws demo progress (#92) + * base for the sm_aws_aarehouse navigation + * progressing in aws navigation + * minor + * several core improvements during navigation testing + * formatting improvements + * progress in aws navigation demo + * format improvements + * format improvements + * more on navigation + * new feature, cb_wait_topic_message: asynchronous client behavior that waits a topic message and optionally checks its contents to success + * formatting + * adding new client behavior add for nav2, wait nav2 nodes subscribing to the /bond topic and waiting they are alive. you optionally can select the nodes to wait + * progress in aws navigation demo + * minor format + * Feature/sm dance bot fixes (#93) + * base for the sm_aws_aarehouse navigation + * progressing in aws navigation + * minor + * several core improvements during navigation testing + * formatting improvements + * progress in aws navigation demo + * format improvements + * format improvements + * more on navigation + * new feature, cb_wait_topic_message: asynchronous client behavior that waits a topic message and optionally checks its contents to success + * formatting + * adding new client behavior add for nav2, wait nav2 nodes subscribing to the /bond topic and waiting they are alive. you optionally can select the nodes to wait + * progress in aws navigation demo + * minor format + * navigation parameters fixes on sm_dance_bot + * minor format + * Feature/sm aws warehouse (#94) + * base for the sm_aws_aarehouse navigation + * progressing in aws navigation + * minor + * several core improvements during navigation testing + * formatting improvements + * progress in aws navigation demo + * format improvements + * format improvements + * more on navigation + * new feature, cb_wait_topic_message: asynchronous client behavior that waits a topic message and optionally checks its contents to success + * formatting + * adding new client behavior add for nav2, wait nav2 nodes subscribing to the /bond topic and waiting they are alive. you optionally can select the nodes to wait + * progress in aws navigation demo + * minor format + * merge and progress + * fix format + * Feature/sm dance bot fixes (#95) + * base for the sm_aws_aarehouse navigation + * progressing in aws navigation + * minor + * several core improvements during navigation testing + * formatting improvements + * progress in aws navigation demo + * format improvements + * format improvements + * more on navigation + * new feature, cb_wait_topic_message: asynchronous client behavior that waits a topic message and optionally checks its contents to success + * formatting + * adding new client behavior add for nav2, wait nav2 nodes subscribing to the /bond topic and waiting they are alive. you optionally can select the nodes to wait + * progress in aws navigation demo + * minor format + * navigation parameters fixes on sm_dance_bot + * minor format + * minor + * minor format + * Remove some compile warnings. (#96) + * Feature/cb pause slam (#98) + * base for the sm_aws_aarehouse navigation + * progressing in aws navigation + * minor + * several core improvements during navigation testing + * formatting improvements + * progress in aws navigation demo + * format improvements + * format improvements + * more on navigation + * new feature, cb_wait_topic_message: asynchronous client behavior that waits a topic message and optionally checks its contents to success + * formatting + * adding new client behavior add for nav2, wait nav2 nodes subscribing to the /bond topic and waiting they are alive. you optionally can select the nodes to wait + * progress in aws navigation demo + * minor format + * navigation parameters fixes on sm_dance_bot + * minor format + * minor + * formatting + * cb pause slam client behavior + * sm_dance_bot_lite (#99) + * sm_dance_bot_lite + * precommit + * Updates yaml + Co-authored-by: Ubuntu 20-04-02-amd64 + * Rename doxygen deployment workflow (#100) + * minor hotfix + * sm_dance_bot visualizing turtlebot3 (#101) + * Feature/dance bot launch gz lidar choice (#102) + * sm_dance_bot visualizing turtlebot3 + * cleaning and lidar show/hide option + * cleaning files and making formatting work + * more fixes + * Feature/sm dance bot lite gazebo fixes (#104) + * sm_dance_bot visualizing turtlebot3 + * cleaning and lidar show/hide option + * cleaning files and making formatting work + * more fixes + * gazebo fixes, to show the robot and the lidar + * format fixes + * sm_multi_stage_1 doubling (#103) + Co-authored-by: Ubuntu 20-04-02-amd64 + * Feature/sm dance bot strikes back gazebo fixes (#105) + * sm_dance_bot visualizing turtlebot3 + * cleaning and lidar show/hide option + * cleaning files and making formatting work + * more fixes + * gazebo fixes, to show the robot and the lidar + * format fixes + * gazebo fixes for sm_dance_bot_strikes_back + * precommit cleanup run (#106) + Co-authored-by: Ubuntu 20-04-02-amd64 + * aws demo (#108) + * aws demo + * format + * got sm_multi_stage_1 working (barely) (#109) + Co-authored-by: Ubuntu 20-04-02-amd64 + * Brettpac branch (#110) + * got sm_multi_stage_1 working (barely) + * gaining traction sm_multi_stage_1 + * more + * don't remember + * making progress + * More + * keep hammering + * two stages + * 3 part + * 4th stage + Co-authored-by: Ubuntu 20-04-02-amd64 + * Brettpac branch (#111) + * got sm_multi_stage_1 working (barely) + * gaining traction sm_multi_stage_1 + * more + * don't remember + * making progress + * More + * keep hammering + * two stages + * 3 part + * 4th stage + * 5th stage + Co-authored-by: Ubuntu 20-04-02-amd64 + * a3 (#113) + Co-authored-by: Ubuntu 20-04-02-amd64 + * Remove neo_simulation2 package. (#112) + * Remove neo_simulation2 package. + * Correct formatting. + * Enable source build on PR for testing. + * Adjust build packages of source CI + * more sm_multi_stage_1 (#114) + Co-authored-by: Ubuntu 20-04-02-amd64 + * mm (#115) + Co-authored-by: Ubuntu 20-04-02-amd64 + * diverse improvements navigation and performance (#116) + * diverse improvements navigation and performance + * minor + Co-authored-by: pabloinigoblasco + * Feature/diverse improvemets navigation performance (#117) + * diverse improvements navigation and performance + * minor + * additional linting and formatting + * Remove merge markers from a python file. (#119) + * Feature/slam toggle and smacc deep history (#122) + * progress in navigation, slam toggle client behaviors and slam_toolbox components. Also smacc2::deep_history syntax + * going forward in testing sm_dance_bot introducing slam pausing/resuming funcionality + * feature/more_sm_dance_bot_fixes + * minor format + * minor (#124) + Co-authored-by: Ubuntu 20-04-02-amd64 + * more changes in sm_dance_bot (#125) + * Move method after the method it calls. Otherwise recursion could happen. (#126) + * Feature/dance bot s pattern (#128) + * more changes in sm_dance_bot + * polishing sm_dance_bot and s-pattern + * noticed typo + Finnaly > Finally + * Feature/dance bot s pattern (#129) + * more changes in sm_dance_bot + * polishing sm_dance_bot and s-pattern + * more refinement in sm_dance_bot + * First working version of sm template and template generator. (#127) + * minor tweaks (#130) + Co-authored-by: Ubuntu 20-04-02-amd64 + * Feature/sm dance bot refine (#131) + * more changes in sm_dance_bot + * minor + * Feature/sm dance bot refine 2 (#132) + * more changes in sm_dance_bot + * minor + * build fix + * waypoints navigator bug (#133) + * minor tuning to mitigate overshot issue cases + * progress in the sm_dance_bot tests (#135) + * some more progress on markers cleanup + * minor format issues (#134) + * sm_dance_bot_lite (#136) + Co-authored-by: Ubuntu 20-04-02-amd64 + * Resolve compile wanings (#137) + * Add SM core test (#138) + * minor navigation improvements (#141) + * using local action msgs (#139) + * using local action msgs + * removing sm_dance_bot_msgs + * pending references + * Feature/nitrosz renaming (#144) + * using local action msgs + * removing sm_dance_bot_msgs + * pending references + * navigation 2 stack renaming + * formatting + * added SVGs to READMEs of atomic, dance_bot, and others (#140) + * added remaining SVGs to READMEs (#145) + * added remaining SVGs to READMEs + * precommit cleanup + * Update package list. (#142) + * removing parameters smacc (#147) + * removing parameters smacc + * workflows update + * workflow + * Noticed launch command was incorrect in README.md + fixed launch command for sm_dance_bot_strikes_back and removed some comments I had made in the past. + * Fix CI: format fix python version (#148) + * Add SM Atomic SM generator. (#143) + * Remove node creation and create only a logger. (#149) + * Rolling Docker environment to be executed from any environment (#154) + * Feature/sm dance bot strikes back refactoring (#152) + Co-authored-by: DecDury + Co-authored-by: Denis Štogl + * slight waypoint 4 and iterations changes so robot can complete course (#155) + * Feature/migration moveit client (#151) + * initial migration to smacc2 + * fixing some errors introduced on formatting + * missing dependency + * fixing some more linting warnings + * minor + * removing test from main moveit cmake + * test ur5 + * progressing in the moveit migration testing + * updating format + * adding .reps dependencies and also fixing some build errors + * repos dependency + * adding dependency to ur5 client + * docker refactoring + * minor + * progress on move_it PR + * minor dockerfile test workaround + * improving dockerfile for building local tests + * minor + * fixing compiling issues + * update readme (#164) + * update readme + * more readme updates + * more + Co-authored-by: Ubuntu 20-04-02-amd64 + * initial state machine transition timestamp (#165) + * moved reference library SMs to smacc2_performance_tools (#166) + * moved reference library SMs to smacc2_performance_tools + * pre-commit cleanup + * Add QOS durability to SmaccPublisherClient (#163) + * feat: add qos durability to SmaccPublisherClient + * fix: add a missing colon + * refactor: remove line + * feat: add reliability qos config + * Feature/testing moveit behaviors (#167) + * more testing on moveit + * progress on moveit + * more testing on moveit behaviors + * minor configuration + * fixing pipeline error + * fixing broken master build + * sm_pubsub_1 (#169) + Co-authored-by: Ubuntu 20-04-02-amd64 + * sm_pubsub_1 part 2 (#170) + * sm_pubsub_1 part 2 + * sm_pubsub_1 part 2 + Co-authored-by: Ubuntu 20-04-02-amd64 + * sm_advanced_recovery_1 renaming (#171) + Co-authored-by: Ubuntu 20-04-02-amd64 + * sm_multi_stage_1 reworking (#172) + * multistage modes + * sm_multi_stage sequences + * sm_multi_state_1 steps + * sm_multi_stage_1 sequence d + * sm_multi_stage_1 c sequence + * mode_5_sequence_b + * mode_4_sequence_b + * sm_multi_stage_1 most + * finishing touches 1 + * readme + Co-authored-by: Ubuntu 20-04-02-amd64 + * Feature/aws navigation sm dance bot (#174) + * repo dependency + * husky launch file in sm_dance_bot + * Add dependencies for husky simulation. + * Fix formatting. + * Update dependencies for husky in rolling and galactic. + * minor + * progress on aws navigation and some other refactorings on navigation clients and behaviors + * more on aws demo + * fixing broken build + * minor + Co-authored-by: Denis Štogl + Co-authored-by: Denis Štogl + * minor changes (#175) + * warehouse2 (#177) + * Waypoint Inputs (#178) + Co-authored-by: Ubuntu 20-04-02-amd64 + * wharehouse2 progress (#179) + * format (#180) + * sm_dance_bot_warehouse_3 (#181) + Co-authored-by: Ubuntu 20-04-02-amd64 + * Feature/sm warehouse 2 13 dec 2 (#182) + * format + * more changes and headless + * merge + * headless and other fixes + * default values + * Brettpac branch (#184) + * sm_dance_bot_warehouse_3 + * Redoing sm_dance_bot_warehouse_3 waypoints + * More Waypoints + Co-authored-by: Ubuntu 20-04-02-amd64 + * SrConditional fixes and formatting (#168) + * fix: some formatting and templating on SrConditional + * fix: move trigger logic into headers + * fix: lint + * Feature/wharehouse2 dec 14 (#185) + * warehouse2 + * minor + * Feature/sm warehouse 2 13 dec 2 (#186) + * format + * more changes and headless + * merge + * headless and other fixes + * default values + * minor + * finetuning waypoints (#187) + Co-authored-by: Ubuntu 20-04-02-amd64 + * Feature/cb pure spinning (#188) + * format + * more changes and headless + * merge + * headless and other fixes + * default values + * minor + * Feature/cb pure spinning (#189) + * format + * more changes and headless + * merge + * headless and other fixes + * default values + * minor + * pure spinning behavior missing files + * minor changes (#190) + * Feature/planner changes 16 12 (#191) + * minor changes + * more fixes + * minor + * minor + * Feature/replanning 16 dec (#193) + * minor changes + * replanning for all our examples + * several fixes (#194) + * minor changes (#195) + * Feature/undo motion 20 12 (#196) + * minor changes + * replanning for all our examples + * improving undo motion navigation warehouse2 + * minor + * tuning warehouse3 (#197) + * Feature/undo motion 20 12 (#198) + * minor changes + * replanning for all our examples + * improving undo motion navigation warehouse2 + * minor + * undo tuning and errors + * format + * Feature/sync 21 12 (#199) + * minor changes + * replanning for all our examples + * format issues + * Feature/warehouse2 22 12 (#200) + * minor changes + * replanning for all our examples + * format issues + * finishing warehouse2 + * Feature/warehouse2 23 12 (#201) + * minor changes + * replanning for all our examples + * tuning and fixes (#202) + * Feature/minor tune (#203) + * tuning and fixes + * minor tune + * fixing warehouse 3 problems, and other core improvements (#204) + * fixing warehouse 3 problems, and other core improvements to remove dead lock, also making continuous integration green + * weird moveit not downloaded repo + * added missing file from warehouse2 (#205) + * backport to foxy + * minor format + * minor linking errors foxy + Co-authored-by: Denis Štogl + Co-authored-by: Denis Štogl + Co-authored-by: Declan Dury <44791484+DecDury@users.noreply.github.com> + Co-authored-by: DecDury + Co-authored-by: reelrbtx + Co-authored-by: brettpac + Co-authored-by: David Revay + * missing + * missing sm + * updating subscriber publisher components + * progress in autowarrrrrrrrrrrrrrrrrrrrrrrrrrrrrrrrrre machine + * refining cp subscriber cp publisher + * improvements in smacc core adding more components mostly developed for autoware demo + * autoware demo + * missing + * foxy ci + * fix + * minor broken build + * some reordering fixes + * minor + * docker files for different revisions, warnings removval and more testing on navigation + * fixing docker for foxy and galactic + * docker build files for all versions + * barrel demo + * barrel search build fix and warehouse3 + * fixing startup problems in warehouse 3 + * fixing format and minor + * minor + * progress in barrel husky + * minor + * barrel demo + * minor + * barrel search updates + * making models local + * red picuup + * multiple controllable leds plugin + * progress in husky demo + * progressing in husky demo + * improving navigation behaviors + * more merge + Co-authored-by: Denis Štogl + Co-authored-by: Denis Štogl + Co-authored-by: Declan Dury <44791484+DecDury@users.noreply.github.com> + Co-authored-by: DecDury + Co-authored-by: reelrbtx + Co-authored-by: brettpac + Co-authored-by: David Revay + Co-authored-by: pabloinigoblasco + * Feature/docker improvements march 2022 (#235) + * minor changes + * replanning for all our examples + * backport to foxy + * minor format + * minor linking errors foxy + * Foxy backport (#206) + * minor formatting fixes + * Fix trailing spaces. + * Correct codespell. + * Correct python linters warnings. + * Add galactic CI build because Navigation2 is broken in rolling. + * Add partial changes for ament_cpplint. + * Add tf2_ros as dependency to find include. + * Disable ament_cpplint. + * Disable some packages and update workflows. + * Bump ccache version. + * Ignore further packages + * Satisfy ament_lint_cmake + * Add missing licences. + * Disable cpplint and cppcheck linters. + * Correct formatters. + * branching example + * Disable disabled packages + * Update ci-build-source.yml + * Change extension + * Change extension of imports. + * Enable cppcheck + * Correct formatting of python file. + * Included necessary package and edited Threesome launch + Changed... + ros2 launch sm_three_some sm_three_some + to + ros2 launch sm_three_some sm_three_some.launch + Added: + First ensure you have the necessary package installed. + ``` + sudo apt-get install ros-rolling-ros2trace + ``` + Then run this command. + * Rename header files and correct format. + * Add workflow for checking doc build. + * Update doxygen-check-build.yml + * Create doxygen-deploy.yml + * Use manual deployment for now. + * Create workflow for testing prerelease builds + * Use docs/ as source folder for documentation + * Use docs/ as output directory. + * Rename to smacc2 and smacc2_msgs + * Correct GitHub branch reference. + * Update name of package and package.xml to pass liter. + * Execute on master update + * Reset all versions to 0.0.0 + * Ignore all packages except smacc2 and smacc2_msgs + * Update changelogs + * 0.1.0 + * Revert "Ignore all packages except smacc2 and smacc2_msgs" + This reverts commit f603166a4b3ccdfe96c64d9f9fb9d8b49fbf0e61. + * Update description table. + * Update table + * Copy initial docs + * Dockerfile w/ ROS distro as argument + use this command "sudo docker build --build-arg ROS2_DISTRO=(desiredRosTag) (directoryHoldingDockerfile)/" + * Opened new folder for additional tracing contents + * Delete tracing directory + * Moved tracing.md to tracing directory + * added setupTracing.sh + Installs necessary packages and configures tracing group. + * Removed manual installation of ros-rolling-ros2trace + This is now automated in setupTracing.sh + location of sh file assumed if user follows README.md under "Getting started" + * Created alternative ManualTracing + * added new sm markdowns + * added a dockerfile for Rolling and Galactic + * Update smacc2_ci/docker/ros_rollingAndGalactic_ubuntu_20.04/buildGalactic.sh + Co-authored-by: Denis Štogl + * Update tracing/ManualTracing.md + Co-authored-by: Denis Štogl + * changed wording "smacc application" to "SMACC2 library" + Co-authored-by: Denis Štogl + * Update smacc_sm_reference_library/sm_atomic/README.md + edit from html to markdown syntax + Co-authored-by: Denis Štogl + * reactivating smacc2 nav clients for rolling via submodules + * renamed tracing events after + * bug in smacc2 component + * reverted markdowns to html + * added README tutorial for Dockerfile + * additional cleanup + * cleanup + * cleanup + * edited tracing.md to reflect new tracing event names + * Enable build of missing rolling repositories. + * Enable Navigation2 for semi-binary build. + * Remove galactic builds from master and kepp only rolling. Remove submodules and use .repos file + * updated mentions of SMACC/ROS to SMACC2/ROS2 + * some progress on navigation rolling + * renamed folders, deleted tracing.md, edited README.md + * added smacc2_performance_tools + * performance tests improvements + * more on performance and other issues + * sm_respira_1 format cleanup + * sm_respira_1 format cleanup pre-commit + * sm_respira_test_2 + * sm_respira_test_2 + * more changes on performance tests + * Do not execute clang-format on smacc2_sm_reference_library package. + * sm_reference_library reformatting + * Correct trailing spaces. + * sm_atomic_24hr + * sm_atomic_performance_trace_1 + * Update smacc2_rta command across readmes + * Clean up of sm_atomic_24hr + * more sm_atomic_24hr cleanup + * Optimized deps in move_base_z_planners_common. + * Renaming of event generator library + * minor formatting + * Add galactic CI setup and rename rolling files. (#58) + * Fix source CI and correct README overview. (#62) + * Update c_cpp_properties.json + * changed launch command to ros2 launch sm_respira_1 sm_respira_1.launch (#69) + also noticed a note I had made while producing these that was not removed + * update doxygen links (#70) + Co-authored-by: Ubuntu 20-04-02-amd64 + * More Readme Updates (#72) + Co-authored-by: Ubuntu 20-04-02-amd64 + * More Readme (#74) + Co-authored-by: Ubuntu 20-04-02-amd64 + * created new sm from sm_respira_1 (#76) + * Feature/core and navigation fixes (#78) + * base for the sm_aws_aarehouse navigation + * progressing in aws navigation + * minor + * several core improvements during navigation testing + * formatting improvements + * progress in aws navigation demo + * format improvements + * Feature/aws demo progress (#80) + * base for the sm_aws_aarehouse navigation + * progressing in aws navigation + * minor + * several core improvements during navigation testing + * formatting improvements + * progress in aws navigation demo + * format improvements + * format improvements + * more on navigation + * sm_advanced_recovery_1 reworked (#83) + * sm_advanced_recovery_1 reworked + * fix pre-commit + * Trying to fix Pre-Commit + Co-authored-by: Ubuntu 20-04-02-amd64 + * more sm_advanced_recovery_1 (#84) + Co-authored-by: Ubuntu 20-04-02-amd64 + * More sm_advanced_recovery_1 work (#85) + Co-authored-by: Ubuntu 20-04-02-amd64 + * sm_advanced_recovery_1 round 4 (#86) + Co-authored-by: Ubuntu 20-04-02-amd64 + * Brettpac branch (#87) + * sm_atomic_performance_test_a_2 + * sm_atomic_performance_test_a_1 + Co-authored-by: Ubuntu 20-04-02-amd64 + * sm_atomic_performance_test_c_1 (#88) + Co-authored-by: Ubuntu 20-04-02-amd64 + * modifying sm_atomic_performance_test_a_2 (#89) + Co-authored-by: Ubuntu 20-04-02-amd64 + * sm_multi_stage_1 (#90) + * sm_multi_stage_1 + * fixing precommit + Co-authored-by: Ubuntu 20-04-02-amd64 + * more sm_multi_stage_1 (#91) + Co-authored-by: Ubuntu 20-04-02-amd64 + * Update README.md + updated launch command + * Wait topic message client behavior (#81) + * base for the sm_aws_aarehouse navigation + * progressing in aws navigation + * minor + * several core improvements during navigation testing + * formatting improvements + * progress in aws navigation demo + * format improvements + * format improvements + * more on navigation + * new feature, cb_wait_topic_message: asynchronous client behavior that waits a topic message and optionally checks its contents to success + * formatting + * attempting precommit fixes + Co-authored-by: Ubuntu 20-04-02-amd64 + Co-authored-by: Denis Štogl + * Feature/wait nav2 nodes client behavior (#82) + * base for the sm_aws_aarehouse navigation + * progressing in aws navigation + * minor + * several core improvements during navigation testing + * formatting improvements + * progress in aws navigation demo + * format improvements + * format improvements + * more on navigation + * new feature, cb_wait_topic_message: asynchronous client behavior that waits a topic message and optionally checks its contents to success + * formatting + * adding new client behavior add for nav2, wait nav2 nodes subscribing to the /bond topic and waiting they are alive. you optionally can select the nodes to wait + * Correct all linters and formaters. + Co-authored-by: Denis Štogl + Co-authored-by: Denis Štogl + * Feature/aws demo progress (#92) + * base for the sm_aws_aarehouse navigation + * progressing in aws navigation + * minor + * several core improvements during navigation testing + * formatting improvements + * progress in aws navigation demo + * format improvements + * format improvements + * more on navigation + * new feature, cb_wait_topic_message: asynchronous client behavior that waits a topic message and optionally checks its contents to success + * formatting + * adding new client behavior add for nav2, wait nav2 nodes subscribing to the /bond topic and waiting they are alive. you optionally can select the nodes to wait + * progress in aws navigation demo + * minor format + * Feature/sm dance bot fixes (#93) + * base for the sm_aws_aarehouse navigation + * progressing in aws navigation + * minor + * several core improvements during navigation testing + * formatting improvements + * progress in aws navigation demo + * format improvements + * format improvements + * more on navigation + * new feature, cb_wait_topic_message: asynchronous client behavior that waits a topic message and optionally checks its contents to success + * formatting + * adding new client behavior add for nav2, wait nav2 nodes subscribing to the /bond topic and waiting they are alive. you optionally can select the nodes to wait + * progress in aws navigation demo + * minor format + * navigation parameters fixes on sm_dance_bot + * minor format + * Feature/sm aws warehouse (#94) + * base for the sm_aws_aarehouse navigation + * progressing in aws navigation + * minor + * several core improvements during navigation testing + * formatting improvements + * progress in aws navigation demo + * format improvements + * format improvements + * more on navigation + * new feature, cb_wait_topic_message: asynchronous client behavior that waits a topic message and optionally checks its contents to success + * formatting + * adding new client behavior add for nav2, wait nav2 nodes subscribing to the /bond topic and waiting they are alive. you optionally can select the nodes to wait + * progress in aws navigation demo + * minor format + * merge and progress + * fix format + * Feature/sm dance bot fixes (#95) + * base for the sm_aws_aarehouse navigation + * progressing in aws navigation + * minor + * several core improvements during navigation testing + * formatting improvements + * progress in aws navigation demo + * format improvements + * format improvements + * more on navigation + * new feature, cb_wait_topic_message: asynchronous client behavior that waits a topic message and optionally checks its contents to success + * formatting + * adding new client behavior add for nav2, wait nav2 nodes subscribing to the /bond topic and waiting they are alive. you optionally can select the nodes to wait + * progress in aws navigation demo + * minor format + * navigation parameters fixes on sm_dance_bot + * minor format + * minor + * minor format + * Remove some compile warnings. (#96) + * Feature/cb pause slam (#98) + * base for the sm_aws_aarehouse navigation + * progressing in aws navigation + * minor + * several core improvements during navigation testing + * formatting improvements + * progress in aws navigation demo + * format improvements + * format improvements + * more on navigation + * new feature, cb_wait_topic_message: asynchronous client behavior that waits a topic message and optionally checks its contents to success + * formatting + * adding new client behavior add for nav2, wait nav2 nodes subscribing to the /bond topic and waiting they are alive. you optionally can select the nodes to wait + * progress in aws navigation demo + * minor format + * navigation parameters fixes on sm_dance_bot + * minor format + * minor + * formatting + * cb pause slam client behavior + * sm_dance_bot_lite (#99) + * sm_dance_bot_lite + * precommit + * Updates yaml + Co-authored-by: Ubuntu 20-04-02-amd64 + * Rename doxygen deployment workflow (#100) + * minor hotfix + * sm_dance_bot visualizing turtlebot3 (#101) + * Feature/dance bot launch gz lidar choice (#102) + * sm_dance_bot visualizing turtlebot3 + * cleaning and lidar show/hide option + * cleaning files and making formatting work + * more fixes + * Feature/sm dance bot lite gazebo fixes (#104) + * sm_dance_bot visualizing turtlebot3 + * cleaning and lidar show/hide option + * cleaning files and making formatting work + * more fixes + * gazebo fixes, to show the robot and the lidar + * format fixes + * sm_multi_stage_1 doubling (#103) + Co-authored-by: Ubuntu 20-04-02-amd64 + * Feature/sm dance bot strikes back gazebo fixes (#105) + * sm_dance_bot visualizing turtlebot3 + * cleaning and lidar show/hide option + * cleaning files and making formatting work + * more fixes + * gazebo fixes, to show the robot and the lidar + * format fixes + * gazebo fixes for sm_dance_bot_strikes_back + * precommit cleanup run (#106) + Co-authored-by: Ubuntu 20-04-02-amd64 + * aws demo (#108) + * aws demo + * format + * got sm_multi_stage_1 working (barely) (#109) + Co-authored-by: Ubuntu 20-04-02-amd64 + * Brettpac branch (#110) + * got sm_multi_stage_1 working (barely) + * gaining traction sm_multi_stage_1 + * more + * don't remember + * making progress + * More + * keep hammering + * two stages + * 3 part + * 4th stage + Co-authored-by: Ubuntu 20-04-02-amd64 + * Brettpac branch (#111) + * got sm_multi_stage_1 working (barely) + * gaining traction sm_multi_stage_1 + * more + * don't remember + * making progress + * More + * keep hammering + * two stages + * 3 part + * 4th stage + * 5th stage + Co-authored-by: Ubuntu 20-04-02-amd64 + * a3 (#113) + Co-authored-by: Ubuntu 20-04-02-amd64 + * Remove neo_simulation2 package. (#112) + * Remove neo_simulation2 package. + * Correct formatting. + * Enable source build on PR for testing. + * Adjust build packages of source CI + * more sm_multi_stage_1 (#114) + Co-authored-by: Ubuntu 20-04-02-amd64 + * mm (#115) + Co-authored-by: Ubuntu 20-04-02-amd64 + * diverse improvements navigation and performance (#116) + * diverse improvements navigation and performance + * minor + Co-authored-by: pabloinigoblasco + * Feature/diverse improvemets navigation performance (#117) + * diverse improvements navigation and performance + * minor + * additional linting and formatting + * Remove merge markers from a python file. (#119) + * Feature/slam toggle and smacc deep history (#122) + * progress in navigation, slam toggle client behaviors and slam_toolbox components. Also smacc2::deep_history syntax + * going forward in testing sm_dance_bot introducing slam pausing/resuming funcionality + * feature/more_sm_dance_bot_fixes + * minor format + * minor (#124) + Co-authored-by: Ubuntu 20-04-02-amd64 + * more changes in sm_dance_bot (#125) + * Move method after the method it calls. Otherwise recursion could happen. (#126) + * Feature/dance bot s pattern (#128) + * more changes in sm_dance_bot + * polishing sm_dance_bot and s-pattern + * noticed typo + Finnaly > Finally + * Feature/dance bot s pattern (#129) + * more changes in sm_dance_bot + * polishing sm_dance_bot and s-pattern + * more refinement in sm_dance_bot + * First working version of sm template and template generator. (#127) + * minor tweaks (#130) + Co-authored-by: Ubuntu 20-04-02-amd64 + * Feature/sm dance bot refine (#131) + * more changes in sm_dance_bot + * minor + * Feature/sm dance bot refine 2 (#132) + * more changes in sm_dance_bot + * minor + * build fix + * waypoints navigator bug (#133) + * minor tuning to mitigate overshot issue cases + * progress in the sm_dance_bot tests (#135) + * some more progress on markers cleanup + * minor format issues (#134) + * sm_dance_bot_lite (#136) + Co-authored-by: Ubuntu 20-04-02-amd64 + * Resolve compile wanings (#137) + * Add SM core test (#138) + * minor navigation improvements (#141) + * using local action msgs (#139) + * using local action msgs + * removing sm_dance_bot_msgs + * pending references + * Feature/nitrosz renaming (#144) + * using local action msgs + * removing sm_dance_bot_msgs + * pending references + * navigation 2 stack renaming + * formatting + * added SVGs to READMEs of atomic, dance_bot, and others (#140) + * added remaining SVGs to READMEs (#145) + * added remaining SVGs to READMEs + * precommit cleanup + * Update package list. (#142) + * removing parameters smacc (#147) + * removing parameters smacc + * workflows update + * workflow + * Noticed launch command was incorrect in README.md + fixed launch command for sm_dance_bot_strikes_back and removed some comments I had made in the past. + * Fix CI: format fix python version (#148) + * Add SM Atomic SM generator. (#143) + * Remove node creation and create only a logger. (#149) + * Rolling Docker environment to be executed from any environment (#154) + * Feature/sm dance bot strikes back refactoring (#152) + Co-authored-by: DecDury + Co-authored-by: Denis Štogl + * slight waypoint 4 and iterations changes so robot can complete course (#155) + * Feature/migration moveit client (#151) + * initial migration to smacc2 + * fixing some errors introduced on formatting + * missing dependency + * fixing some more linting warnings + * minor + * removing test from main moveit cmake + * test ur5 + * progressing in the moveit migration testing + * updating format + * adding .reps dependencies and also fixing some build errors + * repos dependency + * adding dependency to ur5 client + * docker refactoring + * minor + * progress on move_it PR + * minor dockerfile test workaround + * improving dockerfile for building local tests + * minor + * fixing compiling issues + * update readme (#164) + * update readme + * more readme updates + * more + Co-authored-by: Ubuntu 20-04-02-amd64 + * initial state machine transition timestamp (#165) + * moved reference library SMs to smacc2_performance_tools (#166) + * moved reference library SMs to smacc2_performance_tools + * pre-commit cleanup + * Add QOS durability to SmaccPublisherClient (#163) + * feat: add qos durability to SmaccPublisherClient + * fix: add a missing colon + * refactor: remove line + * feat: add reliability qos config + * Feature/testing moveit behaviors (#167) + * more testing on moveit + * progress on moveit + * more testing on moveit behaviors + * minor configuration + * fixing pipeline error + * fixing broken master build + * sm_pubsub_1 (#169) + Co-authored-by: Ubuntu 20-04-02-amd64 + * sm_pubsub_1 part 2 (#170) + * sm_pubsub_1 part 2 + * sm_pubsub_1 part 2 + Co-authored-by: Ubuntu 20-04-02-amd64 + * sm_advanced_recovery_1 renaming (#171) + Co-authored-by: Ubuntu 20-04-02-amd64 + * sm_multi_stage_1 reworking (#172) + * multistage modes + * sm_multi_stage sequences + * sm_multi_state_1 steps + * sm_multi_stage_1 sequence d + * sm_multi_stage_1 c sequence + * mode_5_sequence_b + * mode_4_sequence_b + * sm_multi_stage_1 most + * finishing touches 1 + * readme + Co-authored-by: Ubuntu 20-04-02-amd64 + * Feature/aws navigation sm dance bot (#174) + * repo dependency + * husky launch file in sm_dance_bot + * Add dependencies for husky simulation. + * Fix formatting. + * Update dependencies for husky in rolling and galactic. + * minor + * progress on aws navigation and some other refactorings on navigation clients and behaviors + * more on aws demo + * fixing broken build + * minor + Co-authored-by: Denis Štogl + Co-authored-by: Denis Štogl + * minor changes (#175) + * warehouse2 (#177) + * Waypoint Inputs (#178) + Co-authored-by: Ubuntu 20-04-02-amd64 + * wharehouse2 progress (#179) + * format (#180) + * sm_dance_bot_warehouse_3 (#181) + Co-authored-by: Ubuntu 20-04-02-amd64 + * Feature/sm warehouse 2 13 dec 2 (#182) + * format + * more changes and headless + * merge + * headless and other fixes + * default values + * Brettpac branch (#184) + * sm_dance_bot_warehouse_3 + * Redoing sm_dance_bot_warehouse_3 waypoints + * More Waypoints + Co-authored-by: Ubuntu 20-04-02-amd64 + * SrConditional fixes and formatting (#168) + * fix: some formatting and templating on SrConditional + * fix: move trigger logic into headers + * fix: lint + * Feature/wharehouse2 dec 14 (#185) + * warehouse2 + * minor + * Feature/sm warehouse 2 13 dec 2 (#186) + * format + * more changes and headless + * merge + * headless and other fixes + * default values + * minor + * finetuning waypoints (#187) + Co-authored-by: Ubuntu 20-04-02-amd64 + * Feature/cb pure spinning (#188) + * format + * more changes and headless + * merge + * headless and other fixes + * default values + * minor + * Feature/cb pure spinning (#189) + * format + * more changes and headless + * merge + * headless and other fixes + * default values + * minor + * pure spinning behavior missing files + * minor changes (#190) + * Feature/planner changes 16 12 (#191) + * minor changes + * more fixes + * minor + * minor + * Feature/replanning 16 dec (#193) + * minor changes + * replanning for all our examples + * several fixes (#194) + * minor changes (#195) + * Feature/undo motion 20 12 (#196) + * minor changes + * replanning for all our examples + * improving undo motion navigation warehouse2 + * minor + * tuning warehouse3 (#197) + * Feature/undo motion 20 12 (#198) + * minor changes + * replanning for all our examples + * improving undo motion navigation warehouse2 + * minor + * undo tuning and errors + * format + * Feature/sync 21 12 (#199) + * minor changes + * replanning for all our examples + * format issues + * Feature/warehouse2 22 12 (#200) + * minor changes + * replanning for all our examples + * format issues + * finishing warehouse2 + * Feature/warehouse2 23 12 (#201) + * minor changes + * replanning for all our examples + * tuning and fixes (#202) + * Feature/minor tune (#203) + * tuning and fixes + * minor tune + * fixing warehouse 3 problems, and other core improvements (#204) + * fixing warehouse 3 problems, and other core improvements to remove dead lock, also making continuous integration green + * weird moveit not downloaded repo + * added missing file from warehouse2 (#205) + * backport to foxy + * minor format + * minor linking errors foxy + Co-authored-by: Denis Štogl + Co-authored-by: Denis Štogl + Co-authored-by: Declan Dury <44791484+DecDury@users.noreply.github.com> + Co-authored-by: DecDury + Co-authored-by: reelrbtx + Co-authored-by: brettpac + Co-authored-by: David Revay + * missing + * missing sm + * updating subscriber publisher components + * progress in autowarrrrrrrrrrrrrrrrrrrrrrrrrrrrrrrrrre machine + * refining cp subscriber cp publisher + * improvements in smacc core adding more components mostly developed for autoware demo + * autoware demo + * missing + * foxy ci + * fix + * minor broken build + * some reordering fixes + * minor + * docker files for different revisions, warnings removval and more testing on navigation + * fixing docker for foxy and galactic + * docker build files for all versions + * barrel demo + * barrel search build fix and warehouse3 + * fixing startup problems in warehouse 3 + * fixing format and minor + * minor + * progress in barrel husky + * minor + * barrel demo + * minor + * barrel search updates + * making models local + * red picuup + * multiple controllable leds plugin + * progress in husky demo + * progressing in husky demo + * improving navigation behaviors + * more merge + * docker improvements + Co-authored-by: Denis Štogl + Co-authored-by: Denis Štogl + Co-authored-by: Declan Dury <44791484+DecDury@users.noreply.github.com> + Co-authored-by: DecDury + Co-authored-by: reelrbtx + Co-authored-by: brettpac + Co-authored-by: David Revay + Co-authored-by: pabloinigoblasco + * Use correct upstream .repos files for source builds (#243) + * Correct mergify branch names (#246) + * Correct name of source-build job and bump version of action (#242) (#247) + Co-authored-by: Denis Štogl + Co-authored-by: Pablo Iñigo Blasco + * Update galactic source build job name (#250) + * Galactic source build: update .repos file, bump action version and use correct version of upstream packages (backport #241) (#248) + Co-authored-by: Denis Štogl + * fixing rolling build (#239) + * fixing rolling build + * trying to fix dependencies + * missing repo + * fixing to focal by the moment + * more fixing rolling build + * minor + * cache matrix rolling and source build package + * minor + * minor + * missing repo + * missing deps + * fixing building issue + * typo + * fixing broken build + * build fix + * restoring workflow files (#252) + * restoring files (#253) + * Fix checkout branches for scheduled builds (#254) + * correct checkout branch on scheduled build + * Update foxy-source-build.yml + * Feature/fixing husky build rolling (#257) + * restoring files + * making husky project build on rolling + * Feature/fixing husky build rolling (#258) + * restoring files + * making husky project build on rolling + * husky progress + * Update README.md (#262) + * Feature/fixing ur demos (#261) + * restoring files + * fixes + * Feature/fixing type string walker (#263) + * restoring files + * fixing type string walker threesome demo + * Update README.md (#266) + * Update README.md (#267) + * Update README.md (#268) + * Significant update in Getting Started Instructions (#269) + * Significant update in Getting Started Instructions + * Remove trailing spaces. + Co-authored-by: Denis Štogl + * fixing ur demo (#273) + * fix: initialise conditionFlag as false (#274) + * precommit fix (#280) + I merge this in red for focal-rolling because it was already broken anyway and it is only a minor update of the precommit + * progress on the sm_husky_barrel + * missing code + * more on husky demo for galactic + Co-authored-by: Denis Štogl + Co-authored-by: Denis Štogl + Co-authored-by: Declan Dury <44791484+DecDury@users.noreply.github.com> + Co-authored-by: DecDury + Co-authored-by: reelrbtx + Co-authored-by: brettpac + Co-authored-by: David Revay + Co-authored-by: pabloinigoblasco + Co-authored-by: mergify[bot] <37929162+mergify[bot]@users.noreply.github.com> + Co-authored-by: brettpac +* Revert "Ignore packages which should not be released." + This reverts commit dec14a936a877b2ef722a6a32f1bf3df09312542. +* Contributors: Denis Štogl, Pablo Iñigo Blasco, pabloinigoblasco + +0.3.0 (2022-04-04) +------------------ + +0.0.0 (2022-11-09) +------------------ +* more progress in humble SMACC2 deb generation +* humble check +* publisher +* progress in migration to humble +* Feature/fix mutex galactic (#319) + * bug fix galactic mutex + * testing undo motion and also improving action client + * important refactoring smacc action client + * fix + * progress in smacc action client + * progress in the smacc action client fork based on signals + * more changes + * more testing + * more testing + * more testing on abort + * minor + * adding smaccServiceerver client to galactic + * Update cb_default_keyboard_behavior.hpp + * testing more husky robot + * progress in tests husky demo + * testing abort forward and undo + * finishing cancel and undo behavior tests +* Undo motion in stEvasion after detecting enemy - in testing (#315) + * Undo motion in stEvasion after detecting enemy - in testing + * minor format + * putting in green SMACC2 +* Feature/husky barrel improvements (#314) + * improvements in navigation client behaviors and husky barrel demo + * many improvements in action client and cb sequence for hysky barrel search + * more and better navigation behaviors on husky barrel search demo + * functionality improvements in navigation and improvements of warehouse 3, format + * functionality improvements in navigation and improvements of warehouse 3 and husky + * format + * warehouse 3 improvements + * merge galactic + * merge fix + * minor + * dead branch for husky barrel sm and opencv funcionalities + * final s-pattern and final attack + * format and other + * refining final attack state and also retry states for s-pattern + * fixing PR green + * minor changes +* improvements in navigation client behaviors and husky barrel demo (#311) + * improvements in navigation client behaviors and husky barrel demo + * many improvements in action client and cb sequence for hysky barrel search + * more and better navigation behaviors on husky barrel search demo + * functionality improvements in navigation and improvements of warehouse 3, format + * functionality improvements in navigation and improvements of warehouse 3 and husky + * format +* husky_improvements (#299) + * husky_improvements + * different planners profiles for navigation + * getting changes from galactic + * planner switcher + * using galactic branch files + * fixing breaking changes + * minor fix + * removing nav from source files + * merge +* Feature/barrel husky improvements (#293) + * Rename to smacc2 and smacc2_msgs + * Correct GitHub branch reference. + * Update name of package and package.xml to pass liter. + * Execute on master update + * Reset all versions to 0.0.0 + * Ignore all packages except smacc2 and smacc2_msgs + * Update changelogs + * 0.1.0 + * Revert "Ignore all packages except smacc2 and smacc2_msgs" + This reverts commit f603166a4b3ccdfe96c64d9f9fb9d8b49fbf0e61. + * Update description table. + * Update table + * Copy initial docs + * Dockerfile w/ ROS distro as argument + use this command "sudo docker build --build-arg ROS2_DISTRO=(desiredRosTag) (directoryHoldingDockerfile)/" + * Opened new folder for additional tracing contents + * Delete tracing directory + * Moved tracing.md to tracing directory + * added setupTracing.sh + Installs necessary packages and configures tracing group. + * Removed manual installation of ros-rolling-ros2trace + This is now automated in setupTracing.sh + location of sh file assumed if user follows README.md under "Getting started" + * Created alternative ManualTracing + * added new sm markdowns + * added a dockerfile for Rolling and Galactic + * Update smacc2_ci/docker/ros_rollingAndGalactic_ubuntu_20.04/buildGalactic.sh + Co-authored-by: Denis Štogl + * Update tracing/ManualTracing.md + Co-authored-by: Denis Štogl + * changed wording "smacc application" to "SMACC2 library" + Co-authored-by: Denis Štogl + * Update smacc_sm_reference_library/sm_atomic/README.md + edit from html to markdown syntax + Co-authored-by: Denis Štogl + * reactivating smacc2 nav clients for rolling via submodules + * renamed tracing events after + * bug in smacc2 component + * reverted markdowns to html + * added README tutorial for Dockerfile + * additional cleanup + * cleanup + * cleanup + * edited tracing.md to reflect new tracing event names + * Enable build of missing rolling repositories. + * Enable Navigation2 for semi-binary build. + * Remove galactic builds from master and kepp only rolling. Remove submodules and use .repos file + * updated mentions of SMACC/ROS to SMACC2/ROS2 + * some progress on navigation rolling + * renamed folders, deleted tracing.md, edited README.md + * added smacc2_performance_tools + * performance tests improvements + * more on performance and other issues + * sm_respira_1 format cleanup + * sm_respira_1 format cleanup pre-commit + * sm_respira_test_2 + * sm_respira_test_2 + * more changes on performance tests + * Do not execute clang-format on smacc2_sm_reference_library package. + * sm_reference_library reformatting + * Correct trailing spaces. + * sm_atomic_24hr + * sm_atomic_performance_trace_1 + * Update smacc2_rta command across readmes + * Clean up of sm_atomic_24hr + * more sm_atomic_24hr cleanup + * Optimized deps in move_base_z_planners_common. + * Renaming of event generator library + * minor formatting + * Add galactic CI setup and rename rolling files. (#58) + * Fix source CI and correct README overview. (#62) + * Update c_cpp_properties.json + * changed launch command to ros2 launch sm_respira_1 sm_respira_1.launch (#69) + also noticed a note I had made while producing these that was not removed + * update doxygen links (#70) + Co-authored-by: Ubuntu 20-04-02-amd64 + * More Readme Updates (#72) + Co-authored-by: Ubuntu 20-04-02-amd64 + * More Readme (#74) + Co-authored-by: Ubuntu 20-04-02-amd64 + * created new sm from sm_respira_1 (#76) + * Feature/core and navigation fixes (#78) + * base for the sm_aws_aarehouse navigation + * progressing in aws navigation + * minor + * several core improvements during navigation testing + * formatting improvements + * progress in aws navigation demo + * format improvements + * Feature/aws demo progress (#80) + * base for the sm_aws_aarehouse navigation + * progressing in aws navigation + * minor + * several core improvements during navigation testing + * formatting improvements + * progress in aws navigation demo + * format improvements + * format improvements + * more on navigation + * sm_advanced_recovery_1 reworked (#83) + * sm_advanced_recovery_1 reworked + * fix pre-commit + * Trying to fix Pre-Commit + Co-authored-by: Ubuntu 20-04-02-amd64 + * more sm_advanced_recovery_1 (#84) + Co-authored-by: Ubuntu 20-04-02-amd64 + * More sm_advanced_recovery_1 work (#85) + Co-authored-by: Ubuntu 20-04-02-amd64 + * sm_advanced_recovery_1 round 4 (#86) + Co-authored-by: Ubuntu 20-04-02-amd64 + * Brettpac branch (#87) + * sm_atomic_performance_test_a_2 + * sm_atomic_performance_test_a_1 + Co-authored-by: Ubuntu 20-04-02-amd64 + * sm_atomic_performance_test_c_1 (#88) + Co-authored-by: Ubuntu 20-04-02-amd64 + * modifying sm_atomic_performance_test_a_2 (#89) + Co-authored-by: Ubuntu 20-04-02-amd64 + * sm_multi_stage_1 (#90) + * sm_multi_stage_1 + * fixing precommit + Co-authored-by: Ubuntu 20-04-02-amd64 + * more sm_multi_stage_1 (#91) + Co-authored-by: Ubuntu 20-04-02-amd64 + * Update README.md + updated launch command + * Wait topic message client behavior (#81) + * base for the sm_aws_aarehouse navigation + * progressing in aws navigation + * minor + * several core improvements during navigation testing + * formatting improvements + * progress in aws navigation demo + * format improvements + * format improvements + * more on navigation + * new feature, cb_wait_topic_message: asynchronous client behavior that waits a topic message and optionally checks its contents to success + * formatting + * attempting precommit fixes + Co-authored-by: Ubuntu 20-04-02-amd64 + Co-authored-by: Denis Štogl + * Feature/wait nav2 nodes client behavior (#82) + * base for the sm_aws_aarehouse navigation + * progressing in aws navigation + * minor + * several core improvements during navigation testing + * formatting improvements + * progress in aws navigation demo + * format improvements + * format improvements + * more on navigation + * new feature, cb_wait_topic_message: asynchronous client behavior that waits a topic message and optionally checks its contents to success + * formatting + * adding new client behavior add for nav2, wait nav2 nodes subscribing to the /bond topic and waiting they are alive. you optionally can select the nodes to wait + * Correct all linters and formaters. + Co-authored-by: Denis Štogl + Co-authored-by: Denis Štogl + * Feature/aws demo progress (#92) + * base for the sm_aws_aarehouse navigation + * progressing in aws navigation + * minor + * several core improvements during navigation testing + * formatting improvements + * progress in aws navigation demo + * format improvements + * format improvements + * more on navigation + * new feature, cb_wait_topic_message: asynchronous client behavior that waits a topic message and optionally checks its contents to success + * formatting + * adding new client behavior add for nav2, wait nav2 nodes subscribing to the /bond topic and waiting they are alive. you optionally can select the nodes to wait + * progress in aws navigation demo + * minor format + * Feature/sm dance bot fixes (#93) + * base for the sm_aws_aarehouse navigation + * progressing in aws navigation + * minor + * several core improvements during navigation testing + * formatting improvements + * progress in aws navigation demo + * format improvements + * format improvements + * more on navigation + * new feature, cb_wait_topic_message: asynchronous client behavior that waits a topic message and optionally checks its contents to success + * formatting + * adding new client behavior add for nav2, wait nav2 nodes subscribing to the /bond topic and waiting they are alive. you optionally can select the nodes to wait + * progress in aws navigation demo + * minor format + * navigation parameters fixes on sm_dance_bot + * minor format + * Feature/sm aws warehouse (#94) + * base for the sm_aws_aarehouse navigation + * progressing in aws navigation + * minor + * several core improvements during navigation testing + * formatting improvements + * progress in aws navigation demo + * format improvements + * format improvements + * more on navigation + * new feature, cb_wait_topic_message: asynchronous client behavior that waits a topic message and optionally checks its contents to success + * formatting + * adding new client behavior add for nav2, wait nav2 nodes subscribing to the /bond topic and waiting they are alive. you optionally can select the nodes to wait + * progress in aws navigation demo + * minor format + * merge and progress + * fix format + * Feature/sm dance bot fixes (#95) + * base for the sm_aws_aarehouse navigation + * progressing in aws navigation + * minor + * several core improvements during navigation testing + * formatting improvements + * progress in aws navigation demo + * format improvements + * format improvements + * more on navigation + * new feature, cb_wait_topic_message: asynchronous client behavior that waits a topic message and optionally checks its contents to success + * formatting + * adding new client behavior add for nav2, wait nav2 nodes subscribing to the /bond topic and waiting they are alive. you optionally can select the nodes to wait + * progress in aws navigation demo + * minor format + * navigation parameters fixes on sm_dance_bot + * minor format + * minor + * minor format + * Remove some compile warnings. (#96) + * Feature/cb pause slam (#98) + * base for the sm_aws_aarehouse navigation + * progressing in aws navigation + * minor + * several core improvements during navigation testing + * formatting improvements + * progress in aws navigation demo + * format improvements + * format improvements + * more on navigation + * new feature, cb_wait_topic_message: asynchronous client behavior that waits a topic message and optionally checks its contents to success + * formatting + * adding new client behavior add for nav2, wait nav2 nodes subscribing to the /bond topic and waiting they are alive. you optionally can select the nodes to wait + * progress in aws navigation demo + * minor format + * navigation parameters fixes on sm_dance_bot + * minor format + * minor + * formatting + * cb pause slam client behavior + * sm_dance_bot_lite (#99) + * sm_dance_bot_lite + * precommit + * Updates yaml + Co-authored-by: Ubuntu 20-04-02-amd64 + * Rename doxygen deployment workflow (#100) + * minor hotfix + * sm_dance_bot visualizing turtlebot3 (#101) + * Feature/dance bot launch gz lidar choice (#102) + * sm_dance_bot visualizing turtlebot3 + * cleaning and lidar show/hide option + * cleaning files and making formatting work + * more fixes + * Feature/sm dance bot lite gazebo fixes (#104) + * sm_dance_bot visualizing turtlebot3 + * cleaning and lidar show/hide option + * cleaning files and making formatting work + * more fixes + * gazebo fixes, to show the robot and the lidar + * format fixes + * sm_multi_stage_1 doubling (#103) + Co-authored-by: Ubuntu 20-04-02-amd64 + * Feature/sm dance bot strikes back gazebo fixes (#105) + * sm_dance_bot visualizing turtlebot3 + * cleaning and lidar show/hide option + * cleaning files and making formatting work + * more fixes + * gazebo fixes, to show the robot and the lidar + * format fixes + * gazebo fixes for sm_dance_bot_strikes_back + * precommit cleanup run (#106) + Co-authored-by: Ubuntu 20-04-02-amd64 + * aws demo (#108) + * aws demo + * format + * got sm_multi_stage_1 working (barely) (#109) + Co-authored-by: Ubuntu 20-04-02-amd64 + * Brettpac branch (#110) + * got sm_multi_stage_1 working (barely) + * gaining traction sm_multi_stage_1 + * more + * don't remember + * making progress + * More + * keep hammering + * two stages + * 3 part + * 4th stage + Co-authored-by: Ubuntu 20-04-02-amd64 + * Brettpac branch (#111) + * got sm_multi_stage_1 working (barely) + * gaining traction sm_multi_stage_1 + * more + * don't remember + * making progress + * More + * keep hammering + * two stages + * 3 part + * 4th stage + * 5th stage + Co-authored-by: Ubuntu 20-04-02-amd64 + * a3 (#113) + Co-authored-by: Ubuntu 20-04-02-amd64 + * Remove neo_simulation2 package. (#112) + * Remove neo_simulation2 package. + * Correct formatting. + * Enable source build on PR for testing. + * Adjust build packages of source CI + * more sm_multi_stage_1 (#114) + Co-authored-by: Ubuntu 20-04-02-amd64 + * mm (#115) + Co-authored-by: Ubuntu 20-04-02-amd64 + * diverse improvements navigation and performance (#116) + * diverse improvements navigation and performance + * minor + Co-authored-by: pabloinigoblasco + * Feature/diverse improvemets navigation performance (#117) + * diverse improvements navigation and performance + * minor + * additional linting and formatting + * Remove merge markers from a python file. (#119) + * Feature/slam toggle and smacc deep history (#122) + * progress in navigation, slam toggle client behaviors and slam_toolbox components. Also smacc2::deep_history syntax + * going forward in testing sm_dance_bot introducing slam pausing/resuming funcionality + * feature/more_sm_dance_bot_fixes + * minor format + * minor (#124) + Co-authored-by: Ubuntu 20-04-02-amd64 + * more changes in sm_dance_bot (#125) + * Move method after the method it calls. Otherwise recursion could happen. (#126) + * Feature/dance bot s pattern (#128) + * more changes in sm_dance_bot + * polishing sm_dance_bot and s-pattern + * noticed typo + Finnaly > Finally + * Feature/dance bot s pattern (#129) + * more changes in sm_dance_bot + * polishing sm_dance_bot and s-pattern + * more refinement in sm_dance_bot + * First working version of sm template and template generator. (#127) + * minor tweaks (#130) + Co-authored-by: Ubuntu 20-04-02-amd64 + * Feature/sm dance bot refine (#131) + * more changes in sm_dance_bot + * minor + * Feature/sm dance bot refine 2 (#132) + * more changes in sm_dance_bot + * minor + * build fix + * waypoints navigator bug (#133) + * minor tuning to mitigate overshot issue cases + * progress in the sm_dance_bot tests (#135) + * some more progress on markers cleanup + * minor format issues (#134) + * sm_dance_bot_lite (#136) + Co-authored-by: Ubuntu 20-04-02-amd64 + * Resolve compile wanings (#137) + * Add SM core test (#138) + * minor navigation improvements (#141) + * using local action msgs (#139) + * using local action msgs + * removing sm_dance_bot_msgs + * pending references + * Feature/nitrosz renaming (#144) + * using local action msgs + * removing sm_dance_bot_msgs + * pending references + * navigation 2 stack renaming + * formatting + * added SVGs to READMEs of atomic, dance_bot, and others (#140) + * added remaining SVGs to READMEs (#145) + * added remaining SVGs to READMEs + * precommit cleanup + * Update package list. (#142) + * removing parameters smacc (#147) + * removing parameters smacc + * workflows update + * workflow + * Noticed launch command was incorrect in README.md + fixed launch command for sm_dance_bot_strikes_back and removed some comments I had made in the past. + * Fix CI: format fix python version (#148) + * Add SM Atomic SM generator. (#143) + * Remove node creation and create only a logger. (#149) + * Rolling Docker environment to be executed from any environment (#154) + * Feature/sm dance bot strikes back refactoring (#152) + Co-authored-by: DecDury + Co-authored-by: Denis Štogl + * slight waypoint 4 and iterations changes so robot can complete course (#155) + * Feature/migration moveit client (#151) + * initial migration to smacc2 + * fixing some errors introduced on formatting + * missing dependency + * fixing some more linting warnings + * minor + * removing test from main moveit cmake + * test ur5 + * progressing in the moveit migration testing + * updating format + * adding .reps dependencies and also fixing some build errors + * repos dependency + * adding dependency to ur5 client + * docker refactoring + * minor + * progress on move_it PR + * minor dockerfile test workaround + * improving dockerfile for building local tests + * minor + * fixing compiling issues + * update readme (#164) + * update readme + * more readme updates + * more + Co-authored-by: Ubuntu 20-04-02-amd64 + * initial state machine transition timestamp (#165) + * moved reference library SMs to smacc2_performance_tools (#166) + * moved reference library SMs to smacc2_performance_tools + * pre-commit cleanup + * Add QOS durability to SmaccPublisherClient (#163) + * feat: add qos durability to SmaccPublisherClient + * fix: add a missing colon + * refactor: remove line + * feat: add reliability qos config + * Feature/testing moveit behaviors (#167) + * more testing on moveit + * progress on moveit + * more testing on moveit behaviors + * minor configuration + * fixing pipeline error + * fixing broken master build + * sm_pubsub_1 (#169) + Co-authored-by: Ubuntu 20-04-02-amd64 + * sm_pubsub_1 part 2 (#170) + * sm_pubsub_1 part 2 + * sm_pubsub_1 part 2 + Co-authored-by: Ubuntu 20-04-02-amd64 + * sm_advanced_recovery_1 renaming (#171) + Co-authored-by: Ubuntu 20-04-02-amd64 + * sm_multi_stage_1 reworking (#172) + * multistage modes + * sm_multi_stage sequences + * sm_multi_state_1 steps + * sm_multi_stage_1 sequence d + * sm_multi_stage_1 c sequence + * mode_5_sequence_b + * mode_4_sequence_b + * sm_multi_stage_1 most + * finishing touches 1 + * readme + Co-authored-by: Ubuntu 20-04-02-amd64 + * Feature/aws navigation sm dance bot (#174) + * repo dependency + * husky launch file in sm_dance_bot + * Add dependencies for husky simulation. + * Fix formatting. + * Update dependencies for husky in rolling and galactic. + * minor + * progress on aws navigation and some other refactorings on navigation clients and behaviors + * more on aws demo + * fixing broken build + * minor + Co-authored-by: Denis Štogl + Co-authored-by: Denis Štogl + * minor changes (#175) + * warehouse2 (#177) + * Waypoint Inputs (#178) + Co-authored-by: Ubuntu 20-04-02-amd64 + * wharehouse2 progress (#179) + * format (#180) + * sm_dance_bot_warehouse_3 (#181) + Co-authored-by: Ubuntu 20-04-02-amd64 + * Feature/sm warehouse 2 13 dec 2 (#182) + * format + * more changes and headless + * merge + * headless and other fixes + * default values + * Brettpac branch (#184) + * sm_dance_bot_warehouse_3 + * Redoing sm_dance_bot_warehouse_3 waypoints + * More Waypoints + Co-authored-by: Ubuntu 20-04-02-amd64 + * SrConditional fixes and formatting (#168) + * fix: some formatting and templating on SrConditional + * fix: move trigger logic into headers + * fix: lint + * Feature/wharehouse2 dec 14 (#185) + * warehouse2 + * minor + * Feature/sm warehouse 2 13 dec 2 (#186) + * format + * more changes and headless + * merge + * headless and other fixes + * default values + * minor + * finetuning waypoints (#187) + Co-authored-by: Ubuntu 20-04-02-amd64 + * Feature/cb pure spinning (#188) + * format + * more changes and headless + * merge + * headless and other fixes + * default values + * minor + * Feature/cb pure spinning (#189) + * format + * more changes and headless + * merge + * headless and other fixes + * default values + * minor + * pure spinning behavior missing files + * minor changes (#190) + * Feature/planner changes 16 12 (#191) + * minor changes + * more fixes + * minor + * minor + * Feature/replanning 16 dec (#193) + * minor changes + * replanning for all our examples + * several fixes (#194) + * minor changes (#195) + * Feature/undo motion 20 12 (#196) + * minor changes + * replanning for all our examples + * improving undo motion navigation warehouse2 + * minor + * tuning warehouse3 (#197) + * Feature/undo motion 20 12 (#198) + * minor changes + * replanning for all our examples + * improving undo motion navigation warehouse2 + * minor + * undo tuning and errors + * format + * Feature/sync 21 12 (#199) + * minor changes + * replanning for all our examples + * format issues + * Feature/warehouse2 22 12 (#200) + * minor changes + * replanning for all our examples + * format issues + * finishing warehouse2 + * Feature/warehouse2 23 12 (#201) + * minor changes + * replanning for all our examples + * tuning and fixes (#202) + * Feature/minor tune (#203) + * tuning and fixes + * minor tune + * fixing warehouse 3 problems, and other core improvements (#204) + * fixing warehouse 3 problems, and other core improvements to remove dead lock, also making continuous integration green + * weird moveit not downloaded repo + * added missing file from warehouse2 (#205) + * Update cb_navigate_global_position.hpp + * Merging code from backport foxy and updates about autoware (#208) + * minor changes + * replanning for all our examples + * backport to foxy + * minor format + * minor linking errors foxy + * Foxy backport (#206) + * minor formatting fixes + * Fix trailing spaces. + * Correct codespell. + * Correct python linters warnings. + * Add galactic CI build because Navigation2 is broken in rolling. + * Add partial changes for ament_cpplint. + * Add tf2_ros as dependency to find include. + * Disable ament_cpplint. + * Disable some packages and update workflows. + * Bump ccache version. + * Ignore further packages + * Satisfy ament_lint_cmake + * Add missing licences. + * Disable cpplint and cppcheck linters. + * Correct formatters. + * branching example + * Disable disabled packages + * Update ci-build-source.yml + * Change extension + * Change extension of imports. + * Enable cppcheck + * Correct formatting of python file. + * Included necessary package and edited Threesome launch + Changed... + ros2 launch sm_three_some sm_three_some + to + ros2 launch sm_three_some sm_three_some.launch + Added: + First ensure you have the necessary package installed. + ``` + sudo apt-get install ros-rolling-ros2trace + ``` + Then run this command. + * Rename header files and correct format. + * Add workflow for checking doc build. + * Update doxygen-check-build.yml + * Create doxygen-deploy.yml + * Use manual deployment for now. + * Create workflow for testing prerelease builds + * Use docs/ as source folder for documentation + * Use docs/ as output directory. + * Rename to smacc2 and smacc2_msgs + * Correct GitHub branch reference. + * Update name of package and package.xml to pass liter. + * Execute on master update + * Reset all versions to 0.0.0 + * Ignore all packages except smacc2 and smacc2_msgs + * Update changelogs + * 0.1.0 + * Revert "Ignore all packages except smacc2 and smacc2_msgs" + This reverts commit f603166a4b3ccdfe96c64d9f9fb9d8b49fbf0e61. + * Update description table. + * Update table + * Copy initial docs + * Dockerfile w/ ROS distro as argument + use this command "sudo docker build --build-arg ROS2_DISTRO=(desiredRosTag) (directoryHoldingDockerfile)/" + * Opened new folder for additional tracing contents + * Delete tracing directory + * Moved tracing.md to tracing directory + * added setupTracing.sh + Installs necessary packages and configures tracing group. + * Removed manual installation of ros-rolling-ros2trace + This is now automated in setupTracing.sh + location of sh file assumed if user follows README.md under "Getting started" + * Created alternative ManualTracing + * added new sm markdowns + * added a dockerfile for Rolling and Galactic + * Update smacc2_ci/docker/ros_rollingAndGalactic_ubuntu_20.04/buildGalactic.sh + Co-authored-by: Denis Štogl + * Update tracing/ManualTracing.md + Co-authored-by: Denis Štogl + * changed wording "smacc application" to "SMACC2 library" + Co-authored-by: Denis Štogl + * Update smacc_sm_reference_library/sm_atomic/README.md + edit from html to markdown syntax + Co-authored-by: Denis Štogl + * reactivating smacc2 nav clients for rolling via submodules + * renamed tracing events after + * bug in smacc2 component + * reverted markdowns to html + * added README tutorial for Dockerfile + * additional cleanup + * cleanup + * cleanup + * edited tracing.md to reflect new tracing event names + * Enable build of missing rolling repositories. + * Enable Navigation2 for semi-binary build. + * Remove galactic builds from master and kepp only rolling. Remove submodules and use .repos file + * updated mentions of SMACC/ROS to SMACC2/ROS2 + * some progress on navigation rolling + * renamed folders, deleted tracing.md, edited README.md + * added smacc2_performance_tools + * performance tests improvements + * more on performance and other issues + * sm_respira_1 format cleanup + * sm_respira_1 format cleanup pre-commit + * sm_respira_test_2 + * sm_respira_test_2 + * more changes on performance tests + * Do not execute clang-format on smacc2_sm_reference_library package. + * sm_reference_library reformatting + * Correct trailing spaces. + * sm_atomic_24hr + * sm_atomic_performance_trace_1 + * Update smacc2_rta command across readmes + * Clean up of sm_atomic_24hr + * more sm_atomic_24hr cleanup + * Optimized deps in move_base_z_planners_common. + * Renaming of event generator library + * minor formatting + * Add galactic CI setup and rename rolling files. (#58) + * Fix source CI and correct README overview. (#62) + * Update c_cpp_properties.json + * changed launch command to ros2 launch sm_respira_1 sm_respira_1.launch (#69) + also noticed a note I had made while producing these that was not removed + * update doxygen links (#70) + Co-authored-by: Ubuntu 20-04-02-amd64 + * More Readme Updates (#72) + Co-authored-by: Ubuntu 20-04-02-amd64 + * More Readme (#74) + Co-authored-by: Ubuntu 20-04-02-amd64 + * created new sm from sm_respira_1 (#76) + * Feature/core and navigation fixes (#78) + * base for the sm_aws_aarehouse navigation + * progressing in aws navigation + * minor + * several core improvements during navigation testing + * formatting improvements + * progress in aws navigation demo + * format improvements + * Feature/aws demo progress (#80) + * base for the sm_aws_aarehouse navigation + * progressing in aws navigation + * minor + * several core improvements during navigation testing + * formatting improvements + * progress in aws navigation demo + * format improvements + * format improvements + * more on navigation + * sm_advanced_recovery_1 reworked (#83) + * sm_advanced_recovery_1 reworked + * fix pre-commit + * Trying to fix Pre-Commit + Co-authored-by: Ubuntu 20-04-02-amd64 + * more sm_advanced_recovery_1 (#84) + Co-authored-by: Ubuntu 20-04-02-amd64 + * More sm_advanced_recovery_1 work (#85) + Co-authored-by: Ubuntu 20-04-02-amd64 + * sm_advanced_recovery_1 round 4 (#86) + Co-authored-by: Ubuntu 20-04-02-amd64 + * Brettpac branch (#87) + * sm_atomic_performance_test_a_2 + * sm_atomic_performance_test_a_1 + Co-authored-by: Ubuntu 20-04-02-amd64 + * sm_atomic_performance_test_c_1 (#88) + Co-authored-by: Ubuntu 20-04-02-amd64 + * modifying sm_atomic_performance_test_a_2 (#89) + Co-authored-by: Ubuntu 20-04-02-amd64 + * sm_multi_stage_1 (#90) + * sm_multi_stage_1 + * fixing precommit + Co-authored-by: Ubuntu 20-04-02-amd64 + * more sm_multi_stage_1 (#91) + Co-authored-by: Ubuntu 20-04-02-amd64 + * Update README.md + updated launch command + * Wait topic message client behavior (#81) + * base for the sm_aws_aarehouse navigation + * progressing in aws navigation + * minor + * several core improvements during navigation testing + * formatting improvements + * progress in aws navigation demo + * format improvements + * format improvements + * more on navigation + * new feature, cb_wait_topic_message: asynchronous client behavior that waits a topic message and optionally checks its contents to success + * formatting + * attempting precommit fixes + Co-authored-by: Ubuntu 20-04-02-amd64 + Co-authored-by: Denis Štogl + * Feature/wait nav2 nodes client behavior (#82) + * base for the sm_aws_aarehouse navigation + * progressing in aws navigation + * minor + * several core improvements during navigation testing + * formatting improvements + * progress in aws navigation demo + * format improvements + * format improvements + * more on navigation + * new feature, cb_wait_topic_message: asynchronous client behavior that waits a topic message and optionally checks its contents to success + * formatting + * adding new client behavior add for nav2, wait nav2 nodes subscribing to the /bond topic and waiting they are alive. you optionally can select the nodes to wait + * Correct all linters and formaters. + Co-authored-by: Denis Štogl + Co-authored-by: Denis Štogl + * Feature/aws demo progress (#92) + * base for the sm_aws_aarehouse navigation + * progressing in aws navigation + * minor + * several core improvements during navigation testing + * formatting improvements + * progress in aws navigation demo + * format improvements + * format improvements + * more on navigation + * new feature, cb_wait_topic_message: asynchronous client behavior that waits a topic message and optionally checks its contents to success + * formatting + * adding new client behavior add for nav2, wait nav2 nodes subscribing to the /bond topic and waiting they are alive. you optionally can select the nodes to wait + * progress in aws navigation demo + * minor format + * Feature/sm dance bot fixes (#93) + * base for the sm_aws_aarehouse navigation + * progressing in aws navigation + * minor + * several core improvements during navigation testing + * formatting improvements + * progress in aws navigation demo + * format improvements + * format improvements + * more on navigation + * new feature, cb_wait_topic_message: asynchronous client behavior that waits a topic message and optionally checks its contents to success + * formatting + * adding new client behavior add for nav2, wait nav2 nodes subscribing to the /bond topic and waiting they are alive. you optionally can select the nodes to wait + * progress in aws navigation demo + * minor format + * navigation parameters fixes on sm_dance_bot + * minor format + * Feature/sm aws warehouse (#94) + * base for the sm_aws_aarehouse navigation + * progressing in aws navigation + * minor + * several core improvements during navigation testing + * formatting improvements + * progress in aws navigation demo + * format improvements + * format improvements + * more on navigation + * new feature, cb_wait_topic_message: asynchronous client behavior that waits a topic message and optionally checks its contents to success + * formatting + * adding new client behavior add for nav2, wait nav2 nodes subscribing to the /bond topic and waiting they are alive. you optionally can select the nodes to wait + * progress in aws navigation demo + * minor format + * merge and progress + * fix format + * Feature/sm dance bot fixes (#95) + * base for the sm_aws_aarehouse navigation + * progressing in aws navigation + * minor + * several core improvements during navigation testing + * formatting improvements + * progress in aws navigation demo + * format improvements + * format improvements + * more on navigation + * new feature, cb_wait_topic_message: asynchronous client behavior that waits a topic message and optionally checks its contents to success + * formatting + * adding new client behavior add for nav2, wait nav2 nodes subscribing to the /bond topic and waiting they are alive. you optionally can select the nodes to wait + * progress in aws navigation demo + * minor format + * navigation parameters fixes on sm_dance_bot + * minor format + * minor + * minor format + * Remove some compile warnings. (#96) + * Feature/cb pause slam (#98) + * base for the sm_aws_aarehouse navigation + * progressing in aws navigation + * minor + * several core improvements during navigation testing + * formatting improvements + * progress in aws navigation demo + * format improvements + * format improvements + * more on navigation + * new feature, cb_wait_topic_message: asynchronous client behavior that waits a topic message and optionally checks its contents to success + * formatting + * adding new client behavior add for nav2, wait nav2 nodes subscribing to the /bond topic and waiting they are alive. you optionally can select the nodes to wait + * progress in aws navigation demo + * minor format + * navigation parameters fixes on sm_dance_bot + * minor format + * minor + * formatting + * cb pause slam client behavior + * sm_dance_bot_lite (#99) + * sm_dance_bot_lite + * precommit + * Updates yaml + Co-authored-by: Ubuntu 20-04-02-amd64 + * Rename doxygen deployment workflow (#100) + * minor hotfix + * sm_dance_bot visualizing turtlebot3 (#101) + * Feature/dance bot launch gz lidar choice (#102) + * sm_dance_bot visualizing turtlebot3 + * cleaning and lidar show/hide option + * cleaning files and making formatting work + * more fixes + * Feature/sm dance bot lite gazebo fixes (#104) + * sm_dance_bot visualizing turtlebot3 + * cleaning and lidar show/hide option + * cleaning files and making formatting work + * more fixes + * gazebo fixes, to show the robot and the lidar + * format fixes + * sm_multi_stage_1 doubling (#103) + Co-authored-by: Ubuntu 20-04-02-amd64 + * Feature/sm dance bot strikes back gazebo fixes (#105) + * sm_dance_bot visualizing turtlebot3 + * cleaning and lidar show/hide option + * cleaning files and making formatting work + * more fixes + * gazebo fixes, to show the robot and the lidar + * format fixes + * gazebo fixes for sm_dance_bot_strikes_back + * precommit cleanup run (#106) + Co-authored-by: Ubuntu 20-04-02-amd64 + * aws demo (#108) + * aws demo + * format + * got sm_multi_stage_1 working (barely) (#109) + Co-authored-by: Ubuntu 20-04-02-amd64 + * Brettpac branch (#110) + * got sm_multi_stage_1 working (barely) + * gaining traction sm_multi_stage_1 + * more + * don't remember + * making progress + * More + * keep hammering + * two stages + * 3 part + * 4th stage + Co-authored-by: Ubuntu 20-04-02-amd64 + * Brettpac branch (#111) + * got sm_multi_stage_1 working (barely) + * gaining traction sm_multi_stage_1 + * more + * don't remember + * making progress + * More + * keep hammering + * two stages + * 3 part + * 4th stage + * 5th stage + Co-authored-by: Ubuntu 20-04-02-amd64 + * a3 (#113) + Co-authored-by: Ubuntu 20-04-02-amd64 + * Remove neo_simulation2 package. (#112) + * Remove neo_simulation2 package. + * Correct formatting. + * Enable source build on PR for testing. + * Adjust build packages of source CI + * more sm_multi_stage_1 (#114) + Co-authored-by: Ubuntu 20-04-02-amd64 + * mm (#115) + Co-authored-by: Ubuntu 20-04-02-amd64 + * diverse improvements navigation and performance (#116) + * diverse improvements navigation and performance + * minor + Co-authored-by: pabloinigoblasco + * Feature/diverse improvemets navigation performance (#117) + * diverse improvements navigation and performance + * minor + * additional linting and formatting + * Remove merge markers from a python file. (#119) + * Feature/slam toggle and smacc deep history (#122) + * progress in navigation, slam toggle client behaviors and slam_toolbox components. Also smacc2::deep_history syntax + * going forward in testing sm_dance_bot introducing slam pausing/resuming funcionality + * feature/more_sm_dance_bot_fixes + * minor format + * minor (#124) + Co-authored-by: Ubuntu 20-04-02-amd64 + * more changes in sm_dance_bot (#125) + * Move method after the method it calls. Otherwise recursion could happen. (#126) + * Feature/dance bot s pattern (#128) + * more changes in sm_dance_bot + * polishing sm_dance_bot and s-pattern + * noticed typo + Finnaly > Finally + * Feature/dance bot s pattern (#129) + * more changes in sm_dance_bot + * polishing sm_dance_bot and s-pattern + * more refinement in sm_dance_bot + * First working version of sm template and template generator. (#127) + * minor tweaks (#130) + Co-authored-by: Ubuntu 20-04-02-amd64 + * Feature/sm dance bot refine (#131) + * more changes in sm_dance_bot + * minor + * Feature/sm dance bot refine 2 (#132) + * more changes in sm_dance_bot + * minor + * build fix + * waypoints navigator bug (#133) + * minor tuning to mitigate overshot issue cases + * progress in the sm_dance_bot tests (#135) + * some more progress on markers cleanup + * minor format issues (#134) + * sm_dance_bot_lite (#136) + Co-authored-by: Ubuntu 20-04-02-amd64 + * Resolve compile wanings (#137) + * Add SM core test (#138) + * minor navigation improvements (#141) + * using local action msgs (#139) + * using local action msgs + * removing sm_dance_bot_msgs + * pending references + * Feature/nitrosz renaming (#144) + * using local action msgs + * removing sm_dance_bot_msgs + * pending references + * navigation 2 stack renaming + * formatting + * added SVGs to READMEs of atomic, dance_bot, and others (#140) + * added remaining SVGs to READMEs (#145) + * added remaining SVGs to READMEs + * precommit cleanup + * Update package list. (#142) + * removing parameters smacc (#147) + * removing parameters smacc + * workflows update + * workflow + * Noticed launch command was incorrect in README.md + fixed launch command for sm_dance_bot_strikes_back and removed some comments I had made in the past. + * Fix CI: format fix python version (#148) + * Add SM Atomic SM generator. (#143) + * Remove node creation and create only a logger. (#149) + * Rolling Docker environment to be executed from any environment (#154) + * Feature/sm dance bot strikes back refactoring (#152) + Co-authored-by: DecDury + Co-authored-by: Denis Štogl + * slight waypoint 4 and iterations changes so robot can complete course (#155) + * Feature/migration moveit client (#151) + * initial migration to smacc2 + * fixing some errors introduced on formatting + * missing dependency + * fixing some more linting warnings + * minor + * removing test from main moveit cmake + * test ur5 + * progressing in the moveit migration testing + * updating format + * adding .reps dependencies and also fixing some build errors + * repos dependency + * adding dependency to ur5 client + * docker refactoring + * minor + * progress on move_it PR + * minor dockerfile test workaround + * improving dockerfile for building local tests + * minor + * fixing compiling issues + * update readme (#164) + * update readme + * more readme updates + * more + Co-authored-by: Ubuntu 20-04-02-amd64 + * initial state machine transition timestamp (#165) + * moved reference library SMs to smacc2_performance_tools (#166) + * moved reference library SMs to smacc2_performance_tools + * pre-commit cleanup + * Add QOS durability to SmaccPublisherClient (#163) + * feat: add qos durability to SmaccPublisherClient + * fix: add a missing colon + * refactor: remove line + * feat: add reliability qos config + * Feature/testing moveit behaviors (#167) + * more testing on moveit + * progress on moveit + * more testing on moveit behaviors + * minor configuration + * fixing pipeline error + * fixing broken master build + * sm_pubsub_1 (#169) + Co-authored-by: Ubuntu 20-04-02-amd64 + * sm_pubsub_1 part 2 (#170) + * sm_pubsub_1 part 2 + * sm_pubsub_1 part 2 + Co-authored-by: Ubuntu 20-04-02-amd64 + * sm_advanced_recovery_1 renaming (#171) + Co-authored-by: Ubuntu 20-04-02-amd64 + * sm_multi_stage_1 reworking (#172) + * multistage modes + * sm_multi_stage sequences + * sm_multi_state_1 steps + * sm_multi_stage_1 sequence d + * sm_multi_stage_1 c sequence + * mode_5_sequence_b + * mode_4_sequence_b + * sm_multi_stage_1 most + * finishing touches 1 + * readme + Co-authored-by: Ubuntu 20-04-02-amd64 + * Feature/aws navigation sm dance bot (#174) + * repo dependency + * husky launch file in sm_dance_bot + * Add dependencies for husky simulation. + * Fix formatting. + * Update dependencies for husky in rolling and galactic. + * minor + * progress on aws navigation and some other refactorings on navigation clients and behaviors + * more on aws demo + * fixing broken build + * minor + Co-authored-by: Denis Štogl + Co-authored-by: Denis Štogl + * minor changes (#175) + * warehouse2 (#177) + * Waypoint Inputs (#178) + Co-authored-by: Ubuntu 20-04-02-amd64 + * wharehouse2 progress (#179) + * format (#180) + * sm_dance_bot_warehouse_3 (#181) + Co-authored-by: Ubuntu 20-04-02-amd64 + * Feature/sm warehouse 2 13 dec 2 (#182) + * format + * more changes and headless + * merge + * headless and other fixes + * default values + * Brettpac branch (#184) + * sm_dance_bot_warehouse_3 + * Redoing sm_dance_bot_warehouse_3 waypoints + * More Waypoints + Co-authored-by: Ubuntu 20-04-02-amd64 + * SrConditional fixes and formatting (#168) + * fix: some formatting and templating on SrConditional + * fix: move trigger logic into headers + * fix: lint + * Feature/wharehouse2 dec 14 (#185) + * warehouse2 + * minor + * Feature/sm warehouse 2 13 dec 2 (#186) + * format + * more changes and headless + * merge + * headless and other fixes + * default values + * minor + * finetuning waypoints (#187) + Co-authored-by: Ubuntu 20-04-02-amd64 + * Feature/cb pure spinning (#188) + * format + * more changes and headless + * merge + * headless and other fixes + * default values + * minor + * Feature/cb pure spinning (#189) + * format + * more changes and headless + * merge + * headless and other fixes + * default values + * minor + * pure spinning behavior missing files + * minor changes (#190) + * Feature/planner changes 16 12 (#191) + * minor changes + * more fixes + * minor + * minor + * Feature/replanning 16 dec (#193) + * minor changes + * replanning for all our examples + * several fixes (#194) + * minor changes (#195) + * Feature/undo motion 20 12 (#196) + * minor changes + * replanning for all our examples + * improving undo motion navigation warehouse2 + * minor + * tuning warehouse3 (#197) + * Feature/undo motion 20 12 (#198) + * minor changes + * replanning for all our examples + * improving undo motion navigation warehouse2 + * minor + * undo tuning and errors + * format + * Feature/sync 21 12 (#199) + * minor changes + * replanning for all our examples + * format issues + * Feature/warehouse2 22 12 (#200) + * minor changes + * replanning for all our examples + * format issues + * finishing warehouse2 + * Feature/warehouse2 23 12 (#201) + * minor changes + * replanning for all our examples + * tuning and fixes (#202) + * Feature/minor tune (#203) + * tuning and fixes + * minor tune + * fixing warehouse 3 problems, and other core improvements (#204) + * fixing warehouse 3 problems, and other core improvements to remove dead lock, also making continuous integration green + * weird moveit not downloaded repo + * added missing file from warehouse2 (#205) + * backport to foxy + * minor format + * minor linking errors foxy + Co-authored-by: Denis Štogl + Co-authored-by: Denis Štogl + Co-authored-by: Declan Dury <44791484+DecDury@users.noreply.github.com> + Co-authored-by: DecDury + Co-authored-by: reelrbtx + Co-authored-by: brettpac + Co-authored-by: David Revay + * missing + * missing sm + * updating subscriber publisher components + * progress in autowarrrrrrrrrrrrrrrrrrrrrrrrrrrrrrrrrre machine + * refining cp subscriber cp publisher + * improvements in smacc core adding more components mostly developed for autoware demo + * autoware demo + * missing + * foxy ci + * fix + * minor broken build + Co-authored-by: Denis Štogl + Co-authored-by: Denis Štogl + Co-authored-by: Declan Dury <44791484+DecDury@users.noreply.github.com> + Co-authored-by: DecDury + Co-authored-by: reelrbtx + Co-authored-by: brettpac + Co-authored-by: David Revay + Co-authored-by: pabloinigoblasco + * Add mergify rules file. + * Try fixing CI for rolling. (#209) + Merging to get backport working. + * Remove example things from Foxy CI setup. (#214) + * Add Autoware Auto Msgs into not-released dependencies. (#220) + * Fix rolling builds (#222) + * do not merge yet - Feature/odom tracker improvements and retry motion (#223) + * odom tracker improvements + * adding forward behavior retry funcionality + * removing warnings (#213) + * minor changes + * replanning for all our examples + * backport to foxy + * minor format + * minor linking errors foxy + * Foxy backport (#206) + * minor formatting fixes + * Fix trailing spaces. + * Correct codespell. + * Correct python linters warnings. + * Add galactic CI build because Navigation2 is broken in rolling. + * Add partial changes for ament_cpplint. + * Add tf2_ros as dependency to find include. + * Disable ament_cpplint. + * Disable some packages and update workflows. + * Bump ccache version. + * Ignore further packages + * Satisfy ament_lint_cmake + * Add missing licences. + * Disable cpplint and cppcheck linters. + * Correct formatters. + * branching example + * Disable disabled packages + * Update ci-build-source.yml + * Change extension + * Change extension of imports. + * Enable cppcheck + * Correct formatting of python file. + * Included necessary package and edited Threesome launch + Changed... + ros2 launch sm_three_some sm_three_some + to + ros2 launch sm_three_some sm_three_some.launch + Added: + First ensure you have the necessary package installed. + ``` + sudo apt-get install ros-rolling-ros2trace + ``` + Then run this command. + * Rename header files and correct format. + * Add workflow for checking doc build. + * Update doxygen-check-build.yml + * Create doxygen-deploy.yml + * Use manual deployment for now. + * Create workflow for testing prerelease builds + * Use docs/ as source folder for documentation + * Use docs/ as output directory. + * Rename to smacc2 and smacc2_msgs + * Correct GitHub branch reference. + * Update name of package and package.xml to pass liter. + * Execute on master update + * Reset all versions to 0.0.0 + * Ignore all packages except smacc2 and smacc2_msgs + * Update changelogs + * 0.1.0 + * Revert "Ignore all packages except smacc2 and smacc2_msgs" + This reverts commit f603166a4b3ccdfe96c64d9f9fb9d8b49fbf0e61. + * Update description table. + * Update table + * Copy initial docs + * Dockerfile w/ ROS distro as argument + use this command "sudo docker build --build-arg ROS2_DISTRO=(desiredRosTag) (directoryHoldingDockerfile)/" + * Opened new folder for additional tracing contents + * Delete tracing directory + * Moved tracing.md to tracing directory + * added setupTracing.sh + Installs necessary packages and configures tracing group. + * Removed manual installation of ros-rolling-ros2trace + This is now automated in setupTracing.sh + location of sh file assumed if user follows README.md under "Getting started" + * Created alternative ManualTracing + * added new sm markdowns + * added a dockerfile for Rolling and Galactic + * Update smacc2_ci/docker/ros_rollingAndGalactic_ubuntu_20.04/buildGalactic.sh + Co-authored-by: Denis Štogl + * Update tracing/ManualTracing.md + Co-authored-by: Denis Štogl + * changed wording "smacc application" to "SMACC2 library" + Co-authored-by: Denis Štogl + * Update smacc_sm_reference_library/sm_atomic/README.md + edit from html to markdown syntax + Co-authored-by: Denis Štogl + * reactivating smacc2 nav clients for rolling via submodules + * renamed tracing events after + * bug in smacc2 component + * reverted markdowns to html + * added README tutorial for Dockerfile + * additional cleanup + * cleanup + * cleanup + * edited tracing.md to reflect new tracing event names + * Enable build of missing rolling repositories. + * Enable Navigation2 for semi-binary build. + * Remove galactic builds from master and kepp only rolling. Remove submodules and use .repos file + * updated mentions of SMACC/ROS to SMACC2/ROS2 + * some progress on navigation rolling + * renamed folders, deleted tracing.md, edited README.md + * added smacc2_performance_tools + * performance tests improvements + * more on performance and other issues + * sm_respira_1 format cleanup + * sm_respira_1 format cleanup pre-commit + * sm_respira_test_2 + * sm_respira_test_2 + * more changes on performance tests + * Do not execute clang-format on smacc2_sm_reference_library package. + * sm_reference_library reformatting + * Correct trailing spaces. + * sm_atomic_24hr + * sm_atomic_performance_trace_1 + * Update smacc2_rta command across readmes + * Clean up of sm_atomic_24hr + * more sm_atomic_24hr cleanup + * Optimized deps in move_base_z_planners_common. + * Renaming of event generator library + * minor formatting + * Add galactic CI setup and rename rolling files. (#58) + * Fix source CI and correct README overview. (#62) + * Update c_cpp_properties.json + * changed launch command to ros2 launch sm_respira_1 sm_respira_1.launch (#69) + also noticed a note I had made while producing these that was not removed + * update doxygen links (#70) + Co-authored-by: Ubuntu 20-04-02-amd64 + * More Readme Updates (#72) + Co-authored-by: Ubuntu 20-04-02-amd64 + * More Readme (#74) + Co-authored-by: Ubuntu 20-04-02-amd64 + * created new sm from sm_respira_1 (#76) + * Feature/core and navigation fixes (#78) + * base for the sm_aws_aarehouse navigation + * progressing in aws navigation + * minor + * several core improvements during navigation testing + * formatting improvements + * progress in aws navigation demo + * format improvements + * Feature/aws demo progress (#80) + * base for the sm_aws_aarehouse navigation + * progressing in aws navigation + * minor + * several core improvements during navigation testing + * formatting improvements + * progress in aws navigation demo + * format improvements + * format improvements + * more on navigation + * sm_advanced_recovery_1 reworked (#83) + * sm_advanced_recovery_1 reworked + * fix pre-commit + * Trying to fix Pre-Commit + Co-authored-by: Ubuntu 20-04-02-amd64 + * more sm_advanced_recovery_1 (#84) + Co-authored-by: Ubuntu 20-04-02-amd64 + * More sm_advanced_recovery_1 work (#85) + Co-authored-by: Ubuntu 20-04-02-amd64 + * sm_advanced_recovery_1 round 4 (#86) + Co-authored-by: Ubuntu 20-04-02-amd64 + * Brettpac branch (#87) + * sm_atomic_performance_test_a_2 + * sm_atomic_performance_test_a_1 + Co-authored-by: Ubuntu 20-04-02-amd64 + * sm_atomic_performance_test_c_1 (#88) + Co-authored-by: Ubuntu 20-04-02-amd64 + * modifying sm_atomic_performance_test_a_2 (#89) + Co-authored-by: Ubuntu 20-04-02-amd64 + * sm_multi_stage_1 (#90) + * sm_multi_stage_1 + * fixing precommit + Co-authored-by: Ubuntu 20-04-02-amd64 + * more sm_multi_stage_1 (#91) + Co-authored-by: Ubuntu 20-04-02-amd64 + * Update README.md + updated launch command + * Wait topic message client behavior (#81) + * base for the sm_aws_aarehouse navigation + * progressing in aws navigation + * minor + * several core improvements during navigation testing + * formatting improvements + * progress in aws navigation demo + * format improvements + * format improvements + * more on navigation + * new feature, cb_wait_topic_message: asynchronous client behavior that waits a topic message and optionally checks its contents to success + * formatting + * attempting precommit fixes + Co-authored-by: Ubuntu 20-04-02-amd64 + Co-authored-by: Denis Štogl + * Feature/wait nav2 nodes client behavior (#82) + * base for the sm_aws_aarehouse navigation + * progressing in aws navigation + * minor + * several core improvements during navigation testing + * formatting improvements + * progress in aws navigation demo + * format improvements + * format improvements + * more on navigation + * new feature, cb_wait_topic_message: asynchronous client behavior that waits a topic message and optionally checks its contents to success + * formatting + * adding new client behavior add for nav2, wait nav2 nodes subscribing to the /bond topic and waiting they are alive. you optionally can select the nodes to wait + * Correct all linters and formaters. + Co-authored-by: Denis Štogl + Co-authored-by: Denis Štogl + * Feature/aws demo progress (#92) + * base for the sm_aws_aarehouse navigation + * progressing in aws navigation + * minor + * several core improvements during navigation testing + * formatting improvements + * progress in aws navigation demo + * format improvements + * format improvements + * more on navigation + * new feature, cb_wait_topic_message: asynchronous client behavior that waits a topic message and optionally checks its contents to success + * formatting + * adding new client behavior add for nav2, wait nav2 nodes subscribing to the /bond topic and waiting they are alive. you optionally can select the nodes to wait + * progress in aws navigation demo + * minor format + * Feature/sm dance bot fixes (#93) + * base for the sm_aws_aarehouse navigation + * progressing in aws navigation + * minor + * several core improvements during navigation testing + * formatting improvements + * progress in aws navigation demo + * format improvements + * format improvements + * more on navigation + * new feature, cb_wait_topic_message: asynchronous client behavior that waits a topic message and optionally checks its contents to success + * formatting + * adding new client behavior add for nav2, wait nav2 nodes subscribing to the /bond topic and waiting they are alive. you optionally can select the nodes to wait + * progress in aws navigation demo + * minor format + * navigation parameters fixes on sm_dance_bot + * minor format + * Feature/sm aws warehouse (#94) + * base for the sm_aws_aarehouse navigation + * progressing in aws navigation + * minor + * several core improvements during navigation testing + * formatting improvements + * progress in aws navigation demo + * format improvements + * format improvements + * more on navigation + * new feature, cb_wait_topic_message: asynchronous client behavior that waits a topic message and optionally checks its contents to success + * formatting + * adding new client behavior add for nav2, wait nav2 nodes subscribing to the /bond topic and waiting they are alive. you optionally can select the nodes to wait + * progress in aws navigation demo + * minor format + * merge and progress + * fix format + * Feature/sm dance bot fixes (#95) + * base for the sm_aws_aarehouse navigation + * progressing in aws navigation + * minor + * several core improvements during navigation testing + * formatting improvements + * progress in aws navigation demo + * format improvements + * format improvements + * more on navigation + * new feature, cb_wait_topic_message: asynchronous client behavior that waits a topic message and optionally checks its contents to success + * formatting + * adding new client behavior add for nav2, wait nav2 nodes subscribing to the /bond topic and waiting they are alive. you optionally can select the nodes to wait + * progress in aws navigation demo + * minor format + * navigation parameters fixes on sm_dance_bot + * minor format + * minor + * minor format + * Remove some compile warnings. (#96) + * Feature/cb pause slam (#98) + * base for the sm_aws_aarehouse navigation + * progressing in aws navigation + * minor + * several core improvements during navigation testing + * formatting improvements + * progress in aws navigation demo + * format improvements + * format improvements + * more on navigation + * new feature, cb_wait_topic_message: asynchronous client behavior that waits a topic message and optionally checks its contents to success + * formatting + * adding new client behavior add for nav2, wait nav2 nodes subscribing to the /bond topic and waiting they are alive. you optionally can select the nodes to wait + * progress in aws navigation demo + * minor format + * navigation parameters fixes on sm_dance_bot + * minor format + * minor + * formatting + * cb pause slam client behavior + * sm_dance_bot_lite (#99) + * sm_dance_bot_lite + * precommit + * Updates yaml + Co-authored-by: Ubuntu 20-04-02-amd64 + * Rename doxygen deployment workflow (#100) + * minor hotfix + * sm_dance_bot visualizing turtlebot3 (#101) + * Feature/dance bot launch gz lidar choice (#102) + * sm_dance_bot visualizing turtlebot3 + * cleaning and lidar show/hide option + * cleaning files and making formatting work + * more fixes + * Feature/sm dance bot lite gazebo fixes (#104) + * sm_dance_bot visualizing turtlebot3 + * cleaning and lidar show/hide option + * cleaning files and making formatting work + * more fixes + * gazebo fixes, to show the robot and the lidar + * format fixes + * sm_multi_stage_1 doubling (#103) + Co-authored-by: Ubuntu 20-04-02-amd64 + * Feature/sm dance bot strikes back gazebo fixes (#105) + * sm_dance_bot visualizing turtlebot3 + * cleaning and lidar show/hide option + * cleaning files and making formatting work + * more fixes + * gazebo fixes, to show the robot and the lidar + * format fixes + * gazebo fixes for sm_dance_bot_strikes_back + * precommit cleanup run (#106) + Co-authored-by: Ubuntu 20-04-02-amd64 + * aws demo (#108) + * aws demo + * format + * got sm_multi_stage_1 working (barely) (#109) + Co-authored-by: Ubuntu 20-04-02-amd64 + * Brettpac branch (#110) + * got sm_multi_stage_1 working (barely) + * gaining traction sm_multi_stage_1 + * more + * don't remember + * making progress + * More + * keep hammering + * two stages + * 3 part + * 4th stage + Co-authored-by: Ubuntu 20-04-02-amd64 + * Brettpac branch (#111) + * got sm_multi_stage_1 working (barely) + * gaining traction sm_multi_stage_1 + * more + * don't remember + * making progress + * More + * keep hammering + * two stages + * 3 part + * 4th stage + * 5th stage + Co-authored-by: Ubuntu 20-04-02-amd64 + * a3 (#113) + Co-authored-by: Ubuntu 20-04-02-amd64 + * Remove neo_simulation2 package. (#112) + * Remove neo_simulation2 package. + * Correct formatting. + * Enable source build on PR for testing. + * Adjust build packages of source CI + * more sm_multi_stage_1 (#114) + Co-authored-by: Ubuntu 20-04-02-amd64 + * mm (#115) + Co-authored-by: Ubuntu 20-04-02-amd64 + * diverse improvements navigation and performance (#116) + * diverse improvements navigation and performance + * minor + Co-authored-by: pabloinigoblasco + * Feature/diverse improvemets navigation performance (#117) + * diverse improvements navigation and performance + * minor + * additional linting and formatting + * Remove merge markers from a python file. (#119) + * Feature/slam toggle and smacc deep history (#122) + * progress in navigation, slam toggle client behaviors and slam_toolbox components. Also smacc2::deep_history syntax + * going forward in testing sm_dance_bot introducing slam pausing/resuming funcionality + * feature/more_sm_dance_bot_fixes + * minor format + * minor (#124) + Co-authored-by: Ubuntu 20-04-02-amd64 + * more changes in sm_dance_bot (#125) + * Move method after the method it calls. Otherwise recursion could happen. (#126) + * Feature/dance bot s pattern (#128) + * more changes in sm_dance_bot + * polishing sm_dance_bot and s-pattern + * noticed typo + Finnaly > Finally + * Feature/dance bot s pattern (#129) + * more changes in sm_dance_bot + * polishing sm_dance_bot and s-pattern + * more refinement in sm_dance_bot + * First working version of sm template and template generator. (#127) + * minor tweaks (#130) + Co-authored-by: Ubuntu 20-04-02-amd64 + * Feature/sm dance bot refine (#131) + * more changes in sm_dance_bot + * minor + * Feature/sm dance bot refine 2 (#132) + * more changes in sm_dance_bot + * minor + * build fix + * waypoints navigator bug (#133) + * minor tuning to mitigate overshot issue cases + * progress in the sm_dance_bot tests (#135) + * some more progress on markers cleanup + * minor format issues (#134) + * sm_dance_bot_lite (#136) + Co-authored-by: Ubuntu 20-04-02-amd64 + * Resolve compile wanings (#137) + * Add SM core test (#138) + * minor navigation improvements (#141) + * using local action msgs (#139) + * using local action msgs + * removing sm_dance_bot_msgs + * pending references + * Feature/nitrosz renaming (#144) + * using local action msgs + * removing sm_dance_bot_msgs + * pending references + * navigation 2 stack renaming + * formatting + * added SVGs to READMEs of atomic, dance_bot, and others (#140) + * added remaining SVGs to READMEs (#145) + * added remaining SVGs to READMEs + * precommit cleanup + * Update package list. (#142) + * removing parameters smacc (#147) + * removing parameters smacc + * workflows update + * workflow + * Noticed launch command was incorrect in README.md + fixed launch command for sm_dance_bot_strikes_back and removed some comments I had made in the past. + * Fix CI: format fix python version (#148) + * Add SM Atomic SM generator. (#143) + * Remove node creation and create only a logger. (#149) + * Rolling Docker environment to be executed from any environment (#154) + * Feature/sm dance bot strikes back refactoring (#152) + Co-authored-by: DecDury + Co-authored-by: Denis Štogl + * slight waypoint 4 and iterations changes so robot can complete course (#155) + * Feature/migration moveit client (#151) + * initial migration to smacc2 + * fixing some errors introduced on formatting + * missing dependency + * fixing some more linting warnings + * minor + * removing test from main moveit cmake + * test ur5 + * progressing in the moveit migration testing + * updating format + * adding .reps dependencies and also fixing some build errors + * repos dependency + * adding dependency to ur5 client + * docker refactoring + * minor + * progress on move_it PR + * minor dockerfile test workaround + * improving dockerfile for building local tests + * minor + * fixing compiling issues + * update readme (#164) + * update readme + * more readme updates + * more + Co-authored-by: Ubuntu 20-04-02-amd64 + * initial state machine transition timestamp (#165) + * moved reference library SMs to smacc2_performance_tools (#166) + * moved reference library SMs to smacc2_performance_tools + * pre-commit cleanup + * Add QOS durability to SmaccPublisherClient (#163) + * feat: add qos durability to SmaccPublisherClient + * fix: add a missing colon + * refactor: remove line + * feat: add reliability qos config + * Feature/testing moveit behaviors (#167) + * more testing on moveit + * progress on moveit + * more testing on moveit behaviors + * minor configuration + * fixing pipeline error + * fixing broken master build + * sm_pubsub_1 (#169) + Co-authored-by: Ubuntu 20-04-02-amd64 + * sm_pubsub_1 part 2 (#170) + * sm_pubsub_1 part 2 + * sm_pubsub_1 part 2 + Co-authored-by: Ubuntu 20-04-02-amd64 + * sm_advanced_recovery_1 renaming (#171) + Co-authored-by: Ubuntu 20-04-02-amd64 + * sm_multi_stage_1 reworking (#172) + * multistage modes + * sm_multi_stage sequences + * sm_multi_state_1 steps + * sm_multi_stage_1 sequence d + * sm_multi_stage_1 c sequence + * mode_5_sequence_b + * mode_4_sequence_b + * sm_multi_stage_1 most + * finishing touches 1 + * readme + Co-authored-by: Ubuntu 20-04-02-amd64 + * Feature/aws navigation sm dance bot (#174) + * repo dependency + * husky launch file in sm_dance_bot + * Add dependencies for husky simulation. + * Fix formatting. + * Update dependencies for husky in rolling and galactic. + * minor + * progress on aws navigation and some other refactorings on navigation clients and behaviors + * more on aws demo + * fixing broken build + * minor + Co-authored-by: Denis Štogl + Co-authored-by: Denis Štogl + * minor changes (#175) + * warehouse2 (#177) + * Waypoint Inputs (#178) + Co-authored-by: Ubuntu 20-04-02-amd64 + * wharehouse2 progress (#179) + * format (#180) + * sm_dance_bot_warehouse_3 (#181) + Co-authored-by: Ubuntu 20-04-02-amd64 + * Feature/sm warehouse 2 13 dec 2 (#182) + * format + * more changes and headless + * merge + * headless and other fixes + * default values + * Brettpac branch (#184) + * sm_dance_bot_warehouse_3 + * Redoing sm_dance_bot_warehouse_3 waypoints + * More Waypoints + Co-authored-by: Ubuntu 20-04-02-amd64 + * SrConditional fixes and formatting (#168) + * fix: some formatting and templating on SrConditional + * fix: move trigger logic into headers + * fix: lint + * Feature/wharehouse2 dec 14 (#185) + * warehouse2 + * minor + * Feature/sm warehouse 2 13 dec 2 (#186) + * format + * more changes and headless + * merge + * headless and other fixes + * default values + * minor + * finetuning waypoints (#187) + Co-authored-by: Ubuntu 20-04-02-amd64 + * Feature/cb pure spinning (#188) + * format + * more changes and headless + * merge + * headless and other fixes + * default values + * minor + * Feature/cb pure spinning (#189) + * format + * more changes and headless + * merge + * headless and other fixes + * default values + * minor + * pure spinning behavior missing files + * minor changes (#190) + * Feature/planner changes 16 12 (#191) + * minor changes + * more fixes + * minor + * minor + * Feature/replanning 16 dec (#193) + * minor changes + * replanning for all our examples + * several fixes (#194) + * minor changes (#195) + * Feature/undo motion 20 12 (#196) + * minor changes + * replanning for all our examples + * improving undo motion navigation warehouse2 + * minor + * tuning warehouse3 (#197) + * Feature/undo motion 20 12 (#198) + * minor changes + * replanning for all our examples + * improving undo motion navigation warehouse2 + * minor + * undo tuning and errors + * format + * Feature/sync 21 12 (#199) + * minor changes + * replanning for all our examples + * format issues + * Feature/warehouse2 22 12 (#200) + * minor changes + * replanning for all our examples + * format issues + * finishing warehouse2 + * Feature/warehouse2 23 12 (#201) + * minor changes + * replanning for all our examples + * tuning and fixes (#202) + * Feature/minor tune (#203) + * tuning and fixes + * minor tune + * fixing warehouse 3 problems, and other core improvements (#204) + * fixing warehouse 3 problems, and other core improvements to remove dead lock, also making continuous integration green + * weird moveit not downloaded repo + * added missing file from warehouse2 (#205) + * backport to foxy + * minor format + * minor linking errors foxy + Co-authored-by: Denis Štogl + Co-authored-by: Denis Štogl + Co-authored-by: Declan Dury <44791484+DecDury@users.noreply.github.com> + Co-authored-by: DecDury + Co-authored-by: reelrbtx + Co-authored-by: brettpac + Co-authored-by: David Revay + * missing + * missing sm + * updating subscriber publisher components + * progress in autowarrrrrrrrrrrrrrrrrrrrrrrrrrrrrrrrrre machine + * refining cp subscriber cp publisher + * improvements in smacc core adding more components mostly developed for autoware demo + * autoware demo + * missing + * foxy ci + * fix + * minor broken build + * some reordering fixes + * minor + * docker files for different revisions, warnings removval and more testing on navigation + * fixing docker for foxy and galactic + Co-authored-by: Denis Štogl + Co-authored-by: Denis Štogl + Co-authored-by: Declan Dury <44791484+DecDury@users.noreply.github.com> + Co-authored-by: DecDury + Co-authored-by: reelrbtx + Co-authored-by: brettpac + Co-authored-by: David Revay + Co-authored-by: pabloinigoblasco + * dockerfiles (#225) + * Fix code generators (#221) + * Fix other build issues. + * Update SM template and make example code clearly visible. + * Remove use of node in the sm performance template. + * Updated templated to use Blackboard storage. + * Update template to resolve the global data correctly. + * Update sm_name.hpp + Co-authored-by: Pablo Iñigo Blasco + * Feature/retry behavior warehouse 1 (#226) + * minor changes + * replanning for all our examples + * backport to foxy + * minor format + * minor linking errors foxy + * Foxy backport (#206) + * minor formatting fixes + * Fix trailing spaces. + * Correct codespell. + * Correct python linters warnings. + * Add galactic CI build because Navigation2 is broken in rolling. + * Add partial changes for ament_cpplint. + * Add tf2_ros as dependency to find include. + * Disable ament_cpplint. + * Disable some packages and update workflows. + * Bump ccache version. + * Ignore further packages + * Satisfy ament_lint_cmake + * Add missing licences. + * Disable cpplint and cppcheck linters. + * Correct formatters. + * branching example + * Disable disabled packages + * Update ci-build-source.yml + * Change extension + * Change extension of imports. + * Enable cppcheck + * Correct formatting of python file. + * Included necessary package and edited Threesome launch + Changed... + ros2 launch sm_three_some sm_three_some + to + ros2 launch sm_three_some sm_three_some.launch + Added: + First ensure you have the necessary package installed. + ``` + sudo apt-get install ros-rolling-ros2trace + ``` + Then run this command. + * Rename header files and correct format. + * Add workflow for checking doc build. + * Update doxygen-check-build.yml + * Create doxygen-deploy.yml + * Use manual deployment for now. + * Create workflow for testing prerelease builds + * Use docs/ as source folder for documentation + * Use docs/ as output directory. + * Rename to smacc2 and smacc2_msgs + * Correct GitHub branch reference. + * Update name of package and package.xml to pass liter. + * Execute on master update + * Reset all versions to 0.0.0 + * Ignore all packages except smacc2 and smacc2_msgs + * Update changelogs + * 0.1.0 + * Revert "Ignore all packages except smacc2 and smacc2_msgs" + This reverts commit f603166a4b3ccdfe96c64d9f9fb9d8b49fbf0e61. + * Update description table. + * Update table + * Copy initial docs + * Dockerfile w/ ROS distro as argument + use this command "sudo docker build --build-arg ROS2_DISTRO=(desiredRosTag) (directoryHoldingDockerfile)/" + * Opened new folder for additional tracing contents + * Delete tracing directory + * Moved tracing.md to tracing directory + * added setupTracing.sh + Installs necessary packages and configures tracing group. + * Removed manual installation of ros-rolling-ros2trace + This is now automated in setupTracing.sh + location of sh file assumed if user follows README.md under "Getting started" + * Created alternative ManualTracing + * added new sm markdowns + * added a dockerfile for Rolling and Galactic + * Update smacc2_ci/docker/ros_rollingAndGalactic_ubuntu_20.04/buildGalactic.sh + Co-authored-by: Denis Štogl + * Update tracing/ManualTracing.md + Co-authored-by: Denis Štogl + * changed wording "smacc application" to "SMACC2 library" + Co-authored-by: Denis Štogl + * Update smacc_sm_reference_library/sm_atomic/README.md + edit from html to markdown syntax + Co-authored-by: Denis Štogl + * reactivating smacc2 nav clients for rolling via submodules + * renamed tracing events after + * bug in smacc2 component + * reverted markdowns to html + * added README tutorial for Dockerfile + * additional cleanup + * cleanup + * cleanup + * edited tracing.md to reflect new tracing event names + * Enable build of missing rolling repositories. + * Enable Navigation2 for semi-binary build. + * Remove galactic builds from master and kepp only rolling. Remove submodules and use .repos file + * updated mentions of SMACC/ROS to SMACC2/ROS2 + * some progress on navigation rolling + * renamed folders, deleted tracing.md, edited README.md + * added smacc2_performance_tools + * performance tests improvements + * more on performance and other issues + * sm_respira_1 format cleanup + * sm_respira_1 format cleanup pre-commit + * sm_respira_test_2 + * sm_respira_test_2 + * more changes on performance tests + * Do not execute clang-format on smacc2_sm_reference_library package. + * sm_reference_library reformatting + * Correct trailing spaces. + * sm_atomic_24hr + * sm_atomic_performance_trace_1 + * Update smacc2_rta command across readmes + * Clean up of sm_atomic_24hr + * more sm_atomic_24hr cleanup + * Optimized deps in move_base_z_planners_common. + * Renaming of event generator library + * minor formatting + * Add galactic CI setup and rename rolling files. (#58) + * Fix source CI and correct README overview. (#62) + * Update c_cpp_properties.json + * changed launch command to ros2 launch sm_respira_1 sm_respira_1.launch (#69) + also noticed a note I had made while producing these that was not removed + * update doxygen links (#70) + Co-authored-by: Ubuntu 20-04-02-amd64 + * More Readme Updates (#72) + Co-authored-by: Ubuntu 20-04-02-amd64 + * More Readme (#74) + Co-authored-by: Ubuntu 20-04-02-amd64 + * created new sm from sm_respira_1 (#76) + * Feature/core and navigation fixes (#78) + * base for the sm_aws_aarehouse navigation + * progressing in aws navigation + * minor + * several core improvements during navigation testing + * formatting improvements + * progress in aws navigation demo + * format improvements + * Feature/aws demo progress (#80) + * base for the sm_aws_aarehouse navigation + * progressing in aws navigation + * minor + * several core improvements during navigation testing + * formatting improvements + * progress in aws navigation demo + * format improvements + * format improvements + * more on navigation + * sm_advanced_recovery_1 reworked (#83) + * sm_advanced_recovery_1 reworked + * fix pre-commit + * Trying to fix Pre-Commit + Co-authored-by: Ubuntu 20-04-02-amd64 + * more sm_advanced_recovery_1 (#84) + Co-authored-by: Ubuntu 20-04-02-amd64 + * More sm_advanced_recovery_1 work (#85) + Co-authored-by: Ubuntu 20-04-02-amd64 + * sm_advanced_recovery_1 round 4 (#86) + Co-authored-by: Ubuntu 20-04-02-amd64 + * Brettpac branch (#87) + * sm_atomic_performance_test_a_2 + * sm_atomic_performance_test_a_1 + Co-authored-by: Ubuntu 20-04-02-amd64 + * sm_atomic_performance_test_c_1 (#88) + Co-authored-by: Ubuntu 20-04-02-amd64 + * modifying sm_atomic_performance_test_a_2 (#89) + Co-authored-by: Ubuntu 20-04-02-amd64 + * sm_multi_stage_1 (#90) + * sm_multi_stage_1 + * fixing precommit + Co-authored-by: Ubuntu 20-04-02-amd64 + * more sm_multi_stage_1 (#91) + Co-authored-by: Ubuntu 20-04-02-amd64 + * Update README.md + updated launch command + * Wait topic message client behavior (#81) + * base for the sm_aws_aarehouse navigation + * progressing in aws navigation + * minor + * several core improvements during navigation testing + * formatting improvements + * progress in aws navigation demo + * format improvements + * format improvements + * more on navigation + * new feature, cb_wait_topic_message: asynchronous client behavior that waits a topic message and optionally checks its contents to success + * formatting + * attempting precommit fixes + Co-authored-by: Ubuntu 20-04-02-amd64 + Co-authored-by: Denis Štogl + * Feature/wait nav2 nodes client behavior (#82) + * base for the sm_aws_aarehouse navigation + * progressing in aws navigation + * minor + * several core improvements during navigation testing + * formatting improvements + * progress in aws navigation demo + * format improvements + * format improvements + * more on navigation + * new feature, cb_wait_topic_message: asynchronous client behavior that waits a topic message and optionally checks its contents to success + * formatting + * adding new client behavior add for nav2, wait nav2 nodes subscribing to the /bond topic and waiting they are alive. you optionally can select the nodes to wait + * Correct all linters and formaters. + Co-authored-by: Denis Štogl + Co-authored-by: Denis Štogl + * Feature/aws demo progress (#92) + * base for the sm_aws_aarehouse navigation + * progressing in aws navigation + * minor + * several core improvements during navigation testing + * formatting improvements + * progress in aws navigation demo + * format improvements + * format improvements + * more on navigation + * new feature, cb_wait_topic_message: asynchronous client behavior that waits a topic message and optionally checks its contents to success + * formatting + * adding new client behavior add for nav2, wait nav2 nodes subscribing to the /bond topic and waiting they are alive. you optionally can select the nodes to wait + * progress in aws navigation demo + * minor format + * Feature/sm dance bot fixes (#93) + * base for the sm_aws_aarehouse navigation + * progressing in aws navigation + * minor + * several core improvements during navigation testing + * formatting improvements + * progress in aws navigation demo + * format improvements + * format improvements + * more on navigation + * new feature, cb_wait_topic_message: asynchronous client behavior that waits a topic message and optionally checks its contents to success + * formatting + * adding new client behavior add for nav2, wait nav2 nodes subscribing to the /bond topic and waiting they are alive. you optionally can select the nodes to wait + * progress in aws navigation demo + * minor format + * navigation parameters fixes on sm_dance_bot + * minor format + * Feature/sm aws warehouse (#94) + * base for the sm_aws_aarehouse navigation + * progressing in aws navigation + * minor + * several core improvements during navigation testing + * formatting improvements + * progress in aws navigation demo + * format improvements + * format improvements + * more on navigation + * new feature, cb_wait_topic_message: asynchronous client behavior that waits a topic message and optionally checks its contents to success + * formatting + * adding new client behavior add for nav2, wait nav2 nodes subscribing to the /bond topic and waiting they are alive. you optionally can select the nodes to wait + * progress in aws navigation demo + * minor format + * merge and progress + * fix format + * Feature/sm dance bot fixes (#95) + * base for the sm_aws_aarehouse navigation + * progressing in aws navigation + * minor + * several core improvements during navigation testing + * formatting improvements + * progress in aws navigation demo + * format improvements + * format improvements + * more on navigation + * new feature, cb_wait_topic_message: asynchronous client behavior that waits a topic message and optionally checks its contents to success + * formatting + * adding new client behavior add for nav2, wait nav2 nodes subscribing to the /bond topic and waiting they are alive. you optionally can select the nodes to wait + * progress in aws navigation demo + * minor format + * navigation parameters fixes on sm_dance_bot + * minor format + * minor + * minor format + * Remove some compile warnings. (#96) + * Feature/cb pause slam (#98) + * base for the sm_aws_aarehouse navigation + * progressing in aws navigation + * minor + * several core improvements during navigation testing + * formatting improvements + * progress in aws navigation demo + * format improvements + * format improvements + * more on navigation + * new feature, cb_wait_topic_message: asynchronous client behavior that waits a topic message and optionally checks its contents to success + * formatting + * adding new client behavior add for nav2, wait nav2 nodes subscribing to the /bond topic and waiting they are alive. you optionally can select the nodes to wait + * progress in aws navigation demo + * minor format + * navigation parameters fixes on sm_dance_bot + * minor format + * minor + * formatting + * cb pause slam client behavior + * sm_dance_bot_lite (#99) + * sm_dance_bot_lite + * precommit + * Updates yaml + Co-authored-by: Ubuntu 20-04-02-amd64 + * Rename doxygen deployment workflow (#100) + * minor hotfix + * sm_dance_bot visualizing turtlebot3 (#101) + * Feature/dance bot launch gz lidar choice (#102) + * sm_dance_bot visualizing turtlebot3 + * cleaning and lidar show/hide option + * cleaning files and making formatting work + * more fixes + * Feature/sm dance bot lite gazebo fixes (#104) + * sm_dance_bot visualizing turtlebot3 + * cleaning and lidar show/hide option + * cleaning files and making formatting work + * more fixes + * gazebo fixes, to show the robot and the lidar + * format fixes + * sm_multi_stage_1 doubling (#103) + Co-authored-by: Ubuntu 20-04-02-amd64 + * Feature/sm dance bot strikes back gazebo fixes (#105) + * sm_dance_bot visualizing turtlebot3 + * cleaning and lidar show/hide option + * cleaning files and making formatting work + * more fixes + * gazebo fixes, to show the robot and the lidar + * format fixes + * gazebo fixes for sm_dance_bot_strikes_back + * precommit cleanup run (#106) + Co-authored-by: Ubuntu 20-04-02-amd64 + * aws demo (#108) + * aws demo + * format + * got sm_multi_stage_1 working (barely) (#109) + Co-authored-by: Ubuntu 20-04-02-amd64 + * Brettpac branch (#110) + * got sm_multi_stage_1 working (barely) + * gaining traction sm_multi_stage_1 + * more + * don't remember + * making progress + * More + * keep hammering + * two stages + * 3 part + * 4th stage + Co-authored-by: Ubuntu 20-04-02-amd64 + * Brettpac branch (#111) + * got sm_multi_stage_1 working (barely) + * gaining traction sm_multi_stage_1 + * more + * don't remember + * making progress + * More + * keep hammering + * two stages + * 3 part + * 4th stage + * 5th stage + Co-authored-by: Ubuntu 20-04-02-amd64 + * a3 (#113) + Co-authored-by: Ubuntu 20-04-02-amd64 + * Remove neo_simulation2 package. (#112) + * Remove neo_simulation2 package. + * Correct formatting. + * Enable source build on PR for testing. + * Adjust build packages of source CI + * more sm_multi_stage_1 (#114) + Co-authored-by: Ubuntu 20-04-02-amd64 + * mm (#115) + Co-authored-by: Ubuntu 20-04-02-amd64 + * diverse improvements navigation and performance (#116) + * diverse improvements navigation and performance + * minor + Co-authored-by: pabloinigoblasco + * Feature/diverse improvemets navigation performance (#117) + * diverse improvements navigation and performance + * minor + * additional linting and formatting + * Remove merge markers from a python file. (#119) + * Feature/slam toggle and smacc deep history (#122) + * progress in navigation, slam toggle client behaviors and slam_toolbox components. Also smacc2::deep_history syntax + * going forward in testing sm_dance_bot introducing slam pausing/resuming funcionality + * feature/more_sm_dance_bot_fixes + * minor format + * minor (#124) + Co-authored-by: Ubuntu 20-04-02-amd64 + * more changes in sm_dance_bot (#125) + * Move method after the method it calls. Otherwise recursion could happen. (#126) + * Feature/dance bot s pattern (#128) + * more changes in sm_dance_bot + * polishing sm_dance_bot and s-pattern + * noticed typo + Finnaly > Finally + * Feature/dance bot s pattern (#129) + * more changes in sm_dance_bot + * polishing sm_dance_bot and s-pattern + * more refinement in sm_dance_bot + * First working version of sm template and template generator. (#127) + * minor tweaks (#130) + Co-authored-by: Ubuntu 20-04-02-amd64 + * Feature/sm dance bot refine (#131) + * more changes in sm_dance_bot + * minor + * Feature/sm dance bot refine 2 (#132) + * more changes in sm_dance_bot + * minor + * build fix + * waypoints navigator bug (#133) + * minor tuning to mitigate overshot issue cases + * progress in the sm_dance_bot tests (#135) + * some more progress on markers cleanup + * minor format issues (#134) + * sm_dance_bot_lite (#136) + Co-authored-by: Ubuntu 20-04-02-amd64 + * Resolve compile wanings (#137) + * Add SM core test (#138) + * minor navigation improvements (#141) + * using local action msgs (#139) + * using local action msgs + * removing sm_dance_bot_msgs + * pending references + * Feature/nitrosz renaming (#144) + * using local action msgs + * removing sm_dance_bot_msgs + * pending references + * navigation 2 stack renaming + * formatting + * added SVGs to READMEs of atomic, dance_bot, and others (#140) + * added remaining SVGs to READMEs (#145) + * added remaining SVGs to READMEs + * precommit cleanup + * Update package list. (#142) + * removing parameters smacc (#147) + * removing parameters smacc + * workflows update + * workflow + * Noticed launch command was incorrect in README.md + fixed launch command for sm_dance_bot_strikes_back and removed some comments I had made in the past. + * Fix CI: format fix python version (#148) + * Add SM Atomic SM generator. (#143) + * Remove node creation and create only a logger. (#149) + * Rolling Docker environment to be executed from any environment (#154) + * Feature/sm dance bot strikes back refactoring (#152) + Co-authored-by: DecDury + Co-authored-by: Denis Štogl + * slight waypoint 4 and iterations changes so robot can complete course (#155) + * Feature/migration moveit client (#151) + * initial migration to smacc2 + * fixing some errors introduced on formatting + * missing dependency + * fixing some more linting warnings + * minor + * removing test from main moveit cmake + * test ur5 + * progressing in the moveit migration testing + * updating format + * adding .reps dependencies and also fixing some build errors + * repos dependency + * adding dependency to ur5 client + * docker refactoring + * minor + * progress on move_it PR + * minor dockerfile test workaround + * improving dockerfile for building local tests + * minor + * fixing compiling issues + * update readme (#164) + * update readme + * more readme updates + * more + Co-authored-by: Ubuntu 20-04-02-amd64 + * initial state machine transition timestamp (#165) + * moved reference library SMs to smacc2_performance_tools (#166) + * moved reference library SMs to smacc2_performance_tools + * pre-commit cleanup + * Add QOS durability to SmaccPublisherClient (#163) + * feat: add qos durability to SmaccPublisherClient + * fix: add a missing colon + * refactor: remove line + * feat: add reliability qos config + * Feature/testing moveit behaviors (#167) + * more testing on moveit + * progress on moveit + * more testing on moveit behaviors + * minor configuration + * fixing pipeline error + * fixing broken master build + * sm_pubsub_1 (#169) + Co-authored-by: Ubuntu 20-04-02-amd64 + * sm_pubsub_1 part 2 (#170) + * sm_pubsub_1 part 2 + * sm_pubsub_1 part 2 + Co-authored-by: Ubuntu 20-04-02-amd64 + * sm_advanced_recovery_1 renaming (#171) + Co-authored-by: Ubuntu 20-04-02-amd64 + * sm_multi_stage_1 reworking (#172) + * multistage modes + * sm_multi_stage sequences + * sm_multi_state_1 steps + * sm_multi_stage_1 sequence d + * sm_multi_stage_1 c sequence + * mode_5_sequence_b + * mode_4_sequence_b + * sm_multi_stage_1 most + * finishing touches 1 + * readme + Co-authored-by: Ubuntu 20-04-02-amd64 + * Feature/aws navigation sm dance bot (#174) + * repo dependency + * husky launch file in sm_dance_bot + * Add dependencies for husky simulation. + * Fix formatting. + * Update dependencies for husky in rolling and galactic. + * minor + * progress on aws navigation and some other refactorings on navigation clients and behaviors + * more on aws demo + * fixing broken build + * minor + Co-authored-by: Denis Štogl + Co-authored-by: Denis Štogl + * minor changes (#175) + * warehouse2 (#177) + * Waypoint Inputs (#178) + Co-authored-by: Ubuntu 20-04-02-amd64 + * wharehouse2 progress (#179) + * format (#180) + * sm_dance_bot_warehouse_3 (#181) + Co-authored-by: Ubuntu 20-04-02-amd64 + * Feature/sm warehouse 2 13 dec 2 (#182) + * format + * more changes and headless + * merge + * headless and other fixes + * default values + * Brettpac branch (#184) + * sm_dance_bot_warehouse_3 + * Redoing sm_dance_bot_warehouse_3 waypoints + * More Waypoints + Co-authored-by: Ubuntu 20-04-02-amd64 + * SrConditional fixes and formatting (#168) + * fix: some formatting and templating on SrConditional + * fix: move trigger logic into headers + * fix: lint + * Feature/wharehouse2 dec 14 (#185) + * warehouse2 + * minor + * Feature/sm warehouse 2 13 dec 2 (#186) + * format + * more changes and headless + * merge + * headless and other fixes + * default values + * minor + * finetuning waypoints (#187) + Co-authored-by: Ubuntu 20-04-02-amd64 + * Feature/cb pure spinning (#188) + * format + * more changes and headless + * merge + * headless and other fixes + * default values + * minor + * Feature/cb pure spinning (#189) + * format + * more changes and headless + * merge + * headless and other fixes + * default values + * minor + * pure spinning behavior missing files + * minor changes (#190) + * Feature/planner changes 16 12 (#191) + * minor changes + * more fixes + * minor + * minor + * Feature/replanning 16 dec (#193) + * minor changes + * replanning for all our examples + * several fixes (#194) + * minor changes (#195) + * Feature/undo motion 20 12 (#196) + * minor changes + * replanning for all our examples + * improving undo motion navigation warehouse2 + * minor + * tuning warehouse3 (#197) + * Feature/undo motion 20 12 (#198) + * minor changes + * replanning for all our examples + * improving undo motion navigation warehouse2 + * minor + * undo tuning and errors + * format + * Feature/sync 21 12 (#199) + * minor changes + * replanning for all our examples + * format issues + * Feature/warehouse2 22 12 (#200) + * minor changes + * replanning for all our examples + * format issues + * finishing warehouse2 + * Feature/warehouse2 23 12 (#201) + * minor changes + * replanning for all our examples + * tuning and fixes (#202) + * Feature/minor tune (#203) + * tuning and fixes + * minor tune + * fixing warehouse 3 problems, and other core improvements (#204) + * fixing warehouse 3 problems, and other core improvements to remove dead lock, also making continuous integration green + * weird moveit not downloaded repo + * added missing file from warehouse2 (#205) + * backport to foxy + * minor format + * minor linking errors foxy + Co-authored-by: Denis Štogl + Co-authored-by: Denis Štogl + Co-authored-by: Declan Dury <44791484+DecDury@users.noreply.github.com> + Co-authored-by: DecDury + Co-authored-by: reelrbtx + Co-authored-by: brettpac + Co-authored-by: David Revay + * missing + * missing sm + * updating subscriber publisher components + * progress in autowarrrrrrrrrrrrrrrrrrrrrrrrrrrrrrrrrre machine + * refining cp subscriber cp publisher + * improvements in smacc core adding more components mostly developed for autoware demo + * autoware demo + * missing + * foxy ci + * fix + * minor broken build + * some reordering fixes + * minor + * docker files for different revisions, warnings removval and more testing on navigation + * fixing docker for foxy and galactic + * Update file for fake hardware simulation and add file for gazebo simulation. + * docker build files for all versions + * retry behavior warehouse 1 + * missing file + * minor format fix + * other minor changes + Co-authored-by: Denis Štogl + Co-authored-by: Denis Štogl + Co-authored-by: Declan Dury <44791484+DecDury@users.noreply.github.com> + Co-authored-by: DecDury + Co-authored-by: reelrbtx + Co-authored-by: brettpac + Co-authored-by: David Revay + Co-authored-by: pabloinigoblasco + * fix broken source build (#227) + * Only rolling version should be pre-released on on master. (#230) + * Correct Focal-Rolling builds by fixing the version of rosdep yaml (#234) + * Update file for fake hardware simulation and add file for gazebo simulation. (#224) + * Update file for fake hardware simulation and add file for gazebo simulation. + * Add ignition file and update repos files. + * Feature/improvements warehouse3 (#228) + * minor changes + * replanning for all our examples + * backport to foxy + * minor format + * minor linking errors foxy + * Foxy backport (#206) + * minor formatting fixes + * Fix trailing spaces. + * Correct codespell. + * Correct python linters warnings. + * Add galactic CI build because Navigation2 is broken in rolling. + * Add partial changes for ament_cpplint. + * Add tf2_ros as dependency to find include. + * Disable ament_cpplint. + * Disable some packages and update workflows. + * Bump ccache version. + * Ignore further packages + * Satisfy ament_lint_cmake + * Add missing licences. + * Disable cpplint and cppcheck linters. + * Correct formatters. + * branching example + * Disable disabled packages + * Update ci-build-source.yml + * Change extension + * Change extension of imports. + * Enable cppcheck + * Correct formatting of python file. + * Included necessary package and edited Threesome launch + Changed... + ros2 launch sm_three_some sm_three_some + to + ros2 launch sm_three_some sm_three_some.launch + Added: + First ensure you have the necessary package installed. + ``` + sudo apt-get install ros-rolling-ros2trace + ``` + Then run this command. + * Rename header files and correct format. + * Add workflow for checking doc build. + * Update doxygen-check-build.yml + * Create doxygen-deploy.yml + * Use manual deployment for now. + * Create workflow for testing prerelease builds + * Use docs/ as source folder for documentation + * Use docs/ as output directory. + * Rename to smacc2 and smacc2_msgs + * Correct GitHub branch reference. + * Update name of package and package.xml to pass liter. + * Execute on master update + * Reset all versions to 0.0.0 + * Ignore all packages except smacc2 and smacc2_msgs + * Update changelogs + * 0.1.0 + * Revert "Ignore all packages except smacc2 and smacc2_msgs" + This reverts commit f603166a4b3ccdfe96c64d9f9fb9d8b49fbf0e61. + * Update description table. + * Update table + * Copy initial docs + * Dockerfile w/ ROS distro as argument + use this command "sudo docker build --build-arg ROS2_DISTRO=(desiredRosTag) (directoryHoldingDockerfile)/" + * Opened new folder for additional tracing contents + * Delete tracing directory + * Moved tracing.md to tracing directory + * added setupTracing.sh + Installs necessary packages and configures tracing group. + * Removed manual installation of ros-rolling-ros2trace + This is now automated in setupTracing.sh + location of sh file assumed if user follows README.md under "Getting started" + * Created alternative ManualTracing + * added new sm markdowns + * added a dockerfile for Rolling and Galactic + * Update smacc2_ci/docker/ros_rollingAndGalactic_ubuntu_20.04/buildGalactic.sh + Co-authored-by: Denis Štogl + * Update tracing/ManualTracing.md + Co-authored-by: Denis Štogl + * changed wording "smacc application" to "SMACC2 library" + Co-authored-by: Denis Štogl + * Update smacc_sm_reference_library/sm_atomic/README.md + edit from html to markdown syntax + Co-authored-by: Denis Štogl + * reactivating smacc2 nav clients for rolling via submodules + * renamed tracing events after + * bug in smacc2 component + * reverted markdowns to html + * added README tutorial for Dockerfile + * additional cleanup + * cleanup + * cleanup + * edited tracing.md to reflect new tracing event names + * Enable build of missing rolling repositories. + * Enable Navigation2 for semi-binary build. + * Remove galactic builds from master and kepp only rolling. Remove submodules and use .repos file + * updated mentions of SMACC/ROS to SMACC2/ROS2 + * some progress on navigation rolling + * renamed folders, deleted tracing.md, edited README.md + * added smacc2_performance_tools + * performance tests improvements + * more on performance and other issues + * sm_respira_1 format cleanup + * sm_respira_1 format cleanup pre-commit + * sm_respira_test_2 + * sm_respira_test_2 + * more changes on performance tests + * Do not execute clang-format on smacc2_sm_reference_library package. + * sm_reference_library reformatting + * Correct trailing spaces. + * sm_atomic_24hr + * sm_atomic_performance_trace_1 + * Update smacc2_rta command across readmes + * Clean up of sm_atomic_24hr + * more sm_atomic_24hr cleanup + * Optimized deps in move_base_z_planners_common. + * Renaming of event generator library + * minor formatting + * Add galactic CI setup and rename rolling files. (#58) + * Fix source CI and correct README overview. (#62) + * Update c_cpp_properties.json + * changed launch command to ros2 launch sm_respira_1 sm_respira_1.launch (#69) + also noticed a note I had made while producing these that was not removed + * update doxygen links (#70) + Co-authored-by: Ubuntu 20-04-02-amd64 + * More Readme Updates (#72) + Co-authored-by: Ubuntu 20-04-02-amd64 + * More Readme (#74) + Co-authored-by: Ubuntu 20-04-02-amd64 + * created new sm from sm_respira_1 (#76) + * Feature/core and navigation fixes (#78) + * base for the sm_aws_aarehouse navigation + * progressing in aws navigation + * minor + * several core improvements during navigation testing + * formatting improvements + * progress in aws navigation demo + * format improvements + * Feature/aws demo progress (#80) + * base for the sm_aws_aarehouse navigation + * progressing in aws navigation + * minor + * several core improvements during navigation testing + * formatting improvements + * progress in aws navigation demo + * format improvements + * format improvements + * more on navigation + * sm_advanced_recovery_1 reworked (#83) + * sm_advanced_recovery_1 reworked + * fix pre-commit + * Trying to fix Pre-Commit + Co-authored-by: Ubuntu 20-04-02-amd64 + * more sm_advanced_recovery_1 (#84) + Co-authored-by: Ubuntu 20-04-02-amd64 + * More sm_advanced_recovery_1 work (#85) + Co-authored-by: Ubuntu 20-04-02-amd64 + * sm_advanced_recovery_1 round 4 (#86) + Co-authored-by: Ubuntu 20-04-02-amd64 + * Brettpac branch (#87) + * sm_atomic_performance_test_a_2 + * sm_atomic_performance_test_a_1 + Co-authored-by: Ubuntu 20-04-02-amd64 + * sm_atomic_performance_test_c_1 (#88) + Co-authored-by: Ubuntu 20-04-02-amd64 + * modifying sm_atomic_performance_test_a_2 (#89) + Co-authored-by: Ubuntu 20-04-02-amd64 + * sm_multi_stage_1 (#90) + * sm_multi_stage_1 + * fixing precommit + Co-authored-by: Ubuntu 20-04-02-amd64 + * more sm_multi_stage_1 (#91) + Co-authored-by: Ubuntu 20-04-02-amd64 + * Update README.md + updated launch command + * Wait topic message client behavior (#81) + * base for the sm_aws_aarehouse navigation + * progressing in aws navigation + * minor + * several core improvements during navigation testing + * formatting improvements + * progress in aws navigation demo + * format improvements + * format improvements + * more on navigation + * new feature, cb_wait_topic_message: asynchronous client behavior that waits a topic message and optionally checks its contents to success + * formatting + * attempting precommit fixes + Co-authored-by: Ubuntu 20-04-02-amd64 + Co-authored-by: Denis Štogl + * Feature/wait nav2 nodes client behavior (#82) + * base for the sm_aws_aarehouse navigation + * progressing in aws navigation + * minor + * several core improvements during navigation testing + * formatting improvements + * progress in aws navigation demo + * format improvements + * format improvements + * more on navigation + * new feature, cb_wait_topic_message: asynchronous client behavior that waits a topic message and optionally checks its contents to success + * formatting + * adding new client behavior add for nav2, wait nav2 nodes subscribing to the /bond topic and waiting they are alive. you optionally can select the nodes to wait + * Correct all linters and formaters. + Co-authored-by: Denis Štogl + Co-authored-by: Denis Štogl + * Feature/aws demo progress (#92) + * base for the sm_aws_aarehouse navigation + * progressing in aws navigation + * minor + * several core improvements during navigation testing + * formatting improvements + * progress in aws navigation demo + * format improvements + * format improvements + * more on navigation + * new feature, cb_wait_topic_message: asynchronous client behavior that waits a topic message and optionally checks its contents to success + * formatting + * adding new client behavior add for nav2, wait nav2 nodes subscribing to the /bond topic and waiting they are alive. you optionally can select the nodes to wait + * progress in aws navigation demo + * minor format + * Feature/sm dance bot fixes (#93) + * base for the sm_aws_aarehouse navigation + * progressing in aws navigation + * minor + * several core improvements during navigation testing + * formatting improvements + * progress in aws navigation demo + * format improvements + * format improvements + * more on navigation + * new feature, cb_wait_topic_message: asynchronous client behavior that waits a topic message and optionally checks its contents to success + * formatting + * adding new client behavior add for nav2, wait nav2 nodes subscribing to the /bond topic and waiting they are alive. you optionally can select the nodes to wait + * progress in aws navigation demo + * minor format + * navigation parameters fixes on sm_dance_bot + * minor format + * Feature/sm aws warehouse (#94) + * base for the sm_aws_aarehouse navigation + * progressing in aws navigation + * minor + * several core improvements during navigation testing + * formatting improvements + * progress in aws navigation demo + * format improvements + * format improvements + * more on navigation + * new feature, cb_wait_topic_message: asynchronous client behavior that waits a topic message and optionally checks its contents to success + * formatting + * adding new client behavior add for nav2, wait nav2 nodes subscribing to the /bond topic and waiting they are alive. you optionally can select the nodes to wait + * progress in aws navigation demo + * minor format + * merge and progress + * fix format + * Feature/sm dance bot fixes (#95) + * base for the sm_aws_aarehouse navigation + * progressing in aws navigation + * minor + * several core improvements during navigation testing + * formatting improvements + * progress in aws navigation demo + * format improvements + * format improvements + * more on navigation + * new feature, cb_wait_topic_message: asynchronous client behavior that waits a topic message and optionally checks its contents to success + * formatting + * adding new client behavior add for nav2, wait nav2 nodes subscribing to the /bond topic and waiting they are alive. you optionally can select the nodes to wait + * progress in aws navigation demo + * minor format + * navigation parameters fixes on sm_dance_bot + * minor format + * minor + * minor format + * Remove some compile warnings. (#96) + * Feature/cb pause slam (#98) + * base for the sm_aws_aarehouse navigation + * progressing in aws navigation + * minor + * several core improvements during navigation testing + * formatting improvements + * progress in aws navigation demo + * format improvements + * format improvements + * more on navigation + * new feature, cb_wait_topic_message: asynchronous client behavior that waits a topic message and optionally checks its contents to success + * formatting + * adding new client behavior add for nav2, wait nav2 nodes subscribing to the /bond topic and waiting they are alive. you optionally can select the nodes to wait + * progress in aws navigation demo + * minor format + * navigation parameters fixes on sm_dance_bot + * minor format + * minor + * formatting + * cb pause slam client behavior + * sm_dance_bot_lite (#99) + * sm_dance_bot_lite + * precommit + * Updates yaml + Co-authored-by: Ubuntu 20-04-02-amd64 + * Rename doxygen deployment workflow (#100) + * minor hotfix + * sm_dance_bot visualizing turtlebot3 (#101) + * Feature/dance bot launch gz lidar choice (#102) + * sm_dance_bot visualizing turtlebot3 + * cleaning and lidar show/hide option + * cleaning files and making formatting work + * more fixes + * Feature/sm dance bot lite gazebo fixes (#104) + * sm_dance_bot visualizing turtlebot3 + * cleaning and lidar show/hide option + * cleaning files and making formatting work + * more fixes + * gazebo fixes, to show the robot and the lidar + * format fixes + * sm_multi_stage_1 doubling (#103) + Co-authored-by: Ubuntu 20-04-02-amd64 + * Feature/sm dance bot strikes back gazebo fixes (#105) + * sm_dance_bot visualizing turtlebot3 + * cleaning and lidar show/hide option + * cleaning files and making formatting work + * more fixes + * gazebo fixes, to show the robot and the lidar + * format fixes + * gazebo fixes for sm_dance_bot_strikes_back + * precommit cleanup run (#106) + Co-authored-by: Ubuntu 20-04-02-amd64 + * aws demo (#108) + * aws demo + * format + * got sm_multi_stage_1 working (barely) (#109) + Co-authored-by: Ubuntu 20-04-02-amd64 + * Brettpac branch (#110) + * got sm_multi_stage_1 working (barely) + * gaining traction sm_multi_stage_1 + * more + * don't remember + * making progress + * More + * keep hammering + * two stages + * 3 part + * 4th stage + Co-authored-by: Ubuntu 20-04-02-amd64 + * Brettpac branch (#111) + * got sm_multi_stage_1 working (barely) + * gaining traction sm_multi_stage_1 + * more + * don't remember + * making progress + * More + * keep hammering + * two stages + * 3 part + * 4th stage + * 5th stage + Co-authored-by: Ubuntu 20-04-02-amd64 + * a3 (#113) + Co-authored-by: Ubuntu 20-04-02-amd64 + * Remove neo_simulation2 package. (#112) + * Remove neo_simulation2 package. + * Correct formatting. + * Enable source build on PR for testing. + * Adjust build packages of source CI + * more sm_multi_stage_1 (#114) + Co-authored-by: Ubuntu 20-04-02-amd64 + * mm (#115) + Co-authored-by: Ubuntu 20-04-02-amd64 + * diverse improvements navigation and performance (#116) + * diverse improvements navigation and performance + * minor + Co-authored-by: pabloinigoblasco + * Feature/diverse improvemets navigation performance (#117) + * diverse improvements navigation and performance + * minor + * additional linting and formatting + * Remove merge markers from a python file. (#119) + * Feature/slam toggle and smacc deep history (#122) + * progress in navigation, slam toggle client behaviors and slam_toolbox components. Also smacc2::deep_history syntax + * going forward in testing sm_dance_bot introducing slam pausing/resuming funcionality + * feature/more_sm_dance_bot_fixes + * minor format + * minor (#124) + Co-authored-by: Ubuntu 20-04-02-amd64 + * more changes in sm_dance_bot (#125) + * Move method after the method it calls. Otherwise recursion could happen. (#126) + * Feature/dance bot s pattern (#128) + * more changes in sm_dance_bot + * polishing sm_dance_bot and s-pattern + * noticed typo + Finnaly > Finally + * Feature/dance bot s pattern (#129) + * more changes in sm_dance_bot + * polishing sm_dance_bot and s-pattern + * more refinement in sm_dance_bot + * First working version of sm template and template generator. (#127) + * minor tweaks (#130) + Co-authored-by: Ubuntu 20-04-02-amd64 + * Feature/sm dance bot refine (#131) + * more changes in sm_dance_bot + * minor + * Feature/sm dance bot refine 2 (#132) + * more changes in sm_dance_bot + * minor + * build fix + * waypoints navigator bug (#133) + * minor tuning to mitigate overshot issue cases + * progress in the sm_dance_bot tests (#135) + * some more progress on markers cleanup + * minor format issues (#134) + * sm_dance_bot_lite (#136) + Co-authored-by: Ubuntu 20-04-02-amd64 + * Resolve compile wanings (#137) + * Add SM core test (#138) + * minor navigation improvements (#141) + * using local action msgs (#139) + * using local action msgs + * removing sm_dance_bot_msgs + * pending references + * Feature/nitrosz renaming (#144) + * using local action msgs + * removing sm_dance_bot_msgs + * pending references + * navigation 2 stack renaming + * formatting + * added SVGs to READMEs of atomic, dance_bot, and others (#140) + * added remaining SVGs to READMEs (#145) + * added remaining SVGs to READMEs + * precommit cleanup + * Update package list. (#142) + * removing parameters smacc (#147) + * removing parameters smacc + * workflows update + * workflow + * Noticed launch command was incorrect in README.md + fixed launch command for sm_dance_bot_strikes_back and removed some comments I had made in the past. + * Fix CI: format fix python version (#148) + * Add SM Atomic SM generator. (#143) + * Remove node creation and create only a logger. (#149) + * Rolling Docker environment to be executed from any environment (#154) + * Feature/sm dance bot strikes back refactoring (#152) + Co-authored-by: DecDury + Co-authored-by: Denis Štogl + * slight waypoint 4 and iterations changes so robot can complete course (#155) + * Feature/migration moveit client (#151) + * initial migration to smacc2 + * fixing some errors introduced on formatting + * missing dependency + * fixing some more linting warnings + * minor + * removing test from main moveit cmake + * test ur5 + * progressing in the moveit migration testing + * updating format + * adding .reps dependencies and also fixing some build errors + * repos dependency + * adding dependency to ur5 client + * docker refactoring + * minor + * progress on move_it PR + * minor dockerfile test workaround + * improving dockerfile for building local tests + * minor + * fixing compiling issues + * update readme (#164) + * update readme + * more readme updates + * more + Co-authored-by: Ubuntu 20-04-02-amd64 + * initial state machine transition timestamp (#165) + * moved reference library SMs to smacc2_performance_tools (#166) + * moved reference library SMs to smacc2_performance_tools + * pre-commit cleanup + * Add QOS durability to SmaccPublisherClient (#163) + * feat: add qos durability to SmaccPublisherClient + * fix: add a missing colon + * refactor: remove line + * feat: add reliability qos config + * Feature/testing moveit behaviors (#167) + * more testing on moveit + * progress on moveit + * more testing on moveit behaviors + * minor configuration + * fixing pipeline error + * fixing broken master build + * sm_pubsub_1 (#169) + Co-authored-by: Ubuntu 20-04-02-amd64 + * sm_pubsub_1 part 2 (#170) + * sm_pubsub_1 part 2 + * sm_pubsub_1 part 2 + Co-authored-by: Ubuntu 20-04-02-amd64 + * sm_advanced_recovery_1 renaming (#171) + Co-authored-by: Ubuntu 20-04-02-amd64 + * sm_multi_stage_1 reworking (#172) + * multistage modes + * sm_multi_stage sequences + * sm_multi_state_1 steps + * sm_multi_stage_1 sequence d + * sm_multi_stage_1 c sequence + * mode_5_sequence_b + * mode_4_sequence_b + * sm_multi_stage_1 most + * finishing touches 1 + * readme + Co-authored-by: Ubuntu 20-04-02-amd64 + * Feature/aws navigation sm dance bot (#174) + * repo dependency + * husky launch file in sm_dance_bot + * Add dependencies for husky simulation. + * Fix formatting. + * Update dependencies for husky in rolling and galactic. + * minor + * progress on aws navigation and some other refactorings on navigation clients and behaviors + * more on aws demo + * fixing broken build + * minor + Co-authored-by: Denis Štogl + Co-authored-by: Denis Štogl + * minor changes (#175) + * warehouse2 (#177) + * Waypoint Inputs (#178) + Co-authored-by: Ubuntu 20-04-02-amd64 + * wharehouse2 progress (#179) + * format (#180) + * sm_dance_bot_warehouse_3 (#181) + Co-authored-by: Ubuntu 20-04-02-amd64 + * Feature/sm warehouse 2 13 dec 2 (#182) + * format + * more changes and headless + * merge + * headless and other fixes + * default values + * Brettpac branch (#184) + * sm_dance_bot_warehouse_3 + * Redoing sm_dance_bot_warehouse_3 waypoints + * More Waypoints + Co-authored-by: Ubuntu 20-04-02-amd64 + * SrConditional fixes and formatting (#168) + * fix: some formatting and templating on SrConditional + * fix: move trigger logic into headers + * fix: lint + * Feature/wharehouse2 dec 14 (#185) + * warehouse2 + * minor + * Feature/sm warehouse 2 13 dec 2 (#186) + * format + * more changes and headless + * merge + * headless and other fixes + * default values + * minor + * finetuning waypoints (#187) + Co-authored-by: Ubuntu 20-04-02-amd64 + * Feature/cb pure spinning (#188) + * format + * more changes and headless + * merge + * headless and other fixes + * default values + * minor + * Feature/cb pure spinning (#189) + * format + * more changes and headless + * merge + * headless and other fixes + * default values + * minor + * pure spinning behavior missing files + * minor changes (#190) + * Feature/planner changes 16 12 (#191) + * minor changes + * more fixes + * minor + * minor + * Feature/replanning 16 dec (#193) + * minor changes + * replanning for all our examples + * several fixes (#194) + * minor changes (#195) + * Feature/undo motion 20 12 (#196) + * minor changes + * replanning for all our examples + * improving undo motion navigation warehouse2 + * minor + * tuning warehouse3 (#197) + * Feature/undo motion 20 12 (#198) + * minor changes + * replanning for all our examples + * improving undo motion navigation warehouse2 + * minor + * undo tuning and errors + * format + * Feature/sync 21 12 (#199) + * minor changes + * replanning for all our examples + * format issues + * Feature/warehouse2 22 12 (#200) + * minor changes + * replanning for all our examples + * format issues + * finishing warehouse2 + * Feature/warehouse2 23 12 (#201) + * minor changes + * replanning for all our examples + * tuning and fixes (#202) + * Feature/minor tune (#203) + * tuning and fixes + * minor tune + * fixing warehouse 3 problems, and other core improvements (#204) + * fixing warehouse 3 problems, and other core improvements to remove dead lock, also making continuous integration green + * weird moveit not downloaded repo + * added missing file from warehouse2 (#205) + * backport to foxy + * minor format + * minor linking errors foxy + Co-authored-by: Denis Štogl + Co-authored-by: Denis Štogl + Co-authored-by: Declan Dury <44791484+DecDury@users.noreply.github.com> + Co-authored-by: DecDury + Co-authored-by: reelrbtx + Co-authored-by: brettpac + Co-authored-by: David Revay + * missing + * missing sm + * updating subscriber publisher components + * progress in autowarrrrrrrrrrrrrrrrrrrrrrrrrrrrrrrrrre machine + * refining cp subscriber cp publisher + * improvements in smacc core adding more components mostly developed for autoware demo + * autoware demo + * missing + * foxy ci + * fix + * minor broken build + * some reordering fixes + * minor + * docker files for different revisions, warnings removval and more testing on navigation + * fixing docker for foxy and galactic + * docker build files for all versions + * barrel demo + * barrel search build fix and warehouse3 + * fixing startup problems in warehouse 3 + * fixing format and minor + * minor + * progress in barrel husky + * minor + * barrel demo + * progress + * fixing broken build + Co-authored-by: Denis Štogl + Co-authored-by: Denis Štogl + Co-authored-by: Declan Dury <44791484+DecDury@users.noreply.github.com> + Co-authored-by: DecDury + Co-authored-by: reelrbtx + Co-authored-by: brettpac + Co-authored-by: David Revay + Co-authored-by: pabloinigoblasco + * Feature/barrel - do not merge yet (#233) + * minor changes + * replanning for all our examples + * backport to foxy + * minor format + * minor linking errors foxy + * Foxy backport (#206) + * minor formatting fixes + * Fix trailing spaces. + * Correct codespell. + * Correct python linters warnings. + * Add galactic CI build because Navigation2 is broken in rolling. + * Add partial changes for ament_cpplint. + * Add tf2_ros as dependency to find include. + * Disable ament_cpplint. + * Disable some packages and update workflows. + * Bump ccache version. + * Ignore further packages + * Satisfy ament_lint_cmake + * Add missing licences. + * Disable cpplint and cppcheck linters. + * Correct formatters. + * branching example + * Disable disabled packages + * Update ci-build-source.yml + * Change extension + * Change extension of imports. + * Enable cppcheck + * Correct formatting of python file. + * Included necessary package and edited Threesome launch + Changed... + ros2 launch sm_three_some sm_three_some + to + ros2 launch sm_three_some sm_three_some.launch + Added: + First ensure you have the necessary package installed. + ``` + sudo apt-get install ros-rolling-ros2trace + ``` + Then run this command. + * Rename header files and correct format. + * Add workflow for checking doc build. + * Update doxygen-check-build.yml + * Create doxygen-deploy.yml + * Use manual deployment for now. + * Create workflow for testing prerelease builds + * Use docs/ as source folder for documentation + * Use docs/ as output directory. + * Rename to smacc2 and smacc2_msgs + * Correct GitHub branch reference. + * Update name of package and package.xml to pass liter. + * Execute on master update + * Reset all versions to 0.0.0 + * Ignore all packages except smacc2 and smacc2_msgs + * Update changelogs + * 0.1.0 + * Revert "Ignore all packages except smacc2 and smacc2_msgs" + This reverts commit f603166a4b3ccdfe96c64d9f9fb9d8b49fbf0e61. + * Update description table. + * Update table + * Copy initial docs + * Dockerfile w/ ROS distro as argument + use this command "sudo docker build --build-arg ROS2_DISTRO=(desiredRosTag) (directoryHoldingDockerfile)/" + * Opened new folder for additional tracing contents + * Delete tracing directory + * Moved tracing.md to tracing directory + * added setupTracing.sh + Installs necessary packages and configures tracing group. + * Removed manual installation of ros-rolling-ros2trace + This is now automated in setupTracing.sh + location of sh file assumed if user follows README.md under "Getting started" + * Created alternative ManualTracing + * added new sm markdowns + * added a dockerfile for Rolling and Galactic + * Update smacc2_ci/docker/ros_rollingAndGalactic_ubuntu_20.04/buildGalactic.sh + Co-authored-by: Denis Štogl + * Update tracing/ManualTracing.md + Co-authored-by: Denis Štogl + * changed wording "smacc application" to "SMACC2 library" + Co-authored-by: Denis Štogl + * Update smacc_sm_reference_library/sm_atomic/README.md + edit from html to markdown syntax + Co-authored-by: Denis Štogl + * reactivating smacc2 nav clients for rolling via submodules + * renamed tracing events after + * bug in smacc2 component + * reverted markdowns to html + * added README tutorial for Dockerfile + * additional cleanup + * cleanup + * cleanup + * edited tracing.md to reflect new tracing event names + * Enable build of missing rolling repositories. + * Enable Navigation2 for semi-binary build. + * Remove galactic builds from master and kepp only rolling. Remove submodules and use .repos file + * updated mentions of SMACC/ROS to SMACC2/ROS2 + * some progress on navigation rolling + * renamed folders, deleted tracing.md, edited README.md + * added smacc2_performance_tools + * performance tests improvements + * more on performance and other issues + * sm_respira_1 format cleanup + * sm_respira_1 format cleanup pre-commit + * sm_respira_test_2 + * sm_respira_test_2 + * more changes on performance tests + * Do not execute clang-format on smacc2_sm_reference_library package. + * sm_reference_library reformatting + * Correct trailing spaces. + * sm_atomic_24hr + * sm_atomic_performance_trace_1 + * Update smacc2_rta command across readmes + * Clean up of sm_atomic_24hr + * more sm_atomic_24hr cleanup + * Optimized deps in move_base_z_planners_common. + * Renaming of event generator library + * minor formatting + * Add galactic CI setup and rename rolling files. (#58) + * Fix source CI and correct README overview. (#62) + * Update c_cpp_properties.json + * changed launch command to ros2 launch sm_respira_1 sm_respira_1.launch (#69) + also noticed a note I had made while producing these that was not removed + * update doxygen links (#70) + Co-authored-by: Ubuntu 20-04-02-amd64 + * More Readme Updates (#72) + Co-authored-by: Ubuntu 20-04-02-amd64 + * More Readme (#74) + Co-authored-by: Ubuntu 20-04-02-amd64 + * created new sm from sm_respira_1 (#76) + * Feature/core and navigation fixes (#78) + * base for the sm_aws_aarehouse navigation + * progressing in aws navigation + * minor + * several core improvements during navigation testing + * formatting improvements + * progress in aws navigation demo + * format improvements + * Feature/aws demo progress (#80) + * base for the sm_aws_aarehouse navigation + * progressing in aws navigation + * minor + * several core improvements during navigation testing + * formatting improvements + * progress in aws navigation demo + * format improvements + * format improvements + * more on navigation + * sm_advanced_recovery_1 reworked (#83) + * sm_advanced_recovery_1 reworked + * fix pre-commit + * Trying to fix Pre-Commit + Co-authored-by: Ubuntu 20-04-02-amd64 + * more sm_advanced_recovery_1 (#84) + Co-authored-by: Ubuntu 20-04-02-amd64 + * More sm_advanced_recovery_1 work (#85) + Co-authored-by: Ubuntu 20-04-02-amd64 + * sm_advanced_recovery_1 round 4 (#86) + Co-authored-by: Ubuntu 20-04-02-amd64 + * Brettpac branch (#87) + * sm_atomic_performance_test_a_2 + * sm_atomic_performance_test_a_1 + Co-authored-by: Ubuntu 20-04-02-amd64 + * sm_atomic_performance_test_c_1 (#88) + Co-authored-by: Ubuntu 20-04-02-amd64 + * modifying sm_atomic_performance_test_a_2 (#89) + Co-authored-by: Ubuntu 20-04-02-amd64 + * sm_multi_stage_1 (#90) + * sm_multi_stage_1 + * fixing precommit + Co-authored-by: Ubuntu 20-04-02-amd64 + * more sm_multi_stage_1 (#91) + Co-authored-by: Ubuntu 20-04-02-amd64 + * Update README.md + updated launch command + * Wait topic message client behavior (#81) + * base for the sm_aws_aarehouse navigation + * progressing in aws navigation + * minor + * several core improvements during navigation testing + * formatting improvements + * progress in aws navigation demo + * format improvements + * format improvements + * more on navigation + * new feature, cb_wait_topic_message: asynchronous client behavior that waits a topic message and optionally checks its contents to success + * formatting + * attempting precommit fixes + Co-authored-by: Ubuntu 20-04-02-amd64 + Co-authored-by: Denis Štogl + * Feature/wait nav2 nodes client behavior (#82) + * base for the sm_aws_aarehouse navigation + * progressing in aws navigation + * minor + * several core improvements during navigation testing + * formatting improvements + * progress in aws navigation demo + * format improvements + * format improvements + * more on navigation + * new feature, cb_wait_topic_message: asynchronous client behavior that waits a topic message and optionally checks its contents to success + * formatting + * adding new client behavior add for nav2, wait nav2 nodes subscribing to the /bond topic and waiting they are alive. you optionally can select the nodes to wait + * Correct all linters and formaters. + Co-authored-by: Denis Štogl + Co-authored-by: Denis Štogl + * Feature/aws demo progress (#92) + * base for the sm_aws_aarehouse navigation + * progressing in aws navigation + * minor + * several core improvements during navigation testing + * formatting improvements + * progress in aws navigation demo + * format improvements + * format improvements + * more on navigation + * new feature, cb_wait_topic_message: asynchronous client behavior that waits a topic message and optionally checks its contents to success + * formatting + * adding new client behavior add for nav2, wait nav2 nodes subscribing to the /bond topic and waiting they are alive. you optionally can select the nodes to wait + * progress in aws navigation demo + * minor format + * Feature/sm dance bot fixes (#93) + * base for the sm_aws_aarehouse navigation + * progressing in aws navigation + * minor + * several core improvements during navigation testing + * formatting improvements + * progress in aws navigation demo + * format improvements + * format improvements + * more on navigation + * new feature, cb_wait_topic_message: asynchronous client behavior that waits a topic message and optionally checks its contents to success + * formatting + * adding new client behavior add for nav2, wait nav2 nodes subscribing to the /bond topic and waiting they are alive. you optionally can select the nodes to wait + * progress in aws navigation demo + * minor format + * navigation parameters fixes on sm_dance_bot + * minor format + * Feature/sm aws warehouse (#94) + * base for the sm_aws_aarehouse navigation + * progressing in aws navigation + * minor + * several core improvements during navigation testing + * formatting improvements + * progress in aws navigation demo + * format improvements + * format improvements + * more on navigation + * new feature, cb_wait_topic_message: asynchronous client behavior that waits a topic message and optionally checks its contents to success + * formatting + * adding new client behavior add for nav2, wait nav2 nodes subscribing to the /bond topic and waiting they are alive. you optionally can select the nodes to wait + * progress in aws navigation demo + * minor format + * merge and progress + * fix format + * Feature/sm dance bot fixes (#95) + * base for the sm_aws_aarehouse navigation + * progressing in aws navigation + * minor + * several core improvements during navigation testing + * formatting improvements + * progress in aws navigation demo + * format improvements + * format improvements + * more on navigation + * new feature, cb_wait_topic_message: asynchronous client behavior that waits a topic message and optionally checks its contents to success + * formatting + * adding new client behavior add for nav2, wait nav2 nodes subscribing to the /bond topic and waiting they are alive. you optionally can select the nodes to wait + * progress in aws navigation demo + * minor format + * navigation parameters fixes on sm_dance_bot + * minor format + * minor + * minor format + * Remove some compile warnings. (#96) + * Feature/cb pause slam (#98) + * base for the sm_aws_aarehouse navigation + * progressing in aws navigation + * minor + * several core improvements during navigation testing + * formatting improvements + * progress in aws navigation demo + * format improvements + * format improvements + * more on navigation + * new feature, cb_wait_topic_message: asynchronous client behavior that waits a topic message and optionally checks its contents to success + * formatting + * adding new client behavior add for nav2, wait nav2 nodes subscribing to the /bond topic and waiting they are alive. you optionally can select the nodes to wait + * progress in aws navigation demo + * minor format + * navigation parameters fixes on sm_dance_bot + * minor format + * minor + * formatting + * cb pause slam client behavior + * sm_dance_bot_lite (#99) + * sm_dance_bot_lite + * precommit + * Updates yaml + Co-authored-by: Ubuntu 20-04-02-amd64 + * Rename doxygen deployment workflow (#100) + * minor hotfix + * sm_dance_bot visualizing turtlebot3 (#101) + * Feature/dance bot launch gz lidar choice (#102) + * sm_dance_bot visualizing turtlebot3 + * cleaning and lidar show/hide option + * cleaning files and making formatting work + * more fixes + * Feature/sm dance bot lite gazebo fixes (#104) + * sm_dance_bot visualizing turtlebot3 + * cleaning and lidar show/hide option + * cleaning files and making formatting work + * more fixes + * gazebo fixes, to show the robot and the lidar + * format fixes + * sm_multi_stage_1 doubling (#103) + Co-authored-by: Ubuntu 20-04-02-amd64 + * Feature/sm dance bot strikes back gazebo fixes (#105) + * sm_dance_bot visualizing turtlebot3 + * cleaning and lidar show/hide option + * cleaning files and making formatting work + * more fixes + * gazebo fixes, to show the robot and the lidar + * format fixes + * gazebo fixes for sm_dance_bot_strikes_back + * precommit cleanup run (#106) + Co-authored-by: Ubuntu 20-04-02-amd64 + * aws demo (#108) + * aws demo + * format + * got sm_multi_stage_1 working (barely) (#109) + Co-authored-by: Ubuntu 20-04-02-amd64 + * Brettpac branch (#110) + * got sm_multi_stage_1 working (barely) + * gaining traction sm_multi_stage_1 + * more + * don't remember + * making progress + * More + * keep hammering + * two stages + * 3 part + * 4th stage + Co-authored-by: Ubuntu 20-04-02-amd64 + * Brettpac branch (#111) + * got sm_multi_stage_1 working (barely) + * gaining traction sm_multi_stage_1 + * more + * don't remember + * making progress + * More + * keep hammering + * two stages + * 3 part + * 4th stage + * 5th stage + Co-authored-by: Ubuntu 20-04-02-amd64 + * a3 (#113) + Co-authored-by: Ubuntu 20-04-02-amd64 + * Remove neo_simulation2 package. (#112) + * Remove neo_simulation2 package. + * Correct formatting. + * Enable source build on PR for testing. + * Adjust build packages of source CI + * more sm_multi_stage_1 (#114) + Co-authored-by: Ubuntu 20-04-02-amd64 + * mm (#115) + Co-authored-by: Ubuntu 20-04-02-amd64 + * diverse improvements navigation and performance (#116) + * diverse improvements navigation and performance + * minor + Co-authored-by: pabloinigoblasco + * Feature/diverse improvemets navigation performance (#117) + * diverse improvements navigation and performance + * minor + * additional linting and formatting + * Remove merge markers from a python file. (#119) + * Feature/slam toggle and smacc deep history (#122) + * progress in navigation, slam toggle client behaviors and slam_toolbox components. Also smacc2::deep_history syntax + * going forward in testing sm_dance_bot introducing slam pausing/resuming funcionality + * feature/more_sm_dance_bot_fixes + * minor format + * minor (#124) + Co-authored-by: Ubuntu 20-04-02-amd64 + * more changes in sm_dance_bot (#125) + * Move method after the method it calls. Otherwise recursion could happen. (#126) + * Feature/dance bot s pattern (#128) + * more changes in sm_dance_bot + * polishing sm_dance_bot and s-pattern + * noticed typo + Finnaly > Finally + * Feature/dance bot s pattern (#129) + * more changes in sm_dance_bot + * polishing sm_dance_bot and s-pattern + * more refinement in sm_dance_bot + * First working version of sm template and template generator. (#127) + * minor tweaks (#130) + Co-authored-by: Ubuntu 20-04-02-amd64 + * Feature/sm dance bot refine (#131) + * more changes in sm_dance_bot + * minor + * Feature/sm dance bot refine 2 (#132) + * more changes in sm_dance_bot + * minor + * build fix + * waypoints navigator bug (#133) + * minor tuning to mitigate overshot issue cases + * progress in the sm_dance_bot tests (#135) + * some more progress on markers cleanup + * minor format issues (#134) + * sm_dance_bot_lite (#136) + Co-authored-by: Ubuntu 20-04-02-amd64 + * Resolve compile wanings (#137) + * Add SM core test (#138) + * minor navigation improvements (#141) + * using local action msgs (#139) + * using local action msgs + * removing sm_dance_bot_msgs + * pending references + * Feature/nitrosz renaming (#144) + * using local action msgs + * removing sm_dance_bot_msgs + * pending references + * navigation 2 stack renaming + * formatting + * added SVGs to READMEs of atomic, dance_bot, and others (#140) + * added remaining SVGs to READMEs (#145) + * added remaining SVGs to READMEs + * precommit cleanup + * Update package list. (#142) + * removing parameters smacc (#147) + * removing parameters smacc + * workflows update + * workflow + * Noticed launch command was incorrect in README.md + fixed launch command for sm_dance_bot_strikes_back and removed some comments I had made in the past. + * Fix CI: format fix python version (#148) + * Add SM Atomic SM generator. (#143) + * Remove node creation and create only a logger. (#149) + * Rolling Docker environment to be executed from any environment (#154) + * Feature/sm dance bot strikes back refactoring (#152) + Co-authored-by: DecDury + Co-authored-by: Denis Štogl + * slight waypoint 4 and iterations changes so robot can complete course (#155) + * Feature/migration moveit client (#151) + * initial migration to smacc2 + * fixing some errors introduced on formatting + * missing dependency + * fixing some more linting warnings + * minor + * removing test from main moveit cmake + * test ur5 + * progressing in the moveit migration testing + * updating format + * adding .reps dependencies and also fixing some build errors + * repos dependency + * adding dependency to ur5 client + * docker refactoring + * minor + * progress on move_it PR + * minor dockerfile test workaround + * improving dockerfile for building local tests + * minor + * fixing compiling issues + * update readme (#164) + * update readme + * more readme updates + * more + Co-authored-by: Ubuntu 20-04-02-amd64 + * initial state machine transition timestamp (#165) + * moved reference library SMs to smacc2_performance_tools (#166) + * moved reference library SMs to smacc2_performance_tools + * pre-commit cleanup + * Add QOS durability to SmaccPublisherClient (#163) + * feat: add qos durability to SmaccPublisherClient + * fix: add a missing colon + * refactor: remove line + * feat: add reliability qos config + * Feature/testing moveit behaviors (#167) + * more testing on moveit + * progress on moveit + * more testing on moveit behaviors + * minor configuration + * fixing pipeline error + * fixing broken master build + * sm_pubsub_1 (#169) + Co-authored-by: Ubuntu 20-04-02-amd64 + * sm_pubsub_1 part 2 (#170) + * sm_pubsub_1 part 2 + * sm_pubsub_1 part 2 + Co-authored-by: Ubuntu 20-04-02-amd64 + * sm_advanced_recovery_1 renaming (#171) + Co-authored-by: Ubuntu 20-04-02-amd64 + * sm_multi_stage_1 reworking (#172) + * multistage modes + * sm_multi_stage sequences + * sm_multi_state_1 steps + * sm_multi_stage_1 sequence d + * sm_multi_stage_1 c sequence + * mode_5_sequence_b + * mode_4_sequence_b + * sm_multi_stage_1 most + * finishing touches 1 + * readme + Co-authored-by: Ubuntu 20-04-02-amd64 + * Feature/aws navigation sm dance bot (#174) + * repo dependency + * husky launch file in sm_dance_bot + * Add dependencies for husky simulation. + * Fix formatting. + * Update dependencies for husky in rolling and galactic. + * minor + * progress on aws navigation and some other refactorings on navigation clients and behaviors + * more on aws demo + * fixing broken build + * minor + Co-authored-by: Denis Štogl + Co-authored-by: Denis Štogl + * minor changes (#175) + * warehouse2 (#177) + * Waypoint Inputs (#178) + Co-authored-by: Ubuntu 20-04-02-amd64 + * wharehouse2 progress (#179) + * format (#180) + * sm_dance_bot_warehouse_3 (#181) + Co-authored-by: Ubuntu 20-04-02-amd64 + * Feature/sm warehouse 2 13 dec 2 (#182) + * format + * more changes and headless + * merge + * headless and other fixes + * default values + * Brettpac branch (#184) + * sm_dance_bot_warehouse_3 + * Redoing sm_dance_bot_warehouse_3 waypoints + * More Waypoints + Co-authored-by: Ubuntu 20-04-02-amd64 + * SrConditional fixes and formatting (#168) + * fix: some formatting and templating on SrConditional + * fix: move trigger logic into headers + * fix: lint + * Feature/wharehouse2 dec 14 (#185) + * warehouse2 + * minor + * Feature/sm warehouse 2 13 dec 2 (#186) + * format + * more changes and headless + * merge + * headless and other fixes + * default values + * minor + * finetuning waypoints (#187) + Co-authored-by: Ubuntu 20-04-02-amd64 + * Feature/cb pure spinning (#188) + * format + * more changes and headless + * merge + * headless and other fixes + * default values + * minor + * Feature/cb pure spinning (#189) + * format + * more changes and headless + * merge + * headless and other fixes + * default values + * minor + * pure spinning behavior missing files + * minor changes (#190) + * Feature/planner changes 16 12 (#191) + * minor changes + * more fixes + * minor + * minor + * Feature/replanning 16 dec (#193) + * minor changes + * replanning for all our examples + * several fixes (#194) + * minor changes (#195) + * Feature/undo motion 20 12 (#196) + * minor changes + * replanning for all our examples + * improving undo motion navigation warehouse2 + * minor + * tuning warehouse3 (#197) + * Feature/undo motion 20 12 (#198) + * minor changes + * replanning for all our examples + * improving undo motion navigation warehouse2 + * minor + * undo tuning and errors + * format + * Feature/sync 21 12 (#199) + * minor changes + * replanning for all our examples + * format issues + * Feature/warehouse2 22 12 (#200) + * minor changes + * replanning for all our examples + * format issues + * finishing warehouse2 + * Feature/warehouse2 23 12 (#201) + * minor changes + * replanning for all our examples + * tuning and fixes (#202) + * Feature/minor tune (#203) + * tuning and fixes + * minor tune + * fixing warehouse 3 problems, and other core improvements (#204) + * fixing warehouse 3 problems, and other core improvements to remove dead lock, also making continuous integration green + * weird moveit not downloaded repo + * added missing file from warehouse2 (#205) + * backport to foxy + * minor format + * minor linking errors foxy + Co-authored-by: Denis Štogl + Co-authored-by: Denis Štogl + Co-authored-by: Declan Dury <44791484+DecDury@users.noreply.github.com> + Co-authored-by: DecDury + Co-authored-by: reelrbtx + Co-authored-by: brettpac + Co-authored-by: David Revay + * missing + * missing sm + * updating subscriber publisher components + * progress in autowarrrrrrrrrrrrrrrrrrrrrrrrrrrrrrrrrre machine + * refining cp subscriber cp publisher + * improvements in smacc core adding more components mostly developed for autoware demo + * autoware demo + * missing + * foxy ci + * fix + * minor broken build + * some reordering fixes + * minor + * docker files for different revisions, warnings removval and more testing on navigation + * fixing docker for foxy and galactic + * docker build files for all versions + * barrel demo + * barrel search build fix and warehouse3 + * fixing startup problems in warehouse 3 + * fixing format and minor + * minor + * progress in barrel husky + * minor + * barrel demo + * minor + * barrel search updates + * making models local + * red picuup + * multiple controllable leds plugin + * progress in husky demo + * progressing in husky demo + * improving navigation behaviors + * more merge + Co-authored-by: Denis Štogl + Co-authored-by: Denis Štogl + Co-authored-by: Declan Dury <44791484+DecDury@users.noreply.github.com> + Co-authored-by: DecDury + Co-authored-by: reelrbtx + Co-authored-by: brettpac + Co-authored-by: David Revay + Co-authored-by: pabloinigoblasco + * Feature/docker improvements march 2022 (#235) + * minor changes + * replanning for all our examples + * backport to foxy + * minor format + * minor linking errors foxy + * Foxy backport (#206) + * minor formatting fixes + * Fix trailing spaces. + * Correct codespell. + * Correct python linters warnings. + * Add galactic CI build because Navigation2 is broken in rolling. + * Add partial changes for ament_cpplint. + * Add tf2_ros as dependency to find include. + * Disable ament_cpplint. + * Disable some packages and update workflows. + * Bump ccache version. + * Ignore further packages + * Satisfy ament_lint_cmake + * Add missing licences. + * Disable cpplint and cppcheck linters. + * Correct formatters. + * branching example + * Disable disabled packages + * Update ci-build-source.yml + * Change extension + * Change extension of imports. + * Enable cppcheck + * Correct formatting of python file. + * Included necessary package and edited Threesome launch + Changed... + ros2 launch sm_three_some sm_three_some + to + ros2 launch sm_three_some sm_three_some.launch + Added: + First ensure you have the necessary package installed. + ``` + sudo apt-get install ros-rolling-ros2trace + ``` + Then run this command. + * Rename header files and correct format. + * Add workflow for checking doc build. + * Update doxygen-check-build.yml + * Create doxygen-deploy.yml + * Use manual deployment for now. + * Create workflow for testing prerelease builds + * Use docs/ as source folder for documentation + * Use docs/ as output directory. + * Rename to smacc2 and smacc2_msgs + * Correct GitHub branch reference. + * Update name of package and package.xml to pass liter. + * Execute on master update + * Reset all versions to 0.0.0 + * Ignore all packages except smacc2 and smacc2_msgs + * Update changelogs + * 0.1.0 + * Revert "Ignore all packages except smacc2 and smacc2_msgs" + This reverts commit f603166a4b3ccdfe96c64d9f9fb9d8b49fbf0e61. + * Update description table. + * Update table + * Copy initial docs + * Dockerfile w/ ROS distro as argument + use this command "sudo docker build --build-arg ROS2_DISTRO=(desiredRosTag) (directoryHoldingDockerfile)/" + * Opened new folder for additional tracing contents + * Delete tracing directory + * Moved tracing.md to tracing directory + * added setupTracing.sh + Installs necessary packages and configures tracing group. + * Removed manual installation of ros-rolling-ros2trace + This is now automated in setupTracing.sh + location of sh file assumed if user follows README.md under "Getting started" + * Created alternative ManualTracing + * added new sm markdowns + * added a dockerfile for Rolling and Galactic + * Update smacc2_ci/docker/ros_rollingAndGalactic_ubuntu_20.04/buildGalactic.sh + Co-authored-by: Denis Štogl + * Update tracing/ManualTracing.md + Co-authored-by: Denis Štogl + * changed wording "smacc application" to "SMACC2 library" + Co-authored-by: Denis Štogl + * Update smacc_sm_reference_library/sm_atomic/README.md + edit from html to markdown syntax + Co-authored-by: Denis Štogl + * reactivating smacc2 nav clients for rolling via submodules + * renamed tracing events after + * bug in smacc2 component + * reverted markdowns to html + * added README tutorial for Dockerfile + * additional cleanup + * cleanup + * cleanup + * edited tracing.md to reflect new tracing event names + * Enable build of missing rolling repositories. + * Enable Navigation2 for semi-binary build. + * Remove galactic builds from master and kepp only rolling. Remove submodules and use .repos file + * updated mentions of SMACC/ROS to SMACC2/ROS2 + * some progress on navigation rolling + * renamed folders, deleted tracing.md, edited README.md + * added smacc2_performance_tools + * performance tests improvements + * more on performance and other issues + * sm_respira_1 format cleanup + * sm_respira_1 format cleanup pre-commit + * sm_respira_test_2 + * sm_respira_test_2 + * more changes on performance tests + * Do not execute clang-format on smacc2_sm_reference_library package. + * sm_reference_library reformatting + * Correct trailing spaces. + * sm_atomic_24hr + * sm_atomic_performance_trace_1 + * Update smacc2_rta command across readmes + * Clean up of sm_atomic_24hr + * more sm_atomic_24hr cleanup + * Optimized deps in move_base_z_planners_common. + * Renaming of event generator library + * minor formatting + * Add galactic CI setup and rename rolling files. (#58) + * Fix source CI and correct README overview. (#62) + * Update c_cpp_properties.json + * changed launch command to ros2 launch sm_respira_1 sm_respira_1.launch (#69) + also noticed a note I had made while producing these that was not removed + * update doxygen links (#70) + Co-authored-by: Ubuntu 20-04-02-amd64 + * More Readme Updates (#72) + Co-authored-by: Ubuntu 20-04-02-amd64 + * More Readme (#74) + Co-authored-by: Ubuntu 20-04-02-amd64 + * created new sm from sm_respira_1 (#76) + * Feature/core and navigation fixes (#78) + * base for the sm_aws_aarehouse navigation + * progressing in aws navigation + * minor + * several core improvements during navigation testing + * formatting improvements + * progress in aws navigation demo + * format improvements + * Feature/aws demo progress (#80) + * base for the sm_aws_aarehouse navigation + * progressing in aws navigation + * minor + * several core improvements during navigation testing + * formatting improvements + * progress in aws navigation demo + * format improvements + * format improvements + * more on navigation + * sm_advanced_recovery_1 reworked (#83) + * sm_advanced_recovery_1 reworked + * fix pre-commit + * Trying to fix Pre-Commit + Co-authored-by: Ubuntu 20-04-02-amd64 + * more sm_advanced_recovery_1 (#84) + Co-authored-by: Ubuntu 20-04-02-amd64 + * More sm_advanced_recovery_1 work (#85) + Co-authored-by: Ubuntu 20-04-02-amd64 + * sm_advanced_recovery_1 round 4 (#86) + Co-authored-by: Ubuntu 20-04-02-amd64 + * Brettpac branch (#87) + * sm_atomic_performance_test_a_2 + * sm_atomic_performance_test_a_1 + Co-authored-by: Ubuntu 20-04-02-amd64 + * sm_atomic_performance_test_c_1 (#88) + Co-authored-by: Ubuntu 20-04-02-amd64 + * modifying sm_atomic_performance_test_a_2 (#89) + Co-authored-by: Ubuntu 20-04-02-amd64 + * sm_multi_stage_1 (#90) + * sm_multi_stage_1 + * fixing precommit + Co-authored-by: Ubuntu 20-04-02-amd64 + * more sm_multi_stage_1 (#91) + Co-authored-by: Ubuntu 20-04-02-amd64 + * Update README.md + updated launch command + * Wait topic message client behavior (#81) + * base for the sm_aws_aarehouse navigation + * progressing in aws navigation + * minor + * several core improvements during navigation testing + * formatting improvements + * progress in aws navigation demo + * format improvements + * format improvements + * more on navigation + * new feature, cb_wait_topic_message: asynchronous client behavior that waits a topic message and optionally checks its contents to success + * formatting + * attempting precommit fixes + Co-authored-by: Ubuntu 20-04-02-amd64 + Co-authored-by: Denis Štogl + * Feature/wait nav2 nodes client behavior (#82) + * base for the sm_aws_aarehouse navigation + * progressing in aws navigation + * minor + * several core improvements during navigation testing + * formatting improvements + * progress in aws navigation demo + * format improvements + * format improvements + * more on navigation + * new feature, cb_wait_topic_message: asynchronous client behavior that waits a topic message and optionally checks its contents to success + * formatting + * adding new client behavior add for nav2, wait nav2 nodes subscribing to the /bond topic and waiting they are alive. you optionally can select the nodes to wait + * Correct all linters and formaters. + Co-authored-by: Denis Štogl + Co-authored-by: Denis Štogl + * Feature/aws demo progress (#92) + * base for the sm_aws_aarehouse navigation + * progressing in aws navigation + * minor + * several core improvements during navigation testing + * formatting improvements + * progress in aws navigation demo + * format improvements + * format improvements + * more on navigation + * new feature, cb_wait_topic_message: asynchronous client behavior that waits a topic message and optionally checks its contents to success + * formatting + * adding new client behavior add for nav2, wait nav2 nodes subscribing to the /bond topic and waiting they are alive. you optionally can select the nodes to wait + * progress in aws navigation demo + * minor format + * Feature/sm dance bot fixes (#93) + * base for the sm_aws_aarehouse navigation + * progressing in aws navigation + * minor + * several core improvements during navigation testing + * formatting improvements + * progress in aws navigation demo + * format improvements + * format improvements + * more on navigation + * new feature, cb_wait_topic_message: asynchronous client behavior that waits a topic message and optionally checks its contents to success + * formatting + * adding new client behavior add for nav2, wait nav2 nodes subscribing to the /bond topic and waiting they are alive. you optionally can select the nodes to wait + * progress in aws navigation demo + * minor format + * navigation parameters fixes on sm_dance_bot + * minor format + * Feature/sm aws warehouse (#94) + * base for the sm_aws_aarehouse navigation + * progressing in aws navigation + * minor + * several core improvements during navigation testing + * formatting improvements + * progress in aws navigation demo + * format improvements + * format improvements + * more on navigation + * new feature, cb_wait_topic_message: asynchronous client behavior that waits a topic message and optionally checks its contents to success + * formatting + * adding new client behavior add for nav2, wait nav2 nodes subscribing to the /bond topic and waiting they are alive. you optionally can select the nodes to wait + * progress in aws navigation demo + * minor format + * merge and progress + * fix format + * Feature/sm dance bot fixes (#95) + * base for the sm_aws_aarehouse navigation + * progressing in aws navigation + * minor + * several core improvements during navigation testing + * formatting improvements + * progress in aws navigation demo + * format improvements + * format improvements + * more on navigation + * new feature, cb_wait_topic_message: asynchronous client behavior that waits a topic message and optionally checks its contents to success + * formatting + * adding new client behavior add for nav2, wait nav2 nodes subscribing to the /bond topic and waiting they are alive. you optionally can select the nodes to wait + * progress in aws navigation demo + * minor format + * navigation parameters fixes on sm_dance_bot + * minor format + * minor + * minor format + * Remove some compile warnings. (#96) + * Feature/cb pause slam (#98) + * base for the sm_aws_aarehouse navigation + * progressing in aws navigation + * minor + * several core improvements during navigation testing + * formatting improvements + * progress in aws navigation demo + * format improvements + * format improvements + * more on navigation + * new feature, cb_wait_topic_message: asynchronous client behavior that waits a topic message and optionally checks its contents to success + * formatting + * adding new client behavior add for nav2, wait nav2 nodes subscribing to the /bond topic and waiting they are alive. you optionally can select the nodes to wait + * progress in aws navigation demo + * minor format + * navigation parameters fixes on sm_dance_bot + * minor format + * minor + * formatting + * cb pause slam client behavior + * sm_dance_bot_lite (#99) + * sm_dance_bot_lite + * precommit + * Updates yaml + Co-authored-by: Ubuntu 20-04-02-amd64 + * Rename doxygen deployment workflow (#100) + * minor hotfix + * sm_dance_bot visualizing turtlebot3 (#101) + * Feature/dance bot launch gz lidar choice (#102) + * sm_dance_bot visualizing turtlebot3 + * cleaning and lidar show/hide option + * cleaning files and making formatting work + * more fixes + * Feature/sm dance bot lite gazebo fixes (#104) + * sm_dance_bot visualizing turtlebot3 + * cleaning and lidar show/hide option + * cleaning files and making formatting work + * more fixes + * gazebo fixes, to show the robot and the lidar + * format fixes + * sm_multi_stage_1 doubling (#103) + Co-authored-by: Ubuntu 20-04-02-amd64 + * Feature/sm dance bot strikes back gazebo fixes (#105) + * sm_dance_bot visualizing turtlebot3 + * cleaning and lidar show/hide option + * cleaning files and making formatting work + * more fixes + * gazebo fixes, to show the robot and the lidar + * format fixes + * gazebo fixes for sm_dance_bot_strikes_back + * precommit cleanup run (#106) + Co-authored-by: Ubuntu 20-04-02-amd64 + * aws demo (#108) + * aws demo + * format + * got sm_multi_stage_1 working (barely) (#109) + Co-authored-by: Ubuntu 20-04-02-amd64 + * Brettpac branch (#110) + * got sm_multi_stage_1 working (barely) + * gaining traction sm_multi_stage_1 + * more + * don't remember + * making progress + * More + * keep hammering + * two stages + * 3 part + * 4th stage + Co-authored-by: Ubuntu 20-04-02-amd64 + * Brettpac branch (#111) + * got sm_multi_stage_1 working (barely) + * gaining traction sm_multi_stage_1 + * more + * don't remember + * making progress + * More + * keep hammering + * two stages + * 3 part + * 4th stage + * 5th stage + Co-authored-by: Ubuntu 20-04-02-amd64 + * a3 (#113) + Co-authored-by: Ubuntu 20-04-02-amd64 + * Remove neo_simulation2 package. (#112) + * Remove neo_simulation2 package. + * Correct formatting. + * Enable source build on PR for testing. + * Adjust build packages of source CI + * more sm_multi_stage_1 (#114) + Co-authored-by: Ubuntu 20-04-02-amd64 + * mm (#115) + Co-authored-by: Ubuntu 20-04-02-amd64 + * diverse improvements navigation and performance (#116) + * diverse improvements navigation and performance + * minor + Co-authored-by: pabloinigoblasco + * Feature/diverse improvemets navigation performance (#117) + * diverse improvements navigation and performance + * minor + * additional linting and formatting + * Remove merge markers from a python file. (#119) + * Feature/slam toggle and smacc deep history (#122) + * progress in navigation, slam toggle client behaviors and slam_toolbox components. Also smacc2::deep_history syntax + * going forward in testing sm_dance_bot introducing slam pausing/resuming funcionality + * feature/more_sm_dance_bot_fixes + * minor format + * minor (#124) + Co-authored-by: Ubuntu 20-04-02-amd64 + * more changes in sm_dance_bot (#125) + * Move method after the method it calls. Otherwise recursion could happen. (#126) + * Feature/dance bot s pattern (#128) + * more changes in sm_dance_bot + * polishing sm_dance_bot and s-pattern + * noticed typo + Finnaly > Finally + * Feature/dance bot s pattern (#129) + * more changes in sm_dance_bot + * polishing sm_dance_bot and s-pattern + * more refinement in sm_dance_bot + * First working version of sm template and template generator. (#127) + * minor tweaks (#130) + Co-authored-by: Ubuntu 20-04-02-amd64 + * Feature/sm dance bot refine (#131) + * more changes in sm_dance_bot + * minor + * Feature/sm dance bot refine 2 (#132) + * more changes in sm_dance_bot + * minor + * build fix + * waypoints navigator bug (#133) + * minor tuning to mitigate overshot issue cases + * progress in the sm_dance_bot tests (#135) + * some more progress on markers cleanup + * minor format issues (#134) + * sm_dance_bot_lite (#136) + Co-authored-by: Ubuntu 20-04-02-amd64 + * Resolve compile wanings (#137) + * Add SM core test (#138) + * minor navigation improvements (#141) + * using local action msgs (#139) + * using local action msgs + * removing sm_dance_bot_msgs + * pending references + * Feature/nitrosz renaming (#144) + * using local action msgs + * removing sm_dance_bot_msgs + * pending references + * navigation 2 stack renaming + * formatting + * added SVGs to READMEs of atomic, dance_bot, and others (#140) + * added remaining SVGs to READMEs (#145) + * added remaining SVGs to READMEs + * precommit cleanup + * Update package list. (#142) + * removing parameters smacc (#147) + * removing parameters smacc + * workflows update + * workflow + * Noticed launch command was incorrect in README.md + fixed launch command for sm_dance_bot_strikes_back and removed some comments I had made in the past. + * Fix CI: format fix python version (#148) + * Add SM Atomic SM generator. (#143) + * Remove node creation and create only a logger. (#149) + * Rolling Docker environment to be executed from any environment (#154) + * Feature/sm dance bot strikes back refactoring (#152) + Co-authored-by: DecDury + Co-authored-by: Denis Štogl + * slight waypoint 4 and iterations changes so robot can complete course (#155) + * Feature/migration moveit client (#151) + * initial migration to smacc2 + * fixing some errors introduced on formatting + * missing dependency + * fixing some more linting warnings + * minor + * removing test from main moveit cmake + * test ur5 + * progressing in the moveit migration testing + * updating format + * adding .reps dependencies and also fixing some build errors + * repos dependency + * adding dependency to ur5 client + * docker refactoring + * minor + * progress on move_it PR + * minor dockerfile test workaround + * improving dockerfile for building local tests + * minor + * fixing compiling issues + * update readme (#164) + * update readme + * more readme updates + * more + Co-authored-by: Ubuntu 20-04-02-amd64 + * initial state machine transition timestamp (#165) + * moved reference library SMs to smacc2_performance_tools (#166) + * moved reference library SMs to smacc2_performance_tools + * pre-commit cleanup + * Add QOS durability to SmaccPublisherClient (#163) + * feat: add qos durability to SmaccPublisherClient + * fix: add a missing colon + * refactor: remove line + * feat: add reliability qos config + * Feature/testing moveit behaviors (#167) + * more testing on moveit + * progress on moveit + * more testing on moveit behaviors + * minor configuration + * fixing pipeline error + * fixing broken master build + * sm_pubsub_1 (#169) + Co-authored-by: Ubuntu 20-04-02-amd64 + * sm_pubsub_1 part 2 (#170) + * sm_pubsub_1 part 2 + * sm_pubsub_1 part 2 + Co-authored-by: Ubuntu 20-04-02-amd64 + * sm_advanced_recovery_1 renaming (#171) + Co-authored-by: Ubuntu 20-04-02-amd64 + * sm_multi_stage_1 reworking (#172) + * multistage modes + * sm_multi_stage sequences + * sm_multi_state_1 steps + * sm_multi_stage_1 sequence d + * sm_multi_stage_1 c sequence + * mode_5_sequence_b + * mode_4_sequence_b + * sm_multi_stage_1 most + * finishing touches 1 + * readme + Co-authored-by: Ubuntu 20-04-02-amd64 + * Feature/aws navigation sm dance bot (#174) + * repo dependency + * husky launch file in sm_dance_bot + * Add dependencies for husky simulation. + * Fix formatting. + * Update dependencies for husky in rolling and galactic. + * minor + * progress on aws navigation and some other refactorings on navigation clients and behaviors + * more on aws demo + * fixing broken build + * minor + Co-authored-by: Denis Štogl + Co-authored-by: Denis Štogl + * minor changes (#175) + * warehouse2 (#177) + * Waypoint Inputs (#178) + Co-authored-by: Ubuntu 20-04-02-amd64 + * wharehouse2 progress (#179) + * format (#180) + * sm_dance_bot_warehouse_3 (#181) + Co-authored-by: Ubuntu 20-04-02-amd64 + * Feature/sm warehouse 2 13 dec 2 (#182) + * format + * more changes and headless + * merge + * headless and other fixes + * default values + * Brettpac branch (#184) + * sm_dance_bot_warehouse_3 + * Redoing sm_dance_bot_warehouse_3 waypoints + * More Waypoints + Co-authored-by: Ubuntu 20-04-02-amd64 + * SrConditional fixes and formatting (#168) + * fix: some formatting and templating on SrConditional + * fix: move trigger logic into headers + * fix: lint + * Feature/wharehouse2 dec 14 (#185) + * warehouse2 + * minor + * Feature/sm warehouse 2 13 dec 2 (#186) + * format + * more changes and headless + * merge + * headless and other fixes + * default values + * minor + * finetuning waypoints (#187) + Co-authored-by: Ubuntu 20-04-02-amd64 + * Feature/cb pure spinning (#188) + * format + * more changes and headless + * merge + * headless and other fixes + * default values + * minor + * Feature/cb pure spinning (#189) + * format + * more changes and headless + * merge + * headless and other fixes + * default values + * minor + * pure spinning behavior missing files + * minor changes (#190) + * Feature/planner changes 16 12 (#191) + * minor changes + * more fixes + * minor + * minor + * Feature/replanning 16 dec (#193) + * minor changes + * replanning for all our examples + * several fixes (#194) + * minor changes (#195) + * Feature/undo motion 20 12 (#196) + * minor changes + * replanning for all our examples + * improving undo motion navigation warehouse2 + * minor + * tuning warehouse3 (#197) + * Feature/undo motion 20 12 (#198) + * minor changes + * replanning for all our examples + * improving undo motion navigation warehouse2 + * minor + * undo tuning and errors + * format + * Feature/sync 21 12 (#199) + * minor changes + * replanning for all our examples + * format issues + * Feature/warehouse2 22 12 (#200) + * minor changes + * replanning for all our examples + * format issues + * finishing warehouse2 + * Feature/warehouse2 23 12 (#201) + * minor changes + * replanning for all our examples + * tuning and fixes (#202) + * Feature/minor tune (#203) + * tuning and fixes + * minor tune + * fixing warehouse 3 problems, and other core improvements (#204) + * fixing warehouse 3 problems, and other core improvements to remove dead lock, also making continuous integration green + * weird moveit not downloaded repo + * added missing file from warehouse2 (#205) + * backport to foxy + * minor format + * minor linking errors foxy + Co-authored-by: Denis Štogl + Co-authored-by: Denis Štogl + Co-authored-by: Declan Dury <44791484+DecDury@users.noreply.github.com> + Co-authored-by: DecDury + Co-authored-by: reelrbtx + Co-authored-by: brettpac + Co-authored-by: David Revay + * missing + * missing sm + * updating subscriber publisher components + * progress in autowarrrrrrrrrrrrrrrrrrrrrrrrrrrrrrrrrre machine + * refining cp subscriber cp publisher + * improvements in smacc core adding more components mostly developed for autoware demo + * autoware demo + * missing + * foxy ci + * fix + * minor broken build + * some reordering fixes + * minor + * docker files for different revisions, warnings removval and more testing on navigation + * fixing docker for foxy and galactic + * docker build files for all versions + * barrel demo + * barrel search build fix and warehouse3 + * fixing startup problems in warehouse 3 + * fixing format and minor + * minor + * progress in barrel husky + * minor + * barrel demo + * minor + * barrel search updates + * making models local + * red picuup + * multiple controllable leds plugin + * progress in husky demo + * progressing in husky demo + * improving navigation behaviors + * more merge + * docker improvements + Co-authored-by: Denis Štogl + Co-authored-by: Denis Štogl + Co-authored-by: Declan Dury <44791484+DecDury@users.noreply.github.com> + Co-authored-by: DecDury + Co-authored-by: reelrbtx + Co-authored-by: brettpac + Co-authored-by: David Revay + Co-authored-by: pabloinigoblasco + * Use correct upstream .repos files for source builds (#243) + * Correct mergify branch names (#246) + * Correct name of source-build job and bump version of action (#242) (#247) + Co-authored-by: Denis Štogl + Co-authored-by: Pablo Iñigo Blasco + * Update galactic source build job name (#250) + * Galactic source build: update .repos file, bump action version and use correct version of upstream packages (backport #241) (#248) + Co-authored-by: Denis Štogl + * fixing rolling build (#239) + * fixing rolling build + * trying to fix dependencies + * missing repo + * fixing to focal by the moment + * more fixing rolling build + * minor + * cache matrix rolling and source build package + * minor + * minor + * missing repo + * missing deps + * fixing building issue + * typo + * fixing broken build + * build fix + * restoring workflow files (#252) + * restoring files (#253) + * Fix checkout branches for scheduled builds (#254) + * correct checkout branch on scheduled build + * Update foxy-source-build.yml + * Feature/fixing husky build rolling (#257) + * restoring files + * making husky project build on rolling + * Feature/fixing husky build rolling (#258) + * restoring files + * making husky project build on rolling + * husky progress + * Update README.md (#262) + * Feature/fixing ur demos (#261) + * restoring files + * fixes + * Feature/fixing type string walker (#263) + * restoring files + * fixing type string walker threesome demo + * Update README.md (#266) + * Update README.md (#267) + * Update README.md (#268) + * Significant update in Getting Started Instructions (#269) + * Significant update in Getting Started Instructions + * Remove trailing spaces. + Co-authored-by: Denis Štogl + * fixing ur demo (#273) + * fix: initialise conditionFlag as false (#274) + * precommit fix (#280) + I merge this in red for focal-rolling because it was already broken anyway and it is only a minor update of the precommit + * progress on the sm_husky_barrel + * missing code + * more on husky demo for galactic + Co-authored-by: Denis Štogl + Co-authored-by: Denis Štogl + Co-authored-by: Declan Dury <44791484+DecDury@users.noreply.github.com> + Co-authored-by: DecDury + Co-authored-by: reelrbtx + Co-authored-by: brettpac + Co-authored-by: David Revay + Co-authored-by: pabloinigoblasco + Co-authored-by: mergify[bot] <37929162+mergify[bot]@users.noreply.github.com> + Co-authored-by: brettpac +* Revert "Ignore packages which should not be released." + This reverts commit dec14a936a877b2ef722a6a32f1bf3df09312542. +* Ignore packages which should not be released. +* Feature/master rolling to galactic backport (#236) + * updated mentions of SMACC/ROS to SMACC2/ROS2 + * some progress on navigation rolling + * renamed folders, deleted tracing.md, edited README.md + * added smacc2_performance_tools + * performance tests improvements + * more on performance and other issues + * sm_respira_1 format cleanup + * sm_respira_1 format cleanup pre-commit + * sm_respira_test_2 + * sm_respira_test_2 + * more changes on performance tests + * Do not execute clang-format on smacc2_sm_reference_library package. + * sm_reference_library reformatting + * Correct trailing spaces. + * sm_atomic_24hr + * sm_atomic_performance_trace_1 + * Update smacc2_rta command across readmes + * Clean up of sm_atomic_24hr + * more sm_atomic_24hr cleanup + * Optimized deps in move_base_z_planners_common. + * Renaming of event generator library + * minor formatting + * Add galactic CI setup and rename rolling files. (#58) + * Fix source CI and correct README overview. (#62) + * Update c_cpp_properties.json + * changed launch command to ros2 launch sm_respira_1 sm_respira_1.launch (#69) + also noticed a note I had made while producing these that was not removed + * update doxygen links (#70) + Co-authored-by: Ubuntu 20-04-02-amd64 + * More Readme Updates (#72) + Co-authored-by: Ubuntu 20-04-02-amd64 + * More Readme (#74) + Co-authored-by: Ubuntu 20-04-02-amd64 + * created new sm from sm_respira_1 (#76) + * Feature/core and navigation fixes (#78) + * base for the sm_aws_aarehouse navigation + * progressing in aws navigation + * minor + * several core improvements during navigation testing + * formatting improvements + * progress in aws navigation demo + * format improvements + * Feature/aws demo progress (#80) + * base for the sm_aws_aarehouse navigation + * progressing in aws navigation + * minor + * several core improvements during navigation testing + * formatting improvements + * progress in aws navigation demo + * format improvements + * format improvements + * more on navigation + * sm_advanced_recovery_1 reworked (#83) + * sm_advanced_recovery_1 reworked + * fix pre-commit + * Trying to fix Pre-Commit + Co-authored-by: Ubuntu 20-04-02-amd64 + * more sm_advanced_recovery_1 (#84) + Co-authored-by: Ubuntu 20-04-02-amd64 + * More sm_advanced_recovery_1 work (#85) + Co-authored-by: Ubuntu 20-04-02-amd64 + * sm_advanced_recovery_1 round 4 (#86) + Co-authored-by: Ubuntu 20-04-02-amd64 + * Brettpac branch (#87) + * sm_atomic_performance_test_a_2 + * sm_atomic_performance_test_a_1 + Co-authored-by: Ubuntu 20-04-02-amd64 + * sm_atomic_performance_test_c_1 (#88) + Co-authored-by: Ubuntu 20-04-02-amd64 + * modifying sm_atomic_performance_test_a_2 (#89) + Co-authored-by: Ubuntu 20-04-02-amd64 + * sm_multi_stage_1 (#90) + * sm_multi_stage_1 + * fixing precommit + Co-authored-by: Ubuntu 20-04-02-amd64 + * more sm_multi_stage_1 (#91) + Co-authored-by: Ubuntu 20-04-02-amd64 + * Update README.md + updated launch command + * Wait topic message client behavior (#81) + * base for the sm_aws_aarehouse navigation + * progressing in aws navigation + * minor + * several core improvements during navigation testing + * formatting improvements + * progress in aws navigation demo + * format improvements + * format improvements + * more on navigation + * new feature, cb_wait_topic_message: asynchronous client behavior that waits a topic message and optionally checks its contents to success + * formatting + * attempting precommit fixes + Co-authored-by: Ubuntu 20-04-02-amd64 + Co-authored-by: Denis Štogl + * Feature/wait nav2 nodes client behavior (#82) + * base for the sm_aws_aarehouse navigation + * progressing in aws navigation + * minor + * several core improvements during navigation testing + * formatting improvements + * progress in aws navigation demo + * format improvements + * format improvements + * more on navigation + * new feature, cb_wait_topic_message: asynchronous client behavior that waits a topic message and optionally checks its contents to success + * formatting + * adding new client behavior add for nav2, wait nav2 nodes subscribing to the /bond topic and waiting they are alive. you optionally can select the nodes to wait + * Correct all linters and formaters. + Co-authored-by: Denis Štogl + Co-authored-by: Denis Štogl + * Feature/aws demo progress (#92) + * base for the sm_aws_aarehouse navigation + * progressing in aws navigation + * minor + * several core improvements during navigation testing + * formatting improvements + * progress in aws navigation demo + * format improvements + * format improvements + * more on navigation + * new feature, cb_wait_topic_message: asynchronous client behavior that waits a topic message and optionally checks its contents to success + * formatting + * adding new client behavior add for nav2, wait nav2 nodes subscribing to the /bond topic and waiting they are alive. you optionally can select the nodes to wait + * progress in aws navigation demo + * minor format + * Feature/sm dance bot fixes (#93) + * base for the sm_aws_aarehouse navigation + * progressing in aws navigation + * minor + * several core improvements during navigation testing + * formatting improvements + * progress in aws navigation demo + * format improvements + * format improvements + * more on navigation + * new feature, cb_wait_topic_message: asynchronous client behavior that waits a topic message and optionally checks its contents to success + * formatting + * adding new client behavior add for nav2, wait nav2 nodes subscribing to the /bond topic and waiting they are alive. you optionally can select the nodes to wait + * progress in aws navigation demo + * minor format + * navigation parameters fixes on sm_dance_bot + * minor format + * Feature/sm aws warehouse (#94) + * base for the sm_aws_aarehouse navigation + * progressing in aws navigation + * minor + * several core improvements during navigation testing + * formatting improvements + * progress in aws navigation demo + * format improvements + * format improvements + * more on navigation + * new feature, cb_wait_topic_message: asynchronous client behavior that waits a topic message and optionally checks its contents to success + * formatting + * adding new client behavior add for nav2, wait nav2 nodes subscribing to the /bond topic and waiting they are alive. you optionally can select the nodes to wait + * progress in aws navigation demo + * minor format + * merge and progress + * fix format + * Feature/sm dance bot fixes (#95) + * base for the sm_aws_aarehouse navigation + * progressing in aws navigation + * minor + * several core improvements during navigation testing + * formatting improvements + * progress in aws navigation demo + * format improvements + * format improvements + * more on navigation + * new feature, cb_wait_topic_message: asynchronous client behavior that waits a topic message and optionally checks its contents to success + * formatting + * adding new client behavior add for nav2, wait nav2 nodes subscribing to the /bond topic and waiting they are alive. you optionally can select the nodes to wait + * progress in aws navigation demo + * minor format + * navigation parameters fixes on sm_dance_bot + * minor format + * minor + * minor format + * Remove some compile warnings. (#96) + * Feature/cb pause slam (#98) + * base for the sm_aws_aarehouse navigation + * progressing in aws navigation + * minor + * several core improvements during navigation testing + * formatting improvements + * progress in aws navigation demo + * format improvements + * format improvements + * more on navigation + * new feature, cb_wait_topic_message: asynchronous client behavior that waits a topic message and optionally checks its contents to success + * formatting + * adding new client behavior add for nav2, wait nav2 nodes subscribing to the /bond topic and waiting they are alive. you optionally can select the nodes to wait + * progress in aws navigation demo + * minor format + * navigation parameters fixes on sm_dance_bot + * minor format + * minor + * formatting + * cb pause slam client behavior + * sm_dance_bot_lite (#99) + * sm_dance_bot_lite + * precommit + * Updates yaml + Co-authored-by: Ubuntu 20-04-02-amd64 + * Rename doxygen deployment workflow (#100) + * minor hotfix + * sm_dance_bot visualizing turtlebot3 (#101) + * Feature/dance bot launch gz lidar choice (#102) + * sm_dance_bot visualizing turtlebot3 + * cleaning and lidar show/hide option + * cleaning files and making formatting work + * more fixes + * Feature/sm dance bot lite gazebo fixes (#104) + * sm_dance_bot visualizing turtlebot3 + * cleaning and lidar show/hide option + * cleaning files and making formatting work + * more fixes + * gazebo fixes, to show the robot and the lidar + * format fixes + * sm_multi_stage_1 doubling (#103) + Co-authored-by: Ubuntu 20-04-02-amd64 + * Feature/sm dance bot strikes back gazebo fixes (#105) + * sm_dance_bot visualizing turtlebot3 + * cleaning and lidar show/hide option + * cleaning files and making formatting work + * more fixes + * gazebo fixes, to show the robot and the lidar + * format fixes + * gazebo fixes for sm_dance_bot_strikes_back + * precommit cleanup run (#106) + Co-authored-by: Ubuntu 20-04-02-amd64 + * aws demo (#108) + * aws demo + * format + * got sm_multi_stage_1 working (barely) (#109) + Co-authored-by: Ubuntu 20-04-02-amd64 + * Brettpac branch (#110) + * got sm_multi_stage_1 working (barely) + * gaining traction sm_multi_stage_1 + * more + * don't remember + * making progress + * More + * keep hammering + * two stages + * 3 part + * 4th stage + Co-authored-by: Ubuntu 20-04-02-amd64 + * Brettpac branch (#111) + * got sm_multi_stage_1 working (barely) + * gaining traction sm_multi_stage_1 + * more + * don't remember + * making progress + * More + * keep hammering + * two stages + * 3 part + * 4th stage + * 5th stage + Co-authored-by: Ubuntu 20-04-02-amd64 + * a3 (#113) + Co-authored-by: Ubuntu 20-04-02-amd64 + * Remove neo_simulation2 package. (#112) + * Remove neo_simulation2 package. + * Correct formatting. + * Enable source build on PR for testing. + * Adjust build packages of source CI + * more sm_multi_stage_1 (#114) + Co-authored-by: Ubuntu 20-04-02-amd64 + * mm (#115) + Co-authored-by: Ubuntu 20-04-02-amd64 + * diverse improvements navigation and performance (#116) + * diverse improvements navigation and performance + * minor + Co-authored-by: pabloinigoblasco + * Feature/diverse improvemets navigation performance (#117) + * diverse improvements navigation and performance + * minor + * additional linting and formatting + * Remove merge markers from a python file. (#119) + * Feature/slam toggle and smacc deep history (#122) + * progress in navigation, slam toggle client behaviors and slam_toolbox components. Also smacc2::deep_history syntax + * going forward in testing sm_dance_bot introducing slam pausing/resuming funcionality + * feature/more_sm_dance_bot_fixes + * minor format + * minor (#124) + Co-authored-by: Ubuntu 20-04-02-amd64 + * more changes in sm_dance_bot (#125) + * Move method after the method it calls. Otherwise recursion could happen. (#126) + * Feature/dance bot s pattern (#128) + * more changes in sm_dance_bot + * polishing sm_dance_bot and s-pattern + * noticed typo + Finnaly > Finally + * Feature/dance bot s pattern (#129) + * more changes in sm_dance_bot + * polishing sm_dance_bot and s-pattern + * more refinement in sm_dance_bot + * First working version of sm template and template generator. (#127) + * minor tweaks (#130) + Co-authored-by: Ubuntu 20-04-02-amd64 + * Feature/sm dance bot refine (#131) + * more changes in sm_dance_bot + * minor + * Feature/sm dance bot refine 2 (#132) + * more changes in sm_dance_bot + * minor + * build fix + * waypoints navigator bug (#133) + * minor tuning to mitigate overshot issue cases + * progress in the sm_dance_bot tests (#135) + * some more progress on markers cleanup + * minor format issues (#134) + * sm_dance_bot_lite (#136) + Co-authored-by: Ubuntu 20-04-02-amd64 + * Resolve compile wanings (#137) + * Add SM core test (#138) + * minor navigation improvements (#141) + * using local action msgs (#139) + * using local action msgs + * removing sm_dance_bot_msgs + * pending references + * Feature/nitrosz renaming (#144) + * using local action msgs + * removing sm_dance_bot_msgs + * pending references + * navigation 2 stack renaming + * formatting + * added SVGs to READMEs of atomic, dance_bot, and others (#140) + * added remaining SVGs to READMEs (#145) + * added remaining SVGs to READMEs + * precommit cleanup + * Update package list. (#142) + * removing parameters smacc (#147) + * removing parameters smacc + * workflows update + * workflow + * Noticed launch command was incorrect in README.md + fixed launch command for sm_dance_bot_strikes_back and removed some comments I had made in the past. + * Fix CI: format fix python version (#148) + * Add SM Atomic SM generator. (#143) + * Remove node creation and create only a logger. (#149) + * Rolling Docker environment to be executed from any environment (#154) + * Feature/sm dance bot strikes back refactoring (#152) + Co-authored-by: DecDury + Co-authored-by: Denis Štogl + * slight waypoint 4 and iterations changes so robot can complete course (#155) + * Feature/migration moveit client (#151) + * initial migration to smacc2 + * fixing some errors introduced on formatting + * missing dependency + * fixing some more linting warnings + * minor + * removing test from main moveit cmake + * test ur5 + * progressing in the moveit migration testing + * updating format + * adding .reps dependencies and also fixing some build errors + * repos dependency + * adding dependency to ur5 client + * docker refactoring + * minor + * progress on move_it PR + * minor dockerfile test workaround + * improving dockerfile for building local tests + * minor + * fixing compiling issues + * update readme (#164) + * update readme + * more readme updates + * more + Co-authored-by: Ubuntu 20-04-02-amd64 + * initial state machine transition timestamp (#165) + * moved reference library SMs to smacc2_performance_tools (#166) + * moved reference library SMs to smacc2_performance_tools + * pre-commit cleanup + * Add QOS durability to SmaccPublisherClient (#163) + * feat: add qos durability to SmaccPublisherClient + * fix: add a missing colon + * refactor: remove line + * feat: add reliability qos config + * Feature/testing moveit behaviors (#167) + * more testing on moveit + * progress on moveit + * more testing on moveit behaviors + * minor configuration + * fixing pipeline error + * fixing broken master build + * sm_pubsub_1 (#169) + Co-authored-by: Ubuntu 20-04-02-amd64 + * sm_pubsub_1 part 2 (#170) + * sm_pubsub_1 part 2 + * sm_pubsub_1 part 2 + Co-authored-by: Ubuntu 20-04-02-amd64 + * sm_advanced_recovery_1 renaming (#171) + Co-authored-by: Ubuntu 20-04-02-amd64 + * sm_multi_stage_1 reworking (#172) + * multistage modes + * sm_multi_stage sequences + * sm_multi_state_1 steps + * sm_multi_stage_1 sequence d + * sm_multi_stage_1 c sequence + * mode_5_sequence_b + * mode_4_sequence_b + * sm_multi_stage_1 most + * finishing touches 1 + * readme + Co-authored-by: Ubuntu 20-04-02-amd64 + * Feature/aws navigation sm dance bot (#174) + * repo dependency + * husky launch file in sm_dance_bot + * Add dependencies for husky simulation. + * Fix formatting. + * Update dependencies for husky in rolling and galactic. + * minor + * progress on aws navigation and some other refactorings on navigation clients and behaviors + * more on aws demo + * fixing broken build + * minor + Co-authored-by: Denis Štogl + Co-authored-by: Denis Štogl + * minor changes + * minor changes (#175) + * warehouse2 (#177) + * Waypoint Inputs (#178) + Co-authored-by: Ubuntu 20-04-02-amd64 + * wharehouse2 progress (#179) + * format (#180) + * sm_dance_bot_warehouse_3 (#181) + Co-authored-by: Ubuntu 20-04-02-amd64 + * Feature/sm warehouse 2 13 dec 2 (#182) + * format + * more changes and headless + * merge + * headless and other fixes + * default values + * Brettpac branch (#184) + * sm_dance_bot_warehouse_3 + * Redoing sm_dance_bot_warehouse_3 waypoints + * More Waypoints + Co-authored-by: Ubuntu 20-04-02-amd64 + * SrConditional fixes and formatting (#168) + * fix: some formatting and templating on SrConditional + * fix: move trigger logic into headers + * fix: lint + * Feature/wharehouse2 dec 14 (#185) + * warehouse2 + * minor + * Feature/sm warehouse 2 13 dec 2 (#186) + * format + * more changes and headless + * merge + * headless and other fixes + * default values + * minor + * finetuning waypoints (#187) + Co-authored-by: Ubuntu 20-04-02-amd64 + * Feature/cb pure spinning (#188) + * format + * more changes and headless + * merge + * headless and other fixes + * default values + * minor + * Feature/cb pure spinning (#189) + * format + * more changes and headless + * merge + * headless and other fixes + * default values + * minor + * pure spinning behavior missing files + * minor changes (#190) + * Feature/planner changes 16 12 (#191) + * minor changes + * more fixes + * minor + * minor + * replanning for all our examples + * Feature/replanning 16 dec (#193) + * minor changes + * replanning for all our examples + * several fixes (#194) + * minor changes (#195) + * Feature/undo motion 20 12 (#196) + * minor changes + * replanning for all our examples + * improving undo motion navigation warehouse2 + * minor + * tuning warehouse3 (#197) + * Feature/undo motion 20 12 (#198) + * minor changes + * replanning for all our examples + * improving undo motion navigation warehouse2 + * minor + * undo tuning and errors + * format + * Feature/sync 21 12 (#199) + * minor changes + * replanning for all our examples + * format issues + * Feature/warehouse2 22 12 (#200) + * minor changes + * replanning for all our examples + * format issues + * finishing warehouse2 + * Feature/warehouse2 23 12 (#201) + * minor changes + * replanning for all our examples + * tuning and fixes (#202) + * Feature/minor tune (#203) + * tuning and fixes + * minor tune + * fixing warehouse 3 problems, and other core improvements (#204) + * fixing warehouse 3 problems, and other core improvements to remove dead lock, also making continuous integration green + * weird moveit not downloaded repo + * added missing file from warehouse2 (#205) + * backport to foxy + * minor format + * minor linking errors foxy + * Foxy backport (#206) + * minor formatting fixes + * Fix trailing spaces. + * Correct codespell. + * Correct python linters warnings. + * Add galactic CI build because Navigation2 is broken in rolling. + * Add partial changes for ament_cpplint. + * Add tf2_ros as dependency to find include. + * Disable ament_cpplint. + * Disable some packages and update workflows. + * Bump ccache version. + * Ignore further packages + * Satisfy ament_lint_cmake + * Add missing licences. + * Disable cpplint and cppcheck linters. + * Correct formatters. + * branching example + * Disable disabled packages + * Update ci-build-source.yml + * Change extension + * Change extension of imports. + * Enable cppcheck + * Correct formatting of python file. + * Included necessary package and edited Threesome launch + Changed... + ros2 launch sm_three_some sm_three_some + to + ros2 launch sm_three_some sm_three_some.launch + Added: + First ensure you have the necessary package installed. + ``` + sudo apt-get install ros-rolling-ros2trace + ``` + Then run this command. + * Rename header files and correct format. + * Add workflow for checking doc build. + * Update doxygen-check-build.yml + * Create doxygen-deploy.yml + * Use manual deployment for now. + * Create workflow for testing prerelease builds + * Use docs/ as source folder for documentation + * Use docs/ as output directory. + * Rename to smacc2 and smacc2_msgs + * Correct GitHub branch reference. + * Update name of package and package.xml to pass liter. + * Execute on master update + * Reset all versions to 0.0.0 + * Ignore all packages except smacc2 and smacc2_msgs + * Update changelogs + * 0.1.0 + * Revert "Ignore all packages except smacc2 and smacc2_msgs" + This reverts commit f603166a4b3ccdfe96c64d9f9fb9d8b49fbf0e61. + * Update description table. + * Update table + * Copy initial docs + * Dockerfile w/ ROS distro as argument + use this command "sudo docker build --build-arg ROS2_DISTRO=(desiredRosTag) (directoryHoldingDockerfile)/" + * Opened new folder for additional tracing contents + * Delete tracing directory + * Moved tracing.md to tracing directory + * added setupTracing.sh + Installs necessary packages and configures tracing group. + * Removed manual installation of ros-rolling-ros2trace + This is now automated in setupTracing.sh + location of sh file assumed if user follows README.md under "Getting started" + * Created alternative ManualTracing + * added new sm markdowns + * added a dockerfile for Rolling and Galactic + * Update smacc2_ci/docker/ros_rollingAndGalactic_ubuntu_20.04/buildGalactic.sh + Co-authored-by: Denis Štogl + * Update tracing/ManualTracing.md + Co-authored-by: Denis Štogl + * changed wording "smacc application" to "SMACC2 library" + Co-authored-by: Denis Štogl + * Update smacc_sm_reference_library/sm_atomic/README.md + edit from html to markdown syntax + Co-authored-by: Denis Štogl + * reactivating smacc2 nav clients for rolling via submodules + * renamed tracing events after + * bug in smacc2 component + * reverted markdowns to html + * added README tutorial for Dockerfile + * additional cleanup + * cleanup + * cleanup + * edited tracing.md to reflect new tracing event names + * Enable build of missing rolling repositories. + * Enable Navigation2 for semi-binary build. + * Remove galactic builds from master and kepp only rolling. Remove submodules and use .repos file + * updated mentions of SMACC/ROS to SMACC2/ROS2 + * some progress on navigation rolling + * renamed folders, deleted tracing.md, edited README.md + * added smacc2_performance_tools + * performance tests improvements + * more on performance and other issues + * sm_respira_1 format cleanup + * sm_respira_1 format cleanup pre-commit + * sm_respira_test_2 + * sm_respira_test_2 + * more changes on performance tests + * Do not execute clang-format on smacc2_sm_reference_library package. + * sm_reference_library reformatting + * Correct trailing spaces. + * sm_atomic_24hr + * sm_atomic_performance_trace_1 + * Update smacc2_rta command across readmes + * Clean up of sm_atomic_24hr + * more sm_atomic_24hr cleanup + * Optimized deps in move_base_z_planners_common. + * Renaming of event generator library + * minor formatting + * Add galactic CI setup and rename rolling files. (#58) + * Fix source CI and correct README overview. (#62) + * Update c_cpp_properties.json + * changed launch command to ros2 launch sm_respira_1 sm_respira_1.launch (#69) + also noticed a note I had made while producing these that was not removed + * update doxygen links (#70) + Co-authored-by: Ubuntu 20-04-02-amd64 + * More Readme Updates (#72) + Co-authored-by: Ubuntu 20-04-02-amd64 + * More Readme (#74) + Co-authored-by: Ubuntu 20-04-02-amd64 + * created new sm from sm_respira_1 (#76) + * Feature/core and navigation fixes (#78) + * base for the sm_aws_aarehouse navigation + * progressing in aws navigation + * minor + * several core improvements during navigation testing + * formatting improvements + * progress in aws navigation demo + * format improvements + * Feature/aws demo progress (#80) + * base for the sm_aws_aarehouse navigation + * progressing in aws navigation + * minor + * several core improvements during navigation testing + * formatting improvements + * progress in aws navigation demo + * format improvements + * format improvements + * more on navigation + * sm_advanced_recovery_1 reworked (#83) + * sm_advanced_recovery_1 reworked + * fix pre-commit + * Trying to fix Pre-Commit + Co-authored-by: Ubuntu 20-04-02-amd64 + * more sm_advanced_recovery_1 (#84) + Co-authored-by: Ubuntu 20-04-02-amd64 + * More sm_advanced_recovery_1 work (#85) + Co-authored-by: Ubuntu 20-04-02-amd64 + * sm_advanced_recovery_1 round 4 (#86) + Co-authored-by: Ubuntu 20-04-02-amd64 + * Brettpac branch (#87) + * sm_atomic_performance_test_a_2 + * sm_atomic_performance_test_a_1 + Co-authored-by: Ubuntu 20-04-02-amd64 + * sm_atomic_performance_test_c_1 (#88) + Co-authored-by: Ubuntu 20-04-02-amd64 + * modifying sm_atomic_performance_test_a_2 (#89) + Co-authored-by: Ubuntu 20-04-02-amd64 + * sm_multi_stage_1 (#90) + * sm_multi_stage_1 + * fixing precommit + Co-authored-by: Ubuntu 20-04-02-amd64 + * more sm_multi_stage_1 (#91) + Co-authored-by: Ubuntu 20-04-02-amd64 + * Update README.md + updated launch command + * Wait topic message client behavior (#81) + * base for the sm_aws_aarehouse navigation + * progressing in aws navigation + * minor + * several core improvements during navigation testing + * formatting improvements + * progress in aws navigation demo + * format improvements + * format improvements + * more on navigation + * new feature, cb_wait_topic_message: asynchronous client behavior that waits a topic message and optionally checks its contents to success + * formatting + * attempting precommit fixes + Co-authored-by: Ubuntu 20-04-02-amd64 + Co-authored-by: Denis Štogl + * Feature/wait nav2 nodes client behavior (#82) + * base for the sm_aws_aarehouse navigation + * progressing in aws navigation + * minor + * several core improvements during navigation testing + * formatting improvements + * progress in aws navigation demo + * format improvements + * format improvements + * more on navigation + * new feature, cb_wait_topic_message: asynchronous client behavior that waits a topic message and optionally checks its contents to success + * formatting + * adding new client behavior add for nav2, wait nav2 nodes subscribing to the /bond topic and waiting they are alive. you optionally can select the nodes to wait + * Correct all linters and formaters. + Co-authored-by: Denis Štogl + Co-authored-by: Denis Štogl + * Feature/aws demo progress (#92) + * base for the sm_aws_aarehouse navigation + * progressing in aws navigation + * minor + * several core improvements during navigation testing + * formatting improvements + * progress in aws navigation demo + * format improvements + * format improvements + * more on navigation + * new feature, cb_wait_topic_message: asynchronous client behavior that waits a topic message and optionally checks its contents to success + * formatting + * adding new client behavior add for nav2, wait nav2 nodes subscribing to the /bond topic and waiting they are alive. you optionally can select the nodes to wait + * progress in aws navigation demo + * minor format + * Feature/sm dance bot fixes (#93) + * base for the sm_aws_aarehouse navigation + * progressing in aws navigation + * minor + * several core improvements during navigation testing + * formatting improvements + * progress in aws navigation demo + * format improvements + * format improvements + * more on navigation + * new feature, cb_wait_topic_message: asynchronous client behavior that waits a topic message and optionally checks its contents to success + * formatting + * adding new client behavior add for nav2, wait nav2 nodes subscribing to the /bond topic and waiting they are alive. you optionally can select the nodes to wait + * progress in aws navigation demo + * minor format + * navigation parameters fixes on sm_dance_bot + * minor format + * Feature/sm aws warehouse (#94) + * base for the sm_aws_aarehouse navigation + * progressing in aws navigation + * minor + * several core improvements during navigation testing + * formatting improvements + * progress in aws navigation demo + * format improvements + * format improvements + * more on navigation + * new feature, cb_wait_topic_message: asynchronous client behavior that waits a topic message and optionally checks its contents to success + * formatting + * adding new client behavior add for nav2, wait nav2 nodes subscribing to the /bond topic and waiting they are alive. you optionally can select the nodes to wait + * progress in aws navigation demo + * minor format + * merge and progress + * fix format + * Feature/sm dance bot fixes (#95) + * base for the sm_aws_aarehouse navigation + * progressing in aws navigation + * minor + * several core improvements during navigation testing + * formatting improvements + * progress in aws navigation demo + * format improvements + * format improvements + * more on navigation + * new feature, cb_wait_topic_message: asynchronous client behavior that waits a topic message and optionally checks its contents to success + * formatting + * adding new client behavior add for nav2, wait nav2 nodes subscribing to the /bond topic and waiting they are alive. you optionally can select the nodes to wait + * progress in aws navigation demo + * minor format + * navigation parameters fixes on sm_dance_bot + * minor format + * minor + * minor format + * Remove some compile warnings. (#96) + * Feature/cb pause slam (#98) + * base for the sm_aws_aarehouse navigation + * progressing in aws navigation + * minor + * several core improvements during navigation testing + * formatting improvements + * progress in aws navigation demo + * format improvements + * format improvements + * more on navigation + * new feature, cb_wait_topic_message: asynchronous client behavior that waits a topic message and optionally checks its contents to success + * formatting + * adding new client behavior add for nav2, wait nav2 nodes subscribing to the /bond topic and waiting they are alive. you optionally can select the nodes to wait + * progress in aws navigation demo + * minor format + * navigation parameters fixes on sm_dance_bot + * minor format + * minor + * formatting + * cb pause slam client behavior + * sm_dance_bot_lite (#99) + * sm_dance_bot_lite + * precommit + * Updates yaml + Co-authored-by: Ubuntu 20-04-02-amd64 + * Rename doxygen deployment workflow (#100) + * minor hotfix + * sm_dance_bot visualizing turtlebot3 (#101) + * Feature/dance bot launch gz lidar choice (#102) + * sm_dance_bot visualizing turtlebot3 + * cleaning and lidar show/hide option + * cleaning files and making formatting work + * more fixes + * Feature/sm dance bot lite gazebo fixes (#104) + * sm_dance_bot visualizing turtlebot3 + * cleaning and lidar show/hide option + * cleaning files and making formatting work + * more fixes + * gazebo fixes, to show the robot and the lidar + * format fixes + * sm_multi_stage_1 doubling (#103) + Co-authored-by: Ubuntu 20-04-02-amd64 + * Feature/sm dance bot strikes back gazebo fixes (#105) + * sm_dance_bot visualizing turtlebot3 + * cleaning and lidar show/hide option + * cleaning files and making formatting work + * more fixes + * gazebo fixes, to show the robot and the lidar + * format fixes + * gazebo fixes for sm_dance_bot_strikes_back + * precommit cleanup run (#106) + Co-authored-by: Ubuntu 20-04-02-amd64 + * aws demo (#108) + * aws demo + * format + * got sm_multi_stage_1 working (barely) (#109) + Co-authored-by: Ubuntu 20-04-02-amd64 + * Brettpac branch (#110) + * got sm_multi_stage_1 working (barely) + * gaining traction sm_multi_stage_1 + * more + * don't remember + * making progress + * More + * keep hammering + * two stages + * 3 part + * 4th stage + Co-authored-by: Ubuntu 20-04-02-amd64 + * Brettpac branch (#111) + * got sm_multi_stage_1 working (barely) + * gaining traction sm_multi_stage_1 + * more + * don't remember + * making progress + * More + * keep hammering + * two stages + * 3 part + * 4th stage + * 5th stage + Co-authored-by: Ubuntu 20-04-02-amd64 + * a3 (#113) + Co-authored-by: Ubuntu 20-04-02-amd64 + * Remove neo_simulation2 package. (#112) + * Remove neo_simulation2 package. + * Correct formatting. + * Enable source build on PR for testing. + * Adjust build packages of source CI + * more sm_multi_stage_1 (#114) + Co-authored-by: Ubuntu 20-04-02-amd64 + * mm (#115) + Co-authored-by: Ubuntu 20-04-02-amd64 + * diverse improvements navigation and performance (#116) + * diverse improvements navigation and performance + * minor + Co-authored-by: pabloinigoblasco + * Feature/diverse improvemets navigation performance (#117) + * diverse improvements navigation and performance + * minor + * additional linting and formatting + * Remove merge markers from a python file. (#119) + * Feature/slam toggle and smacc deep history (#122) + * progress in navigation, slam toggle client behaviors and slam_toolbox components. Also smacc2::deep_history syntax + * going forward in testing sm_dance_bot introducing slam pausing/resuming funcionality + * feature/more_sm_dance_bot_fixes + * minor format + * minor (#124) + Co-authored-by: Ubuntu 20-04-02-amd64 + * more changes in sm_dance_bot (#125) + * Move method after the method it calls. Otherwise recursion could happen. (#126) + * Feature/dance bot s pattern (#128) + * more changes in sm_dance_bot + * polishing sm_dance_bot and s-pattern + * noticed typo + Finnaly > Finally + * Feature/dance bot s pattern (#129) + * more changes in sm_dance_bot + * polishing sm_dance_bot and s-pattern + * more refinement in sm_dance_bot + * First working version of sm template and template generator. (#127) + * minor tweaks (#130) + Co-authored-by: Ubuntu 20-04-02-amd64 + * Feature/sm dance bot refine (#131) + * more changes in sm_dance_bot + * minor + * Feature/sm dance bot refine 2 (#132) + * more changes in sm_dance_bot + * minor + * build fix + * waypoints navigator bug (#133) + * minor tuning to mitigate overshot issue cases + * progress in the sm_dance_bot tests (#135) + * some more progress on markers cleanup + * minor format issues (#134) + * sm_dance_bot_lite (#136) + Co-authored-by: Ubuntu 20-04-02-amd64 + * Resolve compile wanings (#137) + * Add SM core test (#138) + * minor navigation improvements (#141) + * using local action msgs (#139) + * using local action msgs + * removing sm_dance_bot_msgs + * pending references + * Feature/nitrosz renaming (#144) + * using local action msgs + * removing sm_dance_bot_msgs + * pending references + * navigation 2 stack renaming + * formatting + * added SVGs to READMEs of atomic, dance_bot, and others (#140) + * added remaining SVGs to READMEs (#145) + * added remaining SVGs to READMEs + * precommit cleanup + * Update package list. (#142) + * removing parameters smacc (#147) + * removing parameters smacc + * workflows update + * workflow + * Noticed launch command was incorrect in README.md + fixed launch command for sm_dance_bot_strikes_back and removed some comments I had made in the past. + * Fix CI: format fix python version (#148) + * Add SM Atomic SM generator. (#143) + * Remove node creation and create only a logger. (#149) + * Rolling Docker environment to be executed from any environment (#154) + * Feature/sm dance bot strikes back refactoring (#152) + Co-authored-by: DecDury + Co-authored-by: Denis Štogl + * slight waypoint 4 and iterations changes so robot can complete course (#155) + * Feature/migration moveit client (#151) + * initial migration to smacc2 + * fixing some errors introduced on formatting + * missing dependency + * fixing some more linting warnings + * minor + * removing test from main moveit cmake + * test ur5 + * progressing in the moveit migration testing + * updating format + * adding .reps dependencies and also fixing some build errors + * repos dependency + * adding dependency to ur5 client + * docker refactoring + * minor + * progress on move_it PR + * minor dockerfile test workaround + * improving dockerfile for building local tests + * minor + * fixing compiling issues + * update readme (#164) + * update readme + * more readme updates + * more + Co-authored-by: Ubuntu 20-04-02-amd64 + * initial state machine transition timestamp (#165) + * moved reference library SMs to smacc2_performance_tools (#166) + * moved reference library SMs to smacc2_performance_tools + * pre-commit cleanup + * Add QOS durability to SmaccPublisherClient (#163) + * feat: add qos durability to SmaccPublisherClient + * fix: add a missing colon + * refactor: remove line + * feat: add reliability qos config + * Feature/testing moveit behaviors (#167) + * more testing on moveit + * progress on moveit + * more testing on moveit behaviors + * minor configuration + * fixing pipeline error + * fixing broken master build + * sm_pubsub_1 (#169) + Co-authored-by: Ubuntu 20-04-02-amd64 + * sm_pubsub_1 part 2 (#170) + * sm_pubsub_1 part 2 + * sm_pubsub_1 part 2 + Co-authored-by: Ubuntu 20-04-02-amd64 + * sm_advanced_recovery_1 renaming (#171) + Co-authored-by: Ubuntu 20-04-02-amd64 + * sm_multi_stage_1 reworking (#172) + * multistage modes + * sm_multi_stage sequences + * sm_multi_state_1 steps + * sm_multi_stage_1 sequence d + * sm_multi_stage_1 c sequence + * mode_5_sequence_b + * mode_4_sequence_b + * sm_multi_stage_1 most + * finishing touches 1 + * readme + Co-authored-by: Ubuntu 20-04-02-amd64 + * Feature/aws navigation sm dance bot (#174) + * repo dependency + * husky launch file in sm_dance_bot + * Add dependencies for husky simulation. + * Fix formatting. + * Update dependencies for husky in rolling and galactic. + * minor + * progress on aws navigation and some other refactorings on navigation clients and behaviors + * more on aws demo + * fixing broken build + * minor + Co-authored-by: Denis Štogl + Co-authored-by: Denis Štogl + * minor changes (#175) + * warehouse2 (#177) + * Waypoint Inputs (#178) + Co-authored-by: Ubuntu 20-04-02-amd64 + * wharehouse2 progress (#179) + * format (#180) + * sm_dance_bot_warehouse_3 (#181) + Co-authored-by: Ubuntu 20-04-02-amd64 + * Feature/sm warehouse 2 13 dec 2 (#182) + * format + * more changes and headless + * merge + * headless and other fixes + * default values + * Brettpac branch (#184) + * sm_dance_bot_warehouse_3 + * Redoing sm_dance_bot_warehouse_3 waypoints + * More Waypoints + Co-authored-by: Ubuntu 20-04-02-amd64 + * SrConditional fixes and formatting (#168) + * fix: some formatting and templating on SrConditional + * fix: move trigger logic into headers + * fix: lint + * Feature/wharehouse2 dec 14 (#185) + * warehouse2 + * minor + * Feature/sm warehouse 2 13 dec 2 (#186) + * format + * more changes and headless + * merge + * headless and other fixes + * default values + * minor + * finetuning waypoints (#187) + Co-authored-by: Ubuntu 20-04-02-amd64 + * Feature/cb pure spinning (#188) + * format + * more changes and headless + * merge + * headless and other fixes + * default values + * minor + * Feature/cb pure spinning (#189) + * format + * more changes and headless + * merge + * headless and other fixes + * default values + * minor + * pure spinning behavior missing files + * minor changes (#190) + * Feature/planner changes 16 12 (#191) + * minor changes + * more fixes + * minor + * minor + * Feature/replanning 16 dec (#193) + * minor changes + * replanning for all our examples + * several fixes (#194) + * minor changes (#195) + * Feature/undo motion 20 12 (#196) + * minor changes + * replanning for all our examples + * improving undo motion navigation warehouse2 + * minor + * tuning warehouse3 (#197) + * Feature/undo motion 20 12 (#198) + * minor changes + * replanning for all our examples + * improving undo motion navigation warehouse2 + * minor + * undo tuning and errors + * format + * Feature/sync 21 12 (#199) + * minor changes + * replanning for all our examples + * format issues + * Feature/warehouse2 22 12 (#200) + * minor changes + * replanning for all our examples + * format issues + * finishing warehouse2 + * Feature/warehouse2 23 12 (#201) + * minor changes + * replanning for all our examples + * tuning and fixes (#202) + * Feature/minor tune (#203) + * tuning and fixes + * minor tune + * fixing warehouse 3 problems, and other core improvements (#204) + * fixing warehouse 3 problems, and other core improvements to remove dead lock, also making continuous integration green + * weird moveit not downloaded repo + * added missing file from warehouse2 (#205) + * backport to foxy + * minor format + * minor linking errors foxy + Co-authored-by: Denis Štogl + Co-authored-by: Denis Štogl + Co-authored-by: Declan Dury <44791484+DecDury@users.noreply.github.com> + Co-authored-by: DecDury + Co-authored-by: reelrbtx + Co-authored-by: brettpac + Co-authored-by: David Revay + * missing + * missing sm + * updating subscriber publisher components + * progress in autowarrrrrrrrrrrrrrrrrrrrrrrrrrrrrrrrrre machine + * refining cp subscriber cp publisher + * Update cb_navigate_global_position.hpp + * improvements in smacc core adding more components mostly developed for autoware demo + * autoware demo + * missing + * foxy ci + * fix + * minor broken build + * Merging code from backport foxy and updates about autoware (#208) + * minor changes + * replanning for all our examples + * backport to foxy + * minor format + * minor linking errors foxy + * Foxy backport (#206) + * minor formatting fixes + * Fix trailing spaces. + * Correct codespell. + * Correct python linters warnings. + * Add galactic CI build because Navigation2 is broken in rolling. + * Add partial changes for ament_cpplint. + * Add tf2_ros as dependency to find include. + * Disable ament_cpplint. + * Disable some packages and update workflows. + * Bump ccache version. + * Ignore further packages + * Satisfy ament_lint_cmake + * Add missing licences. + * Disable cpplint and cppcheck linters. + * Correct formatters. + * branching example + * Disable disabled packages + * Update ci-build-source.yml + * Change extension + * Change extension of imports. + * Enable cppcheck + * Correct formatting of python file. + * Included necessary package and edited Threesome launch + Changed... + ros2 launch sm_three_some sm_three_some + to + ros2 launch sm_three_some sm_three_some.launch + Added: + First ensure you have the necessary package installed. + ``` + sudo apt-get install ros-rolling-ros2trace + ``` + Then run this command. + * Rename header files and correct format. + * Add workflow for checking doc build. + * Update doxygen-check-build.yml + * Create doxygen-deploy.yml + * Use manual deployment for now. + * Create workflow for testing prerelease builds + * Use docs/ as source folder for documentation + * Use docs/ as output directory. + * Rename to smacc2 and smacc2_msgs + * Correct GitHub branch reference. + * Update name of package and package.xml to pass liter. + * Execute on master update + * Reset all versions to 0.0.0 + * Ignore all packages except smacc2 and smacc2_msgs + * Update changelogs + * 0.1.0 + * Revert "Ignore all packages except smacc2 and smacc2_msgs" + This reverts commit f603166a4b3ccdfe96c64d9f9fb9d8b49fbf0e61. + * Update description table. + * Update table + * Copy initial docs + * Dockerfile w/ ROS distro as argument + use this command "sudo docker build --build-arg ROS2_DISTRO=(desiredRosTag) (directoryHoldingDockerfile)/" + * Opened new folder for additional tracing contents + * Delete tracing directory + * Moved tracing.md to tracing directory + * added setupTracing.sh + Installs necessary packages and configures tracing group. + * Removed manual installation of ros-rolling-ros2trace + This is now automated in setupTracing.sh + location of sh file assumed if user follows README.md under "Getting started" + * Created alternative ManualTracing + * added new sm markdowns + * added a dockerfile for Rolling and Galactic + * Update smacc2_ci/docker/ros_rollingAndGalactic_ubuntu_20.04/buildGalactic.sh + Co-authored-by: Denis Štogl + * Update tracing/ManualTracing.md + Co-authored-by: Denis Štogl + * changed wording "smacc application" to "SMACC2 library" + Co-authored-by: Denis Štogl + * Update smacc_sm_reference_library/sm_atomic/README.md + edit from html to markdown syntax + Co-authored-by: Denis Štogl + * reactivating smacc2 nav clients for rolling via submodules + * renamed tracing events after + * bug in smacc2 component + * reverted markdowns to html + * added README tutorial for Dockerfile + * additional cleanup + * cleanup + * cleanup + * edited tracing.md to reflect new tracing event names + * Enable build of missing rolling repositories. + * Enable Navigation2 for semi-binary build. + * Remove galactic builds from master and kepp only rolling. Remove submodules and use .repos file + * updated mentions of SMACC/ROS to SMACC2/ROS2 + * some progress on navigation rolling + * renamed folders, deleted tracing.md, edited README.md + * added smacc2_performance_tools + * performance tests improvements + * more on performance and other issues + * sm_respira_1 format cleanup + * sm_respira_1 format cleanup pre-commit + * sm_respira_test_2 + * sm_respira_test_2 + * more changes on performance tests + * Do not execute clang-format on smacc2_sm_reference_library package. + * sm_reference_library reformatting + * Correct trailing spaces. + * sm_atomic_24hr + * sm_atomic_performance_trace_1 + * Update smacc2_rta command across readmes + * Clean up of sm_atomic_24hr + * more sm_atomic_24hr cleanup + * Optimized deps in move_base_z_planners_common. + * Renaming of event generator library + * minor formatting + * Add galactic CI setup and rename rolling files. (#58) + * Fix source CI and correct README overview. (#62) + * Update c_cpp_properties.json + * changed launch command to ros2 launch sm_respira_1 sm_respira_1.launch (#69) + also noticed a note I had made while producing these that was not removed + * update doxygen links (#70) + Co-authored-by: Ubuntu 20-04-02-amd64 + * More Readme Updates (#72) + Co-authored-by: Ubuntu 20-04-02-amd64 + * More Readme (#74) + Co-authored-by: Ubuntu 20-04-02-amd64 + * created new sm from sm_respira_1 (#76) + * Feature/core and navigation fixes (#78) + * base for the sm_aws_aarehouse navigation + * progressing in aws navigation + * minor + * several core improvements during navigation testing + * formatting improvements + * progress in aws navigation demo + * format improvements + * Feature/aws demo progress (#80) + * base for the sm_aws_aarehouse navigation + * progressing in aws navigation + * minor + * several core improvements during navigation testing + * formatting improvements + * progress in aws navigation demo + * format improvements + * format improvements + * more on navigation + * sm_advanced_recovery_1 reworked (#83) + * sm_advanced_recovery_1 reworked + * fix pre-commit + * Trying to fix Pre-Commit + Co-authored-by: Ubuntu 20-04-02-amd64 + * more sm_advanced_recovery_1 (#84) + Co-authored-by: Ubuntu 20-04-02-amd64 + * More sm_advanced_recovery_1 work (#85) + Co-authored-by: Ubuntu 20-04-02-amd64 + * sm_advanced_recovery_1 round 4 (#86) + Co-authored-by: Ubuntu 20-04-02-amd64 + * Brettpac branch (#87) + * sm_atomic_performance_test_a_2 + * sm_atomic_performance_test_a_1 + Co-authored-by: Ubuntu 20-04-02-amd64 + * sm_atomic_performance_test_c_1 (#88) + Co-authored-by: Ubuntu 20-04-02-amd64 + * modifying sm_atomic_performance_test_a_2 (#89) + Co-authored-by: Ubuntu 20-04-02-amd64 + * sm_multi_stage_1 (#90) + * sm_multi_stage_1 + * fixing precommit + Co-authored-by: Ubuntu 20-04-02-amd64 + * more sm_multi_stage_1 (#91) + Co-authored-by: Ubuntu 20-04-02-amd64 + * Update README.md + updated launch command + * Wait topic message client behavior (#81) + * base for the sm_aws_aarehouse navigation + * progressing in aws navigation + * minor + * several core improvements during navigation testing + * formatting improvements + * progress in aws navigation demo + * format improvements + * format improvements + * more on navigation + * new feature, cb_wait_topic_message: asynchronous client behavior that waits a topic message and optionally checks its contents to success + * formatting + * attempting precommit fixes + Co-authored-by: Ubuntu 20-04-02-amd64 + Co-authored-by: Denis Štogl + * Feature/wait nav2 nodes client behavior (#82) + * base for the sm_aws_aarehouse navigation + * progressing in aws navigation + * minor + * several core improvements during navigation testing + * formatting improvements + * progress in aws navigation demo + * format improvements + * format improvements + * more on navigation + * new feature, cb_wait_topic_message: asynchronous client behavior that waits a topic message and optionally checks its contents to success + * formatting + * adding new client behavior add for nav2, wait nav2 nodes subscribing to the /bond topic and waiting they are alive. you optionally can select the nodes to wait + * Correct all linters and formaters. + Co-authored-by: Denis Štogl + Co-authored-by: Denis Štogl + * Feature/aws demo progress (#92) + * base for the sm_aws_aarehouse navigation + * progressing in aws navigation + * minor + * several core improvements during navigation testing + * formatting improvements + * progress in aws navigation demo + * format improvements + * format improvements + * more on navigation + * new feature, cb_wait_topic_message: asynchronous client behavior that waits a topic message and optionally checks its contents to success + * formatting + * adding new client behavior add for nav2, wait nav2 nodes subscribing to the /bond topic and waiting they are alive. you optionally can select the nodes to wait + * progress in aws navigation demo + * minor format + * Feature/sm dance bot fixes (#93) + * base for the sm_aws_aarehouse navigation + * progressing in aws navigation + * minor + * several core improvements during navigation testing + * formatting improvements + * progress in aws navigation demo + * format improvements + * format improvements + * more on navigation + * new feature, cb_wait_topic_message: asynchronous client behavior that waits a topic message and optionally checks its contents to success + * formatting + * adding new client behavior add for nav2, wait nav2 nodes subscribing to the /bond topic and waiting they are alive. you optionally can select the nodes to wait + * progress in aws navigation demo + * minor format + * navigation parameters fixes on sm_dance_bot + * minor format + * Feature/sm aws warehouse (#94) + * base for the sm_aws_aarehouse navigation + * progressing in aws navigation + * minor + * several core improvements during navigation testing + * formatting improvements + * progress in aws navigation demo + * format improvements + * format improvements + * more on navigation + * new feature, cb_wait_topic_message: asynchronous client behavior that waits a topic message and optionally checks its contents to success + * formatting + * adding new client behavior add for nav2, wait nav2 nodes subscribing to the /bond topic and waiting they are alive. you optionally can select the nodes to wait + * progress in aws navigation demo + * minor format + * merge and progress + * fix format + * Feature/sm dance bot fixes (#95) + * base for the sm_aws_aarehouse navigation + * progressing in aws navigation + * minor + * several core improvements during navigation testing + * formatting improvements + * progress in aws navigation demo + * format improvements + * format improvements + * more on navigation + * new feature, cb_wait_topic_message: asynchronous client behavior that waits a topic message and optionally checks its contents to success + * formatting + * adding new client behavior add for nav2, wait nav2 nodes subscribing to the /bond topic and waiting they are alive. you optionally can select the nodes to wait + * progress in aws navigation demo + * minor format + * navigation parameters fixes on sm_dance_bot + * minor format + * minor + * minor format + * Remove some compile warnings. (#96) + * Feature/cb pause slam (#98) + * base for the sm_aws_aarehouse navigation + * progressing in aws navigation + * minor + * several core improvements during navigation testing + * formatting improvements + * progress in aws navigation demo + * format improvements + * format improvements + * more on navigation + * new feature, cb_wait_topic_message: asynchronous client behavior that waits a topic message and optionally checks its contents to success + * formatting + * adding new client behavior add for nav2, wait nav2 nodes subscribing to the /bond topic and waiting they are alive. you optionally can select the nodes to wait + * progress in aws navigation demo + * minor format + * navigation parameters fixes on sm_dance_bot + * minor format + * minor + * formatting + * cb pause slam client behavior + * sm_dance_bot_lite (#99) + * sm_dance_bot_lite + * precommit + * Updates yaml + Co-authored-by: Ubuntu 20-04-02-amd64 + * Rename doxygen deployment workflow (#100) + * minor hotfix + * sm_dance_bot visualizing turtlebot3 (#101) + * Feature/dance bot launch gz lidar choice (#102) + * sm_dance_bot visualizing turtlebot3 + * cleaning and lidar show/hide option + * cleaning files and making formatting work + * more fixes + * Feature/sm dance bot lite gazebo fixes (#104) + * sm_dance_bot visualizing turtlebot3 + * cleaning and lidar show/hide option + * cleaning files and making formatting work + * more fixes + * gazebo fixes, to show the robot and the lidar + * format fixes + * sm_multi_stage_1 doubling (#103) + Co-authored-by: Ubuntu 20-04-02-amd64 + * Feature/sm dance bot strikes back gazebo fixes (#105) + * sm_dance_bot visualizing turtlebot3 + * cleaning and lidar show/hide option + * cleaning files and making formatting work + * more fixes + * gazebo fixes, to show the robot and the lidar + * format fixes + * gazebo fixes for sm_dance_bot_strikes_back + * precommit cleanup run (#106) + Co-authored-by: Ubuntu 20-04-02-amd64 + * aws demo (#108) + * aws demo + * format + * got sm_multi_stage_1 working (barely) (#109) + Co-authored-by: Ubuntu 20-04-02-amd64 + * Brettpac branch (#110) + * got sm_multi_stage_1 working (barely) + * gaining traction sm_multi_stage_1 + * more + * don't remember + * making progress + * More + * keep hammering + * two stages + * 3 part + * 4th stage + Co-authored-by: Ubuntu 20-04-02-amd64 + * Brettpac branch (#111) + * got sm_multi_stage_1 working (barely) + * gaining traction sm_multi_stage_1 + * more + * don't remember + * making progress + * More + * keep hammering + * two stages + * 3 part + * 4th stage + * 5th stage + Co-authored-by: Ubuntu 20-04-02-amd64 + * a3 (#113) + Co-authored-by: Ubuntu 20-04-02-amd64 + * Remove neo_simulation2 package. (#112) + * Remove neo_simulation2 package. + * Correct formatting. + * Enable source build on PR for testing. + * Adjust build packages of source CI + * more sm_multi_stage_1 (#114) + Co-authored-by: Ubuntu 20-04-02-amd64 + * mm (#115) + Co-authored-by: Ubuntu 20-04-02-amd64 + * diverse improvements navigation and performance (#116) + * diverse improvements navigation and performance + * minor + Co-authored-by: pabloinigoblasco + * Feature/diverse improvemets navigation performance (#117) + * diverse improvements navigation and performance + * minor + * additional linting and formatting + * Remove merge markers from a python file. (#119) + * Feature/slam toggle and smacc deep history (#122) + * progress in navigation, slam toggle client behaviors and slam_toolbox components. Also smacc2::deep_history syntax + * going forward in testing sm_dance_bot introducing slam pausing/resuming funcionality + * feature/more_sm_dance_bot_fixes + * minor format + * minor (#124) + Co-authored-by: Ubuntu 20-04-02-amd64 + * more changes in sm_dance_bot (#125) + * Move method after the method it calls. Otherwise recursion could happen. (#126) + * Feature/dance bot s pattern (#128) + * more changes in sm_dance_bot + * polishing sm_dance_bot and s-pattern + * noticed typo + Finnaly > Finally + * Feature/dance bot s pattern (#129) + * more changes in sm_dance_bot + * polishing sm_dance_bot and s-pattern + * more refinement in sm_dance_bot + * First working version of sm template and template generator. (#127) + * minor tweaks (#130) + Co-authored-by: Ubuntu 20-04-02-amd64 + * Feature/sm dance bot refine (#131) + * more changes in sm_dance_bot + * minor + * Feature/sm dance bot refine 2 (#132) + * more changes in sm_dance_bot + * minor + * build fix + * waypoints navigator bug (#133) + * minor tuning to mitigate overshot issue cases + * progress in the sm_dance_bot tests (#135) + * some more progress on markers cleanup + * minor format issues (#134) + * sm_dance_bot_lite (#136) + Co-authored-by: Ubuntu 20-04-02-amd64 + * Resolve compile wanings (#137) + * Add SM core test (#138) + * minor navigation improvements (#141) + * using local action msgs (#139) + * using local action msgs + * removing sm_dance_bot_msgs + * pending references + * Feature/nitrosz renaming (#144) + * using local action msgs + * removing sm_dance_bot_msgs + * pending references + * navigation 2 stack renaming + * formatting + * added SVGs to READMEs of atomic, dance_bot, and others (#140) + * added remaining SVGs to READMEs (#145) + * added remaining SVGs to READMEs + * precommit cleanup + * Update package list. (#142) + * removing parameters smacc (#147) + * removing parameters smacc + * workflows update + * workflow + * Noticed launch command was incorrect in README.md + fixed launch command for sm_dance_bot_strikes_back and removed some comments I had made in the past. + * Fix CI: format fix python version (#148) + * Add SM Atomic SM generator. (#143) + * Remove node creation and create only a logger. (#149) + * Rolling Docker environment to be executed from any environment (#154) + * Feature/sm dance bot strikes back refactoring (#152) + Co-authored-by: DecDury + Co-authored-by: Denis Štogl + * slight waypoint 4 and iterations changes so robot can complete course (#155) + * Feature/migration moveit client (#151) + * initial migration to smacc2 + * fixing some errors introduced on formatting + * missing dependency + * fixing some more linting warnings + * minor + * removing test from main moveit cmake + * test ur5 + * progressing in the moveit migration testing + * updating format + * adding .reps dependencies and also fixing some build errors + * repos dependency + * adding dependency to ur5 client + * docker refactoring + * minor + * progress on move_it PR + * minor dockerfile test workaround + * improving dockerfile for building local tests + * minor + * fixing compiling issues + * update readme (#164) + * update readme + * more readme updates + * more + Co-authored-by: Ubuntu 20-04-02-amd64 + * initial state machine transition timestamp (#165) + * moved reference library SMs to smacc2_performance_tools (#166) + * moved reference library SMs to smacc2_performance_tools + * pre-commit cleanup + * Add QOS durability to SmaccPublisherClient (#163) + * feat: add qos durability to SmaccPublisherClient + * fix: add a missing colon + * refactor: remove line + * feat: add reliability qos config + * Feature/testing moveit behaviors (#167) + * more testing on moveit + * progress on moveit + * more testing on moveit behaviors + * minor configuration + * fixing pipeline error + * fixing broken master build + * sm_pubsub_1 (#169) + Co-authored-by: Ubuntu 20-04-02-amd64 + * sm_pubsub_1 part 2 (#170) + * sm_pubsub_1 part 2 + * sm_pubsub_1 part 2 + Co-authored-by: Ubuntu 20-04-02-amd64 + * sm_advanced_recovery_1 renaming (#171) + Co-authored-by: Ubuntu 20-04-02-amd64 + * sm_multi_stage_1 reworking (#172) + * multistage modes + * sm_multi_stage sequences + * sm_multi_state_1 steps + * sm_multi_stage_1 sequence d + * sm_multi_stage_1 c sequence + * mode_5_sequence_b + * mode_4_sequence_b + * sm_multi_stage_1 most + * finishing touches 1 + * readme + Co-authored-by: Ubuntu 20-04-02-amd64 + * Feature/aws navigation sm dance bot (#174) + * repo dependency + * husky launch file in sm_dance_bot + * Add dependencies for husky simulation. + * Fix formatting. + * Update dependencies for husky in rolling and galactic. + * minor + * progress on aws navigation and some other refactorings on navigation clients and behaviors + * more on aws demo + * fixing broken build + * minor + Co-authored-by: Denis Štogl + Co-authored-by: Denis Štogl + * minor changes (#175) + * warehouse2 (#177) + * Waypoint Inputs (#178) + Co-authored-by: Ubuntu 20-04-02-amd64 + * wharehouse2 progress (#179) + * format (#180) + * sm_dance_bot_warehouse_3 (#181) + Co-authored-by: Ubuntu 20-04-02-amd64 + * Feature/sm warehouse 2 13 dec 2 (#182) + * format + * more changes and headless + * merge + * headless and other fixes + * default values + * Brettpac branch (#184) + * sm_dance_bot_warehouse_3 + * Redoing sm_dance_bot_warehouse_3 waypoints + * More Waypoints + Co-authored-by: Ubuntu 20-04-02-amd64 + * SrConditional fixes and formatting (#168) + * fix: some formatting and templating on SrConditional + * fix: move trigger logic into headers + * fix: lint + * Feature/wharehouse2 dec 14 (#185) + * warehouse2 + * minor + * Feature/sm warehouse 2 13 dec 2 (#186) + * format + * more changes and headless + * merge + * headless and other fixes + * default values + * minor + * finetuning waypoints (#187) + Co-authored-by: Ubuntu 20-04-02-amd64 + * Feature/cb pure spinning (#188) + * format + * more changes and headless + * merge + * headless and other fixes + * default values + * minor + * Feature/cb pure spinning (#189) + * format + * more changes and headless + * merge + * headless and other fixes + * default values + * minor + * pure spinning behavior missing files + * minor changes (#190) + * Feature/planner changes 16 12 (#191) + * minor changes + * more fixes + * minor + * minor + * Feature/replanning 16 dec (#193) + * minor changes + * replanning for all our examples + * several fixes (#194) + * minor changes (#195) + * Feature/undo motion 20 12 (#196) + * minor changes + * replanning for all our examples + * improving undo motion navigation warehouse2 + * minor + * tuning warehouse3 (#197) + * Feature/undo motion 20 12 (#198) + * minor changes + * replanning for all our examples + * improving undo motion navigation warehouse2 + * minor + * undo tuning and errors + * format + * Feature/sync 21 12 (#199) + * minor changes + * replanning for all our examples + * format issues + * Feature/warehouse2 22 12 (#200) + * minor changes + * replanning for all our examples + * format issues + * finishing warehouse2 + * Feature/warehouse2 23 12 (#201) + * minor changes + * replanning for all our examples + * tuning and fixes (#202) + * Feature/minor tune (#203) + * tuning and fixes + * minor tune + * fixing warehouse 3 problems, and other core improvements (#204) + * fixing warehouse 3 problems, and other core improvements to remove dead lock, also making continuous integration green + * weird moveit not downloaded repo + * added missing file from warehouse2 (#205) + * backport to foxy + * minor format + * minor linking errors foxy + Co-authored-by: Denis Štogl + Co-authored-by: Denis Štogl + Co-authored-by: Declan Dury <44791484+DecDury@users.noreply.github.com> + Co-authored-by: DecDury + Co-authored-by: reelrbtx + Co-authored-by: brettpac + Co-authored-by: David Revay + * missing + * missing sm + * updating subscriber publisher components + * progress in autowarrrrrrrrrrrrrrrrrrrrrrrrrrrrrrrrrre machine + * refining cp subscriber cp publisher + * improvements in smacc core adding more components mostly developed for autoware demo + * autoware demo + * missing + * foxy ci + * fix + * minor broken build + Co-authored-by: Denis Štogl + Co-authored-by: Denis Štogl + Co-authored-by: Declan Dury <44791484+DecDury@users.noreply.github.com> + Co-authored-by: DecDury + Co-authored-by: reelrbtx + Co-authored-by: brettpac + Co-authored-by: David Revay + Co-authored-by: pabloinigoblasco + * Add mergify rules file. + * Try fixing CI for rolling. (#209) + Merging to get backport working. + * some reordering fixes + * Remove example things from Foxy CI setup. (#214) + * Add Autoware Auto Msgs into not-released dependencies. (#220) + * Fix rolling builds (#222) + * do not merge yet - Feature/odom tracker improvements and retry motion (#223) + * odom tracker improvements + * adding forward behavior retry funcionality + * minor + * docker files for different revisions, warnings removval and more testing on navigation + * fixing docker for foxy and galactic + * removing warnings (#213) + * minor changes + * replanning for all our examples + * backport to foxy + * minor format + * minor linking errors foxy + * Foxy backport (#206) + * minor formatting fixes + * Fix trailing spaces. + * Correct codespell. + * Correct python linters warnings. + * Add galactic CI build because Navigation2 is broken in rolling. + * Add partial changes for ament_cpplint. + * Add tf2_ros as dependency to find include. + * Disable ament_cpplint. + * Disable some packages and update workflows. + * Bump ccache version. + * Ignore further packages + * Satisfy ament_lint_cmake + * Add missing licences. + * Disable cpplint and cppcheck linters. + * Correct formatters. + * branching example + * Disable disabled packages + * Update ci-build-source.yml + * Change extension + * Change extension of imports. + * Enable cppcheck + * Correct formatting of python file. + * Included necessary package and edited Threesome launch + Changed... + ros2 launch sm_three_some sm_three_some + to + ros2 launch sm_three_some sm_three_some.launch + Added: + First ensure you have the necessary package installed. + ``` + sudo apt-get install ros-rolling-ros2trace + ``` + Then run this command. + * Rename header files and correct format. + * Add workflow for checking doc build. + * Update doxygen-check-build.yml + * Create doxygen-deploy.yml + * Use manual deployment for now. + * Create workflow for testing prerelease builds + * Use docs/ as source folder for documentation + * Use docs/ as output directory. + * Rename to smacc2 and smacc2_msgs + * Correct GitHub branch reference. + * Update name of package and package.xml to pass liter. + * Execute on master update + * Reset all versions to 0.0.0 + * Ignore all packages except smacc2 and smacc2_msgs + * Update changelogs + * 0.1.0 + * Revert "Ignore all packages except smacc2 and smacc2_msgs" + This reverts commit f603166a4b3ccdfe96c64d9f9fb9d8b49fbf0e61. + * Update description table. + * Update table + * Copy initial docs + * Dockerfile w/ ROS distro as argument + use this command "sudo docker build --build-arg ROS2_DISTRO=(desiredRosTag) (directoryHoldingDockerfile)/" + * Opened new folder for additional tracing contents + * Delete tracing directory + * Moved tracing.md to tracing directory + * added setupTracing.sh + Installs necessary packages and configures tracing group. + * Removed manual installation of ros-rolling-ros2trace + This is now automated in setupTracing.sh + location of sh file assumed if user follows README.md under "Getting started" + * Created alternative ManualTracing + * added new sm markdowns + * added a dockerfile for Rolling and Galactic + * Update smacc2_ci/docker/ros_rollingAndGalactic_ubuntu_20.04/buildGalactic.sh + Co-authored-by: Denis Štogl + * Update tracing/ManualTracing.md + Co-authored-by: Denis Štogl + * changed wording "smacc application" to "SMACC2 library" + Co-authored-by: Denis Štogl + * Update smacc_sm_reference_library/sm_atomic/README.md + edit from html to markdown syntax + Co-authored-by: Denis Štogl + * reactivating smacc2 nav clients for rolling via submodules + * renamed tracing events after + * bug in smacc2 component + * reverted markdowns to html + * added README tutorial for Dockerfile + * additional cleanup + * cleanup + * cleanup + * edited tracing.md to reflect new tracing event names + * Enable build of missing rolling repositories. + * Enable Navigation2 for semi-binary build. + * Remove galactic builds from master and kepp only rolling. Remove submodules and use .repos file + * updated mentions of SMACC/ROS to SMACC2/ROS2 + * some progress on navigation rolling + * renamed folders, deleted tracing.md, edited README.md + * added smacc2_performance_tools + * performance tests improvements + * more on performance and other issues + * sm_respira_1 format cleanup + * sm_respira_1 format cleanup pre-commit + * sm_respira_test_2 + * sm_respira_test_2 + * more changes on performance tests + * Do not execute clang-format on smacc2_sm_reference_library package. + * sm_reference_library reformatting + * Correct trailing spaces. + * sm_atomic_24hr + * sm_atomic_performance_trace_1 + * Update smacc2_rta command across readmes + * Clean up of sm_atomic_24hr + * more sm_atomic_24hr cleanup + * Optimized deps in move_base_z_planners_common. + * Renaming of event generator library + * minor formatting + * Add galactic CI setup and rename rolling files. (#58) + * Fix source CI and correct README overview. (#62) + * Update c_cpp_properties.json + * changed launch command to ros2 launch sm_respira_1 sm_respira_1.launch (#69) + also noticed a note I had made while producing these that was not removed + * update doxygen links (#70) + Co-authored-by: Ubuntu 20-04-02-amd64 + * More Readme Updates (#72) + Co-authored-by: Ubuntu 20-04-02-amd64 + * More Readme (#74) + Co-authored-by: Ubuntu 20-04-02-amd64 + * created new sm from sm_respira_1 (#76) + * Feature/core and navigation fixes (#78) + * base for the sm_aws_aarehouse navigation + * progressing in aws navigation + * minor + * several core improvements during navigation testing + * formatting improvements + * progress in aws navigation demo + * format improvements + * Feature/aws demo progress (#80) + * base for the sm_aws_aarehouse navigation + * progressing in aws navigation + * minor + * several core improvements during navigation testing + * formatting improvements + * progress in aws navigation demo + * format improvements + * format improvements + * more on navigation + * sm_advanced_recovery_1 reworked (#83) + * sm_advanced_recovery_1 reworked + * fix pre-commit + * Trying to fix Pre-Commit + Co-authored-by: Ubuntu 20-04-02-amd64 + * more sm_advanced_recovery_1 (#84) + Co-authored-by: Ubuntu 20-04-02-amd64 + * More sm_advanced_recovery_1 work (#85) + Co-authored-by: Ubuntu 20-04-02-amd64 + * sm_advanced_recovery_1 round 4 (#86) + Co-authored-by: Ubuntu 20-04-02-amd64 + * Brettpac branch (#87) + * sm_atomic_performance_test_a_2 + * sm_atomic_performance_test_a_1 + Co-authored-by: Ubuntu 20-04-02-amd64 + * sm_atomic_performance_test_c_1 (#88) + Co-authored-by: Ubuntu 20-04-02-amd64 + * modifying sm_atomic_performance_test_a_2 (#89) + Co-authored-by: Ubuntu 20-04-02-amd64 + * sm_multi_stage_1 (#90) + * sm_multi_stage_1 + * fixing precommit + Co-authored-by: Ubuntu 20-04-02-amd64 + * more sm_multi_stage_1 (#91) + Co-authored-by: Ubuntu 20-04-02-amd64 + * Update README.md + updated launch command + * Wait topic message client behavior (#81) + * base for the sm_aws_aarehouse navigation + * progressing in aws navigation + * minor + * several core improvements during navigation testing + * formatting improvements + * progress in aws navigation demo + * format improvements + * format improvements + * more on navigation + * new feature, cb_wait_topic_message: asynchronous client behavior that waits a topic message and optionally checks its contents to success + * formatting + * attempting precommit fixes + Co-authored-by: Ubuntu 20-04-02-amd64 + Co-authored-by: Denis Štogl + * Feature/wait nav2 nodes client behavior (#82) + * base for the sm_aws_aarehouse navigation + * progressing in aws navigation + * minor + * several core improvements during navigation testing + * formatting improvements + * progress in aws navigation demo + * format improvements + * format improvements + * more on navigation + * new feature, cb_wait_topic_message: asynchronous client behavior that waits a topic message and optionally checks its contents to success + * formatting + * adding new client behavior add for nav2, wait nav2 nodes subscribing to the /bond topic and waiting they are alive. you optionally can select the nodes to wait + * Correct all linters and formaters. + Co-authored-by: Denis Štogl + Co-authored-by: Denis Štogl + * Feature/aws demo progress (#92) + * base for the sm_aws_aarehouse navigation + * progressing in aws navigation + * minor + * several core improvements during navigation testing + * formatting improvements + * progress in aws navigation demo + * format improvements + * format improvements + * more on navigation + * new feature, cb_wait_topic_message: asynchronous client behavior that waits a topic message and optionally checks its contents to success + * formatting + * adding new client behavior add for nav2, wait nav2 nodes subscribing to the /bond topic and waiting they are alive. you optionally can select the nodes to wait + * progress in aws navigation demo + * minor format + * Feature/sm dance bot fixes (#93) + * base for the sm_aws_aarehouse navigation + * progressing in aws navigation + * minor + * several core improvements during navigation testing + * formatting improvements + * progress in aws navigation demo + * format improvements + * format improvements + * more on navigation + * new feature, cb_wait_topic_message: asynchronous client behavior that waits a topic message and optionally checks its contents to success + * formatting + * adding new client behavior add for nav2, wait nav2 nodes subscribing to the /bond topic and waiting they are alive. you optionally can select the nodes to wait + * progress in aws navigation demo + * minor format + * navigation parameters fixes on sm_dance_bot + * minor format + * Feature/sm aws warehouse (#94) + * base for the sm_aws_aarehouse navigation + * progressing in aws navigation + * minor + * several core improvements during navigation testing + * formatting improvements + * progress in aws navigation demo + * format improvements + * format improvements + * more on navigation + * new feature, cb_wait_topic_message: asynchronous client behavior that waits a topic message and optionally checks its contents to success + * formatting + * adding new client behavior add for nav2, wait nav2 nodes subscribing to the /bond topic and waiting they are alive. you optionally can select the nodes to wait + * progress in aws navigation demo + * minor format + * merge and progress + * fix format + * Feature/sm dance bot fixes (#95) + * base for the sm_aws_aarehouse navigation + * progressing in aws navigation + * minor + * several core improvements during navigation testing + * formatting improvements + * progress in aws navigation demo + * format improvements + * format improvements + * more on navigation + * new feature, cb_wait_topic_message: asynchronous client behavior that waits a topic message and optionally checks its contents to success + * formatting + * adding new client behavior add for nav2, wait nav2 nodes subscribing to the /bond topic and waiting they are alive. you optionally can select the nodes to wait + * progress in aws navigation demo + * minor format + * navigation parameters fixes on sm_dance_bot + * minor format + * minor + * minor format + * Remove some compile warnings. (#96) + * Feature/cb pause slam (#98) + * base for the sm_aws_aarehouse navigation + * progressing in aws navigation + * minor + * several core improvements during navigation testing + * formatting improvements + * progress in aws navigation demo + * format improvements + * format improvements + * more on navigation + * new feature, cb_wait_topic_message: asynchronous client behavior that waits a topic message and optionally checks its contents to success + * formatting + * adding new client behavior add for nav2, wait nav2 nodes subscribing to the /bond topic and waiting they are alive. you optionally can select the nodes to wait + * progress in aws navigation demo + * minor format + * navigation parameters fixes on sm_dance_bot + * minor format + * minor + * formatting + * cb pause slam client behavior + * sm_dance_bot_lite (#99) + * sm_dance_bot_lite + * precommit + * Updates yaml + Co-authored-by: Ubuntu 20-04-02-amd64 + * Rename doxygen deployment workflow (#100) + * minor hotfix + * sm_dance_bot visualizing turtlebot3 (#101) + * Feature/dance bot launch gz lidar choice (#102) + * sm_dance_bot visualizing turtlebot3 + * cleaning and lidar show/hide option + * cleaning files and making formatting work + * more fixes + * Feature/sm dance bot lite gazebo fixes (#104) + * sm_dance_bot visualizing turtlebot3 + * cleaning and lidar show/hide option + * cleaning files and making formatting work + * more fixes + * gazebo fixes, to show the robot and the lidar + * format fixes + * sm_multi_stage_1 doubling (#103) + Co-authored-by: Ubuntu 20-04-02-amd64 + * Feature/sm dance bot strikes back gazebo fixes (#105) + * sm_dance_bot visualizing turtlebot3 + * cleaning and lidar show/hide option + * cleaning files and making formatting work + * more fixes + * gazebo fixes, to show the robot and the lidar + * format fixes + * gazebo fixes for sm_dance_bot_strikes_back + * precommit cleanup run (#106) + Co-authored-by: Ubuntu 20-04-02-amd64 + * aws demo (#108) + * aws demo + * format + * got sm_multi_stage_1 working (barely) (#109) + Co-authored-by: Ubuntu 20-04-02-amd64 + * Brettpac branch (#110) + * got sm_multi_stage_1 working (barely) + * gaining traction sm_multi_stage_1 + * more + * don't remember + * making progress + * More + * keep hammering + * two stages + * 3 part + * 4th stage + Co-authored-by: Ubuntu 20-04-02-amd64 + * Brettpac branch (#111) + * got sm_multi_stage_1 working (barely) + * gaining traction sm_multi_stage_1 + * more + * don't remember + * making progress + * More + * keep hammering + * two stages + * 3 part + * 4th stage + * 5th stage + Co-authored-by: Ubuntu 20-04-02-amd64 + * a3 (#113) + Co-authored-by: Ubuntu 20-04-02-amd64 + * Remove neo_simulation2 package. (#112) + * Remove neo_simulation2 package. + * Correct formatting. + * Enable source build on PR for testing. + * Adjust build packages of source CI + * more sm_multi_stage_1 (#114) + Co-authored-by: Ubuntu 20-04-02-amd64 + * mm (#115) + Co-authored-by: Ubuntu 20-04-02-amd64 + * diverse improvements navigation and performance (#116) + * diverse improvements navigation and performance + * minor + Co-authored-by: pabloinigoblasco + * Feature/diverse improvemets navigation performance (#117) + * diverse improvements navigation and performance + * minor + * additional linting and formatting + * Remove merge markers from a python file. (#119) + * Feature/slam toggle and smacc deep history (#122) + * progress in navigation, slam toggle client behaviors and slam_toolbox components. Also smacc2::deep_history syntax + * going forward in testing sm_dance_bot introducing slam pausing/resuming funcionality + * feature/more_sm_dance_bot_fixes + * minor format + * minor (#124) + Co-authored-by: Ubuntu 20-04-02-amd64 + * more changes in sm_dance_bot (#125) + * Move method after the method it calls. Otherwise recursion could happen. (#126) + * Feature/dance bot s pattern (#128) + * more changes in sm_dance_bot + * polishing sm_dance_bot and s-pattern + * noticed typo + Finnaly > Finally + * Feature/dance bot s pattern (#129) + * more changes in sm_dance_bot + * polishing sm_dance_bot and s-pattern + * more refinement in sm_dance_bot + * First working version of sm template and template generator. (#127) + * minor tweaks (#130) + Co-authored-by: Ubuntu 20-04-02-amd64 + * Feature/sm dance bot refine (#131) + * more changes in sm_dance_bot + * minor + * Feature/sm dance bot refine 2 (#132) + * more changes in sm_dance_bot + * minor + * build fix + * waypoints navigator bug (#133) + * minor tuning to mitigate overshot issue cases + * progress in the sm_dance_bot tests (#135) + * some more progress on markers cleanup + * minor format issues (#134) + * sm_dance_bot_lite (#136) + Co-authored-by: Ubuntu 20-04-02-amd64 + * Resolve compile wanings (#137) + * Add SM core test (#138) + * minor navigation improvements (#141) + * using local action msgs (#139) + * using local action msgs + * removing sm_dance_bot_msgs + * pending references + * Feature/nitrosz renaming (#144) + * using local action msgs + * removing sm_dance_bot_msgs + * pending references + * navigation 2 stack renaming + * formatting + * added SVGs to READMEs of atomic, dance_bot, and others (#140) + * added remaining SVGs to READMEs (#145) + * added remaining SVGs to READMEs + * precommit cleanup + * Update package list. (#142) + * removing parameters smacc (#147) + * removing parameters smacc + * workflows update + * workflow + * Noticed launch command was incorrect in README.md + fixed launch command for sm_dance_bot_strikes_back and removed some comments I had made in the past. + * Fix CI: format fix python version (#148) + * Add SM Atomic SM generator. (#143) + * Remove node creation and create only a logger. (#149) + * Rolling Docker environment to be executed from any environment (#154) + * Feature/sm dance bot strikes back refactoring (#152) + Co-authored-by: DecDury + Co-authored-by: Denis Štogl + * slight waypoint 4 and iterations changes so robot can complete course (#155) + * Feature/migration moveit client (#151) + * initial migration to smacc2 + * fixing some errors introduced on formatting + * missing dependency + * fixing some more linting warnings + * minor + * removing test from main moveit cmake + * test ur5 + * progressing in the moveit migration testing + * updating format + * adding .reps dependencies and also fixing some build errors + * repos dependency + * adding dependency to ur5 client + * docker refactoring + * minor + * progress on move_it PR + * minor dockerfile test workaround + * improving dockerfile for building local tests + * minor + * fixing compiling issues + * update readme (#164) + * update readme + * more readme updates + * more + Co-authored-by: Ubuntu 20-04-02-amd64 + * initial state machine transition timestamp (#165) + * moved reference library SMs to smacc2_performance_tools (#166) + * moved reference library SMs to smacc2_performance_tools + * pre-commit cleanup + * Add QOS durability to SmaccPublisherClient (#163) + * feat: add qos durability to SmaccPublisherClient + * fix: add a missing colon + * refactor: remove line + * feat: add reliability qos config + * Feature/testing moveit behaviors (#167) + * more testing on moveit + * progress on moveit + * more testing on moveit behaviors + * minor configuration + * fixing pipeline error + * fixing broken master build + * sm_pubsub_1 (#169) + Co-authored-by: Ubuntu 20-04-02-amd64 + * sm_pubsub_1 part 2 (#170) + * sm_pubsub_1 part 2 + * sm_pubsub_1 part 2 + Co-authored-by: Ubuntu 20-04-02-amd64 + * sm_advanced_recovery_1 renaming (#171) + Co-authored-by: Ubuntu 20-04-02-amd64 + * sm_multi_stage_1 reworking (#172) + * multistage modes + * sm_multi_stage sequences + * sm_multi_state_1 steps + * sm_multi_stage_1 sequence d + * sm_multi_stage_1 c sequence + * mode_5_sequence_b + * mode_4_sequence_b + * sm_multi_stage_1 most + * finishing touches 1 + * readme + Co-authored-by: Ubuntu 20-04-02-amd64 + * Feature/aws navigation sm dance bot (#174) + * repo dependency + * husky launch file in sm_dance_bot + * Add dependencies for husky simulation. + * Fix formatting. + * Update dependencies for husky in rolling and galactic. + * minor + * progress on aws navigation and some other refactorings on navigation clients and behaviors + * more on aws demo + * fixing broken build + * minor + Co-authored-by: Denis Štogl + Co-authored-by: Denis Štogl + * minor changes (#175) + * warehouse2 (#177) + * Waypoint Inputs (#178) + Co-authored-by: Ubuntu 20-04-02-amd64 + * wharehouse2 progress (#179) + * format (#180) + * sm_dance_bot_warehouse_3 (#181) + Co-authored-by: Ubuntu 20-04-02-amd64 + * Feature/sm warehouse 2 13 dec 2 (#182) + * format + * more changes and headless + * merge + * headless and other fixes + * default values + * Brettpac branch (#184) + * sm_dance_bot_warehouse_3 + * Redoing sm_dance_bot_warehouse_3 waypoints + * More Waypoints + Co-authored-by: Ubuntu 20-04-02-amd64 + * SrConditional fixes and formatting (#168) + * fix: some formatting and templating on SrConditional + * fix: move trigger logic into headers + * fix: lint + * Feature/wharehouse2 dec 14 (#185) + * warehouse2 + * minor + * Feature/sm warehouse 2 13 dec 2 (#186) + * format + * more changes and headless + * merge + * headless and other fixes + * default values + * minor + * finetuning waypoints (#187) + Co-authored-by: Ubuntu 20-04-02-amd64 + * Feature/cb pure spinning (#188) + * format + * more changes and headless + * merge + * headless and other fixes + * default values + * minor + * Feature/cb pure spinning (#189) + * format + * more changes and headless + * merge + * headless and other fixes + * default values + * minor + * pure spinning behavior missing files + * minor changes (#190) + * Feature/planner changes 16 12 (#191) + * minor changes + * more fixes + * minor + * minor + * Feature/replanning 16 dec (#193) + * minor changes + * replanning for all our examples + * several fixes (#194) + * minor changes (#195) + * Feature/undo motion 20 12 (#196) + * minor changes + * replanning for all our examples + * improving undo motion navigation warehouse2 + * minor + * tuning warehouse3 (#197) + * Feature/undo motion 20 12 (#198) + * minor changes + * replanning for all our examples + * improving undo motion navigation warehouse2 + * minor + * undo tuning and errors + * format + * Feature/sync 21 12 (#199) + * minor changes + * replanning for all our examples + * format issues + * Feature/warehouse2 22 12 (#200) + * minor changes + * replanning for all our examples + * format issues + * finishing warehouse2 + * Feature/warehouse2 23 12 (#201) + * minor changes + * replanning for all our examples + * tuning and fixes (#202) + * Feature/minor tune (#203) + * tuning and fixes + * minor tune + * fixing warehouse 3 problems, and other core improvements (#204) + * fixing warehouse 3 problems, and other core improvements to remove dead lock, also making continuous integration green + * weird moveit not downloaded repo + * added missing file from warehouse2 (#205) + * backport to foxy + * minor format + * minor linking errors foxy + Co-authored-by: Denis Štogl + Co-authored-by: Denis Štogl + Co-authored-by: Declan Dury <44791484+DecDury@users.noreply.github.com> + Co-authored-by: DecDury + Co-authored-by: reelrbtx + Co-authored-by: brettpac + Co-authored-by: David Revay + * missing + * missing sm + * updating subscriber publisher components + * progress in autowarrrrrrrrrrrrrrrrrrrrrrrrrrrrrrrrrre machine + * refining cp subscriber cp publisher + * improvements in smacc core adding more components mostly developed for autoware demo + * autoware demo + * missing + * foxy ci + * fix + * minor broken build + * some reordering fixes + * minor + * docker files for different revisions, warnings removval and more testing on navigation + * fixing docker for foxy and galactic + Co-authored-by: Denis Štogl + Co-authored-by: Denis Štogl + Co-authored-by: Declan Dury <44791484+DecDury@users.noreply.github.com> + Co-authored-by: DecDury + Co-authored-by: reelrbtx + Co-authored-by: brettpac + Co-authored-by: David Revay + Co-authored-by: pabloinigoblasco + * docker build files for all versions + * dockerfiles (#225) + * Fix code generators (#221) + * Fix other build issues. + * Update SM template and make example code clearly visible. + * Remove use of node in the sm performance template. + * Updated templated to use Blackboard storage. + * Update template to resolve the global data correctly. + * Update sm_name.hpp + Co-authored-by: Pablo Iñigo Blasco + * Feature/retry behavior warehouse 1 (#226) + * minor changes + * replanning for all our examples + * backport to foxy + * minor format + * minor linking errors foxy + * Foxy backport (#206) + * minor formatting fixes + * Fix trailing spaces. + * Correct codespell. + * Correct python linters warnings. + * Add galactic CI build because Navigation2 is broken in rolling. + * Add partial changes for ament_cpplint. + * Add tf2_ros as dependency to find include. + * Disable ament_cpplint. + * Disable some packages and update workflows. + * Bump ccache version. + * Ignore further packages + * Satisfy ament_lint_cmake + * Add missing licences. + * Disable cpplint and cppcheck linters. + * Correct formatters. + * branching example + * Disable disabled packages + * Update ci-build-source.yml + * Change extension + * Change extension of imports. + * Enable cppcheck + * Correct formatting of python file. + * Included necessary package and edited Threesome launch + Changed... + ros2 launch sm_three_some sm_three_some + to + ros2 launch sm_three_some sm_three_some.launch + Added: + First ensure you have the necessary package installed. + ``` + sudo apt-get install ros-rolling-ros2trace + ``` + Then run this command. + * Rename header files and correct format. + * Add workflow for checking doc build. + * Update doxygen-check-build.yml + * Create doxygen-deploy.yml + * Use manual deployment for now. + * Create workflow for testing prerelease builds + * Use docs/ as source folder for documentation + * Use docs/ as output directory. + * Rename to smacc2 and smacc2_msgs + * Correct GitHub branch reference. + * Update name of package and package.xml to pass liter. + * Execute on master update + * Reset all versions to 0.0.0 + * Ignore all packages except smacc2 and smacc2_msgs + * Update changelogs + * 0.1.0 + * Revert "Ignore all packages except smacc2 and smacc2_msgs" + This reverts commit f603166a4b3ccdfe96c64d9f9fb9d8b49fbf0e61. + * Update description table. + * Update table + * Copy initial docs + * Dockerfile w/ ROS distro as argument + use this command "sudo docker build --build-arg ROS2_DISTRO=(desiredRosTag) (directoryHoldingDockerfile)/" + * Opened new folder for additional tracing contents + * Delete tracing directory + * Moved tracing.md to tracing directory + * added setupTracing.sh + Installs necessary packages and configures tracing group. + * Removed manual installation of ros-rolling-ros2trace + This is now automated in setupTracing.sh + location of sh file assumed if user follows README.md under "Getting started" + * Created alternative ManualTracing + * added new sm markdowns + * added a dockerfile for Rolling and Galactic + * Update smacc2_ci/docker/ros_rollingAndGalactic_ubuntu_20.04/buildGalactic.sh + Co-authored-by: Denis Štogl + * Update tracing/ManualTracing.md + Co-authored-by: Denis Štogl + * changed wording "smacc application" to "SMACC2 library" + Co-authored-by: Denis Štogl + * Update smacc_sm_reference_library/sm_atomic/README.md + edit from html to markdown syntax + Co-authored-by: Denis Štogl + * reactivating smacc2 nav clients for rolling via submodules + * renamed tracing events after + * bug in smacc2 component + * reverted markdowns to html + * added README tutorial for Dockerfile + * additional cleanup + * cleanup + * cleanup + * edited tracing.md to reflect new tracing event names + * Enable build of missing rolling repositories. + * Enable Navigation2 for semi-binary build. + * Remove galactic builds from master and kepp only rolling. Remove submodules and use .repos file + * updated mentions of SMACC/ROS to SMACC2/ROS2 + * some progress on navigation rolling + * renamed folders, deleted tracing.md, edited README.md + * added smacc2_performance_tools + * performance tests improvements + * more on performance and other issues + * sm_respira_1 format cleanup + * sm_respira_1 format cleanup pre-commit + * sm_respira_test_2 + * sm_respira_test_2 + * more changes on performance tests + * Do not execute clang-format on smacc2_sm_reference_library package. + * sm_reference_library reformatting + * Correct trailing spaces. + * sm_atomic_24hr + * sm_atomic_performance_trace_1 + * Update smacc2_rta command across readmes + * Clean up of sm_atomic_24hr + * more sm_atomic_24hr cleanup + * Optimized deps in move_base_z_planners_common. + * Renaming of event generator library + * minor formatting + * Add galactic CI setup and rename rolling files. (#58) + * Fix source CI and correct README overview. (#62) + * Update c_cpp_properties.json + * changed launch command to ros2 launch sm_respira_1 sm_respira_1.launch (#69) + also noticed a note I had made while producing these that was not removed + * update doxygen links (#70) + Co-authored-by: Ubuntu 20-04-02-amd64 + * More Readme Updates (#72) + Co-authored-by: Ubuntu 20-04-02-amd64 + * More Readme (#74) + Co-authored-by: Ubuntu 20-04-02-amd64 + * created new sm from sm_respira_1 (#76) + * Feature/core and navigation fixes (#78) + * base for the sm_aws_aarehouse navigation + * progressing in aws navigation + * minor + * several core improvements during navigation testing + * formatting improvements + * progress in aws navigation demo + * format improvements + * Feature/aws demo progress (#80) + * base for the sm_aws_aarehouse navigation + * progressing in aws navigation + * minor + * several core improvements during navigation testing + * formatting improvements + * progress in aws navigation demo + * format improvements + * format improvements + * more on navigation + * sm_advanced_recovery_1 reworked (#83) + * sm_advanced_recovery_1 reworked + * fix pre-commit + * Trying to fix Pre-Commit + Co-authored-by: Ubuntu 20-04-02-amd64 + * more sm_advanced_recovery_1 (#84) + Co-authored-by: Ubuntu 20-04-02-amd64 + * More sm_advanced_recovery_1 work (#85) + Co-authored-by: Ubuntu 20-04-02-amd64 + * sm_advanced_recovery_1 round 4 (#86) + Co-authored-by: Ubuntu 20-04-02-amd64 + * Brettpac branch (#87) + * sm_atomic_performance_test_a_2 + * sm_atomic_performance_test_a_1 + Co-authored-by: Ubuntu 20-04-02-amd64 + * sm_atomic_performance_test_c_1 (#88) + Co-authored-by: Ubuntu 20-04-02-amd64 + * modifying sm_atomic_performance_test_a_2 (#89) + Co-authored-by: Ubuntu 20-04-02-amd64 + * sm_multi_stage_1 (#90) + * sm_multi_stage_1 + * fixing precommit + Co-authored-by: Ubuntu 20-04-02-amd64 + * more sm_multi_stage_1 (#91) + Co-authored-by: Ubuntu 20-04-02-amd64 + * Update README.md + updated launch command + * Wait topic message client behavior (#81) + * base for the sm_aws_aarehouse navigation + * progressing in aws navigation + * minor + * several core improvements during navigation testing + * formatting improvements + * progress in aws navigation demo + * format improvements + * format improvements + * more on navigation + * new feature, cb_wait_topic_message: asynchronous client behavior that waits a topic message and optionally checks its contents to success + * formatting + * attempting precommit fixes + Co-authored-by: Ubuntu 20-04-02-amd64 + Co-authored-by: Denis Štogl + * Feature/wait nav2 nodes client behavior (#82) + * base for the sm_aws_aarehouse navigation + * progressing in aws navigation + * minor + * several core improvements during navigation testing + * formatting improvements + * progress in aws navigation demo + * format improvements + * format improvements + * more on navigation + * new feature, cb_wait_topic_message: asynchronous client behavior that waits a topic message and optionally checks its contents to success + * formatting + * adding new client behavior add for nav2, wait nav2 nodes subscribing to the /bond topic and waiting they are alive. you optionally can select the nodes to wait + * Correct all linters and formaters. + Co-authored-by: Denis Štogl + Co-authored-by: Denis Štogl + * Feature/aws demo progress (#92) + * base for the sm_aws_aarehouse navigation + * progressing in aws navigation + * minor + * several core improvements during navigation testing + * formatting improvements + * progress in aws navigation demo + * format improvements + * format improvements + * more on navigation + * new feature, cb_wait_topic_message: asynchronous client behavior that waits a topic message and optionally checks its contents to success + * formatting + * adding new client behavior add for nav2, wait nav2 nodes subscribing to the /bond topic and waiting they are alive. you optionally can select the nodes to wait + * progress in aws navigation demo + * minor format + * Feature/sm dance bot fixes (#93) + * base for the sm_aws_aarehouse navigation + * progressing in aws navigation + * minor + * several core improvements during navigation testing + * formatting improvements + * progress in aws navigation demo + * format improvements + * format improvements + * more on navigation + * new feature, cb_wait_topic_message: asynchronous client behavior that waits a topic message and optionally checks its contents to success + * formatting + * adding new client behavior add for nav2, wait nav2 nodes subscribing to the /bond topic and waiting they are alive. you optionally can select the nodes to wait + * progress in aws navigation demo + * minor format + * navigation parameters fixes on sm_dance_bot + * minor format + * Feature/sm aws warehouse (#94) + * base for the sm_aws_aarehouse navigation + * progressing in aws navigation + * minor + * several core improvements during navigation testing + * formatting improvements + * progress in aws navigation demo + * format improvements + * format improvements + * more on navigation + * new feature, cb_wait_topic_message: asynchronous client behavior that waits a topic message and optionally checks its contents to success + * formatting + * adding new client behavior add for nav2, wait nav2 nodes subscribing to the /bond topic and waiting they are alive. you optionally can select the nodes to wait + * progress in aws navigation demo + * minor format + * merge and progress + * fix format + * Feature/sm dance bot fixes (#95) + * base for the sm_aws_aarehouse navigation + * progressing in aws navigation + * minor + * several core improvements during navigation testing + * formatting improvements + * progress in aws navigation demo + * format improvements + * format improvements + * more on navigation + * new feature, cb_wait_topic_message: asynchronous client behavior that waits a topic message and optionally checks its contents to success + * formatting + * adding new client behavior add for nav2, wait nav2 nodes subscribing to the /bond topic and waiting they are alive. you optionally can select the nodes to wait + * progress in aws navigation demo + * minor format + * navigation parameters fixes on sm_dance_bot + * minor format + * minor + * minor format + * Remove some compile warnings. (#96) + * Feature/cb pause slam (#98) + * base for the sm_aws_aarehouse navigation + * progressing in aws navigation + * minor + * several core improvements during navigation testing + * formatting improvements + * progress in aws navigation demo + * format improvements + * format improvements + * more on navigation + * new feature, cb_wait_topic_message: asynchronous client behavior that waits a topic message and optionally checks its contents to success + * formatting + * adding new client behavior add for nav2, wait nav2 nodes subscribing to the /bond topic and waiting they are alive. you optionally can select the nodes to wait + * progress in aws navigation demo + * minor format + * navigation parameters fixes on sm_dance_bot + * minor format + * minor + * formatting + * cb pause slam client behavior + * sm_dance_bot_lite (#99) + * sm_dance_bot_lite + * precommit + * Updates yaml + Co-authored-by: Ubuntu 20-04-02-amd64 + * Rename doxygen deployment workflow (#100) + * minor hotfix + * sm_dance_bot visualizing turtlebot3 (#101) + * Feature/dance bot launch gz lidar choice (#102) + * sm_dance_bot visualizing turtlebot3 + * cleaning and lidar show/hide option + * cleaning files and making formatting work + * more fixes + * Feature/sm dance bot lite gazebo fixes (#104) + * sm_dance_bot visualizing turtlebot3 + * cleaning and lidar show/hide option + * cleaning files and making formatting work + * more fixes + * gazebo fixes, to show the robot and the lidar + * format fixes + * sm_multi_stage_1 doubling (#103) + Co-authored-by: Ubuntu 20-04-02-amd64 + * Feature/sm dance bot strikes back gazebo fixes (#105) + * sm_dance_bot visualizing turtlebot3 + * cleaning and lidar show/hide option + * cleaning files and making formatting work + * more fixes + * gazebo fixes, to show the robot and the lidar + * format fixes + * gazebo fixes for sm_dance_bot_strikes_back + * precommit cleanup run (#106) + Co-authored-by: Ubuntu 20-04-02-amd64 + * aws demo (#108) + * aws demo + * format + * got sm_multi_stage_1 working (barely) (#109) + Co-authored-by: Ubuntu 20-04-02-amd64 + * Brettpac branch (#110) + * got sm_multi_stage_1 working (barely) + * gaining traction sm_multi_stage_1 + * more + * don't remember + * making progress + * More + * keep hammering + * two stages + * 3 part + * 4th stage + Co-authored-by: Ubuntu 20-04-02-amd64 + * Brettpac branch (#111) + * got sm_multi_stage_1 working (barely) + * gaining traction sm_multi_stage_1 + * more + * don't remember + * making progress + * More + * keep hammering + * two stages + * 3 part + * 4th stage + * 5th stage + Co-authored-by: Ubuntu 20-04-02-amd64 + * a3 (#113) + Co-authored-by: Ubuntu 20-04-02-amd64 + * Remove neo_simulation2 package. (#112) + * Remove neo_simulation2 package. + * Correct formatting. + * Enable source build on PR for testing. + * Adjust build packages of source CI + * more sm_multi_stage_1 (#114) + Co-authored-by: Ubuntu 20-04-02-amd64 + * mm (#115) + Co-authored-by: Ubuntu 20-04-02-amd64 + * diverse improvements navigation and performance (#116) + * diverse improvements navigation and performance + * minor + Co-authored-by: pabloinigoblasco + * Feature/diverse improvemets navigation performance (#117) + * diverse improvements navigation and performance + * minor + * additional linting and formatting + * Remove merge markers from a python file. (#119) + * Feature/slam toggle and smacc deep history (#122) + * progress in navigation, slam toggle client behaviors and slam_toolbox components. Also smacc2::deep_history syntax + * going forward in testing sm_dance_bot introducing slam pausing/resuming funcionality + * feature/more_sm_dance_bot_fixes + * minor format + * minor (#124) + Co-authored-by: Ubuntu 20-04-02-amd64 + * more changes in sm_dance_bot (#125) + * Move method after the method it calls. Otherwise recursion could happen. (#126) + * Feature/dance bot s pattern (#128) + * more changes in sm_dance_bot + * polishing sm_dance_bot and s-pattern + * noticed typo + Finnaly > Finally + * Feature/dance bot s pattern (#129) + * more changes in sm_dance_bot + * polishing sm_dance_bot and s-pattern + * more refinement in sm_dance_bot + * First working version of sm template and template generator. (#127) + * minor tweaks (#130) + Co-authored-by: Ubuntu 20-04-02-amd64 + * Feature/sm dance bot refine (#131) + * more changes in sm_dance_bot + * minor + * Feature/sm dance bot refine 2 (#132) + * more changes in sm_dance_bot + * minor + * build fix + * waypoints navigator bug (#133) + * minor tuning to mitigate overshot issue cases + * progress in the sm_dance_bot tests (#135) + * some more progress on markers cleanup + * minor format issues (#134) + * sm_dance_bot_lite (#136) + Co-authored-by: Ubuntu 20-04-02-amd64 + * Resolve compile wanings (#137) + * Add SM core test (#138) + * minor navigation improvements (#141) + * using local action msgs (#139) + * using local action msgs + * removing sm_dance_bot_msgs + * pending references + * Feature/nitrosz renaming (#144) + * using local action msgs + * removing sm_dance_bot_msgs + * pending references + * navigation 2 stack renaming + * formatting + * added SVGs to READMEs of atomic, dance_bot, and others (#140) + * added remaining SVGs to READMEs (#145) + * added remaining SVGs to READMEs + * precommit cleanup + * Update package list. (#142) + * removing parameters smacc (#147) + * removing parameters smacc + * workflows update + * workflow + * Noticed launch command was incorrect in README.md + fixed launch command for sm_dance_bot_strikes_back and removed some comments I had made in the past. + * Fix CI: format fix python version (#148) + * Add SM Atomic SM generator. (#143) + * Remove node creation and create only a logger. (#149) + * Rolling Docker environment to be executed from any environment (#154) + * Feature/sm dance bot strikes back refactoring (#152) + Co-authored-by: DecDury + Co-authored-by: Denis Štogl + * slight waypoint 4 and iterations changes so robot can complete course (#155) + * Feature/migration moveit client (#151) + * initial migration to smacc2 + * fixing some errors introduced on formatting + * missing dependency + * fixing some more linting warnings + * minor + * removing test from main moveit cmake + * test ur5 + * progressing in the moveit migration testing + * updating format + * adding .reps dependencies and also fixing some build errors + * repos dependency + * adding dependency to ur5 client + * docker refactoring + * minor + * progress on move_it PR + * minor dockerfile test workaround + * improving dockerfile for building local tests + * minor + * fixing compiling issues + * update readme (#164) + * update readme + * more readme updates + * more + Co-authored-by: Ubuntu 20-04-02-amd64 + * initial state machine transition timestamp (#165) + * moved reference library SMs to smacc2_performance_tools (#166) + * moved reference library SMs to smacc2_performance_tools + * pre-commit cleanup + * Add QOS durability to SmaccPublisherClient (#163) + * feat: add qos durability to SmaccPublisherClient + * fix: add a missing colon + * refactor: remove line + * feat: add reliability qos config + * Feature/testing moveit behaviors (#167) + * more testing on moveit + * progress on moveit + * more testing on moveit behaviors + * minor configuration + * fixing pipeline error + * fixing broken master build + * sm_pubsub_1 (#169) + Co-authored-by: Ubuntu 20-04-02-amd64 + * sm_pubsub_1 part 2 (#170) + * sm_pubsub_1 part 2 + * sm_pubsub_1 part 2 + Co-authored-by: Ubuntu 20-04-02-amd64 + * sm_advanced_recovery_1 renaming (#171) + Co-authored-by: Ubuntu 20-04-02-amd64 + * sm_multi_stage_1 reworking (#172) + * multistage modes + * sm_multi_stage sequences + * sm_multi_state_1 steps + * sm_multi_stage_1 sequence d + * sm_multi_stage_1 c sequence + * mode_5_sequence_b + * mode_4_sequence_b + * sm_multi_stage_1 most + * finishing touches 1 + * readme + Co-authored-by: Ubuntu 20-04-02-amd64 + * Feature/aws navigation sm dance bot (#174) + * repo dependency + * husky launch file in sm_dance_bot + * Add dependencies for husky simulation. + * Fix formatting. + * Update dependencies for husky in rolling and galactic. + * minor + * progress on aws navigation and some other refactorings on navigation clients and behaviors + * more on aws demo + * fixing broken build + * minor + Co-authored-by: Denis Štogl + Co-authored-by: Denis Štogl + * minor changes (#175) + * warehouse2 (#177) + * Waypoint Inputs (#178) + Co-authored-by: Ubuntu 20-04-02-amd64 + * wharehouse2 progress (#179) + * format (#180) + * sm_dance_bot_warehouse_3 (#181) + Co-authored-by: Ubuntu 20-04-02-amd64 + * Feature/sm warehouse 2 13 dec 2 (#182) + * format + * more changes and headless + * merge + * headless and other fixes + * default values + * Brettpac branch (#184) + * sm_dance_bot_warehouse_3 + * Redoing sm_dance_bot_warehouse_3 waypoints + * More Waypoints + Co-authored-by: Ubuntu 20-04-02-amd64 + * SrConditional fixes and formatting (#168) + * fix: some formatting and templating on SrConditional + * fix: move trigger logic into headers + * fix: lint + * Feature/wharehouse2 dec 14 (#185) + * warehouse2 + * minor + * Feature/sm warehouse 2 13 dec 2 (#186) + * format + * more changes and headless + * merge + * headless and other fixes + * default values + * minor + * finetuning waypoints (#187) + Co-authored-by: Ubuntu 20-04-02-amd64 + * Feature/cb pure spinning (#188) + * format + * more changes and headless + * merge + * headless and other fixes + * default values + * minor + * Feature/cb pure spinning (#189) + * format + * more changes and headless + * merge + * headless and other fixes + * default values + * minor + * pure spinning behavior missing files + * minor changes (#190) + * Feature/planner changes 16 12 (#191) + * minor changes + * more fixes + * minor + * minor + * Feature/replanning 16 dec (#193) + * minor changes + * replanning for all our examples + * several fixes (#194) + * minor changes (#195) + * Feature/undo motion 20 12 (#196) + * minor changes + * replanning for all our examples + * improving undo motion navigation warehouse2 + * minor + * tuning warehouse3 (#197) + * Feature/undo motion 20 12 (#198) + * minor changes + * replanning for all our examples + * improving undo motion navigation warehouse2 + * minor + * undo tuning and errors + * format + * Feature/sync 21 12 (#199) + * minor changes + * replanning for all our examples + * format issues + * Feature/warehouse2 22 12 (#200) + * minor changes + * replanning for all our examples + * format issues + * finishing warehouse2 + * Feature/warehouse2 23 12 (#201) + * minor changes + * replanning for all our examples + * tuning and fixes (#202) + * Feature/minor tune (#203) + * tuning and fixes + * minor tune + * fixing warehouse 3 problems, and other core improvements (#204) + * fixing warehouse 3 problems, and other core improvements to remove dead lock, also making continuous integration green + * weird moveit not downloaded repo + * added missing file from warehouse2 (#205) + * backport to foxy + * minor format + * minor linking errors foxy + Co-authored-by: Denis Štogl + Co-authored-by: Denis Štogl + Co-authored-by: Declan Dury <44791484+DecDury@users.noreply.github.com> + Co-authored-by: DecDury + Co-authored-by: reelrbtx + Co-authored-by: brettpac + Co-authored-by: David Revay + * missing + * missing sm + * updating subscriber publisher components + * progress in autowarrrrrrrrrrrrrrrrrrrrrrrrrrrrrrrrrre machine + * refining cp subscriber cp publisher + * improvements in smacc core adding more components mostly developed for autoware demo + * autoware demo + * missing + * foxy ci + * fix + * minor broken build + * some reordering fixes + * minor + * docker files for different revisions, warnings removval and more testing on navigation + * fixing docker for foxy and galactic + * Update file for fake hardware simulation and add file for gazebo simulation. + * docker build files for all versions + * retry behavior warehouse 1 + * missing file + * minor format fix + * other minor changes + Co-authored-by: Denis Štogl + Co-authored-by: Denis Štogl + Co-authored-by: Declan Dury <44791484+DecDury@users.noreply.github.com> + Co-authored-by: DecDury + Co-authored-by: reelrbtx + Co-authored-by: brettpac + Co-authored-by: David Revay + Co-authored-by: pabloinigoblasco + * barrel demo + * barrel search build fix and warehouse3 + * fixing startup problems in warehouse 3 + * fix broken source build (#227) + * fixing format and minor + * minor + * progress in barrel husky + * minor + * Only rolling version should be pre-released on on master. (#230) + * barrel demo + * minor + * barrel search updates + * making models local + * red picuup + * Correct Focal-Rolling builds by fixing the version of rosdep yaml (#234) + * multiple controllable leds plugin + * progress in husky demo + * Update file for fake hardware simulation and add file for gazebo simulation. (#224) + * Update file for fake hardware simulation and add file for gazebo simulation. + * Add ignition file and update repos files. + * progressing in husky demo + * improving navigation behaviors + * Feature/improvements warehouse3 (#228) + * minor changes + * replanning for all our examples + * backport to foxy + * minor format + * minor linking errors foxy + * Foxy backport (#206) + * minor formatting fixes + * Fix trailing spaces. + * Correct codespell. + * Correct python linters warnings. + * Add galactic CI build because Navigation2 is broken in rolling. + * Add partial changes for ament_cpplint. + * Add tf2_ros as dependency to find include. + * Disable ament_cpplint. + * Disable some packages and update workflows. + * Bump ccache version. + * Ignore further packages + * Satisfy ament_lint_cmake + * Add missing licences. + * Disable cpplint and cppcheck linters. + * Correct formatters. + * branching example + * Disable disabled packages + * Update ci-build-source.yml + * Change extension + * Change extension of imports. + * Enable cppcheck + * Correct formatting of python file. + * Included necessary package and edited Threesome launch + Changed... + ros2 launch sm_three_some sm_three_some + to + ros2 launch sm_three_some sm_three_some.launch + Added: + First ensure you have the necessary package installed. + ``` + sudo apt-get install ros-rolling-ros2trace + ``` + Then run this command. + * Rename header files and correct format. + * Add workflow for checking doc build. + * Update doxygen-check-build.yml + * Create doxygen-deploy.yml + * Use manual deployment for now. + * Create workflow for testing prerelease builds + * Use docs/ as source folder for documentation + * Use docs/ as output directory. + * Rename to smacc2 and smacc2_msgs + * Correct GitHub branch reference. + * Update name of package and package.xml to pass liter. + * Execute on master update + * Reset all versions to 0.0.0 + * Ignore all packages except smacc2 and smacc2_msgs + * Update changelogs + * 0.1.0 + * Revert "Ignore all packages except smacc2 and smacc2_msgs" + This reverts commit f603166a4b3ccdfe96c64d9f9fb9d8b49fbf0e61. + * Update description table. + * Update table + * Copy initial docs + * Dockerfile w/ ROS distro as argument + use this command "sudo docker build --build-arg ROS2_DISTRO=(desiredRosTag) (directoryHoldingDockerfile)/" + * Opened new folder for additional tracing contents + * Delete tracing directory + * Moved tracing.md to tracing directory + * added setupTracing.sh + Installs necessary packages and configures tracing group. + * Removed manual installation of ros-rolling-ros2trace + This is now automated in setupTracing.sh + location of sh file assumed if user follows README.md under "Getting started" + * Created alternative ManualTracing + * added new sm markdowns + * added a dockerfile for Rolling and Galactic + * Update smacc2_ci/docker/ros_rollingAndGalactic_ubuntu_20.04/buildGalactic.sh + Co-authored-by: Denis Štogl + * Update tracing/ManualTracing.md + Co-authored-by: Denis Štogl + * changed wording "smacc application" to "SMACC2 library" + Co-authored-by: Denis Štogl + * Update smacc_sm_reference_library/sm_atomic/README.md + edit from html to markdown syntax + Co-authored-by: Denis Štogl + * reactivating smacc2 nav clients for rolling via submodules + * renamed tracing events after + * bug in smacc2 component + * reverted markdowns to html + * added README tutorial for Dockerfile + * additional cleanup + * cleanup + * cleanup + * edited tracing.md to reflect new tracing event names + * Enable build of missing rolling repositories. + * Enable Navigation2 for semi-binary build. + * Remove galactic builds from master and kepp only rolling. Remove submodules and use .repos file + * updated mentions of SMACC/ROS to SMACC2/ROS2 + * some progress on navigation rolling + * renamed folders, deleted tracing.md, edited README.md + * added smacc2_performance_tools + * performance tests improvements + * more on performance and other issues + * sm_respira_1 format cleanup + * sm_respira_1 format cleanup pre-commit + * sm_respira_test_2 + * sm_respira_test_2 + * more changes on performance tests + * Do not execute clang-format on smacc2_sm_reference_library package. + * sm_reference_library reformatting + * Correct trailing spaces. + * sm_atomic_24hr + * sm_atomic_performance_trace_1 + * Update smacc2_rta command across readmes + * Clean up of sm_atomic_24hr + * more sm_atomic_24hr cleanup + * Optimized deps in move_base_z_planners_common. + * Renaming of event generator library + * minor formatting + * Add galactic CI setup and rename rolling files. (#58) + * Fix source CI and correct README overview. (#62) + * Update c_cpp_properties.json + * changed launch command to ros2 launch sm_respira_1 sm_respira_1.launch (#69) + also noticed a note I had made while producing these that was not removed + * update doxygen links (#70) + Co-authored-by: Ubuntu 20-04-02-amd64 + * More Readme Updates (#72) + Co-authored-by: Ubuntu 20-04-02-amd64 + * More Readme (#74) + Co-authored-by: Ubuntu 20-04-02-amd64 + * created new sm from sm_respira_1 (#76) + * Feature/core and navigation fixes (#78) + * base for the sm_aws_aarehouse navigation + * progressing in aws navigation + * minor + * several core improvements during navigation testing + * formatting improvements + * progress in aws navigation demo + * format improvements + * Feature/aws demo progress (#80) + * base for the sm_aws_aarehouse navigation + * progressing in aws navigation + * minor + * several core improvements during navigation testing + * formatting improvements + * progress in aws navigation demo + * format improvements + * format improvements + * more on navigation + * sm_advanced_recovery_1 reworked (#83) + * sm_advanced_recovery_1 reworked + * fix pre-commit + * Trying to fix Pre-Commit + Co-authored-by: Ubuntu 20-04-02-amd64 + * more sm_advanced_recovery_1 (#84) + Co-authored-by: Ubuntu 20-04-02-amd64 + * More sm_advanced_recovery_1 work (#85) + Co-authored-by: Ubuntu 20-04-02-amd64 + * sm_advanced_recovery_1 round 4 (#86) + Co-authored-by: Ubuntu 20-04-02-amd64 + * Brettpac branch (#87) + * sm_atomic_performance_test_a_2 + * sm_atomic_performance_test_a_1 + Co-authored-by: Ubuntu 20-04-02-amd64 + * sm_atomic_performance_test_c_1 (#88) + Co-authored-by: Ubuntu 20-04-02-amd64 + * modifying sm_atomic_performance_test_a_2 (#89) + Co-authored-by: Ubuntu 20-04-02-amd64 + * sm_multi_stage_1 (#90) + * sm_multi_stage_1 + * fixing precommit + Co-authored-by: Ubuntu 20-04-02-amd64 + * more sm_multi_stage_1 (#91) + Co-authored-by: Ubuntu 20-04-02-amd64 + * Update README.md + updated launch command + * Wait topic message client behavior (#81) + * base for the sm_aws_aarehouse navigation + * progressing in aws navigation + * minor + * several core improvements during navigation testing + * formatting improvements + * progress in aws navigation demo + * format improvements + * format improvements + * more on navigation + * new feature, cb_wait_topic_message: asynchronous client behavior that waits a topic message and optionally checks its contents to success + * formatting + * attempting precommit fixes + Co-authored-by: Ubuntu 20-04-02-amd64 + Co-authored-by: Denis Štogl + * Feature/wait nav2 nodes client behavior (#82) + * base for the sm_aws_aarehouse navigation + * progressing in aws navigation + * minor + * several core improvements during navigation testing + * formatting improvements + * progress in aws navigation demo + * format improvements + * format improvements + * more on navigation + * new feature, cb_wait_topic_message: asynchronous client behavior that waits a topic message and optionally checks its contents to success + * formatting + * adding new client behavior add for nav2, wait nav2 nodes subscribing to the /bond topic and waiting they are alive. you optionally can select the nodes to wait + * Correct all linters and formaters. + Co-authored-by: Denis Štogl + Co-authored-by: Denis Štogl + * Feature/aws demo progress (#92) + * base for the sm_aws_aarehouse navigation + * progressing in aws navigation + * minor + * several core improvements during navigation testing + * formatting improvements + * progress in aws navigation demo + * format improvements + * format improvements + * more on navigation + * new feature, cb_wait_topic_message: asynchronous client behavior that waits a topic message and optionally checks its contents to success + * formatting + * adding new client behavior add for nav2, wait nav2 nodes subscribing to the /bond topic and waiting they are alive. you optionally can select the nodes to wait + * progress in aws navigation demo + * minor format + * Feature/sm dance bot fixes (#93) + * base for the sm_aws_aarehouse navigation + * progressing in aws navigation + * minor + * several core improvements during navigation testing + * formatting improvements + * progress in aws navigation demo + * format improvements + * format improvements + * more on navigation + * new feature, cb_wait_topic_message: asynchronous client behavior that waits a topic message and optionally checks its contents to success + * formatting + * adding new client behavior add for nav2, wait nav2 nodes subscribing to the /bond topic and waiting they are alive. you optionally can select the nodes to wait + * progress in aws navigation demo + * minor format + * navigation parameters fixes on sm_dance_bot + * minor format + * Feature/sm aws warehouse (#94) + * base for the sm_aws_aarehouse navigation + * progressing in aws navigation + * minor + * several core improvements during navigation testing + * formatting improvements + * progress in aws navigation demo + * format improvements + * format improvements + * more on navigation + * new feature, cb_wait_topic_message: asynchronous client behavior that waits a topic message and optionally checks its contents to success + * formatting + * adding new client behavior add for nav2, wait nav2 nodes subscribing to the /bond topic and waiting they are alive. you optionally can select the nodes to wait + * progress in aws navigation demo + * minor format + * merge and progress + * fix format + * Feature/sm dance bot fixes (#95) + * base for the sm_aws_aarehouse navigation + * progressing in aws navigation + * minor + * several core improvements during navigation testing + * formatting improvements + * progress in aws navigation demo + * format improvements + * format improvements + * more on navigation + * new feature, cb_wait_topic_message: asynchronous client behavior that waits a topic message and optionally checks its contents to success + * formatting + * adding new client behavior add for nav2, wait nav2 nodes subscribing to the /bond topic and waiting they are alive. you optionally can select the nodes to wait + * progress in aws navigation demo + * minor format + * navigation parameters fixes on sm_dance_bot + * minor format + * minor + * minor format + * Remove some compile warnings. (#96) + * Feature/cb pause slam (#98) + * base for the sm_aws_aarehouse navigation + * progressing in aws navigation + * minor + * several core improvements during navigation testing + * formatting improvements + * progress in aws navigation demo + * format improvements + * format improvements + * more on navigation + * new feature, cb_wait_topic_message: asynchronous client behavior that waits a topic message and optionally checks its contents to success + * formatting + * adding new client behavior add for nav2, wait nav2 nodes subscribing to the /bond topic and waiting they are alive. you optionally can select the nodes to wait + * progress in aws navigation demo + * minor format + * navigation parameters fixes on sm_dance_bot + * minor format + * minor + * formatting + * cb pause slam client behavior + * sm_dance_bot_lite (#99) + * sm_dance_bot_lite + * precommit + * Updates yaml + Co-authored-by: Ubuntu 20-04-02-amd64 + * Rename doxygen deployment workflow (#100) + * minor hotfix + * sm_dance_bot visualizing turtlebot3 (#101) + * Feature/dance bot launch gz lidar choice (#102) + * sm_dance_bot visualizing turtlebot3 + * cleaning and lidar show/hide option + * cleaning files and making formatting work + * more fixes + * Feature/sm dance bot lite gazebo fixes (#104) + * sm_dance_bot visualizing turtlebot3 + * cleaning and lidar show/hide option + * cleaning files and making formatting work + * more fixes + * gazebo fixes, to show the robot and the lidar + * format fixes + * sm_multi_stage_1 doubling (#103) + Co-authored-by: Ubuntu 20-04-02-amd64 + * Feature/sm dance bot strikes back gazebo fixes (#105) + * sm_dance_bot visualizing turtlebot3 + * cleaning and lidar show/hide option + * cleaning files and making formatting work + * more fixes + * gazebo fixes, to show the robot and the lidar + * format fixes + * gazebo fixes for sm_dance_bot_strikes_back + * precommit cleanup run (#106) + Co-authored-by: Ubuntu 20-04-02-amd64 + * aws demo (#108) + * aws demo + * format + * got sm_multi_stage_1 working (barely) (#109) + Co-authored-by: Ubuntu 20-04-02-amd64 + * Brettpac branch (#110) + * got sm_multi_stage_1 working (barely) + * gaining traction sm_multi_stage_1 + * more + * don't remember + * making progress + * More + * keep hammering + * two stages + * 3 part + * 4th stage + Co-authored-by: Ubuntu 20-04-02-amd64 + * Brettpac branch (#111) + * got sm_multi_stage_1 working (barely) + * gaining traction sm_multi_stage_1 + * more + * don't remember + * making progress + * More + * keep hammering + * two stages + * 3 part + * 4th stage + * 5th stage + Co-authored-by: Ubuntu 20-04-02-amd64 + * a3 (#113) + Co-authored-by: Ubuntu 20-04-02-amd64 + * Remove neo_simulation2 package. (#112) + * Remove neo_simulation2 package. + * Correct formatting. + * Enable source build on PR for testing. + * Adjust build packages of source CI + * more sm_multi_stage_1 (#114) + Co-authored-by: Ubuntu 20-04-02-amd64 + * mm (#115) + Co-authored-by: Ubuntu 20-04-02-amd64 + * diverse improvements navigation and performance (#116) + * diverse improvements navigation and performance + * minor + Co-authored-by: pabloinigoblasco + * Feature/diverse improvemets navigation performance (#117) + * diverse improvements navigation and performance + * minor + * additional linting and formatting + * Remove merge markers from a python file. (#119) + * Feature/slam toggle and smacc deep history (#122) + * progress in navigation, slam toggle client behaviors and slam_toolbox components. Also smacc2::deep_history syntax + * going forward in testing sm_dance_bot introducing slam pausing/resuming funcionality + * feature/more_sm_dance_bot_fixes + * minor format + * minor (#124) + Co-authored-by: Ubuntu 20-04-02-amd64 + * more changes in sm_dance_bot (#125) + * Move method after the method it calls. Otherwise recursion could happen. (#126) + * Feature/dance bot s pattern (#128) + * more changes in sm_dance_bot + * polishing sm_dance_bot and s-pattern + * noticed typo + Finnaly > Finally + * Feature/dance bot s pattern (#129) + * more changes in sm_dance_bot + * polishing sm_dance_bot and s-pattern + * more refinement in sm_dance_bot + * First working version of sm template and template generator. (#127) + * minor tweaks (#130) + Co-authored-by: Ubuntu 20-04-02-amd64 + * Feature/sm dance bot refine (#131) + * more changes in sm_dance_bot + * minor + * Feature/sm dance bot refine 2 (#132) + * more changes in sm_dance_bot + * minor + * build fix + * waypoints navigator bug (#133) + * minor tuning to mitigate overshot issue cases + * progress in the sm_dance_bot tests (#135) + * some more progress on markers cleanup + * minor format issues (#134) + * sm_dance_bot_lite (#136) + Co-authored-by: Ubuntu 20-04-02-amd64 + * Resolve compile wanings (#137) + * Add SM core test (#138) + * minor navigation improvements (#141) + * using local action msgs (#139) + * using local action msgs + * removing sm_dance_bot_msgs + * pending references + * Feature/nitrosz renaming (#144) + * using local action msgs + * removing sm_dance_bot_msgs + * pending references + * navigation 2 stack renaming + * formatting + * added SVGs to READMEs of atomic, dance_bot, and others (#140) + * added remaining SVGs to READMEs (#145) + * added remaining SVGs to READMEs + * precommit cleanup + * Update package list. (#142) + * removing parameters smacc (#147) + * removing parameters smacc + * workflows update + * workflow + * Noticed launch command was incorrect in README.md + fixed launch command for sm_dance_bot_strikes_back and removed some comments I had made in the past. + * Fix CI: format fix python version (#148) + * Add SM Atomic SM generator. (#143) + * Remove node creation and create only a logger. (#149) + * Rolling Docker environment to be executed from any environment (#154) + * Feature/sm dance bot strikes back refactoring (#152) + Co-authored-by: DecDury + Co-authored-by: Denis Štogl + * slight waypoint 4 and iterations changes so robot can complete course (#155) + * Feature/migration moveit client (#151) + * initial migration to smacc2 + * fixing some errors introduced on formatting + * missing dependency + * fixing some more linting warnings + * minor + * removing test from main moveit cmake + * test ur5 + * progressing in the moveit migration testing + * updating format + * adding .reps dependencies and also fixing some build errors + * repos dependency + * adding dependency to ur5 client + * docker refactoring + * minor + * progress on move_it PR + * minor dockerfile test workaround + * improving dockerfile for building local tests + * minor + * fixing compiling issues + * update readme (#164) + * update readme + * more readme updates + * more + Co-authored-by: Ubuntu 20-04-02-amd64 + * initial state machine transition timestamp (#165) + * moved reference library SMs to smacc2_performance_tools (#166) + * moved reference library SMs to smacc2_performance_tools + * pre-commit cleanup + * Add QOS durability to SmaccPublisherClient (#163) + * feat: add qos durability to SmaccPublisherClient + * fix: add a missing colon + * refactor: remove line + * feat: add reliability qos config + * Feature/testing moveit behaviors (#167) + * more testing on moveit + * progress on moveit + * more testing on moveit behaviors + * minor configuration + * fixing pipeline error + * fixing broken master build + * sm_pubsub_1 (#169) + Co-authored-by: Ubuntu 20-04-02-amd64 + * sm_pubsub_1 part 2 (#170) + * sm_pubsub_1 part 2 + * sm_pubsub_1 part 2 + Co-authored-by: Ubuntu 20-04-02-amd64 + * sm_advanced_recovery_1 renaming (#171) + Co-authored-by: Ubuntu 20-04-02-amd64 + * sm_multi_stage_1 reworking (#172) + * multistage modes + * sm_multi_stage sequences + * sm_multi_state_1 steps + * sm_multi_stage_1 sequence d + * sm_multi_stage_1 c sequence + * mode_5_sequence_b + * mode_4_sequence_b + * sm_multi_stage_1 most + * finishing touches 1 + * readme + Co-authored-by: Ubuntu 20-04-02-amd64 + * Feature/aws navigation sm dance bot (#174) + * repo dependency + * husky launch file in sm_dance_bot + * Add dependencies for husky simulation. + * Fix formatting. + * Update dependencies for husky in rolling and galactic. + * minor + * progress on aws navigation and some other refactorings on navigation clients and behaviors + * more on aws demo + * fixing broken build + * minor + Co-authored-by: Denis Štogl + Co-authored-by: Denis Štogl + * minor changes (#175) + * warehouse2 (#177) + * Waypoint Inputs (#178) + Co-authored-by: Ubuntu 20-04-02-amd64 + * wharehouse2 progress (#179) + * format (#180) + * sm_dance_bot_warehouse_3 (#181) + Co-authored-by: Ubuntu 20-04-02-amd64 + * Feature/sm warehouse 2 13 dec 2 (#182) + * format + * more changes and headless + * merge + * headless and other fixes + * default values + * Brettpac branch (#184) + * sm_dance_bot_warehouse_3 + * Redoing sm_dance_bot_warehouse_3 waypoints + * More Waypoints + Co-authored-by: Ubuntu 20-04-02-amd64 + * SrConditional fixes and formatting (#168) + * fix: some formatting and templating on SrConditional + * fix: move trigger logic into headers + * fix: lint + * Feature/wharehouse2 dec 14 (#185) + * warehouse2 + * minor + * Feature/sm warehouse 2 13 dec 2 (#186) + * format + * more changes and headless + * merge + * headless and other fixes + * default values + * minor + * finetuning waypoints (#187) + Co-authored-by: Ubuntu 20-04-02-amd64 + * Feature/cb pure spinning (#188) + * format + * more changes and headless + * merge + * headless and other fixes + * default values + * minor + * Feature/cb pure spinning (#189) + * format + * more changes and headless + * merge + * headless and other fixes + * default values + * minor + * pure spinning behavior missing files + * minor changes (#190) + * Feature/planner changes 16 12 (#191) + * minor changes + * more fixes + * minor + * minor + * Feature/replanning 16 dec (#193) + * minor changes + * replanning for all our examples + * several fixes (#194) + * minor changes (#195) + * Feature/undo motion 20 12 (#196) + * minor changes + * replanning for all our examples + * improving undo motion navigation warehouse2 + * minor + * tuning warehouse3 (#197) + * Feature/undo motion 20 12 (#198) + * minor changes + * replanning for all our examples + * improving undo motion navigation warehouse2 + * minor + * undo tuning and errors + * format + * Feature/sync 21 12 (#199) + * minor changes + * replanning for all our examples + * format issues + * Feature/warehouse2 22 12 (#200) + * minor changes + * replanning for all our examples + * format issues + * finishing warehouse2 + * Feature/warehouse2 23 12 (#201) + * minor changes + * replanning for all our examples + * tuning and fixes (#202) + * Feature/minor tune (#203) + * tuning and fixes + * minor tune + * fixing warehouse 3 problems, and other core improvements (#204) + * fixing warehouse 3 problems, and other core improvements to remove dead lock, also making continuous integration green + * weird moveit not downloaded repo + * added missing file from warehouse2 (#205) + * backport to foxy + * minor format + * minor linking errors foxy + Co-authored-by: Denis Štogl + Co-authored-by: Denis Štogl + Co-authored-by: Declan Dury <44791484+DecDury@users.noreply.github.com> + Co-authored-by: DecDury + Co-authored-by: reelrbtx + Co-authored-by: brettpac + Co-authored-by: David Revay + * missing + * missing sm + * updating subscriber publisher components + * progress in autowarrrrrrrrrrrrrrrrrrrrrrrrrrrrrrrrrre machine + * refining cp subscriber cp publisher + * improvements in smacc core adding more components mostly developed for autoware demo + * autoware demo + * missing + * foxy ci + * fix + * minor broken build + * some reordering fixes + * minor + * docker files for different revisions, warnings removval and more testing on navigation + * fixing docker for foxy and galactic + * docker build files for all versions + * barrel demo + * barrel search build fix and warehouse3 + * fixing startup problems in warehouse 3 + * fixing format and minor + * minor + * progress in barrel husky + * minor + * barrel demo + * progress + * fixing broken build + Co-authored-by: Denis Štogl + Co-authored-by: Denis Štogl + Co-authored-by: Declan Dury <44791484+DecDury@users.noreply.github.com> + Co-authored-by: DecDury + Co-authored-by: reelrbtx + Co-authored-by: brettpac + Co-authored-by: David Revay + Co-authored-by: pabloinigoblasco + * more merge + * docker improvements + * master rolling to galactic backport + * fixing build + * testing dance bot demos + * updating galactic repos + * runtime dependency + * restoring ur dependency + Co-authored-by: DecDury + Co-authored-by: reelrbtx + Co-authored-by: brettpac + Co-authored-by: Denis Štogl + Co-authored-by: Denis Štogl + Co-authored-by: Declan Dury <44791484+DecDury@users.noreply.github.com> + Co-authored-by: David Revay + Co-authored-by: pabloinigoblasco +* Contributors: Denis Štogl, Pablo Iñigo Blasco, pabloinigoblasco diff --git a/smacc2_client_library/nitrosz_client/nitrosz_client/CMakeLists.txt b/smacc2_client_library/nitrosz_client/nitrosz_client/CMakeLists.txt new file mode 100644 index 000000000..8d472bfaa --- /dev/null +++ b/smacc2_client_library/nitrosz_client/nitrosz_client/CMakeLists.txt @@ -0,0 +1,119 @@ +cmake_minimum_required(VERSION 3.5) +project(nitrosz_client) + +# Default to C++17 +if(NOT CMAKE_CXX_STANDARD) + set(CMAKE_CXX_STANDARD 17) +endif() + +if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang") + add_compile_options(-Wall -Wextra -Wpedantic) +endif() + +# find dependencies +find_package(ament_cmake REQUIRED) +find_package(smacc2 REQUIRED) +find_package(std_srvs REQUIRED) +find_package(std_msgs REQUIRED) +find_package(nav2_msgs REQUIRED) +find_package(tf2 REQUIRED) +find_package(tf2_ros REQUIRED) +find_package(angles REQUIRED) +find_package(yaml_cpp_vendor REQUIRED) +find_package(tf2_geometry_msgs REQUIRED) +find_package(pluginlib REQUIRED) +find_package(bond REQUIRED) +find_package(slam_toolbox REQUIRED) +find_package(ament_index_cpp REQUIRED) +find_package(rclcpp_action REQUIRED) +find_package(nav2_util) + + +set(dependencies + smacc2 + std_srvs + std_msgs + nav2_msgs + tf2 + tf2_ros + angles + yaml_cpp_vendor + tf2_geometry_msgs + bond + slam_toolbox + ament_index_cpp + rclcpp_action + nav2_util +) + +include_directories(include) + +add_library(${PROJECT_NAME} SHARED + src/${PROJECT_NAME}/nitrosz_client.cpp + src/${PROJECT_NAME}/common.cpp + + src/${PROJECT_NAME}/client_behaviors/cb_nitrosz_client_behavior_base.cpp + src/${PROJECT_NAME}/client_behaviors/cb_absolute_rotate.cpp + src/${PROJECT_NAME}/client_behaviors/cb_rotate_look_at.cpp + src/${PROJECT_NAME}/client_behaviors/cb_navigate_backward.cpp + src/${PROJECT_NAME}/client_behaviors/cb_navigate_forward.cpp + src/${PROJECT_NAME}/client_behaviors/cb_abort_navigation.cpp + src/${PROJECT_NAME}/client_behaviors/cb_navigate_global_position.cpp + src/${PROJECT_NAME}/client_behaviors/cb_navigate_next_waypoint.cpp + src/${PROJECT_NAME}/client_behaviors/cb_navigate_next_waypoint_until_reached.cpp + src/${PROJECT_NAME}/client_behaviors/cb_navigate_named_waypoint.cpp + src/${PROJECT_NAME}/client_behaviors/cb_rotate.cpp + src/${PROJECT_NAME}/client_behaviors/cb_undo_path_backwards.cpp + src/${PROJECT_NAME}/client_behaviors/cb_wait_pose.cpp + src/${PROJECT_NAME}/client_behaviors/cb_wait_nitros_nodes.cpp + src/${PROJECT_NAME}/client_behaviors/cb_pause_slam.cpp + src/${PROJECT_NAME}/client_behaviors/cb_resume_slam.cpp + src/${PROJECT_NAME}/client_behaviors/cb_wait_transform.cpp + src/${PROJECT_NAME}/client_behaviors/cb_seek_waypoint.cpp + + # COMPONENTS + src/${PROJECT_NAME}/components/costmap_switch/cp_costmap_switch.cpp + #src/${PROJECT_NAME}/components/odom_tracker/cp_odom_tracker_node.cpp + + src/${PROJECT_NAME}/components/odom_tracker/cp_odom_tracker.cpp + src/${PROJECT_NAME}/components/planner_switcher/cp_planner_switcher.cpp + src/${PROJECT_NAME}/components/goal_checker_switcher/cp_goal_checker_switcher.cpp + src/${PROJECT_NAME}/components/pose/cp_pose.cpp + src/${PROJECT_NAME}/components/waypoints_navigator/cp_waypoints_event_dispatcher.cpp + src/${PROJECT_NAME}/components/waypoints_navigator/cp_waypoints_navigator.cpp + src/${PROJECT_NAME}/components/waypoints_navigator/cp_waypoints_visualizer.cpp + src/${PROJECT_NAME}/components/amcl/cp_amcl.cpp + src/${PROJECT_NAME}/components/slam_toolbox/cp_slam_toolbox.cpp + + src/${PROJECT_NAME}/client_behaviors/cb_active_stop.cpp + src/${PROJECT_NAME}/client_behaviors/cb_load_waypoints_file.cpp + src/${PROJECT_NAME}/client_behaviors/cb_navigate_next_waypoint_free.cpp + src/${PROJECT_NAME}/client_behaviors/cb_position_control_free_space.cpp + src/${PROJECT_NAME}/client_behaviors/cb_pure_spinning.cpp + src/${PROJECT_NAME}/client_behaviors/cb_save_slam_map.cpp +) + +ament_target_dependencies(${PROJECT_NAME} + ${dependencies}) + +target_link_libraries(${PROJECT_NAME}) + +ament_export_include_directories(include) +ament_export_libraries(${PROJECT_NAME}) +ament_export_dependencies(${dependencies}) + +install(TARGETS ${PROJECT_NAME} + ARCHIVE DESTINATION lib + LIBRARY DESTINATION lib + RUNTIME DESTINATION bin +) + +install(DIRECTORY include/ + DESTINATION include/ +) + +install(FILES scripts/lidar_completion.py + DESTINATION lib/${PROJECT_NAME}/ +) + +ament_package() diff --git a/smacc2_client_library/nitrosz_client/nitrosz_client/include/nitrosz_client/client_behaviors.hpp b/smacc2_client_library/nitrosz_client/nitrosz_client/include/nitrosz_client/client_behaviors.hpp new file mode 100644 index 000000000..8992dec73 --- /dev/null +++ b/smacc2_client_library/nitrosz_client/nitrosz_client/include/nitrosz_client/client_behaviors.hpp @@ -0,0 +1,45 @@ +// Copyright 2021 RobosoftAI Inc. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#pragma once + +// rotation behavior +#include +#include +#include + +#include +#include +#include +#include +#include + +// nav2 synchronization behaviors +#include +#include +#include + +// others +#include +#include + +// waypoints behaviors +#include +#include +#include +#include + +// slam behaviors +#include +#include diff --git a/smacc2_client_library/nitrosz_client/nitrosz_client/include/nitrosz_client/client_behaviors/cb_abort_navigation.hpp b/smacc2_client_library/nitrosz_client/nitrosz_client/include/nitrosz_client/client_behaviors/cb_abort_navigation.hpp new file mode 100644 index 000000000..096e18d79 --- /dev/null +++ b/smacc2_client_library/nitrosz_client/nitrosz_client/include/nitrosz_client/client_behaviors/cb_abort_navigation.hpp @@ -0,0 +1,48 @@ +// Copyright 2021 RobosoftAI Inc. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +/***************************************************************************************************************** + * + * Authors: Pablo Inigo Blasco, Brett Aldrich + * + ******************************************************************************************************************/ +#pragma once + +#include + +#include "cb_navigate_global_position.hpp" +#include "cb_nitrosz_client_behavior_base.hpp" +#include "tf2_geometry_msgs/tf2_geometry_msgs.hpp" + +namespace cl_nitrosz +{ +class CbAbortNavigation : public smacc2::SmaccAsyncClientBehavior +{ +public: + CbAbortNavigation(); + + template + void onOrthogonalAllocation() + { + this->requiresClient(nitroszClient_); + smacc2::SmaccAsyncClientBehavior::onOrthogonalAllocation(); + } + + void onEntry() override; + void onExit() override; + +private: + cl_nitrosz::ClNitrosZ * nitroszClient_; +}; +} // namespace cl_nitrosz diff --git a/smacc2_client_library/nitrosz_client/nitrosz_client/include/nitrosz_client/client_behaviors/cb_absolute_rotate.hpp b/smacc2_client_library/nitrosz_client/nitrosz_client/include/nitrosz_client/client_behaviors/cb_absolute_rotate.hpp new file mode 100644 index 000000000..c9aa9cd9a --- /dev/null +++ b/smacc2_client_library/nitrosz_client/nitrosz_client/include/nitrosz_client/client_behaviors/cb_absolute_rotate.hpp @@ -0,0 +1,51 @@ +// Copyright 2021 RobosoftAI Inc. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +/***************************************************************************************************************** + * + * Authors: Pablo Inigo Blasco, Brett Aldrich + * + ******************************************************************************************************************/ +#pragma once + +#include + +#include "cb_nitrosz_client_behavior_base.hpp" +#include "tf2_geometry_msgs/tf2_geometry_msgs.hpp" + +namespace cl_nitrosz +{ +class CbAbsoluteRotate : public CbNav2ZClientBehaviorBase +{ +public: + std::shared_ptr listener; + + float absoluteGoalAngleDegree; + std::optional yawGoalTolerance; + std::optional maxVelTheta; // if not defined, default parameter server + std::optional spinningPlanner; + + CbAbsoluteRotate(); + CbAbsoluteRotate(float absoluteGoalAngleDegree, float yawGoalTolerance = -1); + + void onEntry() override; + void onExit() override; + +private: + void updateTemporalBehaviorParameters(bool undo); + float oldYawTolerance; + float oldMaxVelTheta; + float oldMinVelTheta; +}; +} // namespace cl_nitrosz diff --git a/smacc2_client_library/nitrosz_client/nitrosz_client/include/nitrosz_client/client_behaviors/cb_active_stop.hpp b/smacc2_client_library/nitrosz_client/nitrosz_client/include/nitrosz_client/client_behaviors/cb_active_stop.hpp new file mode 100644 index 000000000..e7729af2b --- /dev/null +++ b/smacc2_client_library/nitrosz_client/nitrosz_client/include/nitrosz_client/client_behaviors/cb_active_stop.hpp @@ -0,0 +1,40 @@ +// Copyright 2021 RobosoftAI Inc. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +/***************************************************************************************************************** + * + * Authors: Pablo Inigo Blasco, Brett Aldrich + * + ******************************************************************************************************************/ + +#pragma once + +#include +#include + +namespace cl_nitrosz +{ +struct CbActiveStop : public smacc2::SmaccAsyncClientBehavior +{ +private: + rclcpp::Publisher::SharedPtr cmd_vel_pub_; + +public: + CbActiveStop(); + + void onEntry() override; + + void onExit() override; +}; +} // namespace cl_nitrosz diff --git a/smacc2_client_library/nitrosz_client/nitrosz_client/include/nitrosz_client/client_behaviors/cb_load_waypoints_file.hpp b/smacc2_client_library/nitrosz_client/nitrosz_client/include/nitrosz_client/client_behaviors/cb_load_waypoints_file.hpp new file mode 100644 index 000000000..2a5917360 --- /dev/null +++ b/smacc2_client_library/nitrosz_client/nitrosz_client/include/nitrosz_client/client_behaviors/cb_load_waypoints_file.hpp @@ -0,0 +1,46 @@ +// Copyright 2021 RobosoftAI Inc. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +/***************************************************************************************************************** + * + * Authors: Pablo Inigo Blasco, Brett Aldrich + * + ******************************************************************************************************************/ + +#pragma once + +#include +#include + +namespace cl_nitrosz +{ +struct CbLoadWaypointsFile : public smacc2::SmaccAsyncClientBehavior +{ +public: + CbLoadWaypointsFile(std::string filepath); + + CbLoadWaypointsFile(std::string parameter_name, std::string packagenamespace); + + void onEntry() override; + + void onExit() override; + + std::optional filepath_; + + std::optional parameterName_; + std::optional packageNamespace_; + + cl_nitrosz::CpWaypointNavigatorBase * waypointsNavigator_; +}; +} // namespace cl_nitrosz diff --git a/smacc2_client_library/nitrosz_client/nitrosz_client/include/nitrosz_client/client_behaviors/cb_navigate_backwards.hpp b/smacc2_client_library/nitrosz_client/nitrosz_client/include/nitrosz_client/client_behaviors/cb_navigate_backwards.hpp new file mode 100644 index 000000000..baf61e4ed --- /dev/null +++ b/smacc2_client_library/nitrosz_client/nitrosz_client/include/nitrosz_client/client_behaviors/cb_navigate_backwards.hpp @@ -0,0 +1,50 @@ +// Copyright 2021 RobosoftAI Inc. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +/***************************************************************************************************************** + * + * Authors: Pablo Inigo Blasco, Brett Aldrich + * + ******************************************************************************************************************/ +#pragma once + +#include + +#include + +#include +#include "cb_nitrosz_client_behavior_base.hpp" + +namespace cl_nitrosz +{ +// It sends the mobile base some distance backwards +class CbNavigateBackwards : public CbNav2ZClientBehaviorBase +{ +public: + float backwardDistance; + + // just a stub to show how to use parameterless constructor + std::optional backwardSpeed; + + std::optional goalChecker_; + + cl_nitrosz::odom_tracker::CpOdomTracker * odomTracker_; + + CbNavigateBackwards(float backwardDistanceMeters); + + void onEntry() override; + + void onExit() override; +}; +} // namespace cl_nitrosz diff --git a/smacc2_client_library/nitrosz_client/nitrosz_client/include/nitrosz_client/client_behaviors/cb_navigate_forward.hpp b/smacc2_client_library/nitrosz_client/nitrosz_client/include/nitrosz_client/client_behaviors/cb_navigate_forward.hpp new file mode 100644 index 000000000..afb126ece --- /dev/null +++ b/smacc2_client_library/nitrosz_client/nitrosz_client/include/nitrosz_client/client_behaviors/cb_navigate_forward.hpp @@ -0,0 +1,79 @@ +// Copyright 2021 RobosoftAI Inc. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +/***************************************************************************************************************** + * + * Authors: Pablo Inigo Blasco, Brett Aldrich + * + ******************************************************************************************************************/ +#pragma once + +#include +#include +#include +#include +#include +#include + +#include "cb_nitrosz_client_behavior_base.hpp" + +namespace cl_nitrosz +{ +struct CbNavigateForwardOptions +{ + // just a stub to show how to use parameterless constructor + std::optional forwardSpeed; + + // this may be useful in the case you want to do a straight line with some known direction + // and the robot may not have that specific initial orientation at that moment. + // If it is not set, the orientation of the straight line is the orientation of the initial (current) state. + std::optional forceInitialOrientation; + + // the name of the goal checker selected in the navigation2 stack + std::optional goalChecker_; +}; + +// Performs a relative motion forwards +class CbNavigateForward : public CbNav2ZClientBehaviorBase +{ +public: + CbNavigateForwardOptions options; + + CbNavigateForward(); + + CbNavigateForward(float forwardDistance); + + // alternative forward motion using an absolute goal position instead a relative linear distance. + // It is specially to retry a previous relative incorrect motions based on distance. + // The developer user is in charge of setting a valid goal position that is located forward + // from the current position and orientation. + CbNavigateForward(geometry_msgs::msg::PoseStamped goalPosition); + + virtual ~CbNavigateForward(); + + void onEntry() override; + + void onExit() override; + + void setForwardDistance(float distance_meters); + +protected: + // required component + odom_tracker::CpOdomTracker * odomTracker_; + + std::optional goalPose_; + + std::optional forwardDistance_; +}; +} // namespace cl_nitrosz diff --git a/smacc2_client_library/nitrosz_client/nitrosz_client/include/nitrosz_client/client_behaviors/cb_navigate_global_position.hpp b/smacc2_client_library/nitrosz_client/nitrosz_client/include/nitrosz_client/client_behaviors/cb_navigate_global_position.hpp new file mode 100644 index 000000000..c92d8f556 --- /dev/null +++ b/smacc2_client_library/nitrosz_client/nitrosz_client/include/nitrosz_client/client_behaviors/cb_navigate_global_position.hpp @@ -0,0 +1,68 @@ +// Copyright 2021 RobosoftAI Inc. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +/***************************************************************************************************************** + * + * Authors: Pablo Inigo Blasco, Brett Aldrich + * + ******************************************************************************************************************/ +#pragma once + +#include +#include + +#include "cb_nitrosz_client_behavior_base.hpp" + +namespace cl_nitrosz +{ +struct CbNavigateGlobalPositionOptions +{ + // std::optional yawTolerance; + // std::optional yawToleranceX; + // std::optional yawToleranceY; + + std::optional goalChecker_; + + std::optional controllerName_; +}; + +class CbNavigateGlobalPosition : public CbNav2ZClientBehaviorBase +{ +public: + float goalYaw; + + CbNavigateGlobalPositionOptions options; + geometry_msgs::msg::Point goalPosition; + + CbNavigateGlobalPosition(); + + CbNavigateGlobalPosition( + float x, float y, float yaw /*radians*/, + std::optional options = std::nullopt); + + void setGoal(const geometry_msgs::msg::Pose & pose); + + virtual void onEntry() override; + + // This is the substate destructor. This code will be executed when the + // workflow exits from this substate (that is according to statechart the moment when this object is destroyed) + void onExit() override; + + // auxiliary function that defines the motion that is requested to the navigation2 action server + void execute(); + +private: + void readStartPoseFromParameterServer(ClNitrosZ::Goal & goal); +}; +} // namespace cl_nitrosz diff --git a/smacc2_client_library/nitrosz_client/nitrosz_client/include/nitrosz_client/client_behaviors/cb_navigate_named_waypoint.hpp b/smacc2_client_library/nitrosz_client/nitrosz_client/include/nitrosz_client/client_behaviors/cb_navigate_named_waypoint.hpp new file mode 100644 index 000000000..141ad4840 --- /dev/null +++ b/smacc2_client_library/nitrosz_client/nitrosz_client/include/nitrosz_client/client_behaviors/cb_navigate_named_waypoint.hpp @@ -0,0 +1,45 @@ +// Copyright 2021 RobosoftAI Inc. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +/***************************************************************************************************************** + * + * Authors: Pablo Inigo Blasco, Brett Aldrich + * + ******************************************************************************************************************/ +#pragma once + +#include +#include +#include "cb_navigate_next_waypoint.hpp" +#include "cb_nitrosz_client_behavior_base.hpp" + +namespace cl_nitrosz +{ +class CbNavigateNamedWaypoint : public CbNavigateNextWaypoint +{ +public: + CbNavigateNamedWaypoint( + std::string waypointname, std::optional options = std::nullopt); + + virtual ~CbNavigateNamedWaypoint(); + + void onEntry() override; + + void onExit() override; + + CpWaypointNavigator * waypointsNavigator_; + + std::string waypointname_; +}; +} // namespace cl_nitrosz diff --git a/smacc2_client_library/nitrosz_client/nitrosz_client/include/nitrosz_client/client_behaviors/cb_navigate_next_waypoint.hpp b/smacc2_client_library/nitrosz_client/nitrosz_client/include/nitrosz_client/client_behaviors/cb_navigate_next_waypoint.hpp new file mode 100644 index 000000000..d2a82e874 --- /dev/null +++ b/smacc2_client_library/nitrosz_client/nitrosz_client/include/nitrosz_client/client_behaviors/cb_navigate_next_waypoint.hpp @@ -0,0 +1,47 @@ +// Copyright 2021 RobosoftAI Inc. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +/***************************************************************************************************************** + * + * Authors: Pablo Inigo Blasco, Brett Aldrich + * + ******************************************************************************************************************/ +#pragma once + +#include +#include + +#include "cb_nitrosz_client_behavior_base.hpp" + +namespace cl_nitrosz +{ +class CbNavigateNextWaypoint : public CbNav2ZClientBehaviorBase +{ +public: + CbNavigateNextWaypoint(std::optional options = std::nullopt); + + virtual ~CbNavigateNextWaypoint(); + + void onEntry() override; + + void onExit() override; + +protected: + CpWaypointNavigator * waypointsNavigator_; + + NavigateNextWaypointOptions options_; + + cl_nitrosz::ClNitrosZ::SmaccNavigateResultSignal::SharedPtr navigationCallback_; +}; +} // namespace cl_nitrosz diff --git a/smacc2_client_library/nitrosz_client/nitrosz_client/include/nitrosz_client/client_behaviors/cb_navigate_next_waypoint_free.hpp b/smacc2_client_library/nitrosz_client/nitrosz_client/include/nitrosz_client/client_behaviors/cb_navigate_next_waypoint_free.hpp new file mode 100644 index 000000000..50e156d76 --- /dev/null +++ b/smacc2_client_library/nitrosz_client/nitrosz_client/include/nitrosz_client/client_behaviors/cb_navigate_next_waypoint_free.hpp @@ -0,0 +1,44 @@ +// Copyright 2021 RobosoftAI Inc. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +/***************************************************************************************************************** + * + * Authors: Pablo Inigo Blasco, Brett Aldrich + * + ******************************************************************************************************************/ +#pragma once +#include +#include + +namespace cl_nitrosz +{ + +class CbNavigateNextWaypointFree : public cl_nitrosz::CbPositionControlFreeSpace +{ +public: + CbNavigateNextWaypointFree(); + + virtual ~CbNavigateNextWaypointFree(); + + void onEntry() override; + + void onSucessCallback(); + + void onExit() override; + +protected: + cl_nitrosz::CpWaypointNavigatorBase * waypointsNavigator_; +}; + +} // namespace cl_nitrosz diff --git a/smacc2_client_library/nitrosz_client/nitrosz_client/include/nitrosz_client/client_behaviors/cb_navigate_next_waypoint_until_reached.hpp b/smacc2_client_library/nitrosz_client/nitrosz_client/include/nitrosz_client/client_behaviors/cb_navigate_next_waypoint_until_reached.hpp new file mode 100644 index 000000000..48805d329 --- /dev/null +++ b/smacc2_client_library/nitrosz_client/nitrosz_client/include/nitrosz_client/client_behaviors/cb_navigate_next_waypoint_until_reached.hpp @@ -0,0 +1,103 @@ +// Copyright 2021 RobosoftAI Inc. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +/***************************************************************************************************************** + * + * Authors: Pablo Inigo Blasco, Brett Aldrich + * + ******************************************************************************************************************/ +#pragma once + +#include "cb_navigate_next_waypoint.hpp" + +namespace cl_nitrosz +{ +template +struct EvGoalWaypointReached : sc::event> +{ +}; + +class CbNavigateNextWaypointUntilReached : public CbNavigateNextWaypoint +{ +public: + CbNavigateNextWaypointUntilReached( + std::string goalWaypointName, + std::optional options = std::nullopt); + + virtual ~CbNavigateNextWaypointUntilReached(); + + template + void onOrthogonalAllocation() + { + this->requiresClient(nitroszClient_); + CbNavigateNextWaypoint::onOrthogonalAllocation(); + + postEvGoalWaypointReached_ = [this]() + { + this->postEvent>(); + + // nitroszClient_->onSucceeded(&CbNavigateNextWaypointUntilReached::onWaypointReached, this); + }; + } + + void onEntry() override; + + void onExit() override; + + void onNavigationActionSuccess(const ClNitrosZ::WrappedResult & r) override + { + // if (!isOwnActionResponse(r)) + // { + // RCLCPP_WARN( + // getLogger(), "[%s] Propagating success event skipped. Action response is not ours.", + // getName().c_str()); + // return; + // } + + navigationResult_ = r.code; + + RCLCPP_INFO( + getLogger(), "[%s] Propagating success event from action server", getName().c_str()); + + waypointsNavigator_ = nitroszClient_->getComponent(); + + auto current_waypoint_name = waypointsNavigator_->getCurrentWaypointName(); + + if (current_waypoint_name == this->goalWaypointName_) + { + RCLCPP_INFO( + getLogger(), + "[CbNavigateNextWaypointUntilReached] GoalReached current iteration waypoint i: %ld with " + "name '%s'", + waypointsNavigator_->getCurrentWaypointIndex(), current_waypoint_name->c_str()); + + this->postEvGoalWaypointReached_(); + } + else + { + RCLCPP_INFO( + getLogger(), + "[CbNavigateNextWaypointUntilReached] goal:'%s' current:'%s'. keep navigating.", + goalWaypointName_.c_str(), current_waypoint_name->c_str()); + } + + this->postSuccessEvent(); + } + +private: + std::string goalWaypointName_; + + std::function postEvGoalWaypointReached_; +}; +} // namespace cl_nitrosz diff --git a/smacc2_client_library/nitrosz_client/nitrosz_client/include/nitrosz_client/client_behaviors/cb_nitrosz_client_behavior_base.hpp b/smacc2_client_library/nitrosz_client/nitrosz_client/include/nitrosz_client/client_behaviors/cb_nitrosz_client_behavior_base.hpp new file mode 100644 index 000000000..88233ea16 --- /dev/null +++ b/smacc2_client_library/nitrosz_client/nitrosz_client/include/nitrosz_client/client_behaviors/cb_nitrosz_client_behavior_base.hpp @@ -0,0 +1,67 @@ +// Copyright 2021 RobosoftAI Inc. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +/***************************************************************************************************************** + * + * Authors: Pablo Inigo Blasco, Brett Aldrich + * + ******************************************************************************************************************/ +#pragma once + +#include +#include +#include + +namespace cl_nitrosz +{ +class CbNav2ZClientBehaviorBase : public smacc2::SmaccAsyncClientBehavior +{ +public: + virtual ~CbNav2ZClientBehaviorBase(); + + template + void onOrthogonalAllocation() + { + this->requiresClient(nitroszClient_); + smacc2::SmaccAsyncClientBehavior::onOrthogonalAllocation(); + } + +protected: + void sendGoal(ClNitrosZ::Goal & goal); + + void cancelGoal(); + + // handling results according its type + bool isOwnActionResponse(const ClNitrosZ::WrappedResult &); + virtual void onNavigationResult(const ClNitrosZ::WrappedResult &); + virtual void onNavigationActionSuccess(const ClNitrosZ::WrappedResult &); + virtual void onNavigationActionAbort(const ClNitrosZ::WrappedResult &); + + cl_nitrosz::ClNitrosZ * nitroszClient_; + cl_nitrosz::ClNitrosZ::SmaccNavigateResultSignal::SharedPtr navigationCallback_; + + // deprecated + rclcpp_action::ResultCode navigationResult_; + std::shared_future< + std::shared_ptr > > + goalHandleFuture_; +}; + +enum class SpinningPlanner +{ + Default, + PureSpinning, + Forward +}; +} // namespace cl_nitrosz diff --git a/smacc2_client_library/nitrosz_client/nitrosz_client/include/nitrosz_client/client_behaviors/cb_pause_slam.hpp b/smacc2_client_library/nitrosz_client/nitrosz_client/include/nitrosz_client/client_behaviors/cb_pause_slam.hpp new file mode 100644 index 000000000..5d80f9d60 --- /dev/null +++ b/smacc2_client_library/nitrosz_client/nitrosz_client/include/nitrosz_client/client_behaviors/cb_pause_slam.hpp @@ -0,0 +1,37 @@ +// Copyright 2021 RobosoftAI Inc. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +/***************************************************************************************************************** + * + * Authors: Pablo Inigo Blasco, Brett Aldrich + * + ******************************************************************************************************************/ +#pragma once + +#include +#include +#include + +namespace cl_nitrosz +{ +class CbPauseSlam : public smacc2::client_behaviors::CbServiceCall +{ +public: + CbPauseSlam(std::string serviceName = "/slam_toolbox/pause_new_measurements"); + void onEntry() override; + +protected: + CpSlamToolbox * slam_; +}; +} // namespace cl_nitrosz diff --git a/smacc2_client_library/nitrosz_client/nitrosz_client/include/nitrosz_client/client_behaviors/cb_position_control_free_space.hpp b/smacc2_client_library/nitrosz_client/nitrosz_client/include/nitrosz_client/client_behaviors/cb_position_control_free_space.hpp new file mode 100644 index 000000000..d6b23e1a6 --- /dev/null +++ b/smacc2_client_library/nitrosz_client/nitrosz_client/include/nitrosz_client/client_behaviors/cb_position_control_free_space.hpp @@ -0,0 +1,64 @@ +// Copyright 2021 RobosoftAI Inc. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +/***************************************************************************************************************** + * + * Authors: Pablo Inigo Blasco, Brett Aldrich + * + ******************************************************************************************************************/ + +#pragma once + +#include +#include +#include + +namespace cl_nitrosz +{ +struct CbPositionControlFreeSpace : public smacc2::SmaccAsyncClientBehavior +{ +private: + double targetYaw_; + bool goalReached_; + double k_betta_; + double max_angular_yaw_speed_; + + double prev_error_linear_ = 0.0; + double prev_error_angular_ = 0.0; + double integral_linear_ = 0.0; + double integral_angular_ = 0.0; + + // Limit the maximum linear velocity and angular velocity to avoid sudden + // movements + double max_linear_velocity = 1.0; // Adjust this value according to your needs + double max_angular_velocity = 1.0; // Adjust this value according to your needs + + rclcpp::Publisher::SharedPtr cmd_vel_pub_; + +public: + double yaw_goal_tolerance_rads_ = 0.1; + + double threshold_distance_ = 3.0; + + geometry_msgs::msg::Pose target_pose_; + + CbPositionControlFreeSpace(); + + void updateParameters(); + + void onEntry() override; + + void onExit() override; +}; +} // namespace cl_nitrosz diff --git a/smacc2_client_library/nitrosz_client/nitrosz_client/include/nitrosz_client/client_behaviors/cb_pure_spinning.hpp b/smacc2_client_library/nitrosz_client/nitrosz_client/include/nitrosz_client/client_behaviors/cb_pure_spinning.hpp new file mode 100644 index 000000000..aaf3db7d5 --- /dev/null +++ b/smacc2_client_library/nitrosz_client/nitrosz_client/include/nitrosz_client/client_behaviors/cb_pure_spinning.hpp @@ -0,0 +1,48 @@ +// Copyright 2021 RobosoftAI Inc. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +/***************************************************************************************************************** + * + * Authors: Pablo Inigo Blasco, Brett Aldrich + * + ******************************************************************************************************************/ + +#pragma once + +#include +#include + +namespace cl_nitrosz +{ +struct CbPureSpinning : public smacc2::SmaccAsyncClientBehavior +{ +private: + double targetYaw__rads; + bool goalReached_; + double k_betta_; + double max_angular_yaw_speed_; + + rclcpp::Publisher::SharedPtr cmd_vel_pub_; + +public: + double yaw_goal_tolerance_rads_; + + CbPureSpinning(double targetYaw_rads, double max_angular_yaw_speed = 0.5); + void updateParameters(); + + void onEntry() override; + + void onExit() override; +}; +} // namespace cl_nitrosz diff --git a/smacc2_client_library/nitrosz_client/nitrosz_client/include/nitrosz_client/client_behaviors/cb_resume_slam.hpp b/smacc2_client_library/nitrosz_client/nitrosz_client/include/nitrosz_client/client_behaviors/cb_resume_slam.hpp new file mode 100644 index 000000000..c823e8ad7 --- /dev/null +++ b/smacc2_client_library/nitrosz_client/nitrosz_client/include/nitrosz_client/client_behaviors/cb_resume_slam.hpp @@ -0,0 +1,37 @@ +// Copyright 2021 RobosoftAI Inc. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +/***************************************************************************************************************** + * + * Authors: Pablo Inigo Blasco, Brett Aldrich + * + ******************************************************************************************************************/ +#pragma once + +#include +#include "nitrosz_client/components/slam_toolbox/cp_slam_toolbox.hpp" +#include "smacc2/client_behaviors/cb_call_service.hpp" + +namespace cl_nitrosz +{ +class CbResumeSlam : public smacc2::client_behaviors::CbServiceCall +{ +public: + CbResumeSlam(std::string serviceName = "/slam_toolbox/pause_new_measurements"); + void onEntry() override; + +protected: + CpSlamToolbox * slam_; +}; +} // namespace cl_nitrosz diff --git a/smacc2_client_library/nitrosz_client/nitrosz_client/include/nitrosz_client/client_behaviors/cb_retry_behavior.hpp b/smacc2_client_library/nitrosz_client/nitrosz_client/include/nitrosz_client/client_behaviors/cb_retry_behavior.hpp new file mode 100644 index 000000000..c91ca5c85 --- /dev/null +++ b/smacc2_client_library/nitrosz_client/nitrosz_client/include/nitrosz_client/client_behaviors/cb_retry_behavior.hpp @@ -0,0 +1,51 @@ +// Copyright 2021 RobosoftAI Inc. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +/***************************************************************************************************************** + * + * Authors: Pablo Inigo Blasco, Brett Aldrich + * + ******************************************************************************************************************/ +#pragma once + +#include +#include +#include "cb_nitrosz_client_behavior_base.hpp" + +namespace cl_nitrosz +{ +using odom_tracker::CpOdomTracker; + +template +class CbRetry : public TCbRelativeMotion +{ +public: + CbRetry() {} + void onEntry() override + { + odomTracker_ = this->nitroszClient_->template getComponent(); + auto goal = odomTracker_->getCurrentMotionGoal(); + + if (goal) + { + this->goalPose_ = *goal; + } + + TCbRelativeMotion::onEntry(); + } + +private: + CpOdomTracker * odomTracker_; +}; +} // namespace cl_nitrosz diff --git a/smacc2_client_library/nitrosz_client/nitrosz_client/include/nitrosz_client/client_behaviors/cb_rotate.hpp b/smacc2_client_library/nitrosz_client/nitrosz_client/include/nitrosz_client/client_behaviors/cb_rotate.hpp new file mode 100644 index 000000000..de97c033d --- /dev/null +++ b/smacc2_client_library/nitrosz_client/nitrosz_client/include/nitrosz_client/client_behaviors/cb_rotate.hpp @@ -0,0 +1,47 @@ +// Copyright 2021 RobosoftAI Inc. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +/***************************************************************************************************************** + * + * Authors: Pablo Inigo Blasco, Brett Aldrich + * + ******************************************************************************************************************/ +#pragma once + +#include + +#include "cb_nitrosz_client_behavior_base.hpp" + +namespace cl_nitrosz +{ +class CbRotate : public CbNav2ZClientBehaviorBase +{ +public: + float rotateDegree; + + std::optional goalChecker_; + + std::optional spinningPlanner; + + CbRotate(float rotate_degree); + + CbRotate(float rotate_degree, cl_nitrosz::SpinningPlanner spinning_planner); + + void onEntry() override; + +private: + //TODO: replace this with the Pose Component in the same way it is done in the CbAbsoluteRotateBehavior + std::shared_ptr listener; +}; +} // namespace cl_nitrosz diff --git a/smacc2_client_library/nitrosz_client/nitrosz_client/include/nitrosz_client/client_behaviors/cb_rotate_look_at.hpp b/smacc2_client_library/nitrosz_client/nitrosz_client/include/nitrosz_client/client_behaviors/cb_rotate_look_at.hpp new file mode 100644 index 000000000..d6a1ec5e0 --- /dev/null +++ b/smacc2_client_library/nitrosz_client/nitrosz_client/include/nitrosz_client/client_behaviors/cb_rotate_look_at.hpp @@ -0,0 +1,43 @@ +// Copyright 2021 RobosoftAI Inc. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +/***************************************************************************************************************** + * + * Authors: Pablo Inigo Blasco, Brett Aldrich + * + ******************************************************************************************************************/ +#pragma once + +#include + +#include +#include "cb_absolute_rotate.hpp" +#include "tf2_geometry_msgs/tf2_geometry_msgs.hpp" + +namespace cl_nitrosz +{ +class CbRotateLookAt : public CbAbsoluteRotate +{ +public: + std::shared_ptr listener; + + std::optional lookAtPose_; + + CbRotateLookAt(); + CbRotateLookAt(const geometry_msgs::msg::PoseStamped & lookAtPose); + + void onEntry() override; + // void onExit() override; +}; +} // namespace cl_nitrosz diff --git a/smacc2_client_library/nitrosz_client/nitrosz_client/include/nitrosz_client/client_behaviors/cb_save_slam_map.hpp b/smacc2_client_library/nitrosz_client/nitrosz_client/include/nitrosz_client/client_behaviors/cb_save_slam_map.hpp new file mode 100644 index 000000000..6272aacf4 --- /dev/null +++ b/smacc2_client_library/nitrosz_client/nitrosz_client/include/nitrosz_client/client_behaviors/cb_save_slam_map.hpp @@ -0,0 +1,46 @@ +// Copyright 2021 RobosoftAI Inc. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +/***************************************************************************************************************** + * + * Authors: Pablo Inigo Blasco, Brett Aldrich + * + ******************************************************************************************************************/ + +#pragma once + +#include +#include +#include +#include +#include + +namespace cl_nitrosz +{ +using namespace std::chrono_literals; +template +using CbServiceCall = smacc2::client_behaviors::CbServiceCall; + +struct CbSaveSlamMap : public CbServiceCall +{ +public: + CbSaveSlamMap(); + // void onEntry() override {} + + void onExit() override; + + std::shared_ptr getRequest( + /*slam_toolbox::srv::SaveMap_Request_ >::_name_type saved_map_name*/); +}; +} // namespace cl_nitrosz diff --git a/smacc2_client_library/nitrosz_client/nitrosz_client/include/nitrosz_client/client_behaviors/cb_seek_waypoint.hpp b/smacc2_client_library/nitrosz_client/nitrosz_client/include/nitrosz_client/client_behaviors/cb_seek_waypoint.hpp new file mode 100644 index 000000000..fb2d1d4ab --- /dev/null +++ b/smacc2_client_library/nitrosz_client/nitrosz_client/include/nitrosz_client/client_behaviors/cb_seek_waypoint.hpp @@ -0,0 +1,48 @@ +// Copyright 2021 RobosoftAI Inc. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +/***************************************************************************************************************** + * + * Authors: Pablo Inigo Blasco, Brett Aldrich + * + ******************************************************************************************************************/ +#pragma once + +#include +#include + +#include "cb_nitrosz_client_behavior_base.hpp" + +namespace cl_nitrosz +{ +class CbSeekWaypoint : public smacc2::ISmaccClientBehavior +{ +public: + CbSeekWaypoint(std::string skip_until_name); + + virtual ~CbSeekWaypoint(); + + void onEntry() override; + + void onExit() override; + + CpWaypointNavigator * waypointsNavigator_; + + NavigateNextWaypointOptions options_; + +private: + std::optional count_; + std::optional seekWaypointName_; +}; +} // namespace cl_nitrosz diff --git a/smacc2_client_library/nitrosz_client/nitrosz_client/include/nitrosz_client/client_behaviors/cb_stop_navigation.hpp b/smacc2_client_library/nitrosz_client/nitrosz_client/include/nitrosz_client/client_behaviors/cb_stop_navigation.hpp new file mode 100644 index 000000000..74746aae5 --- /dev/null +++ b/smacc2_client_library/nitrosz_client/nitrosz_client/include/nitrosz_client/client_behaviors/cb_stop_navigation.hpp @@ -0,0 +1,38 @@ +// Copyright 2021 RobosoftAI Inc. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +/***************************************************************************************************************** + * + * Authors: Pablo Inigo Blasco, Brett Aldrich + * + ******************************************************************************************************************/ +#pragma once + +#include + +#include "cb_navigate_global_position.hpp" +#include "cb_nitrosz_client_behavior_base.hpp" +#include "tf2_geometry_msgs/tf2_geometry_msgs.hpp" + +namespace cl_nitrosz +{ +class CbStopNavigation : public CbNavigateGlobalPosition +{ +public: + CbStopNavigation(); + + void onEntry() override; + void onExit() override; +}; +} // namespace cl_nitrosz diff --git a/smacc2_client_library/nitrosz_client/nitrosz_client/include/nitrosz_client/client_behaviors/cb_undo_path_backwards.hpp b/smacc2_client_library/nitrosz_client/nitrosz_client/include/nitrosz_client/client_behaviors/cb_undo_path_backwards.hpp new file mode 100644 index 000000000..9864a9d4b --- /dev/null +++ b/smacc2_client_library/nitrosz_client/nitrosz_client/include/nitrosz_client/client_behaviors/cb_undo_path_backwards.hpp @@ -0,0 +1,56 @@ +// Copyright 2021 RobosoftAI Inc. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +/***************************************************************************************************************** + * + * Authors: Pablo Inigo Blasco, Brett Aldrich + * + ******************************************************************************************************************/ +#pragma once + +#include + +#include +#include "cb_nitrosz_client_behavior_base.hpp" + +namespace cl_nitrosz +{ +using ::cl_nitrosz::odom_tracker::CpOdomTracker; + +struct CbUndoPathBackwardsOptions +{ + // the name of the goal checker selected in the navigation2 stack + std::optional goalCheckerId_; + + // the name of the goal checker selected in the navigation2 stack + std::optional undoControllerName_; +}; + +class CbUndoPathBackwards : public CbNav2ZClientBehaviorBase +{ +public: + CbUndoPathBackwards(std::optional options = std::nullopt); + + void onEntry() override; + + void onExit() override; + +private: + std::shared_ptr listener; + + CpOdomTracker * odomTracker; + + std::optional options_; +}; +} // namespace cl_nitrosz diff --git a/smacc2_client_library/nitrosz_client/nitrosz_client/include/nitrosz_client/client_behaviors/cb_wait_nitros_nodes.hpp b/smacc2_client_library/nitrosz_client/nitrosz_client/include/nitrosz_client/client_behaviors/cb_wait_nitros_nodes.hpp new file mode 100644 index 000000000..cee17bc59 --- /dev/null +++ b/smacc2_client_library/nitrosz_client/nitrosz_client/include/nitrosz_client/client_behaviors/cb_wait_nitros_nodes.hpp @@ -0,0 +1,67 @@ +// Copyright 2021 RobosoftAI Inc. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +/***************************************************************************************************************** + * + * Authors: Pablo Inigo Blasco, Brett Aldrich + * + ******************************************************************************************************************/ +#pragma once + +#include + +#include +#include + +namespace cl_nitrosz +{ +enum class Nav2Nodes +{ + PlannerServer, + ControllerServer, + RecoveriesServer, + BtNavigator, + MapServer, + None +}; + +std::string toString(Nav2Nodes value); + +Nav2Nodes fromString(std::string str); + +class CbWaitNav2Nodes : public smacc2::SmaccAsyncClientBehavior +{ +public: + CbWaitNav2Nodes( + std::string topicname, + std::vector waitNodes = { + Nav2Nodes::PlannerServer, Nav2Nodes::ControllerServer, Nav2Nodes::BtNavigator}); + + CbWaitNav2Nodes( + std::vector waitNodes = { + Nav2Nodes::PlannerServer, Nav2Nodes::ControllerServer, Nav2Nodes::BtNavigator}); + + void onEntry() override; + +private: + void onMessageReceived(const bond::msg::Status & msg); + + std::string topicname_; + + rclcpp::Subscription::SharedPtr sub_; + std::map receivedAliveMsg_; + + std::vector waitNodes_; +}; +} // namespace cl_nitrosz diff --git a/smacc2_client_library/nitrosz_client/nitrosz_client/include/nitrosz_client/client_behaviors/cb_wait_pose.hpp b/smacc2_client_library/nitrosz_client/nitrosz_client/include/nitrosz_client/client_behaviors/cb_wait_pose.hpp new file mode 100644 index 000000000..4116d8be2 --- /dev/null +++ b/smacc2_client_library/nitrosz_client/nitrosz_client/include/nitrosz_client/client_behaviors/cb_wait_pose.hpp @@ -0,0 +1,59 @@ +// Copyright 2021 RobosoftAI Inc. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +/***************************************************************************************************************** + * + * Authors: Pablo Inigo Blasco, Brett Aldrich + * + ******************************************************************************************************************/ +#pragma once + +#include +#include + +namespace cl_nitrosz +{ +// waits a robot pose message. Usually used for the startup synchronization. +enum class WaitPoseStandardReferenceFrame +{ + Map, + Odometry +}; + +// Waits a new pose using the Pose Component +// the specific pose to wait is configured in that component +class CbWaitPose : public smacc2::SmaccAsyncClientBehavior +{ +public: + // waits a new pose update of the Pose Component + CbWaitPose(); + + // waits a new pose update of the Pose Component in some reference frame (if there is no connection it will wait) + CbWaitPose(WaitPoseStandardReferenceFrame frame); + + virtual ~CbWaitPose(); + + template + void onOrthogonalAllocation() + { + this->requiresClient(nitroszClient_); + smacc2::SmaccAsyncClientBehavior::onOrthogonalAllocation(); + } + + void onEntry() override; + +protected: + cl_nitrosz::ClNitrosZ * nitroszClient_; +}; +} // namespace cl_nitrosz diff --git a/smacc2_client_library/nitrosz_client/nitrosz_client/include/nitrosz_client/client_behaviors/cb_wait_transform.hpp b/smacc2_client_library/nitrosz_client/nitrosz_client/include/nitrosz_client/client_behaviors/cb_wait_transform.hpp new file mode 100644 index 000000000..f5b1e06cc --- /dev/null +++ b/smacc2_client_library/nitrosz_client/nitrosz_client/include/nitrosz_client/client_behaviors/cb_wait_transform.hpp @@ -0,0 +1,60 @@ +// Copyright 2021 RobosoftAI Inc. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +/***************************************************************************************************************** + * + * Authors: Pablo Inigo Blasco, Brett Aldrich + * + ******************************************************************************************************************/ +#pragma once + +#include +#include + +#include + +#include +#include +#include +#include + +namespace cl_nitrosz +{ +class CbWaitTransform : public smacc2::SmaccAsyncClientBehavior +{ +public: + CbWaitTransform(std::string targetFrame, std::string referenceFrame, rclcpp::Duration timeout); + + virtual ~CbWaitTransform(); + + template + void onOrthogonalAllocation() + { + smacc2::SmaccAsyncClientBehavior::onOrthogonalAllocation(); + } + + void onEntry() override; + +protected: + // shared static listener + std::shared_ptr tfBuffer_; + std::shared_ptr tfListener_; + + std::string targetFrame_; + std::string referenceFrame_; + rclcpp::Duration timeout_; + + std::optional> result_; +}; +} // namespace cl_nitrosz diff --git a/smacc2_client_library/nitrosz_client/nitrosz_client/include/nitrosz_client/common.hpp b/smacc2_client_library/nitrosz_client/nitrosz_client/include/nitrosz_client/common.hpp new file mode 100644 index 000000000..0bf226e66 --- /dev/null +++ b/smacc2_client_library/nitrosz_client/nitrosz_client/include/nitrosz_client/common.hpp @@ -0,0 +1,34 @@ +// Copyright 2021 RobosoftAI Inc. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#pragma once + +#include + +#include +#include +#include +#include +#include +#include +#include +#include +#include + +std::ostream & operator<<(std::ostream & out, const geometry_msgs::msg::Quaternion & msg); +std::ostream & operator<<(std::ostream & out, const geometry_msgs::msg::Pose & msg); +std::ostream & operator<<(std::ostream & out, const geometry_msgs::msg::Point & msg); +std::ostream & operator<<(std::ostream & out, const geometry_msgs::msg::PoseStamped & msg); +std::ostream & operator<<(std::ostream & out, const nav2_msgs::action::NavigateToPose::Goal & msg); +std::ostream & operator<<(std::ostream & out, const builtin_interfaces::msg::Time & msg); diff --git a/smacc2_client_library/nitrosz_client/nitrosz_client/include/nitrosz_client/components/amcl/cp_amcl.hpp b/smacc2_client_library/nitrosz_client/nitrosz_client/include/nitrosz_client/components/amcl/cp_amcl.hpp new file mode 100644 index 000000000..6c4ce9873 --- /dev/null +++ b/smacc2_client_library/nitrosz_client/nitrosz_client/include/nitrosz_client/components/amcl/cp_amcl.hpp @@ -0,0 +1,43 @@ +// Copyright 2021 RobosoftAI Inc. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +/***************************************************************************************************************** + * + * Authors: Pablo Inigo Blasco, Brett Aldrich + * + ******************************************************************************************************************/ +#pragma once + +#include +#include + +namespace cl_nitrosz +{ +class CpAmcl : public smacc2::ISmaccComponent +{ +public: + CpAmcl(); + virtual ~CpAmcl(); + + std::string getName() const override; + + void onInitialize() override; + + void setInitialPose(const geometry_msgs::msg::PoseWithCovarianceStamped & initialpose); + +private: + rclcpp::Publisher::SharedPtr initalPosePub_; +}; + +} // namespace cl_nitrosz diff --git a/smacc2_client_library/nitrosz_client/nitrosz_client/include/nitrosz_client/components/costmap_switch/cp_costmap_switch.hpp b/smacc2_client_library/nitrosz_client/nitrosz_client/include/nitrosz_client/components/costmap_switch/cp_costmap_switch.hpp new file mode 100644 index 000000000..b6b5e30d3 --- /dev/null +++ b/smacc2_client_library/nitrosz_client/nitrosz_client/include/nitrosz_client/components/costmap_switch/cp_costmap_switch.hpp @@ -0,0 +1,92 @@ +// Copyright 2021 RobosoftAI Inc. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +/***************************************************************************************************************** + * + * Authors: Pablo Inigo Blasco, Brett Aldrich + * + ******************************************************************************************************************/ +#pragma once + +#include +#include +#include +#include +#include + +#include + +#include +#include + +// #include +// #include +// #include + +namespace cl_nitrosz +{ +class CpCostmapProxy; + +class CpCostmapSwitch : public smacc2::ISmaccComponent +{ +public: + enum class StandardLayers + { + GLOBAL_OBSTACLES_LAYER = 0, + LOCAL_OBSTACLES_LAYER = 1, + GLOBAL_INFLATED_LAYER = 2, + LOCAL_INFLATED_LAYER = 3 + }; + + static std::array layerNames; + + CpCostmapSwitch(); + + void onInitialize() override; + + static std::string getStandardCostmapName(StandardLayers layertype); + + bool exists(std::string layerName); + + void enable(std::string layerName); + + void enable(StandardLayers layerType); + + void disable(std::string layerName); + + void disable(StandardLayers layerType); + + void registerProxyFromDynamicReconfigureServer( + std::string costmapName, std::string enablePropertyName = "enabled"); + +private: + std::map> costmapProxies; + cl_nitrosz::ClNitrosZ * nitroszClient_; +}; +//------------------------------------------------------------------------- +class CpCostmapProxy +{ +public: + CpCostmapProxy( + std::string costmap_name, std::string enablePropertyName, rclcpp::Node::SharedPtr nh); + + void setCostmapEnabled(bool value); + +private: + std::string costmapName_; + rclcpp::Node::SharedPtr nh_; + + inline rclcpp::Node::SharedPtr getNode() { return nh_; } +}; +} // namespace cl_nitrosz diff --git a/smacc2_client_library/nitrosz_client/nitrosz_client/include/nitrosz_client/components/goal_checker_switcher/cp_goal_checker_switcher.hpp b/smacc2_client_library/nitrosz_client/nitrosz_client/include/nitrosz_client/components/goal_checker_switcher/cp_goal_checker_switcher.hpp new file mode 100644 index 000000000..63baf8281 --- /dev/null +++ b/smacc2_client_library/nitrosz_client/nitrosz_client/include/nitrosz_client/components/goal_checker_switcher/cp_goal_checker_switcher.hpp @@ -0,0 +1,47 @@ +// Copyright 2021 RobosoftAI Inc. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +/***************************************************************************************************************** + * + * Authors: Pablo Inigo Blasco, Brett Aldrich + * + ******************************************************************************************************************/ +#pragma once + +#include + +#include +#include +#include + +namespace cl_nitrosz +{ +// this class is used to switch the current goal checker of the remote navigation2 stack controller +class CpGoalCheckerSwitcher : public smacc2::ISmaccComponent +{ +public: + CpGoalCheckerSwitcher( + std::string goal_checker_selector_topic = "goal_checker_selector", + std::string default_goal_checker_name = "goal_checker"); + void onInitialize() override; + virtual ~CpGoalCheckerSwitcher(); + void setDefaultGoalChecker(); + void setGoalCheckerId(std::string goal_checker_id); + +private: + std::string goal_checker_selector_topic_; + std::string default_goal_checker_name_; + rclcpp::Publisher::SharedPtr goal_checker_selector_pub_; +}; +} // namespace cl_nitrosz diff --git a/smacc2_client_library/nitrosz_client/nitrosz_client/include/nitrosz_client/components/odom_tracker/cp_odom_tracker.hpp b/smacc2_client_library/nitrosz_client/nitrosz_client/include/nitrosz_client/components/odom_tracker/cp_odom_tracker.hpp new file mode 100644 index 000000000..c00959f69 --- /dev/null +++ b/smacc2_client_library/nitrosz_client/nitrosz_client/include/nitrosz_client/components/odom_tracker/cp_odom_tracker.hpp @@ -0,0 +1,189 @@ +// Copyright 2021 RobosoftAI Inc. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +/***************************************************************************************************************** + * + * Authors: Pablo Inigo Blasco, Brett Aldrich + * + ******************************************************************************************************************/ +#pragma once + +#include +#include +#include + +#include + +#include +#include +#include + +#include +#include +#include +#include +#include + +#include + +namespace cl_nitrosz +{ +namespace odom_tracker +{ +enum class WorkingMode : uint8_t +{ + RECORD_PATH = 0, + CLEAR_PATH = 1, + IDLE = 2 +}; + +/// This object tracks and saves the trajectories performed by the vehicle +/// so that they can be used later to execute operations such as "undo motions" +class CpOdomTracker : public smacc2::ISmaccComponent +{ +public: + // by default, the component start in record_path mode and publishing the + // current path + CpOdomTracker(std::string odomtopicName = "/odom", std::string odomFrame = "odom"); + + // threadsafe + /// odom callback: Updates the path - this must be called periodically for each odometry message. + // The odom parameters is the main input of this tracker + virtual void processOdometryMessage(const nav_msgs::msg::Odometry::SharedPtr odom); + + // ------ CONTROL COMMANDS --------------------- + // threadsafe + void setWorkingMode(WorkingMode workingMode); + + // threadsafe + void setPublishMessages(bool value); + + // threadsafe + void pushPath(); + + // threadsafe + void pushPath(std::string pathname); + + // threadsafe + void popPath(int pathCount = 1, bool keepPreviousPath = false); + + // threadsafe + void clearPath(); + + // threadsafe + void setStartPoint(const geometry_msgs::msg::PoseStamped & pose); + + // threadsafe + void setStartPoint(const geometry_msgs::msg::Pose & pose); + + // threadsafe - use only for recording mode, it is reset always a new path is pushed, popped or cleared + void setCurrentMotionGoal(const geometry_msgs::msg::PoseStamped & pose); + + void setCurrentPathName(const std::string & currentPathName); + + // threadsafe + std::optional getCurrentMotionGoal(); + + // threadsafe + nav_msgs::msg::Path getPath(); + + void logStateString(bool debug = true); + +protected: + void onInitialize() override; + + void updateConfiguration(); + + virtual void rtPublishPaths(rclcpp::Time timestamp); + + // this is called when a new odom message is received in record path mode + virtual bool updateRecordPath(const nav_msgs::msg::Odometry & odom); + + // this is called when a new odom message is received in clear path mode + virtual bool updateClearPath(const nav_msgs::msg::Odometry & odom); + + void updateAggregatedStackPath(); + + // -------------- OUTPUTS --------------------- + rclcpp::Publisher::SharedPtr robotBasePathPub_; + rclcpp::Publisher::SharedPtr robotBasePathStackedPub_; + + // --------------- INPUTS ------------------------ + // optional, this class can be used directly calling the odomProcessing method + // without any subscriber + rclcpp::Subscription::SharedPtr odomSub_; + + // -------------- PARAMETERS ---------------------- + /// How much distance there is between two points of the path + double recordPointDistanceThreshold_; + + /// Meters + double recordAngularDistanceThreshold_; + + /// rads + double clearPointDistanceThreshold_; + + /// rads + double clearAngularDistanceThreshold_; + + std::string odomFrame_; + + std::string odomTopicName_; + + // --------------- STATE --------------- + // default true + bool publishMessages; + + /// Processed path for the mouth of the reel + nav_msgs::msg::Path baseTrajectory_; + + WorkingMode workingMode_; + + std::vector pathStack_; + + struct PathInfo + { + std::string name; + std::optional goalPose; + }; + + std::vector pathInfos_; + + nav_msgs::msg::Path aggregatedStackPathMsg_; + + // subscribes to topic on init if true + bool subscribeToOdometryTopic_; + + std::optional currentMotionGoal_; + std::string currentPathName_; + + std::mutex m_mutex_; +}; + +/** + ****************************************************************************************************************** + * p2pDistance() + ****************************************************************************************************************** + */ +inline double p2pDistance( + const geometry_msgs::msg::Point & p1, const geometry_msgs::msg::Point & p2) +{ + double dx = (p1.x - p2.x); + double dy = (p1.y - p2.y); + double dz = (p2.z - p2.z); + double dist = sqrt(dx * dx + dy * dy + dz * dz); + return dist; +} +} // namespace odom_tracker +} // namespace cl_nitrosz diff --git a/smacc2_client_library/nitrosz_client/nitrosz_client/include/nitrosz_client/components/planner_switcher/cp_planner_switcher.hpp b/smacc2_client_library/nitrosz_client/nitrosz_client/include/nitrosz_client/components/planner_switcher/cp_planner_switcher.hpp new file mode 100644 index 000000000..44fa37163 --- /dev/null +++ b/smacc2_client_library/nitrosz_client/nitrosz_client/include/nitrosz_client/components/planner_switcher/cp_planner_switcher.hpp @@ -0,0 +1,70 @@ +// Copyright 2021 RobosoftAI Inc. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +/***************************************************************************************************************** + * + * Authors: Pablo Inigo Blasco, Brett Aldrich + * + ******************************************************************************************************************/ +#pragma once + +#include +#include +#include +#include + +namespace cl_nitrosz +{ +// this component is used to switch the current planner and controller interacting +// with the remote navigation2 stack nodes (bt_navigator, planner_server, controller_server) +class CpPlannerSwitcher : public smacc2::ISmaccComponent +{ +public: + CpPlannerSwitcher(); + + void onInitialize() override; + + void setDesiredGlobalPlanner(std::string); + + void setDesiredController(std::string); + + void commitPublish(); + + // STANDARD PLANNERS + + void setBackwardPlanner(bool commit = true); + + void setUndoPathBackwardPlanner(bool commit = true); + + void setForwardPlanner(bool commit = true); + + void setPureSpinningPlanner(bool commit = true); + + // sets ROS defaults local and global planners + void setDefaultPlanners(bool commit = true); + +private: + std::string desired_planner_; + + std::string desired_controller_; + + bool set_planners_mode_flag_; + + rclcpp::Publisher::SharedPtr planner_selector_pub_; + + rclcpp::Publisher::SharedPtr controller_selector_pub_; + + rclcpp::Publisher::SharedPtr goal_checker_selector_pub_; +}; +} // namespace cl_nitrosz diff --git a/smacc2_client_library/nitrosz_client/nitrosz_client/include/nitrosz_client/components/pose/cp_pose.hpp b/smacc2_client_library/nitrosz_client/nitrosz_client/include/nitrosz_client/components/pose/cp_pose.hpp new file mode 100644 index 000000000..55a1a495d --- /dev/null +++ b/smacc2_client_library/nitrosz_client/nitrosz_client/include/nitrosz_client/components/pose/cp_pose.hpp @@ -0,0 +1,95 @@ +// Copyright 2021 RobosoftAI Inc. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +/***************************************************************************************************************** + *-2020 + * Authors: Pablo Inigo Blasco, Brett Aldrich + * + ******************************************************************************************************************/ +#pragma once + +#include + +#include +#include +#include +#include +#include +#include +#include + +#include +#include + +namespace cl_nitrosz +{ +enum class StandardReferenceFrames +{ + Map, + Odometry +}; + +class Pose : public smacc2::ISmaccComponent, public smacc2::ISmaccUpdatable +{ +public: + Pose(std::string poseFrameName = "base_link", std::string referenceFrame = "odom"); + + Pose(StandardReferenceFrames referenceFrame); + + void onInitialize() override; + + void update() override; + + // synchronously waits a transform in the current thread + void waitTransformUpdate(rclcpp::Rate r = rclcpp::Rate(20)); + + inline geometry_msgs::msg::Pose toPoseMsg() + { + std::lock_guard guard(m_mutex_); + return this->pose_.pose; + } + + inline geometry_msgs::msg::PoseStamped toPoseStampedMsg() + { + std::lock_guard guard(m_mutex_); + return this->pose_; + } + + // get yaw in radians + float getYaw(); + + float getX(); + float getY(); + float getZ(); + + inline const std::string & getReferenceFrame() const { return referenceFrame_; } + + inline const std::string & getFrameId() const { return poseFrameName_; } + + bool isInitialized; + +private: + geometry_msgs::msg::PoseStamped pose_; + + static std::shared_ptr tfBuffer_; + static std::shared_ptr tfListener_; + + static std::mutex listenerMutex_; + + std::string poseFrameName_; + std::string referenceFrame_; + + std::mutex m_mutex_; +}; +} // namespace cl_nitrosz diff --git a/smacc2_client_library/nitrosz_client/nitrosz_client/include/nitrosz_client/components/slam_toolbox/cp_slam_toolbox.hpp b/smacc2_client_library/nitrosz_client/nitrosz_client/include/nitrosz_client/components/slam_toolbox/cp_slam_toolbox.hpp new file mode 100644 index 000000000..8f802cbf9 --- /dev/null +++ b/smacc2_client_library/nitrosz_client/nitrosz_client/include/nitrosz_client/components/slam_toolbox/cp_slam_toolbox.hpp @@ -0,0 +1,50 @@ +// Copyright 2021 RobosoftAI Inc. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +/***************************************************************************************************************** + * + * Authors: Pablo Inigo Blasco, Brett Aldrich + * + ******************************************************************************************************************/ +#pragma once + +#include +#include +#include +#include + +namespace cl_nitrosz +{ +// stores the state of the last modification of the slam node +// (blind - open loop solution because the node does not provide that information) +class CpSlamToolbox : public smacc2::ISmaccComponent +{ +public: + CpSlamToolbox(); + virtual ~CpSlamToolbox(); + + enum class SlamToolboxState + { + Resumed, + Paused + }; + + inline SlamToolboxState getState() { return state_; } + + void toogleState(); + +private: + SlamToolboxState state_; +}; +} // namespace cl_nitrosz diff --git a/smacc2_client_library/nitrosz_client/nitrosz_client/include/nitrosz_client/components/waypoints_navigator/cp_waypoints_event_dispatcher.hpp b/smacc2_client_library/nitrosz_client/nitrosz_client/include/nitrosz_client/components/waypoints_navigator/cp_waypoints_event_dispatcher.hpp new file mode 100644 index 000000000..ba5134a23 --- /dev/null +++ b/smacc2_client_library/nitrosz_client/nitrosz_client/include/nitrosz_client/components/waypoints_navigator/cp_waypoints_event_dispatcher.hpp @@ -0,0 +1,1864 @@ +// Copyright 2021 RobosoftAI Inc. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +/***************************************************************************************************************** + * + * Authors: Pablo Inigo Blasco, Brett Aldrich + * + ******************************************************************************************************************/ +#pragma once + +#include +#include + +namespace cl_nitrosz + +/// @cond +// The purpose of the cond and endcond tags seen above and below in this file are there to prevent doxygen from indexing the 256 events +// below which makes the documentation produced unuseable. + +{ +template +struct EvWaypoint0 : sc::event> +{ + int waypointIndex; +}; + +template +struct EvWaypoint1 : sc::event> +{ + int waypointIndex; +}; + +template +struct EvWaypoint2 : sc::event> +{ + int waypointIndex; +}; + +template +struct EvWaypoint3 : sc::event> +{ + int waypointIndex; +}; + +template +struct EvWaypoint4 : sc::event> +{ + int waypointIndex; +}; + +template +struct EvWaypoint5 : sc::event> +{ + int waypointIndex; +}; + +template +struct EvWaypoint6 : sc::event> +{ + int waypointIndex; +}; + +template +struct EvWaypoint7 : sc::event> +{ + int waypointIndex; +}; + +template +struct EvWaypoint8 : sc::event> +{ + int waypointIndex; +}; + +template +struct EvWaypoint9 : sc::event> +{ + int waypointIndex; +}; + +template +struct EvWaypoint10 : sc::event> +{ + int waypointIndex; +}; + +template +struct EvWaypoint11 : sc::event> +{ + int waypointIndex; +}; + +template +struct EvWaypoint12 : sc::event> +{ + int waypointIndex; +}; + +template +struct EvWaypoint13 : sc::event> +{ + int waypointIndex; +}; + +template +struct EvWaypoint14 : sc::event> +{ + int waypointIndex; +}; + +template +struct EvWaypoint15 : sc::event> +{ + int waypointIndex; +}; + +template +struct EvWaypoint16 : sc::event> +{ + int waypointIndex; +}; + +template +struct EvWaypoint17 : sc::event> +{ + int waypointIndex; +}; + +template +struct EvWaypoint18 : sc::event> +{ + int waypointIndex; +}; + +template +struct EvWaypoint19 : sc::event> +{ + int waypointIndex; +}; + +template +struct EvWaypoint20 : sc::event> +{ + int waypointIndex; +}; + +template +struct EvWaypoint21 : sc::event> +{ + int waypointIndex; +}; + +template +struct EvWaypoint22 : sc::event> +{ + int waypointIndex; +}; + +template +struct EvWaypoint23 : sc::event> +{ + int waypointIndex; +}; + +template +struct EvWaypoint24 : sc::event> +{ + int waypointIndex; +}; + +template +struct EvWaypoint25 : sc::event> +{ + int waypointIndex; +}; + +template +struct EvWaypoint26 : sc::event> +{ + int waypointIndex; +}; + +template +struct EvWaypoint27 : sc::event> +{ + int waypointIndex; +}; + +template +struct EvWaypoint28 : sc::event> +{ + int waypointIndex; +}; + +template +struct EvWaypoint29 : sc::event> +{ + int waypointIndex; +}; + +template +struct EvWaypoint30 : sc::event> +{ + int waypointIndex; +}; + +template +struct EvWaypoint31 : sc::event> +{ + int waypointIndex; +}; + +template +struct EvWaypoint32 : sc::event> +{ + int waypointIndex; +}; + +template +struct EvWaypoint33 : sc::event> +{ + int waypointIndex; +}; + +template +struct EvWaypoint34 : sc::event> +{ + int waypointIndex; +}; + +template +struct EvWaypoint35 : sc::event> +{ + int waypointIndex; +}; + +template +struct EvWaypoint36 : sc::event> +{ + int waypointIndex; +}; + +template +struct EvWaypoint37 : sc::event> +{ + int waypointIndex; +}; + +template +struct EvWaypoint38 : sc::event> +{ + int waypointIndex; +}; + +template +struct EvWaypoint39 : sc::event> +{ + int waypointIndex; +}; + +template +struct EvWaypoint40 : sc::event> +{ + int waypointIndex; +}; + +template +struct EvWaypoint41 : sc::event> +{ + int waypointIndex; +}; + +template +struct EvWaypoint42 : sc::event> +{ + int waypointIndex; +}; + +template +struct EvWaypoint43 : sc::event> +{ + int waypointIndex; +}; + +template +struct EvWaypoint44 : sc::event> +{ + int waypointIndex; +}; + +template +struct EvWaypoint45 : sc::event> +{ + int waypointIndex; +}; + +template +struct EvWaypoint46 : sc::event> +{ + int waypointIndex; +}; + +template +struct EvWaypoint47 : sc::event> +{ + int waypointIndex; +}; + +template +struct EvWaypoint48 : sc::event> +{ + int waypointIndex; +}; + +template +struct EvWaypoint49 : sc::event> +{ + int waypointIndex; +}; + +template +struct EvWaypoint50 : sc::event> +{ + int waypointIndex; +}; + +template +struct EvWaypoint51 : sc::event> +{ + int waypointIndex; +}; + +template +struct EvWaypoint52 : sc::event> +{ + int waypointIndex; +}; + +template +struct EvWaypoint53 : sc::event> +{ + int waypointIndex; +}; + +template +struct EvWaypoint54 : sc::event> +{ + int waypointIndex; +}; + +template +struct EvWaypoint55 : sc::event> +{ + int waypointIndex; +}; + +template +struct EvWaypoint56 : sc::event> +{ + int waypointIndex; +}; + +template +struct EvWaypoint57 : sc::event> +{ + int waypointIndex; +}; + +template +struct EvWaypoint58 : sc::event> +{ + int waypointIndex; +}; + +template +struct EvWaypoint59 : sc::event> +{ + int waypointIndex; +}; + +template +struct EvWaypoint60 : sc::event> +{ + int waypointIndex; +}; + +template +struct EvWaypoint61 : sc::event> +{ + int waypointIndex; +}; + +template +struct EvWaypoint62 : sc::event> +{ + int waypointIndex; +}; + +template +struct EvWaypoint63 : sc::event> +{ + int waypointIndex; +}; + +template +struct EvWaypoint64 : sc::event> +{ + int waypointIndex; +}; + +template +struct EvWaypoint65 : sc::event> +{ + int waypointIndex; +}; + +template +struct EvWaypoint66 : sc::event> +{ + int waypointIndex; +}; + +template +struct EvWaypoint67 : sc::event> +{ + int waypointIndex; +}; + +template +struct EvWaypoint68 : sc::event> +{ + int waypointIndex; +}; + +template +struct EvWaypoint69 : sc::event> +{ + int waypointIndex; +}; + +template +struct EvWaypoint70 : sc::event> +{ + int waypointIndex; +}; + +template +struct EvWaypoint71 : sc::event> +{ + int waypointIndex; +}; + +template +struct EvWaypoint72 : sc::event> +{ + int waypointIndex; +}; + +template +struct EvWaypoint73 : sc::event> +{ + int waypointIndex; +}; + +template +struct EvWaypoint74 : sc::event> +{ + int waypointIndex; +}; + +template +struct EvWaypoint75 : sc::event> +{ + int waypointIndex; +}; + +template +struct EvWaypoint76 : sc::event> +{ + int waypointIndex; +}; + +template +struct EvWaypoint77 : sc::event> +{ + int waypointIndex; +}; + +template +struct EvWaypoint78 : sc::event> +{ + int waypointIndex; +}; + +template +struct EvWaypoint79 : sc::event> +{ + int waypointIndex; +}; + +template +struct EvWaypoint80 : sc::event> +{ + int waypointIndex; +}; + +template +struct EvWaypoint81 : sc::event> +{ + int waypointIndex; +}; + +template +struct EvWaypoint82 : sc::event> +{ + int waypointIndex; +}; + +template +struct EvWaypoint83 : sc::event> +{ + int waypointIndex; +}; + +template +struct EvWaypoint84 : sc::event> +{ + int waypointIndex; +}; + +template +struct EvWaypoint85 : sc::event> +{ + int waypointIndex; +}; + +template +struct EvWaypoint86 : sc::event> +{ + int waypointIndex; +}; + +template +struct EvWaypoint87 : sc::event> +{ + int waypointIndex; +}; + +template +struct EvWaypoint88 : sc::event> +{ + int waypointIndex; +}; + +template +struct EvWaypoint89 : sc::event> +{ + int waypointIndex; +}; + +template +struct EvWaypoint90 : sc::event> +{ + int waypointIndex; +}; + +template +struct EvWaypoint91 : sc::event> +{ + int waypointIndex; +}; + +template +struct EvWaypoint92 : sc::event> +{ + int waypointIndex; +}; + +template +struct EvWaypoint93 : sc::event> +{ + int waypointIndex; +}; + +template +struct EvWaypoint94 : sc::event> +{ + int waypointIndex; +}; + +template +struct EvWaypoint95 : sc::event> +{ + int waypointIndex; +}; + +template +struct EvWaypoint96 : sc::event> +{ + int waypointIndex; +}; + +template +struct EvWaypoint97 : sc::event> +{ + int waypointIndex; +}; + +template +struct EvWaypoint98 : sc::event> +{ + int waypointIndex; +}; + +template +struct EvWaypoint99 : sc::event> +{ + int waypointIndex; +}; + +template +struct EvWaypoint100 : sc::event> +{ + int waypointIndex; +}; + +template +struct EvWaypoint101 : sc::event> +{ + int waypointIndex; +}; + +template +struct EvWaypoint102 : sc::event> +{ + int waypointIndex; +}; + +template +struct EvWaypoint103 : sc::event> +{ + int waypointIndex; +}; + +template +struct EvWaypoint104 : sc::event> +{ + int waypointIndex; +}; + +template +struct EvWaypoint105 : sc::event> +{ + int waypointIndex; +}; + +template +struct EvWaypoint106 : sc::event> +{ + int waypointIndex; +}; + +template +struct EvWaypoint107 : sc::event> +{ + int waypointIndex; +}; + +template +struct EvWaypoint108 : sc::event> +{ + int waypointIndex; +}; + +template +struct EvWaypoint109 : sc::event> +{ + int waypointIndex; +}; + +template +struct EvWaypoint110 : sc::event> +{ + int waypointIndex; +}; + +template +struct EvWaypoint111 : sc::event> +{ + int waypointIndex; +}; + +template +struct EvWaypoint112 : sc::event> +{ + int waypointIndex; +}; + +template +struct EvWaypoint113 : sc::event> +{ + int waypointIndex; +}; + +template +struct EvWaypoint114 : sc::event> +{ + int waypointIndex; +}; + +template +struct EvWaypoint115 : sc::event> +{ + int waypointIndex; +}; + +template +struct EvWaypoint116 : sc::event> +{ + int waypointIndex; +}; + +template +struct EvWaypoint117 : sc::event> +{ + int waypointIndex; +}; + +template +struct EvWaypoint118 : sc::event> +{ + int waypointIndex; +}; + +template +struct EvWaypoint119 : sc::event> +{ + int waypointIndex; +}; + +template +struct EvWaypoint120 : sc::event> +{ + int waypointIndex; +}; + +template +struct EvWaypoint121 : sc::event> +{ + int waypointIndex; +}; + +template +struct EvWaypoint122 : sc::event> +{ + int waypointIndex; +}; + +template +struct EvWaypoint123 : sc::event> +{ + int waypointIndex; +}; + +template +struct EvWaypoint124 : sc::event> +{ + int waypointIndex; +}; + +template +struct EvWaypoint125 : sc::event> +{ + int waypointIndex; +}; + +template +struct EvWaypoint126 : sc::event> +{ + int waypointIndex; +}; + +template +struct EvWaypoint127 : sc::event> +{ + int waypointIndex; +}; + +template +struct EvWaypoint128 : sc::event> +{ + int waypointIndex; +}; + +template +struct EvWaypoint129 : sc::event> +{ + int waypointIndex; +}; + +template +struct EvWaypoint130 : sc::event> +{ + int waypointIndex; +}; + +template +struct EvWaypoint131 : sc::event> +{ + int waypointIndex; +}; + +template +struct EvWaypoint132 : sc::event> +{ + int waypointIndex; +}; + +template +struct EvWaypoint133 : sc::event> +{ + int waypointIndex; +}; + +template +struct EvWaypoint134 : sc::event> +{ + int waypointIndex; +}; + +template +struct EvWaypoint135 : sc::event> +{ + int waypointIndex; +}; + +template +struct EvWaypoint136 : sc::event> +{ + int waypointIndex; +}; + +template +struct EvWaypoint137 : sc::event> +{ + int waypointIndex; +}; + +template +struct EvWaypoint138 : sc::event> +{ + int waypointIndex; +}; + +template +struct EvWaypoint139 : sc::event> +{ + int waypointIndex; +}; + +template +struct EvWaypoint140 : sc::event> +{ + int waypointIndex; +}; + +template +struct EvWaypoint141 : sc::event> +{ + int waypointIndex; +}; + +template +struct EvWaypoint142 : sc::event> +{ + int waypointIndex; +}; + +template +struct EvWaypoint143 : sc::event> +{ + int waypointIndex; +}; + +template +struct EvWaypoint144 : sc::event> +{ + int waypointIndex; +}; + +template +struct EvWaypoint145 : sc::event> +{ + int waypointIndex; +}; + +template +struct EvWaypoint146 : sc::event> +{ + int waypointIndex; +}; + +template +struct EvWaypoint147 : sc::event> +{ + int waypointIndex; +}; + +template +struct EvWaypoint148 : sc::event> +{ + int waypointIndex; +}; + +template +struct EvWaypoint149 : sc::event> +{ + int waypointIndex; +}; + +template +struct EvWaypoint150 : sc::event> +{ + int waypointIndex; +}; + +template +struct EvWaypoint151 : sc::event> +{ + int waypointIndex; +}; + +template +struct EvWaypoint152 : sc::event> +{ + int waypointIndex; +}; + +template +struct EvWaypoint153 : sc::event> +{ + int waypointIndex; +}; + +template +struct EvWaypoint154 : sc::event> +{ + int waypointIndex; +}; + +template +struct EvWaypoint155 : sc::event> +{ + int waypointIndex; +}; + +template +struct EvWaypoint156 : sc::event> +{ + int waypointIndex; +}; + +template +struct EvWaypoint157 : sc::event> +{ + int waypointIndex; +}; + +template +struct EvWaypoint158 : sc::event> +{ + int waypointIndex; +}; + +template +struct EvWaypoint159 : sc::event> +{ + int waypointIndex; +}; + +template +struct EvWaypoint160 : sc::event> +{ + int waypointIndex; +}; + +template +struct EvWaypoint161 : sc::event> +{ + int waypointIndex; +}; + +template +struct EvWaypoint162 : sc::event> +{ + int waypointIndex; +}; + +template +struct EvWaypoint163 : sc::event> +{ + int waypointIndex; +}; + +template +struct EvWaypoint164 : sc::event> +{ + int waypointIndex; +}; + +template +struct EvWaypoint165 : sc::event> +{ + int waypointIndex; +}; + +template +struct EvWaypoint166 : sc::event> +{ + int waypointIndex; +}; + +template +struct EvWaypoint167 : sc::event> +{ + int waypointIndex; +}; + +template +struct EvWaypoint168 : sc::event> +{ + int waypointIndex; +}; + +template +struct EvWaypoint169 : sc::event> +{ + int waypointIndex; +}; + +template +struct EvWaypoint170 : sc::event> +{ + int waypointIndex; +}; + +template +struct EvWaypoint171 : sc::event> +{ + int waypointIndex; +}; + +template +struct EvWaypoint172 : sc::event> +{ + int waypointIndex; +}; + +template +struct EvWaypoint173 : sc::event> +{ + int waypointIndex; +}; + +template +struct EvWaypoint174 : sc::event> +{ + int waypointIndex; +}; + +template +struct EvWaypoint175 : sc::event> +{ + int waypointIndex; +}; + +template +struct EvWaypoint176 : sc::event> +{ + int waypointIndex; +}; + +template +struct EvWaypoint177 : sc::event> +{ + int waypointIndex; +}; + +template +struct EvWaypoint178 : sc::event> +{ + int waypointIndex; +}; + +template +struct EvWaypoint179 : sc::event> +{ + int waypointIndex; +}; + +template +struct EvWaypoint180 : sc::event> +{ + int waypointIndex; +}; + +template +struct EvWaypoint181 : sc::event> +{ + int waypointIndex; +}; + +template +struct EvWaypoint182 : sc::event> +{ + int waypointIndex; +}; + +template +struct EvWaypoint183 : sc::event> +{ + int waypointIndex; +}; + +template +struct EvWaypoint184 : sc::event> +{ + int waypointIndex; +}; + +template +struct EvWaypoint185 : sc::event> +{ + int waypointIndex; +}; + +template +struct EvWaypoint186 : sc::event> +{ + int waypointIndex; +}; + +template +struct EvWaypoint187 : sc::event> +{ + int waypointIndex; +}; + +template +struct EvWaypoint188 : sc::event> +{ + int waypointIndex; +}; + +template +struct EvWaypoint189 : sc::event> +{ + int waypointIndex; +}; + +template +struct EvWaypoint190 : sc::event> +{ + int waypointIndex; +}; + +template +struct EvWaypoint191 : sc::event> +{ + int waypointIndex; +}; + +template +struct EvWaypoint192 : sc::event> +{ + int waypointIndex; +}; + +template +struct EvWaypoint193 : sc::event> +{ + int waypointIndex; +}; + +template +struct EvWaypoint194 : sc::event> +{ + int waypointIndex; +}; + +template +struct EvWaypoint195 : sc::event> +{ + int waypointIndex; +}; + +template +struct EvWaypoint196 : sc::event> +{ + int waypointIndex; +}; + +template +struct EvWaypoint197 : sc::event> +{ + int waypointIndex; +}; + +template +struct EvWaypoint198 : sc::event> +{ + int waypointIndex; +}; + +template +struct EvWaypoint199 : sc::event> +{ + int waypointIndex; +}; + +template +struct EvWaypoint200 : sc::event> +{ + int waypointIndex; +}; + +template +struct EvWaypoint201 : sc::event> +{ + int waypointIndex; +}; + +template +struct EvWaypoint202 : sc::event> +{ + int waypointIndex; +}; + +template +struct EvWaypoint203 : sc::event> +{ + int waypointIndex; +}; + +template +struct EvWaypoint204 : sc::event> +{ + int waypointIndex; +}; + +template +struct EvWaypoint205 : sc::event> +{ + int waypointIndex; +}; + +template +struct EvWaypoint206 : sc::event> +{ + int waypointIndex; +}; + +template +struct EvWaypoint207 : sc::event> +{ + int waypointIndex; +}; + +template +struct EvWaypoint208 : sc::event> +{ + int waypointIndex; +}; + +template +struct EvWaypoint209 : sc::event> +{ + int waypointIndex; +}; + +template +struct EvWaypoint210 : sc::event> +{ + int waypointIndex; +}; + +template +struct EvWaypoint211 : sc::event> +{ + int waypointIndex; +}; + +template +struct EvWaypoint212 : sc::event> +{ + int waypointIndex; +}; + +template +struct EvWaypoint213 : sc::event> +{ + int waypointIndex; +}; + +template +struct EvWaypoint214 : sc::event> +{ + int waypointIndex; +}; + +template +struct EvWaypoint215 : sc::event> +{ + int waypointIndex; +}; + +template +struct EvWaypoint216 : sc::event> +{ + int waypointIndex; +}; + +template +struct EvWaypoint217 : sc::event> +{ + int waypointIndex; +}; + +template +struct EvWaypoint218 : sc::event> +{ + int waypointIndex; +}; + +template +struct EvWaypoint219 : sc::event> +{ + int waypointIndex; +}; + +template +struct EvWaypoint220 : sc::event> +{ + int waypointIndex; +}; + +template +struct EvWaypoint221 : sc::event> +{ + int waypointIndex; +}; + +template +struct EvWaypoint222 : sc::event> +{ + int waypointIndex; +}; + +template +struct EvWaypoint223 : sc::event> +{ + int waypointIndex; +}; + +template +struct EvWaypoint224 : sc::event> +{ + int waypointIndex; +}; + +template +struct EvWaypoint225 : sc::event> +{ + int waypointIndex; +}; + +template +struct EvWaypoint226 : sc::event> +{ + int waypointIndex; +}; + +template +struct EvWaypoint227 : sc::event> +{ + int waypointIndex; +}; + +template +struct EvWaypoint228 : sc::event> +{ + int waypointIndex; +}; + +template +struct EvWaypoint229 : sc::event> +{ + int waypointIndex; +}; + +template +struct EvWaypoint230 : sc::event> +{ + int waypointIndex; +}; + +template +struct EvWaypoint231 : sc::event> +{ + int waypointIndex; +}; + +template +struct EvWaypoint232 : sc::event> +{ + int waypointIndex; +}; + +template +struct EvWaypoint233 : sc::event> +{ + int waypointIndex; +}; + +template +struct EvWaypoint234 : sc::event> +{ + int waypointIndex; +}; + +template +struct EvWaypoint235 : sc::event> +{ + int waypointIndex; +}; + +template +struct EvWaypoint236 : sc::event> +{ + int waypointIndex; +}; + +template +struct EvWaypoint237 : sc::event> +{ + int waypointIndex; +}; + +template +struct EvWaypoint238 : sc::event> +{ + int waypointIndex; +}; + +template +struct EvWaypoint239 : sc::event> +{ + int waypointIndex; +}; + +template +struct EvWaypoint240 : sc::event> +{ + int waypointIndex; +}; + +template +struct EvWaypoint241 : sc::event> +{ + int waypointIndex; +}; + +template +struct EvWaypoint242 : sc::event> +{ + int waypointIndex; +}; + +template +struct EvWaypoint243 : sc::event> +{ + int waypointIndex; +}; + +template +struct EvWaypoint244 : sc::event> +{ + int waypointIndex; +}; + +template +struct EvWaypoint245 : sc::event> +{ + int waypointIndex; +}; + +template +struct EvWaypoint246 : sc::event> +{ + int waypointIndex; +}; + +template +struct EvWaypoint247 : sc::event> +{ + int waypointIndex; +}; + +template +struct EvWaypoint248 : sc::event> +{ + int waypointIndex; +}; + +template +struct EvWaypoint249 : sc::event> +{ + int waypointIndex; +}; + +template +struct EvWaypoint250 : sc::event> +{ + int waypointIndex; +}; + +template +struct EvWaypoint251 : sc::event> +{ + int waypointIndex; +}; + +template +struct EvWaypoint252 : sc::event> +{ + int waypointIndex; +}; + +template +struct EvWaypoint253 : sc::event> +{ + int waypointIndex; +}; + +template +struct EvWaypoint254 : sc::event> +{ + int waypointIndex; +}; + +template +struct EvWaypoint255 : sc::event> +{ + int waypointIndex; +}; + +template +struct EvWaypoint256 : sc::event> +{ + int waypointIndex; +}; + +// Doxygen endcond tag +/// @endcond + +class ClNitrosZ; + +#define WAYPOINTS_EVENTCOUNT 257 + +class WaypointEventDispatcher +{ + std::function postWaypointFn[WAYPOINTS_EVENTCOUNT]; + +public: + template + void initialize(ClNitrosZ * client); + + void postWaypointEvent(int index); +}; + +template +void configurePostEvWaypoint(std::function * fntarget, ClNitrosZ * client, int index) +{ + fntarget[index] = [=]() { client->template postEvent(); }; +} + +template +void WaypointEventDispatcher::initialize(ClNitrosZ * client) +{ + // Doygen cond tag + /// @cond + configurePostEvWaypoint>(postWaypointFn, client, 0); + configurePostEvWaypoint>(postWaypointFn, client, 1); + configurePostEvWaypoint>(postWaypointFn, client, 2); + configurePostEvWaypoint>(postWaypointFn, client, 3); + configurePostEvWaypoint>(postWaypointFn, client, 4); + configurePostEvWaypoint>(postWaypointFn, client, 5); + configurePostEvWaypoint>(postWaypointFn, client, 6); + configurePostEvWaypoint>(postWaypointFn, client, 7); + configurePostEvWaypoint>(postWaypointFn, client, 8); + configurePostEvWaypoint>(postWaypointFn, client, 9); + configurePostEvWaypoint>(postWaypointFn, client, 10); + configurePostEvWaypoint>(postWaypointFn, client, 11); + configurePostEvWaypoint>(postWaypointFn, client, 12); + configurePostEvWaypoint>(postWaypointFn, client, 13); + configurePostEvWaypoint>(postWaypointFn, client, 14); + configurePostEvWaypoint>(postWaypointFn, client, 15); + configurePostEvWaypoint>(postWaypointFn, client, 16); + configurePostEvWaypoint>(postWaypointFn, client, 17); + configurePostEvWaypoint>(postWaypointFn, client, 18); + configurePostEvWaypoint>(postWaypointFn, client, 19); + configurePostEvWaypoint>(postWaypointFn, client, 20); + configurePostEvWaypoint>(postWaypointFn, client, 21); + configurePostEvWaypoint>(postWaypointFn, client, 22); + configurePostEvWaypoint>(postWaypointFn, client, 23); + configurePostEvWaypoint>(postWaypointFn, client, 24); + configurePostEvWaypoint>(postWaypointFn, client, 25); + configurePostEvWaypoint>(postWaypointFn, client, 26); + configurePostEvWaypoint>(postWaypointFn, client, 27); + configurePostEvWaypoint>(postWaypointFn, client, 28); + configurePostEvWaypoint>(postWaypointFn, client, 29); + configurePostEvWaypoint>(postWaypointFn, client, 30); + configurePostEvWaypoint>(postWaypointFn, client, 31); + configurePostEvWaypoint>(postWaypointFn, client, 32); + configurePostEvWaypoint>(postWaypointFn, client, 33); + configurePostEvWaypoint>(postWaypointFn, client, 34); + configurePostEvWaypoint>(postWaypointFn, client, 35); + configurePostEvWaypoint>(postWaypointFn, client, 36); + configurePostEvWaypoint>(postWaypointFn, client, 37); + configurePostEvWaypoint>(postWaypointFn, client, 38); + configurePostEvWaypoint>(postWaypointFn, client, 39); + configurePostEvWaypoint>(postWaypointFn, client, 40); + configurePostEvWaypoint>(postWaypointFn, client, 41); + configurePostEvWaypoint>(postWaypointFn, client, 42); + configurePostEvWaypoint>(postWaypointFn, client, 43); + configurePostEvWaypoint>(postWaypointFn, client, 44); + configurePostEvWaypoint>(postWaypointFn, client, 45); + configurePostEvWaypoint>(postWaypointFn, client, 46); + configurePostEvWaypoint>(postWaypointFn, client, 47); + configurePostEvWaypoint>(postWaypointFn, client, 48); + configurePostEvWaypoint>(postWaypointFn, client, 49); + configurePostEvWaypoint>(postWaypointFn, client, 50); + configurePostEvWaypoint>(postWaypointFn, client, 51); + configurePostEvWaypoint>(postWaypointFn, client, 52); + configurePostEvWaypoint>(postWaypointFn, client, 53); + configurePostEvWaypoint>(postWaypointFn, client, 54); + configurePostEvWaypoint>(postWaypointFn, client, 55); + configurePostEvWaypoint>(postWaypointFn, client, 56); + configurePostEvWaypoint>(postWaypointFn, client, 57); + configurePostEvWaypoint>(postWaypointFn, client, 58); + configurePostEvWaypoint>(postWaypointFn, client, 59); + configurePostEvWaypoint>(postWaypointFn, client, 60); + configurePostEvWaypoint>(postWaypointFn, client, 61); + configurePostEvWaypoint>(postWaypointFn, client, 62); + configurePostEvWaypoint>(postWaypointFn, client, 63); + configurePostEvWaypoint>(postWaypointFn, client, 64); + configurePostEvWaypoint>(postWaypointFn, client, 65); + configurePostEvWaypoint>(postWaypointFn, client, 66); + configurePostEvWaypoint>(postWaypointFn, client, 67); + configurePostEvWaypoint>(postWaypointFn, client, 68); + configurePostEvWaypoint>(postWaypointFn, client, 69); + configurePostEvWaypoint>(postWaypointFn, client, 70); + configurePostEvWaypoint>(postWaypointFn, client, 71); + configurePostEvWaypoint>(postWaypointFn, client, 72); + configurePostEvWaypoint>(postWaypointFn, client, 73); + configurePostEvWaypoint>(postWaypointFn, client, 74); + configurePostEvWaypoint>(postWaypointFn, client, 75); + configurePostEvWaypoint>(postWaypointFn, client, 76); + configurePostEvWaypoint>(postWaypointFn, client, 77); + configurePostEvWaypoint>(postWaypointFn, client, 78); + configurePostEvWaypoint>(postWaypointFn, client, 79); + configurePostEvWaypoint>(postWaypointFn, client, 80); + configurePostEvWaypoint>(postWaypointFn, client, 81); + configurePostEvWaypoint>(postWaypointFn, client, 82); + configurePostEvWaypoint>(postWaypointFn, client, 83); + configurePostEvWaypoint>(postWaypointFn, client, 84); + configurePostEvWaypoint>(postWaypointFn, client, 85); + configurePostEvWaypoint>(postWaypointFn, client, 86); + configurePostEvWaypoint>(postWaypointFn, client, 87); + configurePostEvWaypoint>(postWaypointFn, client, 88); + configurePostEvWaypoint>(postWaypointFn, client, 89); + configurePostEvWaypoint>(postWaypointFn, client, 90); + configurePostEvWaypoint>(postWaypointFn, client, 91); + configurePostEvWaypoint>(postWaypointFn, client, 92); + configurePostEvWaypoint>(postWaypointFn, client, 93); + configurePostEvWaypoint>(postWaypointFn, client, 94); + configurePostEvWaypoint>(postWaypointFn, client, 95); + configurePostEvWaypoint>(postWaypointFn, client, 96); + configurePostEvWaypoint>(postWaypointFn, client, 97); + configurePostEvWaypoint>(postWaypointFn, client, 98); + configurePostEvWaypoint>(postWaypointFn, client, 99); + configurePostEvWaypoint>(postWaypointFn, client, 100); + configurePostEvWaypoint>(postWaypointFn, client, 101); + configurePostEvWaypoint>(postWaypointFn, client, 102); + configurePostEvWaypoint>(postWaypointFn, client, 103); + configurePostEvWaypoint>(postWaypointFn, client, 104); + configurePostEvWaypoint>(postWaypointFn, client, 105); + configurePostEvWaypoint>(postWaypointFn, client, 106); + configurePostEvWaypoint>(postWaypointFn, client, 107); + configurePostEvWaypoint>(postWaypointFn, client, 108); + configurePostEvWaypoint>(postWaypointFn, client, 109); + configurePostEvWaypoint>(postWaypointFn, client, 110); + configurePostEvWaypoint>(postWaypointFn, client, 111); + configurePostEvWaypoint>(postWaypointFn, client, 112); + configurePostEvWaypoint>(postWaypointFn, client, 113); + configurePostEvWaypoint>(postWaypointFn, client, 114); + configurePostEvWaypoint>(postWaypointFn, client, 115); + configurePostEvWaypoint>(postWaypointFn, client, 116); + configurePostEvWaypoint>(postWaypointFn, client, 117); + configurePostEvWaypoint>(postWaypointFn, client, 118); + configurePostEvWaypoint>(postWaypointFn, client, 119); + configurePostEvWaypoint>(postWaypointFn, client, 120); + configurePostEvWaypoint>(postWaypointFn, client, 121); + configurePostEvWaypoint>(postWaypointFn, client, 122); + configurePostEvWaypoint>(postWaypointFn, client, 123); + configurePostEvWaypoint>(postWaypointFn, client, 124); + configurePostEvWaypoint>(postWaypointFn, client, 125); + configurePostEvWaypoint>(postWaypointFn, client, 126); + configurePostEvWaypoint>(postWaypointFn, client, 127); + configurePostEvWaypoint>(postWaypointFn, client, 128); + configurePostEvWaypoint>(postWaypointFn, client, 129); + configurePostEvWaypoint>(postWaypointFn, client, 130); + configurePostEvWaypoint>(postWaypointFn, client, 131); + configurePostEvWaypoint>(postWaypointFn, client, 132); + configurePostEvWaypoint>(postWaypointFn, client, 133); + configurePostEvWaypoint>(postWaypointFn, client, 134); + configurePostEvWaypoint>(postWaypointFn, client, 135); + configurePostEvWaypoint>(postWaypointFn, client, 136); + configurePostEvWaypoint>(postWaypointFn, client, 137); + configurePostEvWaypoint>(postWaypointFn, client, 138); + configurePostEvWaypoint>(postWaypointFn, client, 139); + configurePostEvWaypoint>(postWaypointFn, client, 140); + configurePostEvWaypoint>(postWaypointFn, client, 141); + configurePostEvWaypoint>(postWaypointFn, client, 142); + configurePostEvWaypoint>(postWaypointFn, client, 143); + configurePostEvWaypoint>(postWaypointFn, client, 144); + configurePostEvWaypoint>(postWaypointFn, client, 145); + configurePostEvWaypoint>(postWaypointFn, client, 146); + configurePostEvWaypoint>(postWaypointFn, client, 147); + configurePostEvWaypoint>(postWaypointFn, client, 148); + configurePostEvWaypoint>(postWaypointFn, client, 149); + configurePostEvWaypoint>(postWaypointFn, client, 150); + configurePostEvWaypoint>(postWaypointFn, client, 151); + configurePostEvWaypoint>(postWaypointFn, client, 152); + configurePostEvWaypoint>(postWaypointFn, client, 153); + configurePostEvWaypoint>(postWaypointFn, client, 154); + configurePostEvWaypoint>(postWaypointFn, client, 155); + configurePostEvWaypoint>(postWaypointFn, client, 156); + configurePostEvWaypoint>(postWaypointFn, client, 157); + configurePostEvWaypoint>(postWaypointFn, client, 158); + configurePostEvWaypoint>(postWaypointFn, client, 159); + configurePostEvWaypoint>(postWaypointFn, client, 160); + configurePostEvWaypoint>(postWaypointFn, client, 161); + configurePostEvWaypoint>(postWaypointFn, client, 162); + configurePostEvWaypoint>(postWaypointFn, client, 163); + configurePostEvWaypoint>(postWaypointFn, client, 164); + configurePostEvWaypoint>(postWaypointFn, client, 165); + configurePostEvWaypoint>(postWaypointFn, client, 166); + configurePostEvWaypoint>(postWaypointFn, client, 167); + configurePostEvWaypoint>(postWaypointFn, client, 168); + configurePostEvWaypoint>(postWaypointFn, client, 169); + configurePostEvWaypoint>(postWaypointFn, client, 170); + configurePostEvWaypoint>(postWaypointFn, client, 171); + configurePostEvWaypoint>(postWaypointFn, client, 172); + configurePostEvWaypoint>(postWaypointFn, client, 173); + configurePostEvWaypoint>(postWaypointFn, client, 174); + configurePostEvWaypoint>(postWaypointFn, client, 175); + configurePostEvWaypoint>(postWaypointFn, client, 176); + configurePostEvWaypoint>(postWaypointFn, client, 177); + configurePostEvWaypoint>(postWaypointFn, client, 178); + configurePostEvWaypoint>(postWaypointFn, client, 179); + configurePostEvWaypoint>(postWaypointFn, client, 180); + configurePostEvWaypoint>(postWaypointFn, client, 181); + configurePostEvWaypoint>(postWaypointFn, client, 182); + configurePostEvWaypoint>(postWaypointFn, client, 183); + configurePostEvWaypoint>(postWaypointFn, client, 184); + configurePostEvWaypoint>(postWaypointFn, client, 185); + configurePostEvWaypoint>(postWaypointFn, client, 186); + configurePostEvWaypoint>(postWaypointFn, client, 187); + configurePostEvWaypoint>(postWaypointFn, client, 188); + configurePostEvWaypoint>(postWaypointFn, client, 189); + configurePostEvWaypoint>(postWaypointFn, client, 190); + configurePostEvWaypoint>(postWaypointFn, client, 191); + configurePostEvWaypoint>(postWaypointFn, client, 192); + configurePostEvWaypoint>(postWaypointFn, client, 193); + configurePostEvWaypoint>(postWaypointFn, client, 194); + configurePostEvWaypoint>(postWaypointFn, client, 195); + configurePostEvWaypoint>(postWaypointFn, client, 196); + configurePostEvWaypoint>(postWaypointFn, client, 197); + configurePostEvWaypoint>(postWaypointFn, client, 198); + configurePostEvWaypoint>(postWaypointFn, client, 199); + configurePostEvWaypoint>(postWaypointFn, client, 200); + configurePostEvWaypoint>(postWaypointFn, client, 201); + configurePostEvWaypoint>(postWaypointFn, client, 202); + configurePostEvWaypoint>(postWaypointFn, client, 203); + configurePostEvWaypoint>(postWaypointFn, client, 204); + configurePostEvWaypoint>(postWaypointFn, client, 205); + configurePostEvWaypoint>(postWaypointFn, client, 206); + configurePostEvWaypoint>(postWaypointFn, client, 207); + configurePostEvWaypoint>(postWaypointFn, client, 208); + configurePostEvWaypoint>(postWaypointFn, client, 209); + configurePostEvWaypoint>(postWaypointFn, client, 210); + configurePostEvWaypoint>(postWaypointFn, client, 211); + configurePostEvWaypoint>(postWaypointFn, client, 212); + configurePostEvWaypoint>(postWaypointFn, client, 213); + configurePostEvWaypoint>(postWaypointFn, client, 214); + configurePostEvWaypoint>(postWaypointFn, client, 215); + configurePostEvWaypoint>(postWaypointFn, client, 216); + configurePostEvWaypoint>(postWaypointFn, client, 217); + configurePostEvWaypoint>(postWaypointFn, client, 218); + configurePostEvWaypoint>(postWaypointFn, client, 219); + configurePostEvWaypoint>(postWaypointFn, client, 220); + configurePostEvWaypoint>(postWaypointFn, client, 221); + configurePostEvWaypoint>(postWaypointFn, client, 222); + configurePostEvWaypoint>(postWaypointFn, client, 223); + configurePostEvWaypoint>(postWaypointFn, client, 224); + configurePostEvWaypoint>(postWaypointFn, client, 225); + configurePostEvWaypoint>(postWaypointFn, client, 226); + configurePostEvWaypoint>(postWaypointFn, client, 227); + configurePostEvWaypoint>(postWaypointFn, client, 228); + configurePostEvWaypoint>(postWaypointFn, client, 229); + configurePostEvWaypoint>(postWaypointFn, client, 230); + configurePostEvWaypoint>(postWaypointFn, client, 231); + configurePostEvWaypoint>(postWaypointFn, client, 232); + configurePostEvWaypoint>(postWaypointFn, client, 233); + configurePostEvWaypoint>(postWaypointFn, client, 234); + configurePostEvWaypoint>(postWaypointFn, client, 235); + configurePostEvWaypoint>(postWaypointFn, client, 236); + configurePostEvWaypoint>(postWaypointFn, client, 237); + configurePostEvWaypoint>(postWaypointFn, client, 238); + configurePostEvWaypoint>(postWaypointFn, client, 239); + configurePostEvWaypoint>(postWaypointFn, client, 240); + configurePostEvWaypoint>(postWaypointFn, client, 241); + configurePostEvWaypoint>(postWaypointFn, client, 242); + configurePostEvWaypoint>(postWaypointFn, client, 243); + configurePostEvWaypoint>(postWaypointFn, client, 244); + configurePostEvWaypoint>(postWaypointFn, client, 245); + configurePostEvWaypoint>(postWaypointFn, client, 246); + configurePostEvWaypoint>(postWaypointFn, client, 247); + configurePostEvWaypoint>(postWaypointFn, client, 248); + configurePostEvWaypoint>(postWaypointFn, client, 249); + configurePostEvWaypoint>(postWaypointFn, client, 250); + configurePostEvWaypoint>(postWaypointFn, client, 251); + configurePostEvWaypoint>(postWaypointFn, client, 252); + configurePostEvWaypoint>(postWaypointFn, client, 253); + configurePostEvWaypoint>(postWaypointFn, client, 254); + configurePostEvWaypoint>(postWaypointFn, client, 255); + configurePostEvWaypoint>(postWaypointFn, client, 256); + // Doxygen endcond tag + /// @endcond +} + +} // namespace cl_nitrosz diff --git a/smacc2_client_library/nitrosz_client/nitrosz_client/include/nitrosz_client/components/waypoints_navigator/cp_waypoints_navigator.hpp b/smacc2_client_library/nitrosz_client/nitrosz_client/include/nitrosz_client/components/waypoints_navigator/cp_waypoints_navigator.hpp new file mode 100644 index 000000000..78d1e7bf3 --- /dev/null +++ b/smacc2_client_library/nitrosz_client/nitrosz_client/include/nitrosz_client/components/waypoints_navigator/cp_waypoints_navigator.hpp @@ -0,0 +1,85 @@ +// Copyright 2021 RobosoftAI Inc. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +/***************************************************************************************************************** + * + * Authors: Pablo Inigo Blasco, Brett Aldrich + * + ******************************************************************************************************************/ +#pragma once + +#include +#include +#include + +#include +#include + +namespace cl_nitrosz +{ +class ClNitrosZ; + +struct EvWaypointFinal : sc::event +{ +}; + +struct NavigateNextWaypointOptions +{ + std::optional controllerName_; + std::optional goalCheckerName_; +}; + +// This component contains a list of waypoints. These waypoints can +// be iterated in the different states using CbNextWaiPoint +// waypoint index is only incremented if the current waypoint is successfully reached +class CpWaypointNavigator : public CpWaypointNavigatorBase +{ +public: + ClNitrosZ * client_; + + CpWaypointNavigator(); + + void onInitialize() override; + + template + void onOrthogonalAllocation() + { + waypointsEventDispatcher.initialize(client_); + } + + std::optional > > > + sendNextGoal( + std::optional options = std::nullopt, + cl_nitrosz::ClNitrosZ::SmaccNavigateResultSignal::WeakPtr callback = + cl_nitrosz::ClNitrosZ::SmaccNavigateResultSignal::WeakPtr()); + + void stopWaitingResult(); + + smacc2::SmaccSignal onNavigationRequestSucceded; + smacc2::SmaccSignal onNavigationRequestAborted; + smacc2::SmaccSignal onNavigationRequestCancelled; + +private: + void onNavigationResult(const ClNitrosZ::WrappedResult & r); + + void onGoalReached(const ClNitrosZ::WrappedResult & res); + void onGoalCancelled(const ClNitrosZ::WrappedResult & /*res*/); + void onGoalAborted(const ClNitrosZ::WrappedResult & /*res*/); + + boost::signals2::connection succeddedNav2ZClientConnection_; + boost::signals2::connection abortedNav2ZClientConnection_; + boost::signals2::connection cancelledNav2ZClientConnection_; +}; +} // namespace cl_nitrosz diff --git a/smacc2_client_library/nitrosz_client/nitrosz_client/include/nitrosz_client/components/waypoints_navigator/cp_waypoints_navigator_base.hpp b/smacc2_client_library/nitrosz_client/nitrosz_client/include/nitrosz_client/components/waypoints_navigator/cp_waypoints_navigator_base.hpp new file mode 100644 index 000000000..0612890c6 --- /dev/null +++ b/smacc2_client_library/nitrosz_client/nitrosz_client/include/nitrosz_client/components/waypoints_navigator/cp_waypoints_navigator_base.hpp @@ -0,0 +1,102 @@ +// Copyright 2021 RobosoftAI Inc. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +/***************************************************************************************************************** + * + * Authors: Pablo Inigo Blasco, Brett Aldrich + * + ******************************************************************************************************************/ +#pragma once + +#include +#include +#include + +#include + +namespace cl_nitrosz +{ +struct Pose2D +{ + Pose2D(double x, double y, double yaw) + { + this->x_ = x; + this->y_ = y; + this->yaw_ = yaw; + } + + double x_; + double y_; + double yaw_; +}; + +// This component contains a list of waypoints. These waypoints can +// be iterated in the different states using CbNextWaiPoint +// waypoint index is only incremented if the current waypoint is successfully reached +class CpWaypointNavigatorBase : public smacc2::ISmaccComponent +{ +public: + WaypointEventDispatcher waypointsEventDispatcher; + + CpWaypointNavigatorBase(); + + virtual ~CpWaypointNavigatorBase(); + + void onInitialize() override; + + // template + // void onOrthogonalAllocation() + // { + // waypointsEventDispatcher.initialize(client_); + // } + + void loadWayPointsFromFile(std::string filepath); + + void loadWayPointsFromFile2(std::string filepath); + + void setWaypoints(const std::vector & waypoints); + + void setWaypoints(const std::vector & waypoints); + + const std::vector & getWaypoints() const; + const std::vector & getWaypointNames() const; + std::optional getNamedPose(std::string name) const; + geometry_msgs::msg::Pose getPose(int index) const; + geometry_msgs::msg::Pose getCurrentPose() const; + + long getCurrentWaypointIndex() const; + std::optional getCurrentWaypointName() const; + + long currentWaypoint_; + + void rewind(int count); + + void forward(int count); + void seekName(std::string name); + + void loadWaypointsFromYamlParameter( + std::string parameter_name, std::string yaml_file_package_name); + + void notifyGoalReached(); + +protected: + void insertWaypoint(int index, geometry_msgs::msg::Pose & newpose); + + void removeWaypoint(int index); + + std::vector waypoints_; + + std::vector waypointsNames_; +}; +} // namespace cl_nitrosz diff --git a/smacc2_client_library/nitrosz_client/nitrosz_client/include/nitrosz_client/components/waypoints_navigator/cp_waypoints_visualizer.hpp b/smacc2_client_library/nitrosz_client/nitrosz_client/include/nitrosz_client/components/waypoints_navigator/cp_waypoints_visualizer.hpp new file mode 100644 index 000000000..342f3bca6 --- /dev/null +++ b/smacc2_client_library/nitrosz_client/nitrosz_client/include/nitrosz_client/components/waypoints_navigator/cp_waypoints_visualizer.hpp @@ -0,0 +1,58 @@ +// Copyright 2021 RobosoftAI Inc. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +/***************************************************************************************************************** + * + * Authors: Pablo Inigo Blasco, Brett Aldrich + * + ******************************************************************************************************************/ +#pragma once + +#include +#include + +#include + +#include +#include + +namespace cl_nitrosz +{ +class ClNitrosZ; + +class CpWaypointsVisualizer : public smacc2::ISmaccComponent, public smacc2::ISmaccUpdatable +{ +public: + cl_nitrosz::CpWaypointNavigator * waypointsNavigator_; + + CpWaypointsVisualizer(rclcpp::Duration duration); + + void onInitialize() override; + +protected: + virtual void update() override; + +private: + std::mutex m_mutex_; + + rclcpp::Publisher::SharedPtr markersPub_; + visualization_msgs::msg::MarkerArray markers_; + visualization_msgs::msg::MarkerArray markerLabels_; + + void createMarker(const geometry_msgs::msg::Pose & waypoint, visualization_msgs::msg::Marker & m); + void createMarkerLabel( + const geometry_msgs::msg::Pose & waypoint, std::string label, + visualization_msgs::msg::Marker & m); +}; +} // namespace cl_nitrosz diff --git a/smacc2_client_library/nitrosz_client/nitrosz_client/include/nitrosz_client/nitrosz_client.hpp b/smacc2_client_library/nitrosz_client/nitrosz_client/include/nitrosz_client/nitrosz_client.hpp new file mode 100644 index 000000000..079c1c172 --- /dev/null +++ b/smacc2_client_library/nitrosz_client/nitrosz_client/include/nitrosz_client/nitrosz_client.hpp @@ -0,0 +1,44 @@ +// Copyright 2021 RobosoftAI Inc. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +/***************************************************************************************************************** + * + * Authors: Pablo Inigo Blasco, Brett Aldrich + * + ******************************************************************************************************************/ +#pragma once + +#include +#include + +#include + +namespace cl_nitrosz +{ +class ClNitrosZ +: public smacc2::client_bases::SmaccActionClientBase +{ +public: + using smacc2::client_bases::SmaccActionClientBase::GoalHandle; + using smacc2::client_bases::SmaccActionClientBase< + nav2_msgs::action::NavigateToPose>::ResultCallback; + + typedef smacc2::SmaccSignal SmaccNavigateResultSignal; + + ClNitrosZ(std::string navigateToPoseAction = "/navigate_to_pose"); + + virtual ~ClNitrosZ(); +}; + +} // namespace cl_nitrosz diff --git a/smacc2_client_library/nitrosz_client/nitrosz_client/package.xml b/smacc2_client_library/nitrosz_client/nitrosz_client/package.xml new file mode 100644 index 000000000..82fac14df --- /dev/null +++ b/smacc2_client_library/nitrosz_client/nitrosz_client/package.xml @@ -0,0 +1,37 @@ + + + + nitrosz_client + 2.3.18 + The nitrosz_client package implements SMACC Action Client Plugin for the ROS Navigation State - move_base node. Developed by Reel Robotics. + + Pablo Inigo Blasco + Pablo Inigo Blasco + + BSDv3 + + ament_cmake + + smacc2 + slam_toolbox + + angles + yaml_cpp_vendor + nav2_msgs + pluginlib + rclcpp_action + + nav2_util + + std_msgs + std_srvs + tf2_ros + tf2_geometry_msgs + bond + + ament_index_cpp + + + ament_cmake + + diff --git a/smacc2_client_library/nitrosz_client/nitrosz_client/scripts/lidar_completion.py b/smacc2_client_library/nitrosz_client/nitrosz_client/scripts/lidar_completion.py new file mode 100755 index 000000000..cdde85763 --- /dev/null +++ b/smacc2_client_library/nitrosz_client/nitrosz_client/scripts/lidar_completion.py @@ -0,0 +1,105 @@ +# Copyright 2024 RobosoftAI Inc. +# +# Licensed under the Apache License, Version 2.0 (the "License"); +# you may not use this file except in compliance with the License. +# You may obtain a copy of the License at +# +# http://www.apache.org/licenses/LICENSE-2.0 +# +# Unless required by applicable law or agreed to in writing, software +# distributed under the License is distributed on an "AS IS" BASIS, +# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +# See the License for the specific language governing permissions and +# limitations under the License. + +#!/usr/bin/env python3 +import rclpy +from rclpy.node import Node +from sensor_msgs.msg import LaserScan +from rclpy.qos import QoSProfile, ReliabilityPolicy, HistoryPolicy +import logging +import copy + + +class LidarProcessor(Node): + def __init__(self): + super().__init__("lidar_processor") + + qos_prof_1 = QoSProfile( + reliability=ReliabilityPolicy.BEST_EFFORT, history=HistoryPolicy.KEEP_LAST, depth=10 + ) + + self.subscription = self.create_subscription( + LaserScan, "/scan_input", self.lidar_callback, qos_profile=qos_prof_1 + ) + + qos_prof_2 = QoSProfile( + reliability=ReliabilityPolicy.RELIABLE, history=HistoryPolicy.KEEP_LAST, depth=10 + ) + + self.msg_buffer = [] + self.publisher = self.create_publisher(LaserScan, "/scan_output", qos_profile=qos_prof_2) + + self.logger = self.get_logger() + self.logger.set_level(logging.INFO) + + def lidar_callback(self, msg): + # self.logger.info("Received message") + self.msg_buffer.append(msg) + if len(self.msg_buffer) > 5: + # self.logger.info("Merging messages") + merged_msg = self.merge_lidar_msgs() + # self.msg_buffer.pop(0) + self.msg_buffer = [] + self.publisher.publish(merged_msg) + + def merge_lidar_msgs(self): + merged_ranges = [0.0] * len(self.msg_buffer[0].ranges) * len(self.msg_buffer) + # cont_inf = 0 + cont_tot = 0 + for i in range(len(self.msg_buffer)): + for j in range(len(self.msg_buffer[i].ranges)): + # if self.msg_buffer[i].ranges[j] != float("inf"): + merged_ranges[cont_tot] = self.msg_buffer[i].ranges[j] + cont_tot += 1 + # else: + # cont_inf += 1 + + # self.logger.info(f"Number of infinite values from total: {cont_inf}/{cont_tot}") + + merged_msg = LaserScan() + # merged_msg = copy.deepcopy(self.msg_buffer[-1]) + merged_msg.header = self.msg_buffer[-1].header + + merged_msg.time_increment = self.msg_buffer[0].time_increment + + merged_msg.angle_increment = self.msg_buffer[0].angle_increment + merged_msg.angle_min = self.msg_buffer[0].angle_min + merged_msg.angle_max = ( + self.msg_buffer[0].angle_min + (len(merged_ranges) - 1) * merged_msg.angle_increment + ) + + # print(f"first angle_min: {self.msg_buffer[0].angle_min}") + # print(f"angle_min: {merged_msg.angle_min}") + # print(f"angle_max: {merged_msg.angle_max}") + + # merged_msg.angle_min = self.msg_buffer[0].angle_min - self.msg_buffer[0].angle_increment*len(self.msg_buffer) + # merged_msg.angle_max = merged_msg.angle_min + (len(merged_ranges)-1)*merged_msg.angle_increment + + merged_msg.range_min = min(msg.range_min for msg in self.msg_buffer) + merged_msg.range_max = max(msg.range_max for msg in self.msg_buffer) + merged_msg.ranges = merged_ranges + + return merged_msg + + +def main(args=None): + rclpy.init(args=args) + lidar_processor = LidarProcessor() + rclpy.spin(lidar_processor) + lidar_processor.destroy_node() + rclpy.shutdown() + + +if __name__ == "__main__": + main() diff --git a/smacc2_client_library/nitrosz_client/nitrosz_client/src/nitrosz_client/client_behaviors/cb_abort_navigation.cpp b/smacc2_client_library/nitrosz_client/nitrosz_client/src/nitrosz_client/client_behaviors/cb_abort_navigation.cpp new file mode 100644 index 000000000..8774450f1 --- /dev/null +++ b/smacc2_client_library/nitrosz_client/nitrosz_client/src/nitrosz_client/client_behaviors/cb_abort_navigation.cpp @@ -0,0 +1,44 @@ +// Copyright 2021 RobosoftAI Inc. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +/***************************************************************************************************************** + * + * Authors: Pablo Inigo Blasco, Brett Aldrich + * + ******************************************************************************************************************/ + +#include +#include +#include +#include +#include +#include + +#include + +namespace cl_nitrosz +{ +CbAbortNavigation::CbAbortNavigation() {} + +void CbAbortNavigation::onEntry() +{ + // this->sendGoal(goal); + + this->nitroszClient_->cancelGoal(); + this->postSuccessEvent(); +} + +void CbAbortNavigation::onExit() {} + +} // namespace cl_nitrosz diff --git a/smacc2_client_library/nitrosz_client/nitrosz_client/src/nitrosz_client/client_behaviors/cb_absolute_rotate.cpp b/smacc2_client_library/nitrosz_client/nitrosz_client/src/nitrosz_client/client_behaviors/cb_absolute_rotate.cpp new file mode 100644 index 000000000..61739fca2 --- /dev/null +++ b/smacc2_client_library/nitrosz_client/nitrosz_client/src/nitrosz_client/client_behaviors/cb_absolute_rotate.cpp @@ -0,0 +1,270 @@ +// Copyright 2021 RobosoftAI Inc. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +/***************************************************************************************************************** + * + * Authors: Pablo Inigo Blasco, Brett Aldrich + * + ******************************************************************************************************************/ + +#include +#include +#include +#include +#include +#include + +#include + +namespace cl_nitrosz +{ +CbAbsoluteRotate::CbAbsoluteRotate() {} +CbAbsoluteRotate::CbAbsoluteRotate(float absoluteGoalAngleDegree, float yaw_goal_tolerance) +{ + this->absoluteGoalAngleDegree = absoluteGoalAngleDegree; + + if (yaw_goal_tolerance >= 0) + { + this->yawGoalTolerance = yaw_goal_tolerance; + } +} + +void CbAbsoluteRotate::onEntry() +{ + listener = std::make_shared(getNode()->get_clock()); + double goal_angle = this->absoluteGoalAngleDegree; + + RCLCPP_INFO_STREAM(getLogger(), "[CbAbsoluteRotate] Absolute yaw Angle:" << goal_angle); + + auto plannerSwitcher = this->nitroszClient_->getComponent(); + // this should work better with a coroutine and await + // this->plannerSwitcher_->setForwardPlanner(); + + if (spinningPlanner && *spinningPlanner == SpinningPlanner::PureSpinning) + { + plannerSwitcher->setPureSpinningPlanner(); + } + else + { + plannerSwitcher->setDefaultPlanners(); + } + + updateTemporalBehaviorParameters(false); + + auto p = nitroszClient_->getComponent(); + auto referenceFrame = p->getReferenceFrame(); + auto currentPoseMsg = p->toPoseMsg(); + + ClNitrosZ::Goal goal; + goal.pose.header.frame_id = referenceFrame; + //goal.pose.header.stamp = getNode()->now(); + + auto targetAngle = goal_angle * M_PI / 180.0; + goal.pose.pose.position = currentPoseMsg.position; + tf2::Quaternion q; + q.setRPY(0, 0, targetAngle); + goal.pose.pose.orientation = tf2::toMsg(q); + + auto odomTracker_ = nitroszClient_->getComponent(); + if (odomTracker_ != nullptr) + { + auto pathname = this->getCurrentState()->getName() + " - " + getName(); + odomTracker_->pushPath(pathname); + odomTracker_->setStartPoint(p->toPoseStampedMsg()); + odomTracker_->setCurrentMotionGoal(goal.pose); + odomTracker_->setWorkingMode(odom_tracker::WorkingMode::RECORD_PATH); + } + + auto goalCheckerSwitcher = nitroszClient_->getComponent(); + goalCheckerSwitcher->setGoalCheckerId("absolute_rotate_goal_checker"); + + RCLCPP_INFO_STREAM( + getLogger(), + "[" << getName() << "] current pose yaw: " << tf2::getYaw(currentPoseMsg.orientation)); + RCLCPP_INFO_STREAM( + getLogger(), + "[" << getName() << "] goal pose yaw: " << tf2::getYaw(goal.pose.pose.orientation)); + this->sendGoal(goal); +} + +void CbAbsoluteRotate::updateTemporalBehaviorParameters(bool undo) +{ + auto log = this->getLogger(); + + std::string nodename = "/controller_server"; + + auto parameters_client = + std::make_shared(this->getNode(), nodename); + + RCLCPP_INFO_STREAM( + log, "[" << getName() << "] using a parameter client to update some controller parameters: " + << nodename << ". Waiting service."); + parameters_client->wait_for_service(); + + RCLCPP_INFO_STREAM(log, "[" << getName() << "] Service found: " << nodename << "."); + + std::string localPlannerName; + std::vector parameters; + + // dynamic_reconfigure::DoubleParameter yaw_goal_tolerance; + rclcpp::Parameter yaw_goal_tolerance("goal_checker.yaw_goal_tolerance"); + + rclcpp::Parameter max_vel_theta, min_vel_theta; + + bool isRosBasePlanner = !spinningPlanner || *spinningPlanner == SpinningPlanner::Default; + bool isPureSpinningPlanner = spinningPlanner && *spinningPlanner == SpinningPlanner::PureSpinning; + + // SELECTING CONTROLLER AND PARAMETERS NAME + if (isPureSpinningPlanner) + { + localPlannerName = "PureSpinningLocalPlanner"; + max_vel_theta = rclcpp::Parameter(localPlannerName + ".max_angular_z_speed"); + min_vel_theta = rclcpp::Parameter(localPlannerName + ".min_vel_theta"); + } + else if (isRosBasePlanner) + { + localPlannerName = "FollowPath"; + max_vel_theta = rclcpp::Parameter(localPlannerName + ".max_vel_theta"); + min_vel_theta = rclcpp::Parameter(localPlannerName + ".min_vel_theta"); + } + + if (!undo) + { + if (yawGoalTolerance) + { + // save old yaw tolerance + auto fut = parameters_client->get_parameters( + {localPlannerName + ".yaw_goal_tolerance"}, + [&](auto futureParameters) + { + auto params = futureParameters.get(); + oldYawTolerance = params[0].as_double(); + }); + + // make synchronous + fut.get(); + + yaw_goal_tolerance = rclcpp::Parameter("goal_checker.yaw_goal_tolerance", *yawGoalTolerance); + parameters.push_back(yaw_goal_tolerance); + RCLCPP_INFO_STREAM( + getLogger(), "[" << getName() + << "] updating yaw tolerance local planner to: " << *yawGoalTolerance + << ", from previous value: " << this->oldYawTolerance); + } + + if (maxVelTheta) + { + if (isRosBasePlanner) + { + // nh.getParam(nodename + "/" + localPlannerName+"/min_vel_theta", oldMinVelTheta); + // getCurrentState()->getParam(nodename + "/" + localPlannerName + "/max_vel_theta", oldMaxVelTheta); + + // save old yaw tolerance + auto fut = parameters_client->get_parameters( + {localPlannerName + ".max_vel_theta"}, + [&](auto futureParameters) + { + auto params = futureParameters.get(); + oldMaxVelTheta = params[0].as_double(); + }); + + // make synchronous + fut.get(); + } + + max_vel_theta = rclcpp::Parameter(localPlannerName + ".max_vel_theta", *maxVelTheta); + min_vel_theta = rclcpp::Parameter(localPlannerName + ".min_vel_theta", -*maxVelTheta); + parameters.push_back(max_vel_theta); + parameters.push_back(min_vel_theta); + + RCLCPP_INFO_STREAM( + log, "[" << getName() << "] updating max vel theta local planner to: " << *maxVelTheta + << ", from previous value: " << this->oldMaxVelTheta); + RCLCPP_INFO_STREAM( + log, "[" << getName() << "] updating min vel theta local planner to: " << -*maxVelTheta + << ", from previous value: " << this->oldMinVelTheta); + } + } + else + { + if (yawGoalTolerance) + { + yaw_goal_tolerance = rclcpp::Parameter("goal_checker.yaw_goal_tolerance", oldYawTolerance); + RCLCPP_INFO_STREAM( + log, "[" << getName() << "] restoring yaw tolerance local planner from: " + << *yawGoalTolerance << " , to previous value: " << this->oldYawTolerance); + } + + if (maxVelTheta) + { + if (isRosBasePlanner) + { + max_vel_theta = rclcpp::Parameter(localPlannerName + ".max_vel_theta", oldMaxVelTheta); + min_vel_theta = rclcpp::Parameter(localPlannerName + ".max_vel_theta", oldMinVelTheta); + } + + parameters.push_back(max_vel_theta); + parameters.push_back(min_vel_theta); + RCLCPP_INFO_STREAM( + log, "[" << getName() << "] restoring max vel theta local planner from: " << *maxVelTheta + << ", to previous value: " << this->oldMaxVelTheta); + RCLCPP_INFO_STREAM( + log, "[" << getName() << "] restoring min vel theta local planner from: " << -(*maxVelTheta) + << ", to previous value: " << this->oldMinVelTheta); + } + } + + if (parameters.size()) + { + std::stringstream ss; + ss << "Executing asynchronous request. Parameters to update: " << std::endl; + for (auto & p : parameters) + { + ss << p.get_name() << " - " << p.value_to_string() << std::endl; + } + + RCLCPP_INFO_STREAM(getLogger(), "[CbAbsoluteRotate] " << ss.str()); + + auto futureResults = parameters_client->set_parameters(parameters); + + int i = 0; + for (auto & res : futureResults.get()) + { + RCLCPP_INFO_STREAM( + getLogger(), "[" << getName() << "] parameter result: " << parameters[i].get_name() << "=" + << parameters[i].as_string() << ". Result: " << res.successful); + i++; + } + + RCLCPP_INFO_STREAM(log, "[" << getName() << "] parameters updated"); + } + else + { + RCLCPP_INFO_STREAM(log, "[" << getName() << "] skipping parameters update"); + } +} + +void CbAbsoluteRotate::onExit() +{ + if (spinningPlanner && *spinningPlanner == SpinningPlanner::PureSpinning) + { + } + else + { + } + + this->updateTemporalBehaviorParameters(true); +} + +} // namespace cl_nitrosz diff --git a/smacc2_client_library/nitrosz_client/nitrosz_client/src/nitrosz_client/client_behaviors/cb_active_stop.cpp b/smacc2_client_library/nitrosz_client/nitrosz_client/src/nitrosz_client/client_behaviors/cb_active_stop.cpp new file mode 100644 index 000000000..61a650f8c --- /dev/null +++ b/smacc2_client_library/nitrosz_client/nitrosz_client/src/nitrosz_client/client_behaviors/cb_active_stop.cpp @@ -0,0 +1,53 @@ +// Copyright 2021 RobosoftAI Inc. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +/***************************************************************************************************************** + * + * Authors: Pablo Inigo Blasco, Brett Aldrich + * + ******************************************************************************************************************/ + +#include +#include +#include + +#include + +namespace cl_nitrosz +{ +CbActiveStop::CbActiveStop() {} + +void CbActiveStop::onEntry() +{ + auto nh = this->getNode(); + cmd_vel_pub_ = nh->create_publisher("/cmd_vel", rclcpp::QoS(1)); + + rclcpp::Rate loop_rate(5); + geometry_msgs::msg::Twist cmd_vel_msg; + while (!this->isShutdownRequested()) + { + cmd_vel_msg.linear.x = 0; + cmd_vel_msg.angular.z = 0; + + cmd_vel_pub_->publish(cmd_vel_msg); + loop_rate.sleep(); + } + RCLCPP_INFO_STREAM(getLogger(), "[" << getName() << "] Finished behavior execution"); + + this->postSuccessEvent(); +} + +void CbActiveStop::onExit() {} + +} // namespace cl_nitrosz diff --git a/smacc2_client_library/nitrosz_client/nitrosz_client/src/nitrosz_client/client_behaviors/cb_load_waypoints_file.cpp b/smacc2_client_library/nitrosz_client/nitrosz_client/src/nitrosz_client/client_behaviors/cb_load_waypoints_file.cpp new file mode 100644 index 000000000..67be0095f --- /dev/null +++ b/smacc2_client_library/nitrosz_client/nitrosz_client/src/nitrosz_client/client_behaviors/cb_load_waypoints_file.cpp @@ -0,0 +1,57 @@ +// Copyright 2021 RobosoftAI Inc. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +/***************************************************************************************************************** + * + * Authors: Pablo Inigo Blasco, Brett Aldrich + * + ******************************************************************************************************************/ + +#include +#include +#include + +namespace cl_nitrosz +{ + +CbLoadWaypointsFile::CbLoadWaypointsFile(std::string filepath) : filepath_(filepath) {} + +CbLoadWaypointsFile::CbLoadWaypointsFile(std::string parameter_name, std::string packagenamespace) +: parameterName_(parameter_name), packageNamespace_(packagenamespace) +{ +} + +void CbLoadWaypointsFile::onEntry() +{ + requiresComponent(waypointsNavigator_); // this is a component from the + // nitrosz_client library + + if (filepath_) + { + this->waypointsNavigator_->loadWayPointsFromFile(filepath_.value()); + } + else + { + RCLCPP_INFO(getLogger(), "Loading waypoints from parameter %s", parameterName_.value().c_str()); + this->waypointsNavigator_->loadWaypointsFromYamlParameter( + parameterName_.value(), packageNamespace_.value()); + } + + // change this to skip some points of the yaml file, default = 0 + waypointsNavigator_->currentWaypoint_ = 0; + this->postSuccessEvent(); +} + +void CbLoadWaypointsFile::onExit() {} +} // namespace cl_nitrosz diff --git a/smacc2_client_library/nitrosz_client/nitrosz_client/src/nitrosz_client/client_behaviors/cb_navigate_backward.cpp b/smacc2_client_library/nitrosz_client/nitrosz_client/src/nitrosz_client/client_behaviors/cb_navigate_backward.cpp new file mode 100644 index 000000000..7198e56e5 --- /dev/null +++ b/smacc2_client_library/nitrosz_client/nitrosz_client/src/nitrosz_client/client_behaviors/cb_navigate_backward.cpp @@ -0,0 +1,100 @@ +// Copyright 2021 RobosoftAI Inc. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +/***************************************************************************************************************** + * + * Authors: Pablo Inigo Blasco, Brett Aldrich + * + ******************************************************************************************************************/ + +#include +#include +#include +#include +#include +#include +#include +#include + +namespace cl_nitrosz +{ +using namespace ::cl_nitrosz::odom_tracker; + +CbNavigateBackwards::CbNavigateBackwards(float backwardDistance) +{ + if (backwardDistance < 0) + { + RCLCPP_ERROR(getLogger(), "[CbNavigateBackwards] distance must be greater or equal than 0"); + this->backwardDistance = 0; + } + this->backwardDistance = backwardDistance; +} + +void CbNavigateBackwards::onEntry() +{ + // straight motion distance + double dist = backwardDistance; + + RCLCPP_INFO_STREAM( + getLogger(), "[CbNavigateBackwards] Straight backwards motion distance: " << dist); + + auto p = nitroszClient_->getComponent(); + auto referenceFrame = p->getReferenceFrame(); + auto currentPoseMsg = p->toPoseMsg(); + tf2::Transform currentPose; + tf2::fromMsg(currentPoseMsg, currentPose); + + tf2::Transform backwardDeltaTransform; + backwardDeltaTransform.setIdentity(); + backwardDeltaTransform.setOrigin(tf2::Vector3(-dist, 0, 0)); + + tf2::Transform targetPose = currentPose * backwardDeltaTransform; + + ClNitrosZ::Goal goal; + goal.pose.header.frame_id = referenceFrame; + //goal.pose.header.stamp = getNode()->now(); + tf2::toMsg(targetPose, goal.pose.pose); + RCLCPP_INFO_STREAM(getLogger(), "[CbNavigateBackwards] TARGET POSE BACKWARDS: " << goal.pose); + + geometry_msgs::msg::PoseStamped currentStampedPoseMsg; + currentStampedPoseMsg.header.frame_id = referenceFrame; + currentStampedPoseMsg.header.stamp = getNode()->now(); + tf2::toMsg(currentPose, currentStampedPoseMsg.pose); + + odomTracker_ = nitroszClient_->getComponent(); + if (odomTracker_ != nullptr) + { + this->odomTracker_->clearPath(); + this->odomTracker_->setStartPoint(currentStampedPoseMsg); + this->odomTracker_->setWorkingMode(WorkingMode::RECORD_PATH); + } + + auto plannerSwitcher = nitroszClient_->getComponent(); + plannerSwitcher->setBackwardPlanner(); + + auto goalCheckerSwitcher = nitroszClient_->getComponent(); + goalCheckerSwitcher->setGoalCheckerId("backward_goal_checker"); + + this->sendGoal(goal); +} + +void CbNavigateBackwards::onExit() +{ + if (odomTracker_) + { + this->odomTracker_->setWorkingMode(WorkingMode::IDLE); + } +} + +} // namespace cl_nitrosz diff --git a/smacc2_client_library/nitrosz_client/nitrosz_client/src/nitrosz_client/client_behaviors/cb_navigate_forward.cpp b/smacc2_client_library/nitrosz_client/nitrosz_client/src/nitrosz_client/client_behaviors/cb_navigate_forward.cpp new file mode 100644 index 000000000..b6cd1f409 --- /dev/null +++ b/smacc2_client_library/nitrosz_client/nitrosz_client/src/nitrosz_client/client_behaviors/cb_navigate_forward.cpp @@ -0,0 +1,158 @@ +// Copyright 2021 RobosoftAI Inc. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +/***************************************************************************************************************** + * + * Authors: Pablo Inigo Blasco, Brett Aldrich + * + ******************************************************************************************************************/ + +#include + +#include +#include +#include +#include + +namespace cl_nitrosz +{ +using ::cl_nitrosz::odom_tracker::CpOdomTracker; +using ::cl_nitrosz::odom_tracker::WorkingMode; + +using ::cl_nitrosz::Pose; + +CbNavigateForward::CbNavigateForward(float distance_meters) : forwardDistance_(distance_meters) {} +CbNavigateForward::CbNavigateForward() {} + +CbNavigateForward::CbNavigateForward(geometry_msgs::msg::PoseStamped goal) : goalPose_(goal) {} + +CbNavigateForward::~CbNavigateForward() {} + +void CbNavigateForward::setForwardDistance(float distance_meters) +{ + if (distance_meters < 0) + { + RCLCPP_INFO_STREAM( + getLogger(), "[" << getName() << "] negative forward distance: " << distance_meters + << ". Resetting to 0."); + distance_meters = 0; + } + forwardDistance_ = distance_meters; + + RCLCPP_INFO_STREAM( + getLogger(), "[" << getName() << "] setting fw motion distance: " << *forwardDistance_); +} + +void CbNavigateForward::onEntry() +{ + if (forwardDistance_) + { + setForwardDistance(*forwardDistance_); + + RCLCPP_INFO_STREAM( + getLogger(), "[" << getName() << "] Straight motion distance: " << *forwardDistance_); + } + + // get current pose + auto p = nitroszClient_->getComponent(); + auto referenceFrame = p->getReferenceFrame(); + auto currentPoseMsg = p->toPoseMsg(); + + RCLCPP_INFO_STREAM( + getLogger(), "[" << getName() << "]" + << "current pose: " << currentPoseMsg); + + // force global orientation if it is requested + if (options.forceInitialOrientation) + { + currentPoseMsg.orientation = *(options.forceInitialOrientation); + RCLCPP_WARN_STREAM( + getLogger(), + "[" << getName() << "]" + << "Forcing initial straight motion orientation: " << currentPoseMsg.orientation); + } + + tf2::Transform currentPose; + tf2::fromMsg(currentPoseMsg, currentPose); + + tf2::Transform targetPose; + if (goalPose_) + { + tf2::fromMsg(goalPose_->pose, targetPose); + } + else if (forwardDistance_) + { + // compute forward goal pose + tf2::Transform forwardDeltaTransform; + forwardDeltaTransform.setIdentity(); + forwardDeltaTransform.setOrigin(tf2::Vector3(*forwardDistance_, 0, 0)); + + targetPose = currentPose * forwardDeltaTransform; + } + else + { + RCLCPP_WARN_STREAM( + getLogger(), + "[" << getName() << "]" + << "No goal Pose or Distance is specified. Aborting. No action request is sent." + << currentPoseMsg.orientation); + + return; + } + + // action goal + ClNitrosZ::Goal goal; + goal.pose.header.frame_id = referenceFrame; + //goal.pose.header.stamp = getNode()->now(); + tf2::toMsg(targetPose, goal.pose.pose); + RCLCPP_INFO_STREAM( + getLogger(), "[" << getName() << "]" + << " TARGET POSE FORWARD: " << goal.pose.pose); + + // current pose + geometry_msgs::msg::PoseStamped currentStampedPoseMsg; + currentStampedPoseMsg.header.frame_id = referenceFrame; + currentStampedPoseMsg.header.stamp = + getNode()->now(); // probably it is better avoid setting that goal timestamp + + tf2::toMsg(currentPose, currentStampedPoseMsg.pose); + + odomTracker_ = nitroszClient_->getComponent(); + if (odomTracker_ != nullptr) + { + auto pathname = this->getCurrentState()->getName() + " - " + getName(); + odomTracker_->pushPath(pathname); + odomTracker_->setStartPoint(currentStampedPoseMsg); + odomTracker_->setCurrentMotionGoal(goal.pose); + odomTracker_->setWorkingMode(WorkingMode::RECORD_PATH); + } + + auto plannerSwitcher = nitroszClient_->getComponent(); + plannerSwitcher->setForwardPlanner(); + + auto goalCheckerSwitcher = nitroszClient_->getComponent(); + goalCheckerSwitcher->setGoalCheckerId("forward_goal_checker"); + + this->sendGoal(goal); +} + +void CbNavigateForward::onExit() +{ + if (odomTracker_) + { + this->odomTracker_->setWorkingMode(WorkingMode::IDLE); + } +} + +} // namespace cl_nitrosz diff --git a/smacc2_client_library/nitrosz_client/nitrosz_client/src/nitrosz_client/client_behaviors/cb_navigate_global_position.cpp b/smacc2_client_library/nitrosz_client/nitrosz_client/src/nitrosz_client/client_behaviors/cb_navigate_global_position.cpp new file mode 100644 index 000000000..69b0f21c2 --- /dev/null +++ b/smacc2_client_library/nitrosz_client/nitrosz_client/src/nitrosz_client/client_behaviors/cb_navigate_global_position.cpp @@ -0,0 +1,109 @@ +// Copyright 2021 RobosoftAI Inc. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +/***************************************************************************************************************** + * + * Authors: Pablo Inigo Blasco, Brett Aldrich + * + ******************************************************************************************************************/ +#include + +#include +#include +#include +#include +#include + +namespace cl_nitrosz +{ +using namespace ::cl_nitrosz::odom_tracker; + +CbNavigateGlobalPosition::CbNavigateGlobalPosition() {} + +CbNavigateGlobalPosition::CbNavigateGlobalPosition( + float x, float y, float yaw, std::optional options) +{ + auto p = geometry_msgs::msg::Point(); + p.x = x; + p.y = y; + goalPosition = p; + goalYaw = yaw; + + if (options) this->options = *options; +} + +void CbNavigateGlobalPosition::setGoal(const geometry_msgs::msg::Pose & pose) +{ + goalPosition = pose.position; + goalYaw = tf2::getYaw(pose.orientation); +} + +void CbNavigateGlobalPosition::onEntry() +{ + RCLCPP_INFO(getLogger(), "Entering Navigate Global position"); + RCLCPP_INFO(getLogger(), "Component requirements completed"); + + auto pose = nitroszClient_->getComponent()->toPoseMsg(); + auto * odomTracker = nitroszClient_->getComponent(); + + auto plannerSwitcher = nitroszClient_->getComponent(); + + plannerSwitcher->setDefaultPlanners(false); + + if (options.controllerName_) + { + plannerSwitcher->setDesiredController(*(options.controllerName_)); + } + + plannerSwitcher->commitPublish(); + + auto goalCheckerSwitcher = nitroszClient_->getComponent(); + goalCheckerSwitcher->setGoalCheckerId("goal_checker"); + + auto pathname = this->getCurrentState()->getName() + " - " + getName(); + odomTracker->pushPath(pathname); + odomTracker->setStartPoint(pose); + odomTracker->setWorkingMode(WorkingMode::RECORD_PATH); + + execute(); +} + +// auxiliary function that defines the motion that is requested to the nav2 action server +void CbNavigateGlobalPosition::execute() +{ + auto p = nitroszClient_->getComponent(); + auto referenceFrame = p->getReferenceFrame(); + // auto currentPoseMsg = p->toPoseMsg(); + + RCLCPP_INFO(getLogger(), "Sending Goal to MoveBase"); + ClNitrosZ::Goal goal; + goal.pose.header.frame_id = referenceFrame; + //goal.pose.header.stamp = getNode()->now(); + + goal.pose.pose.position = goalPosition; + tf2::Quaternion q; + q.setRPY(0, 0, goalYaw); + goal.pose.pose.orientation = tf2::toMsg(q); + + this->sendGoal(goal); +} + +// This is the substate destructor. This code will be executed when the +// workflow exits from this substate (that is according to statechart the moment when this object is destroyed) +void CbNavigateGlobalPosition::onExit() +{ + RCLCPP_INFO(getLogger(), "Exiting move goal Action Client"); +} + +} // namespace cl_nitrosz diff --git a/smacc2_client_library/nitrosz_client/nitrosz_client/src/nitrosz_client/client_behaviors/cb_navigate_named_waypoint.cpp b/smacc2_client_library/nitrosz_client/nitrosz_client/src/nitrosz_client/client_behaviors/cb_navigate_named_waypoint.cpp new file mode 100644 index 000000000..44457241e --- /dev/null +++ b/smacc2_client_library/nitrosz_client/nitrosz_client/src/nitrosz_client/client_behaviors/cb_navigate_named_waypoint.cpp @@ -0,0 +1,59 @@ +// Copyright 2021 RobosoftAI Inc. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +/***************************************************************************************************************** + * + * Authors: Pablo Inigo Blasco, Brett Aldrich + * + ******************************************************************************************************************/ + +#include + +namespace cl_nitrosz +{ +CbNavigateNamedWaypoint::CbNavigateNamedWaypoint( + std::string waypointname, std::optional options) +: CbNavigateNextWaypoint(options) +{ + waypointname_ = waypointname; +} + +CbNavigateNamedWaypoint::~CbNavigateNamedWaypoint() {} + +void CbNavigateNamedWaypoint::onEntry() +{ + // waypointsNavigator_ = nitroszClient_->getComponent(); + // waypointsNavigator_->sendNextGoal(options_); + + // auto waypointname = waypointsNavigator_->getCurrentWaypointName(); + + // if(waypointname) + // { + // RCLCPP_INFO( + // getLogger(), "[CbNavigateNamedWaypoint] current iteration waypoints i: %ld with name '%s'", + // waypointsNavigator_->getCurrentWaypointIndex(), waypointname->c_str()); } + // else + // { + // RCLCPP_INFO( + // getLogger(), "[CbNavigateNamedWaypoint] current iteration waypoints i: %ld", + // waypointsNavigator_->getCurrentWaypointIndex()); } + + auto waypointsNavigator_ = nitroszClient_->getComponent(); + waypointsNavigator_->seekName(waypointname_); + CbNavigateNextWaypoint::onEntry(); +} + +void CbNavigateNamedWaypoint::onExit() { CbNavigateNextWaypoint::onExit(); } + +} // namespace cl_nitrosz diff --git a/smacc2_client_library/nitrosz_client/nitrosz_client/src/nitrosz_client/client_behaviors/cb_navigate_next_waypoint.cpp b/smacc2_client_library/nitrosz_client/nitrosz_client/src/nitrosz_client/client_behaviors/cb_navigate_next_waypoint.cpp new file mode 100644 index 000000000..be89559fb --- /dev/null +++ b/smacc2_client_library/nitrosz_client/nitrosz_client/src/nitrosz_client/client_behaviors/cb_navigate_next_waypoint.cpp @@ -0,0 +1,61 @@ +// Copyright 2021 RobosoftAI Inc. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +/***************************************************************************************************************** + * + * Authors: Pablo Inigo Blasco, Brett Aldrich + * + ******************************************************************************************************************/ + +#include + +namespace cl_nitrosz +{ +CbNavigateNextWaypoint::CbNavigateNextWaypoint(std::optional options) +{ + if (options) options_ = *options; +} + +CbNavigateNextWaypoint::~CbNavigateNextWaypoint() {} + +void CbNavigateNextWaypoint::onEntry() +{ + waypointsNavigator_ = nitroszClient_->getComponent(); + + this->navigationCallback_ = std::make_shared(); + + this->getStateMachine()->createSignalConnection( + *navigationCallback_, &CbNavigateNextWaypoint::onNavigationResult, this); + + auto goalHandle = waypointsNavigator_->sendNextGoal(options_, navigationCallback_); + + auto waypointname = waypointsNavigator_->getCurrentWaypointName(); + + if (waypointname) + { + RCLCPP_INFO( + getLogger(), "[CbNavigateNextWaypoint] current iteration waypoints i: %ld with name '%s'", + waypointsNavigator_->getCurrentWaypointIndex(), waypointname->c_str()); + } + else + { + RCLCPP_INFO( + getLogger(), "[CbNavigateNextWaypoint] current iteration waypoints i: %ld", + waypointsNavigator_->getCurrentWaypointIndex()); + } +} + +void CbNavigateNextWaypoint::onExit() { waypointsNavigator_->stopWaitingResult(); } + +} // namespace cl_nitrosz diff --git a/smacc2_client_library/nitrosz_client/nitrosz_client/src/nitrosz_client/client_behaviors/cb_navigate_next_waypoint_free.cpp b/smacc2_client_library/nitrosz_client/nitrosz_client/src/nitrosz_client/client_behaviors/cb_navigate_next_waypoint_free.cpp new file mode 100644 index 000000000..27915b30f --- /dev/null +++ b/smacc2_client_library/nitrosz_client/nitrosz_client/src/nitrosz_client/client_behaviors/cb_navigate_next_waypoint_free.cpp @@ -0,0 +1,54 @@ +// Copyright 2021 RobosoftAI Inc. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +/***************************************************************************************************************** + * + * Authors: Pablo Inigo Blasco, Brett Aldrich + * + ******************************************************************************************************************/ +#include + +namespace cl_nitrosz +{ + +CbNavigateNextWaypointFree::CbNavigateNextWaypointFree() {} + +CbNavigateNextWaypointFree::~CbNavigateNextWaypointFree() {} + +void CbNavigateNextWaypointFree::onEntry() +{ + requiresComponent(this->waypointsNavigator_); + this->target_pose_ = this->waypointsNavigator_->getCurrentPose(); + + this->onSuccess(&CbNavigateNextWaypointFree::CbNavigateNextWaypointFree::onSucessCallback, this); + RCLCPP_INFO_STREAM( + getLogger(), "[CbNavigateNextWaypoint] initial load file target pose: x: " + << this->target_pose_.position.x << ", y: " << this->target_pose_.position.y); + CbPositionControlFreeSpace::onEntry(); +} + +void CbNavigateNextWaypointFree::onSucessCallback() +{ + RCLCPP_INFO_STREAM(getLogger(), "[CbNavigateNextWaypoint] Success on planning to next waypoint"); + this->waypointsNavigator_->notifyGoalReached(); + this->waypointsNavigator_->forward(1); + RCLCPP_INFO_STREAM( + getLogger(), "[CbNavigateNextWaypoint] next position index: " + << this->waypointsNavigator_->getCurrentWaypointIndex() << "/" + << this->waypointsNavigator_->getWaypoints().size()); +} + +void CbNavigateNextWaypointFree::onExit() {} + +} // namespace cl_nitrosz diff --git a/smacc2_client_library/nitrosz_client/nitrosz_client/src/nitrosz_client/client_behaviors/cb_navigate_next_waypoint_until_reached.cpp b/smacc2_client_library/nitrosz_client/nitrosz_client/src/nitrosz_client/client_behaviors/cb_navigate_next_waypoint_until_reached.cpp new file mode 100644 index 000000000..d09dfeac4 --- /dev/null +++ b/smacc2_client_library/nitrosz_client/nitrosz_client/src/nitrosz_client/client_behaviors/cb_navigate_next_waypoint_until_reached.cpp @@ -0,0 +1,37 @@ +// Copyright 2021 RobosoftAI Inc. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +/***************************************************************************************************************** + * + * Authors: Pablo Inigo Blasco, Brett Aldrich + * + ******************************************************************************************************************/ + +#include + +namespace cl_nitrosz +{ +CbNavigateNextWaypointUntilReached::CbNavigateNextWaypointUntilReached( + std::string goalWaypointName, std::optional options) +: CbNavigateNextWaypoint(options), goalWaypointName_(goalWaypointName) +{ +} + +CbNavigateNextWaypointUntilReached::~CbNavigateNextWaypointUntilReached() {} + +void CbNavigateNextWaypointUntilReached::onEntry() { CbNavigateNextWaypoint::onEntry(); } + +void CbNavigateNextWaypointUntilReached::onExit() { CbNavigateNextWaypoint::onExit(); } + +} // namespace cl_nitrosz diff --git a/smacc2_client_library/nitrosz_client/nitrosz_client/src/nitrosz_client/client_behaviors/cb_nitrosz_client_behavior_base.cpp b/smacc2_client_library/nitrosz_client/nitrosz_client/src/nitrosz_client/client_behaviors/cb_nitrosz_client_behavior_base.cpp new file mode 100644 index 000000000..71d36fa9c --- /dev/null +++ b/smacc2_client_library/nitrosz_client/nitrosz_client/src/nitrosz_client/client_behaviors/cb_nitrosz_client_behavior_base.cpp @@ -0,0 +1,127 @@ +// Copyright 2021 RobosoftAI Inc. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +/***************************************************************************************************************** + * + * Authors: Pablo Inigo Blasco, Brett Aldrich + * + ******************************************************************************************************************/ +#include +#include +namespace cl_nitrosz +{ +CbNav2ZClientBehaviorBase::~CbNav2ZClientBehaviorBase() {} + +void CbNav2ZClientBehaviorBase::sendGoal(ClNitrosZ::Goal & goal) +{ + RCLCPP_INFO_STREAM(getLogger(), "[" << getName() << "] Creating sending goal callback signal."); + + this->navigationCallback_ = std::make_shared(); + + this->getStateMachine()->createSignalConnection( + *navigationCallback_, &CbNav2ZClientBehaviorBase::onNavigationResult, this); + + RCLCPP_INFO_STREAM(getLogger(), "[" << getName() << "] Sending goal"); + goalHandleFuture_ = this->nitroszClient_->sendGoal(goal, navigationCallback_); + RCLCPP_INFO_STREAM( + getLogger(), "[" << getName() << "] Goal set, future valid: " << goalHandleFuture_.valid()); + + // this->goal_uuid_ = gh.get_goal_id () ; +} + +void CbNav2ZClientBehaviorBase::cancelGoal() { this->nitroszClient_->cancelGoal(); } + +bool CbNav2ZClientBehaviorBase::isOwnActionResponse(const ClNitrosZ::WrappedResult & r) +{ + auto name = getName(); + if (isShutdownRequested()) + { + RCLCPP_ERROR( + getLogger(), "[%s] Propagating action client signal skipped. the behavior is shutting down", + name.c_str()); + return false; + } + + if (!goalHandleFuture_.valid()) + { + RCLCPP_ERROR( + this->getLogger(), "[%s]Propagating action client signal, our goal handle is not valid", + name.c_str()); + return false; + } + + auto goalHandle = goalHandleFuture_.get(); + auto goal_uuid_ = goalHandle->get_goal_id(); + + if (r.goal_id != goal_uuid_) + { + RCLCPP_ERROR( + getLogger(), + "[%s] Received a failure event from an action server with a different goal_uuid: %s, " + "expected: %s", + smacc2::demangleType(typeid(*this)).c_str(), rclcpp_action::to_string(r.goal_id).c_str(), + rclcpp_action::to_string(goal_uuid_).c_str()); + return false; + } + + return true; +} + +void CbNav2ZClientBehaviorBase::onNavigationResult(const ClNitrosZ::WrappedResult & r) +{ + RCLCPP_DEBUG( + getLogger(), "[%s] Received result event from action server, result code", getName().c_str()); + + if (r.code == rclcpp_action::ResultCode::SUCCEEDED) + { + this->onNavigationActionSuccess(r); + } + else + { + this->onNavigationActionAbort(r); + } +} + +void CbNav2ZClientBehaviorBase::onNavigationActionSuccess(const ClNitrosZ::WrappedResult & r) +{ + // if (!isOwnActionResponse(r)) + // { + // RCLCPP_WARN( + // getLogger(), "[%s] Propagating success event skipped. Action response is not ours.", + // getName().c_str()); + // return; + // } + + navigationResult_ = r.code; + + RCLCPP_INFO(getLogger(), "[%s] Propagating success event from action server", getName().c_str()); + this->postSuccessEvent(); +} + +void CbNav2ZClientBehaviorBase::onNavigationActionAbort(const ClNitrosZ::WrappedResult & r) +{ + // if (!isOwnActionResponse(r)) + // { + // RCLCPP_WARN( + // getLogger(), "[%s] Propagating success event skipped. Action response is not ours.", + // getName().c_str()); + // return; + // } + + navigationResult_ = r.code; + auto name = getName(); + RCLCPP_INFO(getLogger(), "[%s] Propagating failure event from action server", name.c_str()); + this->postFailureEvent(); +} +} // namespace cl_nitrosz diff --git a/smacc2_client_library/nitrosz_client/nitrosz_client/src/nitrosz_client/client_behaviors/cb_pause_slam.cpp b/smacc2_client_library/nitrosz_client/nitrosz_client/src/nitrosz_client/client_behaviors/cb_pause_slam.cpp new file mode 100644 index 000000000..602bb0be5 --- /dev/null +++ b/smacc2_client_library/nitrosz_client/nitrosz_client/src/nitrosz_client/client_behaviors/cb_pause_slam.cpp @@ -0,0 +1,51 @@ +// Copyright 2021 RobosoftAI Inc. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +/***************************************************************************************************************** + * + * Authors: Pablo Inigo Blasco, Brett Aldrich + * + ******************************************************************************************************************/ +#include + +namespace cl_nitrosz +{ +CbPauseSlam::CbPauseSlam(std::string serviceName) +: smacc2::client_behaviors::CbServiceCall(serviceName.c_str()) +{ +} + +void CbPauseSlam::onEntry() +{ + this->requiresComponent(this->slam_); + + auto currentState = slam_->getState(); + + if (currentState == CpSlamToolbox::SlamToolboxState::Resumed) + { + RCLCPP_INFO( + getLogger(), "[CbPauseSlam] calling pause service to toggle from resumed to paused"); + this->request_ = std::make_shared(); + CbServiceCall::onEntry(); + this->slam_->toogleState(); + } + else + { + this->request_ = nullptr; + RCLCPP_INFO( + getLogger(), "[CbPauseSlam] calling skipped. The current slam state is already paused."); + } +} + +} // namespace cl_nitrosz diff --git a/smacc2_client_library/nitrosz_client/nitrosz_client/src/nitrosz_client/client_behaviors/cb_position_control_free_space.cpp b/smacc2_client_library/nitrosz_client/nitrosz_client/src/nitrosz_client/client_behaviors/cb_position_control_free_space.cpp new file mode 100644 index 000000000..ca41c43f4 --- /dev/null +++ b/smacc2_client_library/nitrosz_client/nitrosz_client/src/nitrosz_client/client_behaviors/cb_position_control_free_space.cpp @@ -0,0 +1,199 @@ +// Copyright 2021 RobosoftAI Inc. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +/***************************************************************************************************************** + * + * Authors: Pablo Inigo Blasco, Brett Aldrich + * + ******************************************************************************************************************/ + +#include +#include +#include + +#include +#include + +namespace cl_nitrosz +{ +CbPositionControlFreeSpace::CbPositionControlFreeSpace() +: targetYaw_(0), k_betta_(1.0), max_angular_yaw_speed_(1.0) +{ +} + +void CbPositionControlFreeSpace::updateParameters() {} + +void CbPositionControlFreeSpace::onEntry() +{ + auto nh = this->getNode(); + cmd_vel_pub_ = nh->create_publisher("/cmd_vel", rclcpp::QoS(1)); + + cl_nitrosz::Pose * pose; + + this->requiresComponent(pose); + + geometry_msgs::msg::Twist cmd_vel; + goalReached_ = false; + + geometry_msgs::msg::Pose currentPose = pose->toPoseMsg(); + + rclcpp::Rate loop_rate(10); + double countAngle = 0; + auto prevyaw = tf2::getYaw(currentPose.orientation); + + // PID controller gains (proportional, integral, and derivative) + double kp_linear = 0.5; + double ki_linear = 0.0; + double kd_linear = 0.1; + + double kp_angular = 0.5; + double ki_angular = 0.0; + double kd_angular = 0.1; + + while (rclcpp::ok() && !goalReached_) + { + RCLCPP_INFO_STREAM_THROTTLE( + getLogger(), *getNode()->get_clock(), 200, + "CbPositionControlFreeSpace, current pose: " << currentPose.position.x << ", " + << currentPose.position.y << ", " + << tf2::getYaw(currentPose.orientation)); + + RCLCPP_INFO_STREAM_THROTTLE( + getLogger(), *getNode()->get_clock(), 200, + "CbPositionControlFreeSpace, target pose: " << target_pose_.position.x << ", " + << target_pose_.position.y << ", " + << tf2::getYaw(target_pose_.orientation)); + + tf2::Quaternion q; + currentPose = pose->toPoseMsg(); + + // Here we implement the control logic to calculate the velocity command + // based on the received position of the vehicle and the target pose. + + // Calculate the errors in x and y + double error_x = target_pose_.position.x - currentPose.position.x; + double error_y = target_pose_.position.y - currentPose.position.y; + + // Calculate the distance to the target pose + double distance_to_target = std::sqrt(error_x * error_x + error_y * error_y); + + RCLCPP_INFO_STREAM( + getLogger(), "[" << getName() << "] distance to target: " << distance_to_target + << " ( th: " << threshold_distance_ << ")"); + + // Check if the robot has reached the target pose + if (distance_to_target < threshold_distance_) + { + RCLCPP_INFO(getLogger(), "Goal reached!"); + // Stop the robot by setting the velocity commands to zero + geometry_msgs::msg::Twist cmd_vel_msg; + cmd_vel_msg.linear.x = 0.0; + cmd_vel_msg.angular.z = 0.0; + cmd_vel_pub_->publish(cmd_vel_msg); + break; + } + else + { + // Calculate the desired orientation angle + double desired_yaw = std::atan2(error_y, error_x); + + // Calculate the difference between the desired orientation and the + // current orientation + double yaw_error = desired_yaw - (tf2::getYaw(currentPose.orientation) + M_PI); + + // Ensure the yaw error is within the range [-pi, pi] + while (yaw_error > M_PI) yaw_error -= 2 * M_PI; + while (yaw_error < -M_PI) yaw_error += 2 * M_PI; + + // Calculate the control signals (velocity commands) using PID controllers + double cmd_linear_x = kp_linear * distance_to_target + ki_linear * integral_linear_ + + kd_linear * (distance_to_target - prev_error_linear_); + double cmd_angular_z = kp_angular * yaw_error + ki_angular * integral_angular_ + + kd_angular * (yaw_error - prev_error_angular_); + + if (cmd_linear_x > max_linear_velocity) + cmd_linear_x = max_linear_velocity; + else if (cmd_linear_x < -max_linear_velocity) + cmd_linear_x = -max_linear_velocity; + + if (cmd_angular_z > max_angular_velocity) + cmd_angular_z = max_angular_velocity; + else if (cmd_angular_z < -max_angular_velocity) + cmd_angular_z = -max_angular_velocity; + + // Construct and publish the velocity command message + geometry_msgs::msg::Twist cmd_vel_msg; + + cmd_vel_msg.linear.x = cmd_linear_x; + cmd_vel_msg.angular.z = cmd_angular_z; + + cmd_vel_pub_->publish(cmd_vel_msg); + + // Update errors and integrals for the next control cycle + prev_error_linear_ = distance_to_target; + prev_error_angular_ = yaw_error; + integral_linear_ += distance_to_target; + integral_angular_ += yaw_error; + + // tf2::fromMsg(currentPose.orientation, q); + // auto currentYaw = tf2::getYaw(currentPose.orientation); + // auto deltaAngle = angles::shortest_angular_distance(prevyaw, + // currentYaw); countAngle += deltaAngle; + + // prevyaw = currentYaw; + // double angular_error = targetYaw_ - countAngle; + + // auto omega = angular_error * k_betta_; + // cmd_vel.linear.x = 0; + // cmd_vel.linear.y = 0; + // cmd_vel.linear.z = 0; + // cmd_vel.angular.z = + // std::min(std::max(omega, -fabs(max_angular_yaw_speed_)), + // fabs(max_angular_yaw_speed_)); + + // RCLCPP_INFO_STREAM(getLogger(), "[" << getName() << "] delta angle: " + // << deltaAngle); RCLCPP_INFO_STREAM(getLogger(), "[" << getName() << "] + // cummulated angle: " << countAngle); RCLCPP_INFO_STREAM(getLogger(), "[" + // << getName() << "] k_betta_: " << k_betta_); + + // RCLCPP_INFO_STREAM( + // getLogger(), "[" << getName() << "] angular error: " << angular_error + // << "(" + // << yaw_goal_tolerance_rads_ << ")"); + // RCLCPP_INFO_STREAM( + // getLogger(), "[" << getName() << "] command angular speed: " << + // cmd_vel.angular.z); + + // if (fabs(angular_error) < yaw_goal_tolerance_rads_) + // { + // RCLCPP_INFO_STREAM(getLogger(), "[" << getName() << "] GOAL REACHED. + // Sending stop command."); goalReached_ = true; cmd_vel.linear.x = 0; + // cmd_vel.angular.z = 0; + // break; + // } + + // this->cmd_vel_pub_->publish(cmd_vel); + + loop_rate.sleep(); + } + } + + RCLCPP_INFO_STREAM(getLogger(), "[" << getName() << "] Finished behavior execution"); + + this->postSuccessEvent(); +} + +void CbPositionControlFreeSpace::onExit() {} + +} // namespace cl_nitrosz diff --git a/smacc2_client_library/nitrosz_client/nitrosz_client/src/nitrosz_client/client_behaviors/cb_pure_spinning.cpp b/smacc2_client_library/nitrosz_client/nitrosz_client/src/nitrosz_client/client_behaviors/cb_pure_spinning.cpp new file mode 100644 index 000000000..e2b12ee93 --- /dev/null +++ b/smacc2_client_library/nitrosz_client/nitrosz_client/src/nitrosz_client/client_behaviors/cb_pure_spinning.cpp @@ -0,0 +1,102 @@ +// Copyright 2021 RobosoftAI Inc. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +/***************************************************************************************************************** + * + * Authors: Pablo Inigo Blasco, Brett Aldrich + * + ******************************************************************************************************************/ + +#include +#include +#include +#include +#include +namespace cl_nitrosz +{ + +CbPureSpinning::CbPureSpinning(double targetYaw_rads, double max_angular_yaw_speed) +: targetYaw__rads(targetYaw_rads), + k_betta_(1.0), + max_angular_yaw_speed_(max_angular_yaw_speed), + yaw_goal_tolerance_rads_(0.1) +{ +} + +void CbPureSpinning::updateParameters() {} + +void CbPureSpinning::onEntry() +{ + auto nh = this->getNode(); + cmd_vel_pub_ = nh->create_publisher("/cmd_vel", rclcpp::QoS(1)); + + cl_nitrosz::Pose * pose; + this->requiresComponent(pose); + + geometry_msgs::msg::Twist cmd_vel; + goalReached_ = false; + + geometry_msgs::msg::Pose currentPose = pose->toPoseMsg(); + + rclcpp::Rate loop_rate(10); + double countAngle = 0; + auto prevyaw = tf2::getYaw(currentPose.orientation); + while (rclcpp::ok() && !goalReached_) + { + tf2::Quaternion q; + currentPose = pose->toPoseMsg(); + tf2::fromMsg(currentPose.orientation, q); + auto currentYaw = tf2::getYaw(currentPose.orientation); + auto deltaAngle = angles::shortest_angular_distance(prevyaw, currentYaw); + countAngle += deltaAngle; + + prevyaw = currentYaw; + double angular_error = targetYaw__rads - countAngle; + + auto omega = angular_error * k_betta_; + cmd_vel.linear.x = 0; + cmd_vel.linear.y = 0; + cmd_vel.linear.z = 0; + cmd_vel.angular.z = + std::min(std::max(omega, -fabs(max_angular_yaw_speed_)), fabs(max_angular_yaw_speed_)); + + RCLCPP_INFO_STREAM(getLogger(), "[" << getName() << "] delta angle: " << deltaAngle); + RCLCPP_INFO_STREAM(getLogger(), "[" << getName() << "] cummulated angle: " << countAngle); + RCLCPP_INFO_STREAM(getLogger(), "[" << getName() << "] k_betta_: " << k_betta_); + + RCLCPP_INFO_STREAM( + getLogger(), "[" << getName() << "] angular error: " << angular_error << "(" + << yaw_goal_tolerance_rads_ << ")"); + RCLCPP_INFO_STREAM( + getLogger(), "[" << getName() << "] command angular speed: " << cmd_vel.angular.z); + + if (fabs(angular_error) < yaw_goal_tolerance_rads_) + { + RCLCPP_INFO_STREAM(getLogger(), "[" << getName() << "] GOAL REACHED. Sending stop command."); + goalReached_ = true; + cmd_vel.linear.x = 0; + cmd_vel.angular.z = 0; + break; + } + + this->cmd_vel_pub_->publish(cmd_vel); + + loop_rate.sleep(); + } + + this->postSuccessEvent(); +} + +void CbPureSpinning::onExit() {} +} // namespace cl_nitrosz diff --git a/smacc2_client_library/nitrosz_client/nitrosz_client/src/nitrosz_client/client_behaviors/cb_resume_slam.cpp b/smacc2_client_library/nitrosz_client/nitrosz_client/src/nitrosz_client/client_behaviors/cb_resume_slam.cpp new file mode 100644 index 000000000..1451b8f71 --- /dev/null +++ b/smacc2_client_library/nitrosz_client/nitrosz_client/src/nitrosz_client/client_behaviors/cb_resume_slam.cpp @@ -0,0 +1,51 @@ +// Copyright 2021 RobosoftAI Inc. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +/***************************************************************************************************************** + * + * Authors: Pablo Inigo Blasco, Brett Aldrich + * + ******************************************************************************************************************/ +#include + +namespace cl_nitrosz +{ +CbResumeSlam::CbResumeSlam(std::string serviceName) +: smacc2::client_behaviors::CbServiceCall(serviceName.c_str()) +{ +} + +void CbResumeSlam::onEntry() +{ + this->requiresComponent(this->slam_); + + auto currentState = slam_->getState(); + + if (currentState == CpSlamToolbox::SlamToolboxState::Paused) + { + RCLCPP_INFO( + getLogger(), "[CbResumeSlam] calling pause service to toggle from paused to resumed"); + this->request_ = std::make_shared(); + smacc2::client_behaviors::CbServiceCall::onEntry(); + this->slam_->toogleState(); + } + else + { + this->request_ = nullptr; + RCLCPP_INFO( + getLogger(), "[CbResumeSlam] calling skipped. The current slam state is already resumed."); + } +} + +} // namespace cl_nitrosz diff --git a/smacc2_client_library/nitrosz_client/nitrosz_client/src/nitrosz_client/client_behaviors/cb_rotate.cpp b/smacc2_client_library/nitrosz_client/nitrosz_client/src/nitrosz_client/client_behaviors/cb_rotate.cpp new file mode 100644 index 000000000..a8f398d4a --- /dev/null +++ b/smacc2_client_library/nitrosz_client/nitrosz_client/src/nitrosz_client/client_behaviors/cb_rotate.cpp @@ -0,0 +1,87 @@ +// Copyright 2021 RobosoftAI Inc. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +/***************************************************************************************************************** + * + * Authors: Pablo Inigo Blasco, Brett Aldrich + * + ******************************************************************************************************************/ +#include + +#include +#include +#include +#include + +namespace cl_nitrosz +{ +CbRotate::CbRotate(float rotate_degree) : rotateDegree(rotate_degree) {} + +CbRotate::CbRotate(float rotate_degree, SpinningPlanner spinning_planner) +: rotateDegree(rotate_degree), spinningPlanner(spinning_planner) +{ +} + +void CbRotate::onEntry() +{ + double angle_increment_degree = rotateDegree; + + auto plannerSwitcher = nitroszClient_->getComponent(); + + if (spinningPlanner && *spinningPlanner == SpinningPlanner::PureSpinning) + { + plannerSwitcher->setPureSpinningPlanner(); + } + else + { + plannerSwitcher->setDefaultPlanners(); + } + + auto p = nitroszClient_->getComponent(); + auto referenceFrame = p->getReferenceFrame(); + auto currentPoseMsg = p->toPoseMsg(); + + tf2::Transform currentPose; + tf2::fromMsg(currentPoseMsg, currentPose); + + auto odomTracker = nitroszClient_->getComponent(); + ClNitrosZ::Goal goal; + goal.pose.header.frame_id = referenceFrame; + //goal.pose.header.stamp = getNode()->now(); + + auto currentAngle = tf2::getYaw(currentPoseMsg.orientation); + auto targetAngle = currentAngle + angle_increment_degree * M_PI / 180.0; + goal.pose.pose.position = currentPoseMsg.position; + tf2::Quaternion q; + q.setRPY(0, 0, targetAngle); + goal.pose.pose.orientation = tf2::toMsg(q); + + geometry_msgs::msg::PoseStamped stampedCurrentPoseMsg; + stampedCurrentPoseMsg.header.frame_id = referenceFrame; + stampedCurrentPoseMsg.header.stamp = getNode()->now(); + stampedCurrentPoseMsg.pose = currentPoseMsg; + + this->requiresClient(nitroszClient_); + auto pathname = this->getCurrentState()->getName() + " - " + getName(); + odomTracker->pushPath(pathname); + + odomTracker->setStartPoint(stampedCurrentPoseMsg); + odomTracker->setWorkingMode(odom_tracker::WorkingMode::RECORD_PATH); + + RCLCPP_INFO_STREAM(getLogger(), "current pose: " << currentPoseMsg); + RCLCPP_INFO_STREAM(getLogger(), "goal pose: " << goal.pose.pose); + this->sendGoal(goal); +} + +} // namespace cl_nitrosz diff --git a/smacc2_client_library/nitrosz_client/nitrosz_client/src/nitrosz_client/client_behaviors/cb_rotate_look_at.cpp b/smacc2_client_library/nitrosz_client/nitrosz_client/src/nitrosz_client/client_behaviors/cb_rotate_look_at.cpp new file mode 100644 index 000000000..c77fc9f1a --- /dev/null +++ b/smacc2_client_library/nitrosz_client/nitrosz_client/src/nitrosz_client/client_behaviors/cb_rotate_look_at.cpp @@ -0,0 +1,58 @@ +// Copyright 2021 RobosoftAI Inc. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +/***************************************************************************************************************** + * + * Authors: Pablo Inigo Blasco, Brett Aldrich + * + ******************************************************************************************************************/ + +#include +#include +#include +#include +#include +#include + +#include + +namespace cl_nitrosz +{ +CbRotateLookAt::CbRotateLookAt() {} + +CbRotateLookAt::CbRotateLookAt(const geometry_msgs::msg::PoseStamped & lookAtPose) +: lookAtPose_(lookAtPose) +{ +} + +void CbRotateLookAt::onEntry() +{ + cl_nitrosz::Pose * pose; + this->requiresComponent(pose); + + pose->waitTransformUpdate(rclcpp::Rate(20)); + auto position = pose->toPoseMsg().position; + + if (lookAtPose_) + { + auto targetPosition = lookAtPose_->pose.position; + double yaw_degrees = + atan2(targetPosition.y - position.y, targetPosition.x - position.x) * 180.0 / M_PI; + this->absoluteGoalAngleDegree = yaw_degrees; + } + + CbAbsoluteRotate::onEntry(); +} + +} // namespace cl_nitrosz diff --git a/smacc2_client_library/nitrosz_client/nitrosz_client/src/nitrosz_client/client_behaviors/cb_save_slam_map.cpp b/smacc2_client_library/nitrosz_client/nitrosz_client/src/nitrosz_client/client_behaviors/cb_save_slam_map.cpp new file mode 100644 index 000000000..c50d72af4 --- /dev/null +++ b/smacc2_client_library/nitrosz_client/nitrosz_client/src/nitrosz_client/client_behaviors/cb_save_slam_map.cpp @@ -0,0 +1,85 @@ +// Copyright 2021 RobosoftAI Inc. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +/***************************************************************************************************************** + * + * Authors: Pablo Inigo Blasco, Brett Aldrich + * + ******************************************************************************************************************/ + +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + +namespace cl_nitrosz +{ +using namespace std::chrono_literals; + +CbSaveSlamMap::CbSaveSlamMap() : CbServiceCall("/map_saver/save_map", getRequest()) +{ + // : CbServiceCall("/slam_toolbox/save_map", + // getRequest()) { + + // map_name.data = "saved_map"; + // auto request = getRequest(map_name); + // RCLCPP_INFO_STREAM(getLogger(), "Save Slam Map builded"); +} + +// void onEntry() override {} + +void CbSaveSlamMap::onExit() {} + +std::shared_ptr CbSaveSlamMap::getRequest( + /*slam_toolbox::srv::SaveMap_Request_ >::_name_type saved_map_name*/) +{ + nav2_msgs::srv::SaveMap_Request map_save; + std_msgs::msg::String map_name; + + // // map_name.data = "saved_map"; + // map_save.map_topic = "map"; + // map_save.map_url = "${workspacesFolder}/maps/saved_map"; + // map_save.image_format = "png"; + // map_save.occupied_thresh = 0.65; + // map_save.free_thresh = 0.25; + // map_save.map_mode = "trinary"; + + // // auto request = std::make_shared(); + // // // request->name = saved_map_name; + // // request->name = map_name; + // // return request; + // auto request = std::make_shared(map_save); + + auto request = std::make_shared(); + request->map_topic = "map"; + request->map_url = "/tmp/saved_map"; + request->image_format = "png"; + request->occupied_thresh = 0.65; + request->free_thresh = 0.25; + request->map_mode = "trinary"; + + return request; +} +} // namespace cl_nitrosz + +// slam_toolbox::srv::SaveMap_Request_ >::_name_type + +// std_msgs::msg::String_ > diff --git a/smacc2_client_library/nitrosz_client/nitrosz_client/src/nitrosz_client/client_behaviors/cb_seek_waypoint.cpp b/smacc2_client_library/nitrosz_client/nitrosz_client/src/nitrosz_client/client_behaviors/cb_seek_waypoint.cpp new file mode 100644 index 000000000..2d82d6d73 --- /dev/null +++ b/smacc2_client_library/nitrosz_client/nitrosz_client/src/nitrosz_client/client_behaviors/cb_seek_waypoint.cpp @@ -0,0 +1,52 @@ +// Copyright 2021 RobosoftAI Inc. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +/***************************************************************************************************************** + * + * Authors: Pablo Inigo Blasco, Brett Aldrich + * + ******************************************************************************************************************/ + +#include + +namespace cl_nitrosz +{ +CbSeekWaypoint::CbSeekWaypoint(std::string seekWaypointName) +: count_(std::nullopt), seekWaypointName_(seekWaypointName) +{ +} + +CbSeekWaypoint::~CbSeekWaypoint() {} + +void CbSeekWaypoint::onEntry() +{ + cl_nitrosz::ClNitrosZ * nitroszClient_; + this->requiresClient(nitroszClient_); + waypointsNavigator_ = nitroszClient_->getComponent(); + + if (count_) + { + waypointsNavigator_->forward(*count_); + count_ = std::nullopt; + } + else if (seekWaypointName_) + { + waypointsNavigator_->seekName(*seekWaypointName_); + seekWaypointName_ = std::nullopt; + } +} + +void CbSeekWaypoint::onExit() {} + +} // namespace cl_nitrosz diff --git a/smacc2_client_library/nitrosz_client/nitrosz_client/src/nitrosz_client/client_behaviors/cb_stop_navigation.cpp b/smacc2_client_library/nitrosz_client/nitrosz_client/src/nitrosz_client/client_behaviors/cb_stop_navigation.cpp new file mode 100644 index 000000000..0cae1120e --- /dev/null +++ b/smacc2_client_library/nitrosz_client/nitrosz_client/src/nitrosz_client/client_behaviors/cb_stop_navigation.cpp @@ -0,0 +1,52 @@ +// Copyright 2021 RobosoftAI Inc. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +/***************************************************************************************************************** + * + * Authors: Pablo Inigo Blasco, Brett Aldrich + * + ******************************************************************************************************************/ + +#include +#include +#include +#include +#include +#include + +#include + +namespace cl_nitrosz +{ +CbStopNavigation::CbStopNavigation() {} + +void CbStopNavigation::onEntry() +{ + // this->sendGoal(goal); + + // this->cancelGoal(); + + cl_nitrosz::Pose * poseComponent; + this->requiresComponent(poseComponent); + + this->setGoal(poseComponent->toPoseMsg()); + + CbNavigateGlobalPosition::onEntry(); + + // this->sendGoal(goal); +} + +void CbStopNavigation::onExit() {} + +} // namespace cl_nitrosz diff --git a/smacc2_client_library/nitrosz_client/nitrosz_client/src/nitrosz_client/client_behaviors/cb_undo_path_backwards.cpp b/smacc2_client_library/nitrosz_client/nitrosz_client/src/nitrosz_client/client_behaviors/cb_undo_path_backwards.cpp new file mode 100644 index 000000000..853a868a9 --- /dev/null +++ b/smacc2_client_library/nitrosz_client/nitrosz_client/src/nitrosz_client/client_behaviors/cb_undo_path_backwards.cpp @@ -0,0 +1,124 @@ +// Copyright 2021 RobosoftAI Inc. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +/***************************************************************************************************************** + * + * Authors: Pablo Inigo Blasco, Brett Aldrich + * + ******************************************************************************************************************/ + +#include +#include +#include +#include + +namespace cl_nitrosz +{ +using ::cl_nitrosz::odom_tracker::CpOdomTracker; +using ::cl_nitrosz::odom_tracker::WorkingMode; + +using namespace std::chrono_literals; + +CbUndoPathBackwards::CbUndoPathBackwards(std::optional options) +{ + options_ = options; +} + +void CbUndoPathBackwards::onEntry() +{ + listener = std::make_shared(this->getNode()->get_clock()); + odomTracker = nitroszClient_->getComponent(); + + odomTracker->logStateString(false); + + auto plannerSwitcher = nitroszClient_->getComponent(); + + nav_msgs::msg::Path forwardpath = odomTracker->getPath(); + // RCLCPP_INFO_STREAM(getLogger(),"[UndoPathBackward] Current path backwards: " << forwardpath); + + odomTracker->setWorkingMode(WorkingMode::CLEAR_PATH); + + ClNitrosZ::Goal goal; + + auto goalCheckerSwitcher = nitroszClient_->getComponent(); + + if (options_ && options_->goalCheckerId_) + { + goalCheckerSwitcher->setGoalCheckerId(*options_->goalCheckerId_); + } + else + { + goalCheckerSwitcher->setGoalCheckerId("undo_path_backwards_goal_checker"); + } + + // WARNING: There might be some race condition with the remote undo global planner were the global path was not + // received yet + // TODO: waiting notification from global planner that it is loaded + rclcpp::sleep_for(1s); + + // this line is used to flush/reset backward planner in the case it were already there + // plannerSwitcher->setDefaultPlanners(); + if (forwardpath.poses.size() > 0) + { + goal.pose = forwardpath.poses.front(); + //goal.pose.header.stamp = getNode()->now(); + goal.pose.header.stamp = rclcpp::Time(0); + + if (options_ && options_->undoControllerName_) + { + plannerSwitcher->setUndoPathBackwardPlanner(false); + RCLCPP_INFO_STREAM( + getLogger(), + "[" << getName() << "] Undoing path with controller: " << *options_->undoControllerName_); + plannerSwitcher->setDesiredController(*options_->undoControllerName_); + plannerSwitcher->commitPublish(); + } + else + { + plannerSwitcher->setUndoPathBackwardPlanner(); + } + + this->sendGoal(goal); + } +} + +void CbUndoPathBackwards::onExit() +{ + RCLCPP_INFO_STREAM(getLogger(), "[" << getName() << "] Exiting: undo navigation "); + + if (this->navigationResult_ == rclcpp_action::ResultCode::SUCCEEDED) + { + RCLCPP_INFO_STREAM( + getLogger(), getName() << " - [CbUndoPathBackwards] Exiting: undo navigation successful, " + "popping odom tracker path"); + odomTracker = nitroszClient_->getComponent(); + odomTracker->popPath(); + + odomTracker->logStateString(false); + } + else + { + RCLCPP_INFO_STREAM( + getLogger(), getName() << " - [CbUndoPathBackwards] Exiting: undo navigation abort, avoiding " + "popping current path"); + + odomTracker = nitroszClient_->getComponent(); + odomTracker->logStateString(false); + // navigation interrupted or aborted. The path may be not totally undone. + // We keep the odom tracker in its current state, probably in the middle of the undoing process. + // Could you try to repeat the behavior? + } +} + +} // namespace cl_nitrosz diff --git a/smacc2_client_library/nitrosz_client/nitrosz_client/src/nitrosz_client/client_behaviors/cb_wait_nitros_nodes.cpp b/smacc2_client_library/nitrosz_client/nitrosz_client/src/nitrosz_client/client_behaviors/cb_wait_nitros_nodes.cpp new file mode 100644 index 000000000..a73744706 --- /dev/null +++ b/smacc2_client_library/nitrosz_client/nitrosz_client/src/nitrosz_client/client_behaviors/cb_wait_nitros_nodes.cpp @@ -0,0 +1,133 @@ +// Copyright 2021 RobosoftAI Inc. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +/***************************************************************************************************************** + * + * Authors: Pablo Inigo Blasco, Brett Aldrich + * + ******************************************************************************************************************/ + +#include +#include + +namespace cl_nitrosz +{ +CbWaitNav2Nodes::CbWaitNav2Nodes(std::vector waitNodes) +: CbWaitNav2Nodes("/bond", waitNodes) +{ +} + +CbWaitNav2Nodes::CbWaitNav2Nodes(std::string topic, std::vector waitNodes) +: topicname_(topic), waitNodes_(waitNodes) +{ +} + +void CbWaitNav2Nodes::onMessageReceived(const bond::msg::Status & msg) +{ + auto value = fromString(msg.id); + bool updated = false; + // RCLCPP_INFO(getLogger(), "[CbWaitNav2Nodes] received '%s'", msg.id.c_str()); + + if (receivedAliveMsg_.count(value) && !receivedAliveMsg_[value]) + { + RCLCPP_INFO(getLogger(), "[CbWaitNav2Nodes] '%s' alive received", msg.id.c_str()); + receivedAliveMsg_[value] = true; + updated = true; + } + + if (updated) + { + bool success = true; + std::stringstream ss; + for (auto & pair : receivedAliveMsg_) + { + if (!pair.second) + { + success = false; + } + ss << "- " << toString(pair.first) << ": " << (pair.second ? "ALIVE" : "WAITING") + << std::endl; + } + + RCLCPP_INFO_STREAM( + getLogger(), "[CbWaitNav2Nodes] waiting nodes status:" << std::endl + << ss.str().c_str()); + + if (success) + { + RCLCPP_INFO(getLogger(), "[CbWaitNav2Nodes] success event"); + this->postSuccessEvent(); + sub_ = nullptr; + } + else + { + RCLCPP_INFO(getLogger(), "[CbWaitNav2Nodes] still missing nodes"); + } + } +} + +void CbWaitNav2Nodes::onEntry() +{ + std::stringstream ss; + for (auto v : waitNodes_) + { + receivedAliveMsg_[v] = false; + ss << "[CbWaitNav2Nodes] - " << toString(v) << ": waiting" << std::endl; + } + RCLCPP_INFO(getLogger(), ss.str().c_str()); + + //rclcpp::SensorDataQoS qos; + rclcpp::SubscriptionOptions sub_option; + + sub_ = getNode()->create_subscription( + topicname_, 20, std::bind(&CbWaitNav2Nodes::onMessageReceived, this, std::placeholders::_1), + sub_option); +} + +std::string toString(Nav2Nodes value) +{ + switch (value) + { + case Nav2Nodes::PlannerServer: + return "planner_server"; + case Nav2Nodes::ControllerServer: + return "controller_server"; + case Nav2Nodes::RecoveriesServer: + return "behavior_server"; + case Nav2Nodes::BtNavigator: + return "bt_navigator"; + case Nav2Nodes::MapServer: + return "map_server"; + default: + return ""; + } +} + +Nav2Nodes fromString(std::string id) +{ + if (id == "planner_server") + return Nav2Nodes::PlannerServer; + else if (id == "controller_server") + return Nav2Nodes::ControllerServer; + else if (id == "behavior_server") + return Nav2Nodes::RecoveriesServer; + else if (id == "bt_navigator") + return Nav2Nodes::BtNavigator; + else if (id == "map_server") + return Nav2Nodes::MapServer; + else + return Nav2Nodes::None; +} + +} // namespace cl_nitrosz diff --git a/smacc2_client_library/nitrosz_client/nitrosz_client/src/nitrosz_client/client_behaviors/cb_wait_pose.cpp b/smacc2_client_library/nitrosz_client/nitrosz_client/src/nitrosz_client/client_behaviors/cb_wait_pose.cpp new file mode 100644 index 000000000..ee3a2b2fd --- /dev/null +++ b/smacc2_client_library/nitrosz_client/nitrosz_client/src/nitrosz_client/client_behaviors/cb_wait_pose.cpp @@ -0,0 +1,52 @@ +// Copyright 2021 RobosoftAI Inc. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +/***************************************************************************************************************** + * + * Authors: Pablo Inigo Blasco, Brett Aldrich + * + ******************************************************************************************************************/ + +#include +#include + +#include +#include + +namespace cl_nitrosz +{ +CbWaitPose::CbWaitPose() {} + +CbWaitPose::~CbWaitPose() {} + +void CbWaitPose::onEntry() +{ + auto pose = this->nitroszClient_->getComponent(); + try + { + pose->waitTransformUpdate(rclcpp::Rate(20)); + auto posemsg = pose->toPoseMsg(); + RCLCPP_INFO_STREAM(getLogger(), "[CbWaitPose] pose arrived: " << std::endl << posemsg); + } + catch (std::exception & ex) + { + RCLCPP_INFO(getLogger(), "[CbWaitPose] error getting the robot pose"); + this->postFailureEvent(); + return; + } + + RCLCPP_INFO(getLogger(), "[CbWaitPose] pose received"); + this->postSuccessEvent(); +} +} // namespace cl_nitrosz diff --git a/smacc2_client_library/nitrosz_client/nitrosz_client/src/nitrosz_client/client_behaviors/cb_wait_transform.cpp b/smacc2_client_library/nitrosz_client/nitrosz_client/src/nitrosz_client/client_behaviors/cb_wait_transform.cpp new file mode 100644 index 000000000..2c242c2bd --- /dev/null +++ b/smacc2_client_library/nitrosz_client/nitrosz_client/src/nitrosz_client/client_behaviors/cb_wait_transform.cpp @@ -0,0 +1,63 @@ +// Copyright 2021 RobosoftAI Inc. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +/***************************************************************************************************************** + * + * Authors: Pablo Inigo Blasco, Brett Aldrich + * + ******************************************************************************************************************/ +#include + +namespace cl_nitrosz +{ +CbWaitTransform::CbWaitTransform( + std::string targetFrame, std::string referenceFrame, rclcpp::Duration timeout) +: targetFrame_(targetFrame), referenceFrame_(referenceFrame), timeout_(timeout) +{ +} + +CbWaitTransform::~CbWaitTransform() {} + +void CbWaitTransform::onEntry() +{ + RCLCPP_INFO( + getLogger(), "[CbWaitTransform] ref %s -> target %s", referenceFrame_.c_str(), + targetFrame_.c_str()); + + tfBuffer_ = std::make_shared(getNode()->get_clock()); + tfListener_ = std::make_shared(*tfBuffer_); + + tf2::Stamped transform; + try + { + auto transformstamped = + tfBuffer_->lookupTransform(targetFrame_, referenceFrame_, getNode()->now(), timeout_); + tf2::fromMsg(transformstamped, transform); + + result_ = transform; + + RCLCPP_INFO( + getLogger(), "[CbWaitTransform] Success wait transform ref %s -> target %s", + referenceFrame_.c_str(), targetFrame_.c_str()); + this->postSuccessEvent(); + } + catch (tf2::TransformException & ex) + { + RCLCPP_ERROR_STREAM( + getLogger(), "[CbWaitTransform] Failure waiting transform ( ref " + << targetFrame_ << "/ target " << referenceFrame_ << " - " << ex.what()); + this->postFailureEvent(); + } +} +} // namespace cl_nitrosz diff --git a/smacc2_client_library/nitrosz_client/nitrosz_client/src/nitrosz_client/common.cpp b/smacc2_client_library/nitrosz_client/nitrosz_client/src/nitrosz_client/common.cpp new file mode 100644 index 000000000..bec10359b --- /dev/null +++ b/smacc2_client_library/nitrosz_client/nitrosz_client/src/nitrosz_client/common.cpp @@ -0,0 +1,64 @@ +// Copyright 2021 RobosoftAI Inc. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +/***************************************************************************************************************** + * + * Authors: Pablo Inigo Blasco, Brett Aldrich + * + ******************************************************************************************************************/ + +#include + +#include +#include +#include + +std::ostream & operator<<(std::ostream & out, const geometry_msgs::msg::Quaternion & msg) +{ + out << " Orientation [" << msg.x << " , " << msg.y << " , " << msg.z << ", " << msg.w + << "] , yaw: " << tf2::getYaw(msg); + return out; +} + +std::ostream & operator<<(std::ostream & out, const geometry_msgs::msg::Point & msg) +{ + out << "[" << msg.x << " , " << msg.y << " , " << msg.z << "]"; + return out; +} + +std::ostream & operator<<(std::ostream & out, const geometry_msgs::msg::Pose & msg) +{ + out << " p " << msg.position; + out << " q [" << msg.orientation.x << " , " << msg.orientation.y << " , " << msg.orientation.z + << ", " << msg.orientation.w << "]"; + return out; +} + +std::ostream & operator<<(std::ostream & out, const geometry_msgs::msg::PoseStamped & msg) +{ + out << msg.pose; + return out; +} + +std::ostream & operator<<(std::ostream & out, const nav2_msgs::action::NavigateToPose::Goal & msg) +{ + out << msg.pose; + return out; +} + +std::ostream & operator<<(std::ostream & out, const builtin_interfaces::msg::Time & msg) +{ + out << "seconds: " << rclcpp::Time(msg).seconds(); + return out; +} diff --git a/smacc2_client_library/nitrosz_client/nitrosz_client/src/nitrosz_client/components/amcl/cp_amcl.cpp b/smacc2_client_library/nitrosz_client/nitrosz_client/src/nitrosz_client/components/amcl/cp_amcl.cpp new file mode 100644 index 000000000..339923708 --- /dev/null +++ b/smacc2_client_library/nitrosz_client/nitrosz_client/src/nitrosz_client/components/amcl/cp_amcl.cpp @@ -0,0 +1,43 @@ +// Copyright 2021 RobosoftAI Inc. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +/***************************************************************************************************************** + * + * Authors: Pablo Inigo Blasco, Brett Aldrich + * + ******************************************************************************************************************/ +#include + +#include + +namespace cl_nitrosz +{ +CpAmcl::CpAmcl() {} + +CpAmcl::~CpAmcl() {} + +std::string CpAmcl::getName() const { return "AMCL"; } + +void CpAmcl::onInitialize() +{ + initalPosePub_ = getNode()->create_publisher( + "initialpose", rclcpp::QoS(10)); +} + +void CpAmcl::setInitialPose(const geometry_msgs::msg::PoseWithCovarianceStamped & initialpose) +{ + initalPosePub_->publish(initialpose); +} + +} // namespace cl_nitrosz diff --git a/smacc2_client_library/nitrosz_client/nitrosz_client/src/nitrosz_client/components/costmap_switch/cp_costmap_switch.cpp b/smacc2_client_library/nitrosz_client/nitrosz_client/src/nitrosz_client/components/costmap_switch/cp_costmap_switch.cpp new file mode 100644 index 000000000..3f1ff2ca8 --- /dev/null +++ b/smacc2_client_library/nitrosz_client/nitrosz_client/src/nitrosz_client/components/costmap_switch/cp_costmap_switch.cpp @@ -0,0 +1,162 @@ +// Copyright 2021 RobosoftAI Inc. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#include + +namespace cl_nitrosz +{ +std::array CpCostmapSwitch::layerNames = { + "global_costmap/obstacles_layer", + "local_costmap/obstacles_layer" + "global_costmap/inflater_layer", + "local_costmap/inflater_layer"}; + +void CpCostmapSwitch::registerProxyFromDynamicReconfigureServer( + std::string costmapName, std::string enablePropertyName) +{ + RCLCPP_INFO(getLogger(), "[CpCostmapSwitch] registering costmap type: %s", costmapName.c_str()); + auto proxy = std::make_shared( + this->nitroszClient_->getName() + "/" + costmapName, enablePropertyName, getNode()); + costmapProxies[costmapName] = proxy; +} + +CpCostmapSwitch::CpCostmapSwitch() {} + +void CpCostmapSwitch::onInitialize() +{ + this->nitroszClient_ = dynamic_cast(owner_); + + if (this->nitroszClient_ == nullptr) + { + RCLCPP_ERROR(getLogger(), "the owner of the CpCostmapSwitch must be a ClNitrosZ"); + } + + registerProxyFromDynamicReconfigureServer( + getStandardCostmapName(StandardLayers::GLOBAL_OBSTACLES_LAYER)); + registerProxyFromDynamicReconfigureServer( + getStandardCostmapName(StandardLayers::LOCAL_OBSTACLES_LAYER)); + registerProxyFromDynamicReconfigureServer( + getStandardCostmapName(StandardLayers::GLOBAL_INFLATED_LAYER)); + registerProxyFromDynamicReconfigureServer( + getStandardCostmapName(StandardLayers::LOCAL_INFLATED_LAYER)); +} + +std::string CpCostmapSwitch::getStandardCostmapName(StandardLayers layertype) +{ + return CpCostmapSwitch::layerNames[(int)layertype]; +} + +bool CpCostmapSwitch::exists(std::string layerName) +{ + if (!this->costmapProxies.count(layerName)) + { + return false; + } + + return true; +} + +void CpCostmapSwitch::enable(std::string layerName) +{ + RCLCPP_INFO(getLogger(), "[CpCostmapSwitch] enabling %s", layerName.c_str()); + + if (!exists(layerName)) + { + RCLCPP_ERROR(getLogger(), "[CpCostmapSwitch] costmap %s does not exist", layerName.c_str()); + return; + } + else + { + RCLCPP_INFO( + getLogger(), "[CpCostmapSwitch] costmap %s found. Calling dynamic reconfigure server.", + layerName.c_str()); + costmapProxies[layerName]->setCostmapEnabled(true); + } +} + +void CpCostmapSwitch::enable(StandardLayers layerType) +{ + this->enable(getStandardCostmapName(layerType)); +} + +void CpCostmapSwitch::disable(std::string layerName) +{ + RCLCPP_INFO(getLogger(), "[CpCostmapSwitch] disabling %s", layerName.c_str()); + + if (!exists(layerName)) + { + RCLCPP_ERROR(getLogger(), "[CpCostmapSwitch] costmap %s does not exist", layerName.c_str()); + return; + } + else + { + RCLCPP_INFO( + getLogger(), "[CpCostmapSwitch] costmap %s found. Calling dynamic reconfigure server.", + layerName.c_str()); + costmapProxies[layerName]->setCostmapEnabled(false); + } +} + +void CpCostmapSwitch::disable(StandardLayers layerType) +{ + this->disable(getStandardCostmapName(layerType)); +} + +//------------------------------------------------------------------------- + +CpCostmapProxy::CpCostmapProxy( + std::string /*costmap_name*/, std::string /*enablePropertyName*/, rclcpp::Node::SharedPtr nh) +: nh_(nh) +{ + // this->costmapName_ = costmap_name + "/set_parameters"; + // dynamic_reconfigure::BoolParameter enableField; + // enableField.name = "enabled"; + // enableField.value = true; + + // enableReq.bools.push_back(enableField); + + // enableField.value = false; + // disableReq.bools.push_back(enableField); + RCLCPP_ERROR(nh->get_logger(), "costmap switch not implemented %s", costmapName_.c_str()); +} + +void CpCostmapProxy::setCostmapEnabled(bool /*value*/) +{ + // dynamic_reconfigure::ReconfigureRequest srv_req; + // dynamic_reconfigure::ReconfigureResponse srv_resp; + + // if (value) + // srv_req.config = enableReq; + // else + // srv_req.config = disableReq; + + // if (ros::service::exists(costmapName_, true)) + // { + // RCLCPP_INFO(getLogger(),"sending dynamic reconfigure request: %s", costmapName_.c_str()); + // ros::service::call(costmapName_, srv_req, srv_resp); + // } + // else + // { + // RCLCPP_WARN(getLogger(),"could not call dynamic reconfigure server. It does not exist: %s", costmapName_.c_str()); + // } + + RCLCPP_ERROR(nh_->get_logger(), "costmap switch not implemented %s", costmapName_.c_str()); +} + +// void CpCostmapProxy::dynreconfCallback(const dynamic_reconfigure::Config::ConstPtr &configuration_update) +// { +// // auto gp = std::find_if(configuration_update->strs.begin(), configuration_update->strs.begin(), +// // [&](const dynamic_reconfigure::StrParameter &p) { return p.name == "base_global_planner" && p.value == desired_global_planner_; }); +// } +} // namespace cl_nitrosz diff --git a/smacc2_client_library/nitrosz_client/nitrosz_client/src/nitrosz_client/components/goal_checker_switcher/cp_goal_checker_switcher.cpp b/smacc2_client_library/nitrosz_client/nitrosz_client/src/nitrosz_client/components/goal_checker_switcher/cp_goal_checker_switcher.cpp new file mode 100644 index 000000000..8f219121f --- /dev/null +++ b/smacc2_client_library/nitrosz_client/nitrosz_client/src/nitrosz_client/components/goal_checker_switcher/cp_goal_checker_switcher.cpp @@ -0,0 +1,75 @@ +// Copyright 2021 RobosoftAI Inc. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +/***************************************************************************************************************** + * + * Authors: Pablo Inigo Blasco, Brett Aldrich + * + ******************************************************************************************************************/ +#include + +namespace cl_nitrosz +{ +CpGoalCheckerSwitcher::CpGoalCheckerSwitcher( + std::string goal_checker_selector_topic, std::string default_goal_checker_name) +: goal_checker_selector_topic_(goal_checker_selector_topic), + default_goal_checker_name_(default_goal_checker_name) +{ +} + +CpGoalCheckerSwitcher::~CpGoalCheckerSwitcher() {} + +void CpGoalCheckerSwitcher::onInitialize() +{ + rclcpp::QoS qos(rclcpp::KeepLast(1)); + qos.transient_local().reliable(); + + this->goal_checker_selector_pub_ = + getNode()->create_publisher(goal_checker_selector_topic_, qos); +} + +void CpGoalCheckerSwitcher::setDefaultGoalChecker() +{ + setGoalCheckerId(default_goal_checker_name_); // default id in navigation2 stack +} + +void CpGoalCheckerSwitcher::setGoalCheckerId(std::string goalcheckerid) +{ + RCLCPP_INFO_STREAM( + getLogger(), "[CpGoalCheckerSwitcher] Setting goal checker: " << goalcheckerid); + + // controller_server_node_->wait_for_service(); + // std::vector params{ rclcpp::Parameter("current_goal_checker", goalcheckerid) }; + // auto futureResults = controller_server_node_->set_parameters(params); + + std_msgs::msg::String msg; + msg.data = goalcheckerid; + this->goal_checker_selector_pub_->publish(msg); + + // int i = 0; + // for (auto& res : futureResults.get()) + // { + // RCLCPP_INFO_STREAM(getLogger(), "[CpGoalCheckerSwitcher] parameter result: " + // << params[i].get_name() << "=" << params[i].as_string() + // << ". Result: " << res.successful); + // i++; + + // if (!res.successful) + // RCLCPP_ERROR_STREAM(this->getLogger(), "[CpGoalCheckerSwitcher] goal checker could not properly + // switch " + // "the goal checker of the controller_server"); + // } +} + +} // namespace cl_nitrosz diff --git a/smacc2_client_library/nitrosz_client/nitrosz_client/src/nitrosz_client/components/odom_tracker/cp_odom_tracker.cpp b/smacc2_client_library/nitrosz_client/nitrosz_client/src/nitrosz_client/components/odom_tracker/cp_odom_tracker.cpp new file mode 100644 index 000000000..ff2e9b873 --- /dev/null +++ b/smacc2_client_library/nitrosz_client/nitrosz_client/src/nitrosz_client/components/odom_tracker/cp_odom_tracker.cpp @@ -0,0 +1,543 @@ +// Copyright 2021 RobosoftAI Inc. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +/***************************************************************************************************************** + * + * Authors: Pablo Inigo Blasco, Brett Aldrich + * + ******************************************************************************************************************/ +#include + +#include +#include +#include + +#include + +namespace cl_nitrosz +{ +namespace odom_tracker +{ +CpOdomTracker::CpOdomTracker(std::string odomTopicName, std::string odomFrame) +{ + workingMode_ = WorkingMode::RECORD_PATH; + publishMessages = true; + subscribeToOdometryTopic_ = true; + odomFrame_ = odomFrame; + odomTopicName_ = odomTopicName; +} + +template +void parameterDeclareAndtryGetOrSet( + rclcpp::Node::SharedPtr & node, std::string param_name, T & value) +{ + if (!node->get_parameter(param_name, value)) + { + RCLCPP_INFO_STREAM( + node->get_logger(), "[CpOdomTracker] autoset " << param_name << ": " << value); + node->declare_parameter(param_name, value); + } + else + { + RCLCPP_INFO_STREAM(node->get_logger(), "[CpOdomTracker] " << param_name << ": " << value); + } +} + +void CpOdomTracker::onInitialize() +{ + // default values + recordPointDistanceThreshold_ = 0.005; // 5 mm + recordAngularDistanceThreshold_ = 0.1; // radians + clearPointDistanceThreshold_ = 0.05; // 5 cm + clearAngularDistanceThreshold_ = 0.1; // radi + + auto nh = getNode(); + RCLCPP_WARN(getLogger(), "[CpOdomTracker] Initializing Odometry Tracker"); + + parameterDeclareAndtryGetOrSet(nh, "odom_frame", this->odomFrame_); + parameterDeclareAndtryGetOrSet( + nh, "record_point_distance_threshold", recordPointDistanceThreshold_); + parameterDeclareAndtryGetOrSet( + nh, "record_angular_distance_threshold", recordAngularDistanceThreshold_); + parameterDeclareAndtryGetOrSet( + nh, "clear_point_distance_threshold", clearPointDistanceThreshold_); + parameterDeclareAndtryGetOrSet( + nh, "clear_angular_distance_threshold", clearAngularDistanceThreshold_); + + if (this->subscribeToOdometryTopic_) + { + RCLCPP_INFO_STREAM( + nh->get_logger(), "[CpOdomTracker] subscribing to odom topic: " << odomTopicName_); + + rclcpp::SensorDataQoS qos; + odomSub_ = nh->create_subscription( + odomTopicName_, qos, + std::bind(&CpOdomTracker::processOdometryMessage, this, std::placeholders::_1)); + } + + robotBasePathPub_ = + nh->create_publisher("odom_tracker_path", rclcpp::QoS(1)); + robotBasePathStackedPub_ = + nh->create_publisher("odom_tracker_stacked_path", rclcpp::QoS(1)); +} + +/** + ****************************************************************************************************************** + * setWorkingMode() + ****************************************************************************************************************** + */ +void CpOdomTracker::setWorkingMode(WorkingMode workingMode) +{ + // RCLCPP_INFO(getLogger(),"odom_tracker m_mutex acquire"); + std::lock_guard lock(m_mutex_); + + switch (workingMode) + { + case WorkingMode::RECORD_PATH: + RCLCPP_INFO_STREAM( + getLogger(), + "[CpOdomTracker] setting working mode to RECORD - record_point_distance_threshold: " + << recordPointDistanceThreshold_ + << ", record_angular_distance_threshold: " << recordAngularDistanceThreshold_); + break; + case WorkingMode::CLEAR_PATH: + RCLCPP_INFO_STREAM( + getLogger(), + "[CpOdomTracker] setting working mode to CLEAR - clear_point_distance_threshold: " + << clearPointDistanceThreshold_ + << ", clear_angular_distance_threshold: " << clearAngularDistanceThreshold_); + break; + case WorkingMode::IDLE: + RCLCPP_INFO_STREAM(getLogger(), "[CpOdomTracker] setting working mode to IDLE"); + break; + default: + + RCLCPP_INFO_STREAM(getLogger(), "[CpOdomTracker] setting working mode to "); + } + + workingMode_ = workingMode; + // RCLCPP_INFO(getLogger(),"odom_tracker m_mutex release"); +} + +/** + ****************************************************************************************************************** + * setPublishMessages() + ****************************************************************************************************************** + */ +void CpOdomTracker::setPublishMessages(bool value) +{ + // RCLCPP_INFO(getLogger(),"odom_tracker m_mutex acquire"); + std::lock_guard lock(m_mutex_); + publishMessages = value; + // RCLCPP_INFO(getLogger(),"odom_tracker m_mutex release"); + this->updateAggregatedStackPath(); +} + +void CpOdomTracker::pushPath() { this->pushPath(std::string()); } + +void CpOdomTracker::pushPath(std::string pathname) +{ + RCLCPP_INFO(getLogger(), "odom_tracker m_mutex acquire"); + std::lock_guard lock(m_mutex_); + this->logStateString(false); + + pathInfos_.push_back({currentPathName_, this->currentMotionGoal_}); + pathStack_.push_back(baseTrajectory_); + + geometry_msgs::msg::PoseStamped goalPose; + if (this->currentMotionGoal_) goalPose = *this->currentMotionGoal_; + + RCLCPP_INFO_STREAM( + getLogger(), "[CpOdomTracker] currentPathName: " << pathname + << " size: " << baseTrajectory_.poses.size() + << " current motion goal: " << goalPose); + + currentPathName_ = pathname; + + // clean the current trajectory to start a new one + baseTrajectory_.poses.clear(); + + this->logStateString(false); + this->updateAggregatedStackPath(); + + this->currentMotionGoal_.reset(); +} + +void CpOdomTracker::popPath(int popCount, bool keepPreviousPath) +{ + RCLCPP_INFO(getLogger(), "odom_tracker m_mutex acquire"); + std::lock_guard lock(m_mutex_); + + RCLCPP_INFO(getLogger(), "POP PATH ENTRY"); + this->logStateString(); + + if (!keepPreviousPath) + { + baseTrajectory_.poses.clear(); + } + + while (popCount > 0 && !pathStack_.empty()) + { + auto & stackedPath = pathStack_.back().poses; + auto & stackedPathInfo = pathInfos_.back(); + + baseTrajectory_.poses.insert( + baseTrajectory_.poses.begin(), stackedPath.begin(), stackedPath.end()); + this->currentMotionGoal_ = stackedPathInfo.goalPose; + pathStack_.pop_back(); + pathInfos_.pop_back(); + popCount--; + + RCLCPP_INFO(getLogger(), "POP PATH Iteration "); + this->logStateString(); + } + + RCLCPP_INFO(getLogger(), "POP PATH EXITING"); + this->logStateString(); + RCLCPP_INFO(getLogger(), "odom_tracker m_mutex release"); + this->updateAggregatedStackPath(); + + this->currentMotionGoal_.reset(); +} + +void CpOdomTracker::logStateString(bool debug) +{ + std::stringstream ss; + ss << "--- odom tracker state ---" << std::endl; + ss + << " - path stack -" << currentPathName_ << " - size:" + << pathStack_.size() + //<< " goal: " << (getCurrentMotionGoal()? getCurrentMotionGoal()->pose: geometry_msgs::msg::Pose()) + << std::endl; + ss << " - [STACK-HEAD active path size: " << baseTrajectory_.poses.size() << "]" << std::endl; + int i = 0; + for (auto & p : pathStack_ | boost::adaptors::reversed) + { + auto & pathinfo = pathInfos_[pathInfos_.size() - i - 1]; + std::string goalstr = "[]"; + if (pathinfo.goalPose) + { + std::stringstream ss; + ss << *(pathinfo.goalPose); + goalstr = ss.str(); + } + + ss << " - p " << i << "[" << p.header.stamp << "], size: " << p.poses.size() << " - " + << pathinfo.name << " - goal: " << goalstr << std::endl; + i++; + } + ss << "---"; + + if (debug) + RCLCPP_DEBUG(getLogger(), ss.str().c_str()); + else + RCLCPP_INFO(getLogger(), ss.str().c_str()); +} + +void CpOdomTracker::clearPath() +{ + std::lock_guard lock(m_mutex_); + baseTrajectory_.poses.clear(); + + rtPublishPaths(getNode()->now()); + this->logStateString(); + this->updateAggregatedStackPath(); + + this->currentMotionGoal_.reset(); +} + +void CpOdomTracker::setCurrentPathName(const std::string & currentPathName) +{ + currentPathName_ = currentPathName; +} + +void CpOdomTracker::setStartPoint(const geometry_msgs::msg::PoseStamped & pose) +{ + std::lock_guard lock(m_mutex_); + RCLCPP_INFO_STREAM(getLogger(), "[CpOdomTracker] set current path starting point: " << pose); + if (baseTrajectory_.poses.size() > 0) + { + baseTrajectory_.poses[0] = pose; + } + else + { + baseTrajectory_.poses.push_back(pose); + } + this->updateAggregatedStackPath(); +} + +void CpOdomTracker::setStartPoint(const geometry_msgs::msg::Pose & pose) +{ + std::lock_guard lock(m_mutex_); + RCLCPP_INFO_STREAM(getLogger(), "[CpOdomTracker] set current path starting point: " << pose); + geometry_msgs::msg::PoseStamped posestamped; + posestamped.header.frame_id = this->odomFrame_; + posestamped.header.stamp = getNode()->now(); + posestamped.pose = pose; + + if (baseTrajectory_.poses.size() > 0) + { + baseTrajectory_.poses[0] = posestamped; + } + else + { + baseTrajectory_.poses.push_back(posestamped); + } + this->updateAggregatedStackPath(); +} + +void CpOdomTracker::setCurrentMotionGoal(const geometry_msgs::msg::PoseStamped & pose) +{ + std::lock_guard lock(m_mutex_); + this->currentMotionGoal_ = pose; +} + +std::optional CpOdomTracker::getCurrentMotionGoal() +{ + std::lock_guard lock(m_mutex_); + return this->currentMotionGoal_; +} + +nav_msgs::msg::Path CpOdomTracker::getPath() +{ + std::lock_guard lock(m_mutex_); + return this->baseTrajectory_; +} + +/** + ****************************************************************************************************************** + * rtPublishPaths() + ****************************************************************************************************************** + */ +void CpOdomTracker::rtPublishPaths(rclcpp::Time timestamp) +{ + baseTrajectory_.header.stamp = timestamp; + robotBasePathPub_->publish(baseTrajectory_); + + aggregatedStackPathMsg_.header.stamp = timestamp; + robotBasePathStackedPub_->publish(aggregatedStackPathMsg_); +} + +void CpOdomTracker::updateAggregatedStackPath() +{ + aggregatedStackPathMsg_.poses.clear(); + for (auto & p : pathStack_) + { + aggregatedStackPathMsg_.poses.insert( + aggregatedStackPathMsg_.poses.end(), p.poses.begin(), p.poses.end()); + } + + aggregatedStackPathMsg_.header.frame_id = this->odomFrame_; +} + +/** + ****************************************************************************************************************** + * updateBackward() + ****************************************************************************************************************** + */ +bool CpOdomTracker::updateClearPath(const nav_msgs::msg::Odometry & odom) +{ + // we initially accept any message if the queue is empty + /// Track robot base pose + geometry_msgs::msg::PoseStamped base_pose; + + base_pose.pose = odom.pose.pose; + base_pose.header = odom.header; + baseTrajectory_.header = odom.header; + + bool acceptBackward = false; + bool clearingError = false; + bool finished = false; + + while (!finished) + { + if ( + baseTrajectory_.poses.size() <= + 1) // we at least keep always the first point of the forward path when clearing + // (this is important for backwards planner replanning and not losing the + // last goal) + { + acceptBackward = false; + finished = true; + } + else + { + auto & carrotPose = baseTrajectory_.poses.back().pose; + auto & carrotPoint = carrotPose.position; + + double carrotAngle = tf2::getYaw(carrotPose.orientation); + + auto & currePose = base_pose.pose; + auto & currePoint = currePose.position; + double currentAngle = tf2::getYaw(currePose.orientation); + + double lastpointdist = p2pDistance(carrotPoint, currePoint); + double goalAngleOffset = fabs(angles::shortest_angular_distance(carrotAngle, currentAngle)); + + acceptBackward = !baseTrajectory_.poses.empty() && + lastpointdist < clearPointDistanceThreshold_ && + goalAngleOffset < clearAngularDistanceThreshold_; + + clearingError = lastpointdist > 2 * clearPointDistanceThreshold_; + RCLCPP_DEBUG_STREAM( + getLogger(), "[CpOdomTracker] clearing (accepted: " << acceptBackward + << ") linerr: " << lastpointdist + << ", anglerr: " << goalAngleOffset); + } + + // RCLCPP_INFO(getLogger(),"Backwards, last distance: %lf < %lf accept: %d", dist, + // minPointDistanceBackwardThresh_, acceptBackward); + if ( + acceptBackward && + baseTrajectory_.poses.size() > 1) /* we always leave at least one item, specially interesting + for the backward local planner reach + the backwards goal with enough precision*/ + { + baseTrajectory_.poses.pop_back(); + } + else if (clearingError) + { + finished = true; + RCLCPP_WARN(getLogger(), "[CpOdomTracker] Incorrect odom clearing motion."); + } + else + { + finished = true; + /// Not removing point because it is enough far from the last cord point + } + } + + return acceptBackward; +} +/** + ****************************************************************************************************************** + * updateRecordPath() + ****************************************************************************************************************** + */ +bool CpOdomTracker::updateRecordPath(const nav_msgs::msg::Odometry & odom) +{ + /// Track robot base pose + geometry_msgs::msg::PoseStamped base_pose; + + base_pose.pose = odom.pose.pose; + base_pose.header = odom.header; + baseTrajectory_.header = odom.header; + + bool enqueueOdomMessage = false; + + double dist = -1; + if (baseTrajectory_.poses.empty()) + { + enqueueOdomMessage = true; + } + else + { + const auto & prevPose = baseTrajectory_.poses.back().pose; + const geometry_msgs::msg::Point & prevPoint = prevPose.position; + double prevAngle = tf2::getYaw(prevPose.orientation); + + const geometry_msgs::msg::Point & currePoint = base_pose.pose.position; + double currentAngle = tf2::getYaw(base_pose.pose.orientation); + + dist = p2pDistance(prevPoint, currePoint); + double goalAngleOffset = fabs(angles::shortest_angular_distance(prevAngle, currentAngle)); + + // RCLCPP_WARN(getLogger(),"dist %lf vs min %lf", dist, recordPointDistanceThreshold_); + + RCLCPP_WARN_THROTTLE(getLogger(), *getNode()->get_clock(), 2000, "odom received"); + + if (dist > recordPointDistanceThreshold_ || goalAngleOffset > recordAngularDistanceThreshold_) + { + enqueueOdomMessage = true; + } + else + { + // RCLCPP_WARN(getLogger(),"skip odom, dist: %lf", dist); + enqueueOdomMessage = false; + } + } + + if (enqueueOdomMessage) + { + RCLCPP_WARN_THROTTLE( + getLogger(), *getNode()->get_clock(), 2000, "enqueue odom tracker pose. dist %lf vs min %lf", + dist, recordPointDistanceThreshold_); + baseTrajectory_.poses.push_back(base_pose); + } + + return enqueueOdomMessage; +} + +/** + ****************************************************************************************************************** + * reconfigCB() + ****************************************************************************************************************** + */ +void CpOdomTracker::updateConfiguration() +{ + if (!getNode()->get_parameter("odom_frame", this->odomFrame_)) + { + } + + if (!getNode()->get_parameter("record_point_distance_threshold", recordPointDistanceThreshold_)) + { + } + + if (!getNode()->get_parameter( + "record_angular_distance_threshold", recordAngularDistanceThreshold_)) + { + } + + if (!getNode()->get_parameter("clear_point_distance_threshold", clearPointDistanceThreshold_)) + { + } + + if (!getNode()->get_parameter("clear_angular_distance_threshold", clearAngularDistanceThreshold_)) + { + } +} + +/** + ****************************************************************************************************************** + * processOdometryMessage() + ****************************************************************************************************************** + */ +void CpOdomTracker::processOdometryMessage(const nav_msgs::msg::Odometry::SharedPtr odom) +{ + RCLCPP_INFO_THROTTLE( + getLogger(), *getNode()->get_clock(), 5000, + "[odom_tracker] processing odom msg update heartbeat"); + std::lock_guard lock(m_mutex_); + + updateConfiguration(); + + if (workingMode_ == WorkingMode::RECORD_PATH) + { + updateRecordPath(*odom); + } + else if (workingMode_ == WorkingMode::CLEAR_PATH) + { + updateClearPath(*odom); + } + + // RCLCPP_WARN(getLogger(),"odomTracker odometry callback"); + if (publishMessages) + { + rtPublishPaths(odom->header.stamp); + } + + // RCLCPP_INFO(getLogger(),"odom_tracker m_mutex release"); +} +} // namespace odom_tracker +} // namespace cl_nitrosz diff --git a/smacc2_client_library/nitrosz_client/nitrosz_client/src/nitrosz_client/components/odom_tracker/cp_odom_tracker_node.cpp b/smacc2_client_library/nitrosz_client/nitrosz_client/src/nitrosz_client/components/odom_tracker/cp_odom_tracker_node.cpp new file mode 100644 index 000000000..fb788a460 --- /dev/null +++ b/smacc2_client_library/nitrosz_client/nitrosz_client/src/nitrosz_client/components/odom_tracker/cp_odom_tracker_node.cpp @@ -0,0 +1,123 @@ +// Copyright 2021 RobosoftAI Inc. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +/***************************************************************************************************************** + * + * Authors: Pablo Inigo Blasco, Brett Aldrich + * + ******************************************************************************************************************/ +//#include +#include +#include +#include + +typedef actionlib::SimpleActionServer Server; + +using namespace odom_tracker; +using namespace cl_nitrosz::odom_tracker; + +class CpOdomTrackerActionServer +{ +public: + std::shared_ptr as_; + CpOdomTracker odomTracker; + + CpOdomTrackerActionServer() : odomTracker("move_base") {} + + /** +****************************************************************************************************************** +* execute() +****************************************************************************************************************** +*/ + void execute( + const CpOdomTrackerGoalConstPtr & goal) // Note: "Action" is not appended to DoDishes here + { + try + { + switch (goal->command) + { + case CpOdomTrackerGoal::RECORD_PATH: + odomTracker.setWorkingMode(WorkingMode::RECORD_PATH); + break; + + case CpOdomTrackerGoal::CLEAR_PATH: + odomTracker.setWorkingMode(WorkingMode::CLEAR_PATH); + break; + + case CpOdomTrackerGoal::IDLE: + odomTracker.setWorkingMode(WorkingMode::IDLE); + break; + + case CpOdomTrackerGoal::START_BROADCAST_PATH: + odomTracker.setPublishMessages(true); + break; + + case CpOdomTrackerGoal::STOP_BROADCAST_PATH: + odomTracker.setPublishMessages(false); + break; + + case CpOdomTrackerGoal::PUSH_PATH: + odomTracker.pushPath(); + break; + + case CpOdomTrackerGoal::POP_PATH: + odomTracker.popPath(); + break; + + default: + + RCLCPP_ERROR( + getLogger(), "Odom Tracker Node - Action Server execute error: incorrect command - %d", + goal->command); + as_->setAborted(); + } + + // never reach succeeded because were are interested in keeping the feedback alive + as_->setSucceeded(); + } + catch (std::exception & ex) + { + RCLCPP_ERROR(getLogger(), "Odom Tracker Node - Action Server execute error: %s", ex.what()); + as_->setAborted(); + } + } + + /** +****************************************************************************************************************** +* run() +****************************************************************************************************************** +*/ + void run() + { + rclcpp::Node::SharedPtr n; + RCLCPP_INFO(getLogger(), "Creating odom tracker action server"); + + as_ = std::make_shared( + n, "odom_tracker", + std::bind(&CpOdomTrackerActionServer::execute, this, std::placeholders::_1), false); + RCLCPP_INFO(getLogger(), "Starting CpOdomTracker Action Server"); + + as_->start(); + + ros::spin(); + } +}; + +int main(int argc, char ** argv) +{ + ros::init(argc, argv, "odom_tracker_node"); + CpOdomTrackerActionServer as; + + as.run(); +} diff --git a/smacc2_client_library/nitrosz_client/nitrosz_client/src/nitrosz_client/components/planner_switcher/cp_planner_switcher.cpp b/smacc2_client_library/nitrosz_client/nitrosz_client/src/nitrosz_client/components/planner_switcher/cp_planner_switcher.cpp new file mode 100644 index 000000000..8178cfb56 --- /dev/null +++ b/smacc2_client_library/nitrosz_client/nitrosz_client/src/nitrosz_client/components/planner_switcher/cp_planner_switcher.cpp @@ -0,0 +1,109 @@ +// Copyright 2021 RobosoftAI Inc. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +/***************************************************************************************************************** + * + * Authors: Pablo Inigo Blasco, Brett Aldrich + * + ******************************************************************************************************************/ +#include +#include + +namespace cl_nitrosz +{ +using namespace std::chrono_literals; + +CpPlannerSwitcher::CpPlannerSwitcher() {} + +void CpPlannerSwitcher::onInitialize() +{ + rclcpp::QoS qos(rclcpp::KeepLast(1)); + qos.transient_local().reliable(); + + this->planner_selector_pub_ = + getNode()->create_publisher("planner_selector", qos); + this->controller_selector_pub_ = + getNode()->create_publisher("controller_selector", qos); +} + +void CpPlannerSwitcher::setDesiredGlobalPlanner(std::string plannerName) +{ + desired_planner_ = plannerName; +} + +void CpPlannerSwitcher::setDesiredController(std::string controllerName) +{ + desired_controller_ = controllerName; +} + +void CpPlannerSwitcher::setUndoPathBackwardPlanner(bool commit) +{ + RCLCPP_INFO(getLogger(), "[CpPlannerSwitcher] Planner Switcher: Trying to set BackwardPlanner"); + + desired_planner_ = "UndoPathGlobalPlanner"; + desired_controller_ = "BackwardLocalPlanner"; + + if (commit) commitPublish(); +} + +void CpPlannerSwitcher::setBackwardPlanner(bool commit) +{ + RCLCPP_INFO(getLogger(), "[CpPlannerSwitcher] Planner Switcher: Trying to set BackwardPlanner"); + + desired_planner_ = "BackwardGlobalPlanner"; + desired_controller_ = "BackwardLocalPlanner"; + + if (commit) commitPublish(); +} + +void CpPlannerSwitcher::setForwardPlanner(bool commit) +{ + RCLCPP_INFO(getLogger(), "[CpPlannerSwitcher] Planner Switcher: Trying to set ForwardPlanner"); + + desired_planner_ = "ForwardGlobalPlanner"; + desired_controller_ = "ForwardLocalPlanner"; + + if (commit) commitPublish(); +} + +void CpPlannerSwitcher::setPureSpinningPlanner(bool commit) +{ + RCLCPP_INFO( + getLogger(), "[CpPlannerSwitcher] Planner Switcher: Trying to set PureSpinningPlanner"); + + desired_planner_ = "ForwardGlobalPlanner"; + desired_controller_ = "PureSpinningLocalPlanner"; + + if (commit) commitPublish(); +} + +void CpPlannerSwitcher::setDefaultPlanners(bool commit) +{ + desired_planner_ = "GridBased"; + desired_controller_ = "FollowPath"; + + if (commit) commitPublish(); +} + +void CpPlannerSwitcher::commitPublish() +{ + std_msgs::msg::String planner_msg; + planner_msg.data = desired_planner_; + this->planner_selector_pub_->publish(planner_msg); + + std_msgs::msg::String controller_msg; + controller_msg.data = desired_controller_; + this->controller_selector_pub_->publish(controller_msg); +} +} // namespace cl_nitrosz diff --git a/smacc2_client_library/nitrosz_client/nitrosz_client/src/nitrosz_client/components/pose/cp_pose.cpp b/smacc2_client_library/nitrosz_client/nitrosz_client/src/nitrosz_client/components/pose/cp_pose.cpp new file mode 100644 index 000000000..efa964a36 --- /dev/null +++ b/smacc2_client_library/nitrosz_client/nitrosz_client/src/nitrosz_client/components/pose/cp_pose.cpp @@ -0,0 +1,153 @@ +// Copyright 2021 RobosoftAI Inc. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +/***************************************************************************************************************** + * + * Authors: Pablo Inigo Blasco, Brett Aldrich + * + ******************************************************************************************************************/ + +#include + +#include +#include + +namespace cl_nitrosz +{ +using namespace std::chrono_literals; + +// static +std::shared_ptr Pose::tfListener_; +std::shared_ptr Pose::tfBuffer_; +std::mutex Pose::listenerMutex_; + +Pose::Pose(std::string targetFrame, std::string referenceFrame) +: isInitialized(false), poseFrameName_(targetFrame), referenceFrame_(referenceFrame) + +{ + this->pose_.header.frame_id = referenceFrame_; +} + +std::string referenceFrameToString(StandardReferenceFrames referenceFrame) +{ + switch (referenceFrame) + { + case StandardReferenceFrames::Map: + return "map"; + case StandardReferenceFrames::Odometry: + return "odom"; + default: + return "odom"; + } +} + +Pose::Pose(StandardReferenceFrames referenceFrame) +: Pose("base_link", referenceFrameToString(referenceFrame)) +{ +} + +void Pose::onInitialize() +{ + RCLCPP_INFO( + getLogger(), "[Pose] Creating Pose tracker component to track %s in the reference frame %s", + poseFrameName_.c_str(), referenceFrame_.c_str()); + + { + // singleton + std::lock_guard guard(listenerMutex_); + if (tfListener_ == nullptr) + { + tfBuffer_ = std::make_shared(getNode()->get_clock()); + tfListener_ = std::make_shared(*tfBuffer_); + } + } +} + +void Pose::waitTransformUpdate(rclcpp::Rate r) +{ + bool found = false; + RCLCPP_INFO(getLogger(), "[Pose Component] waitTransformUpdate"); + while (rclcpp::ok() && !found) + { + tf2::Stamped transform; + try + { + { + RCLCPP_INFO_THROTTLE( + getLogger(), *(getNode()->get_clock()), 1000, + "[Pose Component] waiting transform %s -> %s", referenceFrame_.c_str(), + poseFrameName_.c_str()); + std::lock_guard lock(listenerMutex_); + auto transformstamped = + tfBuffer_->lookupTransform(referenceFrame_, poseFrameName_, getNode()->now()); + tf2::fromMsg(transformstamped, transform); + } + + { + std::lock_guard guard(m_mutex_); + tf2::toMsg(transform, this->pose_.pose); + this->pose_.header.stamp = tf2_ros::toRclcpp(transform.stamp_); + found = true; + this->isInitialized = true; + } + } + catch (tf2::TransformException & ex) + { + RCLCPP_ERROR_STREAM_THROTTLE( + getLogger(), *(getNode()->get_clock()), 1000, + "[Component pose] (" << poseFrameName_ << "/[" << referenceFrame_ + << "] ) is failing on pose update : " << ex.what()); + } + + r.sleep(); + } + RCLCPP_INFO(getLogger(), "[Pose Component] waitTransformUpdate -> pose found!"); +} + +float Pose::getYaw() { return tf2::getYaw(pose_.pose.orientation); } + +float Pose::getX() { return pose_.pose.position.x; } +float Pose::getY() { return pose_.pose.position.y; } +float Pose::getZ() { return pose_.pose.position.z; } + +void Pose::update() +{ + tf2::Stamped transform; + try + { + { + std::lock_guard lock(listenerMutex_); + RCLCPP_DEBUG(getLogger(), "[pose] looking up transform"); + auto transformstamped = + tfBuffer_->lookupTransform(referenceFrame_, poseFrameName_, rclcpp::Time(0)); + tf2::fromMsg(transformstamped, transform); + } + + { + std::lock_guard guard(m_mutex_); + tf2::toMsg(transform, this->pose_.pose); + this->pose_.header.stamp = tf2_ros::toRclcpp(transform.stamp_); + this->isInitialized = true; + } + } + catch (tf2::TransformException & ex) + { + // RCLCPP_DEBUG(getLogger(), "[pose] EXCEPTION"); + RCLCPP_ERROR_STREAM_THROTTLE( + getLogger(), *(getNode()->get_clock()), 1000, + "[Component pose] (" << poseFrameName_ << "/[" << referenceFrame_ + << "] ) is failing on pose update : " << ex.what()); + } +} +} // namespace cl_nitrosz diff --git a/smacc2_client_library/nitrosz_client/nitrosz_client/src/nitrosz_client/components/slam_toolbox/cp_slam_toolbox.cpp b/smacc2_client_library/nitrosz_client/nitrosz_client/src/nitrosz_client/components/slam_toolbox/cp_slam_toolbox.cpp new file mode 100644 index 000000000..1d72ac6f2 --- /dev/null +++ b/smacc2_client_library/nitrosz_client/nitrosz_client/src/nitrosz_client/components/slam_toolbox/cp_slam_toolbox.cpp @@ -0,0 +1,40 @@ +// Copyright 2021 RobosoftAI Inc. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +/***************************************************************************************************************** + * + * Authors: Pablo Inigo Blasco, Brett Aldrich + * + ******************************************************************************************************************/ + +#include + +namespace cl_nitrosz +{ +CpSlamToolbox::CpSlamToolbox() : state_(SlamToolboxState::Resumed) {} +CpSlamToolbox::~CpSlamToolbox() {} + +void CpSlamToolbox::toogleState() +{ + if (state_ == SlamToolboxState::Paused) + { + state_ = SlamToolboxState::Resumed; + } + else + { + state_ = SlamToolboxState::Paused; + } +} + +} // namespace cl_nitrosz diff --git a/smacc2_client_library/nitrosz_client/nitrosz_client/src/nitrosz_client/components/waypoints_navigator/cp_waypoints_event_dispatcher.cpp b/smacc2_client_library/nitrosz_client/nitrosz_client/src/nitrosz_client/components/waypoints_navigator/cp_waypoints_event_dispatcher.cpp new file mode 100644 index 000000000..ac13a4482 --- /dev/null +++ b/smacc2_client_library/nitrosz_client/nitrosz_client/src/nitrosz_client/components/waypoints_navigator/cp_waypoints_event_dispatcher.cpp @@ -0,0 +1,30 @@ +// Copyright 2021 RobosoftAI Inc. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +/***************************************************************************************************************** + * + * Authors: Pablo Inigo Blasco, Brett Aldrich + * + ******************************************************************************************************************/ + +#include + +namespace cl_nitrosz +{ +void WaypointEventDispatcher::postWaypointEvent(int index) +{ + auto & fn = postWaypointFn[index % WAYPOINTS_EVENTCOUNT]; + if (fn != nullptr) fn(); +} +} // namespace cl_nitrosz diff --git a/smacc2_client_library/nitrosz_client/nitrosz_client/src/nitrosz_client/components/waypoints_navigator/cp_waypoints_navigator.cpp b/smacc2_client_library/nitrosz_client/nitrosz_client/src/nitrosz_client/components/waypoints_navigator/cp_waypoints_navigator.cpp new file mode 100644 index 000000000..87adf674b --- /dev/null +++ b/smacc2_client_library/nitrosz_client/nitrosz_client/src/nitrosz_client/components/waypoints_navigator/cp_waypoints_navigator.cpp @@ -0,0 +1,566 @@ +// Copyright 2021 RobosoftAI Inc. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +/***************************************************************************************************************** + * + * Authors: Pablo Inigo Blasco, Brett Aldrich + * + ******************************************************************************************************************/ + +#include +#include + +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + +namespace cl_nitrosz +{ +using namespace std::chrono_literals; +CpWaypointNavigatorBase::CpWaypointNavigatorBase() : currentWaypoint_(0), waypoints_(0) {} + +CpWaypointNavigatorBase::~CpWaypointNavigatorBase() {} + +CpWaypointNavigator::CpWaypointNavigator() {} + +void CpWaypointNavigatorBase::onInitialize() {} + +void CpWaypointNavigator::onInitialize() { client_ = dynamic_cast(owner_); } + +void CpWaypointNavigator::onGoalCancelled(const ClNitrosZ::WrappedResult & /*res*/) +{ + stopWaitingResult(); + + this->onNavigationRequestCancelled(); +} + +void CpWaypointNavigator::onGoalAborted(const ClNitrosZ::WrappedResult & /*res*/) +{ + stopWaitingResult(); + + this->onNavigationRequestAborted(); +} + +void CpWaypointNavigator::onGoalReached(const ClNitrosZ::WrappedResult & /*res*/) +{ + waypointsEventDispatcher.postWaypointEvent(currentWaypoint_); + currentWaypoint_++; + RCLCPP_WARN( + getLogger(), "[CpWaypointNavigator] Goal result received, incrementing waypoint index: %ld", + currentWaypoint_); + stopWaitingResult(); + + this->notifyGoalReached(); + + onNavigationRequestSucceded(); +} + +void CpWaypointNavigatorBase::rewind(int /*count*/) +{ + currentWaypoint_--; + if (currentWaypoint_ < 0) currentWaypoint_ = 0; +} + +void CpWaypointNavigatorBase::forward(int /*count*/) +{ + currentWaypoint_++; + if (currentWaypoint_ >= (long)waypoints_.size() - 1) + currentWaypoint_ = (long)waypoints_.size() - 1; +} + +void CpWaypointNavigatorBase::seekName(std::string name) +{ + bool found = false; + + auto previousWaypoint = currentWaypoint_; + + while (!found && currentWaypoint_ < (long)waypoints_.size()) + { + auto & nextName = waypointsNames_[currentWaypoint_]; + RCLCPP_INFO( + getLogger(), "[CpWaypointNavigator] seeking ,%ld/%ld candidate waypoint: %s", + currentWaypoint_, waypoints_.size(), nextName.c_str()); + if (name == nextName) + { + found = true; + RCLCPP_INFO( + getLogger(), "[CpWaypointNavigator] found target waypoint: %s == %s-> found", + nextName.c_str(), name.c_str()); + } + else + { + RCLCPP_INFO( + getLogger(), "[CpWaypointNavigator] current waypoint: %s != %s -> forward", + nextName.c_str(), name.c_str()); + currentWaypoint_++; + } + } + + if (found) + { + if (currentWaypoint_ >= (long)waypoints_.size() - 1) + currentWaypoint_ = (long)waypoints_.size() - 1; + } + else // search backwards + { + currentWaypoint_ = previousWaypoint; + while (!found && currentWaypoint_ > 0) + { + auto & nextName = waypointsNames_[currentWaypoint_]; + RCLCPP_INFO( + getLogger(), "[CpWaypointNavigator] seeking , candidate waypoint: %s", nextName.c_str()); + if (name == nextName) + { + found = true; + RCLCPP_INFO( + getLogger(), "[CpWaypointNavigator] found target waypoint: %s == %s-> found", + nextName.c_str(), name.c_str()); + } + else + { + RCLCPP_INFO( + getLogger(), "[CpWaypointNavigator] current waypoint: %s != %s -> rewind", + nextName.c_str(), name.c_str()); + currentWaypoint_--; + } + } + } + + RCLCPP_INFO( + getLogger(), "[CpWaypointNavigator] seekName( %s), previous index: %ld, after index: %ld", + name.c_str(), previousWaypoint, currentWaypoint_); +} + +void CpWaypointNavigatorBase::loadWaypointsFromYamlParameter( + std::string parameter_name, std::string yaml_file_package_name) +{ + // if it is the first time and the waypoints navigator is not configured + std::string planfilepath; + planfilepath = getNode()->declare_parameter(parameter_name, planfilepath); + RCLCPP_INFO(getLogger(), "waypoints plan parameter: %s", planfilepath.c_str()); + if (getNode()->get_parameter(parameter_name, planfilepath)) + { + std::string package_share_directory = + ament_index_cpp::get_package_share_directory(yaml_file_package_name); + + RCLCPP_INFO(getLogger(), "file macro path: %s", planfilepath.c_str()); + + boost::replace_all(planfilepath, "$(pkg_share)", package_share_directory); + + RCLCPP_INFO(getLogger(), "package share path: %s", package_share_directory.c_str()); + RCLCPP_INFO(getLogger(), "waypoints plan file: %s", planfilepath.c_str()); + + this->loadWayPointsFromFile(planfilepath); + RCLCPP_INFO(getLogger(), "waypoints plan: %s", planfilepath.c_str()); + } + else + { + RCLCPP_ERROR(getLogger(), "waypoints plan file not found: NONE"); + } +} + +void CpWaypointNavigator::stopWaitingResult() +{ + if (succeddedNav2ZClientConnection_.connected()) + { + this->succeddedNav2ZClientConnection_.disconnect(); + this->cancelledNav2ZClientConnection_.disconnect(); + this->abortedNav2ZClientConnection_.disconnect(); + } +} + +std::optional>>> +CpWaypointNavigator::sendNextGoal( + std::optional options, + cl_nitrosz::ClNitrosZ::SmaccNavigateResultSignal::WeakPtr resultCallback) +{ + if (currentWaypoint_ >= 0 && currentWaypoint_ < (int)waypoints_.size()) + { + auto & next = waypoints_[currentWaypoint_]; + + std::string nextName; + if ((long)waypointsNames_.size() > currentWaypoint_) + { + nextName = waypointsNames_[currentWaypoint_]; + RCLCPP_INFO( + getLogger(), "[CpWaypointNavigator] sending goal, waypoint: %s", nextName.c_str()); + } + else + { + RCLCPP_INFO( + getLogger(), "[CpWaypointNavigator] sending goal, waypoint: %ld", currentWaypoint_); + } + + ClNitrosZ::Goal goal; + auto p = client_->getComponent(); + auto pose = p->toPoseMsg(); + + // configuring goal + goal.pose.header.frame_id = p->getReferenceFrame(); + //goal.pose.header.stamp = getNode()->now(); + goal.pose.pose = next; + + auto plannerSwitcher = client_->getComponent(); + plannerSwitcher->setDefaultPlanners(false); + if (options && options->controllerName_) + { + RCLCPP_WARN( + getLogger(), "[WaypointsNavigator] override controller: %s", + options->controllerName_->c_str()); + + plannerSwitcher->setDesiredController(*options->controllerName_); + } + else + { + RCLCPP_WARN(getLogger(), "[WaypointsNavigator] Configuring default planners"); + } + + auto goalCheckerSwitcher = client_->getComponent(); + + if (options && options->goalCheckerName_) + { + RCLCPP_WARN( + getLogger(), "[WaypointsNavigator] override goal checker: %s", + options->goalCheckerName_->c_str()); + + goalCheckerSwitcher->setGoalCheckerId(*options->goalCheckerName_); + } + else + { + RCLCPP_WARN(getLogger(), "[WaypointsNavigator] Configuring default goal checker"); + goalCheckerSwitcher->setGoalCheckerId("goal_checker"); + } + + plannerSwitcher->commitPublish(); + + // publish stuff + // rclcpp::sleep_for(5s); + + RCLCPP_INFO(getLogger(), "[WaypointsNavigator] Getting odom tracker"); + auto odomTracker = client_->getComponent(); + if (odomTracker != nullptr) + { + RCLCPP_INFO(getLogger(), "[WaypointsNavigator] Storing path in odom tracker"); + + auto pathname = this->owner_->getStateMachine()->getCurrentState()->getName() + " - " + + getName() + " - " + nextName; + odomTracker->pushPath(pathname); + odomTracker->setStartPoint(pose); + odomTracker->setWorkingMode(cl_nitrosz::odom_tracker::WorkingMode::RECORD_PATH); + } + + // SEND GOAL + // if (!succeddedNav2ZClientConnection_.connected()) + // { + // this->succeddedNav2ZClientConnection_ = + // client_->onSucceeded(&WaypointNavigator::onGoalReached, this); + // this->cancelledNav2ZClientConnection_ = + // client_->onAborted(&WaypointNavigator::onGoalCancelled, this); + // this->abortedNav2ZClientConnection_ = + // client_->onCancelled(&WaypointNavigator::onGoalAborted, this); + // } + + auto callbackptr = resultCallback.lock(); + succeddedNav2ZClientConnection_ = this->getStateMachine()->createSignalConnection( + *callbackptr, &CpWaypointNavigator::onNavigationResult, this); + + return client_->sendGoal(goal, resultCallback); + } + else + { + RCLCPP_WARN( + getLogger(), + "[CpWaypointsNavigator] All waypoints were consumed. There is no more waypoints available."); + } + + return std::nullopt; +} + +void CpWaypointNavigatorBase::notifyGoalReached() +{ + // when it is the last waypoint post an finalization EOF event + if (currentWaypoint_ == (long)waypoints_.size() - 1) + { + RCLCPP_WARN(getLogger(), "[CpWaypointNavigator] Last waypoint reached, posting EOF event. "); + this->postEvent(); + } +} + +void CpWaypointNavigator::onNavigationResult(const ClNitrosZ::WrappedResult & r) +{ + if (r.code == rclcpp_action::ResultCode::SUCCEEDED) + { + this->onGoalReached(r); + } + else if (r.code == rclcpp_action::ResultCode::ABORTED) + { + this->onGoalAborted(r); + } + else if (r.code == rclcpp_action::ResultCode::CANCELED) + { + this->onGoalCancelled(r); + } + else + { + this->onGoalAborted(r); + } +} + +void CpWaypointNavigatorBase::insertWaypoint(int index, geometry_msgs::msg::Pose & newpose) +{ + if (index >= 0 && index <= (int)waypoints_.size()) + { + waypoints_.insert(waypoints_.begin(), index, newpose); + } +} + +void CpWaypointNavigatorBase::setWaypoints(const std::vector & waypoints) +{ + this->waypoints_ = waypoints; +} + +void CpWaypointNavigatorBase::setWaypoints(const std::vector & waypoints) +{ + waypoints_.clear(); + waypointsNames_.clear(); + int i = 0; + for (auto & p : waypoints) + { + geometry_msgs::msg::Pose pose; + pose.position.x = p.x_; + pose.position.y = p.y_; + pose.position.z = 0.0; + tf2::Quaternion q; + q.setRPY(0, 0, p.yaw_); + pose.orientation = tf2::toMsg(q); + + waypoints_.push_back(pose); + waypointsNames_.push_back(std::to_string(i++)); + } +} + +void CpWaypointNavigatorBase::removeWaypoint(int index) +{ + if (index >= 0 && index < (int)waypoints_.size()) + { + waypoints_.erase(waypoints_.begin() + index); + } +} + +const std::vector & CpWaypointNavigatorBase::getWaypoints() const +{ + return waypoints_; +} + +geometry_msgs::msg::Pose CpWaypointNavigatorBase::getPose(int index) const +{ + if (index >= 0 && index < (int)waypoints_.size()) + { + return waypoints_[index]; + } + else + { + throw std::out_of_range("Waypoint index out of range"); + } +} +geometry_msgs::msg::Pose CpWaypointNavigatorBase::getCurrentPose() const +{ + if (currentWaypoint_ >= 0 && currentWaypoint_ < (int)waypoints_.size()) + { + return waypoints_[currentWaypoint_]; + } + else + { + throw std::out_of_range("Waypoint index out of range"); + } +} + +std::optional CpWaypointNavigatorBase::getNamedPose( + std::string name) const +{ + if (this->waypointsNames_.size() > 0) + { + for (int i = 0; i < (int)this->waypointsNames_.size(); i++) + { + if (this->waypointsNames_[i] == name) + { + return this->waypoints_[i]; + } + } + } + + return std::nullopt; +} + +const std::vector & CpWaypointNavigatorBase::getWaypointNames() const +{ + return waypointsNames_; +} + +std::optional CpWaypointNavigatorBase::getCurrentWaypointName() const +{ + if (currentWaypoint_ >= 0 && currentWaypoint_ < (int)waypointsNames_.size()) + { + return waypointsNames_[currentWaypoint_]; + } + return std::nullopt; +} + +long CpWaypointNavigatorBase::getCurrentWaypointIndex() const { return currentWaypoint_; } + +#define HAVE_NEW_YAMLCPP +void CpWaypointNavigatorBase::loadWayPointsFromFile(std::string filepath) +{ + RCLCPP_INFO_STREAM(getLogger(), "[CpWaypointNavigatorBase] Loading file:" << filepath); + this->waypoints_.clear(); + std::ifstream ifs(filepath.c_str(), std::ifstream::in); + if (ifs.good() == false) + { + throw std::string("Waypoints file not found"); + } + + try + { +#ifdef HAVE_NEW_YAMLCPP + YAML::Node node = YAML::Load(ifs); +#else + YAML::Parser parser(ifs); + parser.GetNextDocument(node); +#endif + +#ifdef HAVE_NEW_YAMLCPP + const YAML::Node & wp_node_tmp = node["waypoints"]; + const YAML::Node * wp_node = wp_node_tmp ? &wp_node_tmp : NULL; +#else + const YAML::Node * wp_node = node.FindValue("waypoints"); +#endif + + if (wp_node != NULL) + { + for (int64_t i = 0; i < wp_node->size(); ++i) + { + // Parse waypoint entries on YAML + geometry_msgs::msg::Pose wp; + + try + { + // (*wp_node)[i]["name"] >> wp.name; + // (*wp_node)[i]["frame_id"] >> wp.header.frame_id; + + auto wpnodei = (*wp_node)[i]; + wp.position.x = wpnodei["position"]["x"].as(); + wp.position.y = wpnodei["position"]["y"].as(); + wp.position.z = wpnodei["position"]["z"].as(); + wp.orientation.x = wpnodei["orientation"]["x"].as(); + wp.orientation.y = wpnodei["orientation"]["y"].as(); + wp.orientation.z = wpnodei["orientation"]["z"].as(); + wp.orientation.w = wpnodei["orientation"]["w"].as(); + + if (wpnodei["name"].IsDefined()) + { + this->waypointsNames_.push_back(wpnodei["name"].as()); + } + + this->waypoints_.push_back(wp); + } + catch (...) + { + RCLCPP_ERROR(getLogger(), "parsing waypoint file, syntax error in point %ld", i); + } + } + RCLCPP_INFO_STREAM(getLogger(), "Parsed " << this->waypoints_.size() << " waypoints."); + } + else + { + RCLCPP_WARN_STREAM(getLogger(), "Couldn't find any waypoints in the provided yaml file."); + } + } + catch (const YAML::ParserException & ex) + { + RCLCPP_ERROR_STREAM( + getLogger(), "Error loading the Waypoints YAML file. Incorrect syntax: " << ex.what()); + } +} + +void CpWaypointNavigatorBase::loadWayPointsFromFile2(std::string filepath) +{ + RCLCPP_INFO_STREAM(getLogger(), "[CpWaypointNavigator] Loading file:" << filepath); + this->waypoints_.clear(); + std::ifstream ifs(filepath.c_str(), std::ifstream::in); + if (ifs.good() == false) + { + throw std::string("Waypoints file not found"); + } + + try + { +#ifdef HAVE_NEW_YAMLCPP + YAML::Node node = YAML::Load(ifs); +#else + YAML::Parser parser(ifs); + parser.GetNextDocument(node); +#endif + +#ifdef HAVE_NEW_YAMLCPP + const YAML::Node & wp_node_tmp = node["waypoints"]; + const YAML::Node * wp_node = wp_node_tmp ? &wp_node_tmp : NULL; +#else + const YAML::Node * wp_node = node.FindValue("waypoints"); +#endif + + if (wp_node != NULL) + { + for (int64_t i = 0; i < wp_node->size(); ++i) + { + // Parse waypoint entries on YAML + geometry_msgs::msg::Pose wp; + + try + { + // (*wp_node)[i]["name"] >> wp.name; + // (*wp_node)[i]["frame_id"] >> wp.header.frame_id; + wp.position.x = (*wp_node)[i]["x"].as(); + wp.position.y = (*wp_node)[i]["y"].as(); + auto name = (*wp_node)[i]["name"].as(); + + this->waypoints_.push_back(wp); + this->waypointsNames_.push_back(name); + } + catch (...) + { + RCLCPP_ERROR(getLogger(), "parsing waypoint file, syntax error in point %ld", i); + } + } + RCLCPP_INFO_STREAM(getLogger(), "Parsed " << this->waypoints_.size() << " waypoints."); + } + else + { + RCLCPP_WARN_STREAM(getLogger(), "Couldn't find any waypoints in the provided yaml file."); + } + } + catch (const YAML::ParserException & ex) + { + RCLCPP_ERROR_STREAM( + getLogger(), "Error loading the Waypoints YAML file. Incorrect syntax: " << ex.what()); + } +} +} // namespace cl_nitrosz diff --git a/smacc2_client_library/nitrosz_client/nitrosz_client/src/nitrosz_client/components/waypoints_navigator/cp_waypoints_visualizer.cpp b/smacc2_client_library/nitrosz_client/nitrosz_client/src/nitrosz_client/components/waypoints_navigator/cp_waypoints_visualizer.cpp new file mode 100644 index 000000000..964df0f70 --- /dev/null +++ b/smacc2_client_library/nitrosz_client/nitrosz_client/src/nitrosz_client/components/waypoints_navigator/cp_waypoints_visualizer.cpp @@ -0,0 +1,156 @@ +// Copyright 2021 RobosoftAI Inc. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +/***************************************************************************************************************** + * + * Authors: Pablo Inigo Blasco, Brett Aldrich + * + ******************************************************************************************************************/ + +#include + +namespace cl_nitrosz +{ +const std::string frameid = "map"; + +CpWaypointsVisualizer::CpWaypointsVisualizer(rclcpp::Duration duration) : ISmaccUpdatable(duration) +{ +} + +void CpWaypointsVisualizer::onInitialize() +{ + markersPub_ = getNode()->create_publisher( + "cp_waypoints_visualizer/visualization_markers", rclcpp::QoS(rclcpp::KeepLast(1))); + + this->requiresComponent(waypointsNavigator_); + auto & waypoints = waypointsNavigator_->getWaypoints(); + auto & waypointsNames = waypointsNavigator_->getWaypointNames(); + + int i = 0; + for (auto & waypoint : waypoints) + { + std::string name; + if ((long)waypointsNames.size() > i) + { + name = waypointsNames[i]; + } + else + { + name = "waypoint_" + std::to_string(i); + } + + visualization_msgs::msg::Marker marker; + createMarker(waypoint, marker); + markers_.markers.push_back(marker); + + visualization_msgs::msg::Marker markerlabel; + createMarkerLabel(waypoint, name, markerlabel); + markerLabels_.markers.push_back(markerlabel); + + i++; + } +} + +void CpWaypointsVisualizer::createMarkerLabel( + const geometry_msgs::msg::Pose & waypoint, std::string label, + visualization_msgs::msg::Marker & marker) +{ + marker.header.frame_id = frameid; + marker.header.stamp = getNode()->now(); + marker.ns = "waypoints_labels"; + + marker.id = markers_.markers.size(); + marker.type = visualization_msgs::msg::Marker::TEXT_VIEW_FACING; + marker.action = visualization_msgs::msg::Marker::ADD; + marker.scale.x = 0.3; + marker.scale.y = 0.3; + marker.scale.z = 0.3; + marker.text = label; + + marker.color.a = 1.0; + marker.pose = waypoint; + marker.pose.position.z += 0.3; +} + +void CpWaypointsVisualizer::createMarker( + const geometry_msgs::msg::Pose & waypoint, visualization_msgs::msg::Marker & marker) +{ + marker.header.frame_id = frameid; + marker.header.stamp = getNode()->now(); + marker.ns = "waypoints"; + + marker.id = markers_.markers.size(); + marker.type = visualization_msgs::msg::Marker::SPHERE; + marker.action = visualization_msgs::msg::Marker::ADD; + marker.scale.x = 0.1; + marker.scale.y = 0.1; + marker.scale.z = 0.1; + + marker.color.a = 1.0; + marker.pose = waypoint; +} + +void CpWaypointsVisualizer::update() +{ + std::lock_guard guard(m_mutex_); + + auto index = waypointsNavigator_->getCurrentWaypointIndex(); + + int i = 0; + + for (auto & marker : markers_.markers) + { + marker.header.stamp = getNode()->now(); + + if (i >= index) + { + marker.color.r = 1.0; + marker.color.g = 0; + marker.color.b = 0; + } + else + { + marker.color.r = 0.0; + marker.color.g = 1.0; + marker.color.b = 0; + } + i++; + } + + i = 0; + for (auto & marker : markerLabels_.markers) + { + marker.header.stamp = getNode()->now(); + + if (i >= index) + { + marker.color.r = 1.0; + marker.color.g = 0; + marker.color.b = 0; + } + else + { + marker.color.r = 0.0; + marker.color.g = 1.0; + marker.color.b = 0; + } + i++; + } + + //markers_.header.stamp = getNode()->now(); + markersPub_->publish(markers_); + markersPub_->publish(markerLabels_); +} + +} // namespace cl_nitrosz diff --git a/smacc2_client_library/nitrosz_client/nitrosz_client/src/nitrosz_client/nitrosz_client.cpp b/smacc2_client_library/nitrosz_client/nitrosz_client/src/nitrosz_client/nitrosz_client.cpp new file mode 100644 index 000000000..da489b77b --- /dev/null +++ b/smacc2_client_library/nitrosz_client/nitrosz_client/src/nitrosz_client/nitrosz_client.cpp @@ -0,0 +1,39 @@ +// Copyright 2021 RobosoftAI Inc. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +/***************************************************************************************************************** + * + * Authors: Pablo Inigo Blasco, Brett Aldrich + * + ******************************************************************************************************************/ +#include + +#include + +#include + +namespace cl_nitrosz +{ +typedef smacc2::client_bases::SmaccActionClientBase Base; +typedef Base::WrappedResult WrappedResult; + +ClNitrosZ::ClNitrosZ(std::string moveBaseName) : Base(moveBaseName) +{ + // RCLCPP_INFO(getLogger(),"Smacc Move Base Action Client"); +} + +ClNitrosZ::~ClNitrosZ() {} +} // namespace cl_nitrosz + +PLUGINLIB_EXPORT_CLASS(cl_nitrosz::ClNitrosZ, smacc2::ISmaccClient) diff --git a/smacc2_sm_reference_library/sm_multi_panda_sim/sm_multi_panda_sim/README.md b/smacc2_sm_reference_library/sm_multi_panda_sim/sm_multi_panda_sim/README.md new file mode 100644 index 000000000..6827ba2bc --- /dev/null +++ b/smacc2_sm_reference_library/sm_multi_panda_sim/sm_multi_panda_sim/README.md @@ -0,0 +1,56 @@ +

Build Instructions

+ +First, source your ros2 installation. +``` +source /opt/ros/humble/setup.bash +``` + +Before you build, make sure you've installed all the dependencies... + +``` +rosdep install --ignore-src --from-paths src -y -r +``` + +Then build with colcon build... + +``` +colcon build +``` +

Operating Instructions

+ +After you built the project don't forget to setup your environment: First source the gazebo setup via + +``` +source /opt/ros/humble/share/gazebo_ros/local_setup.bash +``` +Then source the proper workspace... + +``` +source ~/workspace/humble_ws/install/setup.sh +``` + +And then run the launch file... + +``` +ros2 launch sm_multi_panda_sim sm_multi_panda_sim.launch.py +``` + +If the simulation is not starting up properly check if your GAZEBO_RESOURCE_PATH is set correctly (echo $GAZEBO_RESOURCE_PATH). Otherwise simply set it with: + +``` +export GAZEBO_RESOURCE_PATH=/usr/share/gazebo-/ +``` + +Note: Sometimes the Gazebo server is not shutdown correctly. If you cannot restart the simulation you might have to look if there is still a instance of the server (gzserver) running. You then have to end it manually. Or just run: +``` +killall -9 gzserver +``` + +

Viewer Instructions

+If you have the SMACC2 Runtime Analyzer installed then type... + +``` +ros2 run smacc2_rta smacc2_rta +``` + +If you don't have the SMACC2 Runtime Analyzer click here diff --git a/smacc2_sm_reference_library/sm_multi_panda_sim/sm_multi_panda_sim/include/sm_multi_panda_sim/sm_multi_panda_sim.hpp b/smacc2_sm_reference_library/sm_multi_panda_sim/sm_multi_panda_sim/include/sm_multi_panda_sim/sm_multi_panda_sim.hpp index 29e2cd42b..fde6b7e46 100644 --- a/smacc2_sm_reference_library/sm_multi_panda_sim/sm_multi_panda_sim/include/sm_multi_panda_sim/sm_multi_panda_sim.hpp +++ b/smacc2_sm_reference_library/sm_multi_panda_sim/sm_multi_panda_sim/include/sm_multi_panda_sim/sm_multi_panda_sim.hpp @@ -77,49 +77,17 @@ class StLeftArmMoves; class StRightArmMoves; class StBothArmsMove; -struct EvToDeep : sc::event -{ -}; - -struct EvFail : sc::event -{ -}; - -struct EvSc : sc::event -{ -}; - -struct EvStart : sc::event -{ -}; - -struct EvReset : sc::event -{ -}; - -struct EvSuspend : sc::event -{ -}; - -struct EvUnSuspend : sc::event -{ -}; - -struct EvHold : sc::event -{ -}; - -struct EvUnhold : sc::event -{ -}; - -struct EvStop : sc::event -{ -}; - -struct EvClear : sc::event -{ -}; +struct EvToDeep : sc::event{}; +struct EvFail : sc::event{}; +struct EvSc : sc::event{}; +struct EvStart : sc::event{}; +struct EvReset : sc::event{}; +struct EvSuspend : sc::event{}; +struct EvUnSuspend : sc::event{}; +struct EvHold : sc::event{}; +struct EvUnhold : sc::event{}; +struct EvStop : sc::event{}; +struct EvClear : sc::event{}; //-------------------------------------------------------------------- // STATE_MACHINE @@ -131,6 +99,7 @@ struct SmMultiPandaSim : public smacc2::SmaccStateMachineBasecreateOrthogonal(); this->createOrthogonal(); + this->createOrthogonal(); } }; diff --git a/smacc2_sm_reference_library/sm_multi_panda_sim/sm_multi_panda_sim/include/sm_multi_panda_sim/states/st_acquire_sensors.hpp b/smacc2_sm_reference_library/sm_multi_panda_sim/sm_multi_panda_sim/include/sm_multi_panda_sim/states/st_acquire_sensors.hpp index b78da909a..7d1eeb476 100644 --- a/smacc2_sm_reference_library/sm_multi_panda_sim/sm_multi_panda_sim/include/sm_multi_panda_sim/states/st_acquire_sensors.hpp +++ b/smacc2_sm_reference_library/sm_multi_panda_sim/sm_multi_panda_sim/include/sm_multi_panda_sim/states/st_acquire_sensors.hpp @@ -34,22 +34,30 @@ using namespace cl_moveit2z; using smacc2::client_behaviors::CbWaitTopicMessage; using namespace std::chrono_literals; using cl_moveit2z::CbWaitJointState; +using namespace cl_keyboard; // STATE DECLARATION struct StAcquireSensors : smacc2::SmaccState { using SmaccState::SmaccState; + // DECLARE CUSTOM OBJECT TAGS + struct NEXT : SUCCESS{}; + struct PREVIOUS : ABORT{}; + // TRANSITION TABLE typedef mpl::list< smacc2::Transition, - smacc2::Transition, StLeftArmMoves, SUCCESS> + smacc2::Transition, StLeftArmMoves, SUCCESS>, + + smacc2::Transition, StLeftArmMoves, NEXT> >reactions; // STATE FUNCTIONS static void staticConfigure() { configure_orthogonal(); + configure_orthogonal(); } void runtimeConfigure() {} diff --git a/smacc2_sm_reference_library/sm_multi_panda_sim/sm_multi_panda_sim/include/sm_multi_panda_sim/states/st_both_arms_move.hpp b/smacc2_sm_reference_library/sm_multi_panda_sim/sm_multi_panda_sim/include/sm_multi_panda_sim/states/st_both_arms_move.hpp index f3735d20e..e0664ce85 100644 --- a/smacc2_sm_reference_library/sm_multi_panda_sim/sm_multi_panda_sim/include/sm_multi_panda_sim/states/st_both_arms_move.hpp +++ b/smacc2_sm_reference_library/sm_multi_panda_sim/sm_multi_panda_sim/include/sm_multi_panda_sim/states/st_both_arms_move.hpp @@ -18,6 +18,7 @@ namespace sm_multi_panda_sim { using smacc2::client_behaviors::CbSequence; +using namespace cl_keyboard; // STATE DECLARATION struct StBothArmsMove : smacc2::SmaccState @@ -25,16 +26,15 @@ struct StBothArmsMove : smacc2::SmaccState using SmaccState::SmaccState; // DECLARE CUSTOM OBJECT TAGS - struct start_sequence_a : SUCCESS - { - }; - struct start_sequence_b : SUCCESS - { - }; + struct start_sequence_a : SUCCESS{}; + struct start_sequence_b : SUCCESS{}; + struct NEXT : SUCCESS{}; + struct PREVIOUS : ABORT{}; // TRANSITION TABLE typedef mpl::list< - smacc2::Transition, StLeftArmMoves, SUCCESS> + smacc2::Transition, StLeftArmMoves, SUCCESS>, + smacc2::Transition, StLeftArmMoves, NEXT> > reactions; @@ -58,6 +58,7 @@ struct StBothArmsMove : smacc2::SmaccState configure_orthogonal(rightJointValues); configure_orthogonal(leftJointValues); + configure_orthogonal(); // configure_orthogonal(15s); } diff --git a/smacc2_sm_reference_library/sm_multi_panda_sim/sm_multi_panda_sim/include/sm_multi_panda_sim/states/st_left_arm_moves.hpp b/smacc2_sm_reference_library/sm_multi_panda_sim/sm_multi_panda_sim/include/sm_multi_panda_sim/states/st_left_arm_moves.hpp index b635833ae..f8322e3f9 100644 --- a/smacc2_sm_reference_library/sm_multi_panda_sim/sm_multi_panda_sim/include/sm_multi_panda_sim/states/st_left_arm_moves.hpp +++ b/smacc2_sm_reference_library/sm_multi_panda_sim/sm_multi_panda_sim/include/sm_multi_panda_sim/states/st_left_arm_moves.hpp @@ -18,6 +18,7 @@ namespace sm_multi_panda_sim { using namespace std::chrono_literals; using smacc2::client_behaviors::CbSequence; +using namespace cl_keyboard; // STATE DECLARATION struct StLeftArmMoves : smacc2::SmaccState @@ -25,18 +26,18 @@ struct StLeftArmMoves : smacc2::SmaccState using SmaccState::SmaccState; // DECLARE CUSTOM OBJECT TAGS - struct start_sequence_a : SUCCESS - { - }; - struct start_sequence_b : SUCCESS - { - }; + struct start_sequence_a : SUCCESS{}; + struct start_sequence_b : SUCCESS{}; + struct NEXT : SUCCESS{}; + struct PREVIOUS : ABORT{}; // TRANSITION TABLE typedef mpl::list< - smacc2::Transition, StRightArmMoves, SUCCESS> + smacc2::Transition, StRightArmMoves, SUCCESS>, // smacc2::Transition, StRightArmMoves, SUCCESS> + smacc2::Transition, StRightArmMoves, NEXT> + > reactions; @@ -54,6 +55,7 @@ struct StLeftArmMoves : smacc2::SmaccState configure_orthogonal(leftJointValues); // configure_orthogonal(15s); + configure_orthogonal(); } void runtimeConfigure() diff --git a/smacc2_sm_reference_library/sm_multi_panda_sim/sm_multi_panda_sim/include/sm_multi_panda_sim/states/st_right_arm_moves.hpp b/smacc2_sm_reference_library/sm_multi_panda_sim/sm_multi_panda_sim/include/sm_multi_panda_sim/states/st_right_arm_moves.hpp index 1c164108b..e6a95be57 100644 --- a/smacc2_sm_reference_library/sm_multi_panda_sim/sm_multi_panda_sim/include/sm_multi_panda_sim/states/st_right_arm_moves.hpp +++ b/smacc2_sm_reference_library/sm_multi_panda_sim/sm_multi_panda_sim/include/sm_multi_panda_sim/states/st_right_arm_moves.hpp @@ -18,6 +18,7 @@ namespace sm_multi_panda_sim { using smacc2::client_behaviors::CbSequence; +using namespace cl_keyboard; // STATE DECLARATION struct StRightArmMoves : smacc2::SmaccState @@ -25,16 +26,15 @@ struct StRightArmMoves : smacc2::SmaccState using SmaccState::SmaccState; // DECLARE CUSTOM OBJECT TAGS - struct start_sequence_a : SUCCESS - { - }; - struct start_sequence_b : SUCCESS - { - }; + struct start_sequence_a : SUCCESS{}; + struct start_sequence_b : SUCCESS{}; + struct NEXT : SUCCESS{}; + struct PREVIOUS : ABORT{}; // TRANSITION TABLE typedef mpl::list< - smacc2::Transition, StBothArmsMove, SUCCESS> + smacc2::Transition, StBothArmsMove, SUCCESS>, + smacc2::Transition, StBothArmsMove, NEXT> > reactions; @@ -52,6 +52,7 @@ struct StRightArmMoves : smacc2::SmaccState configure_orthogonal(rightJointValues); // configure_orthogonal(15s); + configure_orthogonal(); } diff --git a/smacc2_sm_reference_library/sm_panda_moveit2z_cb_inventory/CMakeLists.txt b/smacc2_sm_reference_library/sm_panda_moveit2z_cb_inventory/CMakeLists.txt index 125c3a525..5de540bd7 100644 --- a/smacc2_sm_reference_library/sm_panda_moveit2z_cb_inventory/CMakeLists.txt +++ b/smacc2_sm_reference_library/sm_panda_moveit2z_cb_inventory/CMakeLists.txt @@ -14,13 +14,24 @@ endif() find_package(ament_cmake REQUIRED) find_package(ros_timer_client REQUIRED) find_package(smacc2 REQUIRED) +find_package(smacc2_msgs REQUIRED) +find_package(std_msgs) find_package(Boost COMPONENTS thread REQUIRED) find_package(moveit2z_client REQUIRED) +find_package(keyboard_client REQUIRED) +find_package(rclcpp REQUIRED) +find_package(ros_publisher_client REQUIRED) + set(THIS_PACKAGE_INCLUDE_DEPENDS ros_timer_client smacc2 + smacc2_msgs + std_msgs moveit2z_client + keyboard_client + ros_publisher_client + rclcpp ) include_directories( diff --git a/smacc2_sm_reference_library/sm_panda_moveit2z_cb_inventory/README.md b/smacc2_sm_reference_library/sm_panda_moveit2z_cb_inventory/README.md index b14b40adb..fb44686aa 100644 --- a/smacc2_sm_reference_library/sm_panda_moveit2z_cb_inventory/README.md +++ b/smacc2_sm_reference_library/sm_panda_moveit2z_cb_inventory/README.md @@ -32,7 +32,7 @@ source ~/workspace/humble_ws/install/setup.sh And then run the launch file... ``` -ros2 launch sm_panda_moveit2z_cb_inventory sm_panda_moveit2z_cb_inventory.launch +ros2 launch sm_panda_moveit2z_cb_inventory sm_panda_moveit2z_cb_inventory.launch.py ``` If the simulation is not starting up properly check if your GAZEBO_RESOURCE_PATH is set correctly (echo $GAZEBO_RESOURCE_PATH). Otherwise simply set it with: diff --git a/smacc2_sm_reference_library/sm_panda_moveit2z_cb_inventory/include/sm_panda_moveit2z_cb_inventory/orthogonals/or_keyboard.hpp b/smacc2_sm_reference_library/sm_panda_moveit2z_cb_inventory/include/sm_panda_moveit2z_cb_inventory/orthogonals/or_keyboard.hpp new file mode 100644 index 000000000..3b646afa2 --- /dev/null +++ b/smacc2_sm_reference_library/sm_panda_moveit2z_cb_inventory/include/sm_panda_moveit2z_cb_inventory/orthogonals/or_keyboard.hpp @@ -0,0 +1,36 @@ +// Copyright 2021 RobosoftAI Inc. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +/***************************************************************************************************************** + * + * Authors: Pablo Inigo Blasco, Brett Aldrich + * + ******************************************************************************************************************/ + +#pragma once + +#include +#include +#include + +namespace sm_panda_moveit2z_cb_inventory { +using namespace std::chrono_literals; + +class OrKeyboard : public smacc2::Orthogonal { +public: + void onInitialize() override { + auto keyboardClient = this->createClient(); + } +}; +} // namespace sm_panda_moveit2z_cb_inventory diff --git a/smacc2_sm_reference_library/sm_panda_moveit2z_cb_inventory/include/sm_panda_moveit2z_cb_inventory/sm_panda_moveit2z_cb_inventory.hpp b/smacc2_sm_reference_library/sm_panda_moveit2z_cb_inventory/include/sm_panda_moveit2z_cb_inventory/sm_panda_moveit2z_cb_inventory.hpp index 2c1bd5882..c99b8e63e 100644 --- a/smacc2_sm_reference_library/sm_panda_moveit2z_cb_inventory/include/sm_panda_moveit2z_cb_inventory/sm_panda_moveit2z_cb_inventory.hpp +++ b/smacc2_sm_reference_library/sm_panda_moveit2z_cb_inventory/include/sm_panda_moveit2z_cb_inventory/sm_panda_moveit2z_cb_inventory.hpp @@ -28,21 +28,39 @@ // ORTHOGONALS #include "sm_panda_moveit2z_cb_inventory/orthogonals/or_arm.hpp" +#include "sm_panda_moveit2z_cb_inventory/orthogonals/or_keyboard.hpp" #include #include +#include +#include + #include +// STATE REACTORS +//#include +//#include +//#include + +using namespace cl_moveit2z; +using namespace cl_keyboard; +//using namespace smacc2::state_reactors; + namespace sm_panda_moveit2z_cb_inventory { -using namespace cl_moveit2z; +//using namespace cl_moveit2z; +//using namespace cl_keyboard; +//using namespace smacc2::state_reactors; //STATES struct StAcquireSensors; +struct StAcquireSensors3; struct StMoveJoints; +struct StMoveJoints2; +struct StMoveJoints3; struct StMoveEndEffector; struct StMoveCartesianRelative; struct StMoveCartesianRelative2; @@ -57,21 +75,27 @@ struct StUndoLastTrajectory; //-------------------------------------------------------------------- //STATE_MACHINE -struct SmPandaMoveit2zCbInventory : public smacc2::SmaccStateMachineBase +struct SmPandaMoveit2zCbInventory : public smacc2::SmaccStateMachineBase { using SmaccStateMachineBase::SmaccStateMachineBase; - void onInitialize() override { this->createOrthogonal(); } + void onInitialize() override { + this->createOrthogonal(); + this->createOrthogonal(); + } }; } // namespace sm_panda_moveit2z_cb_inventory // STATES #include "states/st_acquire_sensors.hpp" +#include "states/st_acquire_sensors_3.hpp" #include "states/st_attach_object.hpp" #include "states/st_move_end_effector.hpp" #include "states/st_circular_pivot_motion.hpp" #include "states/st_move_joints.hpp" +#include "states/st_move_joints_2.hpp" +#include "states/st_move_joints_3.hpp" #include "states/st_detach_object.hpp" #include "states/st_move_known_state.hpp" #include "states/st_end_effector_rotate.hpp" diff --git a/smacc2_sm_reference_library/sm_panda_moveit2z_cb_inventory/include/sm_panda_moveit2z_cb_inventory/states/st_acquire_sensors.hpp b/smacc2_sm_reference_library/sm_panda_moveit2z_cb_inventory/include/sm_panda_moveit2z_cb_inventory/states/st_acquire_sensors.hpp index 41e91a126..759805ac3 100644 --- a/smacc2_sm_reference_library/sm_panda_moveit2z_cb_inventory/include/sm_panda_moveit2z_cb_inventory/states/st_acquire_sensors.hpp +++ b/smacc2_sm_reference_library/sm_panda_moveit2z_cb_inventory/include/sm_panda_moveit2z_cb_inventory/states/st_acquire_sensors.hpp @@ -35,15 +35,23 @@ using namespace cl_moveit2z; using smacc2::client_behaviors::CbWaitTopicMessage; using smacc2::client_behaviors::CbSleepFor; using namespace std::chrono_literals; +using namespace cl_keyboard; // STATE DECLARATION struct StAcquireSensors : smacc2::SmaccState { using SmaccState::SmaccState; + // DECLARE CUSTOM OBJECT TAGS + struct NEXT : SUCCESS{}; + struct PREVIOUS : ABORT{}; + // TRANSITION TABLE typedef boost::mpl::list< - Transition, OrArm>, StMoveJoints, SUCCESS> + Transition, OrArm>, StMoveJoints, SUCCESS>, + + Transition, StMoveJoints, NEXT> + > reactions; @@ -52,6 +60,7 @@ struct StAcquireSensors : smacc2::SmaccState>("/joint_states"); configure_orthogonal(5s); + configure_orthogonal(); }; void runtimeConfigure() {} diff --git a/smacc2_sm_reference_library/sm_panda_moveit2z_cb_inventory/include/sm_panda_moveit2z_cb_inventory/states/st_acquire_sensors_2.hpp b/smacc2_sm_reference_library/sm_panda_moveit2z_cb_inventory/include/sm_panda_moveit2z_cb_inventory/states/st_acquire_sensors_2.hpp new file mode 100644 index 000000000..e75c26346 --- /dev/null +++ b/smacc2_sm_reference_library/sm_panda_moveit2z_cb_inventory/include/sm_panda_moveit2z_cb_inventory/states/st_acquire_sensors_2.hpp @@ -0,0 +1,82 @@ +// Copyright 2021 RobosoftAI Inc. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +/***************************************************************************************************************** + * + * Authors: Pablo Inigo Blasco, Brett Aldrich + * + ******************************************************************************************************************/ + +#pragma once + +#include +#include +#include +#include + +namespace sm_panda_moveit2z_cb_inventory +{ +using namespace std::chrono_literals; +using namespace smacc2::default_events; +using namespace cl_keyboard; + +using smacc2::client_behaviors::CbSleepFor; +using smacc2::client_behaviors::CbWaitActionServer; +using smacc2::client_behaviors::CbWaitTopic; +// using cl_nav2z::CbWaitNav2Nodes; +using smacc2::client_behaviors::CbRosLaunch2; + +// STATE DECLARATION +struct StAcquireSensors2 + : smacc2::SmaccState { + using SmaccState::SmaccState; + + // DECLARE CUSTOM OBJECT AND TRANSITION TAGS + struct ON_SENSORS_AVAILABLE : SUCCESS {}; + struct SrAcquireSensors; + struct NEXT : SUCCESS{}; + struct PREVIOUS : ABORT{}; + + // TRANSITION TABLE + typedef mpl::list< + + Transition, StMoveJoints, ON_SENSORS_AVAILABLE>, + + Transition, StAcquireSensors, ABORT>, + + Transition, StMoveJoints, NEXT> + > + reactions; + + // STATE FUNCTIONS + static void staticConfigure() + { + configure_orthogonal(); + configure_orthogonal(10s); + //configure_orthogonal(); + configure_orthogonal(12s); + configure_orthogonal(); + + // Create State Reactor + auto srAllSensorsReady = static_createStateReactor< + SrAllEventsGo, + smacc2::state_reactors::EvAllGo, + mpl::list< + EvCbSuccess//, + // EvCbSuccess + // EvCbSuccess> + >>(); + } +}; +} // namespace sm_panda_moveit2z_cb_inventory diff --git a/smacc2_sm_reference_library/sm_panda_moveit2z_cb_inventory/include/sm_panda_moveit2z_cb_inventory/states/st_acquire_sensors_3.hpp b/smacc2_sm_reference_library/sm_panda_moveit2z_cb_inventory/include/sm_panda_moveit2z_cb_inventory/states/st_acquire_sensors_3.hpp new file mode 100644 index 000000000..16286619b --- /dev/null +++ b/smacc2_sm_reference_library/sm_panda_moveit2z_cb_inventory/include/sm_panda_moveit2z_cb_inventory/states/st_acquire_sensors_3.hpp @@ -0,0 +1,68 @@ +// Copyright 2021 MyName/MyCompany Inc. +// Copyright 2021 RobosoftAI Inc. (template) +// +// 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. + +/***************************************************************************************************************** + * + * Authors: Pablo Inigo Blasco, Brett Aldrich + * + *****************************************************************************************************************/ + +#pragma once + +#include +#include + +namespace sm_panda_moveit2z_cb_inventory +{ +// SMACC2 classes +using smacc2::EvStateRequestFinish; +using smacc2::Transition; +using smacc2::default_transition_tags::SUCCESS; +using namespace smacc2; +using namespace cl_moveit2z; +using smacc2::client_behaviors::CbWaitTopicMessage; +using smacc2::client_behaviors::CbSleepFor; +using namespace std::chrono_literals; +using namespace cl_keyboard; + +// STATE DECLARATION +struct StAcquireSensors3 : smacc2::SmaccState +{ + using SmaccState::SmaccState; + + // DECLARE CUSTOM OBJECT TAGS + struct NEXT : SUCCESS{}; + struct PREVIOUS : ABORT{}; + + // TRANSITION TABLE + typedef boost::mpl::list< + Transition, StAcquireSensors, SUCCESS>, + + Transition, StAcquireSensors, NEXT> + + + > reactions; + + // STATE FUNCTIONS + static void staticConfigure() + { + // configure_orthogonal>("/joint_states"); + configure_orthogonal(15s); + configure_orthogonal(); + }; + + void runtimeConfigure() {} +}; +} // namespace sm_panda_moveit2z_cb_inventory diff --git a/smacc2_sm_reference_library/sm_panda_moveit2z_cb_inventory/include/sm_panda_moveit2z_cb_inventory/states/st_attach_object.hpp b/smacc2_sm_reference_library/sm_panda_moveit2z_cb_inventory/include/sm_panda_moveit2z_cb_inventory/states/st_attach_object.hpp index d093a8d55..124c9f706 100644 --- a/smacc2_sm_reference_library/sm_panda_moveit2z_cb_inventory/include/sm_panda_moveit2z_cb_inventory/states/st_attach_object.hpp +++ b/smacc2_sm_reference_library/sm_panda_moveit2z_cb_inventory/include/sm_panda_moveit2z_cb_inventory/states/st_attach_object.hpp @@ -32,15 +32,21 @@ namespace sm_panda_moveit2z_cb_inventory using smacc2::Transition; using smacc2::default_transition_tags::SUCCESS; using namespace smacc2; +using namespace cl_keyboard; // STATE DECLARATION struct StAttachObject : smacc2::SmaccState { using SmaccState::SmaccState; + // DECLARE CUSTOM OBJECT TAGS + struct NEXT : SUCCESS{}; + struct PREVIOUS : ABORT{}; + // TRANSITION TABLE typedef boost::mpl::list< - Transition, StDetatchObject, SUCCESS> + Transition, StDetatchObject, SUCCESS>, + Transition, StDetatchObject, NEXT> > reactions; @@ -48,6 +54,7 @@ struct StAttachObject : smacc2::SmaccState("virtualBox"); + configure_orthogonal(); } void runtimeConfigure() diff --git a/smacc2_sm_reference_library/sm_panda_moveit2z_cb_inventory/include/sm_panda_moveit2z_cb_inventory/states/st_circular_pivot_motion.hpp b/smacc2_sm_reference_library/sm_panda_moveit2z_cb_inventory/include/sm_panda_moveit2z_cb_inventory/states/st_circular_pivot_motion.hpp index 0c3c4aeb5..0f15e6d46 100644 --- a/smacc2_sm_reference_library/sm_panda_moveit2z_cb_inventory/include/sm_panda_moveit2z_cb_inventory/states/st_circular_pivot_motion.hpp +++ b/smacc2_sm_reference_library/sm_panda_moveit2z_cb_inventory/include/sm_panda_moveit2z_cb_inventory/states/st_circular_pivot_motion.hpp @@ -30,12 +30,17 @@ namespace sm_panda_moveit2z_cb_inventory using smacc2::Transition; using smacc2::default_transition_tags::SUCCESS; using namespace smacc2; +using namespace cl_keyboard; // STATE DECLARATION struct StCircularPivotMotion : smacc2::SmaccState { using SmaccState::SmaccState; + // DECLARE CUSTOM OBJECT TAGS + struct NEXT : SUCCESS{}; + struct PREVIOUS : ABORT{}; + // TRANSITION TABLE typedef boost::mpl::list< // Transition, StMoveCartesianRelative2, SUCCESS> @@ -46,6 +51,7 @@ struct StCircularPivotMotion : smacc2::SmaccState(); + configure_orthogonal(); } void runtimeConfigure() { RCLCPP_INFO(getLogger(), "Entering StCircularPivotMotion"); } diff --git a/smacc2_sm_reference_library/sm_panda_moveit2z_cb_inventory/include/sm_panda_moveit2z_cb_inventory/states/st_detach_object.hpp b/smacc2_sm_reference_library/sm_panda_moveit2z_cb_inventory/include/sm_panda_moveit2z_cb_inventory/states/st_detach_object.hpp index 482e08c91..1c1aed8d3 100644 --- a/smacc2_sm_reference_library/sm_panda_moveit2z_cb_inventory/include/sm_panda_moveit2z_cb_inventory/states/st_detach_object.hpp +++ b/smacc2_sm_reference_library/sm_panda_moveit2z_cb_inventory/include/sm_panda_moveit2z_cb_inventory/states/st_detach_object.hpp @@ -30,15 +30,21 @@ namespace sm_panda_moveit2z_cb_inventory using smacc2::Transition; using smacc2::default_transition_tags::SUCCESS; using namespace smacc2; +using namespace cl_keyboard; // STATE DECLARATION struct StDetatchObject : smacc2::SmaccState { using SmaccState::SmaccState; + // DECLARE CUSTOM OBJECT TAGS + struct NEXT : SUCCESS{}; + struct PREVIOUS : ABORT{}; + // TRANSITION TABLE typedef boost::mpl::list< - Transition, StEndEffectorRotate, SUCCESS> + Transition, StEndEffectorRotate, SUCCESS>, + Transition, StEndEffectorRotate, NEXT> > reactions; @@ -46,6 +52,7 @@ struct StDetatchObject : smacc2::SmaccState(); + configure_orthogonal(); } void runtimeConfigure() { RCLCPP_INFO(getLogger(), "Entering StDetatchObject"); } diff --git a/smacc2_sm_reference_library/sm_panda_moveit2z_cb_inventory/include/sm_panda_moveit2z_cb_inventory/states/st_end_effector_rotate.hpp b/smacc2_sm_reference_library/sm_panda_moveit2z_cb_inventory/include/sm_panda_moveit2z_cb_inventory/states/st_end_effector_rotate.hpp index a78d59854..9704c55aa 100644 --- a/smacc2_sm_reference_library/sm_panda_moveit2z_cb_inventory/include/sm_panda_moveit2z_cb_inventory/states/st_end_effector_rotate.hpp +++ b/smacc2_sm_reference_library/sm_panda_moveit2z_cb_inventory/include/sm_panda_moveit2z_cb_inventory/states/st_end_effector_rotate.hpp @@ -30,16 +30,25 @@ namespace sm_panda_moveit2z_cb_inventory using smacc2::Transition; using smacc2::default_transition_tags::SUCCESS; using namespace smacc2; +using namespace cl_keyboard; // STATE DECLARATION struct StEndEffectorRotate : smacc2::SmaccState { using SmaccState::SmaccState; + // DECLARE CUSTOM OBJECT TAGS + struct NEXT : SUCCESS{}; + struct PREVIOUS : ABORT{}; + // TRANSITION TABLE typedef boost::mpl::list< - Transition, StUndoLastTrajectory, SUCCESS>, - Transition, StEndEffectorRotate, SUCCESS> + + Transition, StMoveKnownState, SUCCESS>, + Transition, StEndEffectorRotate, SUCCESS>, + + Transition, StMoveEndEffector, PREVIOUS>, + Transition, StMoveKnownState, NEXT> > reactions; @@ -49,9 +58,11 @@ struct StEndEffectorRotate : smacc2::SmaccState(deltaRadians, tipLink); + configure_orthogonal(); } void runtimeConfigure() { RCLCPP_INFO(getLogger(), "Entering StEndEffectorRotate"); } diff --git a/smacc2_sm_reference_library/sm_panda_moveit2z_cb_inventory/include/sm_panda_moveit2z_cb_inventory/states/st_execute_last_trajectory.hpp b/smacc2_sm_reference_library/sm_panda_moveit2z_cb_inventory/include/sm_panda_moveit2z_cb_inventory/states/st_execute_last_trajectory.hpp index 1a5f5d795..29f797732 100644 --- a/smacc2_sm_reference_library/sm_panda_moveit2z_cb_inventory/include/sm_panda_moveit2z_cb_inventory/states/st_execute_last_trajectory.hpp +++ b/smacc2_sm_reference_library/sm_panda_moveit2z_cb_inventory/include/sm_panda_moveit2z_cb_inventory/states/st_execute_last_trajectory.hpp @@ -30,15 +30,23 @@ namespace sm_panda_moveit2z_cb_inventory using smacc2::Transition; using smacc2::default_transition_tags::SUCCESS; using namespace smacc2; +using namespace cl_keyboard; // STATE DECLARATION struct StExecuteLastTrajectory : smacc2::SmaccState { using SmaccState::SmaccState; + // DECLARE CUSTOM OBJECT TAGS + struct NEXT : SUCCESS{}; + struct PREVIOUS : ABORT{}; + // TRANSITION TABLE typedef boost::mpl::list< - Transition, StMoveKnownState, SUCCESS> + Transition, StMoveLastTrajectoryInitialState, SUCCESS>, + + Transition, StUndoLastTrajectory, PREVIOUS>, + Transition, StMoveLastTrajectoryInitialState, NEXT> > reactions; @@ -46,6 +54,7 @@ struct StExecuteLastTrajectory : smacc2::SmaccState(); + configure_orthogonal(); } void runtimeConfigure() { RCLCPP_INFO(getLogger(), "Entering StExecuteLastTrajectory"); } diff --git a/smacc2_sm_reference_library/sm_panda_moveit2z_cb_inventory/include/sm_panda_moveit2z_cb_inventory/states/st_move_cartesian_relative.hpp b/smacc2_sm_reference_library/sm_panda_moveit2z_cb_inventory/include/sm_panda_moveit2z_cb_inventory/states/st_move_cartesian_relative.hpp index 3b7d5f987..f0c2ceffe 100644 --- a/smacc2_sm_reference_library/sm_panda_moveit2z_cb_inventory/include/sm_panda_moveit2z_cb_inventory/states/st_move_cartesian_relative.hpp +++ b/smacc2_sm_reference_library/sm_panda_moveit2z_cb_inventory/include/sm_panda_moveit2z_cb_inventory/states/st_move_cartesian_relative.hpp @@ -30,16 +30,23 @@ namespace sm_panda_moveit2z_cb_inventory using smacc2::Transition; using smacc2::default_transition_tags::SUCCESS; using namespace smacc2; +using namespace cl_keyboard; // STATE DECLARATION struct StMoveCartesianRelative : smacc2::SmaccState { using SmaccState::SmaccState; + // DECLARE CUSTOM OBJECT TAGS + struct NEXT : SUCCESS{}; + struct PREVIOUS : ABORT{}; + // TRANSITION TABLE typedef boost::mpl::list< Transition, StAttachObject, SUCCESS> , - Transition, StAttachObject, ABORT> + Transition, StAttachObject, ABORT>, + + Transition, StAttachObject, NEXT> > reactions; @@ -49,6 +56,7 @@ struct StMoveCartesianRelative : smacc2::SmaccState(offset); + configure_orthogonal(); } void runtimeConfigure() { RCLCPP_INFO(getLogger(), "Entering StMoveCartesianRelative"); } diff --git a/smacc2_sm_reference_library/sm_panda_moveit2z_cb_inventory/include/sm_panda_moveit2z_cb_inventory/states/st_move_cartesian_relative2.hpp b/smacc2_sm_reference_library/sm_panda_moveit2z_cb_inventory/include/sm_panda_moveit2z_cb_inventory/states/st_move_cartesian_relative2.hpp index d56022d17..2321bb3e8 100644 --- a/smacc2_sm_reference_library/sm_panda_moveit2z_cb_inventory/include/sm_panda_moveit2z_cb_inventory/states/st_move_cartesian_relative2.hpp +++ b/smacc2_sm_reference_library/sm_panda_moveit2z_cb_inventory/include/sm_panda_moveit2z_cb_inventory/states/st_move_cartesian_relative2.hpp @@ -30,16 +30,23 @@ namespace sm_panda_moveit2z_cb_inventory using smacc2::Transition; using smacc2::default_transition_tags::SUCCESS; using namespace smacc2; +using namespace cl_keyboard; // STATE DECLARATION struct StMoveCartesianRelative2 : smacc2::SmaccState { using SmaccState::SmaccState; + // DECLARE CUSTOM OBJECT TAGS + struct NEXT : SUCCESS{}; + struct PREVIOUS : ABORT{}; + // TRANSITION TABLE typedef boost::mpl::list< Transition, StAttachObject, SUCCESS>, - Transition, StMoveCartesianRelative2, ABORT> // retry + Transition, StMoveCartesianRelative2, ABORT>, // retry + + Transition, StAttachObject, NEXT> > reactions; @@ -52,6 +59,7 @@ struct StMoveCartesianRelative2 : smacc2::SmaccState("tool0", "tool0", position); + configure_orthogonal(); } void runtimeConfigure() { RCLCPP_INFO(getLogger(), "Entering StMoveCartesianRelative2"); } diff --git a/smacc2_sm_reference_library/sm_panda_moveit2z_cb_inventory/include/sm_panda_moveit2z_cb_inventory/states/st_move_end_effector.hpp b/smacc2_sm_reference_library/sm_panda_moveit2z_cb_inventory/include/sm_panda_moveit2z_cb_inventory/states/st_move_end_effector.hpp index 4abfad7f2..86d1a11d6 100644 --- a/smacc2_sm_reference_library/sm_panda_moveit2z_cb_inventory/include/sm_panda_moveit2z_cb_inventory/states/st_move_end_effector.hpp +++ b/smacc2_sm_reference_library/sm_panda_moveit2z_cb_inventory/include/sm_panda_moveit2z_cb_inventory/states/st_move_end_effector.hpp @@ -30,16 +30,24 @@ namespace sm_panda_moveit2z_cb_inventory using smacc2::Transition; using smacc2::default_transition_tags::SUCCESS; using namespace smacc2; +using namespace cl_keyboard; // STATE DECLARATION struct StMoveEndEffector : smacc2::SmaccState { using SmaccState::SmaccState; + // DECLARE CUSTOM OBJECT TAGS + struct NEXT : SUCCESS{}; + struct PREVIOUS : ABORT{}; + // TRANSITION TABLE typedef boost::mpl::list< Transition, StEndEffectorRotate, SUCCESS>, - Transition, StMoveEndEffector, ABORT> + Transition, StMoveEndEffector, ABORT>, + + Transition, StMoveJoints, PREVIOUS>, + Transition, StEndEffectorRotate, NEXT> > reactions; @@ -72,6 +80,7 @@ struct StMoveEndEffector : smacc2::SmaccState(target_pose, "panda_link8"); + configure_orthogonal(); } void runtimeConfigure() { RCLCPP_INFO(getLogger(), "Entering StMoveEndEffector"); } diff --git a/smacc2_sm_reference_library/sm_panda_moveit2z_cb_inventory/include/sm_panda_moveit2z_cb_inventory/states/st_move_joints.hpp b/smacc2_sm_reference_library/sm_panda_moveit2z_cb_inventory/include/sm_panda_moveit2z_cb_inventory/states/st_move_joints.hpp index 5495230fd..86068f602 100644 --- a/smacc2_sm_reference_library/sm_panda_moveit2z_cb_inventory/include/sm_panda_moveit2z_cb_inventory/states/st_move_joints.hpp +++ b/smacc2_sm_reference_library/sm_panda_moveit2z_cb_inventory/include/sm_panda_moveit2z_cb_inventory/states/st_move_joints.hpp @@ -29,16 +29,24 @@ using smacc2::Transition; using smacc2::default_transition_tags::SUCCESS; using namespace smacc2; using namespace cl_moveit2z; +using namespace cl_keyboard; // STATE DECLARATION struct StMoveJoints : smacc2::SmaccState { using SmaccState::SmaccState; + // DECLARE CUSTOM OBJECT TAGS + struct NEXT : SUCCESS{}; + struct PREVIOUS : ABORT{}; + // TRANSITION TABLE typedef boost::mpl::list< - Transition, StMoveEndEffector, SUCCESS> - ,Transition, StMoveJoints, ABORT> + Transition, StMoveJoints2, SUCCESS>, + Transition, StMoveJoints, ABORT>, + + Transition, StAcquireSensors, PREVIOUS>, + Transition, StMoveJoints2, NEXT> > reactions; @@ -62,6 +70,7 @@ struct StMoveJoints : smacc2::SmaccState(jointValues); + configure_orthogonal(); }; void runtimeConfigure() diff --git a/smacc2_sm_reference_library/sm_panda_moveit2z_cb_inventory/include/sm_panda_moveit2z_cb_inventory/states/st_move_joints_2.hpp b/smacc2_sm_reference_library/sm_panda_moveit2z_cb_inventory/include/sm_panda_moveit2z_cb_inventory/states/st_move_joints_2.hpp new file mode 100644 index 000000000..01ce4d51b --- /dev/null +++ b/smacc2_sm_reference_library/sm_panda_moveit2z_cb_inventory/include/sm_panda_moveit2z_cb_inventory/states/st_move_joints_2.hpp @@ -0,0 +1,83 @@ +// Copyright 2021 MyName/MyCompany Inc. +// Copyright 2021 RobosoftAI Inc. (template) +// +// 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. + +/***************************************************************************************************************** + * + * Authors: Pablo Inigo Blasco, Brett Aldrich + * + *****************************************************************************************************************/ + +#pragma once + +namespace sm_panda_moveit2z_cb_inventory +{ +// SMACC2 classes +using smacc2::EvStateRequestFinish; +using smacc2::Transition; +using smacc2::default_transition_tags::SUCCESS; +using namespace smacc2; +using namespace cl_moveit2z; +using namespace cl_keyboard; + +// STATE DECLARATION +struct StMoveJoints2 : smacc2::SmaccState +{ + using SmaccState::SmaccState; + + // DECLARE CUSTOM OBJECT TAGS + struct NEXT : SUCCESS{}; + struct PREVIOUS : ABORT{}; + + // TRANSITION TABLE + typedef boost::mpl::list< + Transition, StMoveJoints3, SUCCESS>, + Transition, StMoveJoints, ABORT>, + + Transition, StMoveJoints, PREVIOUS>, + Transition, StMoveJoints3, NEXT> + + > + reactions; + + // STATE FUNCTIONS + static void staticConfigure() + { + std::map jointValues{ + {"panda_joint1", 0.5}, + {"panda_joint2", 0.0}, + {"panda_joint3", 0.0}, + {"panda_joint4", -M_PI/2}, + {"panda_joint5", 0.0}, + {"panda_joint6", M_PI/2}, + {"panda_joint7", 0.0} + }; + + // panda_joint6: + // panda_joint7: + // panda_finger_joint1: + // panda_finger_joint2: + + configure_orthogonal(jointValues); + configure_orthogonal(); + }; + + void runtimeConfigure() + { + ClMoveit2z * moveGroupClient; + this->requiresClient(moveGroupClient); + this->getClientBehavior()->scalingFactor_ = 1; + } +}; +} // namespace sm_panda_moveit2z_cb_inventory diff --git a/smacc2_sm_reference_library/sm_panda_moveit2z_cb_inventory/include/sm_panda_moveit2z_cb_inventory/states/st_move_joints_3.hpp b/smacc2_sm_reference_library/sm_panda_moveit2z_cb_inventory/include/sm_panda_moveit2z_cb_inventory/states/st_move_joints_3.hpp new file mode 100644 index 000000000..0b3ae26cc --- /dev/null +++ b/smacc2_sm_reference_library/sm_panda_moveit2z_cb_inventory/include/sm_panda_moveit2z_cb_inventory/states/st_move_joints_3.hpp @@ -0,0 +1,83 @@ +// Copyright 2021 MyName/MyCompany Inc. +// Copyright 2021 RobosoftAI Inc. (template) +// +// 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. + +/***************************************************************************************************************** + * + * Authors: Pablo Inigo Blasco, Brett Aldrich + * + *****************************************************************************************************************/ + +#pragma once + +namespace sm_panda_moveit2z_cb_inventory +{ +// SMACC2 classes +using smacc2::EvStateRequestFinish; +using smacc2::Transition; +using smacc2::default_transition_tags::SUCCESS; +using namespace smacc2; +using namespace cl_moveit2z; +using namespace cl_keyboard; + +// STATE DECLARATION +struct StMoveJoints3 : smacc2::SmaccState +{ + using SmaccState::SmaccState; + + // DECLARE CUSTOM OBJECT TAGS + struct NEXT : SUCCESS{}; + struct PREVIOUS : ABORT{}; + + // TRANSITION TABLE + typedef boost::mpl::list< + Transition, StMoveEndEffector, SUCCESS>, + Transition, StMoveJoints2, ABORT>, + + Transition, StMoveJoints2, PREVIOUS>, + Transition, StMoveEndEffector, NEXT> + + > + reactions; + + // STATE FUNCTIONS + static void staticConfigure() + { + std::map jointValues{ + {"panda_joint1", 0.0}, + {"panda_joint2", 0.0}, + {"panda_joint3", 0.0}, + {"panda_joint4", -M_PI/2}, + {"panda_joint5", 0.0}, + {"panda_joint6", M_PI/2}, + {"panda_joint7", 0.0} + }; + + // panda_joint6: + // panda_joint7: + // panda_finger_joint1: + // panda_finger_joint2: + + configure_orthogonal(jointValues); + configure_orthogonal(); + }; + + void runtimeConfigure() + { + ClMoveit2z * moveGroupClient; + this->requiresClient(moveGroupClient); + this->getClientBehavior()->scalingFactor_ = 1; + } +}; +} // namespace sm_panda_moveit2z_cb_inventory diff --git a/smacc2_sm_reference_library/sm_panda_moveit2z_cb_inventory/include/sm_panda_moveit2z_cb_inventory/states/st_move_known_state.hpp b/smacc2_sm_reference_library/sm_panda_moveit2z_cb_inventory/include/sm_panda_moveit2z_cb_inventory/states/st_move_known_state.hpp index 712961f50..1868deb42 100644 --- a/smacc2_sm_reference_library/sm_panda_moveit2z_cb_inventory/include/sm_panda_moveit2z_cb_inventory/states/st_move_known_state.hpp +++ b/smacc2_sm_reference_library/sm_panda_moveit2z_cb_inventory/include/sm_panda_moveit2z_cb_inventory/states/st_move_known_state.hpp @@ -30,16 +30,24 @@ namespace sm_panda_moveit2z_cb_inventory using smacc2::Transition; using smacc2::default_transition_tags::SUCCESS; using namespace smacc2; +using namespace cl_keyboard; // STATE DECLARATION struct StMoveKnownState : smacc2::SmaccState { using SmaccState::SmaccState; + // DECLARE CUSTOM OBJECT TAGS + struct NEXT : SUCCESS{}; + struct PREVIOUS : ABORT{}; + // TRANSITION TABLE typedef boost::mpl::list< Transition, StPouringMotion, SUCCESS>, - Transition, StPouringMotion, ABORT> + Transition, StPouringMotion, ABORT>, + + Transition, StExecuteLastTrajectory, PREVIOUS>, + Transition, StPouringMotion, NEXT> > reactions; @@ -50,6 +58,7 @@ struct StMoveKnownState : smacc2::SmaccState(pkg, filepath); + configure_orthogonal(); } void runtimeConfigure() { RCLCPP_INFO(getLogger(), "Entering StMoveKnownState"); } diff --git a/smacc2_sm_reference_library/sm_panda_moveit2z_cb_inventory/include/sm_panda_moveit2z_cb_inventory/states/st_move_last_trajectory_initial_state.hpp b/smacc2_sm_reference_library/sm_panda_moveit2z_cb_inventory/include/sm_panda_moveit2z_cb_inventory/states/st_move_last_trajectory_initial_state.hpp index a9e429f6c..86799060f 100644 --- a/smacc2_sm_reference_library/sm_panda_moveit2z_cb_inventory/include/sm_panda_moveit2z_cb_inventory/states/st_move_last_trajectory_initial_state.hpp +++ b/smacc2_sm_reference_library/sm_panda_moveit2z_cb_inventory/include/sm_panda_moveit2z_cb_inventory/states/st_move_last_trajectory_initial_state.hpp @@ -30,15 +30,21 @@ namespace sm_panda_moveit2z_cb_inventory using smacc2::Transition; using smacc2::default_transition_tags::SUCCESS; using namespace smacc2; +using namespace cl_keyboard; // STATE DECLARATION struct StMoveLastTrajectoryInitialState : smacc2::SmaccState { using SmaccState::SmaccState; + // DECLARE CUSTOM OBJECT TAGS + struct NEXT : SUCCESS{}; + struct PREVIOUS : ABORT{}; + // TRANSITION TABLE typedef boost::mpl::list< - Transition, StMoveLastTrajectoryInitialState, SUCCESS> + Transition, StUndoLastTrajectory, SUCCESS>, + Transition, StUndoLastTrajectory, NEXT> > reactions; @@ -46,6 +52,7 @@ struct StMoveLastTrajectoryInitialState : smacc2::SmaccState(); + configure_orthogonal(); } void runtimeConfigure() { RCLCPP_INFO(getLogger(), "Entering StMoveLastTrajectoryInitialState"); } diff --git a/smacc2_sm_reference_library/sm_panda_moveit2z_cb_inventory/include/sm_panda_moveit2z_cb_inventory/states/st_move_named_target.hpp b/smacc2_sm_reference_library/sm_panda_moveit2z_cb_inventory/include/sm_panda_moveit2z_cb_inventory/states/st_move_named_target.hpp index d03860eff..871352790 100644 --- a/smacc2_sm_reference_library/sm_panda_moveit2z_cb_inventory/include/sm_panda_moveit2z_cb_inventory/states/st_move_named_target.hpp +++ b/smacc2_sm_reference_library/sm_panda_moveit2z_cb_inventory/include/sm_panda_moveit2z_cb_inventory/states/st_move_named_target.hpp @@ -30,12 +30,17 @@ namespace sm_panda_moveit2z_cb_inventory using smacc2::Transition; using smacc2::default_transition_tags::SUCCESS; using namespace smacc2; +using namespace cl_keyboard; // STATE DECLARATION struct StMoveNamedTarget : smacc2::SmaccState { using SmaccState::SmaccState; + // DECLARE CUSTOM OBJECT TAGS + struct NEXT : SUCCESS{}; + struct PREVIOUS : ABORT{}; + // TRANSITION TABLE typedef boost::mpl::list< // Transition, StMoveCartesianRelative2, SUCCESS> diff --git a/smacc2_sm_reference_library/sm_panda_moveit2z_cb_inventory/include/sm_panda_moveit2z_cb_inventory/states/st_pouring_motion.hpp b/smacc2_sm_reference_library/sm_panda_moveit2z_cb_inventory/include/sm_panda_moveit2z_cb_inventory/states/st_pouring_motion.hpp index a28713890..8b41b57a6 100644 --- a/smacc2_sm_reference_library/sm_panda_moveit2z_cb_inventory/include/sm_panda_moveit2z_cb_inventory/states/st_pouring_motion.hpp +++ b/smacc2_sm_reference_library/sm_panda_moveit2z_cb_inventory/include/sm_panda_moveit2z_cb_inventory/states/st_pouring_motion.hpp @@ -30,16 +30,24 @@ namespace sm_panda_moveit2z_cb_inventory using smacc2::Transition; using smacc2::default_transition_tags::SUCCESS; using namespace smacc2; +using namespace cl_keyboard; // STATE DECLARATION struct StPouringMotion : smacc2::SmaccState { using SmaccState::SmaccState; + // DECLARE CUSTOM OBJECT TAGS + struct NEXT : SUCCESS{}; + struct PREVIOUS : ABORT{}; + // TRANSITION TABLE typedef boost::mpl::list< Transition, StMoveLastTrajectoryInitialState, SUCCESS>, - Transition, StMoveLastTrajectoryInitialState, ABORT> + Transition, StMoveLastTrajectoryInitialState, ABORT>, + + Transition, StMoveKnownState, PREVIOUS>, + Transition, StMoveLastTrajectoryInitialState, NEXT> > reactions; @@ -49,10 +57,11 @@ struct StPouringMotion : smacc2::SmaccState(relativePivotPoint, deltaHeight, tipLink, globalFrame); + configure_orthogonal(); } void runtimeConfigure() { RCLCPP_INFO(getLogger(), "Entering StPouringMotion"); } diff --git a/smacc2_sm_reference_library/sm_panda_moveit2z_cb_inventory/include/sm_panda_moveit2z_cb_inventory/states/st_undo_last_trajectory.hpp b/smacc2_sm_reference_library/sm_panda_moveit2z_cb_inventory/include/sm_panda_moveit2z_cb_inventory/states/st_undo_last_trajectory.hpp index e495322b6..ae9edc5de 100644 --- a/smacc2_sm_reference_library/sm_panda_moveit2z_cb_inventory/include/sm_panda_moveit2z_cb_inventory/states/st_undo_last_trajectory.hpp +++ b/smacc2_sm_reference_library/sm_panda_moveit2z_cb_inventory/include/sm_panda_moveit2z_cb_inventory/states/st_undo_last_trajectory.hpp @@ -30,15 +30,24 @@ namespace sm_panda_moveit2z_cb_inventory using smacc2::Transition; using smacc2::default_transition_tags::SUCCESS; using namespace smacc2; +using namespace cl_keyboard; // STATE DECLARATION struct StUndoLastTrajectory : smacc2::SmaccState { using SmaccState::SmaccState; + // DECLARE CUSTOM OBJECT TAGS + struct NEXT : SUCCESS{}; + struct PREVIOUS : ABORT{}; + // TRANSITION TABLE typedef boost::mpl::list< - Transition, StExecuteLastTrajectory, SUCCESS> + Transition, StExecuteLastTrajectory, SUCCESS>, + + Transition, StEndEffectorRotate, PREVIOUS>, + Transition, StExecuteLastTrajectory, NEXT> + > reactions; @@ -46,6 +55,7 @@ struct StUndoLastTrajectory : smacc2::SmaccState(); + configure_orthogonal(); } void runtimeConfigure() { RCLCPP_INFO(getLogger(), "Entering StUndoLastTrajectory"); } diff --git a/smacc2_sm_reference_library/sm_panda_moveit2z_cb_inventory/launch/sm_panda_moveit2z_cb_inventory.launch.py b/smacc2_sm_reference_library/sm_panda_moveit2z_cb_inventory/launch/sm_panda_moveit2z_cb_inventory.launch.py index b35cfcc7b..de7494186 100644 --- a/smacc2_sm_reference_library/sm_panda_moveit2z_cb_inventory/launch/sm_panda_moveit2z_cb_inventory.launch.py +++ b/smacc2_sm_reference_library/sm_panda_moveit2z_cb_inventory/launch/sm_panda_moveit2z_cb_inventory.launch.py @@ -135,6 +135,15 @@ def launch_setup(context, *args, **kwargs): output="screen", ) + keyboard_client_node = Node( + package="keyboard_client", + executable="keyboard_server_node.py", + name="keyboard_client", + output="screen", + prefix="xterm -hold -e", + arguments=["--ros-args", "--log-level", "INFO"], + ) + hand_controller_spawner = Node( package="controller_manager", executable="spawner", @@ -150,6 +159,7 @@ def launch_setup(context, *args, **kwargs): joint_state_broadcaster_spawner, arm_controller_spawner, hand_controller_spawner, + keyboard_client_node, ] return nodes_to_start diff --git a/smacc2_sm_reference_library/sm_panda_moveit2z_cb_inventory/package.xml b/smacc2_sm_reference_library/sm_panda_moveit2z_cb_inventory/package.xml index 60781efd9..dbc3b7920 100644 --- a/smacc2_sm_reference_library/sm_panda_moveit2z_cb_inventory/package.xml +++ b/smacc2_sm_reference_library/sm_panda_moveit2z_cb_inventory/package.xml @@ -27,6 +27,9 @@ moveit_plugins moveit_ros_robot_interaction moveit + keyboard_client + rclcpp + ros_publisher_client joint_trajectory_controller