From 0574b54cdbdbe91f5e96b0aa1c1f8c08e9899b90 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Pablo=20I=C3=B1igo=20Blasco?= Date: Thu, 24 Aug 2023 21:57:37 +0200 Subject: [PATCH] Feature/sm dancebot ue (#512) MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit * fix navigation humble * more fixes * tests * changes smacc2_client_library/nav2z_client/nav2z_client/src/nav2z_client/components/waypoints_navigator/cp_waypoints_navigator.cpp * missing * progress in navigation ue * docker improvements for rta * ovpn config * instructions * formatting * Got wget command to work * minor * more changes on docker * Updated readme * More readme updates * second pass on docker ue, nvidia drivers and readme.md * minor changes * minor changes * minor * minor * changes * progress * Update README.md * Update README.md * Update README.md * updates in navigation * Bretts runtime instructions added to readme * minor change * Heading sizes in readme * improvements in navigation * fixing build * fixing format * fixing branch * progress * repos * Dockerfile referencing our work * progress on docker image for ue * missing * changes in the code * missing stuff * dockerfile and launching * finishing environment * fix save script * fixing format * missing * missing * rough terrain navigation and behaviors and ice winter demo * turn over state machine * art gallery * minor * push * waypoints fix * more changes * minor * minor * minor * navigation parameters * Añadidos waypoints y cambiado el nombre del nodo en el json de depuración que podía dar fallo * progress state machine * ue_projec_2 refactoring * tunning navigation * missing * art gallery * Update README.md * Update README.md * fixes * merge * missing code * refactoring files * updating docker --------- Co-authored-by: brettpac Co-authored-by: Administrator Co-authored-by: brettpac --- .../cp_topic_subscriber.hpp | 8 +- .../smacc2/impl/smacc_state_machine_impl.hpp | 2 +- .../nav2z_client/nav2z_client/CMakeLists.txt | 2 + .../cp_waypoints_navigator.hpp | 47 +- .../cp_waypoints_navigator_base.hpp | 102 + .../nav2z_client/nav2z_client/package.xml | 1 + .../client_behaviors/cb_absolute_rotate.cpp | 1 + .../cb_nav2z_client_behavior_base.cpp | 3 + .../client_behaviors/cb_navigate_forward.cpp | 7 +- .../cp_waypoints_navigator.cpp | 107 +- smacc2_dev_tools/.vscode/launch.json | 50 + .../launch/sm_dancebot_ue_launch.py | 252 + .../sm_dancebot_artgallery_ue/CHANGELOG.rst | 13067 ++++++++++++++++ .../sm_dancebot_artgallery_ue/CMakeLists.txt | 114 + .../sm_dancebot_artgallery_ue/README.md | 55 + .../docker/.dockerignore | 0 .../docker/.gitignore | 1 + .../docker/Dockerfile | 131 + .../docker/README.md | 296 + .../docker/attach_docker_container.sh | 1 + .../docker/build_docker.sh | 21 + .../docker/build_docker_humble.sh | 4 + .../docker/fastdds_setup.bash | 5 + .../docker/frames_2023-08-05_01.06.53.gv | 18 + .../docker/frames_2023-08-05_01.06.53.pdf | Bin 0 -> 16806 bytes .../docker/join_bash.sh | 1 + .../docker/join_editor.sh | 1 + .../docker/nvidia-driver-check.sh | 62 + .../docker/older/run_docker_container_bash.sh | 8 + .../older/run_docker_container_editor.sh | 8 + .../docker/older/run_editor_smacc.sh | 32 + .../docker/openvpn/login.txt | 0 .../docker/openvpn/ovpnconfig.ovpn | 0 .../docker/openvpn/password.conf | 0 .../docker/openvpn_file_run.sh | 7 + .../docker/remove_container.sh | 1 + .../run_docker_container_bash_development.sh | 8 + .../docker/save_current_docker_image.sh | 11 + .../docker/start_container.sh | 1 + .../docker/stop_container.sh | 1 + .../docker/update_project_files.sh | 2 + .../client_behaviors/cb_active_stop.hpp | 46 + .../cb_load_waypoints_file.hpp | 69 + .../cb_navigate_next_waypoint_free.hpp | 60 + .../cb_position_control_free_space.hpp | 65 + .../client_behaviors/cb_pure_spinning.hpp | 126 + .../clients/components/cp_ue_pose.hpp | 56 + .../modestates/ms_dance_bot_recovery_mode.hpp | 38 + .../modestates/ms_dance_bot_run_mode.hpp | 37 + .../orthogonals/or_navigation.hpp | 71 + .../orthogonals/or_obstacle_perception.hpp | 39 + .../sm_dancebot_artgallery_ue.hpp | 144 + .../sti_fpattern_forward_1.hpp | 66 + .../sti_fpattern_forward_2.hpp | 65 + .../sti_fpattern_loop_start.hpp | 55 + .../sti_fpattern_return_1.hpp | 50 + .../sti_fpattern_rotate_1.hpp | 63 + .../sti_fpattern_rotate_2.hpp | 62 + .../sti_radial_end_point.hpp | 63 + .../sti_radial_loop_start.hpp | 59 + .../sti_radial_return.hpp | 55 + .../sti_radial_rotate.hpp | 57 + .../sti_spattern_forward_1.hpp | 48 + .../sti_spattern_forward_2.hpp | 48 + .../sti_spattern_forward_3.hpp | 46 + .../sti_spattern_forward_4.hpp | 53 + .../sti_spattern_loop_start.hpp | 60 + .../sti_spattern_rotate_1.hpp | 60 + .../sti_spattern_rotate_2.hpp | 61 + .../sti_spattern_rotate_3.hpp | 68 + .../sti_spattern_rotate_4.hpp | 63 + .../states/st_acquire_sensors.hpp | 66 + .../states/st_final_state.hpp | 51 + .../st_navigate_artgallery_waypoints.x.hpp | 70 + .../superstates/ss_f_pattern_1.hpp | 93 + .../superstates/ss_radial_pattern_1.hpp | 78 + .../superstates/ss_radial_pattern_2.hpp | 80 + .../superstates/ss_radial_pattern_3.hpp | 76 + .../superstates/ss_s_pattern_1.hpp | 93 + .../launch/bringup_launch.py | 137 + .../launch/husky_gazebo.launch.yaml | 3 + .../launch/navigation_launch.py | 177 + .../launch/online_sync_launch.py | 61 + .../launch/rviz_launch.py | 122 + .../launch/slam_launch.py | 128 + .../sm_dancebot_artgallery_ue_launch.py | 248 + .../sm_dancebot_artgallery_ue/package.xml | 48 + .../params/mapper_params_online_sync.yaml | 73 + .../params/nav2z_client/nav2_params.yaml | 373 + .../params/nav2z_client/navigation_tree.xml | 33 + .../nav2z_client/waypoints_art_gallery.yaml | 201 + .../sm_dancebot_artgallery_ue_config.yaml | 10 + .../rviz/nav2_default_view.rviz | 808 + .../rviz/nav2_namespaced_view.rviz | 371 + .../scripts/test_oscillation.py | 165 + .../scripts/transform_publisher.py | 123 + ..._navigation_frames_ground_truth_adapter.py | 115 + .../action/LEDControl.action | 18 + .../src/led_action_server_node.cpp | 230 + .../servers/service_node_3/service_node_3.py | 47 + .../src/temperature_sensor_node.cpp | 36 + .../nav2z_client/cb_active_stop.cpp | 54 + .../cb_position_control_free_space.cpp | 195 + .../clients/components/cp_ue_pose.cpp | 59 + .../sm_dancebot_artgallery_ue.cpp | 27 + .../urdf/turtlebot3_waffle.urdf | 293 + .../worlds/ridgeback_race.world | 4864 ++++++ .../worlds/ridgeback_race_empty.world | 4193 +++++ .../worlds/ridgeback_race_no_lidar.world | 4881 ++++++ .../sm_dancebot_ue/CHANGELOG.rst | 13067 ++++++++++++++++ .../sm_dancebot_ue/CMakeLists.txt | 113 + .../sm_dancebot_ue/README.md | 55 + .../sm_dancebot_ue/docker/.dockerignore | 0 .../sm_dancebot_ue/docker/.gitignore | 1 + .../sm_dancebot_ue/docker/Dockerfile | 133 + .../sm_dancebot_ue/docker/README.md | 275 + .../docker/attach_docker_container.sh | 1 + .../sm_dancebot_ue/docker/build_docker.sh | 43 + .../docker/build_docker_humble.sh | 4 + .../sm_dancebot_ue/docker/fastdds_setup.bash | 5 + .../sm_dancebot_ue/docker/join_bash.sh | 1 + .../sm_dancebot_ue/docker/join_editor.sh | 1 + .../docker/nvidia-driver-check.sh | 62 + .../docker/older/run_docker_container_bash.sh | 8 + .../older/run_docker_container_editor.sh | 8 + .../docker/older/run_editor_smacc.sh | 32 + .../sm_dancebot_ue/docker/openvpn/login.txt | 0 .../docker/openvpn/ovpnconfig.ovpn | 0 .../docker/openvpn/password.conf | 0 .../sm_dancebot_ue/docker/openvpn_file_run.sh | 7 + .../sm_dancebot_ue/docker/remove_container.sh | 2 + .../run_docker_container_bash_development.sh | 8 + .../docker/save_current_docker_image.sh | 11 + .../sm_dancebot_ue/docker/start_container.sh | 1 + .../sm_dancebot_ue/docker/stop_container.sh | 1 + .../docker/update_project_files.sh | 2 + .../client_behaviors/cb_active_stop.hpp | 46 + .../cb_load_waypoints_file.hpp | 72 + .../cb_navigate_next_waypoint_free.hpp | 60 + .../cb_position_control_free_space.hpp | 65 + .../client_behaviors/cb_pure_spinning.hpp | 126 + .../clients/components/cp_ue_pose.hpp | 56 + .../modestates/ms_dance_bot_recovery_mode.hpp | 38 + .../modestates/ms_dance_bot_run_mode.hpp | 37 + .../orthogonals/or_navigation.hpp | 45 + .../orthogonals/or_obstacle_perception.hpp | 39 + .../include/sm_dancebot_ue/sm_dancebot_ue.hpp | 149 + .../sti_fpattern_forward_1.hpp | 57 + .../sti_fpattern_forward_2.hpp | 51 + .../sti_fpattern_loop_start.hpp | 55 + .../sti_fpattern_return_1.hpp | 50 + .../sti_fpattern_rotate_1.hpp | 60 + .../sti_fpattern_rotate_2.hpp | 62 + .../sti_radial_end_point.hpp | 49 + .../sti_radial_loop_start.hpp | 59 + .../sti_radial_return.hpp | 55 + .../sti_radial_rotate.hpp | 57 + .../sti_spattern_forward_1.hpp | 48 + .../sti_spattern_forward_2.hpp | 48 + .../sti_spattern_forward_3.hpp | 46 + .../sti_spattern_forward_4.hpp | 50 + .../sti_spattern_loop_start.hpp | 60 + .../sti_spattern_rotate_1.hpp | 59 + .../sti_spattern_rotate_2.hpp | 61 + .../sti_spattern_rotate_3.hpp | 72 + .../sti_spattern_rotate_4.hpp | 60 + .../states/st_acquire_sensors.hpp | 73 + .../states/st_back_on_road_waypoints_x.hpp | 68 + .../sm_dancebot_ue/states/st_final_state.hpp | 51 + .../states/st_inital_road_waypoints_x.hpp | 64 + .../states/st_navigate_field_waypoints_x.hpp | 63 + .../sm_dancebot_ue/states/st_turn_around.hpp | 54 + .../superstates/ss_f_pattern_1.hpp | 91 + .../superstates/ss_radial_pattern_1.hpp | 77 + .../superstates/ss_radial_pattern_2.hpp | 80 + .../superstates/ss_radial_pattern_3.hpp | 75 + .../superstates/ss_s_pattern_1.hpp | 92 + .../sm_dancebot_ue/launch/bringup_launch.py | 137 + .../launch/husky_gazebo.launch.yaml | 3 + .../launch/navigation_launch.py | 169 + .../launch/online_sync_launch.py | 61 + .../sm_dancebot_ue/launch/rviz_launch.py | 122 + .../sm_dancebot_ue/launch/slam_launch.py | 128 + .../launch/sm_dancebot_ue_launch.py | 266 + .../sm_dancebot_ue/package.xml | 48 + .../params/mapper_params_online_sync.yaml | 73 + .../params/nav2z_client/nav2_params.yaml | 373 + .../params/nav2z_client/navigation_tree.xml | 33 + .../params/nav2z_client/waypoints_plan.yaml | 83 + .../waypoints_plan_back_on_road.yaml | 281 + .../waypoints_plan_initial_road.yaml | 251 + .../waypoints_plan_navigate_field.yaml | 111 + .../params/sm_dancebot_ue_config.yaml | 12 + .../rviz/nav2_default_view.rviz | 806 + .../rviz/nav2_namespaced_view.rviz | 371 + .../scripts/test_oscillation.py | 165 + .../scripts/transform_publisher.py | 78 + .../action/LEDControl.action | 18 + .../src/led_action_server_node.cpp | 230 + .../servers/service_node_3/service_node_3.py | 47 + .../src/temperature_sensor_node.cpp | 36 + .../nav2z_client/cb_active_stop.cpp | 53 + .../cb_position_control_free_space.cpp | 195 + .../clients/components/cp_ue_pose.cpp | 59 + .../src/sm_dancebot_ue/sm_dancebot_ue.cpp | 27 + .../urdf/turtlebot3_waffle.urdf | 293 + .../worlds/ridgeback_race.world | 4864 ++++++ .../worlds/ridgeback_race_empty.world | 4193 +++++ .../worlds/ridgeback_race_no_lidar.world | 4881 ++++++ 209 files changed, 69810 insertions(+), 69 deletions(-) create mode 100644 smacc2_client_library/nav2z_client/nav2z_client/include/nav2z_client/components/waypoints_navigator/cp_waypoints_navigator_base.hpp create mode 100644 smacc2_sm_reference_library/sm_dance_bot/launch/sm_dancebot_ue_launch.py create mode 100644 smacc2_sm_reference_library/sm_dancebot_artgallery_ue/CHANGELOG.rst create mode 100644 smacc2_sm_reference_library/sm_dancebot_artgallery_ue/CMakeLists.txt create mode 100644 smacc2_sm_reference_library/sm_dancebot_artgallery_ue/README.md create mode 100644 smacc2_sm_reference_library/sm_dancebot_artgallery_ue/docker/.dockerignore create mode 100644 smacc2_sm_reference_library/sm_dancebot_artgallery_ue/docker/.gitignore create mode 100644 smacc2_sm_reference_library/sm_dancebot_artgallery_ue/docker/Dockerfile create mode 100644 smacc2_sm_reference_library/sm_dancebot_artgallery_ue/docker/README.md create mode 100755 smacc2_sm_reference_library/sm_dancebot_artgallery_ue/docker/attach_docker_container.sh create mode 100755 smacc2_sm_reference_library/sm_dancebot_artgallery_ue/docker/build_docker.sh create mode 100755 smacc2_sm_reference_library/sm_dancebot_artgallery_ue/docker/build_docker_humble.sh create mode 100755 smacc2_sm_reference_library/sm_dancebot_artgallery_ue/docker/fastdds_setup.bash create mode 100644 smacc2_sm_reference_library/sm_dancebot_artgallery_ue/docker/frames_2023-08-05_01.06.53.gv create mode 100644 smacc2_sm_reference_library/sm_dancebot_artgallery_ue/docker/frames_2023-08-05_01.06.53.pdf create mode 100755 smacc2_sm_reference_library/sm_dancebot_artgallery_ue/docker/join_bash.sh create mode 100755 smacc2_sm_reference_library/sm_dancebot_artgallery_ue/docker/join_editor.sh create mode 100755 smacc2_sm_reference_library/sm_dancebot_artgallery_ue/docker/nvidia-driver-check.sh create mode 100755 smacc2_sm_reference_library/sm_dancebot_artgallery_ue/docker/older/run_docker_container_bash.sh create mode 100755 smacc2_sm_reference_library/sm_dancebot_artgallery_ue/docker/older/run_docker_container_editor.sh create mode 100755 smacc2_sm_reference_library/sm_dancebot_artgallery_ue/docker/older/run_editor_smacc.sh create mode 100644 smacc2_sm_reference_library/sm_dancebot_artgallery_ue/docker/openvpn/login.txt create mode 100644 smacc2_sm_reference_library/sm_dancebot_artgallery_ue/docker/openvpn/ovpnconfig.ovpn create mode 100644 smacc2_sm_reference_library/sm_dancebot_artgallery_ue/docker/openvpn/password.conf create mode 100755 smacc2_sm_reference_library/sm_dancebot_artgallery_ue/docker/openvpn_file_run.sh create mode 100755 smacc2_sm_reference_library/sm_dancebot_artgallery_ue/docker/remove_container.sh create mode 100755 smacc2_sm_reference_library/sm_dancebot_artgallery_ue/docker/run_docker_container_bash_development.sh create mode 100755 smacc2_sm_reference_library/sm_dancebot_artgallery_ue/docker/save_current_docker_image.sh create mode 100755 smacc2_sm_reference_library/sm_dancebot_artgallery_ue/docker/start_container.sh create mode 100755 smacc2_sm_reference_library/sm_dancebot_artgallery_ue/docker/stop_container.sh create mode 100755 smacc2_sm_reference_library/sm_dancebot_artgallery_ue/docker/update_project_files.sh create mode 100644 smacc2_sm_reference_library/sm_dancebot_artgallery_ue/include/sm_dancebot_artgallery_ue/clients/cl_nav2z/client_behaviors/cb_active_stop.hpp create mode 100644 smacc2_sm_reference_library/sm_dancebot_artgallery_ue/include/sm_dancebot_artgallery_ue/clients/cl_nav2z/client_behaviors/cb_load_waypoints_file.hpp create mode 100644 smacc2_sm_reference_library/sm_dancebot_artgallery_ue/include/sm_dancebot_artgallery_ue/clients/cl_nav2z/client_behaviors/cb_navigate_next_waypoint_free.hpp create mode 100644 smacc2_sm_reference_library/sm_dancebot_artgallery_ue/include/sm_dancebot_artgallery_ue/clients/cl_nav2z/client_behaviors/cb_position_control_free_space.hpp create mode 100644 smacc2_sm_reference_library/sm_dancebot_artgallery_ue/include/sm_dancebot_artgallery_ue/clients/cl_nav2z/client_behaviors/cb_pure_spinning.hpp create mode 100644 smacc2_sm_reference_library/sm_dancebot_artgallery_ue/include/sm_dancebot_artgallery_ue/clients/components/cp_ue_pose.hpp create mode 100644 smacc2_sm_reference_library/sm_dancebot_artgallery_ue/include/sm_dancebot_artgallery_ue/modestates/ms_dance_bot_recovery_mode.hpp create mode 100644 smacc2_sm_reference_library/sm_dancebot_artgallery_ue/include/sm_dancebot_artgallery_ue/modestates/ms_dance_bot_run_mode.hpp create mode 100644 smacc2_sm_reference_library/sm_dancebot_artgallery_ue/include/sm_dancebot_artgallery_ue/orthogonals/or_navigation.hpp create mode 100644 smacc2_sm_reference_library/sm_dancebot_artgallery_ue/include/sm_dancebot_artgallery_ue/orthogonals/or_obstacle_perception.hpp create mode 100644 smacc2_sm_reference_library/sm_dancebot_artgallery_ue/include/sm_dancebot_artgallery_ue/sm_dancebot_artgallery_ue.hpp create mode 100644 smacc2_sm_reference_library/sm_dancebot_artgallery_ue/include/sm_dancebot_artgallery_ue/states/f_pattern_states/sti_fpattern_forward_1.hpp create mode 100644 smacc2_sm_reference_library/sm_dancebot_artgallery_ue/include/sm_dancebot_artgallery_ue/states/f_pattern_states/sti_fpattern_forward_2.hpp create mode 100644 smacc2_sm_reference_library/sm_dancebot_artgallery_ue/include/sm_dancebot_artgallery_ue/states/f_pattern_states/sti_fpattern_loop_start.hpp create mode 100644 smacc2_sm_reference_library/sm_dancebot_artgallery_ue/include/sm_dancebot_artgallery_ue/states/f_pattern_states/sti_fpattern_return_1.hpp create mode 100644 smacc2_sm_reference_library/sm_dancebot_artgallery_ue/include/sm_dancebot_artgallery_ue/states/f_pattern_states/sti_fpattern_rotate_1.hpp create mode 100644 smacc2_sm_reference_library/sm_dancebot_artgallery_ue/include/sm_dancebot_artgallery_ue/states/f_pattern_states/sti_fpattern_rotate_2.hpp create mode 100644 smacc2_sm_reference_library/sm_dancebot_artgallery_ue/include/sm_dancebot_artgallery_ue/states/radial_motion_states/sti_radial_end_point.hpp create mode 100644 smacc2_sm_reference_library/sm_dancebot_artgallery_ue/include/sm_dancebot_artgallery_ue/states/radial_motion_states/sti_radial_loop_start.hpp create mode 100644 smacc2_sm_reference_library/sm_dancebot_artgallery_ue/include/sm_dancebot_artgallery_ue/states/radial_motion_states/sti_radial_return.hpp create mode 100644 smacc2_sm_reference_library/sm_dancebot_artgallery_ue/include/sm_dancebot_artgallery_ue/states/radial_motion_states/sti_radial_rotate.hpp create mode 100644 smacc2_sm_reference_library/sm_dancebot_artgallery_ue/include/sm_dancebot_artgallery_ue/states/s_pattern_states/sti_spattern_forward_1.hpp create mode 100644 smacc2_sm_reference_library/sm_dancebot_artgallery_ue/include/sm_dancebot_artgallery_ue/states/s_pattern_states/sti_spattern_forward_2.hpp create mode 100644 smacc2_sm_reference_library/sm_dancebot_artgallery_ue/include/sm_dancebot_artgallery_ue/states/s_pattern_states/sti_spattern_forward_3.hpp create mode 100644 smacc2_sm_reference_library/sm_dancebot_artgallery_ue/include/sm_dancebot_artgallery_ue/states/s_pattern_states/sti_spattern_forward_4.hpp create mode 100644 smacc2_sm_reference_library/sm_dancebot_artgallery_ue/include/sm_dancebot_artgallery_ue/states/s_pattern_states/sti_spattern_loop_start.hpp create mode 100644 smacc2_sm_reference_library/sm_dancebot_artgallery_ue/include/sm_dancebot_artgallery_ue/states/s_pattern_states/sti_spattern_rotate_1.hpp create mode 100644 smacc2_sm_reference_library/sm_dancebot_artgallery_ue/include/sm_dancebot_artgallery_ue/states/s_pattern_states/sti_spattern_rotate_2.hpp create mode 100644 smacc2_sm_reference_library/sm_dancebot_artgallery_ue/include/sm_dancebot_artgallery_ue/states/s_pattern_states/sti_spattern_rotate_3.hpp create mode 100644 smacc2_sm_reference_library/sm_dancebot_artgallery_ue/include/sm_dancebot_artgallery_ue/states/s_pattern_states/sti_spattern_rotate_4.hpp create mode 100644 smacc2_sm_reference_library/sm_dancebot_artgallery_ue/include/sm_dancebot_artgallery_ue/states/st_acquire_sensors.hpp create mode 100644 smacc2_sm_reference_library/sm_dancebot_artgallery_ue/include/sm_dancebot_artgallery_ue/states/st_final_state.hpp create mode 100644 smacc2_sm_reference_library/sm_dancebot_artgallery_ue/include/sm_dancebot_artgallery_ue/states/st_navigate_artgallery_waypoints.x.hpp create mode 100644 smacc2_sm_reference_library/sm_dancebot_artgallery_ue/include/sm_dancebot_artgallery_ue/superstates/ss_f_pattern_1.hpp create mode 100644 smacc2_sm_reference_library/sm_dancebot_artgallery_ue/include/sm_dancebot_artgallery_ue/superstates/ss_radial_pattern_1.hpp create mode 100644 smacc2_sm_reference_library/sm_dancebot_artgallery_ue/include/sm_dancebot_artgallery_ue/superstates/ss_radial_pattern_2.hpp create mode 100644 smacc2_sm_reference_library/sm_dancebot_artgallery_ue/include/sm_dancebot_artgallery_ue/superstates/ss_radial_pattern_3.hpp create mode 100644 smacc2_sm_reference_library/sm_dancebot_artgallery_ue/include/sm_dancebot_artgallery_ue/superstates/ss_s_pattern_1.hpp create mode 100644 smacc2_sm_reference_library/sm_dancebot_artgallery_ue/launch/bringup_launch.py create mode 100644 smacc2_sm_reference_library/sm_dancebot_artgallery_ue/launch/husky_gazebo.launch.yaml create mode 100644 smacc2_sm_reference_library/sm_dancebot_artgallery_ue/launch/navigation_launch.py create mode 100644 smacc2_sm_reference_library/sm_dancebot_artgallery_ue/launch/online_sync_launch.py create mode 100644 smacc2_sm_reference_library/sm_dancebot_artgallery_ue/launch/rviz_launch.py create mode 100644 smacc2_sm_reference_library/sm_dancebot_artgallery_ue/launch/slam_launch.py create mode 100644 smacc2_sm_reference_library/sm_dancebot_artgallery_ue/launch/sm_dancebot_artgallery_ue_launch.py create mode 100644 smacc2_sm_reference_library/sm_dancebot_artgallery_ue/package.xml create mode 100644 smacc2_sm_reference_library/sm_dancebot_artgallery_ue/params/mapper_params_online_sync.yaml create mode 100644 smacc2_sm_reference_library/sm_dancebot_artgallery_ue/params/nav2z_client/nav2_params.yaml create mode 100644 smacc2_sm_reference_library/sm_dancebot_artgallery_ue/params/nav2z_client/navigation_tree.xml create mode 100644 smacc2_sm_reference_library/sm_dancebot_artgallery_ue/params/nav2z_client/waypoints_art_gallery.yaml create mode 100644 smacc2_sm_reference_library/sm_dancebot_artgallery_ue/params/sm_dancebot_artgallery_ue_config.yaml create mode 100644 smacc2_sm_reference_library/sm_dancebot_artgallery_ue/rviz/nav2_default_view.rviz create mode 100644 smacc2_sm_reference_library/sm_dancebot_artgallery_ue/rviz/nav2_namespaced_view.rviz create mode 100755 smacc2_sm_reference_library/sm_dancebot_artgallery_ue/scripts/test_oscillation.py create mode 100644 smacc2_sm_reference_library/sm_dancebot_artgallery_ue/scripts/transform_publisher.py create mode 100755 smacc2_sm_reference_library/sm_dancebot_artgallery_ue/scripts/ue_navigation_frames_ground_truth_adapter.py create mode 100644 smacc2_sm_reference_library/sm_dancebot_artgallery_ue/servers/led_action_server/action/LEDControl.action create mode 100644 smacc2_sm_reference_library/sm_dancebot_artgallery_ue/servers/led_action_server/src/led_action_server_node.cpp create mode 100644 smacc2_sm_reference_library/sm_dancebot_artgallery_ue/servers/service_node_3/service_node_3.py create mode 100644 smacc2_sm_reference_library/sm_dancebot_artgallery_ue/servers/temperature_sensor_node/src/temperature_sensor_node.cpp create mode 100644 smacc2_sm_reference_library/sm_dancebot_artgallery_ue/src/sm_dancebot_artgallery_ue/clients/client_behaviors/nav2z_client/cb_active_stop.cpp create mode 100644 smacc2_sm_reference_library/sm_dancebot_artgallery_ue/src/sm_dancebot_artgallery_ue/clients/client_behaviors/nav2z_client/cb_position_control_free_space.cpp create mode 100644 smacc2_sm_reference_library/sm_dancebot_artgallery_ue/src/sm_dancebot_artgallery_ue/clients/components/cp_ue_pose.cpp create mode 100644 smacc2_sm_reference_library/sm_dancebot_artgallery_ue/src/sm_dancebot_artgallery_ue/sm_dancebot_artgallery_ue.cpp create mode 100644 smacc2_sm_reference_library/sm_dancebot_artgallery_ue/urdf/turtlebot3_waffle.urdf create mode 100644 smacc2_sm_reference_library/sm_dancebot_artgallery_ue/worlds/ridgeback_race.world create mode 100644 smacc2_sm_reference_library/sm_dancebot_artgallery_ue/worlds/ridgeback_race_empty.world create mode 100644 smacc2_sm_reference_library/sm_dancebot_artgallery_ue/worlds/ridgeback_race_no_lidar.world create mode 100644 smacc2_sm_reference_library/sm_dancebot_ue/CHANGELOG.rst create mode 100644 smacc2_sm_reference_library/sm_dancebot_ue/CMakeLists.txt create mode 100644 smacc2_sm_reference_library/sm_dancebot_ue/README.md create mode 100644 smacc2_sm_reference_library/sm_dancebot_ue/docker/.dockerignore create mode 100644 smacc2_sm_reference_library/sm_dancebot_ue/docker/.gitignore create mode 100644 smacc2_sm_reference_library/sm_dancebot_ue/docker/Dockerfile create mode 100644 smacc2_sm_reference_library/sm_dancebot_ue/docker/README.md create mode 100755 smacc2_sm_reference_library/sm_dancebot_ue/docker/attach_docker_container.sh create mode 100755 smacc2_sm_reference_library/sm_dancebot_ue/docker/build_docker.sh create mode 100755 smacc2_sm_reference_library/sm_dancebot_ue/docker/build_docker_humble.sh create mode 100755 smacc2_sm_reference_library/sm_dancebot_ue/docker/fastdds_setup.bash create mode 100755 smacc2_sm_reference_library/sm_dancebot_ue/docker/join_bash.sh create mode 100755 smacc2_sm_reference_library/sm_dancebot_ue/docker/join_editor.sh create mode 100755 smacc2_sm_reference_library/sm_dancebot_ue/docker/nvidia-driver-check.sh create mode 100755 smacc2_sm_reference_library/sm_dancebot_ue/docker/older/run_docker_container_bash.sh create mode 100755 smacc2_sm_reference_library/sm_dancebot_ue/docker/older/run_docker_container_editor.sh create mode 100755 smacc2_sm_reference_library/sm_dancebot_ue/docker/older/run_editor_smacc.sh create mode 100644 smacc2_sm_reference_library/sm_dancebot_ue/docker/openvpn/login.txt create mode 100644 smacc2_sm_reference_library/sm_dancebot_ue/docker/openvpn/ovpnconfig.ovpn create mode 100644 smacc2_sm_reference_library/sm_dancebot_ue/docker/openvpn/password.conf create mode 100755 smacc2_sm_reference_library/sm_dancebot_ue/docker/openvpn_file_run.sh create mode 100755 smacc2_sm_reference_library/sm_dancebot_ue/docker/remove_container.sh create mode 100755 smacc2_sm_reference_library/sm_dancebot_ue/docker/run_docker_container_bash_development.sh create mode 100755 smacc2_sm_reference_library/sm_dancebot_ue/docker/save_current_docker_image.sh create mode 100755 smacc2_sm_reference_library/sm_dancebot_ue/docker/start_container.sh create mode 100755 smacc2_sm_reference_library/sm_dancebot_ue/docker/stop_container.sh create mode 100755 smacc2_sm_reference_library/sm_dancebot_ue/docker/update_project_files.sh create mode 100644 smacc2_sm_reference_library/sm_dancebot_ue/include/sm_dancebot_ue/clients/cl_nav2z/client_behaviors/cb_active_stop.hpp create mode 100644 smacc2_sm_reference_library/sm_dancebot_ue/include/sm_dancebot_ue/clients/cl_nav2z/client_behaviors/cb_load_waypoints_file.hpp create mode 100644 smacc2_sm_reference_library/sm_dancebot_ue/include/sm_dancebot_ue/clients/cl_nav2z/client_behaviors/cb_navigate_next_waypoint_free.hpp create mode 100644 smacc2_sm_reference_library/sm_dancebot_ue/include/sm_dancebot_ue/clients/cl_nav2z/client_behaviors/cb_position_control_free_space.hpp create mode 100644 smacc2_sm_reference_library/sm_dancebot_ue/include/sm_dancebot_ue/clients/cl_nav2z/client_behaviors/cb_pure_spinning.hpp create mode 100644 smacc2_sm_reference_library/sm_dancebot_ue/include/sm_dancebot_ue/clients/components/cp_ue_pose.hpp create mode 100644 smacc2_sm_reference_library/sm_dancebot_ue/include/sm_dancebot_ue/modestates/ms_dance_bot_recovery_mode.hpp create mode 100644 smacc2_sm_reference_library/sm_dancebot_ue/include/sm_dancebot_ue/modestates/ms_dance_bot_run_mode.hpp create mode 100644 smacc2_sm_reference_library/sm_dancebot_ue/include/sm_dancebot_ue/orthogonals/or_navigation.hpp create mode 100644 smacc2_sm_reference_library/sm_dancebot_ue/include/sm_dancebot_ue/orthogonals/or_obstacle_perception.hpp create mode 100644 smacc2_sm_reference_library/sm_dancebot_ue/include/sm_dancebot_ue/sm_dancebot_ue.hpp create mode 100644 smacc2_sm_reference_library/sm_dancebot_ue/include/sm_dancebot_ue/states/f_pattern_states/sti_fpattern_forward_1.hpp create mode 100644 smacc2_sm_reference_library/sm_dancebot_ue/include/sm_dancebot_ue/states/f_pattern_states/sti_fpattern_forward_2.hpp create mode 100644 smacc2_sm_reference_library/sm_dancebot_ue/include/sm_dancebot_ue/states/f_pattern_states/sti_fpattern_loop_start.hpp create mode 100644 smacc2_sm_reference_library/sm_dancebot_ue/include/sm_dancebot_ue/states/f_pattern_states/sti_fpattern_return_1.hpp create mode 100644 smacc2_sm_reference_library/sm_dancebot_ue/include/sm_dancebot_ue/states/f_pattern_states/sti_fpattern_rotate_1.hpp create mode 100644 smacc2_sm_reference_library/sm_dancebot_ue/include/sm_dancebot_ue/states/f_pattern_states/sti_fpattern_rotate_2.hpp create mode 100644 smacc2_sm_reference_library/sm_dancebot_ue/include/sm_dancebot_ue/states/radial_motion_states/sti_radial_end_point.hpp create mode 100644 smacc2_sm_reference_library/sm_dancebot_ue/include/sm_dancebot_ue/states/radial_motion_states/sti_radial_loop_start.hpp create mode 100644 smacc2_sm_reference_library/sm_dancebot_ue/include/sm_dancebot_ue/states/radial_motion_states/sti_radial_return.hpp create mode 100644 smacc2_sm_reference_library/sm_dancebot_ue/include/sm_dancebot_ue/states/radial_motion_states/sti_radial_rotate.hpp create mode 100644 smacc2_sm_reference_library/sm_dancebot_ue/include/sm_dancebot_ue/states/s_pattern_states/sti_spattern_forward_1.hpp create mode 100644 smacc2_sm_reference_library/sm_dancebot_ue/include/sm_dancebot_ue/states/s_pattern_states/sti_spattern_forward_2.hpp create mode 100644 smacc2_sm_reference_library/sm_dancebot_ue/include/sm_dancebot_ue/states/s_pattern_states/sti_spattern_forward_3.hpp create mode 100644 smacc2_sm_reference_library/sm_dancebot_ue/include/sm_dancebot_ue/states/s_pattern_states/sti_spattern_forward_4.hpp create mode 100644 smacc2_sm_reference_library/sm_dancebot_ue/include/sm_dancebot_ue/states/s_pattern_states/sti_spattern_loop_start.hpp create mode 100644 smacc2_sm_reference_library/sm_dancebot_ue/include/sm_dancebot_ue/states/s_pattern_states/sti_spattern_rotate_1.hpp create mode 100644 smacc2_sm_reference_library/sm_dancebot_ue/include/sm_dancebot_ue/states/s_pattern_states/sti_spattern_rotate_2.hpp create mode 100644 smacc2_sm_reference_library/sm_dancebot_ue/include/sm_dancebot_ue/states/s_pattern_states/sti_spattern_rotate_3.hpp create mode 100644 smacc2_sm_reference_library/sm_dancebot_ue/include/sm_dancebot_ue/states/s_pattern_states/sti_spattern_rotate_4.hpp create mode 100644 smacc2_sm_reference_library/sm_dancebot_ue/include/sm_dancebot_ue/states/st_acquire_sensors.hpp create mode 100644 smacc2_sm_reference_library/sm_dancebot_ue/include/sm_dancebot_ue/states/st_back_on_road_waypoints_x.hpp create mode 100644 smacc2_sm_reference_library/sm_dancebot_ue/include/sm_dancebot_ue/states/st_final_state.hpp create mode 100644 smacc2_sm_reference_library/sm_dancebot_ue/include/sm_dancebot_ue/states/st_inital_road_waypoints_x.hpp create mode 100644 smacc2_sm_reference_library/sm_dancebot_ue/include/sm_dancebot_ue/states/st_navigate_field_waypoints_x.hpp create mode 100644 smacc2_sm_reference_library/sm_dancebot_ue/include/sm_dancebot_ue/states/st_turn_around.hpp create mode 100644 smacc2_sm_reference_library/sm_dancebot_ue/include/sm_dancebot_ue/superstates/ss_f_pattern_1.hpp create mode 100644 smacc2_sm_reference_library/sm_dancebot_ue/include/sm_dancebot_ue/superstates/ss_radial_pattern_1.hpp create mode 100644 smacc2_sm_reference_library/sm_dancebot_ue/include/sm_dancebot_ue/superstates/ss_radial_pattern_2.hpp create mode 100644 smacc2_sm_reference_library/sm_dancebot_ue/include/sm_dancebot_ue/superstates/ss_radial_pattern_3.hpp create mode 100644 smacc2_sm_reference_library/sm_dancebot_ue/include/sm_dancebot_ue/superstates/ss_s_pattern_1.hpp create mode 100644 smacc2_sm_reference_library/sm_dancebot_ue/launch/bringup_launch.py create mode 100644 smacc2_sm_reference_library/sm_dancebot_ue/launch/husky_gazebo.launch.yaml create mode 100644 smacc2_sm_reference_library/sm_dancebot_ue/launch/navigation_launch.py create mode 100644 smacc2_sm_reference_library/sm_dancebot_ue/launch/online_sync_launch.py create mode 100644 smacc2_sm_reference_library/sm_dancebot_ue/launch/rviz_launch.py create mode 100644 smacc2_sm_reference_library/sm_dancebot_ue/launch/slam_launch.py create mode 100644 smacc2_sm_reference_library/sm_dancebot_ue/launch/sm_dancebot_ue_launch.py create mode 100644 smacc2_sm_reference_library/sm_dancebot_ue/package.xml create mode 100644 smacc2_sm_reference_library/sm_dancebot_ue/params/mapper_params_online_sync.yaml create mode 100644 smacc2_sm_reference_library/sm_dancebot_ue/params/nav2z_client/nav2_params.yaml create mode 100644 smacc2_sm_reference_library/sm_dancebot_ue/params/nav2z_client/navigation_tree.xml create mode 100644 smacc2_sm_reference_library/sm_dancebot_ue/params/nav2z_client/waypoints_plan.yaml create mode 100644 smacc2_sm_reference_library/sm_dancebot_ue/params/nav2z_client/waypoints_plan_back_on_road.yaml create mode 100644 smacc2_sm_reference_library/sm_dancebot_ue/params/nav2z_client/waypoints_plan_initial_road.yaml create mode 100644 smacc2_sm_reference_library/sm_dancebot_ue/params/nav2z_client/waypoints_plan_navigate_field.yaml create mode 100644 smacc2_sm_reference_library/sm_dancebot_ue/params/sm_dancebot_ue_config.yaml create mode 100644 smacc2_sm_reference_library/sm_dancebot_ue/rviz/nav2_default_view.rviz create mode 100644 smacc2_sm_reference_library/sm_dancebot_ue/rviz/nav2_namespaced_view.rviz create mode 100755 smacc2_sm_reference_library/sm_dancebot_ue/scripts/test_oscillation.py create mode 100644 smacc2_sm_reference_library/sm_dancebot_ue/scripts/transform_publisher.py create mode 100644 smacc2_sm_reference_library/sm_dancebot_ue/servers/led_action_server/action/LEDControl.action create mode 100644 smacc2_sm_reference_library/sm_dancebot_ue/servers/led_action_server/src/led_action_server_node.cpp create mode 100644 smacc2_sm_reference_library/sm_dancebot_ue/servers/service_node_3/service_node_3.py create mode 100644 smacc2_sm_reference_library/sm_dancebot_ue/servers/temperature_sensor_node/src/temperature_sensor_node.cpp create mode 100644 smacc2_sm_reference_library/sm_dancebot_ue/src/sm_dancebot_ue/clients/client_behaviors/nav2z_client/cb_active_stop.cpp create mode 100644 smacc2_sm_reference_library/sm_dancebot_ue/src/sm_dancebot_ue/clients/client_behaviors/nav2z_client/cb_position_control_free_space.cpp create mode 100644 smacc2_sm_reference_library/sm_dancebot_ue/src/sm_dancebot_ue/clients/components/cp_ue_pose.cpp create mode 100644 smacc2_sm_reference_library/sm_dancebot_ue/src/sm_dancebot_ue/sm_dancebot_ue.cpp create mode 100644 smacc2_sm_reference_library/sm_dancebot_ue/urdf/turtlebot3_waffle.urdf create mode 100644 smacc2_sm_reference_library/sm_dancebot_ue/worlds/ridgeback_race.world create mode 100644 smacc2_sm_reference_library/sm_dancebot_ue/worlds/ridgeback_race_empty.world create mode 100644 smacc2_sm_reference_library/sm_dancebot_ue/worlds/ridgeback_race_no_lidar.world diff --git a/smacc2/include/smacc2/client_base_components/cp_topic_subscriber.hpp b/smacc2/include/smacc2/client_base_components/cp_topic_subscriber.hpp index 82602b2df..2e9550b18 100644 --- a/smacc2/include/smacc2/client_base_components/cp_topic_subscriber.hpp +++ b/smacc2/include/smacc2/client_base_components/cp_topic_subscriber.hpp @@ -44,12 +44,13 @@ class CpTopicSubscriber : public smacc2::ISmaccComponent virtual ~CpTopicSubscriber() {} - smacc2::SmaccSignal onFirstMessageReceived_; - smacc2::SmaccSignal onMessageReceived_; - std::function postMessageEvent; std::function postInitialMessageEvent; + smacc2::SmaccSignal onFirstMessageReceived_; + smacc2::SmaccSignal onMessageReceived_; + + // signal subscription method. This signal will be triggered when the first message is received template boost::signals2::connection onMessageReceived( void (T::*callback)(const MessageType &), T * object) @@ -57,6 +58,7 @@ class CpTopicSubscriber : public smacc2::ISmaccComponent return this->getStateMachine()->createSignalConnection(onMessageReceived_, callback, object); } + // signal subscription method. This signal will be triggered when the first message is received template boost::signals2::connection onFirstMessageReceived( void (T::*callback)(const MessageType &), T * object) diff --git a/smacc2/include/smacc2/impl/smacc_state_machine_impl.hpp b/smacc2/include/smacc2/impl/smacc_state_machine_impl.hpp index 02cbad34f..da0710d54 100644 --- a/smacc2/include/smacc2/impl/smacc_state_machine_impl.hpp +++ b/smacc2/include/smacc2/impl/smacc_state_machine_impl.hpp @@ -216,7 +216,7 @@ template void ISmaccStateMachine::postEvent(EventLifeTime evlifetime) { auto evname = smacc2::introspection::demangleSymbol(); - RCLCPP_DEBUG_STREAM(getLogger(), "Event " << evname); + RCLCPP_INFO_STREAM(getLogger(), "Event " << evname); auto * ev = new EventType(); this->postEvent(ev, evlifetime); } diff --git a/smacc2_client_library/nav2z_client/nav2z_client/CMakeLists.txt b/smacc2_client_library/nav2z_client/nav2z_client/CMakeLists.txt index 3835806d4..172c36af0 100644 --- a/smacc2_client_library/nav2z_client/nav2z_client/CMakeLists.txt +++ b/smacc2_client_library/nav2z_client/nav2z_client/CMakeLists.txt @@ -24,6 +24,7 @@ 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) set(dependencies smacc2 @@ -37,6 +38,7 @@ set(dependencies tf2_geometry_msgs bond slam_toolbox + ament_index_cpp ) include_directories(include) diff --git a/smacc2_client_library/nav2z_client/nav2z_client/include/nav2z_client/components/waypoints_navigator/cp_waypoints_navigator.hpp b/smacc2_client_library/nav2z_client/nav2z_client/include/nav2z_client/components/waypoints_navigator/cp_waypoints_navigator.hpp index 192d7b3bd..4e71561be 100644 --- a/smacc2_client_library/nav2z_client/nav2z_client/include/nav2z_client/components/waypoints_navigator/cp_waypoints_navigator.hpp +++ b/smacc2_client_library/nav2z_client/nav2z_client/include/nav2z_client/components/waypoints_navigator/cp_waypoints_navigator.hpp @@ -24,23 +24,14 @@ #include #include +#include namespace cl_nav2z { class ClNav2Z; -struct Pose2D +struct EvWaypointFinal : sc::event { - Pose2D(double x, double y, double yaw) - { - this->x_ = x; - this->y_ = y; - this->yaw_ = yaw; - } - - double x_; - double y_; - double yaw_; }; struct NavigateNextWaypointOptions @@ -52,11 +43,9 @@ struct NavigateNextWaypointOptions // 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 smacc2::ISmaccComponent +class CpWaypointNavigator : public CpWaypointNavigatorBase { public: - WaypointEventDispatcher waypointsEventDispatcher; - ClNav2Z * client_; CpWaypointNavigator(); @@ -69,14 +58,6 @@ class CpWaypointNavigator : public smacc2::ISmaccComponent 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); - std::optional > > > sendNextGoal( @@ -86,39 +67,17 @@ class CpWaypointNavigator : public smacc2::ISmaccComponent void stopWaitingResult(); - const std::vector & getWaypoints() const; - const std::vector & getWaypointNames() const; - std::optional getNamedPose(std::string name) const; - - long getCurrentWaypointIndex() const; - std::optional getCurrentWaypointName() const; - - long currentWaypoint_; - - void rewind(int count); - - void forward(int count); - void seekName(std::string name); - smacc2::SmaccSignal onNavigationRequestSucceded; smacc2::SmaccSignal onNavigationRequestAborted; smacc2::SmaccSignal onNavigationRequestCancelled; private: - void insertWaypoint(int index, geometry_msgs::msg::Pose & newpose); - - void removeWaypoint(int index); - void onNavigationResult(const ClNav2Z::WrappedResult & r); void onGoalReached(const ClNav2Z::WrappedResult & res); void onGoalCancelled(const ClNav2Z::WrappedResult & /*res*/); void onGoalAborted(const ClNav2Z::WrappedResult & /*res*/); - std::vector waypoints_; - - std::vector waypointsNames_; - boost::signals2::connection succeddedNav2ZClientConnection_; boost::signals2::connection abortedNav2ZClientConnection_; boost::signals2::connection cancelledNav2ZClientConnection_; diff --git a/smacc2_client_library/nav2z_client/nav2z_client/include/nav2z_client/components/waypoints_navigator/cp_waypoints_navigator_base.hpp b/smacc2_client_library/nav2z_client/nav2z_client/include/nav2z_client/components/waypoints_navigator/cp_waypoints_navigator_base.hpp new file mode 100644 index 000000000..cb6b12465 --- /dev/null +++ b/smacc2_client_library/nav2z_client/nav2z_client/include/nav2z_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_nav2z +{ +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_nav2z diff --git a/smacc2_client_library/nav2z_client/nav2z_client/package.xml b/smacc2_client_library/nav2z_client/nav2z_client/package.xml index 77fa73748..91eed3beb 100644 --- a/smacc2_client_library/nav2z_client/nav2z_client/package.xml +++ b/smacc2_client_library/nav2z_client/nav2z_client/package.xml @@ -24,6 +24,7 @@ tf2_geometry_msgs bond slam_toolbox + ament_index_cpp ament_cmake diff --git a/smacc2_client_library/nav2z_client/nav2z_client/src/nav2z_client/client_behaviors/cb_absolute_rotate.cpp b/smacc2_client_library/nav2z_client/nav2z_client/src/nav2z_client/client_behaviors/cb_absolute_rotate.cpp index 4042eb827..83ce75dcf 100644 --- a/smacc2_client_library/nav2z_client/nav2z_client/src/nav2z_client/client_behaviors/cb_absolute_rotate.cpp +++ b/smacc2_client_library/nav2z_client/nav2z_client/src/nav2z_client/client_behaviors/cb_absolute_rotate.cpp @@ -82,6 +82,7 @@ void CbAbsoluteRotate::onEntry() auto pathname = this->getCurrentState()->getName() + " - " + getName(); odomTracker_->pushPath(pathname); odomTracker_->setStartPoint(p->toPoseStampedMsg()); + odomTracker_->setCurrentMotionGoal(goal.pose); odomTracker_->setWorkingMode(odom_tracker::WorkingMode::RECORD_PATH); } diff --git a/smacc2_client_library/nav2z_client/nav2z_client/src/nav2z_client/client_behaviors/cb_nav2z_client_behavior_base.cpp b/smacc2_client_library/nav2z_client/nav2z_client/src/nav2z_client/client_behaviors/cb_nav2z_client_behavior_base.cpp index 6794e7bf5..31838233b 100644 --- a/smacc2_client_library/nav2z_client/nav2z_client/src/nav2z_client/client_behaviors/cb_nav2z_client_behavior_base.cpp +++ b/smacc2_client_library/nav2z_client/nav2z_client/src/nav2z_client/client_behaviors/cb_nav2z_client_behavior_base.cpp @@ -80,6 +80,9 @@ bool CbNav2ZClientBehaviorBase::isOwnActionResponse(const ClNav2Z::WrappedResult void CbNav2ZClientBehaviorBase::onNavigationResult(const ClNav2Z::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); diff --git a/smacc2_client_library/nav2z_client/nav2z_client/src/nav2z_client/client_behaviors/cb_navigate_forward.cpp b/smacc2_client_library/nav2z_client/nav2z_client/src/nav2z_client/client_behaviors/cb_navigate_forward.cpp index 76d813a4c..2a5a43a74 100644 --- a/smacc2_client_library/nav2z_client/nav2z_client/src/nav2z_client/client_behaviors/cb_navigate_forward.cpp +++ b/smacc2_client_library/nav2z_client/nav2z_client/src/nav2z_client/client_behaviors/cb_navigate_forward.cpp @@ -68,15 +68,13 @@ void CbNavigateForward::onEntry() auto p = nav2zClient_->getComponent(); auto referenceFrame = p->getReferenceFrame(); auto currentPoseMsg = p->toPoseMsg(); - tf2::Transform currentPose; - tf2::fromMsg(currentPoseMsg, currentPose); RCLCPP_INFO_STREAM( getLogger(), "[" << getName() << "]" << "current pose: " << currentPoseMsg); // force global orientation if it is requested - if (options.forwardSpeed) + if (options.forceInitialOrientation) { currentPoseMsg.orientation = *(options.forceInitialOrientation); RCLCPP_WARN_STREAM( @@ -85,6 +83,9 @@ void CbNavigateForward::onEntry() << "Forcing initial straight motion orientation: " << currentPoseMsg.orientation); } + tf2::Transform currentPose; + tf2::fromMsg(currentPoseMsg, currentPose); + tf2::Transform targetPose; if (goalPose_) { diff --git a/smacc2_client_library/nav2z_client/nav2z_client/src/nav2z_client/components/waypoints_navigator/cp_waypoints_navigator.cpp b/smacc2_client_library/nav2z_client/nav2z_client/src/nav2z_client/components/waypoints_navigator/cp_waypoints_navigator.cpp index 858b8e6eb..eb4923a17 100644 --- a/smacc2_client_library/nav2z_client/nav2z_client/src/nav2z_client/components/waypoints_navigator/cp_waypoints_navigator.cpp +++ b/smacc2_client_library/nav2z_client/nav2z_client/src/nav2z_client/components/waypoints_navigator/cp_waypoints_navigator.cpp @@ -21,6 +21,7 @@ #include #include +#include #include #include #include @@ -34,7 +35,13 @@ namespace cl_nav2z { using namespace std::chrono_literals; -CpWaypointNavigator::CpWaypointNavigator() : currentWaypoint_(0), waypoints_(0) {} +CpWaypointNavigatorBase::CpWaypointNavigatorBase() : currentWaypoint_(0), waypoints_(0) {} + +CpWaypointNavigatorBase::~CpWaypointNavigatorBase() {} + +CpWaypointNavigator::CpWaypointNavigator() {} + +void CpWaypointNavigatorBase::onInitialize() {} void CpWaypointNavigator::onInitialize() { client_ = dynamic_cast(owner_); } @@ -61,23 +68,25 @@ void CpWaypointNavigator::onGoalReached(const ClNav2Z::WrappedResult & /*res*/) currentWaypoint_); stopWaitingResult(); + this->notifyGoalReached(); + onNavigationRequestSucceded(); } -void CpWaypointNavigator::rewind(int /*count*/) +void CpWaypointNavigatorBase::rewind(int /*count*/) { currentWaypoint_--; if (currentWaypoint_ < 0) currentWaypoint_ = 0; } -void CpWaypointNavigator::forward(int /*count*/) +void CpWaypointNavigatorBase::forward(int /*count*/) { currentWaypoint_++; if (currentWaypoint_ >= (long)waypoints_.size() - 1) currentWaypoint_ = (long)waypoints_.size() - 1; } -void CpWaypointNavigator::seekName(std::string name) +void CpWaypointNavigatorBase::seekName(std::string name) { bool found = false; @@ -140,6 +149,34 @@ void CpWaypointNavigator::seekName(std::string name) 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()) @@ -258,6 +295,16 @@ CpWaypointNavigator::sendNextGoal( 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 ClNav2Z::WrappedResult & r) { if (r.code == rclcpp_action::ResultCode::SUCCEEDED) @@ -278,7 +325,7 @@ void CpWaypointNavigator::onNavigationResult(const ClNav2Z::WrappedResult & r) } } -void CpWaypointNavigator::insertWaypoint(int index, geometry_msgs::msg::Pose & newpose) +void CpWaypointNavigatorBase::insertWaypoint(int index, geometry_msgs::msg::Pose & newpose) { if (index >= 0 && index <= (int)waypoints_.size()) { @@ -286,12 +333,12 @@ void CpWaypointNavigator::insertWaypoint(int index, geometry_msgs::msg::Pose & n } } -void CpWaypointNavigator::setWaypoints(const std::vector & waypoints) +void CpWaypointNavigatorBase::setWaypoints(const std::vector & waypoints) { this->waypoints_ = waypoints; } -void CpWaypointNavigator::setWaypoints(const std::vector & waypoints) +void CpWaypointNavigatorBase::setWaypoints(const std::vector & waypoints) { waypoints_.clear(); waypointsNames_.clear(); @@ -311,7 +358,7 @@ void CpWaypointNavigator::setWaypoints(const std::vector & waypoints) } } -void CpWaypointNavigator::removeWaypoint(int index) +void CpWaypointNavigatorBase::removeWaypoint(int index) { if (index >= 0 && index < (int)waypoints_.size()) { @@ -319,12 +366,36 @@ void CpWaypointNavigator::removeWaypoint(int index) } } -const std::vector & CpWaypointNavigator::getWaypoints() const +const std::vector & CpWaypointNavigatorBase::getWaypoints() const { return waypoints_; } -std::optional CpWaypointNavigator::getNamedPose(std::string name) const +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) { @@ -340,12 +411,12 @@ std::optional CpWaypointNavigator::getNamedPose(std::s return std::nullopt; } -const std::vector & CpWaypointNavigator::getWaypointNames() const +const std::vector & CpWaypointNavigatorBase::getWaypointNames() const { return waypointsNames_; } -std::optional CpWaypointNavigator::getCurrentWaypointName() const +std::optional CpWaypointNavigatorBase::getCurrentWaypointName() const { if (currentWaypoint_ >= 0 && currentWaypoint_ < (int)waypointsNames_.size()) { @@ -354,12 +425,12 @@ std::optional CpWaypointNavigator::getCurrentWaypointName() const return std::nullopt; } -long CpWaypointNavigator::getCurrentWaypointIndex() const { return currentWaypoint_; } +long CpWaypointNavigatorBase::getCurrentWaypointIndex() const { return currentWaypoint_; } #define HAVE_NEW_YAMLCPP -void CpWaypointNavigator::loadWayPointsFromFile(std::string filepath) +void CpWaypointNavigatorBase::loadWayPointsFromFile(std::string filepath) { - RCLCPP_INFO_STREAM(getLogger(), "[CpWaypointNavigator] Loading file:" << 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) @@ -413,7 +484,7 @@ void CpWaypointNavigator::loadWayPointsFromFile(std::string filepath) } catch (...) { - RCLCPP_ERROR(getLogger(), "parsing waypoint file, syntax error in point %d", i); + RCLCPP_ERROR(getLogger(), "parsing waypoint file, syntax error in point %ld", i); } } RCLCPP_INFO_STREAM(getLogger(), "Parsed " << this->waypoints_.size() << " waypoints."); @@ -430,7 +501,7 @@ void CpWaypointNavigator::loadWayPointsFromFile(std::string filepath) } } -void CpWaypointNavigator::loadWayPointsFromFile2(std::string filepath) +void CpWaypointNavigatorBase::loadWayPointsFromFile2(std::string filepath) { RCLCPP_INFO_STREAM(getLogger(), "[CpWaypointNavigator] Loading file:" << filepath); this->waypoints_.clear(); @@ -476,7 +547,7 @@ void CpWaypointNavigator::loadWayPointsFromFile2(std::string filepath) } catch (...) { - RCLCPP_ERROR(getLogger(), "parsing waypoint file, syntax error in point %d", i); + RCLCPP_ERROR(getLogger(), "parsing waypoint file, syntax error in point %ld", i); } } RCLCPP_INFO_STREAM(getLogger(), "Parsed " << this->waypoints_.size() << " waypoints."); diff --git a/smacc2_dev_tools/.vscode/launch.json b/smacc2_dev_tools/.vscode/launch.json index fdea2811b..273872a9d 100644 --- a/smacc2_dev_tools/.vscode/launch.json +++ b/smacc2_dev_tools/.vscode/launch.json @@ -373,6 +373,56 @@ } ] }, + { + "name": "(gdb) SmDanceBotUE", + "type": "cppdbg", + "request": "launch", + "program": "${workspaceFolder}/../install/sm_dancebot_ue/lib/sm_dancebot_ue/sm_dancebot_ue_node", + "args": [ + "--ros-args", + "-r", + "__node:=SmDanceBotUE", + "--params-file", + "${workspaceFolder}/../install/sm_dancebot_ue/share/sm_dancebot_ue/params/sm_dancebot_ue_config.yaml" + ], + "stopAtEntry": false, + "cwd": "${workspaceFolder}", + "environment": [], + "externalConsole": false, + "MIMode": "gdb", + "setupCommands": [ + { + "description": "Enable pretty-printing for gdb", + "text": "-enable-pretty-printing", + "ignoreFailures": true + } + ] + }, + { + "name": "(gdb) SmDanceBotArtGalleryUE", + "type": "cppdbg", + "request": "launch", + "program": "${workspaceFolder}/../install/sm_dancebot_artgallery_ue/lib/sm_dancebot_artgallery_ue/sm_dancebot_artgallery_ue_node", + "args": [ + "--ros-args", + "-r", + "__node:=SmDanceBotArtGalleryUE", + "--params-file", + "${workspaceFolder}/../install/sm_dancebot_artgallery_ue/share/sm_dancebot_artgallery_ue/params/sm_dancebot_artgallery_ue_config.yaml" + ], + "stopAtEntry": false, + "cwd": "${workspaceFolder}", + "environment": [], + "externalConsole": false, + "MIMode": "gdb", + "setupCommands": [ + { + "description": "Enable pretty-printing for gdb", + "text": "-enable-pretty-printing", + "ignoreFailures": true + } + ] + }, { "name": "(gdb) SmDanceBotWarehouse2", "type": "cppdbg", diff --git a/smacc2_sm_reference_library/sm_dance_bot/launch/sm_dancebot_ue_launch.py b/smacc2_sm_reference_library/sm_dance_bot/launch/sm_dancebot_ue_launch.py new file mode 100644 index 000000000..7f9a595b6 --- /dev/null +++ b/smacc2_sm_reference_library/sm_dance_bot/launch/sm_dancebot_ue_launch.py @@ -0,0 +1,252 @@ +# Copyright (c) 2018 Intel Corporation +# +# Licensed under the Apache License, Version 2.0 (the "License"); +# you may not use this file except in compliance with the License. +# You may obtain a copy of the License at +# +# http://www.apache.org/licenses/LICENSE-2.0 +# +# Unless required by applicable law or agreed to in writing, software +# distributed under the License is distributed on an "AS IS" BASIS, +# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +# See the License for the specific language governing permissions and +# limitations under the License. + +"""This is all-in-one launch script intended for use by nav2 developers.""" + +import os + +from ament_index_python.packages import get_package_share_directory + +from launch import LaunchDescription +from launch.actions import DeclareLaunchArgument, IncludeLaunchDescription +from launch.conditions import IfCondition +from launch.launch_description_sources import PythonLaunchDescriptionSource +from launch.substitutions import LaunchConfiguration +from launch_ros.actions import Node + + +def generate_launch_description(): + # Get the launch directory + sm_dance_bot_dir = get_package_share_directory("sm_dance_bot_ue") + sm_dance_bot_launch_dir = os.path.join(sm_dance_bot_dir, "launch") + + # Create the launch configuration variables + slam = LaunchConfiguration("slam") + namespace = LaunchConfiguration("namespace") + use_namespace = LaunchConfiguration("use_namespace") + map_yaml_file = LaunchConfiguration("map") + use_sim_time = LaunchConfiguration("use_sim_time") + gazebo_headless = LaunchConfiguration("headless") + + params_file = LaunchConfiguration("params_file") + default_nav_to_pose_bt_xml = LaunchConfiguration("default_nav_to_pose_bt_xml") + autostart = LaunchConfiguration("autostart") + show_gz_lidar = LaunchConfiguration("show_gz_lidar") + + # Launch configuration variables specific to simulation + rviz_config_file = LaunchConfiguration("rviz_config_file") + + use_robot_state_pub = LaunchConfiguration("use_robot_state_pub") + use_rviz = LaunchConfiguration("use_rviz") + + urdf = os.path.join(sm_dance_bot_dir, "urdf", "turtlebot3_waffle.urdf") + + # Map fully qualified names to relative ones so the node's namespace can be prepended. + # In case of the transforms (tf), currently, there doesn't seem to be a better alternative + # https://github.com/ros/geometry2/issues/32 + # https://github.com/ros/robot_state_publisher/pull/30 + # TODO(orduno) Substitute with `PushNodeRemapping` + # https://github.com/ros2/launch_ros/issues/56 + remappings = [("/tf", "tf"), ("/tf_static", "tf_static")] + + # Declare the launch arguments + declare_namespace_cmd = DeclareLaunchArgument( + "namespace", default_value="", description="Top-level namespace" + ) + + declare_use_namespace_cmd = DeclareLaunchArgument( + "use_namespace", + default_value="false", + description="Whether to apply a namespace to the navigation stack", + ) + + declare_show_gz_lidar = DeclareLaunchArgument( + "show_gz_lidar", + default_value="true", + description="Whether to apply a namespace to the navigation stack", + ) + + declare_slam_cmd = DeclareLaunchArgument( + "slam", default_value="True", description="Whether run a SLAM" + ) + + declare_use_sim_time_cmd = DeclareLaunchArgument( + "use_sim_time", default_value="true", description="Use simulation (Gazebo) clock if true" + ) + + declare_gazebo_headless_cmd = DeclareLaunchArgument( + "headless", + default_value="false", + description="Use headless Gazebo if true", + ) + + declare_params_file_cmd = DeclareLaunchArgument( + "params_file", + default_value=os.path.join(sm_dance_bot_dir, "params", "nav2z_client", "nav2_params.yaml"), + description="Full path to the ROS2 parameters file to use for all launched nodes", + ) + + declare_bt_xml_cmd = DeclareLaunchArgument( + "default_nav_to_pose_bt_xml", + default_value=os.path.join( + sm_dance_bot_dir, "params", "nav2z_client", "navigation_tree.xml" + ), + description="Full path to the behavior tree xml file to use", + ) + + declare_autostart_cmd = DeclareLaunchArgument( + "autostart", default_value="true", description="Automatically startup the nav2 stack" + ) + + declare_rviz_config_file_cmd = DeclareLaunchArgument( + "rviz_config_file", + default_value=os.path.join(sm_dance_bot_dir, "rviz", "nav2_default_view.rviz"), + description="Full path to the RVIZ config file to use", + ) + + declare_use_robot_state_pub_cmd = DeclareLaunchArgument( + "use_robot_state_pub", + default_value="True", + description="Whether to start the robot state publisher", + ) + + declare_use_rviz_cmd = DeclareLaunchArgument( + "use_rviz", default_value="True", description="Whether to start RVIZ" + ) + + declare_map_yaml_cmd = DeclareLaunchArgument( + "map", + default_value=os.path.join(sm_dance_bot_dir, "maps", "turtlebot3_world.yaml"), + description="Full path to map file to load", + ) + + start_robot_state_publisher_cmd = Node( + condition=IfCondition(use_robot_state_pub), + package="robot_state_publisher", + executable="robot_state_publisher", + name="robot_state_publisher", + namespace=namespace, + output="screen", + parameters=[{"use_sim_time": use_sim_time}], + remappings=remappings, + arguments=[urdf], + ) + + rviz_cmd = IncludeLaunchDescription( + PythonLaunchDescriptionSource(os.path.join(sm_dance_bot_launch_dir, "rviz_launch.py")), + condition=IfCondition(use_rviz), + launch_arguments={ + "namespace": "", + "use_namespace": "False", + "rviz_config": rviz_config_file, + }.items(), + ) + + bringup_cmd = IncludeLaunchDescription( + PythonLaunchDescriptionSource(os.path.join(sm_dance_bot_launch_dir, "bringup_launch.py")), + launch_arguments={ + "namespace": namespace, + "use_namespace": use_namespace, + "autostart": autostart, + "params_file": params_file, + "slam": slam, + "map": map_yaml_file, + "use_sim_time": use_sim_time, + "default_nav_to_pose_bt_xml": default_nav_to_pose_bt_xml, + }.items(), + ) + + gazebo_simulator = IncludeLaunchDescription( + PythonLaunchDescriptionSource(os.path.join(sm_dance_bot_launch_dir, "gazebo_launch.py")), + launch_arguments={"show_gz_lidar": show_gz_lidar, "headless": gazebo_headless}.items(), + ) + + xtermprefix = "xterm -xrm 'XTerm*scrollBar: true' -xrm 'xterm*rightScrollBar: true' -hold -geometry 1000x600 -sl 10000 -e" + + sm_dance_bot_node = Node( + package="sm_dance_bot", + executable="sm_dance_bot_node", + name="SmDanceBot", + output="screen", + prefix=xtermprefix, + parameters=[ + os.path.join( + get_package_share_directory("sm_dance_bot"), + "params/sm_dance_bot_config.yaml", + ) + ], + remappings=[ + # ("/odom", "/odometry/filtered"), + # ("/sm_dance_bot_2/odom_tracker/odom_tracker_path", "/odom_tracker_path"), + # ("/sm_dance_bot_2/odom_tracker/odom_tracker_stacked_path", "/odom_tracker_path_stacked") + ], + arguments=["--ros-args", "--log-level", "INFO"], + ) + + led_action_server_node = Node( + package="sm_dance_bot", + executable="led_action_server_node", + output="screen", + prefix=xtermprefix, + ) + + temperature_action_server = Node( + package="sm_dance_bot", + executable="temperature_sensor_node", + output="screen", + prefix=xtermprefix, + ) + + service3_node = Node( + package="sm_dance_bot", + executable="service_node_3.py", + output="screen", + prefix=xtermprefix, + parameters=[ + {"autostart": True, "node_names": ["ss", "dfa"]}, + ], + ) + + # Create the launch description and populate + ld = LaunchDescription() + + # Declare the launch options + ld.add_action(declare_namespace_cmd) + ld.add_action(declare_use_namespace_cmd) + ld.add_action(declare_slam_cmd) + ld.add_action(declare_map_yaml_cmd) + ld.add_action(declare_use_sim_time_cmd) + ld.add_action(declare_gazebo_headless_cmd) + ld.add_action(declare_params_file_cmd) + ld.add_action(declare_bt_xml_cmd) + ld.add_action(declare_autostart_cmd) + ld.add_action(declare_map_yaml_cmd) + ld.add_action(declare_show_gz_lidar) + + ld.add_action(declare_rviz_config_file_cmd) + ld.add_action(declare_use_robot_state_pub_cmd) + ld.add_action(declare_use_rviz_cmd) + ld.add_action(gazebo_simulator) + + ld.add_action(sm_dance_bot_node) + ld.add_action(service3_node) + ld.add_action(temperature_action_server) + ld.add_action(led_action_server_node) + + # # Add the actions to launch all of the navigation nodes + ld.add_action(start_robot_state_publisher_cmd) + ld.add_action(rviz_cmd) + ld.add_action(bringup_cmd) + + return ld diff --git a/smacc2_sm_reference_library/sm_dancebot_artgallery_ue/CHANGELOG.rst b/smacc2_sm_reference_library/sm_dancebot_artgallery_ue/CHANGELOG.rst new file mode 100644 index 000000000..96ca799e4 --- /dev/null +++ b/smacc2_sm_reference_library/sm_dancebot_artgallery_ue/CHANGELOG.rst @@ -0,0 +1,13067 @@ +^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Changelog for package sm_dancebot_artgallery_ue +^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ + +2.3.6 (2023-03-12) +------------------ + +1.22.1 (2022-11-09) +------------------- +* pre-release +* Contributors: pabloinigoblasco + +* publisher +* Feature/warehouse 3 improvements (#313) + * 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 +* 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 +* Feature/galactic rolling merge (#288) + * 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_dancebot_artgallery_ue + * 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_dancebot_artgallery_ue + * 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_dancebot_artgallery_ue + * 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_dancebot_artgallery_ue visualizing turtlebot3 (#101) + * Feature/dance bot launch gz lidar choice (#102) + * sm_dancebot_artgallery_ue 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_dancebot_artgallery_ue 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_dancebot_artgallery_ue 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_dancebot_artgallery_ue 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_dancebot_artgallery_ue (#125) + * Move method after the method it calls. Otherwise recursion could happen. (#126) + * Feature/dance bot s pattern (#128) + * more changes in sm_dancebot_artgallery_ue + * polishing sm_dancebot_artgallery_ue and s-pattern + * noticed typo + Finnaly > Finally + * Feature/dance bot s pattern (#129) + * more changes in sm_dancebot_artgallery_ue + * polishing sm_dancebot_artgallery_ue and s-pattern + * more refinement in sm_dancebot_artgallery_ue + * 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_dancebot_artgallery_ue + * minor + * Feature/sm dance bot refine 2 (#132) + * more changes in sm_dancebot_artgallery_ue + * minor + * build fix + * waypoints navigator bug (#133) + * minor tuning to mitigate overshot issue cases + * progress in the sm_dancebot_artgallery_ue 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/nav2z 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_dancebot_artgallery_ue + * 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_dancebot_artgallery_ue + * 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_dancebot_artgallery_ue + * 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_dancebot_artgallery_ue + * 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_dancebot_artgallery_ue visualizing turtlebot3 (#101) + * Feature/dance bot launch gz lidar choice (#102) + * sm_dancebot_artgallery_ue 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_dancebot_artgallery_ue 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_dancebot_artgallery_ue 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_dancebot_artgallery_ue 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_dancebot_artgallery_ue (#125) + * Move method after the method it calls. Otherwise recursion could happen. (#126) + * Feature/dance bot s pattern (#128) + * more changes in sm_dancebot_artgallery_ue + * polishing sm_dancebot_artgallery_ue and s-pattern + * noticed typo + Finnaly > Finally + * Feature/dance bot s pattern (#129) + * more changes in sm_dancebot_artgallery_ue + * polishing sm_dancebot_artgallery_ue and s-pattern + * more refinement in sm_dancebot_artgallery_ue + * 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_dancebot_artgallery_ue + * minor + * Feature/sm dance bot refine 2 (#132) + * more changes in sm_dancebot_artgallery_ue + * minor + * build fix + * waypoints navigator bug (#133) + * minor tuning to mitigate overshot issue cases + * progress in the sm_dancebot_artgallery_ue 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/nav2z 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_dancebot_artgallery_ue + * 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_dancebot_artgallery_ue + * 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_dancebot_artgallery_ue + * 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_dancebot_artgallery_ue + * 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_dancebot_artgallery_ue visualizing turtlebot3 (#101) + * Feature/dance bot launch gz lidar choice (#102) + * sm_dancebot_artgallery_ue 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_dancebot_artgallery_ue 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_dancebot_artgallery_ue 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_dancebot_artgallery_ue 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_dancebot_artgallery_ue (#125) + * Move method after the method it calls. Otherwise recursion could happen. (#126) + * Feature/dance bot s pattern (#128) + * more changes in sm_dancebot_artgallery_ue + * polishing sm_dancebot_artgallery_ue and s-pattern + * noticed typo + Finnaly > Finally + * Feature/dance bot s pattern (#129) + * more changes in sm_dancebot_artgallery_ue + * polishing sm_dancebot_artgallery_ue and s-pattern + * more refinement in sm_dancebot_artgallery_ue + * 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_dancebot_artgallery_ue + * minor + * Feature/sm dance bot refine 2 (#132) + * more changes in sm_dancebot_artgallery_ue + * minor + * build fix + * waypoints navigator bug (#133) + * minor tuning to mitigate overshot issue cases + * progress in the sm_dancebot_artgallery_ue 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/nav2z 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_dancebot_artgallery_ue + * 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_dancebot_artgallery_ue + * 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_dancebot_artgallery_ue + * 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_dancebot_artgallery_ue + * 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_dancebot_artgallery_ue visualizing turtlebot3 (#101) + * Feature/dance bot launch gz lidar choice (#102) + * sm_dancebot_artgallery_ue 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_dancebot_artgallery_ue 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_dancebot_artgallery_ue 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_dancebot_artgallery_ue 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_dancebot_artgallery_ue (#125) + * Move method after the method it calls. Otherwise recursion could happen. (#126) + * Feature/dance bot s pattern (#128) + * more changes in sm_dancebot_artgallery_ue + * polishing sm_dancebot_artgallery_ue and s-pattern + * noticed typo + Finnaly > Finally + * Feature/dance bot s pattern (#129) + * more changes in sm_dancebot_artgallery_ue + * polishing sm_dancebot_artgallery_ue and s-pattern + * more refinement in sm_dancebot_artgallery_ue + * 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_dancebot_artgallery_ue + * minor + * Feature/sm dance bot refine 2 (#132) + * more changes in sm_dancebot_artgallery_ue + * minor + * build fix + * waypoints navigator bug (#133) + * minor tuning to mitigate overshot issue cases + * progress in the sm_dancebot_artgallery_ue 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/nav2z 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_dancebot_artgallery_ue + * 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_dancebot_artgallery_ue + * 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_dancebot_artgallery_ue + * 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_dancebot_artgallery_ue + * 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_dancebot_artgallery_ue visualizing turtlebot3 (#101) + * Feature/dance bot launch gz lidar choice (#102) + * sm_dancebot_artgallery_ue 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_dancebot_artgallery_ue 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_dancebot_artgallery_ue 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_dancebot_artgallery_ue 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_dancebot_artgallery_ue (#125) + * Move method after the method it calls. Otherwise recursion could happen. (#126) + * Feature/dance bot s pattern (#128) + * more changes in sm_dancebot_artgallery_ue + * polishing sm_dancebot_artgallery_ue and s-pattern + * noticed typo + Finnaly > Finally + * Feature/dance bot s pattern (#129) + * more changes in sm_dancebot_artgallery_ue + * polishing sm_dancebot_artgallery_ue and s-pattern + * more refinement in sm_dancebot_artgallery_ue + * 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_dancebot_artgallery_ue + * minor + * Feature/sm dance bot refine 2 (#132) + * more changes in sm_dancebot_artgallery_ue + * minor + * build fix + * waypoints navigator bug (#133) + * minor tuning to mitigate overshot issue cases + * progress in the sm_dancebot_artgallery_ue 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/nav2z 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_dancebot_artgallery_ue + * 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_dancebot_artgallery_ue + * 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_dancebot_artgallery_ue + * 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_dancebot_artgallery_ue + * 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_dancebot_artgallery_ue visualizing turtlebot3 (#101) + * Feature/dance bot launch gz lidar choice (#102) + * sm_dancebot_artgallery_ue 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_dancebot_artgallery_ue 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_dancebot_artgallery_ue 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_dancebot_artgallery_ue 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_dancebot_artgallery_ue (#125) + * Move method after the method it calls. Otherwise recursion could happen. (#126) + * Feature/dance bot s pattern (#128) + * more changes in sm_dancebot_artgallery_ue + * polishing sm_dancebot_artgallery_ue and s-pattern + * noticed typo + Finnaly > Finally + * Feature/dance bot s pattern (#129) + * more changes in sm_dancebot_artgallery_ue + * polishing sm_dancebot_artgallery_ue and s-pattern + * more refinement in sm_dancebot_artgallery_ue + * 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_dancebot_artgallery_ue + * minor + * Feature/sm dance bot refine 2 (#132) + * more changes in sm_dancebot_artgallery_ue + * minor + * build fix + * waypoints navigator bug (#133) + * minor tuning to mitigate overshot issue cases + * progress in the sm_dancebot_artgallery_ue 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/nav2z 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_dancebot_artgallery_ue + * 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_dancebot_artgallery_ue + * 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_dancebot_artgallery_ue + * 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_dancebot_artgallery_ue + * 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_dancebot_artgallery_ue visualizing turtlebot3 (#101) + * Feature/dance bot launch gz lidar choice (#102) + * sm_dancebot_artgallery_ue 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_dancebot_artgallery_ue 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_dancebot_artgallery_ue 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_dancebot_artgallery_ue 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_dancebot_artgallery_ue (#125) + * Move method after the method it calls. Otherwise recursion could happen. (#126) + * Feature/dance bot s pattern (#128) + * more changes in sm_dancebot_artgallery_ue + * polishing sm_dancebot_artgallery_ue and s-pattern + * noticed typo + Finnaly > Finally + * Feature/dance bot s pattern (#129) + * more changes in sm_dancebot_artgallery_ue + * polishing sm_dancebot_artgallery_ue and s-pattern + * more refinement in sm_dancebot_artgallery_ue + * 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_dancebot_artgallery_ue + * minor + * Feature/sm dance bot refine 2 (#132) + * more changes in sm_dancebot_artgallery_ue + * minor + * build fix + * waypoints navigator bug (#133) + * minor tuning to mitigate overshot issue cases + * progress in the sm_dancebot_artgallery_ue 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/nav2z 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_dancebot_artgallery_ue + * 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 + * Ignore packages which should not be released. + * Added changelogs. + * 0.4.0 + * Revert "Ignore packages which should not be released." + This reverts commit ee2cc86db3c0a24f9eb0a9e33217de3f7a691a1c. + * Fix urls to index.ros.org (#284) + * Fix foxy source build config to use repos file from foxy branch. (#285) + * adding spawn entity delays + * more on backport + * more on backport + * disappeared ur_msgs denis repo + * fixing sm_dancebot_artgallery_ue examples + * working on fix of image messages for husky_barrel demo + 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) +------------------ +* publisher +* Feature/warehouse 3 improvements (#313) + * 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 +* 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 +* Feature/galactic rolling merge (#288) + * 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_dancebot_artgallery_ue + * 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_dancebot_artgallery_ue + * 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_dancebot_artgallery_ue + * 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_dancebot_artgallery_ue visualizing turtlebot3 (#101) + * Feature/dance bot launch gz lidar choice (#102) + * sm_dancebot_artgallery_ue 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_dancebot_artgallery_ue 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_dancebot_artgallery_ue 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_dancebot_artgallery_ue 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_dancebot_artgallery_ue (#125) + * Move method after the method it calls. Otherwise recursion could happen. (#126) + * Feature/dance bot s pattern (#128) + * more changes in sm_dancebot_artgallery_ue + * polishing sm_dancebot_artgallery_ue and s-pattern + * noticed typo + Finnaly > Finally + * Feature/dance bot s pattern (#129) + * more changes in sm_dancebot_artgallery_ue + * polishing sm_dancebot_artgallery_ue and s-pattern + * more refinement in sm_dancebot_artgallery_ue + * 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_dancebot_artgallery_ue + * minor + * Feature/sm dance bot refine 2 (#132) + * more changes in sm_dancebot_artgallery_ue + * minor + * build fix + * waypoints navigator bug (#133) + * minor tuning to mitigate overshot issue cases + * progress in the sm_dancebot_artgallery_ue 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/nav2z 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_dancebot_artgallery_ue + * 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_dancebot_artgallery_ue + * 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_dancebot_artgallery_ue + * 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_dancebot_artgallery_ue + * 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_dancebot_artgallery_ue visualizing turtlebot3 (#101) + * Feature/dance bot launch gz lidar choice (#102) + * sm_dancebot_artgallery_ue 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_dancebot_artgallery_ue 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_dancebot_artgallery_ue 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_dancebot_artgallery_ue 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_dancebot_artgallery_ue (#125) + * Move method after the method it calls. Otherwise recursion could happen. (#126) + * Feature/dance bot s pattern (#128) + * more changes in sm_dancebot_artgallery_ue + * polishing sm_dancebot_artgallery_ue and s-pattern + * noticed typo + Finnaly > Finally + * Feature/dance bot s pattern (#129) + * more changes in sm_dancebot_artgallery_ue + * polishing sm_dancebot_artgallery_ue and s-pattern + * more refinement in sm_dancebot_artgallery_ue + * 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_dancebot_artgallery_ue + * minor + * Feature/sm dance bot refine 2 (#132) + * more changes in sm_dancebot_artgallery_ue + * minor + * build fix + * waypoints navigator bug (#133) + * minor tuning to mitigate overshot issue cases + * progress in the sm_dancebot_artgallery_ue 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/nav2z 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_dancebot_artgallery_ue + * 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_dancebot_artgallery_ue + * 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_dancebot_artgallery_ue + * 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_dancebot_artgallery_ue + * 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_dancebot_artgallery_ue visualizing turtlebot3 (#101) + * Feature/dance bot launch gz lidar choice (#102) + * sm_dancebot_artgallery_ue 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_dancebot_artgallery_ue 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_dancebot_artgallery_ue 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_dancebot_artgallery_ue 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_dancebot_artgallery_ue (#125) + * Move method after the method it calls. Otherwise recursion could happen. (#126) + * Feature/dance bot s pattern (#128) + * more changes in sm_dancebot_artgallery_ue + * polishing sm_dancebot_artgallery_ue and s-pattern + * noticed typo + Finnaly > Finally + * Feature/dance bot s pattern (#129) + * more changes in sm_dancebot_artgallery_ue + * polishing sm_dancebot_artgallery_ue and s-pattern + * more refinement in sm_dancebot_artgallery_ue + * 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_dancebot_artgallery_ue + * minor + * Feature/sm dance bot refine 2 (#132) + * more changes in sm_dancebot_artgallery_ue + * minor + * build fix + * waypoints navigator bug (#133) + * minor tuning to mitigate overshot issue cases + * progress in the sm_dancebot_artgallery_ue 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/nav2z 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_dancebot_artgallery_ue + * 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_dancebot_artgallery_ue + * 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_dancebot_artgallery_ue + * 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_dancebot_artgallery_ue + * 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_dancebot_artgallery_ue visualizing turtlebot3 (#101) + * Feature/dance bot launch gz lidar choice (#102) + * sm_dancebot_artgallery_ue 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_dancebot_artgallery_ue 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_dancebot_artgallery_ue 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_dancebot_artgallery_ue 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_dancebot_artgallery_ue (#125) + * Move method after the method it calls. Otherwise recursion could happen. (#126) + * Feature/dance bot s pattern (#128) + * more changes in sm_dancebot_artgallery_ue + * polishing sm_dancebot_artgallery_ue and s-pattern + * noticed typo + Finnaly > Finally + * Feature/dance bot s pattern (#129) + * more changes in sm_dancebot_artgallery_ue + * polishing sm_dancebot_artgallery_ue and s-pattern + * more refinement in sm_dancebot_artgallery_ue + * 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_dancebot_artgallery_ue + * minor + * Feature/sm dance bot refine 2 (#132) + * more changes in sm_dancebot_artgallery_ue + * minor + * build fix + * waypoints navigator bug (#133) + * minor tuning to mitigate overshot issue cases + * progress in the sm_dancebot_artgallery_ue 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/nav2z 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_dancebot_artgallery_ue + * 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_dancebot_artgallery_ue + * 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_dancebot_artgallery_ue + * 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_dancebot_artgallery_ue + * 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_dancebot_artgallery_ue visualizing turtlebot3 (#101) + * Feature/dance bot launch gz lidar choice (#102) + * sm_dancebot_artgallery_ue 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_dancebot_artgallery_ue 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_dancebot_artgallery_ue 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_dancebot_artgallery_ue 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_dancebot_artgallery_ue (#125) + * Move method after the method it calls. Otherwise recursion could happen. (#126) + * Feature/dance bot s pattern (#128) + * more changes in sm_dancebot_artgallery_ue + * polishing sm_dancebot_artgallery_ue and s-pattern + * noticed typo + Finnaly > Finally + * Feature/dance bot s pattern (#129) + * more changes in sm_dancebot_artgallery_ue + * polishing sm_dancebot_artgallery_ue and s-pattern + * more refinement in sm_dancebot_artgallery_ue + * 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_dancebot_artgallery_ue + * minor + * Feature/sm dance bot refine 2 (#132) + * more changes in sm_dancebot_artgallery_ue + * minor + * build fix + * waypoints navigator bug (#133) + * minor tuning to mitigate overshot issue cases + * progress in the sm_dancebot_artgallery_ue 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/nav2z 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_dancebot_artgallery_ue + * 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_dancebot_artgallery_ue + * 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_dancebot_artgallery_ue + * 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_dancebot_artgallery_ue + * 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_dancebot_artgallery_ue visualizing turtlebot3 (#101) + * Feature/dance bot launch gz lidar choice (#102) + * sm_dancebot_artgallery_ue 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_dancebot_artgallery_ue 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_dancebot_artgallery_ue 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_dancebot_artgallery_ue 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_dancebot_artgallery_ue (#125) + * Move method after the method it calls. Otherwise recursion could happen. (#126) + * Feature/dance bot s pattern (#128) + * more changes in sm_dancebot_artgallery_ue + * polishing sm_dancebot_artgallery_ue and s-pattern + * noticed typo + Finnaly > Finally + * Feature/dance bot s pattern (#129) + * more changes in sm_dancebot_artgallery_ue + * polishing sm_dancebot_artgallery_ue and s-pattern + * more refinement in sm_dancebot_artgallery_ue + * 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_dancebot_artgallery_ue + * minor + * Feature/sm dance bot refine 2 (#132) + * more changes in sm_dancebot_artgallery_ue + * minor + * build fix + * waypoints navigator bug (#133) + * minor tuning to mitigate overshot issue cases + * progress in the sm_dancebot_artgallery_ue 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/nav2z 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_dancebot_artgallery_ue + * 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_dancebot_artgallery_ue + * 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_dancebot_artgallery_ue + * 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_dancebot_artgallery_ue + * 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_dancebot_artgallery_ue visualizing turtlebot3 (#101) + * Feature/dance bot launch gz lidar choice (#102) + * sm_dancebot_artgallery_ue 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_dancebot_artgallery_ue 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_dancebot_artgallery_ue 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_dancebot_artgallery_ue 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_dancebot_artgallery_ue (#125) + * Move method after the method it calls. Otherwise recursion could happen. (#126) + * Feature/dance bot s pattern (#128) + * more changes in sm_dancebot_artgallery_ue + * polishing sm_dancebot_artgallery_ue and s-pattern + * noticed typo + Finnaly > Finally + * Feature/dance bot s pattern (#129) + * more changes in sm_dancebot_artgallery_ue + * polishing sm_dancebot_artgallery_ue and s-pattern + * more refinement in sm_dancebot_artgallery_ue + * 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_dancebot_artgallery_ue + * minor + * Feature/sm dance bot refine 2 (#132) + * more changes in sm_dancebot_artgallery_ue + * minor + * build fix + * waypoints navigator bug (#133) + * minor tuning to mitigate overshot issue cases + * progress in the sm_dancebot_artgallery_ue 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/nav2z 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_dancebot_artgallery_ue + * 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 + * Ignore packages which should not be released. + * Added changelogs. + * 0.4.0 + * Revert "Ignore packages which should not be released." + This reverts commit ee2cc86db3c0a24f9eb0a9e33217de3f7a691a1c. + * Fix urls to index.ros.org (#284) + * Fix foxy source build config to use repos file from foxy branch. (#285) + * adding spawn entity delays + * more on backport + * more on backport + * disappeared ur_msgs denis repo + * fixing sm_dancebot_artgallery_ue examples + * working on fix of image messages for husky_barrel demo + 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_dancebot_artgallery_ue + * 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_dancebot_artgallery_ue + * 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_dancebot_artgallery_ue + * 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_dancebot_artgallery_ue visualizing turtlebot3 (#101) + * Feature/dance bot launch gz lidar choice (#102) + * sm_dancebot_artgallery_ue 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_dancebot_artgallery_ue 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_dancebot_artgallery_ue 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_dancebot_artgallery_ue 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_dancebot_artgallery_ue (#125) + * Move method after the method it calls. Otherwise recursion could happen. (#126) + * Feature/dance bot s pattern (#128) + * more changes in sm_dancebot_artgallery_ue + * polishing sm_dancebot_artgallery_ue and s-pattern + * noticed typo + Finnaly > Finally + * Feature/dance bot s pattern (#129) + * more changes in sm_dancebot_artgallery_ue + * polishing sm_dancebot_artgallery_ue and s-pattern + * more refinement in sm_dancebot_artgallery_ue + * 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_dancebot_artgallery_ue + * minor + * Feature/sm dance bot refine 2 (#132) + * more changes in sm_dancebot_artgallery_ue + * minor + * build fix + * waypoints navigator bug (#133) + * minor tuning to mitigate overshot issue cases + * progress in the sm_dancebot_artgallery_ue 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/nav2z 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_dancebot_artgallery_ue + * 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_dancebot_artgallery_ue + * 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_dancebot_artgallery_ue + * 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_dancebot_artgallery_ue + * 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_dancebot_artgallery_ue visualizing turtlebot3 (#101) + * Feature/dance bot launch gz lidar choice (#102) + * sm_dancebot_artgallery_ue 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_dancebot_artgallery_ue 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_dancebot_artgallery_ue 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_dancebot_artgallery_ue 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_dancebot_artgallery_ue (#125) + * Move method after the method it calls. Otherwise recursion could happen. (#126) + * Feature/dance bot s pattern (#128) + * more changes in sm_dancebot_artgallery_ue + * polishing sm_dancebot_artgallery_ue and s-pattern + * noticed typo + Finnaly > Finally + * Feature/dance bot s pattern (#129) + * more changes in sm_dancebot_artgallery_ue + * polishing sm_dancebot_artgallery_ue and s-pattern + * more refinement in sm_dancebot_artgallery_ue + * 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_dancebot_artgallery_ue + * minor + * Feature/sm dance bot refine 2 (#132) + * more changes in sm_dancebot_artgallery_ue + * minor + * build fix + * waypoints navigator bug (#133) + * minor tuning to mitigate overshot issue cases + * progress in the sm_dancebot_artgallery_ue 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/nav2z 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_dancebot_artgallery_ue + * 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_dancebot_artgallery_ue + * 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_dancebot_artgallery_ue + * 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_dancebot_artgallery_ue + * 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_dancebot_artgallery_ue visualizing turtlebot3 (#101) + * Feature/dance bot launch gz lidar choice (#102) + * sm_dancebot_artgallery_ue 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_dancebot_artgallery_ue 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_dancebot_artgallery_ue 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_dancebot_artgallery_ue 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_dancebot_artgallery_ue (#125) + * Move method after the method it calls. Otherwise recursion could happen. (#126) + * Feature/dance bot s pattern (#128) + * more changes in sm_dancebot_artgallery_ue + * polishing sm_dancebot_artgallery_ue and s-pattern + * noticed typo + Finnaly > Finally + * Feature/dance bot s pattern (#129) + * more changes in sm_dancebot_artgallery_ue + * polishing sm_dancebot_artgallery_ue and s-pattern + * more refinement in sm_dancebot_artgallery_ue + * 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_dancebot_artgallery_ue + * minor + * Feature/sm dance bot refine 2 (#132) + * more changes in sm_dancebot_artgallery_ue + * minor + * build fix + * waypoints navigator bug (#133) + * minor tuning to mitigate overshot issue cases + * progress in the sm_dancebot_artgallery_ue 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/nav2z 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_dancebot_artgallery_ue + * 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_dancebot_artgallery_ue + * 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_dancebot_artgallery_ue + * 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_dancebot_artgallery_ue + * 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_dancebot_artgallery_ue visualizing turtlebot3 (#101) + * Feature/dance bot launch gz lidar choice (#102) + * sm_dancebot_artgallery_ue 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_dancebot_artgallery_ue 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_dancebot_artgallery_ue 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_dancebot_artgallery_ue 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_dancebot_artgallery_ue (#125) + * Move method after the method it calls. Otherwise recursion could happen. (#126) + * Feature/dance bot s pattern (#128) + * more changes in sm_dancebot_artgallery_ue + * polishing sm_dancebot_artgallery_ue and s-pattern + * noticed typo + Finnaly > Finally + * Feature/dance bot s pattern (#129) + * more changes in sm_dancebot_artgallery_ue + * polishing sm_dancebot_artgallery_ue and s-pattern + * more refinement in sm_dancebot_artgallery_ue + * 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_dancebot_artgallery_ue + * minor + * Feature/sm dance bot refine 2 (#132) + * more changes in sm_dancebot_artgallery_ue + * minor + * build fix + * waypoints navigator bug (#133) + * minor tuning to mitigate overshot issue cases + * progress in the sm_dancebot_artgallery_ue 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/nav2z 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_dancebot_artgallery_ue + * 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_dancebot_artgallery_ue + * 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_dancebot_artgallery_ue + * 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_dancebot_artgallery_ue + * 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_dancebot_artgallery_ue visualizing turtlebot3 (#101) + * Feature/dance bot launch gz lidar choice (#102) + * sm_dancebot_artgallery_ue 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_dancebot_artgallery_ue 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_dancebot_artgallery_ue 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_dancebot_artgallery_ue 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_dancebot_artgallery_ue (#125) + * Move method after the method it calls. Otherwise recursion could happen. (#126) + * Feature/dance bot s pattern (#128) + * more changes in sm_dancebot_artgallery_ue + * polishing sm_dancebot_artgallery_ue and s-pattern + * noticed typo + Finnaly > Finally + * Feature/dance bot s pattern (#129) + * more changes in sm_dancebot_artgallery_ue + * polishing sm_dancebot_artgallery_ue and s-pattern + * more refinement in sm_dancebot_artgallery_ue + * 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_dancebot_artgallery_ue + * minor + * Feature/sm dance bot refine 2 (#132) + * more changes in sm_dancebot_artgallery_ue + * minor + * build fix + * waypoints navigator bug (#133) + * minor tuning to mitigate overshot issue cases + * progress in the sm_dancebot_artgallery_ue 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/nav2z 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_dancebot_artgallery_ue + * 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_dancebot_artgallery_ue + * 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_dancebot_artgallery_ue + * 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_dancebot_artgallery_ue + * 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_dancebot_artgallery_ue visualizing turtlebot3 (#101) + * Feature/dance bot launch gz lidar choice (#102) + * sm_dancebot_artgallery_ue 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_dancebot_artgallery_ue 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_dancebot_artgallery_ue 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_dancebot_artgallery_ue 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_dancebot_artgallery_ue (#125) + * Move method after the method it calls. Otherwise recursion could happen. (#126) + * Feature/dance bot s pattern (#128) + * more changes in sm_dancebot_artgallery_ue + * polishing sm_dancebot_artgallery_ue and s-pattern + * noticed typo + Finnaly > Finally + * Feature/dance bot s pattern (#129) + * more changes in sm_dancebot_artgallery_ue + * polishing sm_dancebot_artgallery_ue and s-pattern + * more refinement in sm_dancebot_artgallery_ue + * 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_dancebot_artgallery_ue + * minor + * Feature/sm dance bot refine 2 (#132) + * more changes in sm_dancebot_artgallery_ue + * minor + * build fix + * waypoints navigator bug (#133) + * minor tuning to mitigate overshot issue cases + * progress in the sm_dancebot_artgallery_ue 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/nav2z 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 +* Backport/initial to galactic (#61) + * reformatting the whole project + * Remove test phase from CMake and remove dependencies from package.xml. + * Compile with navigation and slam_toolbox. + * Enable all packages to compile. + * Resolve missing dependency in smacc_msgs and reorganize them for better overview. + * getLogger functionality and refactoring + * broken sm_respira + * sm_respira code + * Update README.md + ## Additions + - build-status table + - detailed install instructions (adjusted from [here](https://github.com/UniversalRobots/Universal_Robots_ROS2_Driver#readme)) + ## Changes + - default build type as `Release` (it is faster than `Debug` and executables are smaller) + - updated examples section + * 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. + use this command "sudo docker build --build-arg ROS2_DISTRO=(desiredRosTag) (directoryHoldingDockerfile)/" + * 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" + * Update tracing/ManualTracing.md + * reactivating smacc2 nav clients for rolling via submodules + * bug in smacc2 component + * reverted markdowns to html + * added README tutorial for Dockerfile + * edited tracing.md to reflect new tracing event names + * 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 + * Clean up of sm_atomic_24hr + * more sm_atomic_24hr cleanup + * Optimized deps in move_base_z_planners_common. + * Renaming of event generator library + * Correct build-overview table. + * Update and unify CI configurations. + * Use tf_geometry_msgs.h in galactic. + * Use galactic branches in .repos-file. + Co-authored-by: pabloinigoblasco + Co-authored-by: reelrbtx + Co-authored-by: Declan Dury <44791484+DecDury@users.noreply.github.com> + Co-authored-by: DecDury + Co-authored-by: brettpac +* Contributors: Denis Štogl, Pablo Iñigo Blasco, pabloinigoblasco diff --git a/smacc2_sm_reference_library/sm_dancebot_artgallery_ue/CMakeLists.txt b/smacc2_sm_reference_library/sm_dancebot_artgallery_ue/CMakeLists.txt new file mode 100644 index 000000000..053c0f50e --- /dev/null +++ b/smacc2_sm_reference_library/sm_dancebot_artgallery_ue/CMakeLists.txt @@ -0,0 +1,114 @@ +cmake_minimum_required(VERSION 3.5) +project(sm_dancebot_artgallery_ue) + +# 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(ament_cmake_python REQUIRED) +find_package(Boost COMPONENTS thread REQUIRED) + +find_package(smacc2 REQUIRED) +find_package(std_msgs REQUIRED) +find_package(nav2z_client REQUIRED) + +find_package(ros_publisher_client REQUIRED) +find_package(multirole_sensor_client REQUIRED) + +find_package(sr_all_events_go REQUIRED) +find_package(sr_event_countdown REQUIRED) +find_package(sr_conditional REQUIRED) +find_package(ros_timer_client REQUIRED) +find_package(sensor_msgs REQUIRED) +find_package(visualization_msgs REQUIRED) +find_package(ue_msgs REQUIRED) + +set(dependencies + smacc2 + std_msgs + sensor_msgs + nav2z_client + ros_publisher_client + multirole_sensor_client + sr_all_events_go + sr_event_countdown + sr_conditional + ros_timer_client + visualization_msgs + ue_msgs +) + +rosidl_generate_interfaces(${PROJECT_NAME} + "servers/led_action_server/action/LEDControl.action" + DEPENDENCIES builtin_interfaces std_msgs action_msgs sensor_msgs +) + +include_directories(include + ${Boost_INCLUDE_DIRS} + ${CMAKE_BINARY_DIR}/rosidl_generator_cpp) + +add_executable(${PROJECT_NAME}_node + src/${PROJECT_NAME}/sm_dancebot_artgallery_ue.cpp + src/${PROJECT_NAME}/clients/client_behaviors/nav2z_client/cb_position_control_free_space.cpp + src/${PROJECT_NAME}/clients/components/cp_ue_pose.cpp + src/${PROJECT_NAME}/clients/client_behaviors/nav2z_client/cb_active_stop.cpp +) + +add_executable(temperature_sensor_node_${PROJECT_NAME} servers/temperature_sensor_node/src/temperature_sensor_node.cpp) +add_executable(led_action_server_node_${PROJECT_NAME} servers/led_action_server/src/led_action_server_node.cpp) + +ament_target_dependencies(${PROJECT_NAME}_node ${dependencies}) +target_link_libraries(${PROJECT_NAME}_node ${Boost_LIBS} ${PROJECT_NAME}__rosidl_typesupport_cpp) + +ament_target_dependencies(led_action_server_node_${PROJECT_NAME} ${dependencies}) +set_target_properties(led_action_server_node_${PROJECT_NAME} PROPERTIES OUTPUT_NAME "led_action_server_node") +target_link_libraries(led_action_server_node_${PROJECT_NAME} ${Boost_LIBS} ${PROJECT_NAME}__rosidl_typesupport_cpp) + +ament_target_dependencies(temperature_sensor_node_${PROJECT_NAME} ${dependencies}) +set_target_properties(temperature_sensor_node_${PROJECT_NAME} PROPERTIES OUTPUT_NAME "temperature_sensor_node") + +ament_export_include_directories(include) +ament_export_libraries(${PROJECT_NAME}) +ament_export_dependencies(${dependencies}) + + +if(BUILD_TESTING) +endif() + +install(TARGETS + ${PROJECT_NAME}_node + temperature_sensor_node_${PROJECT_NAME} + led_action_server_node_${PROJECT_NAME} + + ARCHIVE DESTINATION lib + LIBRARY DESTINATION lib + RUNTIME DESTINATION lib/${PROJECT_NAME} +) + +#ament_python_install_package(${PROJECT_NAME} +# PACKAGE_DIR servers/service_node_3 +# ) + + +install(FILES + servers/service_node_3/service_node_3.py + scripts/transform_publisher.py + scripts/ue_navigation_frames_ground_truth_adapter.py + DESTINATION + lib/${PROJECT_NAME} + PERMISSIONS OWNER_EXECUTE OWNER_WRITE OWNER_READ + GROUP_EXECUTE GROUP_READ) + +install(DIRECTORY launch DESTINATION share/${PROJECT_NAME}) +install(DIRECTORY rviz DESTINATION share/${PROJECT_NAME}) +install(DIRECTORY params DESTINATION share/${PROJECT_NAME}) +install(DIRECTORY urdf DESTINATION share/${PROJECT_NAME}) + +ament_package() diff --git a/smacc2_sm_reference_library/sm_dancebot_artgallery_ue/README.md b/smacc2_sm_reference_library/sm_dancebot_artgallery_ue/README.md new file mode 100644 index 000000000..8fa9efb95 --- /dev/null +++ b/smacc2_sm_reference_library/sm_dancebot_artgallery_ue/README.md @@ -0,0 +1,55 @@ +

State Machine Diagram

+ + ![sm_dance_bot](docs/SmDanceBot_2023-5-26_151817.svg) + +

Description

A full-featured state machine example, that highlights the capabilities of SMACC2 & the ROS2 Navigation Stack via the MoveBaseZ Client. +.

+Doxygen Namespace & Class Reference + +

Build Instructions

+ +First, source your chosen ros2 distro. +``` +source /opt/ros/rolling/setup.bash +``` +``` +source /opt/ros/galactic/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 build, remember to source the proper install folder... + +``` +source ~/colcon_ws/install/setup.bash +``` + +And then run the launch file... + +``` +ros2 launch sm_dance_bot sm_dance_bot_launch.py +``` +

Headless launch

+ +Alternatively, you can also launch the gazebo simulator in headless mode: +``` +ros2 launch sm_dance_bot sm_dance_bot_launch.py headless:=True +``` +

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_dancebot_artgallery_ue/docker/.dockerignore b/smacc2_sm_reference_library/sm_dancebot_artgallery_ue/docker/.dockerignore new file mode 100644 index 000000000..e69de29bb diff --git a/smacc2_sm_reference_library/sm_dancebot_artgallery_ue/docker/.gitignore b/smacc2_sm_reference_library/sm_dancebot_artgallery_ue/docker/.gitignore new file mode 100644 index 000000000..167d48f57 --- /dev/null +++ b/smacc2_sm_reference_library/sm_dancebot_artgallery_ue/docker/.gitignore @@ -0,0 +1 @@ +UE5.1 diff --git a/smacc2_sm_reference_library/sm_dancebot_artgallery_ue/docker/Dockerfile b/smacc2_sm_reference_library/sm_dancebot_artgallery_ue/docker/Dockerfile new file mode 100644 index 000000000..d082635a3 --- /dev/null +++ b/smacc2_sm_reference_library/sm_dancebot_artgallery_ue/docker/Dockerfile @@ -0,0 +1,131 @@ +ARG ROS_DISTRO +ARG GIT_BRANCH +ARG UBUNTU_VERSION + +FROM ros:$ROS_DISTRO-ros-base-$UBUNTU_VERSION + +ARG ROS_DISTRO +ARG GIT_BRANCH +ARG LOCAL_FOLDER_SOURCE=1 +ENV DEBIAN_FRONTEND=noninteractive + +RUN apt-get update \ + && apt-get upgrade -y \ + && apt-get clean + +RUN apt-get update && apt-get install -q -y --no-install-recommends \ + dirmngr \ + gnupg2 \ + lsb-release \ + python3-colcon-ros \ + && apt-get clean \ + && apt upgrade -y --with-new-pkgs + +RUN apt-get install -y nvidia-driver-525 + +RUN apt install -y xdg-user-dirs + +ENV USER=ros2_ws +RUN bash -c "useradd -ms /bin/bash $USER" +RUN usermod -aG sudo $USER +RUN echo '%sudo ALL=(ALL) NOPASSWD:ALL' >> /etc/sudoers +RUN chown -R $USER:$USER /home/$USER + +USER $USER +WORKDIR /home/$USER + +WORKDIR "/home/$USER/src" + +RUN echo "copying current code version to docker image:" +#ADD . /home/$USER/src/SMACC2 +#WORKDIR "/home/$USER" + +# install dependencies and build +# RUN vcs import src --skip-existing --input src/SMACC2/.github/SMACC2-not-released.$ROS_DISTRO.repos +# RUN ls src + +# RUN apt update +# RUN rosdep install --from-paths src --ignore-src -r -y +# RUN apt-get update && apt-get install -q -y --no-install-recommends xterm + +# RUN bash -c "source /opt/ros/$ROS_DISTRO/setup.sh && colcon build --merge-install --parallel-workers 1" + +COPY --chown=$USER:$USER smacc2_sm_reference_library/sm_dancebot_artgallery_ue/docker/UE5.1/UnrealEngine /home/$USER/src/UE5.1/UnrealEngine + +ENV UE5_DIR=/home/$USER/src/UE5.1/UnrealEngine +ENV PROJECT_BRANCH=smacc2_devel_stable +ENV PROJECT_NAME=ue_project_1 + +RUN sudo apt-get install -y wget gpg +RUN wget -qO- https://packages.microsoft.com/keys/microsoft.asc | sudo gpg --dearmor > packages.microsoft.gpg +RUN sudo install -D -o root -g root -m 644 packages.microsoft.gpg /etc/apt/keyrings/packages.microsoft.gpg +RUN sudo sh -c 'echo "deb [arch=amd64,arm64,armhf signed-by=/etc/apt/keyrings/packages.microsoft.gpg] https://packages.microsoft.com/repos/code stable main" > /etc/apt/sources.list.d/vscode.list' -f packages.microsoft.gpg + +RUN sudo apt install -y apt-transport-https +RUN sudo apt update +RUN sudo apt install -y code git-lfs nano # or code-insiders + +# smacc2_devel +RUN git clone --recurse-submodules https://github.com/robosoft-ai/${PROJECT_NAME}.git -b $PROJECT_BRANCH + +WORKDIR "/home/$USER/src/${PROJECT_NAME}" +# RUN git checkout abdf483ce14f477d8f255fe19f3527975b3a9dbb +# RUN cd Plugins/rclUE && git checkout bae993fa + +#RUN rm -rf Plugins/RapyutaSimulationPlugins && git clone https://github.com/robosoft-ai/UE-Plugins.git Plugins/RapyutaSimulationPlugins +#RUN cd Plugins/RapyutaSimulationPlugins && git checkout bfac015 + +RUN git-lfs pull && git submodule foreach git-lfs pull +RUN ls && echo ${UE5_DIR} +RUN ./update_project_files.sh +ENV UE5_DIR=/home/$USER/src/UE5.1 + +# installing Dependencies +RUN wget http://security.ubuntu.com/ubuntu/pool/universe/t/tinyxml2/libtinyxml2-6a_7.0.0+dfsg-1build1_amd64.deb +RUN wget http://archive.ubuntu.com/ubuntu/pool/main/o/openssl/libssl1.1_1.1.0g-2ubuntu4_amd64.deb +RUN sudo dpkg -i libtinyxml2-6a_7.0.0+dfsg-1build1_amd64.deb +RUN sudo dpkg -i libssl1.1_1.1.0g-2ubuntu4_amd64.deb + +ENV LD_LIBRARY_PATH=/usr/local/cuda/lib64:/usr/lib/x86_64-linux-gnu/gazebo-11/plugins:/opt/ros/humble/opt/rviz_ogre_vendor/lib:/opt/ros/humble/lib/x86_64-linux-gnu:/home/$USER/src/${PROJECT_NAME}/Plugins/rclUE/ThirdParty/ros/lib/ +ENV UE5_DIR=/home/$USER/src/UE5.1/UnrealEngine + +COPY --chown=$USER:$USER smacc2_sm_reference_library/sm_dancebot_artgallery_ue/docker/nvidia-driver-check.sh /home/$USER/src/${PROJECT_NAME}/nvidia-driver-check.sh +RUN cat Makefile +RUN ./update_project_files.sh +#RUN bash + +RUN make ${PROJECT_NAME}Editor + +# # COPY smacc2 /home/$USER/src/SMACC2/smacc2 +# RUN mkdir -p /home/$USER/src/SMACC2/ +# WORKDIR "/home/$USER/src/SMACC2" +# COPY --chown=$USER:$USER smacc2_client_library /home/$USER/src/SMACC2/smacc2_client_library +# COPY --chown=$USER:$USER smacc2_event_generator_library /home/$USER/src/SMACC2/smacc2_event_generator_library +# COPY --chown=$USER:$USER smacc2_performance_tools /home/$USER/src/SMACC2/smacc2_performance_tools +# COPY --chown=$USER:$USER smacc2_state_reactor_library /home/$USER/src/SMACC2/smacc2_state_reactor_library +# COPY --chown=$USER:$USER smacc2 /home/$USER/src/SMACC2/smacc2 +# COPY --chown=$USER:$USER smacc2_dev_tools /home/$USER/src/SMACC2/smacc2_dev_tools +# COPY --chown=$USER:$USER smacc2_msgs /home/$USER/src/SMACC2/smacc2_msgs +# #smacc2_sm_reference_library + +# #RUN git clone +# RUN sudo apt-get update +# RUN rosdep update +# RUN rosdep install --ignore-src --from-paths . -y -r +# RUN sudo apt-get update && sudo apt-get install -y ros-humble-nav2-bringup ros-humble-rviz2 + +# RUN sudo apt-get install -y openvpn +# #COPY --chown=$USER:$USER smacc2_sm_reference_library/sm_dancebot_artgallery_ue/docker/run_editor_smacc.sh /home/$USER/src/${PROJECT_NAME}/run_editor_smacc.sh + + +# RUN touch /home/$USER/src/${PROJECT_NAME}/COLCON_IGNORE +# RUN touch /home/$USER/src/UE5.1/COLCON_IGNORE +# WORKDIR "/home/$USER/" +# RUN bash -c "source /opt/ros/humble/setup.bash && colcon build --symlink-install" + +# # BASIC VPN FRAMEWORK --- +RUN mkdir /home/$USER/ovpnconfig + +WORKDIR "/home/$USER/src/${PROJECT_NAME}" + +# #ENV LD_LIBRARY_PATH=/home/ros2_ws/src/ue_project_1/Plugins/rclUE/ThirdParty/ros/lib/:/usr/local/cuda/lib64:/usr/lib/x86_64-linux-gnu/gazebo-11/plugins:/opt/ros/humble/opt/rviz_ogre_vendor/lib:/opt/ros/humble/lib/x86_64-linux-gnu: diff --git a/smacc2_sm_reference_library/sm_dancebot_artgallery_ue/docker/README.md b/smacc2_sm_reference_library/sm_dancebot_artgallery_ue/docker/README.md new file mode 100644 index 000000000..679de5103 --- /dev/null +++ b/smacc2_sm_reference_library/sm_dancebot_artgallery_ue/docker/README.md @@ -0,0 +1,296 @@ +# SMACC2 and UNREAL ENGINE EDITOR +The README.md file provides a comprehensive guide for setting up and utilizing the Unreal Engine Docker environment. The document covers the necessary prerequisites, such as installing Docker and NVIDIA-Docker2, and provides step-by-step instructions for downloading and loading the prebuilt Docker image containing Unreal Engine. Additionally, it explains how to run the Unreal Editor within the container, create new containers for debugging purposes, and connect the container to a VPN if required. The README.md also includes important notes and optional instructions for rebuilding and running SMACC state machines inside the container. Whether you are new to Docker or an experienced user, this document will help you navigate the process of working with Unreal Engine in a Docker environment effectively. + +## Important Notes about the Solution + +Here are some important notes regarding the solution: + +**Repos Versioning:** Not all versions of `rclUE`, `turtlebot3Editor`, and `RapyputaPlugins` are compatible with each other. Ensuring consistency among them can be challenging, especially when considering that the container maps/volumes some volumes with external contents. The container is already designed to have a correct combination of all of them, but this is hardcoded in the Dockerfile and can be improved. + +**DDS Configuration:** There may be communication issues between the Docker container nodes and host nodes. As a workaround, we currently use `cyclonedds` on the host (until a uniform solution using `fastrtps` is found). To set this up, add the following line to the `.bashrc` file on the host: +``` +export RMW_IMPLEMENTATION=rmw_cyclonedds_cpp +``` + +***Automatic container nvidia driver update*** +The host and the container must have the same nvidia-driver in order to run the ue editor and simulation. +There is a mechanism implemented to automatically sync the driver. The current driver version is passed from the host to the container and then it is updated in the container if that is required. That is done in the nvidia-check.sh script. + +## Pre-Requisites +Before proceeding with the instructions, ensure that you have the necessary prerequisites installed on your system. + +### Docker Installation +To install Docker, follow these steps: + +1. Update the package list: + ``` + sudo apt-get update + ``` + +2. Install the required dependencies: + ``` + sudo apt-get install apt-transport-https ca-certificates curl gnupg-agent software-properties-common + ``` + +3. Add Docker's official GPG key: + ``` + curl -fsSL https://download.docker.com/linux/ubuntu/gpg | sudo apt-key add - + ``` + +4. Add the Docker repository: + ``` + sudo add-apt-repository "deb [arch=amd64] https://download.docker.com/linux/ubuntu bionic stable" + ``` + +5. Update the package list again: + ``` + sudo apt-get update + ``` + +6. Install Docker: + ``` + sudo apt-get install docker-ce + ``` + +### NVIDIA-Docker2 Installation +Ensure that you have NVIDIA-Docker2 installed by executing the following commands: + +1. Add the NVIDIA-Docker GPG key: + ``` + curl -s -L https://nvidia.github.io/nvidia-docker/gpgkey | sudo apt-key add - + ``` + +2. Determine the distribution: + ``` + distribution=$(. /etc/os-release; echo $ID$VERSION_ID) + ``` + +3. Add the NVIDIA-Docker repository: + ``` + curl -s -L https://nvidia.github.io/nvidia-docker/$distribution/nvidia-docker.list | sudo tee /etc/apt/sources.list.d/nvidia-docker.list + ``` + +4. Update the package list: + ``` + sudo apt-get update + ``` + +5. Install NVIDIA-Docker2: + ``` + sudo apt-get install nvidia-docker2 + ``` + +6. Restart the Docker service: + ``` + sudo systemctl restart docker.service + ``` + +### Adding User to Docker Group +To allow a user to run Docker, add the user to the Docker group. Replace `` with the actual user ID: + +``` +sudo usermod -a -G docker +``` + +### Testing the Installation +To test your Docker and NVIDIA-Docker2 installations, run the following command: + +``` +sudo nvidia-docker run --rm nvidia/cuda:10.2-base-ubuntu18.04 nvidia-smi +``` + +## Unreal Engine Docker Image +This section provides instructions for downloading, loading, and running the prebuilt Docker image containing Unreal Engine. + +### Downloading the Prebuilt Image +Download the prebuilt Docker image for Unreal Engine from the provided source. + +### Loading the Docker Image +To load the downloaded image into your Docker image database, use the following command: + +``` +sudo docker load -i ue_editor_rclue.tar +``` + +### (Alternative) Building the Docker Image Locally +If you want to modify the Docker image using a Dockerfile and rebuild it, follow these steps: + +1. Create a link to the UE5.1 folder inside the `sm_dancebot_artgallery_ue/docker` folder: + ``` + mount --bind UE5.1/UnrealEngine + ``` + * Recall the must contain the following folders: `Engine`, `FeaturePacks` and `Template` + +2. Build the image locally by running the following command: + ``` + ./build_docker_humble.sh + ``` + +## Using the Docker Image +This section explains how to run and create a new container from the `ue_editor_rcl` Docker image. + +### Running/Creating a New Container from the ue_editor_rcl Docker Image + +To run the Unreal Editor inside the container, you need to use some auxiliary scripts located in the `sm_dancebot_artgallery_ue` example. Follow these steps: + +1. Download the current SMACC2 repository. + +2. Navigate to the `sm_dancebot_artgallery_ue/docker` folder. + +3. Execute the following command: + ``` + ./run_docker_container_editor.sh + ``` + + This will run and create a new container using the `ue_editor_rcl` Docker image. + +4. The Unreal Engine editor will automatically open in "edition mode." You can launch the simulation with Turtlebot topics accessible from both the container and the host computer by clicking the "play" button. + + Note: When you close the editor, the container will also be finished, but it will remain installed. You can reopen the editor using the command: + ``` + ./start_container.sh + ``` + +### (Alternative) Running/Creating a New Container for Container Debugging + +There is an alternative way to create the container in daemon mode, where the lifetime of the container is not tied to the Unreal Editor window. This is useful, especially for developing new features for the container. To create the container in this mode,: + +1. Execute the following command: + ``` + ./run_docker_container_bash.sh + ``` + + This will create and start a new container as a daemon. The container will be available even after restarting. It is capable of opening the Unreal Engine editor with ROS2, but the editor will not open automatically. + +2. Execute the editor proccess: + ``` + ./join_editor.sh + ``` + +The Unreal Engine editor will automatically open in "edition mode." You can launch the simulation with Turtlebot topics accessible from both the container and the host computer by clicking the "play" button. + +### Stopping and Removing a Running Container + +If you need to reset everything and remove the existing container, follow these steps: + +1. Execute the following command: + ``` + ./stop_container.sh + ``` + + This will stop the running container. + +2. Execute the following command: + ``` + ./remove_container.sh + ``` + + This will remove the existing container. + +### Joining the Container via Bash + +To enter the Docker container and debug or test things from the command line, use the following command: + +``` +./join_bash.sh +``` + +### Connecting the Container to VPN + +The prototype for connecting the container to a VPN has already been completed. For detailed instructions, please refer to the relevant documentation. + +### Optionally Rebuilding and Running a SMACC State Machine inside the Container + +Note that the `ue_editor_rcl` containers are run by mapping the current `smacc2` source folder, allowing for mixed development between the host (using VSCode) and the container (for compiling and running the state machines). + +The SMACC2 source code is already prebuilt inside the image, making it available to any container. However, if you need to modify the `smacc2` code or examples and rebuild it, follow these steps: + +1. Change to the home directory: + ``` + cd ~/ + ``` + +2. Source the Humble ROS 2 installation: + ``` + source /opt/ros/humble/setup.bash + ``` + +3. Build the `smacc2` code: + ``` + colcon build + ``` + +4. To run your current demo, execute the following commands after building: + ``` + source install/setup.bash + ros2 launch sm_dancebot_artgallery_ue sm_dancebot_artgallery_ue_launch.py + ``` + +Enjoy experimenting with Unreal Engine in your Docker environment! + + + +# Brett's runtime notes + +You'll need to open three terminals for this demo. + One for the container where you'll run Unreal Engine + One to run the state machine (on the host) + One for the RTA (on the host) + +### Terminal 1 - For UE5 Simulation in the container + +1. Download the current SMACC2 repository. + ``` + cd ~/workspace/humble_ws/src + git clone https://github.com/robosoft-ai/SMACC2.git + + ``` +2. Build the workspace + + ``` + cd ~/workspace/humble_ws/ + source /opt/ros/humble/setup.bash + colcon build + + ``` + Once everything is done building... +3. Navigate to the `sm_dancebot_artgallery_ue/docker` folder. + + ``` + cd ~/workspace/humble_ws/src/SMACC2/smacc2_sm_reference_library/sm_dancebot_artgallery_ue/docker + ``` +4. Execute the following command: + ``` + ./run_docker_container_editor.sh + ``` + This will create and start a new container as a daemon. The container will be available even after restarting. It is capable of opening the Unreal Engine editor with ROS2, but the editor will not open automatically. + The Unreal Engine editor will automatically open in "edition mode." You can launch the simulation with Turtlebot topics accessible from both the container and the host computer by clicking the "play" button. + +### Terminal 2 - for the state machine on the host + +1. Add the following line to the `.bashrc` file on the host: + ``` + export RMW_IMPLEMENTATION=rmw_cyclonedds_cpp + ``` +2. Source the workspace you just built + ``` + source ~/workspace/humble_ws/install/setup.bash + ``` +3. Launch the state machine + ``` + ros2 launch sm_dancebot_artgallery_ue sm_dancebot_artgallery_ue_launch.py + ``` + This will launch the state machine application, rviz and other required nodes. + +### Terminal 3 - for the SMACC2_RTA on the host +1. Source the install + ``` + source /opt/ros/humble/setup.bash + ``` +2. Launch the SMACC2_RTA + ``` + ros2 run smacc2_rta smacc2_rta + ``` +3. Once the RTA is launched, in the upper left corner select State Machine/Available State Machines/SmDanceBotUE + +And you should be all set. diff --git a/smacc2_sm_reference_library/sm_dancebot_artgallery_ue/docker/attach_docker_container.sh b/smacc2_sm_reference_library/sm_dancebot_artgallery_ue/docker/attach_docker_container.sh new file mode 100755 index 000000000..7842a41d4 --- /dev/null +++ b/smacc2_sm_reference_library/sm_dancebot_artgallery_ue/docker/attach_docker_container.sh @@ -0,0 +1 @@ +sudo docker attach ue_editor_rclue diff --git a/smacc2_sm_reference_library/sm_dancebot_artgallery_ue/docker/build_docker.sh b/smacc2_sm_reference_library/sm_dancebot_artgallery_ue/docker/build_docker.sh new file mode 100755 index 000000000..b0d1f82a3 --- /dev/null +++ b/smacc2_sm_reference_library/sm_dancebot_artgallery_ue/docker/build_docker.sh @@ -0,0 +1,21 @@ +#!/bin/sh +DIR="$(dirname "$(realpath "$0")")" +echo $DIR +cd $DIR/.. +echo `pwd` + +ROS_DISTRO=$1 +GIT_BRANCH=$2 +UBUNTU_VERSION=$3 +NOCACHE="--no-cache" +NOCACHE= + +ROOT_DIR=`realpath $DIR/../../..` + +echo "ros distro: $ROS_DISTRO" +echo "git branch: $GIT_BRANCH" +echo "ubuntu version: $UBUNTU_VERSION" +echo "root path: $ROOT_DIR" + +cd $ROOT_DIR +sudo docker build --build-arg ROS_DISTRO=$ROS_DISTRO --build-arg GIT_BRANCH=$GIT_BRANCH --build-arg UBUNTU_VERSION=$UBUNTU_VERSION -t ue_editor_rclue:$ROS_DISTRO -f $ROOT_DIR/smacc2_sm_reference_library/sm_dancebot_artgallery_ue/docker/Dockerfile . $NOCACHE diff --git a/smacc2_sm_reference_library/sm_dancebot_artgallery_ue/docker/build_docker_humble.sh b/smacc2_sm_reference_library/sm_dancebot_artgallery_ue/docker/build_docker_humble.sh new file mode 100755 index 000000000..ed0429b57 --- /dev/null +++ b/smacc2_sm_reference_library/sm_dancebot_artgallery_ue/docker/build_docker_humble.sh @@ -0,0 +1,4 @@ +#!/bin/sh +DIR="$(dirname "$(realpath "$0")")" +echo $DIR +$DIR/build_docker.sh humble humble jammy diff --git a/smacc2_sm_reference_library/sm_dancebot_artgallery_ue/docker/fastdds_setup.bash b/smacc2_sm_reference_library/sm_dancebot_artgallery_ue/docker/fastdds_setup.bash new file mode 100755 index 000000000..f661c4240 --- /dev/null +++ b/smacc2_sm_reference_library/sm_dancebot_artgallery_ue/docker/fastdds_setup.bash @@ -0,0 +1,5 @@ +#!/bin/bash + +export RMW_IMPLEMENTATION=rmw_fastrtps_cpp +export ROS_DISCOVERY_SERVER="127.0.0.1:11811" +export FASTRTPS_DEFAULT_PROFILES_FILE=${PWD}/fastdds_config.xml diff --git a/smacc2_sm_reference_library/sm_dancebot_artgallery_ue/docker/frames_2023-08-05_01.06.53.gv b/smacc2_sm_reference_library/sm_dancebot_artgallery_ue/docker/frames_2023-08-05_01.06.53.gv new file mode 100644 index 000000000..e40f10c56 --- /dev/null +++ b/smacc2_sm_reference_library/sm_dancebot_artgallery_ue/docker/frames_2023-08-05_01.06.53.gv @@ -0,0 +1,18 @@ +digraph G { +"map" -> "odom"[label=" Broadcaster: default_authority\nAverage rate: 50.4\nBuffer length: 5.0\nMost recent transform: 2384.550098\nOldest transform: 2379.550098\n"]; +"base_footprint" -> "base_link"[label=" Broadcaster: default_authority\nAverage rate: 10000.0\nBuffer length: 0.0\nMost recent transform: 0.0\nOldest transform: 0.0\n"]; +"odom" -> "base_footprint"[label=" Broadcaster: default_authority\nAverage rate: 30.119\nBuffer length: 4.98\nMost recent transform: 2384.390137\nOldest transform: 2379.409912\n"]; +"camera_link" -> "camera_depth_frame"[label=" Broadcaster: default_authority\nAverage rate: 10000.0\nBuffer length: 0.0\nMost recent transform: 0.0\nOldest transform: 0.0\n"]; +"base_link" -> "camera_link"[label=" Broadcaster: default_authority\nAverage rate: 10000.0\nBuffer length: 0.0\nMost recent transform: 0.0\nOldest transform: 0.0\n"]; +"camera_depth_frame" -> "camera_depth_optical_frame"[label=" Broadcaster: default_authority\nAverage rate: 10000.0\nBuffer length: 0.0\nMost recent transform: 0.0\nOldest transform: 0.0\n"]; +"camera_link" -> "camera_rgb_frame"[label=" Broadcaster: default_authority\nAverage rate: 10000.0\nBuffer length: 0.0\nMost recent transform: 0.0\nOldest transform: 0.0\n"]; +"camera_rgb_frame" -> "camera_rgb_optical_frame"[label=" Broadcaster: default_authority\nAverage rate: 10000.0\nBuffer length: 0.0\nMost recent transform: 0.0\nOldest transform: 0.0\n"]; +"base_link" -> "caster_back_left_link"[label=" Broadcaster: default_authority\nAverage rate: 10000.0\nBuffer length: 0.0\nMost recent transform: 0.0\nOldest transform: 0.0\n"]; +"base_link" -> "caster_back_right_link"[label=" Broadcaster: default_authority\nAverage rate: 10000.0\nBuffer length: 0.0\nMost recent transform: 0.0\nOldest transform: 0.0\n"]; +"base_link" -> "imu_link"[label=" Broadcaster: default_authority\nAverage rate: 10000.0\nBuffer length: 0.0\nMost recent transform: 0.0\nOldest transform: 0.0\n"]; +"base_link" -> "base_scan"[label=" Broadcaster: default_authority\nAverage rate: 10000.0\nBuffer length: 0.0\nMost recent transform: 0.0\nOldest transform: 0.0\n"]; +edge [style=invis]; + subgraph cluster_legend { style=bold; color=black; label ="view_frames Result"; +"Recorded at time: 1691190413.1841352"[ shape=plaintext ] ; +}->"map"; +} diff --git a/smacc2_sm_reference_library/sm_dancebot_artgallery_ue/docker/frames_2023-08-05_01.06.53.pdf b/smacc2_sm_reference_library/sm_dancebot_artgallery_ue/docker/frames_2023-08-05_01.06.53.pdf new file mode 100644 index 0000000000000000000000000000000000000000..3b87381c1b28cca1cf68714ca49ae9a777c171d6 GIT binary patch literal 16806 zcmd74WmFy67A;Idf=h6B_j7P3xVyU@+}$Aw8rF(S2_PyhM zt42t$=uUq3wEy@u!LC0BGUQW(ozKWg2KkwDY*iQ{3x z=z8iT!Y=R7w(aKfu&~RPqkQ>9Uu)sJC*0SOMg{NrP~)FLI>tMFaai6T8i~{qG8!^^ zGsfDFA6;4TqPwbuj{RBNV5&QIQzC_`iz8%UO|&Wu4sz@~9c$Z;a)OD~(yqlQa#_CR z#^bmSUy{209Ly7+*)@~1tk`$dUQw1R(uhgg&yxw$fQd^h{)F@xq+0W%KhOmdKbZeB z$UWz^TLIJ}#&PFQQx9;&8SpHtvv3spEgm8={nEo^4YM`iTnwq;sf@}DRUN;XhjzjK zS-FA4O!BbS5#9_v2e>HS`RJ;MQ2=anqU-+lxbT!eQ7!Xkfq4VtLNj#>X>AHU8GP8g z;qrkRJCsqKBeYA;`EGbk<{h^PtEYA@Irp}4tWi}HQ?zghJ=uvX>^VtjZf9t1j)Tn0 zLOjwjq{2YipYXU*%hxGL8mMvFM{P~q1ayI9PJ%+B_`6I4)~*p*bX3jXlQq^)7_M58 z7W4gVHK>SuyDlA_Uwv)It25zlG!wu+3^u`*u5V?L&1WfrU~?`3^ds?o0u7yuXT{Cf zZXJ0pKxQ-zg%BK%+T0~D++nPi^Lg>ErdvJU6#2L{HMPc0dfYx0`&<_LJOmzBvnii$ zOa9bpTOZmTaq$QzFIoIpdAKxksLXoDV|g{RZC(aaR8b+dvAen&_VSJC`axY2)qBd6 zU}?L(7Ot5Z!Tt7luxO7D-U_|CEZn(&V2a9x zbSdsbMjE_F{!<7hS%=>^MzOIBUigssVGCfJrN_mSw&=~8flrmRqlUuLV) zs_$JFaf1EN3uPrpIxTJ5gkxEt6OEW5Dmy)vVJLd1SWgk8BBS!wvMX7gq|2C3GWW&p z4XSIshWRr8iq<_?k=Ux7lJHh8mvIkujn6d$=W@H*VM*6*b-pq@4z97TC< z9JN;QSAt9$B6tFl-JePzX&(>-=^lB9<@V~n%2|A)7nF?P5KNiez%p<3>Vli;8X}zX z&h>f~kjxW6y{Nzu$|xPR`erwsT$(~<bG)yL{KX@)TlLkx zBh!KJg2dVJ+$&Ip&Eq0dfC}D;K*o^+Hu>n45OL8?k`QehCpIQR{oL9V!14U`&06Yg82Z+4-|xX%x(NY(x#KAs%; z(l|@qWT$UA#ehh_d&N3??CD7bfuwT7ss)AV?Rffa+_(a=WM5TLj=-tb4%?~J;UO;3 zB+++l(5<2dB`m-(k4h=zh%fAh9=?03BA^>)a18w~ z8&$<XnlEFmcB@nvzsh=8knkd-cA)I5!AQfqns4$W1ql67vbP7^6GioAB zQD%A3YhNola!?XIN_DbAt+6rvc23UbF_LF1g^G4~ugi~|p$BgwJ!5lg`T~pZhx1uD zAE)ub&EDW+rKCDShQUt0W5@Gd#lbYCs$=cKzKC^*Y~B%q33xBDkBAK)z#&DL7pK4X z9vtQ1Q;q2R!6O;02G)0DuSVeXB4dZ=v)_K^Qj?{O3$hm2%~}zN-_BvLy6B#)_>?z5 zOdwy?Su6fF1Zfw$ha6ag?v|8KWUAB@ke3)n6poZOr3N5r>}4c=pmFfq7Z5Y> zY@@j;x_b(H6K{Tw97~IoAacx)_C$s~*s+3&bq7mzM;2%4VGYH6WId*gqFJ z;fR5V(!AvY+`bM7{dBI@FV*kf5tzv+jduI5QVA(TOk=!?sOJp zk4$p}7a1+-FithXQH0X@u6mwI+U5WZQ@V-;B*R+ORbGSwvox;aJl!%p*itO4Y-fFH zFR=%TmpfZ2U~y|9OKrI3+hufnJ4M=)8=aXuRL2hHw{cBO-bHC-oteKt)fUH3bl{}k z%`Bl9g0x~r-@f||oOw+}umUTxW>VfVdj%t?OhJQ!w}kSJ1F>KlS!l&LVu&+wcwvjD zSh7mIKCRNNYg_e`G)WXbQD$`Ra<(`Wl((}ljEdV5h5Hw#t>W-{EX&ts_>!G!xkmlP zaYp#|+Wt_7aBZa+hNdKwSIr}Di!1_^>Y9=w-shEOrGoCqGOo2OkNQu|T*s(>JlI@2 z-=)v=qfvQW-FwrdmZV_}$V+Y-@|%B#L!%X3F*#8!scuX$c5JA0j= zNQ|s3PQL2#eY5JVl)q*SU0PxWl`K|{=!!XXhIn@17X8H$!<2)p1rcY>!-2KC$9JJo z6pjog4!pDXIk4;{a!@ZG}lWEQJ zGjR?%;)V3Y7PBxp0NKkzS$giZ*Fd{_+M2yuVlvr6Z#na-U4>ZQZ)2k{fnzH2UBT@66?AO#duR$V*4c zi=2%vUGYL(AKYBxG9~*4xLustVT#VgdoVHK5gUn9H3WaoA8CQfk|kFljeOaVehSa@ zlbg`f-QDl7Gar&NV$=XTsi^%_Or%y&4j(k$kN<>IonDAMl{8J`$1^qb%&yx7UNpQssYj{B_5h@z$SRA_+$-*)CQ=n8I+QP?*s)YE&gWdX6&A z^FvO&{nG5nY3Ik9Rp^}9`Fh^|pAN|GnT_nw@D#!6l((r=#mvQgG(0DM)ROB`e%x$6 z5o3W`h+m0Ln|H*0n$?WsV++Q)5@Zwj2+t1phi_DkFQcoi0?m4h?v>|i8%hZ1n$j4e zA15Dh;J;)tz!+JB{(!~LcQ1hY1x3GHpK&xJJ0tr`ftMuHUwHc$!WMRQ6jgM5M%RSg z+%JabgFzqgoFHTn*CMpXPqR3oqrH>SpDDrTQbI<~W`;%zq5?0*KjyDsesw9%@Q+3&$EBhj&_6R@QKEOkb_QAo<^+RiFqP zGGk}=W>X=dz2g_&g9ix3Oy9i?gP4fcL$**%p(dY*?xqBinpxLL*Upk#C5zEtz3cE? zHF;2YopOG}^Iht{j1R^L`ydU;x4IEsPAALO;# zkR(PtDf3q zm-^@g7B6Qa3MSo_8jhqV&Sor=bL8|N!l5C=it3|1{*7R4DVoD&TkPpS{M9()?O=OCnyfeIn`GlvyFLhNh)pf89fUjIB}s$kn~XLFRi&|I8f1pdEimsDem% zasfO+RLr40=X|#xDqSDe@QA#gfQ|GZE~lA+@pkT$UF&s4Sy^Lh5q@$8AG5=H_pt&E zb$_YE*KjY@-6ls~`%E=vCS=*uQO<4x5O!%g#082|K^=QIMyd{KNSlIts%H)j&hRb1 zT0>1iiR3KT*1oxpiV*xy;&ymm`&ZhX7=^H-uvJn>Qmi|UjrP@|p~F=h-EU&KaJN7t z)|9w4xI7I%h6*xDA3po|{M?@_-_0ehWU6@2)tlA3e9_WgD% zhM~j#ip*9hk|XGy2fAnPM=s)PO%3Ki@_MLjaOVv=Vl50fn=EMII$Q2Q&3D^wGC0TM zrK}~Ta(k{RBb^=PQOCVp%lD-DTXy zojzURpVSy6d^hoW(^4gQVJ^;2n{UqHnwnU!wPCidy@=QiS&Q7Nq*Yu7kx)KWA3~3A zmsnugEyBc2QAuoqR8}e|$QVx*W?DPkUpn06mXz>T*-?#KB_W?fL5^AmMWI_rvN^S! z)PgE}!W1t3y)+@p4)pLF=htk>1SFfE4NG$@bEYRW(hXsfLn+Q<{HfMahqPyl>Wj{l zjEXsxQ`Q?X(AHiTe?T37EA>jV6Yb<}7p+}*o?KeVh%;&-NRvUk zni^@m+F-44xWOE@lCMRPyMpW-W1S_PYM z7i-lwW$;rHJvwX-5h!6_v9Ftq@f{p~1Ue-yxYI3{9mi9vYJq%SN1Q6Va$|0Lf+X>9 zv_L}|-%z&u*SoyfyF;%caurRP@{!_h(xC;^F}q%kL(-$83VA^pL@L!FpavF`P^H2a z8Q?{%?*t^8LkU5yT4GqTgD=o(L4|#Ab);73z3H7FJs3@|!aH;7u?^W@n5((m@9xUdLNs{mxBij6_i+Whd3! z15p3BBHfM~f6oWHH;w$6>5%ZKS#iOv-x z@0jKyd;FM3V*5YTp?-DzvizM2qL^%3q-W^feQCR_MD@^|E1LkzrSWXjrb742bh|VS zeiR03M)F+&!|AZ>+^jNBHBQFvL#Dw@EtLtv)5V2$U5W&{wv6PR>9}YUkkH*M=q z7>g=1fQ`=b5H^}Vild#z$UGkK%G&Tt#J9EnwcZc5AMB8Z{0`nOC@*+aYt|%b#^V`l zIAhip8j)z^Cz=g7nz+*kVbEeMV{jiX2q3ZM5meR}Mf#`W`wHr)r^=9e;$;Lr*nX=gJd zTuj)#HpqB4Q=^59#7ym*ciGak-{I%4Vx}8{T5+`VH{dp3-_6v4vZVBq7UWyy_+Bcwv zuOFA5FT774p|beXO5MX3QFPqpTC0a+-L&(wbr|>k9@_8@E}h1vIjc2b4OMnbsTIUn4Onlv9fe-R~IVR2S}k z-td^{bgD|@Vu1&wzHO3u9~Cq>dpg5hoMVgW&yH+d-Ay@hP>>YaOWTkKoJfs^qT|fl zGP;aI;9~X&1vJ>4Chk=)ed~eW#1@?wZTlJp?>LY()GKu-bRVYyTc%J%8SdH<8o&qE z)JsfA5MHJJOXdOoy3;o7`+5Gk59;y<(G@0bm_bFsAJxeahoOvr;1h1kKxPkK6R=nM zJbldf(%scvOSG7?4Sn2-?QnM1X_;NPnd|B7bxJLJ5}CWao_KHF4%5B`rs6G2+9Nz9 z^U2_9>Hdx*Po_k~Nvq0=A}=6~5&GlMR>@4sbV&)z1zD68_c~fBda^-qm>qy>rpQ>v`kv=>K2>` zH93JiWzXxQNr&V6Em;DeE7su@%*NhWu<}!}SD+MJ=pF~q~OC&wfBt;uAiiS%f`j# zeba-X%cEwq_@uC^d=Fj3@-Rja)i3a+Ne(%fhZ>e>D~dtD5+|n7^yf8i+a7X&?DP5h{2a)T-2P zYfR_g&bV7q=k$_eQth#0dh zX1`Jd z&xaPm(Xzu{f_874zvS_Mt*j5BJos(0stFKTjLqpJF7~=b5;<)!x4Nia$wLh*yIRBv z6BUdV!2EScEOL|ml!#iqV}?NXGI;Bwog9VWl}?lc&NTiK>0 z$iANOq@v^lk~gY|QB*3RRXk?nH7Wee&E%EEeN1ZIN10^;a2AvYzX}e){=j#=#Z=t`kN|AiEK_CES3ekT^|5!7%>^#y+;#%8$^>5(R~d+$eOT!IzNC*c9Bc!A zsKr_EV{!7T%fQLuVG7bZi;aFR1!6oUb+_s1yrUr3Q$gpxBs@U#y?3p`>H^}%AvrlN zBzNgsuDdUT1Adn?u9E6*bxM1bMrE#e8X!7^pQsX{qSh)wszRS(z?cQ}E-+3RGe6yL zI~96g$jsytbm97ze&cIXPIllU?GPGMF zwObMxtr{G+PMU#g!!mp*>Ye(TvC7QI1XVgT6^Gfa6ReCaM9kmV;|JU-@S3ku^r;n* z?ncn5?qs-~BI2Rd=&^peX*iGMHZ_;@p;uLR|0yob7sZ+F@#^v%gE*#eKj5L^gA8_} zjg*wk8yPusBgvUO2nsM*s8Uc<4|J$GISq%VGDwdkmbj$zEh-5I=2Hqn!96sktBv0ew;k1NG|1y2e#+My{YFk1d87xrEikBK_gi(4%Hu*cc^fIxYM zMK!@ufC@hG5k7-73SaD#IV}f(!z^l=WSn*>jjuv|1O7*ONUKEjDoC0uxPV~wc;+S?>n>uVA{gy54;$3X&+Q(1x zhF^5JVOIUk;_c;&)}hQ}T`5?UH{m>JUb9Av05C*%%@HokWum6q{8ul^^PM|Bd)?01 z*s`5od1lq1vAc0JO^~SU5x{Tr1ep+MU||g|;OZn_n6z&{oex9S&HC`2ay)T;u4tSU z^8~4El{9R%wNdWERX*2U@BDrP z!-7Z|m#O}MMdpdh#|>riuZ8Nv2#* z)(AcstbOubaVKPp27Tt%@EsbOTk|_sr|Bo@=j0y)FG{eNyOZWCOjAvj8fHvZdl}?Z z27^;pIOmp}RYY4H9cR}9R<*MP<5shRVQ!<2AH!7IJqo*~&k;@%lHOt1nTn8_IuQ;8 zs3?-HC>R%6rL9-0e-rDrj&BHIRH5U&5eeis$pQ7N;kB4`W~qk>)AqugqVA*ip%zpj z$C?VDA3EyAkX5&v@y*usY+zKsyQdv z#aOm2ljf_&bz$~yc<2Pyj9NpqE`ps)2i_ncdFFXr)oJX}UrL&l$eT$}sZ*mNLU+Tq zcQYfARhK5zaSG)s2xilGfGZx0o~L_rL-Ljjw>d|;`yd(zt}QQ}T$QmT5d7TPuG+45 zxn#Jsne(hR$aB3P@THJ#pzr6kGZ>4fi&#=&4Evr{!P~N>)ah}9(Q()P{u|GoEWSC9 za{YCcsEeg1lzFffVPCoT@`%s|3A8xA!XB!`M&9H_-f54zIDW~XU#S#?fngdg4va=M z9FjF$5CE*dqr+OMtHDUhe#uTLzS}Q%`8$aS0xj6b(9NxQ9uJLBI;%V1eU}5Zr?o4i zzR^9ljt7PZ8xtS9wIZ7et(z;VWyhbJ17Bs+xW#9!t;`pj%ZhfbuJ7mwwAya@hwhH% zQ8aa1ZNK-FQNUSxHR{G=#)bwHi}&MDk}WdU&1zF4*%X11i`xZJ8=zp7O!XDh&qgDY z!E2KnfpHLi-6QWA4eq|J$~Z&eJ2bgJ_3q$XL%c;f8b7jK{$2~OMEdnEM#eT$xX?8c z2q#NqLV>6IXwrt*QgLY@&R62&Id*!dx`?mt{aEVleEku;#f88$J3WyXUbCY)7|ko> zu~sK%EI8t+-VsYCAB8@Z?`f$)*K7ZpoKIuMCGIlG879+KNSLPd00wchkFcr_#+ik~ zR7tjBa%1-`v@&!cN{~GBYC5H&02RbnENC6{4~vnntEKWE%UbSi*jid_STE_cnmlg? zg?R!`K&>WkafiFF*Uelx+g16pGA3pBDX1mU%e(CgWb{`Dg+nfzZkZT%GF0d^aC-)S z5C#yi>`xI0d-`Dx9d`djKtLFB@aSJl7tR`6)xHdqu=tdFgiz;;svjyPpqWJ zqrjQOPzUpvy?5HTw_^jIGb6=pKxs-8#2=weh$ke*5Tn@5tq)z83uguV|x zwU=`bE+Wysp|72M?!8xPR>Sw9rXQ{hybG}R>o(1m=8N9f#l!&`_0Brt9>#3M-mv3y zs2ReShJ(SZFR93;YOiYIO(3r2f$x(Wf2q5lJ?o!E`Rla(jGgyy z^ggmH_M`4*f1KeBDRK+}&f=oH`JT88B6;xjHhU0kwdBgD+2Lz{G*xxc_6f%YcYZ>? z!1iSmg84@G`qF4=3j_(L@B|}qY?}lv`X6u!-6(_P@2S-KR6KC2 z<e8po3)a>x? z-BBJ*nLK*27tSMa8N@z?xU{%dY&zwgm?8&wY{w@d?#vKX2o9n}>xm7t93 za8F6bjT}|9Ap1hq$Iatuigjn)rqI zdC5GoGXbNlW-%J4*rs5^k%~h)C;Kpkobs8yRIXaQaH~1fB6)9dS9Q;Y$?atD z8}>4vQxPmm*&G@tTtI_a8;|H06uUeA`(3j(@MFGtj~sEnj_bnj{ygaSLPmLycE^x% zv1yChK64mkl+#pH^td6Cot?fJ z?D80E04kZ)D`8mko=Wum*=os`jKt&7$VQTlQ+6}360wvz8rnr6V8VoAmLVZK0GWjp zE-WmE=BAeJR8LJt8G6~eEwd#)z7*@A_jBg^Bs8NLX|KoKzR!WBO|wlE@b}oENqp3} z2U`h_=<8dhEgOFd%1*b?Lp|{nQe@I6hyd+W<-ivr4*L5yLh$(!&#Hgq*E^Q$X5eZc>jC=-yIk|Jr_HXi zC;MG14^-)5$YduqG$j&82iu@;t&eNN>d%KzH_$_Yjh^0yJj_}SjJqCDL~|$XJBhbx zPymOw7vPSVCt?Du8=~~K+m8unk3P3wh&A3OTL>Nt+`S7w5o=s14(+p>guAse&w~9T;y^ye|oIJd6G+SR%r_ zv;z1QAL@geK=n4`h$Cz`UTDK)@9(}AzTV^pm*qn~>@#ykry@6Md`Fsl0%SyUXUl?V z#?6v9o)UU4uIsoBNYNfKOPLDVO96BzPEBav2WhpuDmzWA~`UepoS@?j~AYif(r9HHS8B-4miyLUL4C|9SkMP%CofL$H|Z7 zNCJ$LPqnto+Htm7*f*x7VT zOSNo^;aHH?I3}fVv~NwnSD+YM7+?rjPts-Y2h!ZQvAWg!-P5My4v>#=Yj<(y#1u&O zD~3657lG^>ML-A?G4Z>b;z``VSZ13rlVqo}7(vE!6ao&?7+-1UZ9PV_Pj=GIlc-sU zoner;o1G%f6EfH&7aIehr-+)w2hN4-#NGo*@`%GaV1(Ib6)Wy%sEZXI;+i>;t9pIZ zrjQkqXr4nyL1mt)6PSgEO8J4i1WpGsd{Vq#IjS6g{H1YZp2Dxa2qR0%vN&j4CQM3x03-n-?~A+_$BxkzCzp;*630$*4fIcsMV`ibaBEz{R>LEe@L?d4#`slof$Z$^6-4PXzA@1pSMHbEextkox zMhbl+P1<0s2&mqjgTmH2U51h%m5KnlQA&y+ZHSAo9yFdTqJ&w{E`Tamk&S}od?rNv zKs zxkE<@Pvj&>U0L}#zgRVbrs%mhpB4~rJ9s|&+RDd;~Ge84Y(e^N-5+%@vj{?rq=-TFAO{MD|!` zNz$wJkV~>tUuvS_%HC?893ieznQozGKW?GuTZtw zk()B}4!oCf`Xr+vSV%OXty%YG7y4MI?bQzmo;gw1oNc@_8J1T5Hhk{(v&>$LFhN;W zH{0?#2eU$rPn)`tJ`?GQJUa&n_vL4X&J{H%^I{87aJg`JhUl~0m||GmXFBHyV_$Y1 z2rHwik>(4lKL*AIb_&KpGI6FwbqWq{O!Uq1>g5k|6Z4tlAB%mW|GH7Oud(y!eW2MI zM_i(k(0Y_;J(qg3!KIdbhN z_#%1Yc-CmWjOb;6FKIePCYE0YL48{>BQq1zXI0d5GrvrVjz(50gzUfTza+%|OuS6- zd5aq{626F${w_{>QE2@sPWmm0dMW$6IVS_hUs5ckXZh1h-Y=_wzJt*(t=Io_jhVfJ zqoAq2{htny(*Gk3Wc;n>`geyD{#ugXA}wL-=U@&qYZF2SRWoaTYX`GG4ZpfpH3K=C zIy_6hn1C-Tvj27d&Shm{CH(*M%k*6C?=7)0G7_>ez39gNc>)29FXAqC;LA$}K*+}O zvYGz-$pUz>vv52Qg8BK%`mBX}xpDvrnb>~cvobS3PvoT~*5|$Oa>v2U{#=UrInDU1 zG=Pwqjh&DM$VA8t0RGBhVPz&{XL-@^z1;oMx;<;wY?Q6dUV>lPIEU7L>~$?x4O)Nu&yqupWjVU*)~k2edNh*3!DJ*Gvy^E`@Hs3tApPOq zQte2)Bq^o~u>P`Kvxd57s4>INc(vH-q1sm<)CG8TWuvy#;nuQM5t@{t0;Mgaq$RS0 zt7?L_F(T!rtyXTqYCe}&*yO%pT?VjTZnI-sbH35?&KSaoG60c~hc3(8SBM_nFENIVC-X)-OG{U~Us(a8I7Nd1CVXg!u4q<*TpfmGwFArE%W%T=$H(To zwK^f?I(&%X_W>AB+N;Xm7SyB=yV<%-F}9j(&y`!eOJei3Wwru zHy>D9=KD>OQ3)zN`xi;FHA^u(dcgKwx<_jtf@|~q=m-?4uLPr}2RUS9KdXFYZvM$} z?lxPHwb}}tg+3S(>lnZn)EL`6Lo>g!t-O=Da(cef~KY`^UhCV12J99r~eK{?V90 z;PAb#0s0)ycL|J>X~oNpYXh(%Ho5nz-NgIElKI9}B0UOMM?zv;hE%9{#KC#Pm5Ek? z-3Eswqr}|npZrN3rIp;n{El=nWfCLm+U-)O{V zeun;O1-;8~bHT^KS5|CkZFWxX%~P>UwBH&|USC&Vl`LRseAk*!{pg!}WG;MmdQ?WE zV$60gjJl*yK=0jPIXLK3s=MqC_}XZ6M|zpwghGec*)hwjOzBs05~4}+&|4OG#Sln; z)4e0zK36pnd+Qy#HN_<4&mL#Y)NSVdLx8iEk11Z9$JpCm`@0ugxi(#%g}xTk){Mga zD2)w!}AVoMCi#~m<=pnniI=(ta9*H3PeW))&i|u@D zpc)r5_!u5;_77A9eA5D7L3Yd-$g8{E_gn~x_Iz`7s}Wuv+O-u>chNZsBl1mi{yeGf zvY0L4uFkmaIDJeA-Cg>@RNUVz#;4x8QYQ<`zC{}8DoY7!k3NCBMYp2^&b4 zx=5@}QFLDIL(vwz@Dorq;c|k{q9NxZ@!qo(EubRtF5SwkyGBAVl%`jqDOK1}Q#0F3xoaLQN6vCRlN!6*t^ zC-3o7nifRjd|8BpSwzBZ$ykJYV}3?8?w7o$RXiX%Vx+DOD%jIf>H{f@Bo?n1mY&EN zF;O>uhC!s3;_#QuKo+^#1dAK{0;0FH2QzAorSdd_xF7XAYsjf34V2K7?^=46>lzG- zv;J(o*|Z33m=|90bY&PakWo1hu#-YxkOvKRAxjPS`YD7mK#l)YK?PXwgWbTmr83Lv zr+(Cm2({>i6D)wjcaV)-K`uRTG z?lX<1A5y{l;-(LR19mhc)v3%;m3-UI~&ud@{=N$?7-HvH+K%6R?y!S?zWKzL;@ul(HF-3 zig_fR#@&l#h?;zEWx`e-G0Ttg8$~|`dt_jgd!AtpmJdf4SZ6~}A-(?YpiY%9Fmn#8 zC}P|9U%dDyD1F8vjEw)Ha4(?mpIG&OLU&0~aYbdt|10Ru{0ncs;MD(jkozwz_iy0t z?|}vX3%d)73%yL^CHprW`4@iwAO3l+q3GuD%pSz8jcs21@UmwWj7-d)$?!AV;eU=7 zG5SSwUXY@!J?NPeJp*=1anLh8F>`eLD@)PI*4EO<>V>rYVkCdWN&F}H|0^DZL6uMw z@C@!*pOG`$OC*Q(-(o2K`_lhfCB?sJ%JUq5bD#g?9RH*hgbaTUn@;RkW4~z@NdK8G zJ$vniYXE;Srr%*De?!{8qtX8r!1D5^gn!?gF99qLzZ#V^1HH8LLKYPM3Q%!+iD&uS z41Y0!KU/dev/null 2>&1; then + echo "Package $package_name is installed." + echo "Checking if it has version $package_version." + + installed_package_description=$(apt list --installed | grep -w "$package_name") + echo "Installed package: $installed_package_description" + + if [[ $installed_package_description =~ $regex ]]; then + installed_version="${BASH_REMATCH[2]}" + fi + echo "Installed version: $installed_version" + + # Verify if the installed version matches the desired version + if [[ "$installed_version" == "$package_version" ]]; then + echo "Package $package_name-$package_version is already installed." + else + echo "WARNING: Package $package_name is installed, but it does not have the same debian package version $package_version." + DO_INSTALL=1 + fi +else + echo "Package $package_name is not installed." + DO_INSTALL=1 +fi + +if [ "$DO_INSTALL" == "1" ]; then + echo "Attempting to install package $package_name-$package_version." + # Attempt to install the package + sudo apt-get install -y "$package_name" + + # Check if the installation was successful + if dpkg -s "$package_name" >/dev/null 2>&1; then + echo "Package $package_name-$package_version has been successfully installed." + else + echo "Unable to install package $package_name-$package_version." + fi +else + echo "Checking nvidia-driver. Nothing to do. Package $package_name-$package_version is already installed." +fi diff --git a/smacc2_sm_reference_library/sm_dancebot_artgallery_ue/docker/older/run_docker_container_bash.sh b/smacc2_sm_reference_library/sm_dancebot_artgallery_ue/docker/older/run_docker_container_bash.sh new file mode 100755 index 000000000..a450ca485 --- /dev/null +++ b/smacc2_sm_reference_library/sm_dancebot_artgallery_ue/docker/older/run_docker_container_bash.sh @@ -0,0 +1,8 @@ +#!/bin/sh +xhost + + +DIR="$(dirname "$(realpath "$0")")" +ROOT_DIR=`realpath $DIR/../../../..` +DRIVER=$(apt list --installed | grep nvidia-driver) + +sudo nvidia-docker run --net host -it -e DISPLAY -e QT_X11_NO_MITSHM=1 --privileged --name ue_editor_rclue -v /dev/dri:/dev/dri -v /tmp/.X11-unix:/tmp/.X11-unix ue_editor_rclue:humble /bin/bash -c "./nvidia-driver-check.sh '$DRIVER' && bash" diff --git a/smacc2_sm_reference_library/sm_dancebot_artgallery_ue/docker/older/run_docker_container_editor.sh b/smacc2_sm_reference_library/sm_dancebot_artgallery_ue/docker/older/run_docker_container_editor.sh new file mode 100755 index 000000000..8dfad38c4 --- /dev/null +++ b/smacc2_sm_reference_library/sm_dancebot_artgallery_ue/docker/older/run_docker_container_editor.sh @@ -0,0 +1,8 @@ +#!/bin/sh +xhost + + +DIR="$(dirname "$(realpath "$0")")" +ROOT_DIR=`realpath $DIR/../../../..` + +DRIVER=$(apt list --installed | grep nvidia-driver) +sudo nvidia-docker run --net host -it -e DISPLAY -e QT_X11_NO_MITSHM=1 --privileged --name ue_editor_rclue -v $ROOT_DIR/ue_project_2:/home/ros2_ws/src/ue_project_2 -v $ROOT_DIR/ue_project_1:/home/ros2_ws/src/ue_project_1 -v $ROOT_DIR/SMACC2:/home/ros2_ws/src/SMACC2 -v $ROOT_DIR/UE-Plugins:/home/ros2_ws/src/turtlebot3-UE/Plugins/RapyutaSimulationPlugins -v /dev/dri:/dev/dri -v /tmp/.X11-unix:/tmp/.X11-unix ue_editor_rclue:humble /bin/bash -c "./nvidia-driver-check.sh '$DRIVER' && ./run_editor_smacc.sh" diff --git a/smacc2_sm_reference_library/sm_dancebot_artgallery_ue/docker/older/run_editor_smacc.sh b/smacc2_sm_reference_library/sm_dancebot_artgallery_ue/docker/older/run_editor_smacc.sh new file mode 100755 index 000000000..aa13d0389 --- /dev/null +++ b/smacc2_sm_reference_library/sm_dancebot_artgallery_ue/docker/older/run_editor_smacc.sh @@ -0,0 +1,32 @@ +#!/bin/bash +# Copyright 2020-2022 Rapyuta Robotics Co., Ltd. + +export LD_LIBRARY_PATH=$(echo "$LD_LIBRARY_PATH" | sed 's#/opt/ros/humble/lib:##') +echo "UPDATED LD_LIBRARY_PATH: $LD_LIBRARY_PATH" + +if [ -z "${UE5_DIR}" ]; then + printf "Please set UE5_DIR to path of UE5 folder\n" + exit 1 +fi + +DISCOVERY_SERVER=${1:-true} +CURRENT_DIR=$(cd -- "$(dirname -- "${BASH_SOURCE[0]}")" &> /dev/null && pwd) +TB3_UE_DIR=${2:-"${CURRENT_DIR}"} +# if $DISCOVERY_SERVER; then +# # Run discovery service for FastDDS +# (exec "${TB3_UE_DIR}/run_discovery_service.sh") + +# # Configure environment for FastDDS discovery +# source ${TB3_UE_DIR}/fastdds_setup.sh +# fi + +#change default level, generating DefautlEngine.ini +DEFAULT_LEVEL=${LEVEL_NAME:-"Default"} +DEFAULT_RATE=${FIXED_FRAME_RATE:-"100.0"} +DEFAULT_RTF=${TARGET_RTF:-"1.0"} +sed -e 's/${LEVEL_NAME}/'${DEFAULT_LEVEL}'/g' Config/DefaultEngineBase.ini > Config/DefaultEngine.ini +sed -i -e 's/${FIXED_FRAME_RATE}/'${DEFAULT_RATE}'/g' Config/DefaultEngine.ini +sed -i -e 's/${TARGET_RTF}/'${DEFAULT_RTF}'/g' Config/DefaultEngine.ini + +UE_EDITOR="${UE5_DIR}/Engine/Binaries/Linux/UnrealEditor" +(exec "$UE_EDITOR" "${TB3_UE_DIR}/ue_project1.uproject" -norelativemousemode) diff --git a/smacc2_sm_reference_library/sm_dancebot_artgallery_ue/docker/openvpn/login.txt b/smacc2_sm_reference_library/sm_dancebot_artgallery_ue/docker/openvpn/login.txt new file mode 100644 index 000000000..e69de29bb diff --git a/smacc2_sm_reference_library/sm_dancebot_artgallery_ue/docker/openvpn/ovpnconfig.ovpn b/smacc2_sm_reference_library/sm_dancebot_artgallery_ue/docker/openvpn/ovpnconfig.ovpn new file mode 100644 index 000000000..e69de29bb diff --git a/smacc2_sm_reference_library/sm_dancebot_artgallery_ue/docker/openvpn/password.conf b/smacc2_sm_reference_library/sm_dancebot_artgallery_ue/docker/openvpn/password.conf new file mode 100644 index 000000000..e69de29bb diff --git a/smacc2_sm_reference_library/sm_dancebot_artgallery_ue/docker/openvpn_file_run.sh b/smacc2_sm_reference_library/sm_dancebot_artgallery_ue/docker/openvpn_file_run.sh new file mode 100755 index 000000000..dc16db9e7 --- /dev/null +++ b/smacc2_sm_reference_library/sm_dancebot_artgallery_ue/docker/openvpn_file_run.sh @@ -0,0 +1,7 @@ +#!/bin/sh +xhost + + +DIR="$(dirname "$(realpath "$0")")" +ROOT_DIR=`realpath $DIR/../../../..` + +sudo nvidia-docker run --net host -it -e DISPLAY -e QT_X11_NO_MITSHM=1 --privileged --name ue_editor_rclue -v $ROOT_DIR/SMACC2:/home/ros2_ws/src/SMACC2 -v $ROOT_DIR/SMACC2_RTA:/home/ros2_ws/src/SMACC2_RTA -v /dev/dri:/dev/dri -v /tmp/.X11-unix:/tmp/.X11-unix ue_editor_rclue:humble /bin/bash diff --git a/smacc2_sm_reference_library/sm_dancebot_artgallery_ue/docker/remove_container.sh b/smacc2_sm_reference_library/sm_dancebot_artgallery_ue/docker/remove_container.sh new file mode 100755 index 000000000..f83e1e49b --- /dev/null +++ b/smacc2_sm_reference_library/sm_dancebot_artgallery_ue/docker/remove_container.sh @@ -0,0 +1 @@ +sudo docker rm ue_editor_rclue diff --git a/smacc2_sm_reference_library/sm_dancebot_artgallery_ue/docker/run_docker_container_bash_development.sh b/smacc2_sm_reference_library/sm_dancebot_artgallery_ue/docker/run_docker_container_bash_development.sh new file mode 100755 index 000000000..257b64e62 --- /dev/null +++ b/smacc2_sm_reference_library/sm_dancebot_artgallery_ue/docker/run_docker_container_bash_development.sh @@ -0,0 +1,8 @@ +#!/bin/sh +xhost + + +DIR="$(dirname "$(realpath "$0")")" +ROOT_DIR=`realpath $DIR/../../../..` +DRIVER=$(apt list --installed | grep nvidia-driver) + +sudo nvidia-docker run --net host --ipc=host -it -e DISPLAY -e QT_X11_NO_MITSHM=1 --privileged --name ue_editor_rclue -v $ROOT_DIR/ue_project_1:/home/ros2_ws/src/ue_project_1 -v $ROOT_DIR/ue_project_2:/home/ros2_ws/src/ue_project_2 -v $ROOT_DIR/SMACC2:/home/ros2_ws/src/SMACC2 -v $ROOT_DIR/UE-Plugins:/home/ros2_ws/src/ue_project_1/Plugins/RapyutaSimulationPlugins -v $ROOT_DIR/rclUE:/home/ros2_ws/src/ue_project_1/Plugins/rclUE -v $ROOT_DIR/UE-Plugins:/home/ros2_ws/src/ue_project_2/Plugins/RapyutaSimulationPlugins -v $ROOT_DIR/rclUE:/home/ros2_ws/src/ue_project_2/Plugins/rclUE -v /dev/dri:/dev/dri -v /tmp/.X11-unix:/tmp/.X11-unix ue_editor_rclue:humble /bin/bash -c "./nvidia-driver-check.sh '$DRIVER' && bash" diff --git a/smacc2_sm_reference_library/sm_dancebot_artgallery_ue/docker/save_current_docker_image.sh b/smacc2_sm_reference_library/sm_dancebot_artgallery_ue/docker/save_current_docker_image.sh new file mode 100755 index 000000000..3cd242149 --- /dev/null +++ b/smacc2_sm_reference_library/sm_dancebot_artgallery_ue/docker/save_current_docker_image.sh @@ -0,0 +1,11 @@ +#!/bin/bash + +OUTPUT_PATH=${1} + +if [ -z "$OUTPUT_PATH" ]; then + echo "Usage: $0 " + exit 1 +fi + +DATE_SUFFIX=`date +%Y%m%d_%H%M%S` +docker save -o "$OUTPUT_PATH/unreal_editor_smacc_$DATE_SUFFIX.tar" ue_editor_rclue:humble diff --git a/smacc2_sm_reference_library/sm_dancebot_artgallery_ue/docker/start_container.sh b/smacc2_sm_reference_library/sm_dancebot_artgallery_ue/docker/start_container.sh new file mode 100755 index 000000000..6b339f906 --- /dev/null +++ b/smacc2_sm_reference_library/sm_dancebot_artgallery_ue/docker/start_container.sh @@ -0,0 +1 @@ +sudo docker start ue_editor_rclue diff --git a/smacc2_sm_reference_library/sm_dancebot_artgallery_ue/docker/stop_container.sh b/smacc2_sm_reference_library/sm_dancebot_artgallery_ue/docker/stop_container.sh new file mode 100755 index 000000000..0c06686cc --- /dev/null +++ b/smacc2_sm_reference_library/sm_dancebot_artgallery_ue/docker/stop_container.sh @@ -0,0 +1 @@ +sudo docker stop ue_editor_rclue diff --git a/smacc2_sm_reference_library/sm_dancebot_artgallery_ue/docker/update_project_files.sh b/smacc2_sm_reference_library/sm_dancebot_artgallery_ue/docker/update_project_files.sh new file mode 100755 index 000000000..086a474a3 --- /dev/null +++ b/smacc2_sm_reference_library/sm_dancebot_artgallery_ue/docker/update_project_files.sh @@ -0,0 +1,2 @@ +#!/bin/bash +/home/ros2_ws/src/UE5.1/Engine/Build/BatchFiles/Linux/GenerateProjectFiles.sh: diff --git a/smacc2_sm_reference_library/sm_dancebot_artgallery_ue/include/sm_dancebot_artgallery_ue/clients/cl_nav2z/client_behaviors/cb_active_stop.hpp b/smacc2_sm_reference_library/sm_dancebot_artgallery_ue/include/sm_dancebot_artgallery_ue/clients/cl_nav2z/client_behaviors/cb_active_stop.hpp new file mode 100644 index 000000000..077f8efcd --- /dev/null +++ b/smacc2_sm_reference_library/sm_dancebot_artgallery_ue/include/sm_dancebot_artgallery_ue/clients/cl_nav2z/client_behaviors/cb_active_stop.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 sm_dancebot_artgallery_ue +{ +struct CbActiveStop : public smacc2::SmaccAsyncClientBehavior +{ +private: + + + rclcpp::Publisher::SharedPtr cmd_vel_pub_; + +public: + + CbActiveStop(); + + void onEntry() override; + + void onExit() override; +}; +} // namespace sm_dancebot_artgallery_ue diff --git a/smacc2_sm_reference_library/sm_dancebot_artgallery_ue/include/sm_dancebot_artgallery_ue/clients/cl_nav2z/client_behaviors/cb_load_waypoints_file.hpp b/smacc2_sm_reference_library/sm_dancebot_artgallery_ue/include/sm_dancebot_artgallery_ue/clients/cl_nav2z/client_behaviors/cb_load_waypoints_file.hpp new file mode 100644 index 000000000..c8a44946e --- /dev/null +++ b/smacc2_sm_reference_library/sm_dancebot_artgallery_ue/include/sm_dancebot_artgallery_ue/clients/cl_nav2z/client_behaviors/cb_load_waypoints_file.hpp @@ -0,0 +1,69 @@ +// 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 sm_dancebot_artgallery_ue +{ +struct CbLoadWaypointsFile : public smacc2::SmaccClientBehavior +{ +public: + CbLoadWaypointsFile(std::string filepath) : filepath_(filepath) {} + + CbLoadWaypointsFile(std::string parameter_name, std::string packagenamesapce) + : + parameterName_(parameter_name), + packageNamespace_(packagenamesapce) + { + } + + void onEntry() override + { + requiresComponent(waypointsNavigator_); // this is a component from the nav2z_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; + } + + void onExit() override {} + + std::optional filepath_; + + std::optional parameterName_; + std::optional packageNamespace_; + + cl_nav2z::CpWaypointNavigatorBase * waypointsNavigator_; +}; +} // namespace sm_dancebot_artgallery_ue diff --git a/smacc2_sm_reference_library/sm_dancebot_artgallery_ue/include/sm_dancebot_artgallery_ue/clients/cl_nav2z/client_behaviors/cb_navigate_next_waypoint_free.hpp b/smacc2_sm_reference_library/sm_dancebot_artgallery_ue/include/sm_dancebot_artgallery_ue/clients/cl_nav2z/client_behaviors/cb_navigate_next_waypoint_free.hpp new file mode 100644 index 000000000..c2fb7df22 --- /dev/null +++ b/smacc2_sm_reference_library/sm_dancebot_artgallery_ue/include/sm_dancebot_artgallery_ue/clients/cl_nav2z/client_behaviors/cb_navigate_next_waypoint_free.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 + +namespace sm_dancebot_artgallery_ue +{ + +class CbNavigateNextWaypointFree : public sm_dancebot_artgallery_ue::CbPositionControlFreeSpace +{ +public: + CbNavigateNextWaypointFree() { } + + virtual ~CbNavigateNextWaypointFree() {} + + void onEntry() override + { + requiresComponent(this->waypointsNavigator_); + this->target_pose_ = this->waypointsNavigator_->getCurrentPose(); + + this->onSuccess(&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 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 onExit() override + { + } + +protected: + cl_nav2z::CpWaypointNavigatorBase * waypointsNavigator_; +}; + +} // namespace sm_dancebot_artgallery_ue diff --git a/smacc2_sm_reference_library/sm_dancebot_artgallery_ue/include/sm_dancebot_artgallery_ue/clients/cl_nav2z/client_behaviors/cb_position_control_free_space.hpp b/smacc2_sm_reference_library/sm_dancebot_artgallery_ue/include/sm_dancebot_artgallery_ue/clients/cl_nav2z/client_behaviors/cb_position_control_free_space.hpp new file mode 100644 index 000000000..43528ca56 --- /dev/null +++ b/smacc2_sm_reference_library/sm_dancebot_artgallery_ue/include/sm_dancebot_artgallery_ue/clients/cl_nav2z/client_behaviors/cb_position_control_free_space.hpp @@ -0,0 +1,65 @@ +// 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 sm_dancebot_artgallery_ue +{ +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 sm_dancebot_artgallery_ue diff --git a/smacc2_sm_reference_library/sm_dancebot_artgallery_ue/include/sm_dancebot_artgallery_ue/clients/cl_nav2z/client_behaviors/cb_pure_spinning.hpp b/smacc2_sm_reference_library/sm_dancebot_artgallery_ue/include/sm_dancebot_artgallery_ue/clients/cl_nav2z/client_behaviors/cb_pure_spinning.hpp new file mode 100644 index 000000000..c0acfa7b3 --- /dev/null +++ b/smacc2_sm_reference_library/sm_dancebot_artgallery_ue/include/sm_dancebot_artgallery_ue/clients/cl_nav2z/client_behaviors/cb_pure_spinning.hpp @@ -0,0 +1,126 @@ +// 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 + +namespace sm_dancebot_artgallery_ue +{ +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) + : targetYaw__rads(targetYaw_rads), + k_betta_(1.0), + max_angular_yaw_speed_(max_angular_yaw_speed), + yaw_goal_tolerance_rads_(0.1) + { + + } + + void updateParameters() + { + } + + void onEntry() override + { + auto nh = this->getNode(); + cmd_vel_pub_ = nh->create_publisher("/cmd_vel", rclcpp::QoS(1)); + + sm_dancebot_artgallery_ue::CpUEPose* 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(), "["<cmd_vel_pub_->publish(cmd_vel); + + loop_rate.sleep(); + } + + this->postSuccessEvent(); + } + + void onExit() override + { + } + }; +} diff --git a/smacc2_sm_reference_library/sm_dancebot_artgallery_ue/include/sm_dancebot_artgallery_ue/clients/components/cp_ue_pose.hpp b/smacc2_sm_reference_library/sm_dancebot_artgallery_ue/include/sm_dancebot_artgallery_ue/clients/components/cp_ue_pose.hpp new file mode 100644 index 000000000..ea79a2c8b --- /dev/null +++ b/smacc2_sm_reference_library/sm_dancebot_artgallery_ue/include/sm_dancebot_artgallery_ue/clients/components/cp_ue_pose.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. + +/***************************************************************************************************************** + *-2020 + * Authors: Pablo Inigo Blasco, Brett Aldrich + * + ******************************************************************************************************************/ +#pragma once + +#include + +#include +#include +#include +#include +#include +#include +#include + +#include +#include +#include +#include +#include + +namespace sm_dancebot_artgallery_ue +{ + +class CpUEPose : public smacc2::components::CpTopicSubscriber +{ +public: + CpUEPose(std::string topicname); + + void onInitialize() override; + + void onPoseMessageReceived(const ue_msgs::msg::EntityState& msg); + + geometry_msgs::msg::Pose toPoseMsg(); + +private: + ue_msgs::msg::EntityState entityStateMsg_; + +}; +} // namespace sm_dancebot_artgallery_ue diff --git a/smacc2_sm_reference_library/sm_dancebot_artgallery_ue/include/sm_dancebot_artgallery_ue/modestates/ms_dance_bot_recovery_mode.hpp b/smacc2_sm_reference_library/sm_dancebot_artgallery_ue/include/sm_dancebot_artgallery_ue/modestates/ms_dance_bot_recovery_mode.hpp new file mode 100644 index 000000000..2ce6116b8 --- /dev/null +++ b/smacc2_sm_reference_library/sm_dancebot_artgallery_ue/include/sm_dancebot_artgallery_ue/modestates/ms_dance_bot_recovery_mode.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 + * + ******************************************************************************************************************/ + +#include +namespace sm_dancebot_artgallery_ue +{ +// STATE DECLARATION +class MsDanceBotRecoveryMode : public smacc2::SmaccState +{ +public: + using SmaccState::SmaccState; + + // TRANSITION TABLE + typedef mpl::list< + + Transition> + + >reactions; + // typedef Transition reactions; +}; +} // namespace sm_dancebot_artgallery_ue diff --git a/smacc2_sm_reference_library/sm_dancebot_artgallery_ue/include/sm_dancebot_artgallery_ue/modestates/ms_dance_bot_run_mode.hpp b/smacc2_sm_reference_library/sm_dancebot_artgallery_ue/include/sm_dancebot_artgallery_ue/modestates/ms_dance_bot_run_mode.hpp new file mode 100644 index 000000000..c7133b396 --- /dev/null +++ b/smacc2_sm_reference_library/sm_dancebot_artgallery_ue/include/sm_dancebot_artgallery_ue/modestates/ms_dance_bot_run_mode.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 + * + ******************************************************************************************************************/ + +#include +namespace sm_dancebot_artgallery_ue +{ +// STATE DECLARATION +class MsDanceBotRunMode : public smacc2::SmaccState +{ +public: + using SmaccState::SmaccState; + + // TRANSITION TABLE + typedef mpl::list< + + Transition + + >reactions; +}; +} // namespace sm_dancebot_artgallery_ue diff --git a/smacc2_sm_reference_library/sm_dancebot_artgallery_ue/include/sm_dancebot_artgallery_ue/orthogonals/or_navigation.hpp b/smacc2_sm_reference_library/sm_dancebot_artgallery_ue/include/sm_dancebot_artgallery_ue/orthogonals/or_navigation.hpp new file mode 100644 index 000000000..6b1883e13 --- /dev/null +++ b/smacc2_sm_reference_library/sm_dancebot_artgallery_ue/include/sm_dancebot_artgallery_ue/orthogonals/or_navigation.hpp @@ -0,0 +1,71 @@ +// 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 + +namespace sm_dancebot_artgallery_ue +{ +using namespace cl_nav2z; +using namespace std::chrono_literals; + +class OrNavigation : public smacc2::Orthogonal +{ +public: + void onInitialize() override + { + auto nav2zClient = this->createClient(); + + // create pose component + nav2zClient->createComponent(); + + // create planner switcher + nav2zClient->createComponent(); + + // create goal checker switcher + nav2zClient->createComponent(); + + // create odom tracker + nav2zClient->createComponent(); + + // create odom tracker + nav2zClient->createComponent(); + + // create waypoints navigator component + auto waypointsNavigator = nav2zClient->createComponent(); + + // nav2zClient->createComponent("/ue_ros/map_origin_entity_state"); + + /*auto waypointsNavigator = */ + // nav2zClient->createComponent<::cl_nav2z::CpWaypointNavigatorBase>(); + } +}; +} // namespace sm_dancebot_artgallery_ue diff --git a/smacc2_sm_reference_library/sm_dancebot_artgallery_ue/include/sm_dancebot_artgallery_ue/orthogonals/or_obstacle_perception.hpp b/smacc2_sm_reference_library/sm_dancebot_artgallery_ue/include/sm_dancebot_artgallery_ue/orthogonals/or_obstacle_perception.hpp new file mode 100644 index 000000000..04f327273 --- /dev/null +++ b/smacc2_sm_reference_library/sm_dancebot_artgallery_ue/include/sm_dancebot_artgallery_ue/orthogonals/or_obstacle_perception.hpp @@ -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 + * + ******************************************************************************************************************/ + +#pragma once + +#include + +namespace sm_dancebot_artgallery_ue +{ + using namespace std::chrono_literals; +class OrObstaclePerception : public smacc2::Orthogonal +{ +public: + void onInitialize() override + { + // auto lidarClient = this->createClient(); + + // lidarClient->topicName = "/scan"; + // lidarClient->timeout_ = rclcpp::Duration(10s); + } +}; +} // namespace sm_dancebot_artgallery_ue diff --git a/smacc2_sm_reference_library/sm_dancebot_artgallery_ue/include/sm_dancebot_artgallery_ue/sm_dancebot_artgallery_ue.hpp b/smacc2_sm_reference_library/sm_dancebot_artgallery_ue/include/sm_dancebot_artgallery_ue/sm_dancebot_artgallery_ue.hpp new file mode 100644 index 000000000..5d5ce96a1 --- /dev/null +++ b/smacc2_sm_reference_library/sm_dancebot_artgallery_ue/include/sm_dancebot_artgallery_ue/sm_dancebot_artgallery_ue.hpp @@ -0,0 +1,144 @@ +// 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 + +// CLIENT BEHAVIORS +#include + +#include +#include +#include + +// #include +#include +#include +#include +#include +#include + +#include +#include +#include + +#include + +//STATE REACTORS +#include +#include +#include + +// ORTHOGONALS +#include +#include + +using namespace cl_nav2z; +using namespace smacc2::state_reactors; + +namespace sm_dancebot_artgallery_ue +{ +//STATE FORWARD DECLARATIONS +class StAcquireSensors; +class StNavigateArtGalleryWaypointsX; +class StFinalState; + +//SUPERSTATE FORWARD DECLARATIONS +//MODE STATES FORWARD DECLARATIONS +class MsDanceBotRunMode; +class MsDanceBotRecoveryMode; + +namespace SS1 +{ +class SsRadialPattern1; +} + +namespace SS2 +{ +class SsRadialPattern2; +} + +namespace SS3 +{ +class SsRadialPattern3; +} + +namespace SS4 +{ +class SsFPattern1; +} + +namespace SS5 +{ +class SsSPattern1; +} + +// custom smd_dance_bot event +struct EvGlobalError : sc::event +{ +}; + +} // namespace sm_dancebot_artgallery_ue + +using namespace sm_dancebot_artgallery_ue; +using namespace cl_ros_timer; +using namespace smacc2; + +namespace sm_dancebot_artgallery_ue +{ +/// \brief Advanced example of state machine with smacc that shows multiple techniques +/// for the development of state machines +struct SmDanceBot : public smacc2::SmaccStateMachineBase +{ + typedef mpl::bool_ shallow_history; + typedef mpl::bool_ deep_history; + typedef mpl::bool_ inherited_deep_history; + + using SmaccStateMachineBase::SmaccStateMachineBase; + + void onInitialize() override + { + this->createOrthogonal(); + this->createOrthogonal(); + } +}; + +} // namespace sm_dancebot_artgallery_ue + +//MODE STATES +#include + +#include + +//SUPERSTATES +#include +#include +#include +#include +#include + +//STATES +#include + +#include + +#include diff --git a/smacc2_sm_reference_library/sm_dancebot_artgallery_ue/include/sm_dancebot_artgallery_ue/states/f_pattern_states/sti_fpattern_forward_1.hpp b/smacc2_sm_reference_library/sm_dancebot_artgallery_ue/include/sm_dancebot_artgallery_ue/states/f_pattern_states/sti_fpattern_forward_1.hpp new file mode 100644 index 000000000..9c0de7c51 --- /dev/null +++ b/smacc2_sm_reference_library/sm_dancebot_artgallery_ue/include/sm_dancebot_artgallery_ue/states/f_pattern_states/sti_fpattern_forward_1.hpp @@ -0,0 +1,66 @@ +// 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 + * + ******************************************************************************************************************/ + +namespace sm_dancebot_artgallery_ue +{ +namespace f_pattern_states +{ +// STATE DECLARATION +template +struct StiFPatternForward1 : public smacc2::SmaccState, SS> +{ + typedef SmaccState, SS> TSti; + using TSti::context_type; + using TSti::SmaccState; + + // TRANSITION TABLE + typedef mpl::list< + + Transition, StiFPatternReturn1>, + Transition, StiFPatternReturn1, ABORT> + + + >reactions; + + // STATE FUNCTIONS + static void staticConfigure() + { + TSti::template configure_orthogonal(SS::ray_lenght_meters()); + TSti::template configure_orthogonal(); + } + + void runtimeConfigure() + { + cl_nav2z::odom_tracker::CpOdomTracker * odomTracker; + this->requiresComponent(odomTracker); + auto* cbForwardMotion = this->template getOrthogonal()->template getClientBehavior(); + auto previousGoal = odomTracker->getCurrentMotionGoal(); + + if (previousGoal) + { + cbForwardMotion->options.forceInitialOrientation = previousGoal->pose.orientation; + RCLCPP_ERROR_STREAM(this->getLogger(), "Previous goal orientation: " << previousGoal->pose.orientation.x << ", " << previousGoal->pose.orientation.y << ", " << previousGoal->pose.orientation.z << ", " << previousGoal->pose.orientation.w); + }; + + RCLCPP_ERROR_STREAM(this->getLogger(), ".."); + } +}; +} // namespace f_pattern_states +} // namespace sm_dancebot_artgallery_ue diff --git a/smacc2_sm_reference_library/sm_dancebot_artgallery_ue/include/sm_dancebot_artgallery_ue/states/f_pattern_states/sti_fpattern_forward_2.hpp b/smacc2_sm_reference_library/sm_dancebot_artgallery_ue/include/sm_dancebot_artgallery_ue/states/f_pattern_states/sti_fpattern_forward_2.hpp new file mode 100644 index 000000000..01d9abb52 --- /dev/null +++ b/smacc2_sm_reference_library/sm_dancebot_artgallery_ue/include/sm_dancebot_artgallery_ue/states/f_pattern_states/sti_fpattern_forward_2.hpp @@ -0,0 +1,65 @@ +// 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 + * + ******************************************************************************************************************/ + +namespace sm_dancebot_artgallery_ue +{ +namespace f_pattern_states +{ +// STATE DECLARATION +template +struct StiFPatternForward2 : smacc2::SmaccState, SS> +{ + typedef SmaccState, SS> TSti; + using TSti::context_type; + using TSti::SmaccState; + + // TRANSITION TABLE + typedef mpl::list< + + Transition, StiFPatternStartLoop>, + Transition, StiFPatternStartLoop> + + >reactions; + + // STATE FUNCTIONS + static void staticConfigure() + { + TSti::template configure_orthogonal(SS::pitch_lenght_meters()); + TSti::template configure_orthogonal(); + } + + void runtimeConfigure() + { + cl_nav2z::odom_tracker::CpOdomTracker * odomTracker; + this->requiresComponent(odomTracker); + auto* cbForwardMotion = this->template getOrthogonal()->template getClientBehavior(); + auto previousGoal = odomTracker->getCurrentMotionGoal(); + + if (previousGoal) + { + cbForwardMotion->options.forceInitialOrientation = previousGoal->pose.orientation; + RCLCPP_ERROR_STREAM(this->getLogger(), "Previous goal orientation: " << previousGoal->pose.orientation.x << ", " << previousGoal->pose.orientation.y << ", " << previousGoal->pose.orientation.z << ", " << previousGoal->pose.orientation.w); + }; + + RCLCPP_ERROR_STREAM(this->getLogger(), ".."); + } +}; +} // namespace f_pattern_states +} // namespace sm_dancebot_artgallery_ue diff --git a/smacc2_sm_reference_library/sm_dancebot_artgallery_ue/include/sm_dancebot_artgallery_ue/states/f_pattern_states/sti_fpattern_loop_start.hpp b/smacc2_sm_reference_library/sm_dancebot_artgallery_ue/include/sm_dancebot_artgallery_ue/states/f_pattern_states/sti_fpattern_loop_start.hpp new file mode 100644 index 000000000..2ae634f00 --- /dev/null +++ b/smacc2_sm_reference_library/sm_dancebot_artgallery_ue/include/sm_dancebot_artgallery_ue/states/f_pattern_states/sti_fpattern_loop_start.hpp @@ -0,0 +1,55 @@ +// 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 + * + ******************************************************************************************************************/ + +namespace sm_dancebot_artgallery_ue +{ +namespace f_pattern_states +{ +// STATE DECLARATION +template +struct StiFPatternStartLoop : smacc2::SmaccState, SS> +{ + typedef SmaccState, SS> TSti; + using TSti::context_type; + using TSti::SmaccState; + + // TRANSITION TABLE + typedef mpl::list< + + Transition>, StiFPatternRotate1, CONTINUELOOP> + + >reactions; + + // STATE FUNCTIONS + static void staticConfigure() {} + + bool loopCondition() + { + auto & superstate = TSti::template context(); + return superstate.iteration_count++ < superstate.total_iterations(); + } + + void onEntry() + { + TSti::checkWhileLoopConditionAndThrowEvent(&StiFPatternStartLoop::loopCondition); + } +}; +} // namespace f_pattern_states +} // namespace sm_dancebot_artgallery_ue diff --git a/smacc2_sm_reference_library/sm_dancebot_artgallery_ue/include/sm_dancebot_artgallery_ue/states/f_pattern_states/sti_fpattern_return_1.hpp b/smacc2_sm_reference_library/sm_dancebot_artgallery_ue/include/sm_dancebot_artgallery_ue/states/f_pattern_states/sti_fpattern_return_1.hpp new file mode 100644 index 000000000..2a847492a --- /dev/null +++ b/smacc2_sm_reference_library/sm_dancebot_artgallery_ue/include/sm_dancebot_artgallery_ue/states/f_pattern_states/sti_fpattern_return_1.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 + * + ******************************************************************************************************************/ + +namespace sm_dancebot_artgallery_ue +{ +namespace f_pattern_states +{ +// STATE DECLARATION +template +struct StiFPatternReturn1 : smacc2::SmaccState, SS> +{ + typedef SmaccState, SS> TSti; + using TSti::context_type; + using TSti::SmaccState; + + // TRANSITION TABLE + typedef mpl::list< + + Transition, StiFPatternRotate2> + + >reactions; + + // STATE FUNCTIONS + static void staticConfigure() + { + TSti::template configure_orthogonal(); + TSti::template configure_orthogonal(); + } + + void runtimeConfigure() {} +}; +} // namespace f_pattern_states +} // namespace sm_dancebot_artgallery_ue diff --git a/smacc2_sm_reference_library/sm_dancebot_artgallery_ue/include/sm_dancebot_artgallery_ue/states/f_pattern_states/sti_fpattern_rotate_1.hpp b/smacc2_sm_reference_library/sm_dancebot_artgallery_ue/include/sm_dancebot_artgallery_ue/states/f_pattern_states/sti_fpattern_rotate_1.hpp new file mode 100644 index 000000000..ba02636d3 --- /dev/null +++ b/smacc2_sm_reference_library/sm_dancebot_artgallery_ue/include/sm_dancebot_artgallery_ue/states/f_pattern_states/sti_fpattern_rotate_1.hpp @@ -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 + * + ******************************************************************************************************************/ + +namespace sm_dancebot_artgallery_ue +{ +namespace f_pattern_states +{ +// STATE DECLARATION +template +struct StiFPatternRotate1 : smacc2::SmaccState, SS> +{ + typedef SmaccState, SS> TSti; + using TSti::context_type; + using TSti::SmaccState; + + // TRANSITION TABLE + typedef mpl::list< + + Transition, StiFPatternForward1>, + Transition, StiFPatternRotate1> + + >reactions; + + // STATE FUNCTIONS + static void staticConfigure() + { + float angle = 0; + double offset = 0; //-1.5; // for a better behaving + + if (SS::direction() == TDirection::LEFT) + angle = 90 + offset; + else + angle = -90 - offset; + + //TSti::template configure_orthogonal(angle); + TSti::template configure_orthogonal( + angle); // absolute aligned to the y-axis + } + + void runtimeConfigure() + { + + } +}; +} // namespace f_pattern_states +} // namespace sm_dancebot_artgallery_ue diff --git a/smacc2_sm_reference_library/sm_dancebot_artgallery_ue/include/sm_dancebot_artgallery_ue/states/f_pattern_states/sti_fpattern_rotate_2.hpp b/smacc2_sm_reference_library/sm_dancebot_artgallery_ue/include/sm_dancebot_artgallery_ue/states/f_pattern_states/sti_fpattern_rotate_2.hpp new file mode 100644 index 000000000..8dd70ec2f --- /dev/null +++ b/smacc2_sm_reference_library/sm_dancebot_artgallery_ue/include/sm_dancebot_artgallery_ue/states/f_pattern_states/sti_fpattern_rotate_2.hpp @@ -0,0 +1,62 @@ +// 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 + * + ******************************************************************************************************************/ + +namespace sm_dancebot_artgallery_ue +{ +namespace f_pattern_states +{ +// STATE DECLARATION +template +struct StiFPatternRotate2 : smacc2::SmaccState, SS> +{ + typedef SmaccState, SS> TSti; + using TSti::context_type; + using TSti::SmaccState; + + // TRANSITION TABLE + typedef mpl::list< + + Transition, StiFPatternForward2>, + Transition, StiFPatternRotate2, ABORT> + + >reactions; + + // STATE FUNCTIONS + static void staticConfigure() + { + double offset = 0;//1.5; // for a better behaving + // float angle = 0; + + // if (SS::direction() == TDirection::LEFT) + // angle = -90 - offset; + // else + // angle = 90 + offset; + + //TSti::template configure_orthogonal(angle); + TSti::template configure_orthogonal( + 0 + offset); // absolute horizontal + + TSti::template configure_orthogonal(); + } + + void runtimeConfigure() {} +}; +} // namespace f_pattern_states +} // namespace sm_dancebot_artgallery_ue diff --git a/smacc2_sm_reference_library/sm_dancebot_artgallery_ue/include/sm_dancebot_artgallery_ue/states/radial_motion_states/sti_radial_end_point.hpp b/smacc2_sm_reference_library/sm_dancebot_artgallery_ue/include/sm_dancebot_artgallery_ue/states/radial_motion_states/sti_radial_end_point.hpp new file mode 100644 index 000000000..3d4dde52f --- /dev/null +++ b/smacc2_sm_reference_library/sm_dancebot_artgallery_ue/include/sm_dancebot_artgallery_ue/states/radial_motion_states/sti_radial_end_point.hpp @@ -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 + * + ******************************************************************************************************************/ + +namespace sm_dancebot_artgallery_ue +{ +namespace radial_motion_states +{ +// STATE DECLARATION +struct StiRadialEndPoint : smacc2::SmaccState +{ + using SmaccState::SmaccState; + + // TRANSITION TABLE + typedef mpl::list< + + Transition, StiRadialReturn, SUCCESS>, + Transition, StiRadialReturn, ABORT> + + >reactions; + + // STATE FUNCTIONS + static void staticConfigure() + { + //RCLCPP_INFO(getLogger(),"ssr radial end point, distance in meters: %lf", SS::ray_length_meters()); + configure_orthogonal(SS::ray_length_meters()); + configure_orthogonal(); + } + + void runtimeConfigure() + { + cl_nav2z::odom_tracker::CpOdomTracker * odomTracker; + this->requiresComponent(odomTracker); + auto* cbForwardMotion = this->template getOrthogonal()->template getClientBehavior(); + auto previousGoal = odomTracker->getCurrentMotionGoal(); + + if (previousGoal) + { + cbForwardMotion->options.forceInitialOrientation = previousGoal->pose.orientation; + RCLCPP_ERROR_STREAM(this->getLogger(), "Previous goal orientation: " << previousGoal->pose.orientation.x << ", " << previousGoal->pose.orientation.y << ", " << previousGoal->pose.orientation.z << ", " << previousGoal->pose.orientation.w); + }; + + RCLCPP_ERROR_STREAM(this->getLogger(), ".."); + } +}; +} // namespace radial_motion_states +} // namespace sm_dancebot_artgallery_ue diff --git a/smacc2_sm_reference_library/sm_dancebot_artgallery_ue/include/sm_dancebot_artgallery_ue/states/radial_motion_states/sti_radial_loop_start.hpp b/smacc2_sm_reference_library/sm_dancebot_artgallery_ue/include/sm_dancebot_artgallery_ue/states/radial_motion_states/sti_radial_loop_start.hpp new file mode 100644 index 000000000..4c5d5acca --- /dev/null +++ b/smacc2_sm_reference_library/sm_dancebot_artgallery_ue/include/sm_dancebot_artgallery_ue/states/radial_motion_states/sti_radial_loop_start.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 + * + ******************************************************************************************************************/ + +namespace sm_dancebot_artgallery_ue +{ +namespace radial_motion_states +{ +// STATE DECLARATION +struct StiRadialLoopStart : smacc2::SmaccState +{ + using SmaccState::SmaccState; + + // TRANSITION TABLE + typedef mpl::list< + + Transition, StiRadialRotate, CONTINUELOOP> + + >reactions; + + // STATE FUNCTIONS + static void staticConfigure() {} + + void runtimeConfigure() {} + + bool loopWhileCondition() + { + auto & superstate = this->context(); + + RCLCPP_INFO( + getLogger(), "Loop start, current iterations: %d, total iterations: %d", + superstate.iteration_count, superstate.total_iterations()); + return superstate.iteration_count++ < superstate.total_iterations(); + } + + void onEntry() + { + RCLCPP_INFO(getLogger(), "LOOP START ON ENTRY"); + checkWhileLoopConditionAndThrowEvent(&StiRadialLoopStart::loopWhileCondition); + } +}; +} // namespace radial_motion_states +} // namespace sm_dancebot_artgallery_ue diff --git a/smacc2_sm_reference_library/sm_dancebot_artgallery_ue/include/sm_dancebot_artgallery_ue/states/radial_motion_states/sti_radial_return.hpp b/smacc2_sm_reference_library/sm_dancebot_artgallery_ue/include/sm_dancebot_artgallery_ue/states/radial_motion_states/sti_radial_return.hpp new file mode 100644 index 000000000..8169a6885 --- /dev/null +++ b/smacc2_sm_reference_library/sm_dancebot_artgallery_ue/include/sm_dancebot_artgallery_ue/states/radial_motion_states/sti_radial_return.hpp @@ -0,0 +1,55 @@ +// 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 + * + ******************************************************************************************************************/ + +namespace sm_dancebot_artgallery_ue +{ +namespace radial_motion_states +{ +// STATE DECLARATION +struct StiRadialReturn : smacc2::SmaccState +{ + using SmaccState::SmaccState; + + // TRANSITION TABLE + typedef mpl::list< + + Transition, StiRadialLoopStart, SUCCESS>, + Transition, StiRadialEndPoint, ABORT> + + >reactions; + + // STATE FUNCTIONS + static void staticConfigure() + { + configure_orthogonal(); + configure_orthogonal(); + } + + void onExit() + { + ClNav2Z * moveBase; + this->requiresClient(moveBase); + + auto odomTracker = moveBase->getComponent(); + odomTracker->clearPath(); + } +}; +} // namespace radial_motion_states +} // namespace sm_dancebot_artgallery_ue diff --git a/smacc2_sm_reference_library/sm_dancebot_artgallery_ue/include/sm_dancebot_artgallery_ue/states/radial_motion_states/sti_radial_rotate.hpp b/smacc2_sm_reference_library/sm_dancebot_artgallery_ue/include/sm_dancebot_artgallery_ue/states/radial_motion_states/sti_radial_rotate.hpp new file mode 100644 index 000000000..8f5d77a6f --- /dev/null +++ b/smacc2_sm_reference_library/sm_dancebot_artgallery_ue/include/sm_dancebot_artgallery_ue/states/radial_motion_states/sti_radial_rotate.hpp @@ -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 + * + ******************************************************************************************************************/ + +namespace sm_dancebot_artgallery_ue +{ +namespace radial_motion_states +{ +// STATE DECLARATION +struct StiRadialRotate : smacc2::SmaccState +{ + using SmaccState::SmaccState; + + // TRANSITION TABLE + typedef mpl::list< + + Transition, StiRadialEndPoint, SUCCESS>, + Transition, StiRadialRotate, ABORT> + + >reactions; + + // STATE FUNCTIONS + static void staticConfigure() + { + configure_orthogonal(); + configure_orthogonal(); + } + + void runtimeConfigure() + { + auto cbAbsRotate = this->getClientBehavior(); + + cbAbsRotate->spinningPlanner = SpinningPlanner::PureSpinning; + + auto & superstate = this->context(); + cbAbsRotate->absoluteGoalAngleDegree = + superstate.iteration_count * SS::ray_angle_increment_degree(); + } +}; +} // namespace radial_motion_states +} // namespace sm_dancebot_artgallery_ue diff --git a/smacc2_sm_reference_library/sm_dancebot_artgallery_ue/include/sm_dancebot_artgallery_ue/states/s_pattern_states/sti_spattern_forward_1.hpp b/smacc2_sm_reference_library/sm_dancebot_artgallery_ue/include/sm_dancebot_artgallery_ue/states/s_pattern_states/sti_spattern_forward_1.hpp new file mode 100644 index 000000000..e5d7d1889 --- /dev/null +++ b/smacc2_sm_reference_library/sm_dancebot_artgallery_ue/include/sm_dancebot_artgallery_ue/states/s_pattern_states/sti_spattern_forward_1.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 + * + ******************************************************************************************************************/ + +namespace sm_dancebot_artgallery_ue +{ +namespace s_pattern_states +{ +// STATE DECLARATION +struct StiSPatternForward1 : public smacc2::SmaccState +{ + using SmaccState::SmaccState; + + // TRANSITION TABLE + typedef mpl::list< + + Transition, StiSPatternRotate2>, + Transition, StiSPatternRotate1> + + >reactions; + + // STATE FUNCTIONS + static void staticConfigure() + { + configure_orthogonal(SS::pitch1_lenght_meters()); + configure_orthogonal(); + } + + void runtimeConfigure() {} +}; +} // namespace s_pattern_states +} // namespace sm_dancebot_artgallery_ue diff --git a/smacc2_sm_reference_library/sm_dancebot_artgallery_ue/include/sm_dancebot_artgallery_ue/states/s_pattern_states/sti_spattern_forward_2.hpp b/smacc2_sm_reference_library/sm_dancebot_artgallery_ue/include/sm_dancebot_artgallery_ue/states/s_pattern_states/sti_spattern_forward_2.hpp new file mode 100644 index 000000000..f011dd8df --- /dev/null +++ b/smacc2_sm_reference_library/sm_dancebot_artgallery_ue/include/sm_dancebot_artgallery_ue/states/s_pattern_states/sti_spattern_forward_2.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 + * + ******************************************************************************************************************/ + +namespace sm_dancebot_artgallery_ue +{ +namespace s_pattern_states +{ +// STATE DECLARATION +struct StiSPatternForward2 : public smacc2::SmaccState +{ + using SmaccState::SmaccState; + + // TRANSITION TABLE + typedef mpl::list< + + Transition, StiSPatternRotate3>, + Transition, StiSPatternRotate2> + + >reactions; + + // STATE FUNCTIONS + static void staticConfigure() + { + configure_orthogonal(SS::pitch2_lenght_meters()); + configure_orthogonal(); + } + + void runtimeConfigure() {} +}; +} // namespace s_pattern_states +} // namespace sm_dancebot_artgallery_ue diff --git a/smacc2_sm_reference_library/sm_dancebot_artgallery_ue/include/sm_dancebot_artgallery_ue/states/s_pattern_states/sti_spattern_forward_3.hpp b/smacc2_sm_reference_library/sm_dancebot_artgallery_ue/include/sm_dancebot_artgallery_ue/states/s_pattern_states/sti_spattern_forward_3.hpp new file mode 100644 index 000000000..3075f9bdb --- /dev/null +++ b/smacc2_sm_reference_library/sm_dancebot_artgallery_ue/include/sm_dancebot_artgallery_ue/states/s_pattern_states/sti_spattern_forward_3.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 + * + ******************************************************************************************************************/ + +namespace sm_dancebot_artgallery_ue +{ +namespace s_pattern_states +{ +// STATE DECLARATION +struct StiSPatternForward3 : public smacc2::SmaccState +{ + using SmaccState::SmaccState; + + // TRANSITION TABLE + typedef mpl::list< + + Transition, StiSPatternRotate4>, + Transition, StiSPatternRotate3> + + >reactions; + + // STATE FUNCTIONS + static void staticConfigure() + { + configure_orthogonal(SS::pitch1_lenght_meters()); + configure_orthogonal(); + } +}; +} // namespace s_pattern_states +} // namespace sm_dancebot_artgallery_ue diff --git a/smacc2_sm_reference_library/sm_dancebot_artgallery_ue/include/sm_dancebot_artgallery_ue/states/s_pattern_states/sti_spattern_forward_4.hpp b/smacc2_sm_reference_library/sm_dancebot_artgallery_ue/include/sm_dancebot_artgallery_ue/states/s_pattern_states/sti_spattern_forward_4.hpp new file mode 100644 index 000000000..64aaed8da --- /dev/null +++ b/smacc2_sm_reference_library/sm_dancebot_artgallery_ue/include/sm_dancebot_artgallery_ue/states/s_pattern_states/sti_spattern_forward_4.hpp @@ -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 + * + ******************************************************************************************************************/ + +namespace sm_dancebot_artgallery_ue +{ +namespace s_pattern_states +{ +// STATE DECLARATION +struct StiSPatternForward4 : public smacc2::SmaccState +{ + using SmaccState::SmaccState; + + // TRANSITION TABLE + typedef mpl::list< + + Transition, StiSPatternLoopStart>, + Transition, StiSPatternRotate4> + + >reactions; + + // STATE FUNCTIONS + static void staticConfigure() + { + + configure_orthogonal(SS::pitch2_lenght_meters()); + configure_orthogonal(); + } + + void runtimeConfigure() + { + //auto &superstate = this->context(); + + } +}; +} // namespace s_pattern_states +} // namespace sm_dancebot_artgallery_ue diff --git a/smacc2_sm_reference_library/sm_dancebot_artgallery_ue/include/sm_dancebot_artgallery_ue/states/s_pattern_states/sti_spattern_loop_start.hpp b/smacc2_sm_reference_library/sm_dancebot_artgallery_ue/include/sm_dancebot_artgallery_ue/states/s_pattern_states/sti_spattern_loop_start.hpp new file mode 100644 index 000000000..69e373bee --- /dev/null +++ b/smacc2_sm_reference_library/sm_dancebot_artgallery_ue/include/sm_dancebot_artgallery_ue/states/s_pattern_states/sti_spattern_loop_start.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 + * + ******************************************************************************************************************/ + +namespace sm_dancebot_artgallery_ue +{ +namespace s_pattern_states +{ +template +struct B : smacc2::SmaccState +{ +public: + using SmaccState::SmaccState; + //typedef typename TDerived::reactions reactions; +}; + +// STATE DECLARATION +struct StiSPatternLoopStart : public B +{ + using B::B; + + // TRANSITION TABLE + typedef mpl::list< + + Transition, StiSPatternRotate1, CONTINUELOOP> + + >reactions; + + // STATE FUNCTIONS + static void staticConfigure() {} + + void runtimeConfigure() {} + + bool loopCondition() + { + auto & superstate = this->context(); + return superstate.iteration_count++ < superstate.total_iterations(); + } + + void onEntry() { checkWhileLoopConditionAndThrowEvent(&StiSPatternLoopStart::loopCondition); } +}; + +} // namespace s_pattern_states +} // namespace sm_dancebot_artgallery_ue diff --git a/smacc2_sm_reference_library/sm_dancebot_artgallery_ue/include/sm_dancebot_artgallery_ue/states/s_pattern_states/sti_spattern_rotate_1.hpp b/smacc2_sm_reference_library/sm_dancebot_artgallery_ue/include/sm_dancebot_artgallery_ue/states/s_pattern_states/sti_spattern_rotate_1.hpp new file mode 100644 index 000000000..45c1df93e --- /dev/null +++ b/smacc2_sm_reference_library/sm_dancebot_artgallery_ue/include/sm_dancebot_artgallery_ue/states/s_pattern_states/sti_spattern_rotate_1.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 + * + ******************************************************************************************************************/ + +namespace sm_dancebot_artgallery_ue +{ +namespace s_pattern_states +{ +// STATE DECLARATION +struct StiSPatternRotate1 : smacc2::SmaccState +{ + using SmaccState::SmaccState; + + // TRANSITION TABLE + typedef mpl::list< + + Transition, StiSPatternForward1>, + Transition, StiSPatternRotate1> + + >reactions; + + // STATE FUNCTIONS + static void staticConfigure() + { + float offset = 0; + if (SS::direction() == TDirection::RIGHT) + { + // - offset because we are looking to the north and we have to turn clockwise + configure_orthogonal(0 - offset); + } + else + { + // - offset because we are looking to the south and we have to turn counter-clockwise + configure_orthogonal(180 + offset); + } + } + + void runtimeConfigure() + { + + } +}; +} // namespace s_pattern_states +} // namespace sm_dancebot_artgallery_ue diff --git a/smacc2_sm_reference_library/sm_dancebot_artgallery_ue/include/sm_dancebot_artgallery_ue/states/s_pattern_states/sti_spattern_rotate_2.hpp b/smacc2_sm_reference_library/sm_dancebot_artgallery_ue/include/sm_dancebot_artgallery_ue/states/s_pattern_states/sti_spattern_rotate_2.hpp new file mode 100644 index 000000000..b2954e212 --- /dev/null +++ b/smacc2_sm_reference_library/sm_dancebot_artgallery_ue/include/sm_dancebot_artgallery_ue/states/s_pattern_states/sti_spattern_rotate_2.hpp @@ -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 + * + ******************************************************************************************************************/ + +namespace sm_dancebot_artgallery_ue +{ +namespace s_pattern_states +{ +// STATE DECLARATION +struct StiSPatternRotate2 : smacc2::SmaccState +{ + using SmaccState::SmaccState; + + // TRANSITION TABLE + typedef mpl::list< + + Transition, StiSPatternForward2>, + Transition, StiSPatternForward2> + + >reactions; + + // STATE FUNCTIONS + static void staticConfigure() + { + float offset = 0; + float angle = 0; + if (SS::direction() == TDirection::LEFT) + angle = 90 + offset; + else + angle = -90 - offset; + + configure_orthogonal(angle); + configure_orthogonal(); + } + + void runtimeConfigure() + { + auto & superstate = this->context(); + RCLCPP_INFO( + getLogger(), "[StiSPatternRotate] SpatternRotate rotate: SS current iteration: %d/%d", + superstate.iteration_count, SS::total_iterations()); + } +}; +} // namespace s_pattern_states +} // namespace sm_dancebot_artgallery_ue diff --git a/smacc2_sm_reference_library/sm_dancebot_artgallery_ue/include/sm_dancebot_artgallery_ue/states/s_pattern_states/sti_spattern_rotate_3.hpp b/smacc2_sm_reference_library/sm_dancebot_artgallery_ue/include/sm_dancebot_artgallery_ue/states/s_pattern_states/sti_spattern_rotate_3.hpp new file mode 100644 index 000000000..20516030c --- /dev/null +++ b/smacc2_sm_reference_library/sm_dancebot_artgallery_ue/include/sm_dancebot_artgallery_ue/states/s_pattern_states/sti_spattern_rotate_3.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 + * + ******************************************************************************************************************/ + +namespace sm_dancebot_artgallery_ue +{ +namespace s_pattern_states +{ +// STATE DECLARATION +struct StiSPatternRotate3 : smacc2::SmaccState +{ + using SmaccState::SmaccState; + + // TRANSITION TABLE + typedef mpl::list< + + Transition, StiSPatternForward3>, + Transition, StiSPatternRotate3> + + >reactions; + + // STATE FUNCTIONS + static void staticConfigure() + { + float offset = 0; + + if (SS::direction() == TDirection::RIGHT) + { + // - offset because we are looking to the north and we have to turn clockwise + configure_orthogonal(0 - offset); + } + else + { + // - offset because we are looking to the south and we have to turn counter-clockwise + configure_orthogonal(180 + offset); + } + + configure_orthogonal(); + } + + void runtimeConfigure() + { + auto & superstate = this->context(); + RCLCPP_INFO( + getLogger(), "[StiSPatternRotate] SpatternRotate rotate: SS current iteration: %d/%d", + superstate.iteration_count, SS::total_iterations()); + + + } +}; +} // namespace s_pattern_states +} // namespace sm_dancebot_artgallery_ue diff --git a/smacc2_sm_reference_library/sm_dancebot_artgallery_ue/include/sm_dancebot_artgallery_ue/states/s_pattern_states/sti_spattern_rotate_4.hpp b/smacc2_sm_reference_library/sm_dancebot_artgallery_ue/include/sm_dancebot_artgallery_ue/states/s_pattern_states/sti_spattern_rotate_4.hpp new file mode 100644 index 000000000..e34959977 --- /dev/null +++ b/smacc2_sm_reference_library/sm_dancebot_artgallery_ue/include/sm_dancebot_artgallery_ue/states/s_pattern_states/sti_spattern_rotate_4.hpp @@ -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 + * + ******************************************************************************************************************/ + +namespace sm_dancebot_artgallery_ue +{ +namespace s_pattern_states +{ +// STATE DECLARATION +struct StiSPatternRotate4 : smacc2::SmaccState +{ + using SmaccState::SmaccState; + + // TRANSITION TABLE + typedef mpl::list< + + Transition, StiSPatternForward4>, + Transition, StiSPatternRotate4> + + >reactions; + + // STATE FUNCTIONS + static void staticConfigure() + { + float offset = 0; + float angle = 0; + if (SS::direction() == TDirection::LEFT) + angle = -90 - offset; + else + angle = 90 + offset; + + configure_orthogonal(angle); + configure_orthogonal(); + } + + void runtimeConfigure() + { + + auto & superstate = this->context(); + RCLCPP_INFO( + getLogger(), "[SsrSPatternRotate] SpatternRotate rotate: SS current iteration: %d/%d", + superstate.iteration_count, SS::total_iterations()); + + } +}; +} // namespace s_pattern_states +} // namespace sm_dancebot_artgallery_ue diff --git a/smacc2_sm_reference_library/sm_dancebot_artgallery_ue/include/sm_dancebot_artgallery_ue/states/st_acquire_sensors.hpp b/smacc2_sm_reference_library/sm_dancebot_artgallery_ue/include/sm_dancebot_artgallery_ue/states/st_acquire_sensors.hpp new file mode 100644 index 000000000..50365f9dd --- /dev/null +++ b/smacc2_sm_reference_library/sm_dancebot_artgallery_ue/include/sm_dancebot_artgallery_ue/states/st_acquire_sensors.hpp @@ -0,0 +1,66 @@ +// 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 + +namespace sm_dancebot_artgallery_ue +{ +using namespace smacc2::default_events; +using smacc2::client_behaviors::CbSleepFor; +using namespace std::chrono_literals; + +// STATE DECLARATION +struct StAcquireSensors : smacc2::SmaccState +{ + using SmaccState::SmaccState; + + // DECLARE CUSTOM OBJECT TAGS + struct ON_SENSORS_AVAILABLE : SUCCESS{}; + struct SrAcquireSensors; + + // TRANSITION TABLE + typedef mpl::list< + + // + Transition, StNavigateArtGalleryWaypointsX, ON_SENSORS_AVAILABLE>, + Transition + + >reactions; + + // STATE FUNCTIONS + static void staticConfigure() + { + configure_orthogonal("waypoints_art_gallery_1", "sm_dancebot_artgallery_ue"); + configure_orthogonal(5s); + + + // Create State Reactor + auto srAllSensorsReady = static_createStateReactor< + SrAllEventsGo, smacc2::state_reactors::EvAllGo, + mpl::list< + // EvTopicMessage, + // EvCbSuccess, + EvCbSuccess + >>(); + } +}; +} // namespace sm_dancebot_artgallery_ue diff --git a/smacc2_sm_reference_library/sm_dancebot_artgallery_ue/include/sm_dancebot_artgallery_ue/states/st_final_state.hpp b/smacc2_sm_reference_library/sm_dancebot_artgallery_ue/include/sm_dancebot_artgallery_ue/states/st_final_state.hpp new file mode 100644 index 000000000..c856d7fea --- /dev/null +++ b/smacc2_sm_reference_library/sm_dancebot_artgallery_ue/include/sm_dancebot_artgallery_ue/states/st_final_state.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 + +namespace sm_dancebot_artgallery_ue +{ +using namespace smacc2::default_events; +using smacc2::client_behaviors::CbSleepFor; +using namespace std::chrono_literals; + +// STATE DECLARATION +struct StFinalState : smacc2::SmaccState +{ + using SmaccState::SmaccState; + + // TRANSITION TABLE + typedef mpl::list< + >reactions; + + // STATE FUNCTIONS + static void staticConfigure() + { + configure_orthogonal(); + } + + void runtimeConfigure() + { + + } +}; +} // namespace sm_dancebot_artgallery_ue diff --git a/smacc2_sm_reference_library/sm_dancebot_artgallery_ue/include/sm_dancebot_artgallery_ue/states/st_navigate_artgallery_waypoints.x.hpp b/smacc2_sm_reference_library/sm_dancebot_artgallery_ue/include/sm_dancebot_artgallery_ue/states/st_navigate_artgallery_waypoints.x.hpp new file mode 100644 index 000000000..527fa4618 --- /dev/null +++ b/smacc2_sm_reference_library/sm_dancebot_artgallery_ue/include/sm_dancebot_artgallery_ue/states/st_navigate_artgallery_waypoints.x.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 + +namespace sm_dancebot_artgallery_ue +{ + +// STATE DECLARATION +struct StNavigateArtGalleryWaypointsX : smacc2::SmaccState +{ + using SmaccState::SmaccState; + + // CUSTOM TRANSITION TAGS + struct TRANSITION_1 : SUCCESS{}; + struct TRANSITION_2 : SUCCESS{}; + struct TRANSITION_3 : SUCCESS{}; + struct TRANSITION_4 : SUCCESS{}; + struct TRANSITION_5 : SUCCESS{}; + struct TRANSITION_6 : SUCCESS{}; + + // TRANSITION TABLE + typedef mpl::list< + Transition, + Transition, SS5::SsSPattern1, SUCCESS>, + Transition, SS4::SsFPattern1, SUCCESS>, + Transition, SS1::SsRadialPattern1, SUCCESS>, + Transition, StNavigateArtGalleryWaypointsX, SUCCESS>, + Transition, StNavigateArtGalleryWaypointsX, ABORT>, + Transition, StNavigateArtGalleryWaypointsX, ABORT> + // Transition, SS2::SsRadialPattern2, TRANSITION_4> + >reactions; + + // STATE FUNCTIONS + static void staticConfigure() + { + // configure_orthogonal(); + configure_orthogonal(); + } + + void onEntry() + { + } + + void runtimeConfigure() {} + + void onExit(ABORT) + { + } +}; +} // namespace sm_dancebot_artgallery_ue diff --git a/smacc2_sm_reference_library/sm_dancebot_artgallery_ue/include/sm_dancebot_artgallery_ue/superstates/ss_f_pattern_1.hpp b/smacc2_sm_reference_library/sm_dancebot_artgallery_ue/include/sm_dancebot_artgallery_ue/superstates/ss_f_pattern_1.hpp new file mode 100644 index 000000000..c429f53ce --- /dev/null +++ b/smacc2_sm_reference_library/sm_dancebot_artgallery_ue/include/sm_dancebot_artgallery_ue/superstates/ss_f_pattern_1.hpp @@ -0,0 +1,93 @@ +// 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 sm_dancebot_artgallery_ue +{ +namespace f_pattern_states +{ +enum class TDirection +{ + LEFT, + RIGHT +}; + +// FORWARD DECLARATIONS OF INNER STATES +template class StiFPatternRotate1; +template class StiFPatternForward1; +template class StiFPatternReturn1; +template class StiFPatternRotate2; +template class StiFPatternForward2; +template class StiFPatternStartLoop; + +} // namespace f_pattern_states +} // namespace sm_dancebot_artgallery_ue +namespace sm_dancebot_artgallery_ue +{ +namespace SS4 +{ +using namespace f_pattern_states; + +// STATE DECLARATION +struct SsFPattern1 +: smacc2::SmaccState> +{ +public: + using SmaccState::SmaccState; + + // TRANSITION TABLE + typedef mpl::list< + + Transition>, StNavigateArtGalleryWaypointsX, ENDLOOP> //, + + >reactions; + + // STATE VARIABLES + // superstate parameters + static constexpr float ray_lenght_meters() { return 3.25; } + static constexpr float pitch_lenght_meters() { return 0.75; } + static constexpr int total_iterations() { return 8; } + static constexpr TDirection direction() { return TDirection::RIGHT; } + + // superstate state variables + int iteration_count; + + double last_goal_angle_rad; + + // STATE FUNCTIONS + static void staticConfigure() + { + } + + void runtimeConfigure() { iteration_count = 0; } +}; // namespace SS4 + +// FORWARD DECLARATION FOR THE SUPERSTATE + +} // namespace SS4 +} // namespace sm_dancebot_artgallery_ue + +#include +#include +#include +#include +#include +#include diff --git a/smacc2_sm_reference_library/sm_dancebot_artgallery_ue/include/sm_dancebot_artgallery_ue/superstates/ss_radial_pattern_1.hpp b/smacc2_sm_reference_library/sm_dancebot_artgallery_ue/include/sm_dancebot_artgallery_ue/superstates/ss_radial_pattern_1.hpp new file mode 100644 index 000000000..9bb9c920e --- /dev/null +++ b/smacc2_sm_reference_library/sm_dancebot_artgallery_ue/include/sm_dancebot_artgallery_ue/superstates/ss_radial_pattern_1.hpp @@ -0,0 +1,78 @@ +// 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 sm_dancebot_artgallery_ue +{ +namespace SS1 +{ +namespace sm_dancebot_artgallery_ue +{ +namespace radial_motion_states +{ +//FORWARD DECLARATION OF INNER STATES +class StiRadialRotate; +class StiRadialReturn; +class StiRadialEndPoint; +class StiRadialLoopStart; + +} // namespace radial_motion_states +} // namespace sm_dancebot_artgallery_ue +using namespace sm_dancebot_artgallery_ue::radial_motion_states; + +// STATE DECLARATION +struct SsRadialPattern1 +: smacc2::SmaccState +{ +public: + using SmaccState::SmaccState; + + // TRANSITION TABLE + typedef mpl::list< + Transition, StNavigateArtGalleryWaypointsX, ENDLOOP> //, + + // Transition, StRotateDegrees1, ENDLOOP> + // Transition, StNavigateReverse1, ENDLOOP> + + >reactions; + + static constexpr int total_iterations() { return 16; } + static constexpr float ray_angle_increment_degree() { return 360.0 / total_iterations(); } + static constexpr float ray_length_meters() { return 3.0; } + + int iteration_count = 0; + + // STATE FUNCTIONS + static void staticConfigure() + { + } + + void runtimeConfigure() {} +}; + +// FORWARD DECLARATION FOR THE SUPERSTATE +using SS = SsRadialPattern1; +#include +#include +#include +#include +} // namespace SS1 +} // namespace sm_dancebot_artgallery_ue diff --git a/smacc2_sm_reference_library/sm_dancebot_artgallery_ue/include/sm_dancebot_artgallery_ue/superstates/ss_radial_pattern_2.hpp b/smacc2_sm_reference_library/sm_dancebot_artgallery_ue/include/sm_dancebot_artgallery_ue/superstates/ss_radial_pattern_2.hpp new file mode 100644 index 000000000..77994d6a3 --- /dev/null +++ b/smacc2_sm_reference_library/sm_dancebot_artgallery_ue/include/sm_dancebot_artgallery_ue/superstates/ss_radial_pattern_2.hpp @@ -0,0 +1,80 @@ +// 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 sm_dancebot_artgallery_ue +{ +namespace SS2 +{ +namespace sm_dancebot_artgallery_ue +{ +namespace radial_motion_states +{ +// FORWARD DECLARATIONS OF INNER STATES +class StiRadialRotate; +class StiRadialReturn; +class StiRadialEndPoint; +class StiRadialLoopStart; +} // namespace radial_motion_states +} // namespace sm_dancebot_artgallery_ue +using namespace sm_dancebot_artgallery_ue::radial_motion_states; + +// STATE DECLARATION +struct SsRadialPattern2 +: smacc2::SmaccState +{ +public: + using SmaccState::SmaccState; + + // TRANSITION TABLE + typedef mpl::list< + + Transition, StNavigateArtGalleryWaypointsX, ENDLOOP> + + >reactions; + + // STATE FUNCTIONS + static void staticConfigure() + { + } + + void runtimeConfigure() {} + + int iteration_count = 0; + static constexpr int total_iterations() { return 16; } + static constexpr float ray_angle_increment_degree() { return 360.0 / total_iterations(); } + static constexpr float ray_length_meters() { return 5; } + + void onExit() + { + rclcpp::sleep_for(5s); + RCLCPP_INFO(getLogger(), "[SsRadialPattern1] waiting 5 seconds"); + } +}; + +// FORWARD DECLARATION FOR THE SUPERSTATE +using SS = SsRadialPattern2; +#include +#include +#include +#include +} // namespace SS2 +} // namespace sm_dancebot_artgallery_ue diff --git a/smacc2_sm_reference_library/sm_dancebot_artgallery_ue/include/sm_dancebot_artgallery_ue/superstates/ss_radial_pattern_3.hpp b/smacc2_sm_reference_library/sm_dancebot_artgallery_ue/include/sm_dancebot_artgallery_ue/superstates/ss_radial_pattern_3.hpp new file mode 100644 index 000000000..c97f03b85 --- /dev/null +++ b/smacc2_sm_reference_library/sm_dancebot_artgallery_ue/include/sm_dancebot_artgallery_ue/superstates/ss_radial_pattern_3.hpp @@ -0,0 +1,76 @@ +// 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 sm_dancebot_artgallery_ue +{ +namespace SS3 +{ +namespace sm_dancebot_artgallery_ue +{ +namespace radial_motion_states +{ +// FORWARD DECLARATION OF INNER STATES +class StiRadialRotate; +class StiRadialReturn; +class StiRadialEndPoint; +class StiRadialLoopStart; +} // namespace radial_motion_states +} // namespace sm_dancebot_artgallery_ue +using namespace sm_dancebot_artgallery_ue::radial_motion_states; + +// STATE DECLARATION +struct SsRadialPattern3 +: smacc2::SmaccState +{ +public: + using SmaccState::SmaccState; + + // TRANSITION TABLE + typedef mpl::list< + Transition, StNavigateArtGalleryWaypointsX, ENDLOOP> //, + + // Transition, StRotateDegrees4, ENDLOOP> + + >reactions; + + // STATE FUNCTIONS + static void staticConfigure() + { + } + + int iteration_count; + + static constexpr int total_iterations() { return 16; } + static constexpr float ray_angle_increment_degree() { return 360.0 / total_iterations(); } + static constexpr float ray_length_meters() { return 4; } + + void runtimeConfigure() { iteration_count = 0; } +}; + +// FORWARD DECLARATION FOR THE SUPERSTATE +using SS = SsRadialPattern3; +#include +#include +#include +#include +} // namespace SS3 +} // namespace sm_dancebot_artgallery_ue diff --git a/smacc2_sm_reference_library/sm_dancebot_artgallery_ue/include/sm_dancebot_artgallery_ue/superstates/ss_s_pattern_1.hpp b/smacc2_sm_reference_library/sm_dancebot_artgallery_ue/include/sm_dancebot_artgallery_ue/superstates/ss_s_pattern_1.hpp new file mode 100644 index 000000000..bbbe37af4 --- /dev/null +++ b/smacc2_sm_reference_library/sm_dancebot_artgallery_ue/include/sm_dancebot_artgallery_ue/superstates/ss_s_pattern_1.hpp @@ -0,0 +1,93 @@ +// 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 sm_dancebot_artgallery_ue +{ +namespace SS5 +{ +namespace sm_dancebot_artgallery_ue +{ +namespace s_pattern_states +{ +// FORWARD DECLARATIONS OF INNER STATES +class StiSPatternRotate1; +class StiSPatternForward1; +class StiSPatternRotate2; +class StiSPatternForward2; +class StiSPatternRotate3; +class StiSPatternForward3; +class StiSPatternRotate4; +class StiSPatternForward4; +class StiSPatternLoopStart; +} // namespace s_pattern_states +} // namespace sm_dancebot_artgallery_ue + +enum class TDirection +{ + LEFT, + RIGHT +}; + +using namespace sm_dancebot_artgallery_ue::s_pattern_states; + +// STATE DECLARATION +struct SsSPattern1 : smacc2::SmaccState +{ +public: + using SmaccState::SmaccState; + + // TRANSITION TABLE + typedef mpl::list< + + // Transition, StNavigateReverse3, ENDLOOP> + Transition, StNavigateArtGalleryWaypointsX, ENDLOOP> //, + + >reactions; + + // STATE FUNCTIONS + static void staticConfigure() + { + } + + static constexpr float pitch1_lenght_meters() { return 0.75; } + static constexpr float pitch2_lenght_meters() { return 1.45; } + static constexpr int total_iterations() { return 9; } + static constexpr TDirection direction() { return TDirection::RIGHT; } + + int iteration_count; + + void runtimeConfigure() { this->iteration_count = 0; } +}; + +// FORWARD DECLARATION FOR THE SUPERSTATE +using SS = SsSPattern1; +#include +#include +#include +#include +#include +#include +#include +#include +#include +} // namespace SS5 +} // namespace sm_dancebot_artgallery_ue diff --git a/smacc2_sm_reference_library/sm_dancebot_artgallery_ue/launch/bringup_launch.py b/smacc2_sm_reference_library/sm_dancebot_artgallery_ue/launch/bringup_launch.py new file mode 100644 index 000000000..a9804d127 --- /dev/null +++ b/smacc2_sm_reference_library/sm_dancebot_artgallery_ue/launch/bringup_launch.py @@ -0,0 +1,137 @@ +# Copyright (c) 2018 Intel Corporation +# +# Licensed under the Apache License, Version 2.0 (the "License"); +# you may not use this file except in compliance with the License. +# You may obtain a copy of the License at +# +# http://www.apache.org/licenses/LICENSE-2.0 +# +# Unless required by applicable law or agreed to in writing, software +# distributed under the License is distributed on an "AS IS" BASIS, +# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +# See the License for the specific language governing permissions and +# limitations under the License. + +import os + +from ament_index_python.packages import get_package_share_directory + +from launch import LaunchDescription +from launch.actions import ( + DeclareLaunchArgument, + GroupAction, + IncludeLaunchDescription, + SetEnvironmentVariable, +) +from launch.conditions import IfCondition +from launch.launch_description_sources import PythonLaunchDescriptionSource +from launch.substitutions import LaunchConfiguration +from launch_ros.actions import PushRosNamespace + + +def generate_launch_description(): + # Get the launch directory + sm_dance_bot_dir = get_package_share_directory("sm_dancebot_artgallery_ue") + launch_dir = os.path.join(sm_dance_bot_dir, "launch") + + # Create the launch configuration variables + namespace = LaunchConfiguration("namespace") + use_namespace = LaunchConfiguration("use_namespace") + slam = LaunchConfiguration("slam") + # map_yaml_file = LaunchConfiguration("map") + use_sim_time = LaunchConfiguration("use_sim_time") + params_file = LaunchConfiguration("params_file") + default_nav_to_pose_bt_xml = LaunchConfiguration("default_nav_to_pose_bt_xml") + autostart = LaunchConfiguration("autostart") + + stdout_linebuf_envvar = SetEnvironmentVariable("RCUTILS_LOGGING_BUFFERED_STREAM", "1") + + declare_namespace_cmd = DeclareLaunchArgument( + "namespace", default_value="", description="Top-level namespace" + ) + + declare_use_namespace_cmd = DeclareLaunchArgument( + "use_namespace", + default_value="false", + description="Whether to apply a namespace to the navigation stack", + ) + + declare_slam_cmd = DeclareLaunchArgument( + "slam", default_value="true", description="Whether run a SLAM" + ) + + declare_map_yaml_cmd = DeclareLaunchArgument( + "map", description="Full path to map yaml file to load" + ) + + declare_use_sim_time_cmd = DeclareLaunchArgument( + "use_sim_time", default_value="false", description="Use simulation clock if true" + ) + + declare_params_file_cmd = DeclareLaunchArgument( + "params_file", + default_value=os.path.join(sm_dance_bot_dir, "params", "nav2z_client", "nav2_params.yaml"), + description="Full path to the ROS2 parameters file to use for all launched nodes", + ) + + declare_bt_xml_cmd = DeclareLaunchArgument( + "default_nav_to_pose_bt_xml", + default_value=os.path.join( + sm_dance_bot_dir, "params", "nav2z_client", "navigation_tree.xml" + ), + description="Full path to the behavior tree xml file to use", + ) + + declare_autostart_cmd = DeclareLaunchArgument( + "autostart", default_value="true", description="Automatically startup the nav2 stack" + ) + + # Specify the actions + bringup_cmd_group = GroupAction( + [ + PushRosNamespace(condition=IfCondition(use_namespace), namespace=namespace), + IncludeLaunchDescription( + PythonLaunchDescriptionSource(os.path.join(launch_dir, "slam_launch.py")), + condition=IfCondition(slam), + launch_arguments={ + "namespace": namespace, + "use_sim_time": use_sim_time, + "autostart": autostart, + "params_file": params_file, + }.items(), + ), + IncludeLaunchDescription( + PythonLaunchDescriptionSource(os.path.join(launch_dir, "navigation_launch.py")), + launch_arguments={ + "namespace": namespace, + "use_sim_time": use_sim_time, + "autostart": autostart, + "params_file": params_file, + "default_nav_to_pose_bt_xml": default_nav_to_pose_bt_xml, + "use_lifecycle_mgr": "false", + "map_subscribe_transient_local": "true", + }.items(), + ), + ] + ) + + # Create the launch description and populate + ld = LaunchDescription() + + # Set environment variables + ld.add_action(stdout_linebuf_envvar) + + # Declare the launch options + ld.add_action(declare_namespace_cmd) + ld.add_action(declare_use_namespace_cmd) + ld.add_action(declare_slam_cmd) + ld.add_action(declare_map_yaml_cmd) + ld.add_action(declare_use_sim_time_cmd) + ld.add_action(declare_params_file_cmd) + ld.add_action(declare_autostart_cmd) + ld.add_action(declare_bt_xml_cmd) + + # Add the actions to launch all of the navigation nodes + ld.add_action(bringup_cmd_group) + + return ld diff --git a/smacc2_sm_reference_library/sm_dancebot_artgallery_ue/launch/husky_gazebo.launch.yaml b/smacc2_sm_reference_library/sm_dancebot_artgallery_ue/launch/husky_gazebo.launch.yaml new file mode 100644 index 000000000..66bb498b0 --- /dev/null +++ b/smacc2_sm_reference_library/sm_dancebot_artgallery_ue/launch/husky_gazebo.launch.yaml @@ -0,0 +1,3 @@ +launch: + - include: + file: "$(find-pkg-share husky_gazebo)/launch/gazebo_launch.py" diff --git a/smacc2_sm_reference_library/sm_dancebot_artgallery_ue/launch/navigation_launch.py b/smacc2_sm_reference_library/sm_dancebot_artgallery_ue/launch/navigation_launch.py new file mode 100644 index 000000000..ec67fd974 --- /dev/null +++ b/smacc2_sm_reference_library/sm_dancebot_artgallery_ue/launch/navigation_launch.py @@ -0,0 +1,177 @@ +# Copyright (c) 2018 Intel Corporation +# +# Licensed under the Apache License, Version 2.0 (the "License"); +# you may not use this file except in compliance with the License. +# You may obtain a copy of the License at +# +# http://www.apache.org/licenses/LICENSE-2.0 +# +# Unless required by applicable law or agreed to in writing, software +# distributed under the License is distributed on an "AS IS" BASIS, +# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +# See the License for the specific language governing permissions and +# limitations under the License. + +import os + +from ament_index_python.packages import get_package_share_directory + +from launch import LaunchDescription +from launch.actions import DeclareLaunchArgument, SetEnvironmentVariable +from launch.substitutions import LaunchConfiguration +from launch_ros.actions import Node +from nav2_common.launch import RewrittenYaml + + +def generate_launch_description(): + # Get the launch directory + sm_dance_bot_dir = get_package_share_directory("sm_dancebot_artgallery_ue") + + namespace = LaunchConfiguration("namespace") + use_sim_time = LaunchConfiguration("use_sim_time") + autostart = LaunchConfiguration("autostart") + params_file = LaunchConfiguration("params_file") + default_nav_to_pose_bt_xml = LaunchConfiguration("default_nav_to_pose_bt_xml") + map_subscribe_transient_local = LaunchConfiguration("map_subscribe_transient_local") + + lifecycle_nodes = ["controller_server", "planner_server", "behavior_server", "bt_navigator"] + + # Map fully qualified names to relative ones so the node's namespace can be prepended. + # In case of the transforms (tf), currently, there doesn't seem to be a better alternative + # https://github.com/ros/geometry2/issues/32 + # https://github.com/ros/robot_state_publisher/pull/30 + # TODO(orduno) Substitute with `PushNodeRemapping` + # https://github.com/ros2/launch_ros/issues/56 + remappings = [("/tf", "tf"), ("/tf_static", "tf_static")] + + # Create our own temporary YAML files that include substitutions + param_substitutions = { + "use_sim_time": use_sim_time, + # 'default_nav_to_pose_bt_xml': default_nav_to_pose_bt_xml, + "default_nav_to_pose_bt_xml": os.path.join( + sm_dance_bot_dir, "params", "nav2z_client", "navigation_tree.xml" + ), + "autostart": autostart, + "map_subscribe_transient_local": map_subscribe_transient_local, + } + + configured_params = RewrittenYaml( + source_file=params_file, + root_key=namespace, + param_rewrites=param_substitutions, + convert_types=True, + ) + + xtermprefix = "xterm -xrm 'XTerm*scrollBar: true' -xrm 'xterm*rightScrollBar: true' -hold -geometry 1000x600 -sl 10000 -e" + + print("+********************************") + print(str(param_substitutions)) + print(str(default_nav_to_pose_bt_xml)) + + return LaunchDescription( + [ + # Set env var to print messages to stdout immediately + SetEnvironmentVariable("RCUTILS_LOGGING_BUFFERED_STREAM", "1"), + DeclareLaunchArgument( + "namespace", default_value="", description="Top-level namespace" + ), + DeclareLaunchArgument( + "use_sim_time", + default_value="false", + description="Use simulation clock if true", + ), + DeclareLaunchArgument( + "autostart", + default_value="true", + description="Automatically startup the nav2 stack", + ), + DeclareLaunchArgument( + "params_file", + default_value=os.path.join( + sm_dance_bot_dir, "params", "nav2z_client", "nav2_params.yaml" + ), + description="Full path to the ROS2 parameters file to use", + ), + DeclareLaunchArgument( + "default_nav_to_pose_bt_xml", + # default_value=os.path.join(get_package_share_directory('nav2_bt_navigator'),'behavior_trees', 'navigate_w_replanning_and_recovery.xml'), + default_value=os.path.join( + sm_dance_bot_dir, "params", "nav2z_client", "navigation_tree.xml" + ), + description="Full path to the behavior tree xml file to use", + ), + DeclareLaunchArgument( + "map_subscribe_transient_local", + default_value="false", + description="Whether to set the map subscriber QoS to transient local", + ), + Node( + package="nav2_controller", + executable="controller_server", + output="screen", + prefix=xtermprefix, + parameters=[configured_params], + remappings=remappings, + arguments=[ + "--ros-args", + "--log-level", + "INFO", + "--log-level", + "custom_planners:=DEBUG", + "--log-level", + "controller_server:=DEBUG", + ], + ), + Node( + package="nav2_planner", + executable="planner_server", + name="planner_server", + output="screen", + prefix=xtermprefix, + parameters=[configured_params], + remappings=remappings, + arguments=["--ros-args", "--log-level", "INFO"], + ), + Node( + package="nav2_behaviors", + executable="behavior_server", + name="behavior_server", + output="screen", + prefix=xtermprefix, + parameters=[configured_params], + remappings=remappings, + arguments=["--ros-args", "--log-level", "INFO"], + ), + Node( + package="nav2_bt_navigator", + executable="bt_navigator", + name="bt_navigator", + output="screen", + prefix=xtermprefix, + parameters=[configured_params], + remappings=remappings, + arguments=["--ros-args", "--log-level", "INFO"], + ), + Node( + package="nav2_waypoint_follower", + executable="waypoint_follower", + name="waypoint_follower", + output="screen", + parameters=[configured_params], + remappings=remappings, + ), + Node( + package="nav2_lifecycle_manager", + executable="lifecycle_manager", + name="lifecycle_manager_navigation", + output="screen", + prefix=xtermprefix, + parameters=[ + {"use_sim_time": use_sim_time}, + {"autostart": autostart}, + {"node_names": lifecycle_nodes}, + ], + arguments=["--ros-args", "--log-level", "INFO"], + ), + ] + ) diff --git a/smacc2_sm_reference_library/sm_dancebot_artgallery_ue/launch/online_sync_launch.py b/smacc2_sm_reference_library/sm_dancebot_artgallery_ue/launch/online_sync_launch.py new file mode 100644 index 000000000..38c20d044 --- /dev/null +++ b/smacc2_sm_reference_library/sm_dancebot_artgallery_ue/launch/online_sync_launch.py @@ -0,0 +1,61 @@ +# Copyright (c) 2023 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 + + +import os + +from launch import LaunchDescription +from launch.actions import DeclareLaunchArgument +from launch.substitutions import LaunchConfiguration +from launch_ros.actions import Node +from ament_index_python.packages import get_package_share_directory + + +def generate_launch_description(): + use_sim_time = LaunchConfiguration("use_sim_time") + slam_params_file = LaunchConfiguration("slam_params_file") + xtermprefix = "xterm -xrm 'XTerm*scrollBar: true' -xrm 'xterm*rightScrollBar: true' -hold -geometry 1000x600 -sl 10000 -e" + + declare_use_sim_time_argument = DeclareLaunchArgument( + "use_sim_time", default_value="true", description="Use simulation/Gazebo clock" + ) + declare_slam_params_file_cmd = DeclareLaunchArgument( + "slam_params_file", + default_value=os.path.join( + # get_package_share_directory("slam_toolbox"), "config", "mapper_params_online_sync.yaml" + get_package_share_directory("sm_dancebot_artgallery_ue"), + "params", + "mapper_params_online_sync.yaml", + ), + description="Full path to the ROS2 parameters file to use for the slam_toolbox node", + ) + + start_sync_slam_toolbox_node = Node( + parameters=[slam_params_file, {"use_sim_time": use_sim_time}], + package="slam_toolbox", + executable="sync_slam_toolbox_node", + name="slam_toolbox", + output="screen", + prefix=xtermprefix, + ) + + ld = LaunchDescription() + + ld.add_action(declare_use_sim_time_argument) + ld.add_action(declare_slam_params_file_cmd) + ld.add_action(start_sync_slam_toolbox_node) + + return ld diff --git a/smacc2_sm_reference_library/sm_dancebot_artgallery_ue/launch/rviz_launch.py b/smacc2_sm_reference_library/sm_dancebot_artgallery_ue/launch/rviz_launch.py new file mode 100644 index 000000000..056b5f8f0 --- /dev/null +++ b/smacc2_sm_reference_library/sm_dancebot_artgallery_ue/launch/rviz_launch.py @@ -0,0 +1,122 @@ +# Copyright (c) 2018 Intel Corporation +# +# Licensed under the Apache License, Version 2.0 (the "License"); +# you may not use this file except in compliance with the License. +# You may obtain a copy of the License at +# +# http://www.apache.org/licenses/LICENSE-2.0 +# +# Unless required by applicable law or agreed to in writing, software +# distributed under the License is distributed on an "AS IS" BASIS, +# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +# See the License for the specific language governing permissions and +# limitations under the License. + +import os + +from ament_index_python.packages import get_package_share_directory + +from launch import LaunchDescription +from launch.actions import DeclareLaunchArgument, EmitEvent, RegisterEventHandler +from launch.conditions import IfCondition, UnlessCondition +from launch.event_handlers import OnProcessExit +from launch.events import Shutdown +from launch.substitutions import LaunchConfiguration +from launch_ros.actions import Node +from nav2_common.launch import ReplaceString + + +def generate_launch_description(): + # Get the launch directory + bringup_dir = get_package_share_directory("nav2_bringup") + + # Create the launch configuration variables + namespace = LaunchConfiguration("namespace") + use_namespace = LaunchConfiguration("use_namespace") + rviz_config_file = LaunchConfiguration("rviz_config") + + # Declare the launch arguments + declare_namespace_cmd = DeclareLaunchArgument( + "namespace", + default_value="navigation", + description=( + "Top-level namespace. The value will be used to replace the " + " keyword on the rviz config file." + ), + ) + + declare_use_namespace_cmd = DeclareLaunchArgument( + "use_namespace", + default_value="false", + description="Whether to apply a namespace to the navigation stack", + ) + + declare_rviz_config_file_cmd = DeclareLaunchArgument( + "rviz_config", + default_value=os.path.join(bringup_dir, "rviz", "nav2_default_view.rviz"), + description="Full path to the RVIZ config file to use", + ) + + # Launch rviz + start_rviz_cmd = Node( + condition=UnlessCondition(use_namespace), + package="rviz2", + executable="rviz2", + name="rviz2", + arguments=["-d", rviz_config_file], + output="screen", + ) + + namespaced_rviz_config_file = ReplaceString( + source_file=rviz_config_file, replacements={"": ("/", namespace)} + ) + + start_namespaced_rviz_cmd = Node( + condition=IfCondition(use_namespace), + package="rviz2", + executable="rviz2", + name="rviz2", + namespace=namespace, + arguments=["-d", namespaced_rviz_config_file], + output="screen", + remappings=[ + ("/tf", "tf"), + ("/tf_static", "tf_static"), + ("/goal_pose", "goal_pose"), + ("/clicked_point", "clicked_point"), + ("/initialpose", "initialpose"), + ], + ) + + exit_event_handler = RegisterEventHandler( + condition=UnlessCondition(use_namespace), + event_handler=OnProcessExit( + target_action=start_rviz_cmd, on_exit=EmitEvent(event=Shutdown(reason="rviz exited")) + ), + ) + + exit_event_handler_namespaced = RegisterEventHandler( + condition=IfCondition(use_namespace), + event_handler=OnProcessExit( + target_action=start_namespaced_rviz_cmd, + on_exit=EmitEvent(event=Shutdown(reason="rviz exited")), + ), + ) + + # Create the launch description and populate + ld = LaunchDescription() + + # Declare the launch options + ld.add_action(declare_namespace_cmd) + ld.add_action(declare_use_namespace_cmd) + ld.add_action(declare_rviz_config_file_cmd) + + # Add any conditioned actions + ld.add_action(start_rviz_cmd) + ld.add_action(start_namespaced_rviz_cmd) + + # Add other nodes and processes we need + ld.add_action(exit_event_handler) + ld.add_action(exit_event_handler_namespaced) + + return ld diff --git a/smacc2_sm_reference_library/sm_dancebot_artgallery_ue/launch/slam_launch.py b/smacc2_sm_reference_library/sm_dancebot_artgallery_ue/launch/slam_launch.py new file mode 100644 index 000000000..b26de6624 --- /dev/null +++ b/smacc2_sm_reference_library/sm_dancebot_artgallery_ue/launch/slam_launch.py @@ -0,0 +1,128 @@ +# Copyright (c) 2020 Samsung Research Russia +# +# Licensed under the Apache License, Version 2.0 (the "License"); +# you may not use this file except in compliance with the License. +# You may obtain a copy of the License at +# +# http://www.apache.org/licenses/LICENSE-2.0 +# +# Unless required by applicable law or agreed to in writing, software +# distributed under the License is distributed on an "AS IS" BASIS, +# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +# See the License for the specific language governing permissions and +# limitations under the License. + +import os + +from ament_index_python.packages import get_package_share_directory + +from launch import LaunchDescription +from launch.actions import DeclareLaunchArgument, IncludeLaunchDescription +from launch.launch_description_sources import PythonLaunchDescriptionSource +from launch.substitutions import LaunchConfiguration + +# from launch_ros.actions import Node +# from nav2_common.launch import RewrittenYaml + + +def generate_launch_description(): + # Input parameters declaration + # namespace = LaunchConfiguration("namespace") + # params_file = LaunchConfiguration("params_file") + use_sim_time = LaunchConfiguration("use_sim_time") + # autostart = LaunchConfiguration("autostart") + sm_dance_bot_dir = get_package_share_directory("sm_dancebot_artgallery_ue") + + # Variables + # lifecycle_nodes = ["slam_toolbox"] + + # Getting directories and launch-files + # bringup_dir = get_package_share_directory("nav2_bringup") + # slam_toolbox_dir = get_package_share_directory("slam_toolbox") + # slam_launch_file = os.path.join(sm_dance_bot_dir, 'launch', 'online_sync_launch.py') + slam_launch_file = os.path.join(sm_dance_bot_dir, "launch", "online_sync_launch.py") + + # Create our own temporary YAML files that include substitutions + # param_substitutions = {"use_sim_time": use_sim_time} + + # configured_params = RewrittenYaml( + # source_file=params_file, + # root_key=namespace, + # param_rewrites=param_substitutions, + # convert_types=True, + # ) + + # Declare the launch arguments + declare_namespace_cmd = DeclareLaunchArgument( + "namespace", default_value="", description="Top-level namespace" + ) + + # declare_params_file_cmd = DeclareLaunchArgument( + # 'params_file', + # default_value=os.path.join(bringup_dir, 'params', 'nav2_params.yaml'), + # description='Full path to the ROS2 parameters file to use for all launched nodes') + + declare_use_sim_time_cmd = DeclareLaunchArgument( + "use_sim_time", default_value="True", description="Use simulation (Gazebo) clock if true" + ) + + declare_autostart_cmd = DeclareLaunchArgument( + "autostart", default_value="True", description="Automatically startup the nav2 stack" + ) + + # Nodes launching commands + + # start_map_saver_server_cmd = Node( + # package='nav2_map_server', + # executable='map_saver_server', + # output='screen', + # parameters=[configured_params]) + + # xtermprefix = "xterm -xrm 'XTerm*scrollBar: true' -xrm 'xterm*rightScrollBar: true' " \ + # "-hold -geometry 1000x600 -sl 10000 -e" + + # start_lifecycle_manager_cmd = Node( + # package="nav2_lifecycle_manager", + # executable="lifecycle_manager", + # name="lifecycle_manager_slam", + # output="screen", + # parameters=[ + # {"use_sim_time": use_sim_time}, + # {"autostart": autostart}, + # {"node_names": lifecycle_nodes}, + # ], + # prefix=xtermprefix, + # ) + + # If the provided param file doesn't have slam_toolbox params, we must remove the 'params_file' + # LaunchConfiguration, or it will be passed automatically to slam_toolbox and will not load + # the default file + + start_slam_toolbox_cmd = IncludeLaunchDescription( + PythonLaunchDescriptionSource(slam_launch_file), + launch_arguments={"use_sim_time": use_sim_time}.items(), + ) + + # Pop (or load) previous LaunchConfiguration, resetting the state of params_file + # pop_launch_config = PopLaunchConfigurations( + # condition=UnlessCondition(has_slam_toolbox_params)) + + ld = LaunchDescription() + + # Declare the launch options + ld.add_action(declare_namespace_cmd) + # ld.add_action(declare_params_file_cmd) + ld.add_action(declare_use_sim_time_cmd) + ld.add_action(declare_autostart_cmd) + + # Running Map Saver Server + # ld.add_action(start_map_saver_server_cmd) + # ld.add_action(start_lifecycle_manager_cmd) + + # Running SLAM Toolbox + # ld.add_action(push_launch_config) + # ld.add_action(remove_params_file) + ld.add_action(start_slam_toolbox_cmd) + # ld.add_action(pop_launch_config) + + return ld diff --git a/smacc2_sm_reference_library/sm_dancebot_artgallery_ue/launch/sm_dancebot_artgallery_ue_launch.py b/smacc2_sm_reference_library/sm_dancebot_artgallery_ue/launch/sm_dancebot_artgallery_ue_launch.py new file mode 100644 index 000000000..9b32bec05 --- /dev/null +++ b/smacc2_sm_reference_library/sm_dancebot_artgallery_ue/launch/sm_dancebot_artgallery_ue_launch.py @@ -0,0 +1,248 @@ +# Copyright (c) 2018 Intel Corporation +# +# Licensed under the Apache License, Version 2.0 (the "License"); +# you may not use this file except in compliance with the License. +# You may obtain a copy of the License at +# +# http://www.apache.org/licenses/LICENSE-2.0 +# +# Unless required by applicable law or agreed to in writing, software +# distributed under the License is distributed on an "AS IS" BASIS, +# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +# See the License for the specific language governing permissions and +# limitations under the License. + +"""This is all-in-one launch script intended for use by nav2 developers.""" + +import os + +from ament_index_python.packages import get_package_share_directory + +from launch import LaunchDescription +from launch.actions import DeclareLaunchArgument, IncludeLaunchDescription +from launch.conditions import IfCondition +from launch.launch_description_sources import PythonLaunchDescriptionSource +from launch.substitutions import LaunchConfiguration +from launch_ros.actions import Node + + +def generate_launch_description(): + + xtermprefix = "xterm -xrm 'XTerm*scrollBar: true' -xrm 'xterm*rightScrollBar: true' -hold -geometry 1000x600 -sl 10000 -e" + + # Get the launch directory + sm_dance_bot_dir = get_package_share_directory("sm_dancebot_artgallery_ue") + sm_dance_bot_launch_dir = os.path.join(sm_dance_bot_dir, "launch") + + # Create the launch configuration variables + slam = LaunchConfiguration("slam") + namespace = LaunchConfiguration("namespace") + use_namespace = LaunchConfiguration("use_namespace") + map_yaml_file = LaunchConfiguration("map") + use_sim_time = LaunchConfiguration("use_sim_time") + + params_file = LaunchConfiguration("params_file") + default_nav_to_pose_bt_xml = LaunchConfiguration("default_nav_to_pose_bt_xml") + autostart = LaunchConfiguration("autostart") + + # Launch configuration variables specific to simulation + rviz_config_file = LaunchConfiguration("rviz_config_file") + + use_robot_state_pub = LaunchConfiguration("use_robot_state_pub") + use_rviz = LaunchConfiguration("use_rviz") + + urdf = os.path.join(sm_dance_bot_dir, "urdf", "turtlebot3_waffle.urdf") + + # Map fully qualified names to relative ones so the node's namespace can be prepended. + # In case of the transforms (tf), currently, there doesn't seem to be a better alternative + # https://github.com/ros/geometry2/issues/32 + # https://github.com/ros/robot_state_publisher/pull/30 + # TODO(orduno) Substitute with `PushNodeRemapping` + # https://github.com/ros2/launch_ros/issues/56 + remappings = [("/tf", "tf"), ("/tf_static", "tf_static")] + + # Declare the launch arguments + declare_namespace_cmd = DeclareLaunchArgument( + "namespace", default_value="", description="Top-level namespace" + ) + + declare_use_namespace_cmd = DeclareLaunchArgument( + "use_namespace", + default_value="false", + description="Whether to apply a namespace to the navigation stack", + ) + + declare_show_gz_lidar = DeclareLaunchArgument( + "show_gz_lidar", + default_value="true", + description="Whether to apply a namespace to the navigation stack", + ) + + declare_slam_cmd = DeclareLaunchArgument( + "slam", default_value="False", description="Whether run a SLAM" + ) + + declare_use_sim_time_cmd = DeclareLaunchArgument( + "use_sim_time", default_value="true", description="Use simulation clock if true" + ) + + declare_gazebo_headless_cmd = DeclareLaunchArgument( + "headless", + default_value="false", + description="Use headless Gazebo if true", + ) + + declare_params_file_cmd = DeclareLaunchArgument( + "params_file", + default_value=os.path.join(sm_dance_bot_dir, "params", "nav2z_client", "nav2_params.yaml"), + description="Full path to the ROS2 parameters file to use for all launched nodes", + ) + + declare_bt_xml_cmd = DeclareLaunchArgument( + "default_nav_to_pose_bt_xml", + default_value=os.path.join( + sm_dance_bot_dir, "params", "nav2z_client", "navigation_tree.xml" + ), + description="Full path to the behavior tree xml file to use", + ) + + declare_autostart_cmd = DeclareLaunchArgument( + "autostart", default_value="true", description="Automatically startup the nav2 stack" + ) + + declare_rviz_config_file_cmd = DeclareLaunchArgument( + "rviz_config_file", + default_value=os.path.join(sm_dance_bot_dir, "rviz", "nav2_default_view.rviz"), + description="Full path to the RVIZ config file to use", + ) + + declare_use_robot_state_pub_cmd = DeclareLaunchArgument( + "use_robot_state_pub", + default_value="True", + description="Whether to start the robot state publisher", + ) + + declare_use_rviz_cmd = DeclareLaunchArgument( + "use_rviz", default_value="True", description="Whether to start RVIZ" + ) + + declare_map_yaml_cmd = DeclareLaunchArgument( + "map", + default_value=os.path.join(sm_dance_bot_dir, "maps", "turtlebot3_world.yaml"), + description="Full path to map file to load", + ) + + start_robot_state_publisher_cmd = Node( + condition=IfCondition(use_robot_state_pub), + package="robot_state_publisher", + executable="robot_state_publisher", + name="robot_state_publisher", + namespace=namespace, + output="screen", + parameters=[{"use_sim_time": use_sim_time}], + remappings=remappings, + arguments=[urdf], + prefix=xtermprefix, + ) + + static_transform_publisher = Node( + package="sm_dancebot_artgallery_ue", + executable="transform_publisher.py", + name="static_transform_publisherxx", + output="screen", + # arguments=["-0.064", "0", "0.122", "0", "0", "0", "base_scan", "base_link"], + prefix=xtermprefix, + parameters=[{"use_sim_time": use_sim_time}], + ) + + # static_transform_publisher_2 = Node( + # package="tf2_ros", + # executable="static_transform_publisher", + # name="static_transform_publisher", + # output="screen", + # arguments=["0", "0", "0", "0", "0", "0", "base_link", "base_footprint"], + # prefix=xtermprefix, + # ) + + rviz_cmd = IncludeLaunchDescription( + PythonLaunchDescriptionSource(os.path.join(sm_dance_bot_launch_dir, "rviz_launch.py")), + condition=IfCondition(use_rviz), + launch_arguments={ + "namespace": "", + "use_namespace": "False", + "rviz_config": rviz_config_file, + }.items(), + ) + + bringup_cmd = IncludeLaunchDescription( + PythonLaunchDescriptionSource(os.path.join(sm_dance_bot_launch_dir, "bringup_launch.py")), + launch_arguments={ + "namespace": namespace, + "use_namespace": use_namespace, + "autostart": autostart, + "params_file": params_file, + "slam": slam, + "map": map_yaml_file, + "use_sim_time": use_sim_time, + "default_nav_to_pose_bt_xml": default_nav_to_pose_bt_xml, + }.items(), + ) + + sm_dance_bot_node = Node( + package="sm_dancebot_artgallery_ue", + executable="sm_dancebot_artgallery_ue_node", + name="SmDanceBotArtGalleryUE", + output="screen", + prefix=xtermprefix + " gdb -ex run --args", + parameters=[ + os.path.join( + get_package_share_directory("sm_dancebot_artgallery_ue"), + "params/sm_dancebot_artgallery_ue_config.yaml", + ) + ], + remappings=[ + # ("/odom", "/odometry/filtered"), + # ("/sm_dance_bot_2/odom_tracker/odom_tracker_path", "/odom_tracker_path"), + # ("/sm_dance_bot_2/odom_tracker/odom_tracker_stacked_path", "/odom_tracker_path_stacked") + ], + arguments=["--ros-args", "--log-level", "INFO"], + ) + + gt_transform_publisher = Node( + package="sm_dancebot_artgallery_ue", + executable="ue_navigation_frames_ground_truth_adapter.py", + output="screen", + prefix=xtermprefix, + parameters=[{"use_sim_time": use_sim_time}], + ) + + # Create the launch description and populate + ld = LaunchDescription() + + # Declare the launch options + ld.add_action(declare_namespace_cmd) + ld.add_action(declare_use_namespace_cmd) + ld.add_action(declare_slam_cmd) + ld.add_action(declare_map_yaml_cmd) + ld.add_action(declare_use_sim_time_cmd) + ld.add_action(declare_gazebo_headless_cmd) + ld.add_action(declare_params_file_cmd) + ld.add_action(declare_bt_xml_cmd) + ld.add_action(declare_autostart_cmd) + ld.add_action(declare_map_yaml_cmd) + ld.add_action(declare_show_gz_lidar) + + ld.add_action(declare_rviz_config_file_cmd) + ld.add_action(declare_use_robot_state_pub_cmd) + ld.add_action(declare_use_rviz_cmd) + + ld.add_action(sm_dance_bot_node) + ld.add_action(static_transform_publisher) + ld.add_action(gt_transform_publisher) + + # # Add the actions to launch all of the navigation nodes + ld.add_action(start_robot_state_publisher_cmd) + ld.add_action(rviz_cmd) + ld.add_action(bringup_cmd) + + return ld diff --git a/smacc2_sm_reference_library/sm_dancebot_artgallery_ue/package.xml b/smacc2_sm_reference_library/sm_dancebot_artgallery_ue/package.xml new file mode 100644 index 000000000..4189699a2 --- /dev/null +++ b/smacc2_sm_reference_library/sm_dancebot_artgallery_ue/package.xml @@ -0,0 +1,48 @@ + + + + sm_dancebot_artgallery_ue + 2.2.2 + The dance_bot package + Pablo Inigo Blasco + + Apache-2.0 + + ament_cmake + + nav2z_client + multirole_sensor_client + navigation2 + nav2_bringup + + robot_state_publisher + ros_publisher_client + ros_timer_client + slam_toolbox + smacc2 + std_msgs + sr_all_events_go + sr_event_countdown + sr_conditional + visualization_msgs + tf2 + ue_msgs + + backward_global_planner + backward_local_planner + forward_global_planner + forward_local_planner + nav2z_planners_common + pure_spinning_local_planner + undo_path_global_planner + + rosidl_default_generators + action_msgs + rosidl_interface_packages + rosidl_default_runtime + + + ament_cmake + + + diff --git a/smacc2_sm_reference_library/sm_dancebot_artgallery_ue/params/mapper_params_online_sync.yaml b/smacc2_sm_reference_library/sm_dancebot_artgallery_ue/params/mapper_params_online_sync.yaml new file mode 100644 index 000000000..9b56c9f21 --- /dev/null +++ b/smacc2_sm_reference_library/sm_dancebot_artgallery_ue/params/mapper_params_online_sync.yaml @@ -0,0 +1,73 @@ +slam_toolbox: + ros__parameters: + + # Plugin params + solver_plugin: solver_plugins::CeresSolver + ceres_linear_solver: SPARSE_NORMAL_CHOLESKY + ceres_preconditioner: SCHUR_JACOBI + ceres_trust_strategy: LEVENBERG_MARQUARDT + ceres_dogleg_type: TRADITIONAL_DOGLEG + ceres_loss_function: None + + # ROS Parameters + odom_frame: odom + map_frame: map + base_frame: base_footprint + scan_topic: /scan + mode: mapping #localization + + # if you'd like to immediately start continuing a map at a given pose + # or at the dock, but they are mutually exclusive, if pose is given + # will use pose + #map_file_name: test_steve + #map_start_pose: [0.0, 0.0, 0.0] + #map_start_at_dock: true + + debug_logging: true + throttle_scans: 1 + transform_publish_period: 0.02 #if 0 never publishes odometry + map_update_interval: 0.05 + resolution: 0.04 + max_laser_range: 16.0 #for rastering images + minimum_time_interval: 0.3 + transform_timeout: 1.0 + tf_buffer_duration: 60. + stack_size_to_use: 40000000 #// program needs a larger stack size to serialize large maps + enable_interactive_mode: true + + # General Parameters + use_scan_matching: true + use_scan_barycenter: false + minimum_travel_distance: 0.1 + minimum_travel_heading: 0.1 + scan_buffer_size: 10 + scan_buffer_maximum_scan_distance: 10.0 + link_match_minimum_response_fine: 0.1 + link_scan_maximum_distance: 1.5 + loop_search_maximum_distance: 3.0 + do_loop_closing: true + loop_match_minimum_chain_size: 10 + loop_match_maximum_variance_coarse: 3.0 + loop_match_minimum_response_coarse: 0.35 + loop_match_minimum_response_fine: 0.45 + + # Correlation Parameters - Correlation Parameters + correlation_search_space_dimension: 0.5 + correlation_search_space_resolution: 0.01 + correlation_search_space_smear_deviation: 0.1 + + # Correlation Parameters - Loop Closure Parameters + loop_search_space_dimension: 8.0 + loop_search_space_resolution: 0.05 + loop_search_space_smear_deviation: 0.03 + + # Scan Matcher Parameters + distance_variance_penalty: 0.5 + angle_variance_penalty: 1.0 + + fine_search_angle_offset: 0.00349 + coarse_search_angle_offset: 0.349 + coarse_angle_resolution: 0.0349 + minimum_angle_penalty: 0.9 + minimum_distance_penalty: 0.5 + use_response_expansion: true diff --git a/smacc2_sm_reference_library/sm_dancebot_artgallery_ue/params/nav2z_client/nav2_params.yaml b/smacc2_sm_reference_library/sm_dancebot_artgallery_ue/params/nav2z_client/nav2_params.yaml new file mode 100644 index 000000000..21f3cf2bf --- /dev/null +++ b/smacc2_sm_reference_library/sm_dancebot_artgallery_ue/params/nav2z_client/nav2_params.yaml @@ -0,0 +1,373 @@ +amcl: + ros__parameters: + use_sim_time: True + alpha1: 0.2 + alpha2: 0.2 + alpha3: 0.2 + alpha4: 0.2 + alpha5: 0.2 + base_frame_id: "base_footprint" + beam_skip_distance: 0.5 + beam_skip_error_threshold: 0.9 + beam_skip_threshold: 0.3 + do_beamskip: false + global_frame_id: "map" + lambda_short: 0.1 + laser_likelihood_max_dist: 2.0 + laser_max_range: 100.0 + laser_min_range: -1.0 + laser_model_type: "likelihood_field" + max_beams: 60 + max_particles: 2000 + min_particles: 500 + odom_frame_id: "odom" + pf_err: 0.05 + pf_z: 0.99 + recovery_alpha_fast: 0.0 + recovery_alpha_slow: 0.0 + resample_interval: 1 + robot_model_type: "differential" + save_pose_rate: 0.5 + sigma_hit: 0.2 + tf_broadcast: true + transform_tolerance: 1.0 + update_min_a: 0.2 + update_min_d: 0.25 + z_hit: 0.5 + z_max: 0.05 + z_rand: 0.5 + z_short: 0.05 + scan_topic: scan + +amcl_map_client: + ros__parameters: + use_sim_time: True + +amcl_rclcpp_node: + ros__parameters: + use_sim_time: True + +bt_navigator: + ros__parameters: + use_sim_time: True + global_frame: map + robot_base_frame: base_footprint + odom_topic: /odom + enable_groot_monitoring: True + groot_zmq_publisher_port: 1666 + groot_zmq_server_port: 1667 + default_nav_to_pose_bt_xml: RUNTIMEFILL + plugin_lib_names: + - nav2_compute_path_to_pose_action_bt_node + - nav2_compute_path_through_poses_action_bt_node + - nav2_follow_path_action_bt_node + - nav2_back_up_action_bt_node + - nav2_spin_action_bt_node + - nav2_wait_action_bt_node + - nav2_clear_costmap_service_bt_node + - nav2_is_stuck_condition_bt_node + - nav2_goal_reached_condition_bt_node + - nav2_goal_updated_condition_bt_node + - nav2_initial_pose_received_condition_bt_node + - nav2_reinitialize_global_localization_service_bt_node + - nav2_rate_controller_bt_node + - nav2_distance_controller_bt_node + - nav2_speed_controller_bt_node + - nav2_truncate_path_action_bt_node + - nav2_goal_updater_node_bt_node + - nav2_recovery_node_bt_node + - nav2_pipeline_sequence_bt_node + - nav2_round_robin_node_bt_node + - nav2_transform_available_condition_bt_node + - nav2_time_expired_condition_bt_node + - nav2_distance_traveled_condition_bt_node + - nav2_single_trigger_bt_node + - nav2_is_battery_low_condition_bt_node + - nav2_navigate_through_poses_action_bt_node + - nav2_navigate_to_pose_action_bt_node + - nav2_remove_passed_goals_action_bt_node + - nav2_planner_selector_bt_node + - nav2_controller_selector_bt_node + - nav2_goal_checker_selector_bt_node + + +bt_navigator_rclcpp_node: + ros__parameters: + use_sim_time: True + +controller_server: + ros__parameters: + use_sim_time: True + controller_frequency: 20.0 + min_x_velocity_threshold: 0.001 + min_y_velocity_threshold: 0.5 + min_theta_velocity_threshold: 0.001 + progress_checker_plugin: "progress_checker" + goal_checker_plugins: ["goal_checker", "forward_goal_checker", "backward_goal_checker", "absolute_rotate_goal_checker", "undo_path_backwards_goal_checker"] + controller_plugins: ["FollowPath","BackwardLocalPlanner", "ForwardLocalPlanner", "PureSpinningLocalPlanner"] + + # Progress checker parameters + progress_checker: + plugin: "nav2_controller::SimpleProgressChecker" + required_movement_radius: 0.001 + movement_time_allowance: 3000.0 + + # Goal checker parameters + goal_checker: + plugin: "nav2_controller::SimpleGoalChecker" + xy_goal_tolerance: 0.257 + yaw_goal_tolerance: 0.1 # 1.65 # almost free orientation + stateful: True + backward_goal_checker: + plugin: "nav2_controller::SimpleGoalChecker" + xy_goal_tolerance: 0.25 #0.9 + yaw_goal_tolerance: 0.05 #0.05 + stateful: True + forward_goal_checker: + plugin: "nav2_controller::SimpleGoalChecker" + xy_goal_tolerance: 0.08 #0.09 + yaw_goal_tolerance: 0.1 #0.05 + stateful: True + absolute_rotate_goal_checker: + plugin: "nav2_controller::SimpleGoalChecker" + xy_goal_tolerance: 0.15 + yaw_goal_tolerance: 0.015 #0.005 + stateful: True + undo_path_backwards_goal_checker: + plugin: "nav2_controller::SimpleGoalChecker" + xy_goal_tolerance: 0.02 # 4 cm + yaw_goal_tolerance: 0.34 + stateful: True + + # DWB parameters + FollowPath: + plugin: "dwb_core::DWBLocalPlanner" + debug_trajectory_details: True + min_vel_x: 0.0 + min_vel_y: 0.0 + max_vel_x: 0.4 + max_vel_y: 0.0 + max_vel_theta: 1.5 + min_speed_xy: 0.0 + max_speed_xy: 0.4 + min_speed_theta: 0.0 + # Add high threshold velocity for turtlebot 3 issue. + # https://github.com/ROBOTIS-GIT/turtlebot3_simulations/issues/75 + acc_lim_x: 2.5 + acc_lim_y: 0.0 + acc_lim_theta: 3.2 + decel_lim_x: -2.5 + decel_lim_y: 0.0 + decel_lim_theta: -3.2 + vx_samples: 20 + vy_samples: 5 + vtheta_samples: 20 + sim_time: 1.7 + linear_granularity: 0.05 + angular_granularity: 0.025 + transform_tolerance: 0.2 + xy_goal_tolerance: 0.157 + yaw_goal_tolerance: 0.15 + trans_stopped_velocity: 0.25 + short_circuit_trajectory_evaluation: True + stateful: True + critics: ["RotateToGoal", "Oscillation", "BaseObstacle", "GoalAlign", "PathAlign", "PathDist", "GoalDist"] + BaseObstacle.scale: 0.02 + PathAlign.scale: 32.0 + PathAlign.forward_point_distance: 0.1 + GoalAlign.scale: 24.0 + GoalAlign.forward_point_distance: 0.1 + PathDist.scale: 32.0 + GoalDist.scale: 24.0 + RotateToGoal.scale: 32.0 + RotateToGoal.slowing_factor: 5.0 + RotateToGoal.lookahead_time: -1.0 + BackwardLocalPlanner: + plugin: "backward_local_planner::BackwardLocalPlanner" + transform_tolerance: 0.5 + k_rho: -1.4 # linear distance to the goal gain, default = -1.0 (related with carrot distance) + k_alpha: -16.4 #-0.1 + k_betta: -0.01 # -1.0 + carrot_distance: 0.8 #meters default 0.5 + carrot_angular_distance: 0.3 # no constraint, max 3.1416 + pure_spinning_straight_line_mode: true + max_linear_x_speed: 0.25 + max_angular_z_speed: 0.4 + ForwardLocalPlanner: + plugin: "cl_nav2z::forward_local_planner::ForwardLocalPlanner" + transform_tolerance: 0.5 + k_rho: 2.5 + k_alpha: -2.4 + k_betta: -0.1 + carrot_distance: 0.8 #meters + carrot_angular_distance: 0.5 # no constraint, max 3.1416 + max_linear_x_speed: 0.3 + max_angular_z_speed: 0.4 + PureSpinningLocalPlanner: + plugin: "pure_spinning_local_planner::PureSpinningLocalPlanner" + transform_tolerance: 0.5 + k_betta: 25.0 + max_angular_z_speed: 0.3 + use_shortest_angular_distance: true + + +controller_server_rclcpp_node: + ros__parameters: + use_sim_time: True + +local_costmap: + local_costmap: + ros__parameters: + update_frequency: 5.0 + publish_frequency: 2.0 + global_frame: odom + robot_base_frame: base_footprint #base_link + use_sim_time: True + rolling_window: true + transform_tolerance: 15.0 + width: 3 + height: 3 + resolution: 0.05 + robot_radius: 0.22 + plugins: ["voxel_layer", "inflation_layer"] + inflation_layer: + plugin: "nav2_costmap_2d::InflationLayer" + cost_scaling_factor: 3.0 + inflation_radius: 0.55 + voxel_layer: + plugin: "nav2_costmap_2d::VoxelLayer" + enabled: True + publish_voxel_map: True + origin_z: 0.0 + z_resolution: 0.05 + z_voxels: 16 + max_obstacle_height: 2.0 + mark_threshold: 0 + observation_sources: scan + scan: + topic: /scan + max_obstacle_height: 2.0 + clearing: True + marking: True + data_type: "LaserScan" + raytrace_max_range: 15.0 + raytrace_min_range: 0.0 + obstacle_max_range: 15.5 + obstacle_min_range: 0.0 + static_layer: + map_subscribe_transient_local: True + always_send_full_costmap: True + local_costmap_client: + ros__parameters: + use_sim_time: True + local_costmap_rclcpp_node: + ros__parameters: + use_sim_time: True + +global_costmap: + global_costmap: + ros__parameters: + update_frequency: 2.0 + publish_frequency: 1.0 + transform_tolerance: 15.0 + width: 60 + height: 60 + global_frame: map + robot_base_frame: base_footprint + use_sim_time: True + rolling_window: true + + robot_radius: 0.22 + resolution: 0.1 + track_unknown_space: true + plugins: ["obstacle_layer", "inflation_layer"] # "static_layer" + obstacle_layer: + plugin: "nav2_costmap_2d::ObstacleLayer" + enabled: True + observation_sources: scan + scan: + topic: /scan + max_obstacle_height: 2.0 + clearing: True + marking: True + data_type: "LaserScan" + raytrace_max_range: 15.0 + raytrace_min_range: 0.0 + obstacle_max_range: 15.5 + obstacle_min_range: 0.0 + static_layer: + plugin: "nav2_costmap_2d::StaticLayer" + map_subscribe_transient_local: True + inflation_layer: + plugin: "nav2_costmap_2d::InflationLayer" + cost_scaling_factor: 1.0 + inflation_radius: 0.55 + always_send_full_costmap: True + global_costmap_client: + ros__parameters: + use_sim_time: True + global_costmap_rclcpp_node: + ros__parameters: + use_sim_time: True + +map_server: + ros__parameters: + use_sim_time: True + yaml_filename: "turtlebot3_world.yaml" + +map_saver: + ros__parameters: + use_sim_time: True + save_map_timeout: 5000 + free_thresh_default: 0.25 + occupied_thresh_default: 0.65 + +planner_server: + ros__parameters: + expected_planner_frequency: 20.0 + use_sim_time: True + planner_plugins: ["GridBased", "ForwardGlobalPlanner", "BackwardGlobalPlanner", "UndoPathGlobalPlanner"] + GridBased: + plugin: "nav2_navfn_planner/NavfnPlanner" + tolerance: 10.0 #meters + use_astar: false + allow_unknown: true + ForwardGlobalPlanner: + plugin: "forward_global_planner::ForwardGlobalPlanner" + transform_tolerance: 0.5 + BackwardGlobalPlanner: + plugin: "backward_global_planner::BackwardGlobalPlanner" + transform_tolerance: 0.5 + UndoPathGlobalPlanner: + plugin: "undo_path_global_planner::UndoPathGlobalPlanner" + transform_tolerance: 0.5 + +planner_server_rclcpp_node: + ros__parameters: + use_sim_time: True + +recoveries_server: + ros__parameters: + costmap_topic: local_costmap/costmap_raw + footprint_topic: local_costmap/published_footprint + cycle_frequency: 10.0 + recovery_plugins: ["spin", "backup", "wait"] + #recovery_plugins: ["spin"] + spin: + plugin: "nav2_recoveries/Spin" + backup: + plugin: "nav2_recoveries/BackUp" + wait: + plugin: "nav2_recoveries/Wait" + global_frame: odom + robot_base_frame: base_footprint + transform_timeout: 0.1 + use_sim_time: true + simulate_ahead_time: 2.0 + max_rotational_vel: 1.0 + min_rotational_vel: 0.4 + rotational_acc_lim: 3.2 + +robot_state_publisher: + ros__parameters: + use_sim_time: True diff --git a/smacc2_sm_reference_library/sm_dancebot_artgallery_ue/params/nav2z_client/navigation_tree.xml b/smacc2_sm_reference_library/sm_dancebot_artgallery_ue/params/nav2z_client/navigation_tree.xml new file mode 100644 index 000000000..ebd9f201f --- /dev/null +++ b/smacc2_sm_reference_library/sm_dancebot_artgallery_ue/params/nav2z_client/navigation_tree.xml @@ -0,0 +1,33 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/smacc2_sm_reference_library/sm_dancebot_artgallery_ue/params/nav2z_client/waypoints_art_gallery.yaml b/smacc2_sm_reference_library/sm_dancebot_artgallery_ue/params/nav2z_client/waypoints_art_gallery.yaml new file mode 100644 index 000000000..1d1555c04 --- /dev/null +++ b/smacc2_sm_reference_library/sm_dancebot_artgallery_ue/params/nav2z_client/waypoints_art_gallery.yaml @@ -0,0 +1,201 @@ +waypoints: + # # POINT R1 + # - position: + # x: 18.10907715 + # y: 52.59328613 + # z: 0.0 + # orientation: + # x: 0.0 + # y: -0.0 + # z: 0.0 + # w: 1.0 + # # POINT R2 + # - position: + # x: 2.850 + # y: 52.0 + # z: 0.0 + # orientation: + # x: 0.0 + # y: -0.0 + # z: 0.0 + # w: 1.0 + # # POINT R3 + # - position: + # x: 2.85 + # y: 42.0 + # z: 0.0 + # orientation: + # x: 0.0 + # y: -0.0 + # z: 0.0 + # w: 1.0 + # # POINT R4 + # - position: + # x: 7.85 + # y: 36.2 + # z: 0.0 + # orientation: + # x: 0.0 + # y: -0.0 + # z: 0.0 + # w: 1.0 + # # POINT R5 + # - position: + # x: 7.85 + # y: 24.0 + # z: 0.0 + # orientation: + # x: 0.0 + # y: -0.0 + # z: 0.0 + # w: 1.0 + # POINT R7 + # - position: + # x: 1.40 + # y: 26.0 + # z: 0.0 + # orientation: + # x: 0.0 + # y: -0.0 + # z: 0.0 + # w: 1.0 + # POINT R8 + - position: + x: -3.70 + y: 26.70 + z: 0.0 + orientation: + x: 0.0 + y: -0.0 + z: 0.0 + w: 1.0 + # POINT R10 + - position: + x: -8.0 + y: 43.0 + z: 0.0 + orientation: + x: 0.0 + y: -0.0 + z: 0.0 + w: 1.0 + # POINT R11 + - position: + x: -29.7 + y: 43.0 + z: 0.0 + orientation: + x: 0.0 + y: -0.0 + z: 0.0 + w: 1.0 + # POINT R12 + - position: + x: -29.7 + y: 65.20 + z: 0.0 + orientation: + x: 0.0 + y: -0.0 + z: 0.0 + w: 1.0 + # POINT R13 + - position: + x: -35.15 + y: 65.2 + z: 0.0 + orientation: + x: 0.0 + y: -0.0 + z: 0.0 + w: 1.0 + # POINT R14 + - position: + x: -35.15 + y: 70.50 + z: 0.0 + orientation: + x: 0.0 + y: -0.0 + z: 0.0 + w: 1.0 + # POINT R15 + - position: + x: -22.70 + y: 70.50 + z: 0.0 + orientation: + x: 0.0 + y: -0.0 + z: 0.0 + w: 1.0 + # POINT R16 + - position: + x: -20.80 + y: 63.90 + z: 0.0 + orientation: + x: 0.0 + y: -0.0 + z: 0.0 + w: 1.0 + # POINT R17 + - position: + x: 1.95 + y: 63.90 + z: 0.0 + orientation: + x: 0.0 + y: -0.0 + z: 0.0 + w: 1.0 + # POINT R18 + - position: + x: 1.95.80 + y: 71.15 + z: 0.0 + orientation: + x: 0.0 + y: -0.0 + z: 0.0 + w: 1.0 + # POINT R19 + - position: + x: -4.25 + y: 71.15 + z: 0.0 + orientation: + x: 0.0 + y: -0.0 + z: 0.0 + w: 1.0 + # POINT R20 + - position: + x: -4.25 + y: 62.00 + z: 0.0 + orientation: + x: 0.0 + y: -0.0 + z: 0.0 + w: 1.0 + # POINT R21 + - position: + x: -13.80 + y: 62.00 + z: 0.0 + orientation: + x: 0.0 + y: -0.0 + z: 0.0 + w: 1.0 + # POINT R22 + - position: + x: -15.50 + y: 51.0 + z: 0.0 + orientation: + x: 0.0 + y: -0.0 + z: 0.0 + w: 1.0 diff --git a/smacc2_sm_reference_library/sm_dancebot_artgallery_ue/params/sm_dancebot_artgallery_ue_config.yaml b/smacc2_sm_reference_library/sm_dancebot_artgallery_ue/params/sm_dancebot_artgallery_ue_config.yaml new file mode 100644 index 000000000..225135196 --- /dev/null +++ b/smacc2_sm_reference_library/sm_dancebot_artgallery_ue/params/sm_dancebot_artgallery_ue_config.yaml @@ -0,0 +1,10 @@ +SmDanceBotArtGalleryUE: + ros__parameters: + use_sim_time: true + signal_detector_loop_freq: 20.0 + clear_angular_distance_threshold: 0.1 # 0.05 + clear_point_distance_threshold: 0.4 #0.1 + record_angular_distance_threshold: 0.005 + record_point_distance_threshold: 0.1 + + waypoints_art_gallery_1: $(pkg_share)/params/nav2z_client/waypoints_art_gallery.yaml diff --git a/smacc2_sm_reference_library/sm_dancebot_artgallery_ue/rviz/nav2_default_view.rviz b/smacc2_sm_reference_library/sm_dancebot_artgallery_ue/rviz/nav2_default_view.rviz new file mode 100644 index 000000000..b4582e7a6 --- /dev/null +++ b/smacc2_sm_reference_library/sm_dancebot_artgallery_ue/rviz/nav2_default_view.rviz @@ -0,0 +1,808 @@ +Panels: + - Class: rviz_common/Displays + Help Height: 0 + Name: Displays + Property Tree Widget: + Expanded: + - /Global Options1 + - /Grid1 + - /TF1 + - /TF1/Frames1 + - /TF1/Tree1 + - /Global Planner1 + - /Global Planner1/Global Costmap1 + - /Controller1 + - /Odometry1 + - /Odometry1/Shape1 + - /LaserScan1/Topic1 + Splitter Ratio: 0.5833333134651184 + Tree Height: 441 + - Class: rviz_common/Selection + Name: Selection + - Class: rviz_common/Tool Properties + Expanded: + - /Publish Point1 + Name: Tool Properties + Splitter Ratio: 0.5886790156364441 + - Class: rviz_common/Views + Expanded: + - /Current View1 + Name: Views + Splitter Ratio: 0.5 + - Class: nav2_rviz_plugins/Navigation 2 + Name: Navigation 2 +Visualization Manager: + Class: "" + Displays: + - Alpha: 0.5 + Cell Size: 1 + Class: rviz_default_plugins/Grid + Color: 160; 160; 164 + Enabled: true + Line Style: + Line Width: 0.029999999329447746 + Value: Lines + Name: Grid + Normal Cell Count: 0 + Offset: + X: 0 + Y: 0 + Z: 0 + Plane: XY + Plane Cell Count: 100 + Reference Frame: + Value: true + - Alpha: 1 + Class: rviz_default_plugins/RobotModel + Collision Enabled: false + Description File: "" + Description Source: Topic + Description Topic: + Depth: 5 + Durability Policy: Volatile + History Policy: Keep Last + Reliability Policy: Reliable + Value: /robot_description + Enabled: false + Links: + All Links Enabled: true + Expand Joint Details: false + Expand Link Details: false + Expand Tree: false + Link Tree Style: "" + Mass Properties: + Inertia: false + Mass: false + Name: RobotModel + TF Prefix: "" + Update Interval: 0 + Value: false + Visual Enabled: true + - Class: rviz_default_plugins/TF + Enabled: true + Frame Timeout: 15 + Frames: + All Enabled: false + base_footprint: + Value: true + base_link: + Value: true + base_scan: + Value: true + camera_depth_frame: + Value: true + camera_depth_optical_frame: + Value: true + camera_link: + Value: true + camera_rgb_frame: + Value: true + camera_rgb_optical_frame: + Value: true + caster_back_left_link: + Value: true + caster_back_right_link: + Value: true + imu_link: + Value: true + map: + Value: true + odom: + Value: true + Marker Scale: 1 + Name: TF + Show Arrows: false + Show Axes: true + Show Names: false + Tree: + map: + odom: + base_footprint: + base_link: + base_scan: + {} + camera_link: + camera_depth_frame: + camera_depth_optical_frame: + {} + camera_rgb_frame: + camera_rgb_optical_frame: + {} + caster_back_left_link: + {} + caster_back_right_link: + {} + imu_link: + {} + Update Interval: 0 + Value: true + - Alpha: 1 + Autocompute Intensity Bounds: true + Autocompute Value Bounds: + Max Value: 10 + Min Value: -10 + Value: true + Axis: Z + Channel Name: intensity + Class: rviz_default_plugins/PointCloud2 + Color: 255; 255; 255 + Color Transformer: "" + Decay Time: 0 + Enabled: true + Invert Rainbow: false + Max Color: 255; 255; 255 + Max Intensity: 4096 + Min Color: 0; 0; 0 + Min Intensity: 0 + Name: Bumper Hit + Position Transformer: "" + Selectable: true + Size (Pixels): 3 + Size (m): 0.07999999821186066 + Style: Spheres + Topic: + Depth: 5 + Durability Policy: Volatile + Filter size: 10 + History Policy: Keep Last + Reliability Policy: Best Effort + Value: /mobile_base/sensors/bumper_pointcloud + Use Fixed Frame: true + Use rainbow: true + Value: true + - Alpha: 1 + Class: rviz_default_plugins/Map + Color Scheme: map + Draw Behind: true + Enabled: true + Name: Map + Topic: + Depth: 1 + Durability Policy: Transient Local + Filter size: 10 + History Policy: Keep Last + Reliability Policy: Reliable + Value: /map + Update Topic: + Depth: 5 + Durability Policy: Volatile + History Policy: Keep Last + Reliability Policy: Reliable + Value: /map_updates + Use Timestamp: false + Value: true + - Alpha: 1 + Arrow Length: 0.019999999552965164 + Axes Length: 0.30000001192092896 + Axes Radius: 0.009999999776482582 + Class: rviz_default_plugins/PoseArray + Color: 0; 180; 0 + Enabled: true + Head Length: 0.07000000029802322 + Head Radius: 0.029999999329447746 + Name: Amcl Particle Swarm + Shaft Length: 0.23000000417232513 + Shaft Radius: 0.009999999776482582 + Shape: Arrow (Flat) + Topic: + Depth: 5 + Durability Policy: Volatile + Filter size: 10 + History Policy: Keep Last + Reliability Policy: Best Effort + Value: /particlecloud + Value: true + - Class: rviz_common/Group + Displays: + - Alpha: 0.4000000059604645 + Class: rviz_default_plugins/Map + Color Scheme: costmap + Draw Behind: false + Enabled: true + Name: Global Costmap + Topic: + Depth: 1 + Durability Policy: Transient Local + Filter size: 10 + History Policy: Keep Last + Reliability Policy: Reliable + Value: /global_costmap/costmap + Update Topic: + Depth: 5 + Durability Policy: Volatile + History Policy: Keep Last + Reliability Policy: Reliable + Value: /global_costmap/costmap_updates + Use Timestamp: false + Value: true + - Alpha: 1 + Buffer Length: 1 + Class: rviz_default_plugins/Path + Color: 255; 0; 0 + Enabled: true + Head Diameter: 0.019999999552965164 + Head Length: 0.019999999552965164 + Length: 0.30000001192092896 + Line Style: Lines + Line Width: 0.029999999329447746 + Name: Path + Offset: + X: 0 + Y: 0 + Z: 0 + Pose Color: 255; 85; 255 + Pose Style: Arrows + Radius: 0.029999999329447746 + Shaft Diameter: 0.004999999888241291 + Shaft Length: 0.019999999552965164 + Topic: + Depth: 5 + Durability Policy: Volatile + Filter size: 10 + History Policy: Keep Last + Reliability Policy: Reliable + Value: /plan + Value: true + - Alpha: 1 + Autocompute Intensity Bounds: true + Autocompute Value Bounds: + Max Value: 10 + Min Value: -10 + Value: true + Axis: Z + Channel Name: intensity + Class: rviz_default_plugins/PointCloud + Color: 125; 125; 125 + Color Transformer: FlatColor + Decay Time: 0 + Enabled: true + Invert Rainbow: false + Max Color: 255; 255; 255 + Max Intensity: 4096 + Min Color: 0; 0; 0 + Min Intensity: 0 + Name: VoxelGrid + Position Transformer: XYZ + Selectable: true + Size (Pixels): 3 + Size (m): 0.05000000074505806 + Style: Boxes + Topic: + Depth: 5 + Durability Policy: Volatile + Filter size: 10 + History Policy: Keep Last + Reliability Policy: Reliable + Value: /global_costmap/voxel_marked_cloud + Use Fixed Frame: true + Use rainbow: true + Value: true + - Alpha: 1 + Class: rviz_default_plugins/Polygon + Color: 25; 255; 0 + Enabled: false + Name: Polygon + Topic: + Depth: 5 + Durability Policy: Volatile + Filter size: 10 + History Policy: Keep Last + Reliability Policy: Reliable + Value: /global_costmap/published_footprint + Value: false + Enabled: true + Name: Global Planner + - Class: rviz_common/Group + Displays: + - Alpha: 0.4000000059604645 + Class: rviz_default_plugins/Map + Color Scheme: costmap + Draw Behind: false + Enabled: true + Name: Local Costmap + Topic: + Depth: 1 + Durability Policy: Transient Local + Filter size: 10 + History Policy: Keep Last + Reliability Policy: Reliable + Value: /local_costmap/costmap + Update Topic: + Depth: 5 + Durability Policy: Volatile + History Policy: Keep Last + Reliability Policy: Reliable + Value: /local_costmap/costmap_updates + Use Timestamp: true + Value: true + - Alpha: 1 + Buffer Length: 1 + Class: rviz_default_plugins/Path + Color: 0; 12; 255 + Enabled: true + Head Diameter: 0.30000001192092896 + Head Length: 0.20000000298023224 + Length: 0.30000001192092896 + Line Style: Lines + Line Width: 0.029999999329447746 + Name: Local Plan + Offset: + X: 0 + Y: 0 + Z: 0 + Pose Color: 255; 85; 255 + Pose Style: None + Radius: 0.029999999329447746 + Shaft Diameter: 0.10000000149011612 + Shaft Length: 0.10000000149011612 + Topic: + Depth: 5 + Durability Policy: Volatile + Filter size: 10 + History Policy: Keep Last + Reliability Policy: Reliable + Value: /local_plan + Value: true + - Class: rviz_default_plugins/MarkerArray + Enabled: false + Name: Trajectories + Namespaces: + {} + Topic: + Depth: 5 + Durability Policy: Volatile + History Policy: Keep Last + Reliability Policy: Reliable + Value: /marker + Value: false + - Alpha: 1 + Class: rviz_default_plugins/Polygon + Color: 25; 255; 0 + Enabled: true + Name: Polygon + Topic: + Depth: 5 + Durability Policy: Volatile + Filter size: 10 + History Policy: Keep Last + Reliability Policy: Reliable + Value: /local_costmap/published_footprint + Value: true + - Alpha: 1 + Autocompute Intensity Bounds: true + Autocompute Value Bounds: + Max Value: 10 + Min Value: -10 + Value: true + Axis: Z + Channel Name: intensity + Class: rviz_default_plugins/PointCloud + Color: 255; 255; 255 + Color Transformer: RGB8 + Decay Time: 0 + Enabled: true + Invert Rainbow: false + Max Color: 255; 255; 255 + Max Intensity: 4096 + Min Color: 0; 0; 0 + Min Intensity: 0 + Name: VoxelGrid + Position Transformer: XYZ + Selectable: true + Size (Pixels): 3 + Size (m): 0.009999999776482582 + Style: Flat Squares + Topic: + Depth: 5 + Durability Policy: Volatile + Filter size: 10 + History Policy: Keep Last + Reliability Policy: Reliable + Value: /local_costmap/voxel_marked_cloud + Use Fixed Frame: true + Use rainbow: true + Value: true + Enabled: true + Name: Controller + - Class: rviz_common/Group + Displays: + - Class: rviz_default_plugins/Image + Enabled: true + Max Value: 1 + Median window: 5 + Min Value: 0 + Name: RealsenseCamera + Normalize Range: true + Topic: + Depth: 5 + Durability Policy: Volatile + History Policy: Keep Last + Reliability Policy: Reliable + Value: /intel_realsense_r200_depth/image_raw + Value: true + - Alpha: 1 + Autocompute Intensity Bounds: true + Autocompute Value Bounds: + Max Value: 10 + Min Value: -10 + Value: true + Axis: Z + Channel Name: intensity + Class: rviz_default_plugins/PointCloud2 + Color: 255; 255; 255 + Color Transformer: RGB8 + Decay Time: 0 + Enabled: true + Invert Rainbow: false + Max Color: 255; 255; 255 + Max Intensity: 4096 + Min Color: 0; 0; 0 + Min Intensity: 0 + Name: RealsenseDepthImage + Position Transformer: XYZ + Selectable: true + Size (Pixels): 3 + Size (m): 0.009999999776482582 + Style: Flat Squares + Topic: + Depth: 5 + Durability Policy: Volatile + Filter size: 10 + History Policy: Keep Last + Reliability Policy: Reliable + Value: /intel_realsense_r200_depth/points + Use Fixed Frame: true + Use rainbow: true + Value: true + Enabled: false + Name: Realsense + - Alpha: 1 + Buffer Length: 1 + Class: rviz_default_plugins/Path + Color: 25; 255; 0 + Enabled: true + Head Diameter: 0.30000001192092896 + Head Length: 0.20000000298023224 + Length: 0.10000000149011612 + Line Style: Lines + Line Width: 0.029999999329447746 + Name: CpOdomTracker + Offset: + X: 0 + Y: 0 + Z: 0 + Pose Color: 255; 85; 255 + Pose Style: Axes + Radius: 0.009999999776482582 + Shaft Diameter: 0.10000000149011612 + Shaft Length: 0.10000000149011612 + Topic: + Depth: 5 + Durability Policy: Volatile + Filter size: 10 + History Policy: Keep Last + Reliability Policy: Reliable + Value: /odom_tracker_path + Value: true + - Alpha: 1 + Buffer Length: 1 + Class: rviz_default_plugins/Path + Color: 25; 255; 0 + Enabled: true + Head Diameter: 0.30000001192092896 + Head Length: 0.20000000298023224 + Length: 0.30000001192092896 + Line Style: Lines + Line Width: 0.029999999329447746 + Name: BackwardsGlobalPlan + Offset: + X: 0 + Y: 0 + Z: 0 + Pose Color: 255; 85; 255 + Pose Style: None + Radius: 0.029999999329447746 + Shaft Diameter: 0.10000000149011612 + Shaft Length: 0.10000000149011612 + Topic: + Depth: 5 + Durability Policy: Volatile + Filter size: 10 + History Policy: Keep Last + Reliability Policy: Reliable + Value: /backward_planner/global_plan + Value: true + - Angle Tolerance: 0.10000000149011612 + Class: rviz_default_plugins/Odometry + Covariance: + Orientation: + Alpha: 0.5 + Color: 255; 255; 127 + Color Style: Unique + Frame: Local + Offset: 1 + Scale: 1 + Value: true + Position: + Alpha: 0.30000001192092896 + Color: 204; 51; 204 + Scale: 1 + Value: true + Value: false + Enabled: true + Keep: 10000 + Name: Odometry + Position Tolerance: 0.10000000149011612 + Shape: + Alpha: 1 + Axes Length: 1 + Axes Radius: 0.10000000149011612 + Color: 255; 25; 0 + Head Length: 0 + Head Radius: 0 + Shaft Length: 0.03799999877810478 + Shaft Radius: 0.03999999910593033 + Value: Arrow + Topic: + Depth: 5 + Durability Policy: Volatile + Filter size: 10 + History Policy: Keep Last + Reliability Policy: Reliable + Value: /odom + Value: true + - Class: rviz_default_plugins/MarkerArray + Enabled: true + Name: WayPointsMarkers + Namespaces: + {} + Topic: + Depth: 5 + Durability Policy: Volatile + History Policy: Keep Last + Reliability Policy: Reliable + Value: /waypoints + Value: true + - Class: rviz_default_plugins/MarkerArray + Enabled: true + Name: VisualizationMarkers + Namespaces: + {} + Topic: + Depth: 5 + Durability Policy: Volatile + History Policy: Keep Last + Reliability Policy: Reliable + Value: visualization_marker_array + Value: true + - Class: rviz_default_plugins/MarkerArray + Enabled: true + Name: BackwardsGlobalPlanMarkers + Namespaces: + {} + Topic: + Depth: 5 + Durability Policy: Volatile + History Policy: Keep Last + Reliability Policy: Reliable + Value: backward_planner/markers + Value: true + - Class: rviz_default_plugins/MarkerArray + Enabled: true + Name: BackwardLocalPlannerGoalMarker + Namespaces: + {} + Topic: + Depth: 5 + Durability Policy: Volatile + History Policy: Keep Last + Reliability Policy: Reliable + Value: /backward_local_planner/goal_marker + Value: true + - Class: rviz_default_plugins/MarkerArray + Enabled: true + Name: ForwardCarrot + Namespaces: + {} + Topic: + Depth: 5 + Durability Policy: Volatile + History Policy: Keep Last + Reliability Policy: Reliable + Value: /forward_local_planner/carrot_goal_marker + Value: true + - Class: rviz_default_plugins/MarkerArray + Enabled: true + Name: UndoGlobalPlannerMarkers + Namespaces: + {} + Topic: + Depth: 5 + Durability Policy: Volatile + History Policy: Keep Last + Reliability Policy: Reliable + Value: /undo_path_planner/markers + Value: true + - Alpha: 1 + Buffer Length: 1 + Class: rviz_default_plugins/Path + Color: 25; 255; 0 + Enabled: true + Head Diameter: 0.30000001192092896 + Head Length: 0.20000000298023224 + Length: 0.30000001192092896 + Line Style: Lines + Line Width: 0.029999999329447746 + Name: UndoGlobalPlan + Offset: + X: 0 + Y: 0 + Z: 0 + Pose Color: 255; 85; 255 + Pose Style: None + Radius: 0.029999999329447746 + Shaft Diameter: 0.10000000149011612 + Shaft Length: 0.10000000149011612 + Topic: + Depth: 5 + Durability Policy: Volatile + Filter size: 10 + History Policy: Keep Last + Reliability Policy: Reliable + Value: /undo_path_planner/global_plan + Value: true + - Alpha: 1 + Buffer Length: 1 + Class: rviz_default_plugins/Path + Color: 255; 255; 0 + Enabled: true + Head Diameter: 0.05999999865889549 + Head Length: 0.03999999910593033 + Length: 0.30000001192092896 + Line Style: Billboards + Line Width: 0.0010000000474974513 + Name: BackwardLocalPlan + Offset: + X: 0 + Y: 0 + Z: 0 + Pose Color: 255; 85; 0 + Pose Style: Arrows + Radius: 0.029999999329447746 + Shaft Diameter: 0.019999999552965164 + Shaft Length: 0.009999999776482582 + Topic: + Depth: 5 + Durability Policy: Volatile + Filter size: 10 + History Policy: Keep Last + Reliability Policy: Reliable + Value: /backward_local_planner/path + Value: true + - Alpha: 1 + Autocompute Intensity Bounds: true + Autocompute Value Bounds: + Max Value: 10 + Min Value: -10 + Value: true + Axis: Z + Channel Name: intensity + Class: rviz_default_plugins/LaserScan + Color: 224; 27; 36 + Color Transformer: FlatColor + Decay Time: 0 + Enabled: true + Invert Rainbow: false + Max Color: 255; 255; 255 + Max Intensity: 1246.8863525390625 + Min Color: 0; 0; 0 + Min Intensity: 726.621337890625 + Name: LaserScan + Position Transformer: XYZ + Selectable: true + Size (Pixels): 3 + Size (m): 0.20000000298023224 + Style: Flat Squares + Topic: + Depth: 5 + Durability Policy: Volatile + Filter size: 10 + History Policy: Keep Last + Reliability Policy: Reliable + Value: /scan + Use Fixed Frame: true + Use rainbow: true + Value: true + Enabled: true + Global Options: + Background Color: 48; 48; 48 + Fixed Frame: map + Frame Rate: 30 + Name: root + Tools: + - Class: rviz_default_plugins/MoveCamera + - Class: rviz_default_plugins/Select + - Class: rviz_default_plugins/FocusCamera + - Class: rviz_default_plugins/Measure + Line color: 128; 128; 0 + - Class: rviz_default_plugins/SetInitialPose + Covariance x: 0.25 + Covariance y: 0.25 + Covariance yaw: 0.06853891909122467 + Topic: + Depth: 5 + Durability Policy: Volatile + History Policy: Keep Last + Reliability Policy: Reliable + Value: /initialpose + - Class: rviz_default_plugins/PublishPoint + Single click: true + Topic: + Depth: 5 + Durability Policy: Volatile + History Policy: Keep Last + Reliability Policy: Reliable + Value: /clicked_point + - Class: nav2_rviz_plugins/GoalTool + Transformation: + Current: + Class: rviz_default_plugins/TF + Value: true + Views: + Current: + Angle: -1.5699937343597412 + Class: rviz_default_plugins/TopDownOrtho + Enable Stereo Rendering: + Stereo Eye Separation: 0.05999999865889549 + Stereo Focal Distance: 1 + Swap Stereo Eyes: false + Value: false + Invert Z Axis: false + Name: Current View + Near Clip Distance: 0.009999999776482582 + Scale: -49.22471237182617 + Target Frame: base_link + Value: TopDownOrtho (rviz_default_plugins) + X: 0.5208465456962585 + Y: 0.037297897040843964 + Saved: ~ +Window Geometry: + Displays: + collapsed: true + Height: 836 + Hide Left Dock: true + Hide Right Dock: true + Navigation 2: + collapsed: false + QMainWindow State: 000000ff00000000fd000000040000000000000286000002eafc020000000bfb0000001200530065006c0065006300740069006f006e000000003d000000830000005c00fffffffb0000001e0054006f006f006c002000500072006f007000650072007400690065007302000001ed000001df00000185000000a3fb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb000000100044006900730070006c006100790073000000003d000002ea000000c900fffffffb0000002000730065006c0065006300740069006f006e00200062007500660066006500720200000138000000aa0000023a00000294fb00000014005700690064006500530074006500720065006f02000000e6000000d2000003ee0000030bfb0000000c004b0069006e0065006300740200000186000001060000030c00000261fb00000018004e0061007600690067006100740069006f006e0020003200000002da000001010000013900fffffffb0000001e005200650061006c00730065006e0073006500430061006d00650072006100000002c6000000c10000002800fffffffb0000001e005200650061006c00730065006e0073006500430061006d0065007200610100000177000000fc0000000000000000000000010000010f0000039efc0200000003fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000000a00560069006500770073000000003d0000039e000000a400fffffffb0000001200530065006c0065006300740069006f006e010000025a000000b200000000000000000000000200000490000000a9fc0100000001fb0000000a00560069006500770073030000004e00000080000002e10000019700000003000004420000003efc0100000002fb0000000800540069006d00650100000000000004420000000000000000fb0000000800540069006d006501000000000000045000000000000000000000039d0000031100000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730000000000ffffffff0000000000000000 + RealsenseCamera: + collapsed: false + Selection: + collapsed: false + Tool Properties: + collapsed: false + Views: + collapsed: true + Width: 925 + X: 85 + Y: 28 diff --git a/smacc2_sm_reference_library/sm_dancebot_artgallery_ue/rviz/nav2_namespaced_view.rviz b/smacc2_sm_reference_library/sm_dancebot_artgallery_ue/rviz/nav2_namespaced_view.rviz new file mode 100644 index 000000000..2a024f75b --- /dev/null +++ b/smacc2_sm_reference_library/sm_dancebot_artgallery_ue/rviz/nav2_namespaced_view.rviz @@ -0,0 +1,371 @@ +Panels: + - Class: rviz_common/Displays + Help Height: 195 + Name: Displays + Property Tree Widget: + Expanded: + - /Global Options1 + - /RobotModel1/Status1 + - /TF1/Frames1 + - /TF1/Tree1 + - /Global Planner1/Global Costmap1 + Splitter Ratio: 0.5833333134651184 + Tree Height: 464 + - Class: rviz_common/Selection + Name: Selection + - Class: rviz_common/Tool Properties + Expanded: + - /Publish Point1 + Name: Tool Properties + Splitter Ratio: 0.5886790156364441 + - Class: rviz_common/Views + Expanded: + - /Current View1 + Name: Views + Splitter Ratio: 0.5 + - Class: nav2_rviz_plugins/Navigation 2 + Name: Navigation 2 +Visualization Manager: + Class: "" + Displays: + - Alpha: 0.5 + Cell Size: 1 + Class: rviz_default_plugins/Grid + Color: 160; 160; 164 + Enabled: true + Line Style: + Line Width: 0.029999999329447746 + Value: Lines + Name: Grid + Normal Cell Count: 0 + Offset: + X: 0 + Y: 0 + Z: 0 + Plane: XY + Plane Cell Count: 10 + Reference Frame: + Value: true + - Alpha: 1 + Class: rviz_default_plugins/RobotModel + Collision Enabled: false + Description File: "" + Description Source: Topic + Description Topic: + Depth: 5 + Durability Policy: Volatile + History Policy: Keep Last + Reliability Policy: Reliable + Value: /robot_description + Enabled: false + Links: + All Links Enabled: true + Expand Joint Details: false + Expand Link Details: false + Expand Tree: false + Link Tree Style: Links in Alphabetic Order + Name: RobotModel + TF Prefix: "" + Update Interval: 0 + Value: true + Visual Enabled: true + - Class: rviz_default_plugins/TF + Enabled: true + Frame Timeout: 15 + Frames: + All Enabled: false + Marker Scale: 1 + Name: TF + Show Arrows: true + Show Axes: true + Show Names: false + Tree: + {} + Update Interval: 0 + Value: true + - Alpha: 1 + Autocompute Intensity Bounds: true + Autocompute Value Bounds: + Max Value: 10 + Min Value: -10 + Value: true + Axis: Z + Channel Name: intensity + Class: rviz_default_plugins/LaserScan + Color: 255; 255; 255 + Color Transformer: Intensity + Decay Time: 0 + Enabled: true + Invert Rainbow: false + Max Color: 255; 255; 255 + Max Intensity: 0 + Min Color: 0; 0; 0 + Min Intensity: 0 + Name: LaserScan + Position Transformer: XYZ + Selectable: true + Size (Pixels): 3 + Size (m): 0.009999999776482582 + Style: Flat Squares + Topic: + Depth: 5 + Durability Policy: Volatile + History Policy: Keep Last + Reliability Policy: Best Effort + Value: /scan + Use Fixed Frame: true + Use rainbow: true + Value: true + - Alpha: 1 + Autocompute Intensity Bounds: true + Autocompute Value Bounds: + Max Value: 10 + Min Value: -10 + Value: true + Axis: Z + Channel Name: intensity + Class: rviz_default_plugins/PointCloud2 + Color: 255; 255; 255 + Color Transformer: "" + Decay Time: 0 + Enabled: true + Invert Rainbow: false + Max Color: 255; 255; 255 + Max Intensity: 4096 + Min Color: 0; 0; 0 + Min Intensity: 0 + Name: Bumper Hit + Position Transformer: "" + Selectable: true + Size (Pixels): 3 + Size (m): 0.07999999821186066 + Style: Spheres + Topic: + Depth: 5 + Durability Policy: Volatile + History Policy: Keep Last + Reliability Policy: Best Effort + Value: /mobile_base/sensors/bumper_pointcloud + Use Fixed Frame: true + Use rainbow: true + Value: true + - Alpha: 1 + Class: rviz_default_plugins/Map + Color Scheme: map + Draw Behind: true + Enabled: true + Name: Map + Topic: + Depth: 1 + Durability Policy: Transient Local + History Policy: Keep Last + Reliability Policy: Reliable + Value: /map + Use Timestamp: false + Value: true + - Alpha: 1 + Arrow Length: 0.019999999552965164 + Axes Length: 0.30000001192092896 + Axes Radius: 0.009999999776482582 + Class: rviz_default_plugins/PoseArray + Color: 0; 180; 0 + Enabled: true + Head Length: 0.07000000029802322 + Head Radius: 0.029999999329447746 + Name: Amcl Particle Swarm + Shaft Length: 0.23000000417232513 + Shaft Radius: 0.009999999776482582 + Shape: Arrow (Flat) + Topic: + Depth: 5 + Durability Policy: Volatile + History Policy: Keep Last + Reliability Policy: Best Effort + Value: /particlecloud + Value: true + - Class: rviz_common/Group + Displays: + - Alpha: 0.30000001192092896 + Class: rviz_default_plugins/Map + Color Scheme: costmap + Draw Behind: false + Enabled: true + Name: Global Costmap + Topic: + Depth: 1 + Durability Policy: Transient Local + History Policy: Keep Last + Reliability Policy: Reliable + Value: /global_costmap/costmap + Use Timestamp: false + Value: true + - Alpha: 1 + Buffer Length: 1 + Class: rviz_default_plugins/Path + Color: 255; 0; 0 + Enabled: true + Head Diameter: 0.019999999552965164 + Head Length: 0.019999999552965164 + Length: 0.30000001192092896 + Line Style: Lines + Line Width: 0.029999999329447746 + Name: Path + Offset: + X: 0 + Y: 0 + Z: 0 + Pose Color: 255; 85; 255 + Pose Style: Arrows + Radius: 0.029999999329447746 + Shaft Diameter: 0.004999999888241291 + Shaft Length: 0.019999999552965164 + Topic: + Depth: 5 + Durability Policy: Volatile + History Policy: Keep Last + Reliability Policy: Reliable + Value: /plan + Value: true + - Alpha: 1 + Class: rviz_default_plugins/Polygon + Color: 25; 255; 0 + Enabled: false + Name: Polygon + Topic: + Depth: 5 + Durability Policy: Volatile + History Policy: Keep Last + Reliability Policy: Reliable + Value: /global_costmap/published_footprint + Value: false + Enabled: true + Name: Global Planner + - Class: rviz_common/Group + Displays: + - Alpha: 0.699999988079071 + Class: rviz_default_plugins/Map + Color Scheme: costmap + Draw Behind: false + Enabled: true + Name: Local Costmap + Topic: + Depth: 1 + Durability Policy: Transient Local + History Policy: Keep Last + Reliability Policy: Reliable + Value: /local_costmap/costmap + Use Timestamp: false + Value: true + - Alpha: 1 + Buffer Length: 1 + Class: rviz_default_plugins/Path + Color: 0; 12; 255 + Enabled: true + Head Diameter: 0.30000001192092896 + Head Length: 0.20000000298023224 + Length: 0.30000001192092896 + Line Style: Lines + Line Width: 0.029999999329447746 + Name: Local Plan + Offset: + X: 0 + Y: 0 + Z: 0 + Pose Color: 255; 85; 255 + Pose Style: None + Radius: 0.029999999329447746 + Shaft Diameter: 0.10000000149011612 + Shaft Length: 0.10000000149011612 + Topic: + Depth: 5 + Durability Policy: Volatile + History Policy: Keep Last + Reliability Policy: Reliable + Value: /local_plan + Value: true + - Class: rviz_default_plugins/MarkerArray + Enabled: false + Name: Trajectories + Namespaces: + {} + Topic: + Depth: 5 + Durability Policy: Volatile + History Policy: Keep Last + Reliability Policy: Reliable + Value: /marker + Value: false + - Alpha: 1 + Class: rviz_default_plugins/Polygon + Color: 25; 255; 0 + Enabled: true + Name: Polygon + Topic: + Depth: 5 + Durability Policy: Volatile + History Policy: Keep Last + Reliability Policy: Reliable + Value: /local_costmap/published_footprint + Value: true + Enabled: true + Name: Controller + Enabled: true + Global Options: + Background Color: 48; 48; 48 + Fixed Frame: map + Frame Rate: 30 + Name: root + Tools: + - Class: rviz_default_plugins/MoveCamera + - Class: rviz_default_plugins/Select + - Class: rviz_default_plugins/FocusCamera + - Class: rviz_default_plugins/Measure + Line color: 128; 128; 0 + - Class: rviz_default_plugins/SetInitialPose + Topic: + Depth: 5 + Durability Policy: Volatile + History Policy: Keep Last + Reliability Policy: Reliable + Value: /initialpose + - Class: nav2_rviz_plugins/GoalTool + Transformation: + Current: + Class: rviz_default_plugins/TF + Value: true + Views: + Current: + Angle: -1.5700000524520874 + Class: rviz_default_plugins/TopDownOrtho + Enable Stereo Rendering: + Stereo Eye Separation: 0.05999999865889549 + Stereo Focal Distance: 1 + Swap Stereo Eyes: false + Value: false + Invert Z Axis: false + Name: Current View + Near Clip Distance: 0.009999999776482582 + Scale: 134.638427734375 + Target Frame: + Value: TopDownOrtho (rviz_default_plugins) + X: -0.032615214586257935 + Y: -0.0801941454410553 + Saved: ~ +Window Geometry: + Displays: + collapsed: false + Height: 914 + Hide Left Dock: false + Hide Right Dock: true + Navigation 2: + collapsed: false + QMainWindow State: 000000ff00000000fd00000004000000000000016a00000338fc0200000009fb0000001200530065006c0065006300740069006f006e00000001e10000009b0000005c00fffffffb0000001e0054006f006f006c002000500072006f007000650072007400690065007302000001ed000001df00000185000000a3fb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb000000100044006900730070006c006100790073010000003d000002c4000000c900fffffffb0000002000730065006c0065006300740069006f006e00200062007500660066006500720200000138000000aa0000023a00000294fb00000014005700690064006500530074006500720065006f02000000e6000000d2000003ee0000030bfb0000000c004b0069006e0065006300740200000186000001060000030c00000261fb00000018004e0061007600690067006100740069006f006e0020003201000003070000006e0000006200ffffff000000010000010f00000338fc0200000003fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000000a00560069006500770073000000003d00000338000000a400fffffffb0000001200530065006c0065006300740069006f006e010000025a000000b200000000000000000000000200000490000000a9fc0100000001fb0000000a00560069006500770073030000004e00000080000002e10000019700000003000004420000003efc0100000002fb0000000800540069006d00650100000000000004420000000000000000fb0000000800540069006d00650100000000000004500000000000000000000004990000033800000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000 + Selection: + collapsed: false + Tool Properties: + collapsed: false + Views: + collapsed: true + Width: 1545 + X: 186 + Y: 117 diff --git a/smacc2_sm_reference_library/sm_dancebot_artgallery_ue/scripts/test_oscillation.py b/smacc2_sm_reference_library/sm_dancebot_artgallery_ue/scripts/test_oscillation.py new file mode 100755 index 000000000..3008e133b --- /dev/null +++ b/smacc2_sm_reference_library/sm_dancebot_artgallery_ue/scripts/test_oscillation.py @@ -0,0 +1,165 @@ +#!/usr/bin/env python3 + +# Copyright (c) 2018 Intel Corporation +# +# Licensed under the Apache License, Version 2.0 (the "License"); +# you may not use this file except in compliance with the License. +# You may obtain a copy of the License at +# +# http://www.apache.org/licenses/LICENSE-2.0 +# +# Unless required by applicable law or agreed to in writing, software +# distributed under the License is distributed on an "AS IS" BASIS, +# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +# See the License for the specific language governing permissions and +# limitations under the License. + +import rclpy +from rclpy.node import Node +from geometry_msgs.msg import Twist +from tf2_ros import TransformListener +from tf2_ros import Buffer +import math +import tf_transformations +import std_msgs.msg + + +class RotationOscillation(Node): + def __init__(self): + super().__init__("rotation_oscillation") + self.cmd_vel_pub = self.create_publisher(Twist, "cmd_vel", 1) + self.yaw_pub = self.create_publisher(std_msgs.msg.Float64, "yaw", 1) + + self.tf_buffer = Buffer() + self.tf_listener = TransformListener(self.tf_buffer, self) + self.on_timer = self.create_timer(0.05, self.on_timer_callback) + + self.base_link_frame = "base_footprint" + self.fixed_frame = "odom" + + def on_timer_callback(self): + try: + base_link_transform = self.tf_buffer.lookup_transform( + self.fixed_frame, self.base_link_frame, rclpy.time.Time() + ) + q = base_link_transform.transform.rotation + euler = tf_transformations.euler_from_quaternion([q.x, q.y, q.z, q.w]) + self.get_logger().info("Current rotation: %f" % math.degrees(euler[2])) + self.get_logger().info("Transform available!") + + t = float(self.get_clock().now().nanoseconds) / 1e9 + cmd_vel = Twist() + cmd_vel.angular.z = 1.0 if math.sin(t) > 0.0 else -1.0 + self.cmd_vel_pub.publish(cmd_vel) + + self.yaw_pub.publish(std_msgs.msg.Float64(data=euler[2])) + + self.get_logger().info("Current time: %s" % str(t)) + except Exception as e: + self.get_logger().error("Error getting transform: %s" % str(e)) + + def oscillate_rotation(self): + rate = self.create_rate(1) + angle = math.radians(25) + # timeout = rclpy.duration.Duration(seconds=1.0) # Timeout for waiting on transforms + + fixed_frame = "odom" + base_link_frame = "base_footprint" + + print(f'Waiting for transform from "{fixed_frame}" to "{base_link_frame}"...') + while rclpy.ok(): + # self.get_logger().info(f'Waiting for transform from "{fixed_frame}" to "{base_link_frame}"...') + rclpy.spin_once(self) + # self.get_logger().info('Spin once, now: %s' % str(self.get_clock().now().toSec())) + rate.sleep() + + self.get_logger().info("Transform available!") + + # Get initial position + print("Getting initial position...") + try: + base_link_transform = self.tf_buffer.lookup_transform( + "map", base_link_frame, rclpy.time.Time() + ) + initial_position = base_link_transform.transform.translation + except Exception as e: + self.get_logger().error("Error getting initial position: %s" % str(e)) + return + + # Rotate 25 degrees to the left + print("Rotating 25 degrees to the left...") + self.get_logger().info("Rotating 25 degrees to the left...") + twist_msg = Twist() + twist_msg.angular.z = angle + self.cmd_vel_pub.publish(twist_msg) + rclpy.spin_once(self) + + # Get position after left rotation + print("Getting position after left rotation...") + try: + print("Getting position after left rotation...") + base_link_transform = self.tf_buffer.lookup_transform( + "map", base_link_frame, rclpy.time.Time() + ) + left_rotation_position = base_link_transform.transform.translation + except Exception as e: + self.get_logger().error("Error getting position after left rotation: %s" % str(e)) + return + + # Rotate 25 degrees to the right + print("Rotating 25 degrees to the right...") + self.get_logger().info("Rotating 25 degrees to the right...") + twist_msg.angular.z = -angle + self.cmd_vel_pub.publish(twist_msg) + rclpy.spin_once(self) + + # Get position after right rotation + try: + base_link_transform = self.tf_buffer.lookup_transform( + "map", base_link_frame, rclpy.time.Time() + ) + right_rotation_position = base_link_transform.transform.translation + except Exception as e: + self.get_logger().error("Error getting position after right rotation: %s" % str(e)) + return + + # Calculate phase difference + print("Calculating phase difference...") + left_rotation_angle = math.atan2( + left_rotation_position.y - initial_position.y, + left_rotation_position.x - initial_position.x, + ) + right_rotation_angle = math.atan2( + right_rotation_position.y - initial_position.y, + right_rotation_position.x - initial_position.x, + ) + phase_difference = math.degrees(right_rotation_angle - left_rotation_angle) + + self.get_logger().info("Phase difference: %f degrees" % phase_difference) + + # Stop the robot + twist_msg.angular.z = 0.0 + self.cmd_vel_pub.publish(twist_msg) + + def shutdown(self): + self.get_logger().info("Shutting down...") + self.destroy_node() + + +def main(args=None): + rclpy.init(args=args) + rotation_oscillation = RotationOscillation() + + rclpy.spin(rotation_oscillation) + + try: + rotation_oscillation.oscillate_rotation() + except Exception as e: + rotation_oscillation.get_logger().error("Error during rotation oscillation: %s" % str(e)) + + rotation_oscillation.shutdown() + rclpy.shutdown() + + +if __name__ == "__main__": + main() diff --git a/smacc2_sm_reference_library/sm_dancebot_artgallery_ue/scripts/transform_publisher.py b/smacc2_sm_reference_library/sm_dancebot_artgallery_ue/scripts/transform_publisher.py new file mode 100644 index 000000000..2f9af0eef --- /dev/null +++ b/smacc2_sm_reference_library/sm_dancebot_artgallery_ue/scripts/transform_publisher.py @@ -0,0 +1,123 @@ +#!/usr/bin/env python3 + +# Copyright (c) 2018 Intel Corporation +# +# Licensed under the Apache License, Version 2.0 (the "License"); +# you may not use this file except in compliance with the License. +# You may obtain a copy of the License at +# +# http://www.apache.org/licenses/LICENSE-2.0 +# +# Unless required by applicable law or agreed to in writing, software +# distributed under the License is distributed on an "AS IS" BASIS, +# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +# See the License for the specific language governing permissions and +# limitations under the License. + +import rclpy +from rclpy.node import Node +import tf2_ros +from geometry_msgs.msg import TransformStamped +import rosgraph_msgs.msg + +tfbrod = None +clock_msg = None + + +class StaticTransformPublisher: + def __init__(self, node, parent_frame, child_frame, xyz=[0.0, 0.0, 0.0], rpy=[0.0, 0.0, 0.0]): + self.node = node + global tfbrod + print("Initializing static transform publisher") + + self.transform_broadcaster = tfbrod + print("xyz: ", xyz) + + try: + self.transform = TransformStamped() + self.transform.header.frame_id = parent_frame + self.transform.child_frame_id = child_frame + self.transform.transform.translation.x = xyz[0] + self.transform.transform.translation.y = xyz[1] + self.transform.transform.translation.z = xyz[2] + + self.transform.transform.rotation.x = 0.0 + self.transform.transform.rotation.y = 0.0 + self.transform.transform.rotation.z = 0.0 + self.transform.transform.rotation.w = 1.0 + except Exception as e: + print(e) + self.node.get_logger().error("Exception in StaticTransformPublisher: %r" % e) + + self.timer = node.create_timer(0.05, self.publish_transform) + + def publish_transform(self): + global clock_msg + if clock_msg is None: + return + + time = rclpy.time.Time(seconds=clock_msg.clock.sec, nanoseconds=clock_msg.clock.nanosec) + # time = time + rclpy.time.Duration(seconds=0.01) + self.transform.header.stamp = time.to_msg() + self.transform_broadcaster.sendTransform(self.transform) + self.node.get_logger().info( + "Publishing transform from %s to %s" + % (self.transform.header.frame_id, self.transform.child_frame_id) + ) + + +def main(args=None): + print("Initializing static transform publisher") + rclpy.init() + + node = rclpy.create_node("static_transform_publisher_1") + + global tfbrod + + tfbrod = tf2_ros.TransformBroadcaster(node) + + def callback(msg): + global clock_msg + clock_msg = msg + + clock_sub = node.create_subscription( + rosgraph_msgs.msg.Clock, + "/clock", + callback, + qos_profile=rclpy.qos.qos_profile_sensor_data, + ) + + print("Creating static transform publisher nodes") + # Create the first instance of StaticTransformPublisher + node1 = StaticTransformPublisher( + node, + "base_footprint", + "base_link", + ) + + # + # Create the second instance of StaticTransformPublisher + node2 = StaticTransformPublisher( + node, + "base_link", + "base_scan", + xyz=[-0.064, 0.0, 0.122], + ) + + node3 = StaticTransformPublisher( + node, + "map", + "odom", + xyz=[0.0, 0.0, 0.0], + ) + + rclpy.spin(node) + rclpy.shutdown() + + +if __name__ == "__main__": + import sys + + print("Starting static transform publisher") + + main() diff --git a/smacc2_sm_reference_library/sm_dancebot_artgallery_ue/scripts/ue_navigation_frames_ground_truth_adapter.py b/smacc2_sm_reference_library/sm_dancebot_artgallery_ue/scripts/ue_navigation_frames_ground_truth_adapter.py new file mode 100755 index 000000000..c2e5fe041 --- /dev/null +++ b/smacc2_sm_reference_library/sm_dancebot_artgallery_ue/scripts/ue_navigation_frames_ground_truth_adapter.py @@ -0,0 +1,115 @@ +#!/usr/bin/env python3 + +# Copyright (c) 2018 Intel Corporation +# +# Licensed under the Apache License, Version 2.0 (the "License"); +# you may not use this file except in compliance with the License. +# You may obtain a copy of the License at +# +# http://www.apache.org/licenses/LICENSE-2.0 +# +# Unless required by applicable law or agreed to in writing, software +# distributed under the License is distributed on an "AS IS" BASIS, +# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +# See the License for the specific language governing permissions and +# limitations under the License. + +import rclpy +from rclpy.node import Node +import tf2_ros +from geometry_msgs.msg import TransformStamped +import rosgraph_msgs.msg +import ue_msgs.msg +import nav_msgs.msg + + +class UENavigationFramesGroundTruthAdapter: + def __init__(self, parent_frame, child_frame): + self.node = rclpy.create_node("static_transform_publisher_1") + self.transform_broadcaster = tf2_ros.TransformBroadcaster(self.node) + self.clock_msg = None + + self.parent_frame = parent_frame + self.child_frame = child_frame + + print("Initializing static transform publisher") + + self.uemsgs_sub = self.node.create_subscription( + ue_msgs.msg.EntityState, + "/ue_ros/model_state", + self.uemsgs_callback, + # reliability reliable + rclpy.qos.qos_profile_services_default, + ) + + self.odom_pub = self.node.create_publisher( + nav_msgs.msg.Odometry, "/odom", rclpy.qos.qos_profile_services_default + ) + + self.clock_sub = self.node.create_subscription( + rosgraph_msgs.msg.Clock, + "/clock", + self.clock_callback, + qos_profile=rclpy.qos.qos_profile_sensor_data, + ) + + def clock_callback(self, msg): + self.clock_msg = msg + + def uemsgs_callback(self, msg): + + if self.clock_msg is None: + return + + t = TransformStamped() + t.header.stamp = self.clock_msg.clock + t.header.frame_id = self.parent_frame + + t.child_frame_id = self.child_frame + t.transform.translation.x = msg.pose.position.x + t.transform.translation.y = msg.pose.position.y + t.transform.translation.z = msg.pose.position.z + + t.transform.rotation.x = msg.pose.orientation.x + t.transform.rotation.y = msg.pose.orientation.y + t.transform.rotation.z = msg.pose.orientation.z + t.transform.rotation.w = msg.pose.orientation.w + + # self.node.get_logger().info("uemsgs_callback" + str(t)) + + self.transform_broadcaster.sendTransform(t) + + odom = nav_msgs.msg.Odometry() + odom.header.stamp = self.clock_msg.clock + odom.header.frame_id = self.parent_frame + odom.child_frame_id = self.child_frame + odom.pose.pose = msg.pose + + # set covariance diagonal to 0.1 + odom.pose.covariance = [0.1 if i % 7 == 0 else 0.0 for i in range(36)] + + odom.twist.twist = msg.twist + odom.twist.covariance = [0.1 if i % 7 == 0 else 0.0 for i in range(36)] + + self.odom_pub.publish(odom) + + def spin(self): + rclpy.spin(self.node) + + +def main(args=None): + print("Initializing static transform publisher") + rclpy.init() + + ue_navigation_frames_ground_truth_adapter = UENavigationFramesGroundTruthAdapter( + "odom", "base_footprint" + ) + ue_navigation_frames_ground_truth_adapter.spin() + rclpy.shutdown() + + +if __name__ == "__main__": + import sys + + print("Starting static transform publisher") + main() diff --git a/smacc2_sm_reference_library/sm_dancebot_artgallery_ue/servers/led_action_server/action/LEDControl.action b/smacc2_sm_reference_library/sm_dancebot_artgallery_ue/servers/led_action_server/action/LEDControl.action new file mode 100644 index 000000000..e4dc1d976 --- /dev/null +++ b/smacc2_sm_reference_library/sm_dancebot_artgallery_ue/servers/led_action_server/action/LEDControl.action @@ -0,0 +1,18 @@ +uint8 command + +# Possible command values +uint8 CMD_ON = 0 +uint8 CMD_OFF = 1 +--- +uint8 state + +# Possible feeedback values (tool states) +uint8 STATE_UNKNOWN = 0 +uint8 STATE_RUNNING = 1 +uint8 STATE_IDLE = 2 +--- +uint8 result + +# Possible result values +uint8 RESULT_SUCCESS = 0 +uint8 RESULT_ERROR = 1 diff --git a/smacc2_sm_reference_library/sm_dancebot_artgallery_ue/servers/led_action_server/src/led_action_server_node.cpp b/smacc2_sm_reference_library/sm_dancebot_artgallery_ue/servers/led_action_server/src/led_action_server_node.cpp new file mode 100644 index 000000000..1ac313ce1 --- /dev/null +++ b/smacc2_sm_reference_library/sm_dancebot_artgallery_ue/servers/led_action_server/src/led_action_server_node.cpp @@ -0,0 +1,230 @@ +// 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 +#include +#include + +// This class describes a preemptable-on/off tool action server to be used from smacc +// shows in rviz a sphere whose color describe the current state (unknown, running, idle) +class LEDActionServer : public rclcpp::Node +{ +public: + std::shared_ptr> as_; + using GoalHandleLEDControl = + rclcpp_action::ServerGoalHandle; + + rclcpp::Publisher::SharedPtr stateMarkerPublisher_; + + uint8_t cmd; + + uint8_t currentState_; + + /** +****************************************************************************************************************** +* constructor() +****************************************************************************************************************** +*/ + LEDActionServer(const rclcpp::NodeOptions & options = rclcpp::NodeOptions()) + : Node("led_action_server_node", options) + { + currentState_ = sm_dancebot_artgallery_ue::action::LEDControl::Result::STATE_UNKNOWN; + } + + /** +****************************************************************************************************************** +* execute() +****************************************************************************************************************** +*/ + void execute(const std::shared_ptr + gh) // Note: "Action" is not appended to DoDishes here + { + auto goal = gh->get_goal(); + RCLCPP_INFO_STREAM(get_logger(), "Tool action server request: " << goal->command); + + cmd = goal->command; + + if (goal->command == sm_dancebot_artgallery_ue::action::LEDControl_Goal::CMD_ON) + { + RCLCPP_INFO(this->get_logger(), "ON"); + currentState_ = sm_dancebot_artgallery_ue::action::LEDControl_Result::STATE_RUNNING; + } + else if (goal->command == sm_dancebot_artgallery_ue::action::LEDControl_Goal::CMD_OFF) + { + RCLCPP_INFO(this->get_logger(), "OFF"); + currentState_ = sm_dancebot_artgallery_ue::action::LEDControl_Result::STATE_IDLE; + } + + auto feedback_msg = std::make_shared(); + + // 10Hz internal loop + rclcpp::Rate rate(20); + + while (rclcpp::ok()) + { + gh->publish_feedback(feedback_msg); + + publishStateMarker(); + rate.sleep(); + RCLCPP_INFO_THROTTLE(this->get_logger(), *(this->get_clock()), 2000, "Loop feedback"); + } + + auto result = std::make_shared(); + result->state = this->currentState_; + + // never reach succeeded because were are interested in keeping the feedback alive + //as_->setSucceeded(); + gh->succeed(result); + } + + rclcpp_action::GoalResponse handle_goal( + const rclcpp_action::GoalUUID & /*uuid*/, + std::shared_ptr /*goal*/) + { + // (void)uuid; + // // Let's reject sequences that are over 9000 + // if (goal->order > 9000) { + // return rclcpp_action::GoalResponse::REJECT; + // } + + // lets accept everything + RCLCPP_INFO(this->get_logger(), "Handle goal"); + return rclcpp_action::GoalResponse::ACCEPT_AND_EXECUTE; + } + + rclcpp_action::CancelResponse handle_cancel( + const std::shared_ptr /*goal_handle*/) + { + RCLCPP_INFO(rclcpp::get_logger("server"), "Got request to cancel goal"); + //(void)goal_handle; + return rclcpp_action::CancelResponse::ACCEPT; + } + + void handle_accepted(const std::shared_ptr goal_handle) + { + // this needs to return quickly to avoid blocking the executor, so spin up a new thread + std::thread{std::bind(&LEDActionServer::execute, this, std::placeholders::_1), goal_handle} + .detach(); + } + + /** +****************************************************************************************************************** +* run() +****************************************************************************************************************** +*/ + void run() + { + RCLCPP_INFO(this->get_logger(), "Creating tool action server"); + //as_ = std::make_shared(n, "led_action_server", boost::bind(&LEDActionServer::execute, this, _1), false); + + this->as_ = rclcpp_action::create_server( + this, "led_action_server", + std::bind(&LEDActionServer::handle_goal, this, std::placeholders::_1, std::placeholders::_2), + std::bind(&LEDActionServer::handle_cancel, this, std::placeholders::_1), + std::bind(&LEDActionServer::handle_accepted, this, std::placeholders::_1)); + + RCLCPP_INFO(get_logger(), "Starting Tool Action Server"); + stateMarkerPublisher_ = + this->create_publisher("tool_markers", 1); + } + + /** +****************************************************************************************************************** +* publishStateMarker() +****************************************************************************************************************** +*/ + void publishStateMarker() + { + visualization_msgs::msg::Marker marker; + marker.header.frame_id = "base_link"; + marker.header.stamp = this->now(); + + marker.ns = "tool_namespace"; + marker.id = 0; + marker.type = visualization_msgs::msg::Marker::SPHERE; + marker.action = visualization_msgs::msg::Marker::ADD; + + marker.scale.x = 0.2; + marker.scale.y = 0.2; + marker.scale.z = 0.2; + + marker.color.a = 1; + + if (currentState_ == sm_dancebot_artgallery_ue::action::LEDControl::Result::STATE_RUNNING) + { + // show green ball + marker.color.r = 0; + marker.color.g = 1; + marker.color.b = 0; + } + else if (currentState_ == sm_dancebot_artgallery_ue::action::LEDControl::Result::STATE_IDLE) + { + // show gray ball + marker.color.r = 0.7; + marker.color.g = 0.7; + marker.color.b = 0.7; + } + else + { + // show black ball + marker.color.r = 0; + marker.color.g = 0; + marker.color.b = 0; + } + + marker.pose.orientation.w = 1; + marker.pose.position.x = 0; + marker.pose.position.y = 0; + marker.pose.position.z = 1; + + visualization_msgs::msg::MarkerArray ma; + ma.markers.push_back(marker); + + stateMarkerPublisher_->publish(ma); + } +}; + +/** +****************************************************************************************************************** +* main() +****************************************************************************************************************** +*/ +int main(int argc, char ** argv) +{ + rclcpp::init(argc, argv); + auto ledactionserver = std::make_shared(); + ledactionserver->run(); + + rclcpp::spin(ledactionserver); + rclcpp::shutdown(); + return 0; +} diff --git a/smacc2_sm_reference_library/sm_dancebot_artgallery_ue/servers/service_node_3/service_node_3.py b/smacc2_sm_reference_library/sm_dancebot_artgallery_ue/servers/service_node_3/service_node_3.py new file mode 100644 index 000000000..a6694cde3 --- /dev/null +++ b/smacc2_sm_reference_library/sm_dancebot_artgallery_ue/servers/service_node_3/service_node_3.py @@ -0,0 +1,47 @@ +#!/usr/bin/env python3 + +# 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. + +# import roslib +# import rospy +import rclpy +from rclpy.node import Node + +import std_srvs +import std_srvs.srv + +if __name__ == "__main__": + + class Service3(Node): + def __init__(self): + super().__init__("Service3") + self.s = self.create_service( + std_srvs.srv.SetBool, "service_node3", self.set_bool_service + ) + + def set_bool_service(self, req, res): + self.get_logger().info("RECEIVING SET BOOL SERVICE REQUEST: value=" + str(req.data)) + + res.message = "OK, value set" + res.success = True + return res + + rclpy.init(args=None) + s = Service3() + + # s = rospy.Service('service_node3', std_srvs.srv.SetBool, set_bool_service) + # rospy.spin() + rclpy.spin(s) + rclpy.shutdown() diff --git a/smacc2_sm_reference_library/sm_dancebot_artgallery_ue/servers/temperature_sensor_node/src/temperature_sensor_node.cpp b/smacc2_sm_reference_library/sm_dancebot_artgallery_ue/servers/temperature_sensor_node/src/temperature_sensor_node.cpp new file mode 100644 index 000000000..ce66d640a --- /dev/null +++ b/smacc2_sm_reference_library/sm_dancebot_artgallery_ue/servers/temperature_sensor_node/src/temperature_sensor_node.cpp @@ -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. + +#include +#include + +int main(int argc, char ** argv) +{ + rclcpp::init(argc, argv); + + auto nh = rclcpp::Node::make_shared("temperature_sensor_node"); + auto pub = nh->create_publisher("/temperature", 1); + + rclcpp::Rate r(10); + + while (rclcpp::ok()) + { + sensor_msgs::msg::Temperature msg; + msg.temperature = sin(nh->now().seconds() / 2.0) * 50; + pub->publish(msg); + + r.sleep(); + rclcpp::spin_some(nh); + } +} diff --git a/smacc2_sm_reference_library/sm_dancebot_artgallery_ue/src/sm_dancebot_artgallery_ue/clients/client_behaviors/nav2z_client/cb_active_stop.cpp b/smacc2_sm_reference_library/sm_dancebot_artgallery_ue/src/sm_dancebot_artgallery_ue/clients/client_behaviors/nav2z_client/cb_active_stop.cpp new file mode 100644 index 000000000..8e8201148 --- /dev/null +++ b/smacc2_sm_reference_library/sm_dancebot_artgallery_ue/src/sm_dancebot_artgallery_ue/clients/client_behaviors/nav2z_client/cb_active_stop.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 +#include +#include + +#include +#include + +namespace sm_dancebot_artgallery_ue +{ +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 sm_dancebot_artgallery_ue diff --git a/smacc2_sm_reference_library/sm_dancebot_artgallery_ue/src/sm_dancebot_artgallery_ue/clients/client_behaviors/nav2z_client/cb_position_control_free_space.cpp b/smacc2_sm_reference_library/sm_dancebot_artgallery_ue/src/sm_dancebot_artgallery_ue/clients/client_behaviors/nav2z_client/cb_position_control_free_space.cpp new file mode 100644 index 000000000..5ffd9df29 --- /dev/null +++ b/smacc2_sm_reference_library/sm_dancebot_artgallery_ue/src/sm_dancebot_artgallery_ue/clients/client_behaviors/nav2z_client/cb_position_control_free_space.cpp @@ -0,0 +1,195 @@ +// 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 sm_dancebot_artgallery_ue +{ +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)); + + sm_dancebot_artgallery_ue::CpUEPose * 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 sm_dancebot_artgallery_ue diff --git a/smacc2_sm_reference_library/sm_dancebot_artgallery_ue/src/sm_dancebot_artgallery_ue/clients/components/cp_ue_pose.cpp b/smacc2_sm_reference_library/sm_dancebot_artgallery_ue/src/sm_dancebot_artgallery_ue/clients/components/cp_ue_pose.cpp new file mode 100644 index 000000000..0dc99bb63 --- /dev/null +++ b/smacc2_sm_reference_library/sm_dancebot_artgallery_ue/src/sm_dancebot_artgallery_ue/clients/components/cp_ue_pose.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. + +/***************************************************************************************************************** + *-2020 + * Authors: Pablo Inigo Blasco, Brett Aldrich + * + ******************************************************************************************************************/ +#include + +#include +#include +#include +#include +#include +#include +#include + +#include +#include +#include + +namespace sm_dancebot_artgallery_ue +{ +CpUEPose::CpUEPose(std::string topicname) + : CpTopicSubscriber(topicname) +{ +} + +void CpUEPose::onInitialize() +{ + CpTopicSubscriber::onInitialize(); + this->onMessageReceived(&CpUEPose::onPoseMessageReceived, this); +} + +void CpUEPose::onPoseMessageReceived(const ue_msgs::msg::EntityState& msg) +{ + this->entityStateMsg_ = msg;; + + RCLCPP_INFO_STREAM_THROTTLE(getLogger(), *(getNode()->get_clock()), 200, "Received UEPose x: " << msg.pose.position.x << " y: " << msg.pose.position.y << " z: " << msg.pose.position.z); +} + +geometry_msgs::msg::Pose CpUEPose::toPoseMsg() +{ + return this->entityStateMsg_.pose; +} + +} // namespace cl_nav2z diff --git a/smacc2_sm_reference_library/sm_dancebot_artgallery_ue/src/sm_dancebot_artgallery_ue/sm_dancebot_artgallery_ue.cpp b/smacc2_sm_reference_library/sm_dancebot_artgallery_ue/src/sm_dancebot_artgallery_ue/sm_dancebot_artgallery_ue.cpp new file mode 100644 index 000000000..8d9c457ef --- /dev/null +++ b/smacc2_sm_reference_library/sm_dancebot_artgallery_ue/src/sm_dancebot_artgallery_ue/sm_dancebot_artgallery_ue.cpp @@ -0,0 +1,27 @@ +// 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 + +int main(int argc, char ** argv) +{ + rclcpp::init(argc, argv); + smacc2::run(); +} diff --git a/smacc2_sm_reference_library/sm_dancebot_artgallery_ue/urdf/turtlebot3_waffle.urdf b/smacc2_sm_reference_library/sm_dancebot_artgallery_ue/urdf/turtlebot3_waffle.urdf new file mode 100644 index 000000000..f4307672c --- /dev/null +++ b/smacc2_sm_reference_library/sm_dancebot_artgallery_ue/urdf/turtlebot3_waffle.urdf @@ -0,0 +1,293 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/smacc2_sm_reference_library/sm_dancebot_artgallery_ue/worlds/ridgeback_race.world b/smacc2_sm_reference_library/sm_dancebot_artgallery_ue/worlds/ridgeback_race.world new file mode 100644 index 000000000..3ff47663c --- /dev/null +++ b/smacc2_sm_reference_library/sm_dancebot_artgallery_ue/worlds/ridgeback_race.world @@ -0,0 +1,4864 @@ + + + + 1 + 0 0 10 0 -0 0 + 0.8 0.8 0.8 1 + 0.2 0.2 0.2 1 + + 1000 + 0.9 + 0.01 + 0.001 + + -0.5 0.1 -0.9 + + 0 + 0 + 0 + + + + 1 + + + + + 0 0 1 + 100 100 + + + + + + 100 + 50 + + + + + + + + + + + 10 + + + 0 + + + 0 0 1 + 100 100 + + + + + + + + 0 + 0 + + 0 + 0 + 1 + 0 + + + + 0.001 + 1 + 1000 + + + 0.4 0.4 0.4 1 + 0.7 0.7 0.7 1 + 1 + + + EARTH_WGS84 + 0 + 0 + 0 + 0 + + + 1 + 1 + + + + + https://fuel.ignitionrobotics.org/1.0/openrobotics/models/drc practice: orange jersey barrier/2/files/meshes/jersey_barrier.dae + + + + + + + https://fuel.ignitionrobotics.org/1.0/openrobotics/models/drc practice: orange jersey barrier/2/files/meshes/jersey_barrier.dae + + + 10 + + + + + + + + + + + + + + + 0 + 0 + + 0 0 0 0 0 -1.5707 + 0 + 0 + 1 + 0 + + 1 + + + + + https://fuel.ignitionrobotics.org/1.0/openrobotics/models/drc practice: white jersey barrier/2/files/meshes/jersey_barrier.dae + + + + + + + https://fuel.ignitionrobotics.org/1.0/openrobotics/models/drc practice: white jersey barrier/2/files/meshes/jersey_barrier.dae + + + 10 + + + + + + + + + + + + + + + 0 + 0 + + 1.7646 -0.008563 0 0 0 -1.5707 + 0 + 0 + 1 + 0 + + 1 + + + + + https://fuel.ignitionrobotics.org/1.0/openrobotics/models/drc practice: orange jersey barrier/2/files/meshes/jersey_barrier.dae + + + + + + + https://fuel.ignitionrobotics.org/1.0/openrobotics/models/drc practice: orange jersey barrier/2/files/meshes/jersey_barrier.dae + + + 10 + + + + + + + + + + + + + + + 0 + 0 + + 3.52922 -0.016171 0 0 0 -1.5707 + 0 + 0 + 1 + 0 + + 1 + + + + + https://fuel.ignitionrobotics.org/1.0/openrobotics/models/drc practice: white jersey barrier/2/files/meshes/jersey_barrier.dae + + + + + + + https://fuel.ignitionrobotics.org/1.0/openrobotics/models/drc practice: white jersey barrier/2/files/meshes/jersey_barrier.dae + + + 10 + + + + + + + + + + + + + + + 0 + 0 + + 5.13431 0.54837 0 0 0 -0.785398 + 0 + 0 + 1 + 0 + + 1 + + + + + https://fuel.ignitionrobotics.org/1.0/openrobotics/models/drc practice: orange jersey barrier/2/files/meshes/jersey_barrier.dae + + + + + + + https://fuel.ignitionrobotics.org/1.0/openrobotics/models/drc practice: orange jersey barrier/2/files/meshes/jersey_barrier.dae + + + 10 + + + + + + + + + + + + + + + 0 + 0 + + 6.3884 1.79074 0 0 0 -0.785398 + 0 + 0 + 1 + 0 + + 1 + + + + + https://fuel.ignitionrobotics.org/1.0/openrobotics/models/drc practice: white jersey barrier/2/files/meshes/jersey_barrier.dae + + + + + + + https://fuel.ignitionrobotics.org/1.0/openrobotics/models/drc practice: white jersey barrier/2/files/meshes/jersey_barrier.dae + + + 10 + + + + + + + + + + + + + + + 0 + 0 + + 7.6423 3.03258 0 0 0 -0.785398 + 0 + 0 + 1 + 0 + + 1 + + + + + https://fuel.ignitionrobotics.org/1.0/openrobotics/models/drc practice: orange jersey barrier/2/files/meshes/jersey_barrier.dae + + + + + + + https://fuel.ignitionrobotics.org/1.0/openrobotics/models/drc practice: orange jersey barrier/2/files/meshes/jersey_barrier.dae + + + 10 + + + + + + + + + + + + + + + 0 + 0 + + 8.89611 4.27529 0 0 0 -0.785398 + 0 + 0 + 1 + 0 + + 1 + + + + + https://fuel.ignitionrobotics.org/1.0/openrobotics/models/drc practice: white jersey barrier/2/files/meshes/jersey_barrier.dae + + + + + + + https://fuel.ignitionrobotics.org/1.0/openrobotics/models/drc practice: white jersey barrier/2/files/meshes/jersey_barrier.dae + + + 10 + + + + + + + + + + + + + + + 0 + 0 + + 10.1496 5.51614 0 0 0 -0.785398 + 0 + 0 + 1 + 0 + + 1 + + + + + model://blue_cylinder/meshes/cylinder.dae + + + + + + + model://blue_cylinder/meshes/cylinder.dae + + + 10 + + + + + + + + + + + + + + + 0 + 0 + + 11.0288 6.44203 0 0 -0 0 + 0 + 0 + 1 + 0 + + 1 + + + + + model://blue_cylinder/meshes/cylinder.dae + + + + + + + model://blue_cylinder/meshes/cylinder.dae + + + 10 + + + + + + + + + + + + + + + 0 + 0 + + 11.0288 6.44203 0.866 0 -0 0 + 0 + 0 + 1 + 0 + + -1 -6 0 0 -0 0 + + + 1 + + + + + https://fuel.ignitionrobotics.org/1.0/openrobotics/models/drc practice: white jersey barrier/2/files/meshes/jersey_barrier.dae + + + + + + + https://fuel.ignitionrobotics.org/1.0/openrobotics/models/drc practice: white jersey barrier/2/files/meshes/jersey_barrier.dae + + + 10 + + + + + + + + + + + + + + + 0 + 0 + + 0 + 0 + 1 + 0 + + -4 1.55828 0 0 -0 0 + + + 3116 755000000 + 356 954966630 + 1605650970 323926707 + 355029 + + -2.6481 -4.01574 -0.045465 0 0 -0.74752 + 1 1 1 + + 8.54538 -6.87025 -0.045465 0 0 -1.53292 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + + + 3.25439 -7.04543 -0.045465 0 0 -1.53292 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + + + 1.49008 -7.104 -0.045465 0 0 -1.53292 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + + + 9.8196 -6.78892 -0.045465 0 0 -0.74752 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + + + -2.6481 -4.01574 -0.045465 0 0 -2.31822 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + + + 6.78254 -6.92811 -0.045465 0 0 -1.53292 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + + + 5.0182 -6.98712 -0.045465 0 0 -1.53292 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + + + 9.8196 -6.78892 0.820535 0 0 -0.74752 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + + + -0.070844 -6.42684 -0.045465 0 0 -2.31822 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + + + -1.3598 -5.22163 -0.045465 0 0 -2.31822 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + + + + -15.3977 -1.55916 0 0 -0 1.04571 + 1 1 1 + + -15.0829 9.98827 0 0 -0 0.260314 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + + + -13.7448 4.86628 0 0 -0 0.260314 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + + + -13.2984 3.15835 0 0 -0 0.260314 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + + + -15.4433 11.2132 0 0 -0 1.04571 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + + + -15.3977 -1.55916 0 0 0 -0.524988 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + + + -14.6375 8.28162 0 0 -0 0.260314 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + + + -14.1908 6.57377 0 0 -0 0.260314 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + + + -15.4433 11.2132 0.866 0 -0 1.04571 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + + + -13.6146 1.4865 0 0 0 -0.524988 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + + + -14.5057 -0.036576 0 0 0 -0.524988 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + + + + -2.60683 -10.2645 0 0 0 -0.76918 + 1 1 1 + + 8.5222 -13.3608 0 0 0 -1.55458 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + + + 3.22866 -13.4213 0 0 0 -1.55458 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + + + 1.46349 -13.4417 0 0 0 -1.55458 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + + + 9.79789 -13.3071 0 0 0 -0.76918 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + + + -2.60683 -10.2645 0 0 0 -2.33988 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + + + 6.75852 -13.3804 0 0 0 -1.55458 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + + + 4.99332 -13.4012 0 0 0 -1.55458 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + + + 9.79789 -13.3071 0.866 0 0 -0.76918 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + + + -0.082399 -12.7309 0 0 0 -2.33988 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + + + -1.34495 -11.498 0 0 0 -2.33988 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + + + + 10.3347 -5.65314 0 0 -0 1.56441 + 1 1 1 + + 4.88352 4.5315 0 0 -0 0.779009 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + + + 8.58481 0.746571 0 0 -0 0.779009 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + + + 9.81915 -0.515431 0 0 -0 0.779009 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + + + 3.96327 5.4166 0 0 -0 1.56441 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + + + 10.3347 -5.65314 0 0 0 -0.006293 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + + + 6.11634 3.2701 0 0 -0 0.779009 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + + + 7.35101 2.00838 0 0 -0 0.779009 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + + + 3.96327 5.4166 0.866 0 -0 1.56441 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + + + 10.3734 -2.1241 0 0 0 -0.006293 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + + + 10.3545 -3.88863 0 0 0 -0.006293 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + + + + -16.0335 -6.41848 0 0 0 -0.7879 + 1 1 1 + + -4.96438 -9.72253 0 0 0 -1.5733 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + + + -10.2581 -9.68398 0 0 0 -1.5733 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + + + -12.0234 -9.67127 0 0 0 -1.5733 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + + + -3.68791 -9.69271 0 0 0 -0.7879 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + + + -16.0335 -6.41848 0 0 0 -2.3586 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + + + -6.72812 -9.70918 0 0 0 -1.5733 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + + + -8.4934 -9.69692 0 0 0 -1.5733 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + + + -3.68791 -9.69271 0.866 0 0 -0.7879 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + + + -13.5557 -8.93166 0 0 0 -2.3586 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + + + -14.7949 -7.6754 0 0 0 -2.3586 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + + + + -1 16 0 0 -0 0 + 1 1 1 + + 9.2248 10.3902 0 0 0 -2.3562 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + + + 5.49818 14.15 0 0 0 -2.3562 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + + + 4.2559 15.4033 0 0 0 -2.3562 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + + + 10.0288 9.55797 0 0 -0 0 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + + + -1 16 0 0 0 -1.5707 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + + + 7.98256 11.6439 0 0 0 -2.3562 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + + + 6.74031 12.8974 0 0 0 -2.3562 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + + + 10.0288 9.55797 0.866 0 -0 0 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + + + 2.52922 15.9838 0 0 0 -1.5707 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + + + 0.7646 15.9914 0 0 0 -1.5707 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + + + + -14.6681 12.3809 0 0 -0 0.810038 + 1 1 1 + + -3.55516 15.9191 0 0 0 -1.54616 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + + + -8.84779 15.8121 0 0 0 -1.54616 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + + + -10.6121 15.7764 0 0 0 -1.54616 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + + + -2.39802 15.9276 0 0 -0 0.810038 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + + + -14.6681 12.3809 0 0 0 -0.760662 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + + + -5.3197 15.8837 0 0 0 -1.54616 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + + + -7.08412 15.8482 0 0 0 -1.54616 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + + + -2.39802 15.9276 0.866 0 -0 0.810038 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + + + -12.2231 14.926 0 0 0 -0.760662 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + + + -13.4453 13.6531 0 0 0 -0.760662 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + + + + -8.62781 9.94618 0 0 -0 0.2056 + 1 1 1 + + 2.5269 6.54195 0 0 0 -2.1506 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + + + -1.88879 9.46171 0 0 0 -2.1506 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + + + -3.36079 10.4351 0 0 0 -2.1506 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + + + 3.48387 5.89136 0 0 -0 0.205596 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + + + -8.62781 9.94618 0 0 0 -1.3651 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + + + 1.05488 7.51562 0 0 0 -2.1506 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + + + -0.41711 8.4891 0 0 0 -2.1506 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + + + 3.48387 5.89136 0.866 0 -0 0.205596 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + + + -5.16961 10.6508 0 0 0 -1.3651 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + + + -6.89862 10.298 0 0 0 -1.3651 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + + + + -16.7548 -4.64388 0 0 -0 0 + 1 1 1 + + -16.7548 -4.64388 0 0 -0 0 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + + + + -17.1282 -4.13814 0 0 -0 0 + 1 1 1 + + -17.1282 -4.13814 0 0 -0 0 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + + + + 10.3218 -9.21435 0 0 -0 0 + 1 1 1 + + 10.3218 -9.21435 0 0 -0 0 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + + + + 10.3067 -12.7762 0 0 -0 0 + 1 1 1 + + 10.3067 -12.7762 0 0 -0 0 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + + + + 10.3176 -7.4365 0 0 -0 0 + 1 1 1 + + 10.3176 -7.4365 0 0 -0 0 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + + + + 10.3136 -11.0025 0 0 -0 0 + 1 1 1 + + 10.3136 -11.0025 0 0 -0 0 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + + + + -16.2875 -3.11229 0 0 0 -0.509254 + 1 1 1 + + -16.2875 -3.11229 0 0 0 -0.509254 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + + + + -17.3228 -5.14571 0 0 0 -2.31969 + 1 1 1 + + -17.3228 -5.14571 0 0 0 -2.31969 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + + + + 0 0 0 0 -0 0 + 1 1 1 + + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + + + + 0.127058 -0.012625 0.008401 -5.7e-05 0.003353 -0.002954 + 1 1 1 + + 0.127058 -0.012625 0.008401 -5.7e-05 0.003353 -0.002954 + 0.001954 -0.000866 -0.061964 -0.008321 0.009978 -0.001386 + 0.001244 0.001765 0.002452 -0.135997 -0.01007 0.000878 + 0.001244 0.001765 0.002452 0 -0 0 + + + 0.127066 -0.012614 0.008412 -2.8e-05 0.003314 -0.002936 + 0.001162 -0.002502 -0.062977 -0.012557 0.01357 -0.00402 + -0.002239 0.004917 -0.002962 -0.159433 0.012841 0.004083 + -0.002239 0.004917 -0.002962 0 -0 0 + + + 0.127057 -0.012617 0.0084 -6.9e-05 0.003501 -0.00297 + 0.002415 -0.00193 -0.061395 -0.005008 -0.011677 0.001501 + -0.003148 0.006146 -0.003368 -0.028835 -0.016126 -0.002503 + -0.000393 0.000768 -0.000421 0 -0 0 + + + 0.127059 -0.012617 0.008403 -5.8e-05 0.003458 -0.002962 + 0.002127 -0.002056 -0.061757 -0.007075 -0.00588 0.000236 + -0.001696 0.001569 -0.002879 -0.058616 -0.009069 -0.001539 + -0.001696 0.001569 -0.002879 0 -0 0 + + + -0.049765 0.051908 0.005015 -0.045565 -1.47685 -1.24358 + 0.002355 -0.002164 -0.063759 1.02255 1.31025 -0 + -0.00117 0.003147 -0.007182 0 -0 0 + -1e-06 3e-06 -7e-06 0 -0 0 + + + -0.050165 -0.076075 0.004993 1.90095 0.495873 2.6548 + 0.005449 -0.004628 -0.024591 0.926043 1.08964 0 + -0.023688 0.021829 0.247248 1.35553 -0.826371 3.14159 + -2.4e-05 2.2e-05 0.000247 0 -0 0 + + + 0.060947 -0.012566 -19432.4 0 -0 0 + 0 0 -617.145 0 -0 0 + 0 0 -9.8 0 -0 0 + 0 0 -9.8 0 -0 0 + + + 0.106509 -0.01259 0.041208 3.13937 1.12267 3.13728 + 0.002393 -0.00053 -0.06195 0.008576 0.03027 0.007711 + -0.002374 0.001791 -0.006185 -0.054285 -0.072005 -0.002259 + -0.000237 0.000179 -0.000618 0 -0 0 + + + 0.106061 -0.012586 0.040984 3.13916 1.14848 3.13713 + 0.000902 -0.000269 -0.06003 0.004974 0.030614 0.00275 + -0.001844 0.001728 0.003565 -0.052403 -0.057989 -0.04024 + -0.000184 0.000173 0.000357 0 -0 0 + + + + 0 0 10 0 -0 0 + + + + + 1.03059 2.69165 18.1021 -0 1.5578 -3.12116 + orbit + perspective + + + + 1 + + + + + https://fuel.ignitionrobotics.org/1.0/openrobotics/models/drc practice: orange jersey barrier/2/files/meshes/jersey_barrier.dae + + + + + + + https://fuel.ignitionrobotics.org/1.0/openrobotics/models/drc practice: orange jersey barrier/2/files/meshes/jersey_barrier.dae + + + 10 + + + + + + + + + + + + + + + 0 + 0 + + 0 + 0 + 1 + 0 + + 10 -9.57988 0 0 -0 0 + + + 1 + + + + + https://fuel.ignitionrobotics.org/1.0/openrobotics/models/drc practice: white jersey barrier/2/files/meshes/jersey_barrier.dae + + + + + + + https://fuel.ignitionrobotics.org/1.0/openrobotics/models/drc practice: white jersey barrier/2/files/meshes/jersey_barrier.dae + + + 10 + + + + + + + + + + + + + + + 0 + 0 + + 0 + 0 + 1 + 0 + + 10.4084 -11 0 0 -0 0 + + + 1 + + + + + https://fuel.ignitionrobotics.org/1.0/openrobotics/models/drc practice: orange jersey barrier/2/files/meshes/jersey_barrier.dae + + + + + + + https://fuel.ignitionrobotics.org/1.0/openrobotics/models/drc practice: orange jersey barrier/2/files/meshes/jersey_barrier.dae + + + 10 + + + + + + + + + + + + + + + 0 + 0 + + 0 + 0 + 1 + 0 + + 10.4143 -13 0 0 -0 0 + + + 1 + 1 + + + + + https://fuel.ignitionrobotics.org/1.0/openrobotics/models/drc practice: orange jersey barrier/2/files/meshes/jersey_barrier.dae + + + + + + + https://fuel.ignitionrobotics.org/1.0/openrobotics/models/drc practice: orange jersey barrier/2/files/meshes/jersey_barrier.dae + + + 10 + + + + + + + + + + + + + + + 0 + 0 + + 0 0 0 0 0 -1.5707 + 0 + 0 + 1 + 0 + + 1 + + + + + https://fuel.ignitionrobotics.org/1.0/openrobotics/models/drc practice: white jersey barrier/2/files/meshes/jersey_barrier.dae + + + + + + + https://fuel.ignitionrobotics.org/1.0/openrobotics/models/drc practice: white jersey barrier/2/files/meshes/jersey_barrier.dae + + + 10 + + + + + + + + + + + + + + + 0 + 0 + + 1.7646 -0.008563 0 0 0 -1.5707 + 0 + 0 + 1 + 0 + + 1 + + + + + https://fuel.ignitionrobotics.org/1.0/openrobotics/models/drc practice: orange jersey barrier/2/files/meshes/jersey_barrier.dae + + + + + + + https://fuel.ignitionrobotics.org/1.0/openrobotics/models/drc practice: orange jersey barrier/2/files/meshes/jersey_barrier.dae + + + 10 + + + + + + + + + + + + + + + 0 + 0 + + 3.52922 -0.016171 0 0 0 -1.5707 + 0 + 0 + 1 + 0 + + 1 + + + + + https://fuel.ignitionrobotics.org/1.0/openrobotics/models/drc practice: white jersey barrier/2/files/meshes/jersey_barrier.dae + + + + + + + https://fuel.ignitionrobotics.org/1.0/openrobotics/models/drc practice: white jersey barrier/2/files/meshes/jersey_barrier.dae + + + 10 + + + + + + + + + + + + + + + 0 + 0 + + 5.13431 0.54837 0 0 0 -0.785398 + 0 + 0 + 1 + 0 + + 1 + + + + + https://fuel.ignitionrobotics.org/1.0/openrobotics/models/drc practice: orange jersey barrier/2/files/meshes/jersey_barrier.dae + + + + + + + https://fuel.ignitionrobotics.org/1.0/openrobotics/models/drc practice: orange jersey barrier/2/files/meshes/jersey_barrier.dae + + + 10 + + + + + + + + + + + + + + + 0 + 0 + + 6.3884 1.79074 0 0 0 -0.785398 + 0 + 0 + 1 + 0 + + 1 + + + + + https://fuel.ignitionrobotics.org/1.0/openrobotics/models/drc practice: white jersey barrier/2/files/meshes/jersey_barrier.dae + + + + + + + https://fuel.ignitionrobotics.org/1.0/openrobotics/models/drc practice: white jersey barrier/2/files/meshes/jersey_barrier.dae + + + 10 + + + + + + + + + + + + + + + 0 + 0 + + 7.6423 3.03258 0 0 0 -0.785398 + 0 + 0 + 1 + 0 + + 1 + + + + + https://fuel.ignitionrobotics.org/1.0/openrobotics/models/drc practice: orange jersey barrier/2/files/meshes/jersey_barrier.dae + + + + + + + https://fuel.ignitionrobotics.org/1.0/openrobotics/models/drc practice: orange jersey barrier/2/files/meshes/jersey_barrier.dae + + + 10 + + + + + + + + + + + + + + + 0 + 0 + + 8.89611 4.27529 0 0 0 -0.785398 + 0 + 0 + 1 + 0 + + 1 + + + + + https://fuel.ignitionrobotics.org/1.0/openrobotics/models/drc practice: white jersey barrier/2/files/meshes/jersey_barrier.dae + + + + + + + https://fuel.ignitionrobotics.org/1.0/openrobotics/models/drc practice: white jersey barrier/2/files/meshes/jersey_barrier.dae + + + 10 + + + + + + + + + + + + + + + 0 + 0 + + 10.1496 5.51614 0 0 0 -0.785398 + 0 + 0 + 1 + 0 + + 1 + + + + + model://blue_cylinder/meshes/cylinder.dae + + + + + + + model://blue_cylinder/meshes/cylinder.dae + + + 10 + + + + + + + + + + + + + + + 0 + 0 + + 11.0288 6.44203 0 0 -0 0 + 0 + 0 + 1 + 0 + + 1 + + + + + model://blue_cylinder/meshes/cylinder.dae + + + + + + + model://blue_cylinder/meshes/cylinder.dae + + + 10 + + + + + + + + + + + + + + + 0 + 0 + + 11.0288 6.44203 0.866 0 -0 0 + 0 + 0 + 1 + 0 + + -1.5566 -14 0 0 -0 0 + + + 1 + 1 + + + + + https://fuel.ignitionrobotics.org/1.0/openrobotics/models/drc practice: orange jersey barrier/2/files/meshes/jersey_barrier.dae + + + + + + + https://fuel.ignitionrobotics.org/1.0/openrobotics/models/drc practice: orange jersey barrier/2/files/meshes/jersey_barrier.dae + + + 10 + + + + + + + + + + + + + + + 0 + 0 + + 0 0 0 0 0 -1.5707 + 0 + 0 + 1 + 0 + + 1 + + + + + https://fuel.ignitionrobotics.org/1.0/openrobotics/models/drc practice: white jersey barrier/2/files/meshes/jersey_barrier.dae + + + + + + + https://fuel.ignitionrobotics.org/1.0/openrobotics/models/drc practice: white jersey barrier/2/files/meshes/jersey_barrier.dae + + + 10 + + + + + + + + + + + + + + + 0 + 0 + + 1.7646 -0.008563 0 0 0 -1.5707 + 0 + 0 + 1 + 0 + + 1 + + + + + https://fuel.ignitionrobotics.org/1.0/openrobotics/models/drc practice: orange jersey barrier/2/files/meshes/jersey_barrier.dae + + + + + + + https://fuel.ignitionrobotics.org/1.0/openrobotics/models/drc practice: orange jersey barrier/2/files/meshes/jersey_barrier.dae + + + 10 + + + + + + + + + + + + + + + 0 + 0 + + 3.52922 -0.016171 0 0 0 -1.5707 + 0 + 0 + 1 + 0 + + 1 + + + + + https://fuel.ignitionrobotics.org/1.0/openrobotics/models/drc practice: white jersey barrier/2/files/meshes/jersey_barrier.dae + + + + + + + https://fuel.ignitionrobotics.org/1.0/openrobotics/models/drc practice: white jersey barrier/2/files/meshes/jersey_barrier.dae + + + 10 + + + + + + + + + + + + + + + 0 + 0 + + 5.13431 0.54837 0 0 0 -0.785398 + 0 + 0 + 1 + 0 + + 1 + + + + + https://fuel.ignitionrobotics.org/1.0/openrobotics/models/drc practice: orange jersey barrier/2/files/meshes/jersey_barrier.dae + + + + + + + https://fuel.ignitionrobotics.org/1.0/openrobotics/models/drc practice: orange jersey barrier/2/files/meshes/jersey_barrier.dae + + + 10 + + + + + + + + + + + + + + + 0 + 0 + + 6.3884 1.79074 0 0 0 -0.785398 + 0 + 0 + 1 + 0 + + 1 + + + + + https://fuel.ignitionrobotics.org/1.0/openrobotics/models/drc practice: white jersey barrier/2/files/meshes/jersey_barrier.dae + + + + + + + https://fuel.ignitionrobotics.org/1.0/openrobotics/models/drc practice: white jersey barrier/2/files/meshes/jersey_barrier.dae + + + 10 + + + + + + + + + + + + + + + 0 + 0 + + 7.6423 3.03258 0 0 0 -0.785398 + 0 + 0 + 1 + 0 + + 1 + + + + + https://fuel.ignitionrobotics.org/1.0/openrobotics/models/drc practice: orange jersey barrier/2/files/meshes/jersey_barrier.dae + + + + + + + https://fuel.ignitionrobotics.org/1.0/openrobotics/models/drc practice: orange jersey barrier/2/files/meshes/jersey_barrier.dae + + + 10 + + + + + + + + + + + + + + + 0 + 0 + + 8.89611 4.27529 0 0 0 -0.785398 + 0 + 0 + 1 + 0 + + 1 + + + + + https://fuel.ignitionrobotics.org/1.0/openrobotics/models/drc practice: white jersey barrier/2/files/meshes/jersey_barrier.dae + + + + + + + https://fuel.ignitionrobotics.org/1.0/openrobotics/models/drc practice: white jersey barrier/2/files/meshes/jersey_barrier.dae + + + 10 + + + + + + + + + + + + + + + 0 + 0 + + 10.1496 5.51614 0 0 0 -0.785398 + 0 + 0 + 1 + 0 + + 1 + + + + + model://blue_cylinder/meshes/cylinder.dae + + + + + + + model://blue_cylinder/meshes/cylinder.dae + + + 10 + + + + + + + + + + + + + + + 0 + 0 + + 11.0288 6.44203 0 0 -0 0 + 0 + 0 + 1 + 0 + + 1 + + + + + model://blue_cylinder/meshes/cylinder.dae + + + + + + + model://blue_cylinder/meshes/cylinder.dae + + + 10 + + + + + + + + + + + + + + + 0 + 0 + + 11.0288 6.44203 0.866 0 -0 0 + 0 + 0 + 1 + 0 + + -1 -1 0 0 -0 0 + + + 1 + 1 + + + + + https://fuel.ignitionrobotics.org/1.0/openrobotics/models/drc practice: orange jersey barrier/2/files/meshes/jersey_barrier.dae + + + + + + + https://fuel.ignitionrobotics.org/1.0/openrobotics/models/drc practice: orange jersey barrier/2/files/meshes/jersey_barrier.dae + + + 10 + + + + + + + + + + + + + + + 0 + 0 + + 0 0 0 0 0 -1.5707 + 0 + 0 + 1 + 0 + + 1 + + + + + https://fuel.ignitionrobotics.org/1.0/openrobotics/models/drc practice: white jersey barrier/2/files/meshes/jersey_barrier.dae + + + + + + + https://fuel.ignitionrobotics.org/1.0/openrobotics/models/drc practice: white jersey barrier/2/files/meshes/jersey_barrier.dae + + + 10 + + + + + + + + + + + + + + + 0 + 0 + + 1.7646 -0.008563 0 0 0 -1.5707 + 0 + 0 + 1 + 0 + + 1 + + + + + https://fuel.ignitionrobotics.org/1.0/openrobotics/models/drc practice: orange jersey barrier/2/files/meshes/jersey_barrier.dae + + + + + + + https://fuel.ignitionrobotics.org/1.0/openrobotics/models/drc practice: orange jersey barrier/2/files/meshes/jersey_barrier.dae + + + 10 + + + + + + + + + + + + + + + 0 + 0 + + 3.52922 -0.016171 0 0 0 -1.5707 + 0 + 0 + 1 + 0 + + 1 + + + + + https://fuel.ignitionrobotics.org/1.0/openrobotics/models/drc practice: white jersey barrier/2/files/meshes/jersey_barrier.dae + + + + + + + https://fuel.ignitionrobotics.org/1.0/openrobotics/models/drc practice: white jersey barrier/2/files/meshes/jersey_barrier.dae + + + 10 + + + + + + + + + + + + + + + 0 + 0 + + 5.2559 -0.59667 0 0 0 -2.3562 + 0 + 0 + 1 + 0 + + 1 + + + + + https://fuel.ignitionrobotics.org/1.0/openrobotics/models/drc practice: orange jersey barrier/2/files/meshes/jersey_barrier.dae + + + + + + + https://fuel.ignitionrobotics.org/1.0/openrobotics/models/drc practice: orange jersey barrier/2/files/meshes/jersey_barrier.dae + + + 10 + + + + + + + + + + + + + + + 0 + 0 + + 6.49818 -1.85004 0 0 0 -2.3562 + 0 + 0 + 1 + 0 + + 1 + + + + + https://fuel.ignitionrobotics.org/1.0/openrobotics/models/drc practice: white jersey barrier/2/files/meshes/jersey_barrier.dae + + + + + + + https://fuel.ignitionrobotics.org/1.0/openrobotics/models/drc practice: white jersey barrier/2/files/meshes/jersey_barrier.dae + + + 10 + + + + + + + + + + + + + + + 0 + 0 + + 7.74031 -3.10261 0 0 0 -2.3562 + 0 + 0 + 1 + 0 + + 1 + + + + + https://fuel.ignitionrobotics.org/1.0/openrobotics/models/drc practice: orange jersey barrier/2/files/meshes/jersey_barrier.dae + + + + + + + https://fuel.ignitionrobotics.org/1.0/openrobotics/models/drc practice: orange jersey barrier/2/files/meshes/jersey_barrier.dae + + + 10 + + + + + + + + + + + + + + + 0 + 0 + + 8.98256 -4.3561 0 0 0 -2.3562 + 0 + 0 + 1 + 0 + + 1 + + + + + https://fuel.ignitionrobotics.org/1.0/openrobotics/models/drc practice: white jersey barrier/2/files/meshes/jersey_barrier.dae + + + + + + + https://fuel.ignitionrobotics.org/1.0/openrobotics/models/drc practice: white jersey barrier/2/files/meshes/jersey_barrier.dae + + + 10 + + + + + + + + + + + + + + + 0 + 0 + + 10.2248 -5.60978 0 0 0 -2.3562 + 0 + 0 + 1 + 0 + + 1 + + + + + model://blue_cylinder/meshes/cylinder.dae + + + + + + + model://blue_cylinder/meshes/cylinder.dae + + + 10 + + + + + + + + + + + + + + + 0 + 0 + + 11.0288 -6.44203 0 0 -0 0 + 0 + 0 + 1 + 0 + + 1 + + + + + model://blue_cylinder/meshes/cylinder.dae + + + + + + + model://blue_cylinder/meshes/cylinder.dae + + + 10 + + + + + + + + + + + + + + + 0 + 0 + + 11.0288 -6.44203 0.866 0 -0 0 + 0 + 0 + 1 + 0 + + -1 16 0 0 -0 0 + + + 1 + 1 + + + + + https://fuel.ignitionrobotics.org/1.0/openrobotics/models/drc practice: orange jersey barrier/2/files/meshes/jersey_barrier.dae + + + + + + + https://fuel.ignitionrobotics.org/1.0/openrobotics/models/drc practice: orange jersey barrier/2/files/meshes/jersey_barrier.dae + + + 10 + + + + + + + + + + + + + + + 0 + 0 + + 0 0 0 0 0 -1.5707 + 0 + 0 + 1 + 0 + + 1 + + + + + https://fuel.ignitionrobotics.org/1.0/openrobotics/models/drc practice: white jersey barrier/2/files/meshes/jersey_barrier.dae + + + + + + + https://fuel.ignitionrobotics.org/1.0/openrobotics/models/drc practice: white jersey barrier/2/files/meshes/jersey_barrier.dae + + + 10 + + + + + + + + + + + + + + + 0 + 0 + + 1.7646 -0.008563 0 0 0 -1.5707 + 0 + 0 + 1 + 0 + + 1 + + + + + https://fuel.ignitionrobotics.org/1.0/openrobotics/models/drc practice: orange jersey barrier/2/files/meshes/jersey_barrier.dae + + + + + + + https://fuel.ignitionrobotics.org/1.0/openrobotics/models/drc practice: orange jersey barrier/2/files/meshes/jersey_barrier.dae + + + 10 + + + + + + + + + + + + + + + 0 + 0 + + 3.52922 -0.016171 0 0 0 -1.5707 + 0 + 0 + 1 + 0 + + 1 + + + + + https://fuel.ignitionrobotics.org/1.0/openrobotics/models/drc practice: white jersey barrier/2/files/meshes/jersey_barrier.dae + + + + + + + https://fuel.ignitionrobotics.org/1.0/openrobotics/models/drc practice: white jersey barrier/2/files/meshes/jersey_barrier.dae + + + 10 + + + + + + + + + + + + + + + 0 + 0 + + 5.13431 0.54837 0 0 0 -0.785398 + 0 + 0 + 1 + 0 + + 1 + + + + + https://fuel.ignitionrobotics.org/1.0/openrobotics/models/drc practice: orange jersey barrier/2/files/meshes/jersey_barrier.dae + + + + + + + https://fuel.ignitionrobotics.org/1.0/openrobotics/models/drc practice: orange jersey barrier/2/files/meshes/jersey_barrier.dae + + + 10 + + + + + + + + + + + + + + + 0 + 0 + + 6.3884 1.79074 0 0 0 -0.785398 + 0 + 0 + 1 + 0 + + 1 + + + + + https://fuel.ignitionrobotics.org/1.0/openrobotics/models/drc practice: white jersey barrier/2/files/meshes/jersey_barrier.dae + + + + + + + https://fuel.ignitionrobotics.org/1.0/openrobotics/models/drc practice: white jersey barrier/2/files/meshes/jersey_barrier.dae + + + 10 + + + + + + + + + + + + + + + 0 + 0 + + 7.6423 3.03258 0 0 0 -0.785398 + 0 + 0 + 1 + 0 + + 1 + + + + + https://fuel.ignitionrobotics.org/1.0/openrobotics/models/drc practice: orange jersey barrier/2/files/meshes/jersey_barrier.dae + + + + + + + https://fuel.ignitionrobotics.org/1.0/openrobotics/models/drc practice: orange jersey barrier/2/files/meshes/jersey_barrier.dae + + + 10 + + + + + + + + + + + + + + + 0 + 0 + + 8.89611 4.27529 0 0 0 -0.785398 + 0 + 0 + 1 + 0 + + 1 + + + + + https://fuel.ignitionrobotics.org/1.0/openrobotics/models/drc practice: white jersey barrier/2/files/meshes/jersey_barrier.dae + + + + + + + https://fuel.ignitionrobotics.org/1.0/openrobotics/models/drc practice: white jersey barrier/2/files/meshes/jersey_barrier.dae + + + 10 + + + + + + + + + + + + + + + 0 + 0 + + 10.1496 5.51614 0 0 0 -0.785398 + 0 + 0 + 1 + 0 + + 1 + + + + + model://blue_cylinder/meshes/cylinder.dae + + + + + + + model://blue_cylinder/meshes/cylinder.dae + + + 10 + + + + + + + + + + + + + + + 0 + 0 + + 11.0288 6.44203 0 0 -0 0 + 0 + 0 + 1 + 0 + + 1 + + + + + model://blue_cylinder/meshes/cylinder.dae + + + + + + + model://blue_cylinder/meshes/cylinder.dae + + + 10 + + + + + + + + + + + + + + + 0 + 0 + + 11.0288 6.44203 0.866 0 -0 0 + 0 + 0 + 1 + 0 + + -14.5593 -16 0 0 -0 0 + + + 1 + 1 + + + + + https://fuel.ignitionrobotics.org/1.0/openrobotics/models/drc practice: orange jersey barrier/2/files/meshes/jersey_barrier.dae + + + + + + + https://fuel.ignitionrobotics.org/1.0/openrobotics/models/drc practice: orange jersey barrier/2/files/meshes/jersey_barrier.dae + + + 10 + + + + + + + + + + + + + + + 0 + 0 + + 0 0 0 0 0 -1.5707 + 0 + 0 + 1 + 0 + + 1 + + + + + https://fuel.ignitionrobotics.org/1.0/openrobotics/models/drc practice: white jersey barrier/2/files/meshes/jersey_barrier.dae + + + + + + + https://fuel.ignitionrobotics.org/1.0/openrobotics/models/drc practice: white jersey barrier/2/files/meshes/jersey_barrier.dae + + + 10 + + + + + + + + + + + + + + + 0 + 0 + + 1.7646 -0.008563 0 0 0 -1.5707 + 0 + 0 + 1 + 0 + + 1 + + + + + https://fuel.ignitionrobotics.org/1.0/openrobotics/models/drc practice: orange jersey barrier/2/files/meshes/jersey_barrier.dae + + + + + + + https://fuel.ignitionrobotics.org/1.0/openrobotics/models/drc practice: orange jersey barrier/2/files/meshes/jersey_barrier.dae + + + 10 + + + + + + + + + + + + + + + 0 + 0 + + 3.52922 -0.016171 0 0 0 -1.5707 + 0 + 0 + 1 + 0 + + 1 + + + + + https://fuel.ignitionrobotics.org/1.0/openrobotics/models/drc practice: white jersey barrier/2/files/meshes/jersey_barrier.dae + + + + + + + https://fuel.ignitionrobotics.org/1.0/openrobotics/models/drc practice: white jersey barrier/2/files/meshes/jersey_barrier.dae + + + 10 + + + + + + + + + + + + + + + 0 + 0 + + 5.2559 -0.59667 0 0 0 -2.3562 + 0 + 0 + 1 + 0 + + 1 + + + + + https://fuel.ignitionrobotics.org/1.0/openrobotics/models/drc practice: orange jersey barrier/2/files/meshes/jersey_barrier.dae + + + + + + + https://fuel.ignitionrobotics.org/1.0/openrobotics/models/drc practice: orange jersey barrier/2/files/meshes/jersey_barrier.dae + + + 10 + + + + + + + + + + + + + + + 0 + 0 + + 6.49818 -1.85004 0 0 0 -2.3562 + 0 + 0 + 1 + 0 + + 1 + + + + + https://fuel.ignitionrobotics.org/1.0/openrobotics/models/drc practice: white jersey barrier/2/files/meshes/jersey_barrier.dae + + + + + + + https://fuel.ignitionrobotics.org/1.0/openrobotics/models/drc practice: white jersey barrier/2/files/meshes/jersey_barrier.dae + + + 10 + + + + + + + + + + + + + + + 0 + 0 + + 7.74031 -3.10261 0 0 0 -2.3562 + 0 + 0 + 1 + 0 + + 1 + + + + + https://fuel.ignitionrobotics.org/1.0/openrobotics/models/drc practice: orange jersey barrier/2/files/meshes/jersey_barrier.dae + + + + + + + https://fuel.ignitionrobotics.org/1.0/openrobotics/models/drc practice: orange jersey barrier/2/files/meshes/jersey_barrier.dae + + + 10 + + + + + + + + + + + + + + + 0 + 0 + + 8.98256 -4.3561 0 0 0 -2.3562 + 0 + 0 + 1 + 0 + + 1 + + + + + https://fuel.ignitionrobotics.org/1.0/openrobotics/models/drc practice: white jersey barrier/2/files/meshes/jersey_barrier.dae + + + + + + + https://fuel.ignitionrobotics.org/1.0/openrobotics/models/drc practice: white jersey barrier/2/files/meshes/jersey_barrier.dae + + + 10 + + + + + + + + + + + + + + + 0 + 0 + + 10.2248 -5.60978 0 0 0 -2.3562 + 0 + 0 + 1 + 0 + + 1 + + + + + model://blue_cylinder/meshes/cylinder.dae + + + + + + + model://blue_cylinder/meshes/cylinder.dae + + + 10 + + + + + + + + + + + + + + + 0 + 0 + + 11.0288 -6.44203 0 0 -0 0 + 0 + 0 + 1 + 0 + + 1 + + + + + model://blue_cylinder/meshes/cylinder.dae + + + + + + + model://blue_cylinder/meshes/cylinder.dae + + + 10 + + + + + + + + + + + + + + + 0 + 0 + + 11.0288 -6.44203 0.866 0 -0 0 + 0 + 0 + 1 + 0 + + 25 -15 0 0 -0 0 + + + 1 + 1 + + + + + https://fuel.ignitionrobotics.org/1.0/openrobotics/models/drc practice: orange jersey barrier/2/files/meshes/jersey_barrier.dae + + + + + + + https://fuel.ignitionrobotics.org/1.0/openrobotics/models/drc practice: orange jersey barrier/2/files/meshes/jersey_barrier.dae + + + 10 + + + + + + + + + + + + + + + 0 + 0 + + 0 0 0 0 0 -1.5707 + 0 + 0 + 1 + 0 + + 1 + + + + + https://fuel.ignitionrobotics.org/1.0/openrobotics/models/drc practice: white jersey barrier/2/files/meshes/jersey_barrier.dae + + + + + + + https://fuel.ignitionrobotics.org/1.0/openrobotics/models/drc practice: white jersey barrier/2/files/meshes/jersey_barrier.dae + + + 10 + + + + + + + + + + + + + + + 0 + 0 + + 1.7646 -0.008563 0 0 0 -1.5707 + 0 + 0 + 1 + 0 + + 1 + + + + + https://fuel.ignitionrobotics.org/1.0/openrobotics/models/drc practice: orange jersey barrier/2/files/meshes/jersey_barrier.dae + + + + + + + https://fuel.ignitionrobotics.org/1.0/openrobotics/models/drc practice: orange jersey barrier/2/files/meshes/jersey_barrier.dae + + + 10 + + + + + + + + + + + + + + + 0 + 0 + + 3.52922 -0.016171 0 0 0 -1.5707 + 0 + 0 + 1 + 0 + + 1 + + + + + https://fuel.ignitionrobotics.org/1.0/openrobotics/models/drc practice: white jersey barrier/2/files/meshes/jersey_barrier.dae + + + + + + + https://fuel.ignitionrobotics.org/1.0/openrobotics/models/drc practice: white jersey barrier/2/files/meshes/jersey_barrier.dae + + + 10 + + + + + + + + + + + + + + + 0 + 0 + + 5.13431 0.54837 0 0 0 -0.785398 + 0 + 0 + 1 + 0 + + 1 + + + + + https://fuel.ignitionrobotics.org/1.0/openrobotics/models/drc practice: orange jersey barrier/2/files/meshes/jersey_barrier.dae + + + + + + + https://fuel.ignitionrobotics.org/1.0/openrobotics/models/drc practice: orange jersey barrier/2/files/meshes/jersey_barrier.dae + + + 10 + + + + + + + + + + + + + + + 0 + 0 + + 6.3884 1.79074 0 0 0 -0.785398 + 0 + 0 + 1 + 0 + + 1 + + + + + https://fuel.ignitionrobotics.org/1.0/openrobotics/models/drc practice: white jersey barrier/2/files/meshes/jersey_barrier.dae + + + + + + + https://fuel.ignitionrobotics.org/1.0/openrobotics/models/drc practice: white jersey barrier/2/files/meshes/jersey_barrier.dae + + + 10 + + + + + + + + + + + + + + + 0 + 0 + + 7.6423 3.03258 0 0 0 -0.785398 + 0 + 0 + 1 + 0 + + 1 + + + + + https://fuel.ignitionrobotics.org/1.0/openrobotics/models/drc practice: orange jersey barrier/2/files/meshes/jersey_barrier.dae + + + + + + + https://fuel.ignitionrobotics.org/1.0/openrobotics/models/drc practice: orange jersey barrier/2/files/meshes/jersey_barrier.dae + + + 10 + + + + + + + + + + + + + + + 0 + 0 + + 8.89611 4.27529 0 0 0 -0.785398 + 0 + 0 + 1 + 0 + + 1 + + + + + https://fuel.ignitionrobotics.org/1.0/openrobotics/models/drc practice: white jersey barrier/2/files/meshes/jersey_barrier.dae + + + + + + + https://fuel.ignitionrobotics.org/1.0/openrobotics/models/drc practice: white jersey barrier/2/files/meshes/jersey_barrier.dae + + + 10 + + + + + + + + + + + + + + + 0 + 0 + + 10.1496 5.51614 0 0 0 -0.785398 + 0 + 0 + 1 + 0 + + 1 + + + + + model://blue_cylinder/meshes/cylinder.dae + + + + + + + model://blue_cylinder/meshes/cylinder.dae + + + 10 + + + + + + + + + + + + + + + 0 + 0 + + 11.0288 6.44203 0 0 -0 0 + 0 + 0 + 1 + 0 + + 1 + + + + + model://blue_cylinder/meshes/cylinder.dae + + + + + + + model://blue_cylinder/meshes/cylinder.dae + + + 10 + + + + + + + + + + + + + + + 0 + 0 + + 11.0288 6.44203 0.866 0 -0 0 + 0 + 0 + 1 + 0 + + -25 -4 0 0 -0 0 + + + 1 + + + + + https://fuel.ignitionrobotics.org/1.0/openrobotics/models/drc practice: white jersey barrier/2/files/meshes/jersey_barrier.dae + + + + + + + https://fuel.ignitionrobotics.org/1.0/openrobotics/models/drc practice: white jersey barrier/2/files/meshes/jersey_barrier.dae + + + 10 + + + + + + + + + + + + + + + 0 + 0 + + 0 + 0 + 1 + 0 + + -16.4458 -4 0 0 -0 0 + + + 1 + + + + + https://fuel.ignitionrobotics.org/1.0/openrobotics/models/drc practice: white jersey barrier/2/files/meshes/jersey_barrier.dae + + + + + + + https://fuel.ignitionrobotics.org/1.0/openrobotics/models/drc practice: white jersey barrier/2/files/meshes/jersey_barrier.dae + + + 10 + + + + + + + + + + + + + + + 0 + 0 + + 0 + 0 + 1 + 0 + + -17.5466 -5 0 0 -0 0 + + + 1 + + + + + model://blue_cylinder/meshes/cylinder.dae + + + + + + + model://blue_cylinder/meshes/cylinder.dae + + + 10 + + + + + + + + + + + + + + + 0 + 0 + + 0 + 0 + 1 + 0 + + -17 -4.51853 0 0 -0 0 + + + 1 + + + + + model://blue_cylinder/meshes/cylinder.dae + + + + + + + model://blue_cylinder/meshes/cylinder.dae + + + 10 + + + + + + + + + + + + + + + 0 + 0 + + 0 + 0 + 1 + 0 + + -17.425 -4 0 0 -0 0 + + + 1 + 1 + + + + + https://fuel.ignitionrobotics.org/1.0/openrobotics/models/drc practice: orange jersey barrier/2/files/meshes/jersey_barrier.dae + + + + + + + https://fuel.ignitionrobotics.org/1.0/openrobotics/models/drc practice: orange jersey barrier/2/files/meshes/jersey_barrier.dae + + + 10 + + + + + + + + + + + + + + + 0 + 0 + + 0 0 0 0 0 -1.5707 + 0 + 0 + 1 + 0 + + 1 + + + + + https://fuel.ignitionrobotics.org/1.0/openrobotics/models/drc practice: white jersey barrier/2/files/meshes/jersey_barrier.dae + + + + + + + https://fuel.ignitionrobotics.org/1.0/openrobotics/models/drc practice: white jersey barrier/2/files/meshes/jersey_barrier.dae + + + 10 + + + + + + + + + + + + + + + 0 + 0 + + 1.7646 -0.008563 0 0 0 -1.5707 + 0 + 0 + 1 + 0 + + 1 + + + + + https://fuel.ignitionrobotics.org/1.0/openrobotics/models/drc practice: orange jersey barrier/2/files/meshes/jersey_barrier.dae + + + + + + + https://fuel.ignitionrobotics.org/1.0/openrobotics/models/drc practice: orange jersey barrier/2/files/meshes/jersey_barrier.dae + + + 10 + + + + + + + + + + + + + + + 0 + 0 + + 3.52922 -0.016171 0 0 0 -1.5707 + 0 + 0 + 1 + 0 + + 1 + + + + + https://fuel.ignitionrobotics.org/1.0/openrobotics/models/drc practice: white jersey barrier/2/files/meshes/jersey_barrier.dae + + + + + + + https://fuel.ignitionrobotics.org/1.0/openrobotics/models/drc practice: white jersey barrier/2/files/meshes/jersey_barrier.dae + + + 10 + + + + + + + + + + + + + + + 0 + 0 + + 5.2559 -0.59667 0 0 0 -2.3562 + 0 + 0 + 1 + 0 + + 1 + + + + + https://fuel.ignitionrobotics.org/1.0/openrobotics/models/drc practice: orange jersey barrier/2/files/meshes/jersey_barrier.dae + + + + + + + https://fuel.ignitionrobotics.org/1.0/openrobotics/models/drc practice: orange jersey barrier/2/files/meshes/jersey_barrier.dae + + + 10 + + + + + + + + + + + + + + + 0 + 0 + + 6.49818 -1.85004 0 0 0 -2.3562 + 0 + 0 + 1 + 0 + + 1 + + + + + https://fuel.ignitionrobotics.org/1.0/openrobotics/models/drc practice: white jersey barrier/2/files/meshes/jersey_barrier.dae + + + + + + + https://fuel.ignitionrobotics.org/1.0/openrobotics/models/drc practice: white jersey barrier/2/files/meshes/jersey_barrier.dae + + + 10 + + + + + + + + + + + + + + + 0 + 0 + + 7.74031 -3.10261 0 0 0 -2.3562 + 0 + 0 + 1 + 0 + + 1 + + + + + https://fuel.ignitionrobotics.org/1.0/openrobotics/models/drc practice: orange jersey barrier/2/files/meshes/jersey_barrier.dae + + + + + + + https://fuel.ignitionrobotics.org/1.0/openrobotics/models/drc practice: orange jersey barrier/2/files/meshes/jersey_barrier.dae + + + 10 + + + + + + + + + + + + + + + 0 + 0 + + 8.98256 -4.3561 0 0 0 -2.3562 + 0 + 0 + 1 + 0 + + 1 + + + + + https://fuel.ignitionrobotics.org/1.0/openrobotics/models/drc practice: white jersey barrier/2/files/meshes/jersey_barrier.dae + + + + + + + https://fuel.ignitionrobotics.org/1.0/openrobotics/models/drc practice: white jersey barrier/2/files/meshes/jersey_barrier.dae + + + 10 + + + + + + + + + + + + + + + 0 + 0 + + 10.2248 -5.60978 0 0 0 -2.3562 + 0 + 0 + 1 + 0 + + 1 + + + + + model://blue_cylinder/meshes/cylinder.dae + + + + + + + model://blue_cylinder/meshes/cylinder.dae + + + 10 + + + + + + + + + + + + + + + 0 + 0 + + 11.0288 -6.44203 0 0 -0 0 + 0 + 0 + 1 + 0 + + 1 + + + + + model://blue_cylinder/meshes/cylinder.dae + + + + + + + model://blue_cylinder/meshes/cylinder.dae + + + 10 + + + + + + + + + + + + + + + 0 + 0 + + 11.0288 -6.44203 0.866 0 -0 0 + 0 + 0 + 1 + 0 + + -9 10 0 0 -0 0 + + 0 0 -9.8 + 6e-06 2.3e-05 -4.2e-05 + + + + -2 -0.5 0.01 0 -0 0 + + 0 + + 0 0 0 0 -0 0 + + 1 + 0 + 0 + 1 + 0 + 1 + + 1 + + 0 + 0 + + + + -0.064 0 0.048 0 -0 0 + + 0.001 + 0 + 0 + 0.001 + 0 + 0.001 + + 1 + + + -0.064 0 0.048 0 -0 0 + + + 0.265 0.265 0.089 + + + + + + + + + + + + + + + 10 + + + -0.064 0 0 0 -0 0 + + + model://turtlebot3_waffle/meshes/waffle_base.dae + 0.001 0.001 0.001 + + + + 0 + 0 + 0 + + + + 1 + 200 + + + + + 0 + 0.0002 + + + + + 0 + 0.0002 + + + + + 0 + 0.0002 + + + + + + + 0 + 0.017 + + + + + 0 + 0.017 + + + + + 0 + 0.017 + + + + + + + ~/out:=imu + + + + 0 + + 0 0 0 0 -0 0 + + 1 + 0 + 0 + 1 + 0 + 1 + + 1 + + 0 + 0 + + + + -0.052 0 0.111 0 -0 0 + + 0.001 + 0 + 0 + 0.001 + 0 + 0.001 + + 0.125 + + + -0.052 0 0.111 0 -0 0 + + + 0.0508 + 0.055 + + + + + + + + + + + + + + + 10 + + + -0.064 0 0.121 0 -0 0 + + + model://turtlebot3_waffle/meshes/lds.dae + 0.001 0.001 0.001 + + + + + 1 + 1 + -0.064 0 0.121 0 -0 0 + 5 + + + + 360 + 1 + 0 + 6.28 + + + 1 + 0 + 0 + + + + 0.32 + 20.5 + 0.015 + + + gaussian + 0 + 0.01 + + + + + ~/out:=scan + + sensor_msgs/LaserScan + base_scan + + + 0 + 0 + 0 + + + + 0 0.144 0.023 -1.57 0 0 + + 0.001 + 0 + 0 + 0.001 + 0 + 0.001 + + 0.1 + + + 0 0.144 0.023 -1.57 0 0 + + + 0.033 + 0.018 + + + + + + 100000 + 100000 + 0 0 0 + 0 + 0 + + + + + + + + 0 + 0.2 + 100000 + 1 + 0.01 + 0.001 + + + + + 10 + + + 0 0.144 0.023 0 -0 0 + + + model://turtlebot3_waffle/meshes/left_tire.dae + 0.001 0.001 0.001 + + + + 0 + 0 + 0 + + + + 0 -0.144 0.023 -1.57 0 0 + + 0.001 + 0 + 0 + 0.001 + 0 + 0.001 + + 0.1 + + + 0 -0.144 0.023 -1.57 0 0 + + + 0.033 + 0.018 + + + + + + 100000 + 100000 + 0 0 0 + 0 + 0 + + + + + + + + 0 + 0.2 + 100000 + 1 + 0.01 + 0.001 + + + + + 10 + + + 0 -0.144 0.023 0 -0 0 + + + model://turtlebot3_waffle/meshes/right_tire.dae + 0.001 0.001 0.001 + + + + 0 + 0 + 0 + + + -0.177 -0.064 -0.004 0 -0 0 + + 0.001 + + 1e-05 + 0 + 0 + 1e-05 + 0 + 1e-05 + + 0 0 0 0 -0 0 + + + + + 0.005 + + + + + + 0 + 0.2 + 100000 + 1 + 0.01 + 0.001 + + + + + + + + + + + 10 + + 0 + 0 + 0 + + + -0.177 0.064 -0.004 0 -0 0 + + 0.001 + + 1e-05 + 0 + 0 + 1e-05 + 0 + 1e-05 + + 0 0 0 0 -0 0 + + + + + 0.005 + + + + + + 0 + 0.2 + 100000 + 1 + 0.01 + 0.001 + + + + + + + + + + + 10 + + 0 + 0 + 0 + + + + 0.069 -0.047 0.107 0 -0 0 + + 0.001 + 0 + 0 + 0.001 + 0 + 0.001 + + 0.035 + + + 0 0.047 -0.005 0 -0 0 + + + 0.008 0.13 0.022 + + + + + + + + + + + + + + + 10 + + 0.069 -0.047 0.107 0 -0 0 + + 1 + 5 + 0.064 -0.047 0.107 0 -0 0 + + 1.047 + + 320 + 240 + + + 0.1 + 100 + + + + + intel_realsense_r200_depth + camera_depth_frame + 0.07 + 0.001 + 5.0 + + + + 0 0.047 -0.005 0 -0 0 + + + 0.008 0.13 0.022 + + + + + + + + + + + + + + + 10 + + 0.069 -0.047 0.107 0 -0 0 + 0 + 0 + 0 + + + base_footprint + base_link + 0 0 0.01 0 -0 0 + + + base_link + wheel_left_link + 0 0.144 0.023 -1.57 0 0 + + 0 0 1 + + -1e+16 + 1e+16 + + + 0 + 0 + + + + + base_link + wheel_right_link + 0 -0.144 0.023 -1.57 0 0 + + 0 0 1 + + -1e+16 + 1e+16 + + + 0 + 0 + + + + + base_link + caster_back_right_link + + + base_link + caster_back_left_link + + + base_link + base_scan + -0.064 0 0.121 0 -0 0 + + 0 0 1 + + -1e+16 + 1e+16 + + + + + base_link + camera_link + 0.064 -0.065 0.094 0 -0 0 + + 0 0 1 + + -1e+16 + 1e+16 + + + + + + + 30 + wheel_left_joint + wheel_right_joint + 0.287 + 0.066 + 20 + 1.0 + cmd_vel + 1 + 1 + 0 + odom + odom + base_footprint + + + + ~/out:=joint_states + + 30 + wheel_left_joint + wheel_right_joint + + + + diff --git a/smacc2_sm_reference_library/sm_dancebot_artgallery_ue/worlds/ridgeback_race_empty.world b/smacc2_sm_reference_library/sm_dancebot_artgallery_ue/worlds/ridgeback_race_empty.world new file mode 100644 index 000000000..3ebb28ab2 --- /dev/null +++ b/smacc2_sm_reference_library/sm_dancebot_artgallery_ue/worlds/ridgeback_race_empty.world @@ -0,0 +1,4193 @@ + + + + 1 + 0 0 10 0 -0 0 + 0.8 0.8 0.8 1 + 0.2 0.2 0.2 1 + + 1000 + 0.9 + 0.01 + 0.001 + + -0.5 0.1 -0.9 + + 0 + 0 + 0 + + + + 1 + + + + + 0 0 1 + 100 100 + + + + + + 100 + 50 + + + + + + + + + + + 10 + + + 0 + + + 0 0 1 + 100 100 + + + + + + + + 0 + 0 + + 0 + 0 + 1 + 0 + + + + 0.001 + 1 + 1000 + + + 0.4 0.4 0.4 1 + 0.7 0.7 0.7 1 + 1 + + + EARTH_WGS84 + 0 + 0 + 0 + 0 + + + 1 + 1 + + + + + https://fuel.ignitionrobotics.org/1.0/openrobotics/models/drc practice: orange jersey barrier/2/files/meshes/jersey_barrier.dae + + + + + + + https://fuel.ignitionrobotics.org/1.0/openrobotics/models/drc practice: orange jersey barrier/2/files/meshes/jersey_barrier.dae + + + 10 + + + + + + + + + + + + + + + 0 + 0 + + 0 0 0 0 0 -1.5707 + 0 + 0 + 1 + 0 + + 1 + + + + + https://fuel.ignitionrobotics.org/1.0/openrobotics/models/drc practice: white jersey barrier/2/files/meshes/jersey_barrier.dae + + + + + + + https://fuel.ignitionrobotics.org/1.0/openrobotics/models/drc practice: white jersey barrier/2/files/meshes/jersey_barrier.dae + + + 10 + + + + + + + + + + + + + + + 0 + 0 + + 1.7646 -0.008563 0 0 0 -1.5707 + 0 + 0 + 1 + 0 + + 1 + + + + + https://fuel.ignitionrobotics.org/1.0/openrobotics/models/drc practice: orange jersey barrier/2/files/meshes/jersey_barrier.dae + + + + + + + https://fuel.ignitionrobotics.org/1.0/openrobotics/models/drc practice: orange jersey barrier/2/files/meshes/jersey_barrier.dae + + + 10 + + + + + + + + + + + + + + + 0 + 0 + + 3.52922 -0.016171 0 0 0 -1.5707 + 0 + 0 + 1 + 0 + + 1 + + + + + https://fuel.ignitionrobotics.org/1.0/openrobotics/models/drc practice: white jersey barrier/2/files/meshes/jersey_barrier.dae + + + + + + + https://fuel.ignitionrobotics.org/1.0/openrobotics/models/drc practice: white jersey barrier/2/files/meshes/jersey_barrier.dae + + + 10 + + + + + + + + + + + + + + + 0 + 0 + + 5.13431 0.54837 0 0 0 -0.785398 + 0 + 0 + 1 + 0 + + 1 + + + + + https://fuel.ignitionrobotics.org/1.0/openrobotics/models/drc practice: orange jersey barrier/2/files/meshes/jersey_barrier.dae + + + + + + + https://fuel.ignitionrobotics.org/1.0/openrobotics/models/drc practice: orange jersey barrier/2/files/meshes/jersey_barrier.dae + + + 10 + + + + + + + + + + + + + + + 0 + 0 + + 6.3884 1.79074 0 0 0 -0.785398 + 0 + 0 + 1 + 0 + + 1 + + + + + https://fuel.ignitionrobotics.org/1.0/openrobotics/models/drc practice: white jersey barrier/2/files/meshes/jersey_barrier.dae + + + + + + + https://fuel.ignitionrobotics.org/1.0/openrobotics/models/drc practice: white jersey barrier/2/files/meshes/jersey_barrier.dae + + + 10 + + + + + + + + + + + + + + + 0 + 0 + + 7.6423 3.03258 0 0 0 -0.785398 + 0 + 0 + 1 + 0 + + 1 + + + + + https://fuel.ignitionrobotics.org/1.0/openrobotics/models/drc practice: orange jersey barrier/2/files/meshes/jersey_barrier.dae + + + + + + + https://fuel.ignitionrobotics.org/1.0/openrobotics/models/drc practice: orange jersey barrier/2/files/meshes/jersey_barrier.dae + + + 10 + + + + + + + + + + + + + + + 0 + 0 + + 8.89611 4.27529 0 0 0 -0.785398 + 0 + 0 + 1 + 0 + + 1 + + + + + https://fuel.ignitionrobotics.org/1.0/openrobotics/models/drc practice: white jersey barrier/2/files/meshes/jersey_barrier.dae + + + + + + + https://fuel.ignitionrobotics.org/1.0/openrobotics/models/drc practice: white jersey barrier/2/files/meshes/jersey_barrier.dae + + + 10 + + + + + + + + + + + + + + + 0 + 0 + + 10.1496 5.51614 0 0 0 -0.785398 + 0 + 0 + 1 + 0 + + 1 + + + + + model://blue_cylinder/meshes/cylinder.dae + + + + + + + model://blue_cylinder/meshes/cylinder.dae + + + 10 + + + + + + + + + + + + + + + 0 + 0 + + 11.0288 6.44203 0 0 -0 0 + 0 + 0 + 1 + 0 + + 1 + + + + + model://blue_cylinder/meshes/cylinder.dae + + + + + + + model://blue_cylinder/meshes/cylinder.dae + + + 10 + + + + + + + + + + + + + + + 0 + 0 + + 11.0288 6.44203 0.866 0 -0 0 + 0 + 0 + 1 + 0 + + -1 -6 0 0 -0 0 + + + 1 + + + + + https://fuel.ignitionrobotics.org/1.0/openrobotics/models/drc practice: white jersey barrier/2/files/meshes/jersey_barrier.dae + + + + + + + https://fuel.ignitionrobotics.org/1.0/openrobotics/models/drc practice: white jersey barrier/2/files/meshes/jersey_barrier.dae + + + 10 + + + + + + + + + + + + + + + 0 + 0 + + 0 + 0 + 1 + 0 + + -4 1.55828 0 0 -0 0 + + + 3116 755000000 + 356 954966630 + 1605650970 323926707 + 355029 + + -2.6481 -4.01574 -0.045465 0 0 -0.74752 + 1 1 1 + + 8.54538 -6.87025 -0.045465 0 0 -1.53292 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + + + 3.25439 -7.04543 -0.045465 0 0 -1.53292 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + + + 1.49008 -7.104 -0.045465 0 0 -1.53292 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + + + 9.8196 -6.78892 -0.045465 0 0 -0.74752 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + + + -2.6481 -4.01574 -0.045465 0 0 -2.31822 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + + + 6.78254 -6.92811 -0.045465 0 0 -1.53292 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + + + 5.0182 -6.98712 -0.045465 0 0 -1.53292 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + + + 9.8196 -6.78892 0.820535 0 0 -0.74752 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + + + -0.070844 -6.42684 -0.045465 0 0 -2.31822 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + + + -1.3598 -5.22163 -0.045465 0 0 -2.31822 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + + + + -15.3977 -1.55916 0 0 -0 1.04571 + 1 1 1 + + -15.0829 9.98827 0 0 -0 0.260314 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + + + -13.7448 4.86628 0 0 -0 0.260314 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + + + -13.2984 3.15835 0 0 -0 0.260314 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + + + -15.4433 11.2132 0 0 -0 1.04571 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + + + -15.3977 -1.55916 0 0 0 -0.524988 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + + + -14.6375 8.28162 0 0 -0 0.260314 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + + + -14.1908 6.57377 0 0 -0 0.260314 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + + + -15.4433 11.2132 0.866 0 -0 1.04571 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + + + -13.6146 1.4865 0 0 0 -0.524988 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + + + -14.5057 -0.036576 0 0 0 -0.524988 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + + + + -2.60683 -10.2645 0 0 0 -0.76918 + 1 1 1 + + 8.5222 -13.3608 0 0 0 -1.55458 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + + + 3.22866 -13.4213 0 0 0 -1.55458 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + + + 1.46349 -13.4417 0 0 0 -1.55458 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + + + 9.79789 -13.3071 0 0 0 -0.76918 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + + + -2.60683 -10.2645 0 0 0 -2.33988 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + + + 6.75852 -13.3804 0 0 0 -1.55458 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + + + 4.99332 -13.4012 0 0 0 -1.55458 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + + + 9.79789 -13.3071 0.866 0 0 -0.76918 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + + + -0.082399 -12.7309 0 0 0 -2.33988 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + + + -1.34495 -11.498 0 0 0 -2.33988 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + + + + 10.3347 -5.65314 0 0 -0 1.56441 + 1 1 1 + + 4.88352 4.5315 0 0 -0 0.779009 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + + + 8.58481 0.746571 0 0 -0 0.779009 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + + + 9.81915 -0.515431 0 0 -0 0.779009 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + + + 3.96327 5.4166 0 0 -0 1.56441 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + + + 10.3347 -5.65314 0 0 0 -0.006293 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + + + 6.11634 3.2701 0 0 -0 0.779009 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + + + 7.35101 2.00838 0 0 -0 0.779009 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + + + 3.96327 5.4166 0.866 0 -0 1.56441 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + + + 10.3734 -2.1241 0 0 0 -0.006293 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + + + 10.3545 -3.88863 0 0 0 -0.006293 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + + + + -16.0335 -6.41848 0 0 0 -0.7879 + 1 1 1 + + -4.96438 -9.72253 0 0 0 -1.5733 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + + + -10.2581 -9.68398 0 0 0 -1.5733 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + + + -12.0234 -9.67127 0 0 0 -1.5733 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + + + -3.68791 -9.69271 0 0 0 -0.7879 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + + + -16.0335 -6.41848 0 0 0 -2.3586 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + + + -6.72812 -9.70918 0 0 0 -1.5733 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + + + -8.4934 -9.69692 0 0 0 -1.5733 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + + + -3.68791 -9.69271 0.866 0 0 -0.7879 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + + + -13.5557 -8.93166 0 0 0 -2.3586 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + + + -14.7949 -7.6754 0 0 0 -2.3586 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + + + + -1 16 0 0 -0 0 + 1 1 1 + + 9.2248 10.3902 0 0 0 -2.3562 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + + + 5.49818 14.15 0 0 0 -2.3562 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + + + 4.2559 15.4033 0 0 0 -2.3562 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + + + 10.0288 9.55797 0 0 -0 0 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + + + -1 16 0 0 0 -1.5707 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + + + 7.98256 11.6439 0 0 0 -2.3562 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + + + 6.74031 12.8974 0 0 0 -2.3562 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + + + 10.0288 9.55797 0.866 0 -0 0 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + + + 2.52922 15.9838 0 0 0 -1.5707 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + + + 0.7646 15.9914 0 0 0 -1.5707 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + + + + -14.6681 12.3809 0 0 -0 0.810038 + 1 1 1 + + -3.55516 15.9191 0 0 0 -1.54616 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + + + -8.84779 15.8121 0 0 0 -1.54616 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + + + -10.6121 15.7764 0 0 0 -1.54616 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + + + -2.39802 15.9276 0 0 -0 0.810038 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + + + -14.6681 12.3809 0 0 0 -0.760662 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + + + -5.3197 15.8837 0 0 0 -1.54616 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + + + -7.08412 15.8482 0 0 0 -1.54616 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + + + -2.39802 15.9276 0.866 0 -0 0.810038 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + + + -12.2231 14.926 0 0 0 -0.760662 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + + + -13.4453 13.6531 0 0 0 -0.760662 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + + + + -8.62781 9.94618 0 0 -0 0.2056 + 1 1 1 + + 2.5269 6.54195 0 0 0 -2.1506 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + + + -1.88879 9.46171 0 0 0 -2.1506 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + + + -3.36079 10.4351 0 0 0 -2.1506 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + + + 3.48387 5.89136 0 0 -0 0.205596 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + + + -8.62781 9.94618 0 0 0 -1.3651 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + + + 1.05488 7.51562 0 0 0 -2.1506 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + + + -0.41711 8.4891 0 0 0 -2.1506 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + + + 3.48387 5.89136 0.866 0 -0 0.205596 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + + + -5.16961 10.6508 0 0 0 -1.3651 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + + + -6.89862 10.298 0 0 0 -1.3651 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + + + + -16.7548 -4.64388 0 0 -0 0 + 1 1 1 + + -16.7548 -4.64388 0 0 -0 0 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + + + + -17.1282 -4.13814 0 0 -0 0 + 1 1 1 + + -17.1282 -4.13814 0 0 -0 0 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + + + + 10.3218 -9.21435 0 0 -0 0 + 1 1 1 + + 10.3218 -9.21435 0 0 -0 0 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + + + + 10.3067 -12.7762 0 0 -0 0 + 1 1 1 + + 10.3067 -12.7762 0 0 -0 0 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + + + + 10.3176 -7.4365 0 0 -0 0 + 1 1 1 + + 10.3176 -7.4365 0 0 -0 0 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + + + + 10.3136 -11.0025 0 0 -0 0 + 1 1 1 + + 10.3136 -11.0025 0 0 -0 0 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + + + + -16.2875 -3.11229 0 0 0 -0.509254 + 1 1 1 + + -16.2875 -3.11229 0 0 0 -0.509254 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + + + + -17.3228 -5.14571 0 0 0 -2.31969 + 1 1 1 + + -17.3228 -5.14571 0 0 0 -2.31969 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + + + + 0 0 0 0 -0 0 + 1 1 1 + + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + + + + 0 0 10 0 -0 0 + + + + + 1.03059 2.69165 18.1021 -0 1.5578 -3.12116 + orbit + perspective + + + + 1 + + + + + https://fuel.ignitionrobotics.org/1.0/openrobotics/models/drc practice: orange jersey barrier/2/files/meshes/jersey_barrier.dae + + + + + + + https://fuel.ignitionrobotics.org/1.0/openrobotics/models/drc practice: orange jersey barrier/2/files/meshes/jersey_barrier.dae + + + 10 + + + + + + + + + + + + + + + 0 + 0 + + 0 + 0 + 1 + 0 + + 10 -9.57988 0 0 -0 0 + + + 1 + + + + + https://fuel.ignitionrobotics.org/1.0/openrobotics/models/drc practice: white jersey barrier/2/files/meshes/jersey_barrier.dae + + + + + + + https://fuel.ignitionrobotics.org/1.0/openrobotics/models/drc practice: white jersey barrier/2/files/meshes/jersey_barrier.dae + + + 10 + + + + + + + + + + + + + + + 0 + 0 + + 0 + 0 + 1 + 0 + + 10.4084 -11 0 0 -0 0 + + + 1 + + + + + https://fuel.ignitionrobotics.org/1.0/openrobotics/models/drc practice: orange jersey barrier/2/files/meshes/jersey_barrier.dae + + + + + + + https://fuel.ignitionrobotics.org/1.0/openrobotics/models/drc practice: orange jersey barrier/2/files/meshes/jersey_barrier.dae + + + 10 + + + + + + + + + + + + + + + 0 + 0 + + 0 + 0 + 1 + 0 + + 10.4143 -13 0 0 -0 0 + + + 1 + 1 + + + + + https://fuel.ignitionrobotics.org/1.0/openrobotics/models/drc practice: orange jersey barrier/2/files/meshes/jersey_barrier.dae + + + + + + + https://fuel.ignitionrobotics.org/1.0/openrobotics/models/drc practice: orange jersey barrier/2/files/meshes/jersey_barrier.dae + + + 10 + + + + + + + + + + + + + + + 0 + 0 + + 0 0 0 0 0 -1.5707 + 0 + 0 + 1 + 0 + + 1 + + + + + https://fuel.ignitionrobotics.org/1.0/openrobotics/models/drc practice: white jersey barrier/2/files/meshes/jersey_barrier.dae + + + + + + + https://fuel.ignitionrobotics.org/1.0/openrobotics/models/drc practice: white jersey barrier/2/files/meshes/jersey_barrier.dae + + + 10 + + + + + + + + + + + + + + + 0 + 0 + + 1.7646 -0.008563 0 0 0 -1.5707 + 0 + 0 + 1 + 0 + + 1 + + + + + https://fuel.ignitionrobotics.org/1.0/openrobotics/models/drc practice: orange jersey barrier/2/files/meshes/jersey_barrier.dae + + + + + + + https://fuel.ignitionrobotics.org/1.0/openrobotics/models/drc practice: orange jersey barrier/2/files/meshes/jersey_barrier.dae + + + 10 + + + + + + + + + + + + + + + 0 + 0 + + 3.52922 -0.016171 0 0 0 -1.5707 + 0 + 0 + 1 + 0 + + 1 + + + + + https://fuel.ignitionrobotics.org/1.0/openrobotics/models/drc practice: white jersey barrier/2/files/meshes/jersey_barrier.dae + + + + + + + https://fuel.ignitionrobotics.org/1.0/openrobotics/models/drc practice: white jersey barrier/2/files/meshes/jersey_barrier.dae + + + 10 + + + + + + + + + + + + + + + 0 + 0 + + 5.13431 0.54837 0 0 0 -0.785398 + 0 + 0 + 1 + 0 + + 1 + + + + + https://fuel.ignitionrobotics.org/1.0/openrobotics/models/drc practice: orange jersey barrier/2/files/meshes/jersey_barrier.dae + + + + + + + https://fuel.ignitionrobotics.org/1.0/openrobotics/models/drc practice: orange jersey barrier/2/files/meshes/jersey_barrier.dae + + + 10 + + + + + + + + + + + + + + + 0 + 0 + + 6.3884 1.79074 0 0 0 -0.785398 + 0 + 0 + 1 + 0 + + 1 + + + + + https://fuel.ignitionrobotics.org/1.0/openrobotics/models/drc practice: white jersey barrier/2/files/meshes/jersey_barrier.dae + + + + + + + https://fuel.ignitionrobotics.org/1.0/openrobotics/models/drc practice: white jersey barrier/2/files/meshes/jersey_barrier.dae + + + 10 + + + + + + + + + + + + + + + 0 + 0 + + 7.6423 3.03258 0 0 0 -0.785398 + 0 + 0 + 1 + 0 + + 1 + + + + + https://fuel.ignitionrobotics.org/1.0/openrobotics/models/drc practice: orange jersey barrier/2/files/meshes/jersey_barrier.dae + + + + + + + https://fuel.ignitionrobotics.org/1.0/openrobotics/models/drc practice: orange jersey barrier/2/files/meshes/jersey_barrier.dae + + + 10 + + + + + + + + + + + + + + + 0 + 0 + + 8.89611 4.27529 0 0 0 -0.785398 + 0 + 0 + 1 + 0 + + 1 + + + + + https://fuel.ignitionrobotics.org/1.0/openrobotics/models/drc practice: white jersey barrier/2/files/meshes/jersey_barrier.dae + + + + + + + https://fuel.ignitionrobotics.org/1.0/openrobotics/models/drc practice: white jersey barrier/2/files/meshes/jersey_barrier.dae + + + 10 + + + + + + + + + + + + + + + 0 + 0 + + 10.1496 5.51614 0 0 0 -0.785398 + 0 + 0 + 1 + 0 + + 1 + + + + + model://blue_cylinder/meshes/cylinder.dae + + + + + + + model://blue_cylinder/meshes/cylinder.dae + + + 10 + + + + + + + + + + + + + + + 0 + 0 + + 11.0288 6.44203 0 0 -0 0 + 0 + 0 + 1 + 0 + + 1 + + + + + model://blue_cylinder/meshes/cylinder.dae + + + + + + + model://blue_cylinder/meshes/cylinder.dae + + + 10 + + + + + + + + + + + + + + + 0 + 0 + + 11.0288 6.44203 0.866 0 -0 0 + 0 + 0 + 1 + 0 + + -1.5566 -14 0 0 -0 0 + + + 1 + 1 + + + + + https://fuel.ignitionrobotics.org/1.0/openrobotics/models/drc practice: orange jersey barrier/2/files/meshes/jersey_barrier.dae + + + + + + + https://fuel.ignitionrobotics.org/1.0/openrobotics/models/drc practice: orange jersey barrier/2/files/meshes/jersey_barrier.dae + + + 10 + + + + + + + + + + + + + + + 0 + 0 + + 0 0 0 0 0 -1.5707 + 0 + 0 + 1 + 0 + + 1 + + + + + https://fuel.ignitionrobotics.org/1.0/openrobotics/models/drc practice: white jersey barrier/2/files/meshes/jersey_barrier.dae + + + + + + + https://fuel.ignitionrobotics.org/1.0/openrobotics/models/drc practice: white jersey barrier/2/files/meshes/jersey_barrier.dae + + + 10 + + + + + + + + + + + + + + + 0 + 0 + + 1.7646 -0.008563 0 0 0 -1.5707 + 0 + 0 + 1 + 0 + + 1 + + + + + https://fuel.ignitionrobotics.org/1.0/openrobotics/models/drc practice: orange jersey barrier/2/files/meshes/jersey_barrier.dae + + + + + + + https://fuel.ignitionrobotics.org/1.0/openrobotics/models/drc practice: orange jersey barrier/2/files/meshes/jersey_barrier.dae + + + 10 + + + + + + + + + + + + + + + 0 + 0 + + 3.52922 -0.016171 0 0 0 -1.5707 + 0 + 0 + 1 + 0 + + 1 + + + + + https://fuel.ignitionrobotics.org/1.0/openrobotics/models/drc practice: white jersey barrier/2/files/meshes/jersey_barrier.dae + + + + + + + https://fuel.ignitionrobotics.org/1.0/openrobotics/models/drc practice: white jersey barrier/2/files/meshes/jersey_barrier.dae + + + 10 + + + + + + + + + + + + + + + 0 + 0 + + 5.13431 0.54837 0 0 0 -0.785398 + 0 + 0 + 1 + 0 + + 1 + + + + + https://fuel.ignitionrobotics.org/1.0/openrobotics/models/drc practice: orange jersey barrier/2/files/meshes/jersey_barrier.dae + + + + + + + https://fuel.ignitionrobotics.org/1.0/openrobotics/models/drc practice: orange jersey barrier/2/files/meshes/jersey_barrier.dae + + + 10 + + + + + + + + + + + + + + + 0 + 0 + + 6.3884 1.79074 0 0 0 -0.785398 + 0 + 0 + 1 + 0 + + 1 + + + + + https://fuel.ignitionrobotics.org/1.0/openrobotics/models/drc practice: white jersey barrier/2/files/meshes/jersey_barrier.dae + + + + + + + https://fuel.ignitionrobotics.org/1.0/openrobotics/models/drc practice: white jersey barrier/2/files/meshes/jersey_barrier.dae + + + 10 + + + + + + + + + + + + + + + 0 + 0 + + 7.6423 3.03258 0 0 0 -0.785398 + 0 + 0 + 1 + 0 + + 1 + + + + + https://fuel.ignitionrobotics.org/1.0/openrobotics/models/drc practice: orange jersey barrier/2/files/meshes/jersey_barrier.dae + + + + + + + https://fuel.ignitionrobotics.org/1.0/openrobotics/models/drc practice: orange jersey barrier/2/files/meshes/jersey_barrier.dae + + + 10 + + + + + + + + + + + + + + + 0 + 0 + + 8.89611 4.27529 0 0 0 -0.785398 + 0 + 0 + 1 + 0 + + 1 + + + + + https://fuel.ignitionrobotics.org/1.0/openrobotics/models/drc practice: white jersey barrier/2/files/meshes/jersey_barrier.dae + + + + + + + https://fuel.ignitionrobotics.org/1.0/openrobotics/models/drc practice: white jersey barrier/2/files/meshes/jersey_barrier.dae + + + 10 + + + + + + + + + + + + + + + 0 + 0 + + 10.1496 5.51614 0 0 0 -0.785398 + 0 + 0 + 1 + 0 + + 1 + + + + + model://blue_cylinder/meshes/cylinder.dae + + + + + + + model://blue_cylinder/meshes/cylinder.dae + + + 10 + + + + + + + + + + + + + + + 0 + 0 + + 11.0288 6.44203 0 0 -0 0 + 0 + 0 + 1 + 0 + + 1 + + + + + model://blue_cylinder/meshes/cylinder.dae + + + + + + + model://blue_cylinder/meshes/cylinder.dae + + + 10 + + + + + + + + + + + + + + + 0 + 0 + + 11.0288 6.44203 0.866 0 -0 0 + 0 + 0 + 1 + 0 + + -1 -1 0 0 -0 0 + + + 1 + 1 + + + + + https://fuel.ignitionrobotics.org/1.0/openrobotics/models/drc practice: orange jersey barrier/2/files/meshes/jersey_barrier.dae + + + + + + + https://fuel.ignitionrobotics.org/1.0/openrobotics/models/drc practice: orange jersey barrier/2/files/meshes/jersey_barrier.dae + + + 10 + + + + + + + + + + + + + + + 0 + 0 + + 0 0 0 0 0 -1.5707 + 0 + 0 + 1 + 0 + + 1 + + + + + https://fuel.ignitionrobotics.org/1.0/openrobotics/models/drc practice: white jersey barrier/2/files/meshes/jersey_barrier.dae + + + + + + + https://fuel.ignitionrobotics.org/1.0/openrobotics/models/drc practice: white jersey barrier/2/files/meshes/jersey_barrier.dae + + + 10 + + + + + + + + + + + + + + + 0 + 0 + + 1.7646 -0.008563 0 0 0 -1.5707 + 0 + 0 + 1 + 0 + + 1 + + + + + https://fuel.ignitionrobotics.org/1.0/openrobotics/models/drc practice: orange jersey barrier/2/files/meshes/jersey_barrier.dae + + + + + + + https://fuel.ignitionrobotics.org/1.0/openrobotics/models/drc practice: orange jersey barrier/2/files/meshes/jersey_barrier.dae + + + 10 + + + + + + + + + + + + + + + 0 + 0 + + 3.52922 -0.016171 0 0 0 -1.5707 + 0 + 0 + 1 + 0 + + 1 + + + + + https://fuel.ignitionrobotics.org/1.0/openrobotics/models/drc practice: white jersey barrier/2/files/meshes/jersey_barrier.dae + + + + + + + https://fuel.ignitionrobotics.org/1.0/openrobotics/models/drc practice: white jersey barrier/2/files/meshes/jersey_barrier.dae + + + 10 + + + + + + + + + + + + + + + 0 + 0 + + 5.2559 -0.59667 0 0 0 -2.3562 + 0 + 0 + 1 + 0 + + 1 + + + + + https://fuel.ignitionrobotics.org/1.0/openrobotics/models/drc practice: orange jersey barrier/2/files/meshes/jersey_barrier.dae + + + + + + + https://fuel.ignitionrobotics.org/1.0/openrobotics/models/drc practice: orange jersey barrier/2/files/meshes/jersey_barrier.dae + + + 10 + + + + + + + + + + + + + + + 0 + 0 + + 6.49818 -1.85004 0 0 0 -2.3562 + 0 + 0 + 1 + 0 + + 1 + + + + + https://fuel.ignitionrobotics.org/1.0/openrobotics/models/drc practice: white jersey barrier/2/files/meshes/jersey_barrier.dae + + + + + + + https://fuel.ignitionrobotics.org/1.0/openrobotics/models/drc practice: white jersey barrier/2/files/meshes/jersey_barrier.dae + + + 10 + + + + + + + + + + + + + + + 0 + 0 + + 7.74031 -3.10261 0 0 0 -2.3562 + 0 + 0 + 1 + 0 + + 1 + + + + + https://fuel.ignitionrobotics.org/1.0/openrobotics/models/drc practice: orange jersey barrier/2/files/meshes/jersey_barrier.dae + + + + + + + https://fuel.ignitionrobotics.org/1.0/openrobotics/models/drc practice: orange jersey barrier/2/files/meshes/jersey_barrier.dae + + + 10 + + + + + + + + + + + + + + + 0 + 0 + + 8.98256 -4.3561 0 0 0 -2.3562 + 0 + 0 + 1 + 0 + + 1 + + + + + https://fuel.ignitionrobotics.org/1.0/openrobotics/models/drc practice: white jersey barrier/2/files/meshes/jersey_barrier.dae + + + + + + + https://fuel.ignitionrobotics.org/1.0/openrobotics/models/drc practice: white jersey barrier/2/files/meshes/jersey_barrier.dae + + + 10 + + + + + + + + + + + + + + + 0 + 0 + + 10.2248 -5.60978 0 0 0 -2.3562 + 0 + 0 + 1 + 0 + + 1 + + + + + model://blue_cylinder/meshes/cylinder.dae + + + + + + + model://blue_cylinder/meshes/cylinder.dae + + + 10 + + + + + + + + + + + + + + + 0 + 0 + + 11.0288 -6.44203 0 0 -0 0 + 0 + 0 + 1 + 0 + + 1 + + + + + model://blue_cylinder/meshes/cylinder.dae + + + + + + + model://blue_cylinder/meshes/cylinder.dae + + + 10 + + + + + + + + + + + + + + + 0 + 0 + + 11.0288 -6.44203 0.866 0 -0 0 + 0 + 0 + 1 + 0 + + -1 16 0 0 -0 0 + + + 1 + 1 + + + + + https://fuel.ignitionrobotics.org/1.0/openrobotics/models/drc practice: orange jersey barrier/2/files/meshes/jersey_barrier.dae + + + + + + + https://fuel.ignitionrobotics.org/1.0/openrobotics/models/drc practice: orange jersey barrier/2/files/meshes/jersey_barrier.dae + + + 10 + + + + + + + + + + + + + + + 0 + 0 + + 0 0 0 0 0 -1.5707 + 0 + 0 + 1 + 0 + + 1 + + + + + https://fuel.ignitionrobotics.org/1.0/openrobotics/models/drc practice: white jersey barrier/2/files/meshes/jersey_barrier.dae + + + + + + + https://fuel.ignitionrobotics.org/1.0/openrobotics/models/drc practice: white jersey barrier/2/files/meshes/jersey_barrier.dae + + + 10 + + + + + + + + + + + + + + + 0 + 0 + + 1.7646 -0.008563 0 0 0 -1.5707 + 0 + 0 + 1 + 0 + + 1 + + + + + https://fuel.ignitionrobotics.org/1.0/openrobotics/models/drc practice: orange jersey barrier/2/files/meshes/jersey_barrier.dae + + + + + + + https://fuel.ignitionrobotics.org/1.0/openrobotics/models/drc practice: orange jersey barrier/2/files/meshes/jersey_barrier.dae + + + 10 + + + + + + + + + + + + + + + 0 + 0 + + 3.52922 -0.016171 0 0 0 -1.5707 + 0 + 0 + 1 + 0 + + 1 + + + + + https://fuel.ignitionrobotics.org/1.0/openrobotics/models/drc practice: white jersey barrier/2/files/meshes/jersey_barrier.dae + + + + + + + https://fuel.ignitionrobotics.org/1.0/openrobotics/models/drc practice: white jersey barrier/2/files/meshes/jersey_barrier.dae + + + 10 + + + + + + + + + + + + + + + 0 + 0 + + 5.13431 0.54837 0 0 0 -0.785398 + 0 + 0 + 1 + 0 + + 1 + + + + + https://fuel.ignitionrobotics.org/1.0/openrobotics/models/drc practice: orange jersey barrier/2/files/meshes/jersey_barrier.dae + + + + + + + https://fuel.ignitionrobotics.org/1.0/openrobotics/models/drc practice: orange jersey barrier/2/files/meshes/jersey_barrier.dae + + + 10 + + + + + + + + + + + + + + + 0 + 0 + + 6.3884 1.79074 0 0 0 -0.785398 + 0 + 0 + 1 + 0 + + 1 + + + + + https://fuel.ignitionrobotics.org/1.0/openrobotics/models/drc practice: white jersey barrier/2/files/meshes/jersey_barrier.dae + + + + + + + https://fuel.ignitionrobotics.org/1.0/openrobotics/models/drc practice: white jersey barrier/2/files/meshes/jersey_barrier.dae + + + 10 + + + + + + + + + + + + + + + 0 + 0 + + 7.6423 3.03258 0 0 0 -0.785398 + 0 + 0 + 1 + 0 + + 1 + + + + + https://fuel.ignitionrobotics.org/1.0/openrobotics/models/drc practice: orange jersey barrier/2/files/meshes/jersey_barrier.dae + + + + + + + https://fuel.ignitionrobotics.org/1.0/openrobotics/models/drc practice: orange jersey barrier/2/files/meshes/jersey_barrier.dae + + + 10 + + + + + + + + + + + + + + + 0 + 0 + + 8.89611 4.27529 0 0 0 -0.785398 + 0 + 0 + 1 + 0 + + 1 + + + + + https://fuel.ignitionrobotics.org/1.0/openrobotics/models/drc practice: white jersey barrier/2/files/meshes/jersey_barrier.dae + + + + + + + https://fuel.ignitionrobotics.org/1.0/openrobotics/models/drc practice: white jersey barrier/2/files/meshes/jersey_barrier.dae + + + 10 + + + + + + + + + + + + + + + 0 + 0 + + 10.1496 5.51614 0 0 0 -0.785398 + 0 + 0 + 1 + 0 + + 1 + + + + + model://blue_cylinder/meshes/cylinder.dae + + + + + + + model://blue_cylinder/meshes/cylinder.dae + + + 10 + + + + + + + + + + + + + + + 0 + 0 + + 11.0288 6.44203 0 0 -0 0 + 0 + 0 + 1 + 0 + + 1 + + + + + model://blue_cylinder/meshes/cylinder.dae + + + + + + + model://blue_cylinder/meshes/cylinder.dae + + + 10 + + + + + + + + + + + + + + + 0 + 0 + + 11.0288 6.44203 0.866 0 -0 0 + 0 + 0 + 1 + 0 + + -14.5593 -16 0 0 -0 0 + + + 1 + 1 + + + + + https://fuel.ignitionrobotics.org/1.0/openrobotics/models/drc practice: orange jersey barrier/2/files/meshes/jersey_barrier.dae + + + + + + + https://fuel.ignitionrobotics.org/1.0/openrobotics/models/drc practice: orange jersey barrier/2/files/meshes/jersey_barrier.dae + + + 10 + + + + + + + + + + + + + + + 0 + 0 + + 0 0 0 0 0 -1.5707 + 0 + 0 + 1 + 0 + + 1 + + + + + https://fuel.ignitionrobotics.org/1.0/openrobotics/models/drc practice: white jersey barrier/2/files/meshes/jersey_barrier.dae + + + + + + + https://fuel.ignitionrobotics.org/1.0/openrobotics/models/drc practice: white jersey barrier/2/files/meshes/jersey_barrier.dae + + + 10 + + + + + + + + + + + + + + + 0 + 0 + + 1.7646 -0.008563 0 0 0 -1.5707 + 0 + 0 + 1 + 0 + + 1 + + + + + https://fuel.ignitionrobotics.org/1.0/openrobotics/models/drc practice: orange jersey barrier/2/files/meshes/jersey_barrier.dae + + + + + + + https://fuel.ignitionrobotics.org/1.0/openrobotics/models/drc practice: orange jersey barrier/2/files/meshes/jersey_barrier.dae + + + 10 + + + + + + + + + + + + + + + 0 + 0 + + 3.52922 -0.016171 0 0 0 -1.5707 + 0 + 0 + 1 + 0 + + 1 + + + + + https://fuel.ignitionrobotics.org/1.0/openrobotics/models/drc practice: white jersey barrier/2/files/meshes/jersey_barrier.dae + + + + + + + https://fuel.ignitionrobotics.org/1.0/openrobotics/models/drc practice: white jersey barrier/2/files/meshes/jersey_barrier.dae + + + 10 + + + + + + + + + + + + + + + 0 + 0 + + 5.2559 -0.59667 0 0 0 -2.3562 + 0 + 0 + 1 + 0 + + 1 + + + + + https://fuel.ignitionrobotics.org/1.0/openrobotics/models/drc practice: orange jersey barrier/2/files/meshes/jersey_barrier.dae + + + + + + + https://fuel.ignitionrobotics.org/1.0/openrobotics/models/drc practice: orange jersey barrier/2/files/meshes/jersey_barrier.dae + + + 10 + + + + + + + + + + + + + + + 0 + 0 + + 6.49818 -1.85004 0 0 0 -2.3562 + 0 + 0 + 1 + 0 + + 1 + + + + + https://fuel.ignitionrobotics.org/1.0/openrobotics/models/drc practice: white jersey barrier/2/files/meshes/jersey_barrier.dae + + + + + + + https://fuel.ignitionrobotics.org/1.0/openrobotics/models/drc practice: white jersey barrier/2/files/meshes/jersey_barrier.dae + + + 10 + + + + + + + + + + + + + + + 0 + 0 + + 7.74031 -3.10261 0 0 0 -2.3562 + 0 + 0 + 1 + 0 + + 1 + + + + + https://fuel.ignitionrobotics.org/1.0/openrobotics/models/drc practice: orange jersey barrier/2/files/meshes/jersey_barrier.dae + + + + + + + https://fuel.ignitionrobotics.org/1.0/openrobotics/models/drc practice: orange jersey barrier/2/files/meshes/jersey_barrier.dae + + + 10 + + + + + + + + + + + + + + + 0 + 0 + + 8.98256 -4.3561 0 0 0 -2.3562 + 0 + 0 + 1 + 0 + + 1 + + + + + https://fuel.ignitionrobotics.org/1.0/openrobotics/models/drc practice: white jersey barrier/2/files/meshes/jersey_barrier.dae + + + + + + + https://fuel.ignitionrobotics.org/1.0/openrobotics/models/drc practice: white jersey barrier/2/files/meshes/jersey_barrier.dae + + + 10 + + + + + + + + + + + + + + + 0 + 0 + + 10.2248 -5.60978 0 0 0 -2.3562 + 0 + 0 + 1 + 0 + + 1 + + + + + model://blue_cylinder/meshes/cylinder.dae + + + + + + + model://blue_cylinder/meshes/cylinder.dae + + + 10 + + + + + + + + + + + + + + + 0 + 0 + + 11.0288 -6.44203 0 0 -0 0 + 0 + 0 + 1 + 0 + + 1 + + + + + model://blue_cylinder/meshes/cylinder.dae + + + + + + + model://blue_cylinder/meshes/cylinder.dae + + + 10 + + + + + + + + + + + + + + + 0 + 0 + + 11.0288 -6.44203 0.866 0 -0 0 + 0 + 0 + 1 + 0 + + 25 -15 0 0 -0 0 + + + 1 + 1 + + + + + https://fuel.ignitionrobotics.org/1.0/openrobotics/models/drc practice: orange jersey barrier/2/files/meshes/jersey_barrier.dae + + + + + + + https://fuel.ignitionrobotics.org/1.0/openrobotics/models/drc practice: orange jersey barrier/2/files/meshes/jersey_barrier.dae + + + 10 + + + + + + + + + + + + + + + 0 + 0 + + 0 0 0 0 0 -1.5707 + 0 + 0 + 1 + 0 + + 1 + + + + + https://fuel.ignitionrobotics.org/1.0/openrobotics/models/drc practice: white jersey barrier/2/files/meshes/jersey_barrier.dae + + + + + + + https://fuel.ignitionrobotics.org/1.0/openrobotics/models/drc practice: white jersey barrier/2/files/meshes/jersey_barrier.dae + + + 10 + + + + + + + + + + + + + + + 0 + 0 + + 1.7646 -0.008563 0 0 0 -1.5707 + 0 + 0 + 1 + 0 + + 1 + + + + + https://fuel.ignitionrobotics.org/1.0/openrobotics/models/drc practice: orange jersey barrier/2/files/meshes/jersey_barrier.dae + + + + + + + https://fuel.ignitionrobotics.org/1.0/openrobotics/models/drc practice: orange jersey barrier/2/files/meshes/jersey_barrier.dae + + + 10 + + + + + + + + + + + + + + + 0 + 0 + + 3.52922 -0.016171 0 0 0 -1.5707 + 0 + 0 + 1 + 0 + + 1 + + + + + https://fuel.ignitionrobotics.org/1.0/openrobotics/models/drc practice: white jersey barrier/2/files/meshes/jersey_barrier.dae + + + + + + + https://fuel.ignitionrobotics.org/1.0/openrobotics/models/drc practice: white jersey barrier/2/files/meshes/jersey_barrier.dae + + + 10 + + + + + + + + + + + + + + + 0 + 0 + + 5.13431 0.54837 0 0 0 -0.785398 + 0 + 0 + 1 + 0 + + 1 + + + + + https://fuel.ignitionrobotics.org/1.0/openrobotics/models/drc practice: orange jersey barrier/2/files/meshes/jersey_barrier.dae + + + + + + + https://fuel.ignitionrobotics.org/1.0/openrobotics/models/drc practice: orange jersey barrier/2/files/meshes/jersey_barrier.dae + + + 10 + + + + + + + + + + + + + + + 0 + 0 + + 6.3884 1.79074 0 0 0 -0.785398 + 0 + 0 + 1 + 0 + + 1 + + + + + https://fuel.ignitionrobotics.org/1.0/openrobotics/models/drc practice: white jersey barrier/2/files/meshes/jersey_barrier.dae + + + + + + + https://fuel.ignitionrobotics.org/1.0/openrobotics/models/drc practice: white jersey barrier/2/files/meshes/jersey_barrier.dae + + + 10 + + + + + + + + + + + + + + + 0 + 0 + + 7.6423 3.03258 0 0 0 -0.785398 + 0 + 0 + 1 + 0 + + 1 + + + + + https://fuel.ignitionrobotics.org/1.0/openrobotics/models/drc practice: orange jersey barrier/2/files/meshes/jersey_barrier.dae + + + + + + + https://fuel.ignitionrobotics.org/1.0/openrobotics/models/drc practice: orange jersey barrier/2/files/meshes/jersey_barrier.dae + + + 10 + + + + + + + + + + + + + + + 0 + 0 + + 8.89611 4.27529 0 0 0 -0.785398 + 0 + 0 + 1 + 0 + + 1 + + + + + https://fuel.ignitionrobotics.org/1.0/openrobotics/models/drc practice: white jersey barrier/2/files/meshes/jersey_barrier.dae + + + + + + + https://fuel.ignitionrobotics.org/1.0/openrobotics/models/drc practice: white jersey barrier/2/files/meshes/jersey_barrier.dae + + + 10 + + + + + + + + + + + + + + + 0 + 0 + + 10.1496 5.51614 0 0 0 -0.785398 + 0 + 0 + 1 + 0 + + 1 + + + + + model://blue_cylinder/meshes/cylinder.dae + + + + + + + model://blue_cylinder/meshes/cylinder.dae + + + 10 + + + + + + + + + + + + + + + 0 + 0 + + 11.0288 6.44203 0 0 -0 0 + 0 + 0 + 1 + 0 + + 1 + + + + + model://blue_cylinder/meshes/cylinder.dae + + + + + + + model://blue_cylinder/meshes/cylinder.dae + + + 10 + + + + + + + + + + + + + + + 0 + 0 + + 11.0288 6.44203 0.866 0 -0 0 + 0 + 0 + 1 + 0 + + -25 -4 0 0 -0 0 + + + 1 + + + + + https://fuel.ignitionrobotics.org/1.0/openrobotics/models/drc practice: white jersey barrier/2/files/meshes/jersey_barrier.dae + + + + + + + https://fuel.ignitionrobotics.org/1.0/openrobotics/models/drc practice: white jersey barrier/2/files/meshes/jersey_barrier.dae + + + 10 + + + + + + + + + + + + + + + 0 + 0 + + 0 + 0 + 1 + 0 + + -16.4458 -4 0 0 -0 0 + + + 1 + + + + + https://fuel.ignitionrobotics.org/1.0/openrobotics/models/drc practice: white jersey barrier/2/files/meshes/jersey_barrier.dae + + + + + + + https://fuel.ignitionrobotics.org/1.0/openrobotics/models/drc practice: white jersey barrier/2/files/meshes/jersey_barrier.dae + + + 10 + + + + + + + + + + + + + + + 0 + 0 + + 0 + 0 + 1 + 0 + + -17.5466 -5 0 0 -0 0 + + + 1 + + + + + model://blue_cylinder/meshes/cylinder.dae + + + + + + + model://blue_cylinder/meshes/cylinder.dae + + + 10 + + + + + + + + + + + + + + + 0 + 0 + + 0 + 0 + 1 + 0 + + -17 -4.51853 0 0 -0 0 + + + 1 + + + + + model://blue_cylinder/meshes/cylinder.dae + + + + + + + model://blue_cylinder/meshes/cylinder.dae + + + 10 + + + + + + + + + + + + + + + 0 + 0 + + 0 + 0 + 1 + 0 + + -17.425 -4 0 0 -0 0 + + + 1 + 1 + + + + + https://fuel.ignitionrobotics.org/1.0/openrobotics/models/drc practice: orange jersey barrier/2/files/meshes/jersey_barrier.dae + + + + + + + https://fuel.ignitionrobotics.org/1.0/openrobotics/models/drc practice: orange jersey barrier/2/files/meshes/jersey_barrier.dae + + + 10 + + + + + + + + + + + + + + + 0 + 0 + + 0 0 0 0 0 -1.5707 + 0 + 0 + 1 + 0 + + 1 + + + + + https://fuel.ignitionrobotics.org/1.0/openrobotics/models/drc practice: white jersey barrier/2/files/meshes/jersey_barrier.dae + + + + + + + https://fuel.ignitionrobotics.org/1.0/openrobotics/models/drc practice: white jersey barrier/2/files/meshes/jersey_barrier.dae + + + 10 + + + + + + + + + + + + + + + 0 + 0 + + 1.7646 -0.008563 0 0 0 -1.5707 + 0 + 0 + 1 + 0 + + 1 + + + + + https://fuel.ignitionrobotics.org/1.0/openrobotics/models/drc practice: orange jersey barrier/2/files/meshes/jersey_barrier.dae + + + + + + + https://fuel.ignitionrobotics.org/1.0/openrobotics/models/drc practice: orange jersey barrier/2/files/meshes/jersey_barrier.dae + + + 10 + + + + + + + + + + + + + + + 0 + 0 + + 3.52922 -0.016171 0 0 0 -1.5707 + 0 + 0 + 1 + 0 + + 1 + + + + + https://fuel.ignitionrobotics.org/1.0/openrobotics/models/drc practice: white jersey barrier/2/files/meshes/jersey_barrier.dae + + + + + + + https://fuel.ignitionrobotics.org/1.0/openrobotics/models/drc practice: white jersey barrier/2/files/meshes/jersey_barrier.dae + + + 10 + + + + + + + + + + + + + + + 0 + 0 + + 5.2559 -0.59667 0 0 0 -2.3562 + 0 + 0 + 1 + 0 + + 1 + + + + + https://fuel.ignitionrobotics.org/1.0/openrobotics/models/drc practice: orange jersey barrier/2/files/meshes/jersey_barrier.dae + + + + + + + https://fuel.ignitionrobotics.org/1.0/openrobotics/models/drc practice: orange jersey barrier/2/files/meshes/jersey_barrier.dae + + + 10 + + + + + + + + + + + + + + + 0 + 0 + + 6.49818 -1.85004 0 0 0 -2.3562 + 0 + 0 + 1 + 0 + + 1 + + + + + https://fuel.ignitionrobotics.org/1.0/openrobotics/models/drc practice: white jersey barrier/2/files/meshes/jersey_barrier.dae + + + + + + + https://fuel.ignitionrobotics.org/1.0/openrobotics/models/drc practice: white jersey barrier/2/files/meshes/jersey_barrier.dae + + + 10 + + + + + + + + + + + + + + + 0 + 0 + + 7.74031 -3.10261 0 0 0 -2.3562 + 0 + 0 + 1 + 0 + + 1 + + + + + https://fuel.ignitionrobotics.org/1.0/openrobotics/models/drc practice: orange jersey barrier/2/files/meshes/jersey_barrier.dae + + + + + + + https://fuel.ignitionrobotics.org/1.0/openrobotics/models/drc practice: orange jersey barrier/2/files/meshes/jersey_barrier.dae + + + 10 + + + + + + + + + + + + + + + 0 + 0 + + 8.98256 -4.3561 0 0 0 -2.3562 + 0 + 0 + 1 + 0 + + 1 + + + + + https://fuel.ignitionrobotics.org/1.0/openrobotics/models/drc practice: white jersey barrier/2/files/meshes/jersey_barrier.dae + + + + + + + https://fuel.ignitionrobotics.org/1.0/openrobotics/models/drc practice: white jersey barrier/2/files/meshes/jersey_barrier.dae + + + 10 + + + + + + + + + + + + + + + 0 + 0 + + 10.2248 -5.60978 0 0 0 -2.3562 + 0 + 0 + 1 + 0 + + 1 + + + + + model://blue_cylinder/meshes/cylinder.dae + + + + + + + model://blue_cylinder/meshes/cylinder.dae + + + 10 + + + + + + + + + + + + + + + 0 + 0 + + 11.0288 -6.44203 0 0 -0 0 + 0 + 0 + 1 + 0 + + 1 + + + + + model://blue_cylinder/meshes/cylinder.dae + + + + + + + model://blue_cylinder/meshes/cylinder.dae + + + 10 + + + + + + + + + + + + + + + 0 + 0 + + 11.0288 -6.44203 0.866 0 -0 0 + 0 + 0 + 1 + 0 + + -9 10 0 0 -0 0 + + 0 0 -9.8 + 6e-06 2.3e-05 -4.2e-05 + + + + diff --git a/smacc2_sm_reference_library/sm_dancebot_artgallery_ue/worlds/ridgeback_race_no_lidar.world b/smacc2_sm_reference_library/sm_dancebot_artgallery_ue/worlds/ridgeback_race_no_lidar.world new file mode 100644 index 000000000..7a6a32d6e --- /dev/null +++ b/smacc2_sm_reference_library/sm_dancebot_artgallery_ue/worlds/ridgeback_race_no_lidar.world @@ -0,0 +1,4881 @@ + + + + 1 + 0 0 10 0 -0 0 + 0.8 0.8 0.8 1 + 0.2 0.2 0.2 1 + + 1000 + 0.9 + 0.01 + 0.001 + + -0.5 0.1 -0.9 + + 0 + 0 + 0 + + + + 1 + + + + + 0 0 1 + 100 100 + + + + + + 100 + 50 + + + + + + + + + + + 10 + + + 0 + + + 0 0 1 + 100 100 + + + + + + + + 0 + 0 + + 0 + 0 + 1 + 0 + + + + 0.001 + 1 + 1000 + + + 0.4 0.4 0.4 1 + 0.7 0.7 0.7 1 + 1 + + + EARTH_WGS84 + 0 + 0 + 0 + 0 + + + 1 + 1 + + + + + https://fuel.ignitionrobotics.org/1.0/openrobotics/models/drc practice: orange jersey barrier/2/files/meshes/jersey_barrier.dae + + + + + + + https://fuel.ignitionrobotics.org/1.0/openrobotics/models/drc practice: orange jersey barrier/2/files/meshes/jersey_barrier.dae + + + 10 + + + + + + + + + + + + + + + 0 + 0 + + 0 0 0 0 0 -1.5707 + 0 + 0 + 1 + 0 + + 1 + + + + + https://fuel.ignitionrobotics.org/1.0/openrobotics/models/drc practice: white jersey barrier/2/files/meshes/jersey_barrier.dae + + + + + + + https://fuel.ignitionrobotics.org/1.0/openrobotics/models/drc practice: white jersey barrier/2/files/meshes/jersey_barrier.dae + + + 10 + + + + + + + + + + + + + + + 0 + 0 + + 1.7646 -0.008563 0 0 0 -1.5707 + 0 + 0 + 1 + 0 + + 1 + + + + + https://fuel.ignitionrobotics.org/1.0/openrobotics/models/drc practice: orange jersey barrier/2/files/meshes/jersey_barrier.dae + + + + + + + https://fuel.ignitionrobotics.org/1.0/openrobotics/models/drc practice: orange jersey barrier/2/files/meshes/jersey_barrier.dae + + + 10 + + + + + + + + + + + + + + + 0 + 0 + + 3.52922 -0.016171 0 0 0 -1.5707 + 0 + 0 + 1 + 0 + + 1 + + + + + https://fuel.ignitionrobotics.org/1.0/openrobotics/models/drc practice: white jersey barrier/2/files/meshes/jersey_barrier.dae + + + + + + + https://fuel.ignitionrobotics.org/1.0/openrobotics/models/drc practice: white jersey barrier/2/files/meshes/jersey_barrier.dae + + + 10 + + + + + + + + + + + + + + + 0 + 0 + + 5.13431 0.54837 0 0 0 -0.785398 + 0 + 0 + 1 + 0 + + 1 + + + + + https://fuel.ignitionrobotics.org/1.0/openrobotics/models/drc practice: orange jersey barrier/2/files/meshes/jersey_barrier.dae + + + + + + + https://fuel.ignitionrobotics.org/1.0/openrobotics/models/drc practice: orange jersey barrier/2/files/meshes/jersey_barrier.dae + + + 10 + + + + + + + + + + + + + + + 0 + 0 + + 6.3884 1.79074 0 0 0 -0.785398 + 0 + 0 + 1 + 0 + + 1 + + + + + https://fuel.ignitionrobotics.org/1.0/openrobotics/models/drc practice: white jersey barrier/2/files/meshes/jersey_barrier.dae + + + + + + + https://fuel.ignitionrobotics.org/1.0/openrobotics/models/drc practice: white jersey barrier/2/files/meshes/jersey_barrier.dae + + + 10 + + + + + + + + + + + + + + + 0 + 0 + + 7.6423 3.03258 0 0 0 -0.785398 + 0 + 0 + 1 + 0 + + 1 + + + + + https://fuel.ignitionrobotics.org/1.0/openrobotics/models/drc practice: orange jersey barrier/2/files/meshes/jersey_barrier.dae + + + + + + + https://fuel.ignitionrobotics.org/1.0/openrobotics/models/drc practice: orange jersey barrier/2/files/meshes/jersey_barrier.dae + + + 10 + + + + + + + + + + + + + + + 0 + 0 + + 8.89611 4.27529 0 0 0 -0.785398 + 0 + 0 + 1 + 0 + + 1 + + + + + https://fuel.ignitionrobotics.org/1.0/openrobotics/models/drc practice: white jersey barrier/2/files/meshes/jersey_barrier.dae + + + + + + + https://fuel.ignitionrobotics.org/1.0/openrobotics/models/drc practice: white jersey barrier/2/files/meshes/jersey_barrier.dae + + + 10 + + + + + + + + + + + + + + + 0 + 0 + + 10.1496 5.51614 0 0 0 -0.785398 + 0 + 0 + 1 + 0 + + 1 + + + + + model://blue_cylinder/meshes/cylinder.dae + + + + + + + model://blue_cylinder/meshes/cylinder.dae + + + 10 + + + + + + + + + + + + + + + 0 + 0 + + 11.0288 6.44203 0 0 -0 0 + 0 + 0 + 1 + 0 + + 1 + + + + + model://blue_cylinder/meshes/cylinder.dae + + + + + + + model://blue_cylinder/meshes/cylinder.dae + + + 10 + + + + + + + + + + + + + + + 0 + 0 + + 11.0288 6.44203 0.866 0 -0 0 + 0 + 0 + 1 + 0 + + -1 -6 0 0 -0 0 + + + 1 + + + + + https://fuel.ignitionrobotics.org/1.0/openrobotics/models/drc practice: white jersey barrier/2/files/meshes/jersey_barrier.dae + + + + + + + https://fuel.ignitionrobotics.org/1.0/openrobotics/models/drc practice: white jersey barrier/2/files/meshes/jersey_barrier.dae + + + 10 + + + + + + + + + + + + + + + 0 + 0 + + 0 + 0 + 1 + 0 + + -4 1.55828 0 0 -0 0 + + + 3116 755000000 + 356 954966630 + 1605650970 323926707 + 355029 + + -2.6481 -4.01574 -0.045465 0 0 -0.74752 + 1 1 1 + + 8.54538 -6.87025 -0.045465 0 0 -1.53292 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + + + 3.25439 -7.04543 -0.045465 0 0 -1.53292 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + + + 1.49008 -7.104 -0.045465 0 0 -1.53292 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + + + 9.8196 -6.78892 -0.045465 0 0 -0.74752 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + + + -2.6481 -4.01574 -0.045465 0 0 -2.31822 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + + + 6.78254 -6.92811 -0.045465 0 0 -1.53292 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + + + 5.0182 -6.98712 -0.045465 0 0 -1.53292 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + + + 9.8196 -6.78892 0.820535 0 0 -0.74752 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + + + -0.070844 -6.42684 -0.045465 0 0 -2.31822 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + + + -1.3598 -5.22163 -0.045465 0 0 -2.31822 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + + + + -15.3977 -1.55916 0 0 -0 1.04571 + 1 1 1 + + -15.0829 9.98827 0 0 -0 0.260314 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + + + -13.7448 4.86628 0 0 -0 0.260314 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + + + -13.2984 3.15835 0 0 -0 0.260314 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + + + -15.4433 11.2132 0 0 -0 1.04571 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + + + -15.3977 -1.55916 0 0 0 -0.524988 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + + + -14.6375 8.28162 0 0 -0 0.260314 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + + + -14.1908 6.57377 0 0 -0 0.260314 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + + + -15.4433 11.2132 0.866 0 -0 1.04571 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + + + -13.6146 1.4865 0 0 0 -0.524988 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + + + -14.5057 -0.036576 0 0 0 -0.524988 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + + + + -2.60683 -10.2645 0 0 0 -0.76918 + 1 1 1 + + 8.5222 -13.3608 0 0 0 -1.55458 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + + + 3.22866 -13.4213 0 0 0 -1.55458 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + + + 1.46349 -13.4417 0 0 0 -1.55458 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + + + 9.79789 -13.3071 0 0 0 -0.76918 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + + + -2.60683 -10.2645 0 0 0 -2.33988 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + + + 6.75852 -13.3804 0 0 0 -1.55458 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + + + 4.99332 -13.4012 0 0 0 -1.55458 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + + + 9.79789 -13.3071 0.866 0 0 -0.76918 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + + + -0.082399 -12.7309 0 0 0 -2.33988 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + + + -1.34495 -11.498 0 0 0 -2.33988 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + + + + 10.3347 -5.65314 0 0 -0 1.56441 + 1 1 1 + + 4.88352 4.5315 0 0 -0 0.779009 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + + + 8.58481 0.746571 0 0 -0 0.779009 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + + + 9.81915 -0.515431 0 0 -0 0.779009 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + + + 3.96327 5.4166 0 0 -0 1.56441 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + + + 10.3347 -5.65314 0 0 0 -0.006293 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + + + 6.11634 3.2701 0 0 -0 0.779009 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + + + 7.35101 2.00838 0 0 -0 0.779009 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + + + 3.96327 5.4166 0.866 0 -0 1.56441 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + + + 10.3734 -2.1241 0 0 0 -0.006293 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + + + 10.3545 -3.88863 0 0 0 -0.006293 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + + + + -16.0335 -6.41848 0 0 0 -0.7879 + 1 1 1 + + -4.96438 -9.72253 0 0 0 -1.5733 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + + + -10.2581 -9.68398 0 0 0 -1.5733 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + + + -12.0234 -9.67127 0 0 0 -1.5733 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + + + -3.68791 -9.69271 0 0 0 -0.7879 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + + + -16.0335 -6.41848 0 0 0 -2.3586 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + + + -6.72812 -9.70918 0 0 0 -1.5733 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + + + -8.4934 -9.69692 0 0 0 -1.5733 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + + + -3.68791 -9.69271 0.866 0 0 -0.7879 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + + + -13.5557 -8.93166 0 0 0 -2.3586 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + + + -14.7949 -7.6754 0 0 0 -2.3586 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + + + + -1 16 0 0 -0 0 + 1 1 1 + + 9.2248 10.3902 0 0 0 -2.3562 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + + + 5.49818 14.15 0 0 0 -2.3562 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + + + 4.2559 15.4033 0 0 0 -2.3562 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + + + 10.0288 9.55797 0 0 -0 0 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + + + -1 16 0 0 0 -1.5707 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + + + 7.98256 11.6439 0 0 0 -2.3562 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + + + 6.74031 12.8974 0 0 0 -2.3562 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + + + 10.0288 9.55797 0.866 0 -0 0 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + + + 2.52922 15.9838 0 0 0 -1.5707 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + + + 0.7646 15.9914 0 0 0 -1.5707 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + + + + -14.6681 12.3809 0 0 -0 0.810038 + 1 1 1 + + -3.55516 15.9191 0 0 0 -1.54616 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + + + -8.84779 15.8121 0 0 0 -1.54616 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + + + -10.6121 15.7764 0 0 0 -1.54616 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + + + -2.39802 15.9276 0 0 -0 0.810038 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + + + -14.6681 12.3809 0 0 0 -0.760662 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + + + -5.3197 15.8837 0 0 0 -1.54616 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + + + -7.08412 15.8482 0 0 0 -1.54616 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + + + -2.39802 15.9276 0.866 0 -0 0.810038 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + + + -12.2231 14.926 0 0 0 -0.760662 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + + + -13.4453 13.6531 0 0 0 -0.760662 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + + + + -8.62781 9.94618 0 0 -0 0.2056 + 1 1 1 + + 2.5269 6.54195 0 0 0 -2.1506 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + + + -1.88879 9.46171 0 0 0 -2.1506 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + + + -3.36079 10.4351 0 0 0 -2.1506 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + + + 3.48387 5.89136 0 0 -0 0.205596 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + + + -8.62781 9.94618 0 0 0 -1.3651 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + + + 1.05488 7.51562 0 0 0 -2.1506 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + + + -0.41711 8.4891 0 0 0 -2.1506 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + + + 3.48387 5.89136 0.866 0 -0 0.205596 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + + + -5.16961 10.6508 0 0 0 -1.3651 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + + + -6.89862 10.298 0 0 0 -1.3651 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + + + + -16.7548 -4.64388 0 0 -0 0 + 1 1 1 + + -16.7548 -4.64388 0 0 -0 0 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + + + + -17.1282 -4.13814 0 0 -0 0 + 1 1 1 + + -17.1282 -4.13814 0 0 -0 0 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + + + + 10.3218 -9.21435 0 0 -0 0 + 1 1 1 + + 10.3218 -9.21435 0 0 -0 0 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + + + + 10.3067 -12.7762 0 0 -0 0 + 1 1 1 + + 10.3067 -12.7762 0 0 -0 0 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + + + + 10.3176 -7.4365 0 0 -0 0 + 1 1 1 + + 10.3176 -7.4365 0 0 -0 0 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + + + + 10.3136 -11.0025 0 0 -0 0 + 1 1 1 + + 10.3136 -11.0025 0 0 -0 0 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + + + + -16.2875 -3.11229 0 0 0 -0.509254 + 1 1 1 + + -16.2875 -3.11229 0 0 0 -0.509254 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + + + + -17.3228 -5.14571 0 0 0 -2.31969 + 1 1 1 + + -17.3228 -5.14571 0 0 0 -2.31969 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + + + + 0 0 0 0 -0 0 + 1 1 1 + + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + + + + 0.127058 -0.012625 0.008401 -5.7e-05 0.003353 -0.002954 + 1 1 1 + + 0.127058 -0.012625 0.008401 -5.7e-05 0.003353 -0.002954 + 0.001954 -0.000866 -0.061964 -0.008321 0.009978 -0.001386 + 0.001244 0.001765 0.002452 -0.135997 -0.01007 0.000878 + 0.001244 0.001765 0.002452 0 -0 0 + + + 0.127066 -0.012614 0.008412 -2.8e-05 0.003314 -0.002936 + 0.001162 -0.002502 -0.062977 -0.012557 0.01357 -0.00402 + -0.002239 0.004917 -0.002962 -0.159433 0.012841 0.004083 + -0.002239 0.004917 -0.002962 0 -0 0 + + + 0.127057 -0.012617 0.0084 -6.9e-05 0.003501 -0.00297 + 0.002415 -0.00193 -0.061395 -0.005008 -0.011677 0.001501 + -0.003148 0.006146 -0.003368 -0.028835 -0.016126 -0.002503 + -0.000393 0.000768 -0.000421 0 -0 0 + + + 0.127059 -0.012617 0.008403 -5.8e-05 0.003458 -0.002962 + 0.002127 -0.002056 -0.061757 -0.007075 -0.00588 0.000236 + -0.001696 0.001569 -0.002879 -0.058616 -0.009069 -0.001539 + -0.001696 0.001569 -0.002879 0 -0 0 + + + 0.196289 -0.059815 0.115167 -5.7e-05 0.003458 -0.002962 + 0.001535 -0.001423 -0.061086 -0.007193 -0.005937 0.000309 + -0.003549 0.013582 0.00361 -0.058136 -0.009259 -0.001821 + -0.000124 0.000475 0.000126 0 -0 0 + + + -0.049765 0.051908 0.005015 -0.045565 -1.47685 -1.24358 + 0.002355 -0.002164 -0.063759 1.02255 1.31025 -0 + -0.00117 0.003147 -0.007182 0 -0 0 + -1e-06 3e-06 -7e-06 0 -0 0 + + + -0.050165 -0.076075 0.004993 1.90095 0.495873 2.6548 + 0.005449 -0.004628 -0.024591 0.926043 1.08964 0 + -0.023688 0.021829 0.247248 1.35553 -0.826371 3.14159 + -2.4e-05 2.2e-05 0.000247 0 -0 0 + + + 0.060947 -0.012566 -19432.4 0 -0 0 + 0 0 -617.145 0 -0 0 + 0 0 -9.8 0 -0 0 + 0 0 -9.8 0 -0 0 + + + 0.106509 -0.01259 0.041208 3.13937 1.12267 3.13728 + 0.002393 -0.00053 -0.06195 0.008576 0.03027 0.007711 + -0.002374 0.001791 -0.006185 -0.054285 -0.072005 -0.002259 + -0.000237 0.000179 -0.000618 0 -0 0 + + + 0.106061 -0.012586 0.040984 3.13916 1.14848 3.13713 + 0.000902 -0.000269 -0.06003 0.004974 0.030614 0.00275 + -0.001844 0.001728 0.003565 -0.052403 -0.057989 -0.04024 + -0.000184 0.000173 0.000357 0 -0 0 + + + + 0 0 10 0 -0 0 + + + + + 1.03059 2.69165 18.1021 -0 1.5578 -3.12116 + orbit + perspective + + + + 1 + + + + + https://fuel.ignitionrobotics.org/1.0/openrobotics/models/drc practice: orange jersey barrier/2/files/meshes/jersey_barrier.dae + + + + + + + https://fuel.ignitionrobotics.org/1.0/openrobotics/models/drc practice: orange jersey barrier/2/files/meshes/jersey_barrier.dae + + + 10 + + + + + + + + + + + + + + + 0 + 0 + + 0 + 0 + 1 + 0 + + 10 -9.57988 0 0 -0 0 + + + 1 + + + + + https://fuel.ignitionrobotics.org/1.0/openrobotics/models/drc practice: white jersey barrier/2/files/meshes/jersey_barrier.dae + + + + + + + https://fuel.ignitionrobotics.org/1.0/openrobotics/models/drc practice: white jersey barrier/2/files/meshes/jersey_barrier.dae + + + 10 + + + + + + + + + + + + + + + 0 + 0 + + 0 + 0 + 1 + 0 + + 10.4084 -11 0 0 -0 0 + + + 1 + + + + + https://fuel.ignitionrobotics.org/1.0/openrobotics/models/drc practice: orange jersey barrier/2/files/meshes/jersey_barrier.dae + + + + + + + https://fuel.ignitionrobotics.org/1.0/openrobotics/models/drc practice: orange jersey barrier/2/files/meshes/jersey_barrier.dae + + + 10 + + + + + + + + + + + + + + + 0 + 0 + + 0 + 0 + 1 + 0 + + 10.4143 -13 0 0 -0 0 + + + 1 + 1 + + + + + https://fuel.ignitionrobotics.org/1.0/openrobotics/models/drc practice: orange jersey barrier/2/files/meshes/jersey_barrier.dae + + + + + + + https://fuel.ignitionrobotics.org/1.0/openrobotics/models/drc practice: orange jersey barrier/2/files/meshes/jersey_barrier.dae + + + 10 + + + + + + + + + + + + + + + 0 + 0 + + 0 0 0 0 0 -1.5707 + 0 + 0 + 1 + 0 + + 1 + + + + + https://fuel.ignitionrobotics.org/1.0/openrobotics/models/drc practice: white jersey barrier/2/files/meshes/jersey_barrier.dae + + + + + + + https://fuel.ignitionrobotics.org/1.0/openrobotics/models/drc practice: white jersey barrier/2/files/meshes/jersey_barrier.dae + + + 10 + + + + + + + + + + + + + + + 0 + 0 + + 1.7646 -0.008563 0 0 0 -1.5707 + 0 + 0 + 1 + 0 + + 1 + + + + + https://fuel.ignitionrobotics.org/1.0/openrobotics/models/drc practice: orange jersey barrier/2/files/meshes/jersey_barrier.dae + + + + + + + https://fuel.ignitionrobotics.org/1.0/openrobotics/models/drc practice: orange jersey barrier/2/files/meshes/jersey_barrier.dae + + + 10 + + + + + + + + + + + + + + + 0 + 0 + + 3.52922 -0.016171 0 0 0 -1.5707 + 0 + 0 + 1 + 0 + + 1 + + + + + https://fuel.ignitionrobotics.org/1.0/openrobotics/models/drc practice: white jersey barrier/2/files/meshes/jersey_barrier.dae + + + + + + + https://fuel.ignitionrobotics.org/1.0/openrobotics/models/drc practice: white jersey barrier/2/files/meshes/jersey_barrier.dae + + + 10 + + + + + + + + + + + + + + + 0 + 0 + + 5.13431 0.54837 0 0 0 -0.785398 + 0 + 0 + 1 + 0 + + 1 + + + + + https://fuel.ignitionrobotics.org/1.0/openrobotics/models/drc practice: orange jersey barrier/2/files/meshes/jersey_barrier.dae + + + + + + + https://fuel.ignitionrobotics.org/1.0/openrobotics/models/drc practice: orange jersey barrier/2/files/meshes/jersey_barrier.dae + + + 10 + + + + + + + + + + + + + + + 0 + 0 + + 6.3884 1.79074 0 0 0 -0.785398 + 0 + 0 + 1 + 0 + + 1 + + + + + https://fuel.ignitionrobotics.org/1.0/openrobotics/models/drc practice: white jersey barrier/2/files/meshes/jersey_barrier.dae + + + + + + + https://fuel.ignitionrobotics.org/1.0/openrobotics/models/drc practice: white jersey barrier/2/files/meshes/jersey_barrier.dae + + + 10 + + + + + + + + + + + + + + + 0 + 0 + + 7.6423 3.03258 0 0 0 -0.785398 + 0 + 0 + 1 + 0 + + 1 + + + + + https://fuel.ignitionrobotics.org/1.0/openrobotics/models/drc practice: orange jersey barrier/2/files/meshes/jersey_barrier.dae + + + + + + + https://fuel.ignitionrobotics.org/1.0/openrobotics/models/drc practice: orange jersey barrier/2/files/meshes/jersey_barrier.dae + + + 10 + + + + + + + + + + + + + + + 0 + 0 + + 8.89611 4.27529 0 0 0 -0.785398 + 0 + 0 + 1 + 0 + + 1 + + + + + https://fuel.ignitionrobotics.org/1.0/openrobotics/models/drc practice: white jersey barrier/2/files/meshes/jersey_barrier.dae + + + + + + + https://fuel.ignitionrobotics.org/1.0/openrobotics/models/drc practice: white jersey barrier/2/files/meshes/jersey_barrier.dae + + + 10 + + + + + + + + + + + + + + + 0 + 0 + + 10.1496 5.51614 0 0 0 -0.785398 + 0 + 0 + 1 + 0 + + 1 + + + + + model://blue_cylinder/meshes/cylinder.dae + + + + + + + model://blue_cylinder/meshes/cylinder.dae + + + 10 + + + + + + + + + + + + + + + 0 + 0 + + 11.0288 6.44203 0 0 -0 0 + 0 + 0 + 1 + 0 + + 1 + + + + + model://blue_cylinder/meshes/cylinder.dae + + + + + + + model://blue_cylinder/meshes/cylinder.dae + + + 10 + + + + + + + + + + + + + + + 0 + 0 + + 11.0288 6.44203 0.866 0 -0 0 + 0 + 0 + 1 + 0 + + -1.5566 -14 0 0 -0 0 + + + 1 + 1 + + + + + https://fuel.ignitionrobotics.org/1.0/openrobotics/models/drc practice: orange jersey barrier/2/files/meshes/jersey_barrier.dae + + + + + + + https://fuel.ignitionrobotics.org/1.0/openrobotics/models/drc practice: orange jersey barrier/2/files/meshes/jersey_barrier.dae + + + 10 + + + + + + + + + + + + + + + 0 + 0 + + 0 0 0 0 0 -1.5707 + 0 + 0 + 1 + 0 + + 1 + + + + + https://fuel.ignitionrobotics.org/1.0/openrobotics/models/drc practice: white jersey barrier/2/files/meshes/jersey_barrier.dae + + + + + + + https://fuel.ignitionrobotics.org/1.0/openrobotics/models/drc practice: white jersey barrier/2/files/meshes/jersey_barrier.dae + + + 10 + + + + + + + + + + + + + + + 0 + 0 + + 1.7646 -0.008563 0 0 0 -1.5707 + 0 + 0 + 1 + 0 + + 1 + + + + + https://fuel.ignitionrobotics.org/1.0/openrobotics/models/drc practice: orange jersey barrier/2/files/meshes/jersey_barrier.dae + + + + + + + https://fuel.ignitionrobotics.org/1.0/openrobotics/models/drc practice: orange jersey barrier/2/files/meshes/jersey_barrier.dae + + + 10 + + + + + + + + + + + + + + + 0 + 0 + + 3.52922 -0.016171 0 0 0 -1.5707 + 0 + 0 + 1 + 0 + + 1 + + + + + https://fuel.ignitionrobotics.org/1.0/openrobotics/models/drc practice: white jersey barrier/2/files/meshes/jersey_barrier.dae + + + + + + + https://fuel.ignitionrobotics.org/1.0/openrobotics/models/drc practice: white jersey barrier/2/files/meshes/jersey_barrier.dae + + + 10 + + + + + + + + + + + + + + + 0 + 0 + + 5.13431 0.54837 0 0 0 -0.785398 + 0 + 0 + 1 + 0 + + 1 + + + + + https://fuel.ignitionrobotics.org/1.0/openrobotics/models/drc practice: orange jersey barrier/2/files/meshes/jersey_barrier.dae + + + + + + + https://fuel.ignitionrobotics.org/1.0/openrobotics/models/drc practice: orange jersey barrier/2/files/meshes/jersey_barrier.dae + + + 10 + + + + + + + + + + + + + + + 0 + 0 + + 6.3884 1.79074 0 0 0 -0.785398 + 0 + 0 + 1 + 0 + + 1 + + + + + https://fuel.ignitionrobotics.org/1.0/openrobotics/models/drc practice: white jersey barrier/2/files/meshes/jersey_barrier.dae + + + + + + + https://fuel.ignitionrobotics.org/1.0/openrobotics/models/drc practice: white jersey barrier/2/files/meshes/jersey_barrier.dae + + + 10 + + + + + + + + + + + + + + + 0 + 0 + + 7.6423 3.03258 0 0 0 -0.785398 + 0 + 0 + 1 + 0 + + 1 + + + + + https://fuel.ignitionrobotics.org/1.0/openrobotics/models/drc practice: orange jersey barrier/2/files/meshes/jersey_barrier.dae + + + + + + + https://fuel.ignitionrobotics.org/1.0/openrobotics/models/drc practice: orange jersey barrier/2/files/meshes/jersey_barrier.dae + + + 10 + + + + + + + + + + + + + + + 0 + 0 + + 8.89611 4.27529 0 0 0 -0.785398 + 0 + 0 + 1 + 0 + + 1 + + + + + https://fuel.ignitionrobotics.org/1.0/openrobotics/models/drc practice: white jersey barrier/2/files/meshes/jersey_barrier.dae + + + + + + + https://fuel.ignitionrobotics.org/1.0/openrobotics/models/drc practice: white jersey barrier/2/files/meshes/jersey_barrier.dae + + + 10 + + + + + + + + + + + + + + + 0 + 0 + + 10.1496 5.51614 0 0 0 -0.785398 + 0 + 0 + 1 + 0 + + 1 + + + + + model://blue_cylinder/meshes/cylinder.dae + + + + + + + model://blue_cylinder/meshes/cylinder.dae + + + 10 + + + + + + + + + + + + + + + 0 + 0 + + 11.0288 6.44203 0 0 -0 0 + 0 + 0 + 1 + 0 + + 1 + + + + + model://blue_cylinder/meshes/cylinder.dae + + + + + + + model://blue_cylinder/meshes/cylinder.dae + + + 10 + + + + + + + + + + + + + + + 0 + 0 + + 11.0288 6.44203 0.866 0 -0 0 + 0 + 0 + 1 + 0 + + -1 -1 0 0 -0 0 + + + 1 + 1 + + + + + https://fuel.ignitionrobotics.org/1.0/openrobotics/models/drc practice: orange jersey barrier/2/files/meshes/jersey_barrier.dae + + + + + + + https://fuel.ignitionrobotics.org/1.0/openrobotics/models/drc practice: orange jersey barrier/2/files/meshes/jersey_barrier.dae + + + 10 + + + + + + + + + + + + + + + 0 + 0 + + 0 0 0 0 0 -1.5707 + 0 + 0 + 1 + 0 + + 1 + + + + + https://fuel.ignitionrobotics.org/1.0/openrobotics/models/drc practice: white jersey barrier/2/files/meshes/jersey_barrier.dae + + + + + + + https://fuel.ignitionrobotics.org/1.0/openrobotics/models/drc practice: white jersey barrier/2/files/meshes/jersey_barrier.dae + + + 10 + + + + + + + + + + + + + + + 0 + 0 + + 1.7646 -0.008563 0 0 0 -1.5707 + 0 + 0 + 1 + 0 + + 1 + + + + + https://fuel.ignitionrobotics.org/1.0/openrobotics/models/drc practice: orange jersey barrier/2/files/meshes/jersey_barrier.dae + + + + + + + https://fuel.ignitionrobotics.org/1.0/openrobotics/models/drc practice: orange jersey barrier/2/files/meshes/jersey_barrier.dae + + + 10 + + + + + + + + + + + + + + + 0 + 0 + + 3.52922 -0.016171 0 0 0 -1.5707 + 0 + 0 + 1 + 0 + + 1 + + + + + https://fuel.ignitionrobotics.org/1.0/openrobotics/models/drc practice: white jersey barrier/2/files/meshes/jersey_barrier.dae + + + + + + + https://fuel.ignitionrobotics.org/1.0/openrobotics/models/drc practice: white jersey barrier/2/files/meshes/jersey_barrier.dae + + + 10 + + + + + + + + + + + + + + + 0 + 0 + + 5.2559 -0.59667 0 0 0 -2.3562 + 0 + 0 + 1 + 0 + + 1 + + + + + https://fuel.ignitionrobotics.org/1.0/openrobotics/models/drc practice: orange jersey barrier/2/files/meshes/jersey_barrier.dae + + + + + + + https://fuel.ignitionrobotics.org/1.0/openrobotics/models/drc practice: orange jersey barrier/2/files/meshes/jersey_barrier.dae + + + 10 + + + + + + + + + + + + + + + 0 + 0 + + 6.49818 -1.85004 0 0 0 -2.3562 + 0 + 0 + 1 + 0 + + 1 + + + + + https://fuel.ignitionrobotics.org/1.0/openrobotics/models/drc practice: white jersey barrier/2/files/meshes/jersey_barrier.dae + + + + + + + https://fuel.ignitionrobotics.org/1.0/openrobotics/models/drc practice: white jersey barrier/2/files/meshes/jersey_barrier.dae + + + 10 + + + + + + + + + + + + + + + 0 + 0 + + 7.74031 -3.10261 0 0 0 -2.3562 + 0 + 0 + 1 + 0 + + 1 + + + + + https://fuel.ignitionrobotics.org/1.0/openrobotics/models/drc practice: orange jersey barrier/2/files/meshes/jersey_barrier.dae + + + + + + + https://fuel.ignitionrobotics.org/1.0/openrobotics/models/drc practice: orange jersey barrier/2/files/meshes/jersey_barrier.dae + + + 10 + + + + + + + + + + + + + + + 0 + 0 + + 8.98256 -4.3561 0 0 0 -2.3562 + 0 + 0 + 1 + 0 + + 1 + + + + + https://fuel.ignitionrobotics.org/1.0/openrobotics/models/drc practice: white jersey barrier/2/files/meshes/jersey_barrier.dae + + + + + + + https://fuel.ignitionrobotics.org/1.0/openrobotics/models/drc practice: white jersey barrier/2/files/meshes/jersey_barrier.dae + + + 10 + + + + + + + + + + + + + + + 0 + 0 + + 10.2248 -5.60978 0 0 0 -2.3562 + 0 + 0 + 1 + 0 + + 1 + + + + + model://blue_cylinder/meshes/cylinder.dae + + + + + + + model://blue_cylinder/meshes/cylinder.dae + + + 10 + + + + + + + + + + + + + + + 0 + 0 + + 11.0288 -6.44203 0 0 -0 0 + 0 + 0 + 1 + 0 + + 1 + + + + + model://blue_cylinder/meshes/cylinder.dae + + + + + + + model://blue_cylinder/meshes/cylinder.dae + + + 10 + + + + + + + + + + + + + + + 0 + 0 + + 11.0288 -6.44203 0.866 0 -0 0 + 0 + 0 + 1 + 0 + + -1 16 0 0 -0 0 + + + 1 + 1 + + + + + https://fuel.ignitionrobotics.org/1.0/openrobotics/models/drc practice: orange jersey barrier/2/files/meshes/jersey_barrier.dae + + + + + + + https://fuel.ignitionrobotics.org/1.0/openrobotics/models/drc practice: orange jersey barrier/2/files/meshes/jersey_barrier.dae + + + 10 + + + + + + + + + + + + + + + 0 + 0 + + 0 0 0 0 0 -1.5707 + 0 + 0 + 1 + 0 + + 1 + + + + + https://fuel.ignitionrobotics.org/1.0/openrobotics/models/drc practice: white jersey barrier/2/files/meshes/jersey_barrier.dae + + + + + + + https://fuel.ignitionrobotics.org/1.0/openrobotics/models/drc practice: white jersey barrier/2/files/meshes/jersey_barrier.dae + + + 10 + + + + + + + + + + + + + + + 0 + 0 + + 1.7646 -0.008563 0 0 0 -1.5707 + 0 + 0 + 1 + 0 + + 1 + + + + + https://fuel.ignitionrobotics.org/1.0/openrobotics/models/drc practice: orange jersey barrier/2/files/meshes/jersey_barrier.dae + + + + + + + https://fuel.ignitionrobotics.org/1.0/openrobotics/models/drc practice: orange jersey barrier/2/files/meshes/jersey_barrier.dae + + + 10 + + + + + + + + + + + + + + + 0 + 0 + + 3.52922 -0.016171 0 0 0 -1.5707 + 0 + 0 + 1 + 0 + + 1 + + + + + https://fuel.ignitionrobotics.org/1.0/openrobotics/models/drc practice: white jersey barrier/2/files/meshes/jersey_barrier.dae + + + + + + + https://fuel.ignitionrobotics.org/1.0/openrobotics/models/drc practice: white jersey barrier/2/files/meshes/jersey_barrier.dae + + + 10 + + + + + + + + + + + + + + + 0 + 0 + + 5.13431 0.54837 0 0 0 -0.785398 + 0 + 0 + 1 + 0 + + 1 + + + + + https://fuel.ignitionrobotics.org/1.0/openrobotics/models/drc practice: orange jersey barrier/2/files/meshes/jersey_barrier.dae + + + + + + + https://fuel.ignitionrobotics.org/1.0/openrobotics/models/drc practice: orange jersey barrier/2/files/meshes/jersey_barrier.dae + + + 10 + + + + + + + + + + + + + + + 0 + 0 + + 6.3884 1.79074 0 0 0 -0.785398 + 0 + 0 + 1 + 0 + + 1 + + + + + https://fuel.ignitionrobotics.org/1.0/openrobotics/models/drc practice: white jersey barrier/2/files/meshes/jersey_barrier.dae + + + + + + + https://fuel.ignitionrobotics.org/1.0/openrobotics/models/drc practice: white jersey barrier/2/files/meshes/jersey_barrier.dae + + + 10 + + + + + + + + + + + + + + + 0 + 0 + + 7.6423 3.03258 0 0 0 -0.785398 + 0 + 0 + 1 + 0 + + 1 + + + + + https://fuel.ignitionrobotics.org/1.0/openrobotics/models/drc practice: orange jersey barrier/2/files/meshes/jersey_barrier.dae + + + + + + + https://fuel.ignitionrobotics.org/1.0/openrobotics/models/drc practice: orange jersey barrier/2/files/meshes/jersey_barrier.dae + + + 10 + + + + + + + + + + + + + + + 0 + 0 + + 8.89611 4.27529 0 0 0 -0.785398 + 0 + 0 + 1 + 0 + + 1 + + + + + https://fuel.ignitionrobotics.org/1.0/openrobotics/models/drc practice: white jersey barrier/2/files/meshes/jersey_barrier.dae + + + + + + + https://fuel.ignitionrobotics.org/1.0/openrobotics/models/drc practice: white jersey barrier/2/files/meshes/jersey_barrier.dae + + + 10 + + + + + + + + + + + + + + + 0 + 0 + + 10.1496 5.51614 0 0 0 -0.785398 + 0 + 0 + 1 + 0 + + 1 + + + + + model://blue_cylinder/meshes/cylinder.dae + + + + + + + model://blue_cylinder/meshes/cylinder.dae + + + 10 + + + + + + + + + + + + + + + 0 + 0 + + 11.0288 6.44203 0 0 -0 0 + 0 + 0 + 1 + 0 + + 1 + + + + + model://blue_cylinder/meshes/cylinder.dae + + + + + + + model://blue_cylinder/meshes/cylinder.dae + + + 10 + + + + + + + + + + + + + + + 0 + 0 + + 11.0288 6.44203 0.866 0 -0 0 + 0 + 0 + 1 + 0 + + -14.5593 -16 0 0 -0 0 + + + 1 + 1 + + + + + https://fuel.ignitionrobotics.org/1.0/openrobotics/models/drc practice: orange jersey barrier/2/files/meshes/jersey_barrier.dae + + + + + + + https://fuel.ignitionrobotics.org/1.0/openrobotics/models/drc practice: orange jersey barrier/2/files/meshes/jersey_barrier.dae + + + 10 + + + + + + + + + + + + + + + 0 + 0 + + 0 0 0 0 0 -1.5707 + 0 + 0 + 1 + 0 + + 1 + + + + + https://fuel.ignitionrobotics.org/1.0/openrobotics/models/drc practice: white jersey barrier/2/files/meshes/jersey_barrier.dae + + + + + + + https://fuel.ignitionrobotics.org/1.0/openrobotics/models/drc practice: white jersey barrier/2/files/meshes/jersey_barrier.dae + + + 10 + + + + + + + + + + + + + + + 0 + 0 + + 1.7646 -0.008563 0 0 0 -1.5707 + 0 + 0 + 1 + 0 + + 1 + + + + + https://fuel.ignitionrobotics.org/1.0/openrobotics/models/drc practice: orange jersey barrier/2/files/meshes/jersey_barrier.dae + + + + + + + https://fuel.ignitionrobotics.org/1.0/openrobotics/models/drc practice: orange jersey barrier/2/files/meshes/jersey_barrier.dae + + + 10 + + + + + + + + + + + + + + + 0 + 0 + + 3.52922 -0.016171 0 0 0 -1.5707 + 0 + 0 + 1 + 0 + + 1 + + + + + https://fuel.ignitionrobotics.org/1.0/openrobotics/models/drc practice: white jersey barrier/2/files/meshes/jersey_barrier.dae + + + + + + + https://fuel.ignitionrobotics.org/1.0/openrobotics/models/drc practice: white jersey barrier/2/files/meshes/jersey_barrier.dae + + + 10 + + + + + + + + + + + + + + + 0 + 0 + + 5.2559 -0.59667 0 0 0 -2.3562 + 0 + 0 + 1 + 0 + + 1 + + + + + https://fuel.ignitionrobotics.org/1.0/openrobotics/models/drc practice: orange jersey barrier/2/files/meshes/jersey_barrier.dae + + + + + + + https://fuel.ignitionrobotics.org/1.0/openrobotics/models/drc practice: orange jersey barrier/2/files/meshes/jersey_barrier.dae + + + 10 + + + + + + + + + + + + + + + 0 + 0 + + 6.49818 -1.85004 0 0 0 -2.3562 + 0 + 0 + 1 + 0 + + 1 + + + + + https://fuel.ignitionrobotics.org/1.0/openrobotics/models/drc practice: white jersey barrier/2/files/meshes/jersey_barrier.dae + + + + + + + https://fuel.ignitionrobotics.org/1.0/openrobotics/models/drc practice: white jersey barrier/2/files/meshes/jersey_barrier.dae + + + 10 + + + + + + + + + + + + + + + 0 + 0 + + 7.74031 -3.10261 0 0 0 -2.3562 + 0 + 0 + 1 + 0 + + 1 + + + + + https://fuel.ignitionrobotics.org/1.0/openrobotics/models/drc practice: orange jersey barrier/2/files/meshes/jersey_barrier.dae + + + + + + + https://fuel.ignitionrobotics.org/1.0/openrobotics/models/drc practice: orange jersey barrier/2/files/meshes/jersey_barrier.dae + + + 10 + + + + + + + + + + + + + + + 0 + 0 + + 8.98256 -4.3561 0 0 0 -2.3562 + 0 + 0 + 1 + 0 + + 1 + + + + + https://fuel.ignitionrobotics.org/1.0/openrobotics/models/drc practice: white jersey barrier/2/files/meshes/jersey_barrier.dae + + + + + + + https://fuel.ignitionrobotics.org/1.0/openrobotics/models/drc practice: white jersey barrier/2/files/meshes/jersey_barrier.dae + + + 10 + + + + + + + + + + + + + + + 0 + 0 + + 10.2248 -5.60978 0 0 0 -2.3562 + 0 + 0 + 1 + 0 + + 1 + + + + + model://blue_cylinder/meshes/cylinder.dae + + + + + + + model://blue_cylinder/meshes/cylinder.dae + + + 10 + + + + + + + + + + + + + + + 0 + 0 + + 11.0288 -6.44203 0 0 -0 0 + 0 + 0 + 1 + 0 + + 1 + + + + + model://blue_cylinder/meshes/cylinder.dae + + + + + + + model://blue_cylinder/meshes/cylinder.dae + + + 10 + + + + + + + + + + + + + + + 0 + 0 + + 11.0288 -6.44203 0.866 0 -0 0 + 0 + 0 + 1 + 0 + + 25 -15 0 0 -0 0 + + + 1 + 1 + + + + + https://fuel.ignitionrobotics.org/1.0/openrobotics/models/drc practice: orange jersey barrier/2/files/meshes/jersey_barrier.dae + + + + + + + https://fuel.ignitionrobotics.org/1.0/openrobotics/models/drc practice: orange jersey barrier/2/files/meshes/jersey_barrier.dae + + + 10 + + + + + + + + + + + + + + + 0 + 0 + + 0 0 0 0 0 -1.5707 + 0 + 0 + 1 + 0 + + 1 + + + + + https://fuel.ignitionrobotics.org/1.0/openrobotics/models/drc practice: white jersey barrier/2/files/meshes/jersey_barrier.dae + + + + + + + https://fuel.ignitionrobotics.org/1.0/openrobotics/models/drc practice: white jersey barrier/2/files/meshes/jersey_barrier.dae + + + 10 + + + + + + + + + + + + + + + 0 + 0 + + 1.7646 -0.008563 0 0 0 -1.5707 + 0 + 0 + 1 + 0 + + 1 + + + + + https://fuel.ignitionrobotics.org/1.0/openrobotics/models/drc practice: orange jersey barrier/2/files/meshes/jersey_barrier.dae + + + + + + + https://fuel.ignitionrobotics.org/1.0/openrobotics/models/drc practice: orange jersey barrier/2/files/meshes/jersey_barrier.dae + + + 10 + + + + + + + + + + + + + + + 0 + 0 + + 3.52922 -0.016171 0 0 0 -1.5707 + 0 + 0 + 1 + 0 + + 1 + + + + + https://fuel.ignitionrobotics.org/1.0/openrobotics/models/drc practice: white jersey barrier/2/files/meshes/jersey_barrier.dae + + + + + + + https://fuel.ignitionrobotics.org/1.0/openrobotics/models/drc practice: white jersey barrier/2/files/meshes/jersey_barrier.dae + + + 10 + + + + + + + + + + + + + + + 0 + 0 + + 5.13431 0.54837 0 0 0 -0.785398 + 0 + 0 + 1 + 0 + + 1 + + + + + https://fuel.ignitionrobotics.org/1.0/openrobotics/models/drc practice: orange jersey barrier/2/files/meshes/jersey_barrier.dae + + + + + + + https://fuel.ignitionrobotics.org/1.0/openrobotics/models/drc practice: orange jersey barrier/2/files/meshes/jersey_barrier.dae + + + 10 + + + + + + + + + + + + + + + 0 + 0 + + 6.3884 1.79074 0 0 0 -0.785398 + 0 + 0 + 1 + 0 + + 1 + + + + + https://fuel.ignitionrobotics.org/1.0/openrobotics/models/drc practice: white jersey barrier/2/files/meshes/jersey_barrier.dae + + + + + + + https://fuel.ignitionrobotics.org/1.0/openrobotics/models/drc practice: white jersey barrier/2/files/meshes/jersey_barrier.dae + + + 10 + + + + + + + + + + + + + + + 0 + 0 + + 7.6423 3.03258 0 0 0 -0.785398 + 0 + 0 + 1 + 0 + + 1 + + + + + https://fuel.ignitionrobotics.org/1.0/openrobotics/models/drc practice: orange jersey barrier/2/files/meshes/jersey_barrier.dae + + + + + + + https://fuel.ignitionrobotics.org/1.0/openrobotics/models/drc practice: orange jersey barrier/2/files/meshes/jersey_barrier.dae + + + 10 + + + + + + + + + + + + + + + 0 + 0 + + 8.89611 4.27529 0 0 0 -0.785398 + 0 + 0 + 1 + 0 + + 1 + + + + + https://fuel.ignitionrobotics.org/1.0/openrobotics/models/drc practice: white jersey barrier/2/files/meshes/jersey_barrier.dae + + + + + + + https://fuel.ignitionrobotics.org/1.0/openrobotics/models/drc practice: white jersey barrier/2/files/meshes/jersey_barrier.dae + + + 10 + + + + + + + + + + + + + + + 0 + 0 + + 10.1496 5.51614 0 0 0 -0.785398 + 0 + 0 + 1 + 0 + + 1 + + + + + model://blue_cylinder/meshes/cylinder.dae + + + + + + + model://blue_cylinder/meshes/cylinder.dae + + + 10 + + + + + + + + + + + + + + + 0 + 0 + + 11.0288 6.44203 0 0 -0 0 + 0 + 0 + 1 + 0 + + 1 + + + + + model://blue_cylinder/meshes/cylinder.dae + + + + + + + model://blue_cylinder/meshes/cylinder.dae + + + 10 + + + + + + + + + + + + + + + 0 + 0 + + 11.0288 6.44203 0.866 0 -0 0 + 0 + 0 + 1 + 0 + + -25 -4 0 0 -0 0 + + + 1 + + + + + https://fuel.ignitionrobotics.org/1.0/openrobotics/models/drc practice: white jersey barrier/2/files/meshes/jersey_barrier.dae + + + + + + + https://fuel.ignitionrobotics.org/1.0/openrobotics/models/drc practice: white jersey barrier/2/files/meshes/jersey_barrier.dae + + + 10 + + + + + + + + + + + + + + + 0 + 0 + + 0 + 0 + 1 + 0 + + -16.4458 -4 0 0 -0 0 + + + 1 + + + + + https://fuel.ignitionrobotics.org/1.0/openrobotics/models/drc practice: white jersey barrier/2/files/meshes/jersey_barrier.dae + + + + + + + https://fuel.ignitionrobotics.org/1.0/openrobotics/models/drc practice: white jersey barrier/2/files/meshes/jersey_barrier.dae + + + 10 + + + + + + + + + + + + + + + 0 + 0 + + 0 + 0 + 1 + 0 + + -17.5466 -5 0 0 -0 0 + + + 1 + + + + + model://blue_cylinder/meshes/cylinder.dae + + + + + + + model://blue_cylinder/meshes/cylinder.dae + + + 10 + + + + + + + + + + + + + + + 0 + 0 + + 0 + 0 + 1 + 0 + + -17 -4.51853 0 0 -0 0 + + + 1 + + + + + model://blue_cylinder/meshes/cylinder.dae + + + + + + + model://blue_cylinder/meshes/cylinder.dae + + + 10 + + + + + + + + + + + + + + + 0 + 0 + + 0 + 0 + 1 + 0 + + -17.425 -4 0 0 -0 0 + + + 1 + 1 + + + + + https://fuel.ignitionrobotics.org/1.0/openrobotics/models/drc practice: orange jersey barrier/2/files/meshes/jersey_barrier.dae + + + + + + + https://fuel.ignitionrobotics.org/1.0/openrobotics/models/drc practice: orange jersey barrier/2/files/meshes/jersey_barrier.dae + + + 10 + + + + + + + + + + + + + + + 0 + 0 + + 0 0 0 0 0 -1.5707 + 0 + 0 + 1 + 0 + + 1 + + + + + https://fuel.ignitionrobotics.org/1.0/openrobotics/models/drc practice: white jersey barrier/2/files/meshes/jersey_barrier.dae + + + + + + + https://fuel.ignitionrobotics.org/1.0/openrobotics/models/drc practice: white jersey barrier/2/files/meshes/jersey_barrier.dae + + + 10 + + + + + + + + + + + + + + + 0 + 0 + + 1.7646 -0.008563 0 0 0 -1.5707 + 0 + 0 + 1 + 0 + + 1 + + + + + https://fuel.ignitionrobotics.org/1.0/openrobotics/models/drc practice: orange jersey barrier/2/files/meshes/jersey_barrier.dae + + + + + + + https://fuel.ignitionrobotics.org/1.0/openrobotics/models/drc practice: orange jersey barrier/2/files/meshes/jersey_barrier.dae + + + 10 + + + + + + + + + + + + + + + 0 + 0 + + 3.52922 -0.016171 0 0 0 -1.5707 + 0 + 0 + 1 + 0 + + 1 + + + + + https://fuel.ignitionrobotics.org/1.0/openrobotics/models/drc practice: white jersey barrier/2/files/meshes/jersey_barrier.dae + + + + + + + https://fuel.ignitionrobotics.org/1.0/openrobotics/models/drc practice: white jersey barrier/2/files/meshes/jersey_barrier.dae + + + 10 + + + + + + + + + + + + + + + 0 + 0 + + 5.2559 -0.59667 0 0 0 -2.3562 + 0 + 0 + 1 + 0 + + 1 + + + + + https://fuel.ignitionrobotics.org/1.0/openrobotics/models/drc practice: orange jersey barrier/2/files/meshes/jersey_barrier.dae + + + + + + + https://fuel.ignitionrobotics.org/1.0/openrobotics/models/drc practice: orange jersey barrier/2/files/meshes/jersey_barrier.dae + + + 10 + + + + + + + + + + + + + + + 0 + 0 + + 6.49818 -1.85004 0 0 0 -2.3562 + 0 + 0 + 1 + 0 + + 1 + + + + + https://fuel.ignitionrobotics.org/1.0/openrobotics/models/drc practice: white jersey barrier/2/files/meshes/jersey_barrier.dae + + + + + + + https://fuel.ignitionrobotics.org/1.0/openrobotics/models/drc practice: white jersey barrier/2/files/meshes/jersey_barrier.dae + + + 10 + + + + + + + + + + + + + + + 0 + 0 + + 7.74031 -3.10261 0 0 0 -2.3562 + 0 + 0 + 1 + 0 + + 1 + + + + + https://fuel.ignitionrobotics.org/1.0/openrobotics/models/drc practice: orange jersey barrier/2/files/meshes/jersey_barrier.dae + + + + + + + https://fuel.ignitionrobotics.org/1.0/openrobotics/models/drc practice: orange jersey barrier/2/files/meshes/jersey_barrier.dae + + + 10 + + + + + + + + + + + + + + + 0 + 0 + + 8.98256 -4.3561 0 0 0 -2.3562 + 0 + 0 + 1 + 0 + + 1 + + + + + https://fuel.ignitionrobotics.org/1.0/openrobotics/models/drc practice: white jersey barrier/2/files/meshes/jersey_barrier.dae + + + + + + + https://fuel.ignitionrobotics.org/1.0/openrobotics/models/drc practice: white jersey barrier/2/files/meshes/jersey_barrier.dae + + + 10 + + + + + + + + + + + + + + + 0 + 0 + + 10.2248 -5.60978 0 0 0 -2.3562 + 0 + 0 + 1 + 0 + + 1 + + + + + model://blue_cylinder/meshes/cylinder.dae + + + + + + + model://blue_cylinder/meshes/cylinder.dae + + + 10 + + + + + + + + + + + + + + + 0 + 0 + + 11.0288 -6.44203 0 0 -0 0 + 0 + 0 + 1 + 0 + + 1 + + + + + model://blue_cylinder/meshes/cylinder.dae + + + + + + + model://blue_cylinder/meshes/cylinder.dae + + + 10 + + + + + + + + + + + + + + + 0 + 0 + + 11.0288 -6.44203 0.866 0 -0 0 + 0 + 0 + 1 + 0 + + -9 10 0 0 -0 0 + + 0 0 -9.8 + 6e-06 2.3e-05 -4.2e-05 + + + + -2 -0.5 0.01 0 -0 0 + + 0 + + 0 0 0 0 -0 0 + + 1 + 0 + 0 + 1 + 0 + 1 + + 1 + + 0 + 0 + + + + -0.064 0 0.048 0 -0 0 + + 0.001 + 0 + 0 + 0.001 + 0 + 0.001 + + 1 + + + -0.064 0 0.048 0 -0 0 + + + 0.265 0.265 0.089 + + + + + + + + + + + + + + + 10 + + + -0.064 0 0 0 -0 0 + + + model://turtlebot3_waffle/meshes/waffle_base.dae + 0.001 0.001 0.001 + + + + 0 + 0 + 0 + + + + 1 + 200 + + + + + 0 + 0.0002 + + + + + 0 + 0.0002 + + + + + 0 + 0.0002 + + + + + + + 0 + 0.017 + + + + + 0 + 0.017 + + + + + 0 + 0.017 + + + + + + + ~/out:=imu + + + + 0 + + 0 0 0 0 -0 0 + + 1 + 0 + 0 + 1 + 0 + 1 + + 1 + + 0 + 0 + + + + -0.052 0 0.111 0 -0 0 + + 0.001 + 0 + 0 + 0.001 + 0 + 0.001 + + 0.125 + + + -0.052 0 0.111 0 -0 0 + + + 0.0508 + 0.055 + + + + + + + + + + + + + + + 10 + + + -0.064 0 0.121 0 -0 0 + + + model://turtlebot3_waffle/meshes/lds.dae + 0.001 0.001 0.001 + + + + + 1 + 0 + -0.064 0 0.121 0 -0 0 + 5 + + + + 360 + 1 + 0 + 6.28 + + + 1 + 0 + 0 + + + + 0.32 + 20.5 + 0.015 + + + gaussian + 0 + 0.01 + + + + + ~/out:=scan + + sensor_msgs/LaserScan + base_scan + + + 0 + 0 + 0 + + + + 0 0.144 0.023 -1.57 0 0 + + 0.001 + 0 + 0 + 0.001 + 0 + 0.001 + + 0.1 + + + 0 0.144 0.023 -1.57 0 0 + + + 0.033 + 0.018 + + + + + + 100000 + 100000 + 0 0 0 + 0 + 0 + + + + + + + + 0 + 0.2 + 100000 + 1 + 0.01 + 0.001 + + + + + 10 + + + 0 0.144 0.023 0 -0 0 + + + model://turtlebot3_waffle/meshes/left_tire.dae + 0.001 0.001 0.001 + + + + 0 + 0 + 0 + + + + 0 -0.144 0.023 -1.57 0 0 + + 0.001 + 0 + 0 + 0.001 + 0 + 0.001 + + 0.1 + + + 0 -0.144 0.023 -1.57 0 0 + + + 0.033 + 0.018 + + + + + + 100000 + 100000 + 0 0 0 + 0 + 0 + + + + + + + + 0 + 0.2 + 100000 + 1 + 0.01 + 0.001 + + + + + 10 + + + 0 -0.144 0.023 0 -0 0 + + + model://turtlebot3_waffle/meshes/right_tire.dae + 0.001 0.001 0.001 + + + + 0 + 0 + 0 + + + -0.177 -0.064 -0.004 0 -0 0 + + 0.001 + + 1e-05 + 0 + 0 + 1e-05 + 0 + 1e-05 + + 0 0 0 0 -0 0 + + + + + 0.005 + + + + + + 0 + 0.2 + 100000 + 1 + 0.01 + 0.001 + + + + + + + + + + + 10 + + 0 + 0 + 0 + + + -0.177 0.064 -0.004 0 -0 0 + + 0.001 + + 1e-05 + 0 + 0 + 1e-05 + 0 + 1e-05 + + 0 0 0 0 -0 0 + + + + + 0.005 + + + + + + 0 + 0.2 + 100000 + 1 + 0.01 + 0.001 + + + + + + + + + + + 10 + + 0 + 0 + 0 + + + + 0.069 -0.047 0.107 0 -0 0 + + 0.001 + 0 + 0 + 0.001 + 0 + 0.001 + + 0.035 + + + 0 0.047 -0.005 0 -0 0 + + + 0.008 0.13 0.022 + + + + + + + + + + + + + + + 10 + + 0.069 -0.047 0.107 0 -0 0 + + 1 + 5 + 0.064 -0.047 0.107 0 -0 0 + + 1.047 + + 320 + 240 + + + 0.1 + 100 + + + + + intel_realsense_r200_depth + camera_depth_frame + 0.07 + 0.001 + 5.0 + + + + 0 0.047 -0.005 0 -0 0 + + + 0.008 0.13 0.022 + + + + + + + + + + + + + + + 10 + + 0.069 -0.047 0.107 0 -0 0 + 0 + 0 + 0 + + + base_footprint + base_link + 0 0 0.01 0 -0 0 + + + base_link + wheel_left_link + 0 0.144 0.023 -1.57 0 0 + + 0 0 1 + + -1e+16 + 1e+16 + + + 0 + 0 + + + + + base_link + wheel_right_link + 0 -0.144 0.023 -1.57 0 0 + + 0 0 1 + + -1e+16 + 1e+16 + + + 0 + 0 + + + + + base_link + caster_back_right_link + + + base_link + caster_back_left_link + + + base_link + base_scan + -0.064 0 0.121 0 -0 0 + + 0 0 1 + + -1e+16 + 1e+16 + + + + + base_link + camera_link + 0.064 -0.065 0.094 0 -0 0 + + 0 0 1 + + -1e+16 + 1e+16 + + + + + camera_link + camera_rgb_frame + 0.005 0.018 0.013 0 -0 0 + + 0 0 1 + + -1e+16 + 1e+16 + + + + + + 30 + wheel_left_joint + wheel_right_joint + 0.287 + 0.066 + 20 + 1.0 + cmd_vel + 1 + 1 + 0 + odom + odom + base_footprint + + + + ~/out:=joint_states + + 30 + wheel_left_joint + wheel_right_joint + + + + diff --git a/smacc2_sm_reference_library/sm_dancebot_ue/CHANGELOG.rst b/smacc2_sm_reference_library/sm_dancebot_ue/CHANGELOG.rst new file mode 100644 index 000000000..d4752cf81 --- /dev/null +++ b/smacc2_sm_reference_library/sm_dancebot_ue/CHANGELOG.rst @@ -0,0 +1,13067 @@ +^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Changelog for package sm_dancebot_ue +^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ + +2.3.6 (2023-03-12) +------------------ + +1.22.1 (2022-11-09) +------------------- +* pre-release +* Contributors: pabloinigoblasco + +* publisher +* Feature/warehouse 3 improvements (#313) + * 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 +* 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 +* Feature/galactic rolling merge (#288) + * 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_dancebot_ue + * 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_dancebot_ue + * 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_dancebot_ue + * 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_dancebot_ue visualizing turtlebot3 (#101) + * Feature/dance bot launch gz lidar choice (#102) + * sm_dancebot_ue 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_dancebot_ue 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_dancebot_ue 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_dancebot_ue 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_dancebot_ue (#125) + * Move method after the method it calls. Otherwise recursion could happen. (#126) + * Feature/dance bot s pattern (#128) + * more changes in sm_dancebot_ue + * polishing sm_dancebot_ue and s-pattern + * noticed typo + Finnaly > Finally + * Feature/dance bot s pattern (#129) + * more changes in sm_dancebot_ue + * polishing sm_dancebot_ue and s-pattern + * more refinement in sm_dancebot_ue + * 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_dancebot_ue + * minor + * Feature/sm dance bot refine 2 (#132) + * more changes in sm_dancebot_ue + * minor + * build fix + * waypoints navigator bug (#133) + * minor tuning to mitigate overshot issue cases + * progress in the sm_dancebot_ue 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/nav2z 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_dancebot_ue + * 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_dancebot_ue + * 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_dancebot_ue + * 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_dancebot_ue + * 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_dancebot_ue visualizing turtlebot3 (#101) + * Feature/dance bot launch gz lidar choice (#102) + * sm_dancebot_ue 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_dancebot_ue 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_dancebot_ue 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_dancebot_ue 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_dancebot_ue (#125) + * Move method after the method it calls. Otherwise recursion could happen. (#126) + * Feature/dance bot s pattern (#128) + * more changes in sm_dancebot_ue + * polishing sm_dancebot_ue and s-pattern + * noticed typo + Finnaly > Finally + * Feature/dance bot s pattern (#129) + * more changes in sm_dancebot_ue + * polishing sm_dancebot_ue and s-pattern + * more refinement in sm_dancebot_ue + * 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_dancebot_ue + * minor + * Feature/sm dance bot refine 2 (#132) + * more changes in sm_dancebot_ue + * minor + * build fix + * waypoints navigator bug (#133) + * minor tuning to mitigate overshot issue cases + * progress in the sm_dancebot_ue 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/nav2z 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_dancebot_ue + * 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_dancebot_ue + * 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_dancebot_ue + * 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_dancebot_ue + * 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_dancebot_ue visualizing turtlebot3 (#101) + * Feature/dance bot launch gz lidar choice (#102) + * sm_dancebot_ue 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_dancebot_ue 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_dancebot_ue 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_dancebot_ue 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_dancebot_ue (#125) + * Move method after the method it calls. Otherwise recursion could happen. (#126) + * Feature/dance bot s pattern (#128) + * more changes in sm_dancebot_ue + * polishing sm_dancebot_ue and s-pattern + * noticed typo + Finnaly > Finally + * Feature/dance bot s pattern (#129) + * more changes in sm_dancebot_ue + * polishing sm_dancebot_ue and s-pattern + * more refinement in sm_dancebot_ue + * 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_dancebot_ue + * minor + * Feature/sm dance bot refine 2 (#132) + * more changes in sm_dancebot_ue + * minor + * build fix + * waypoints navigator bug (#133) + * minor tuning to mitigate overshot issue cases + * progress in the sm_dancebot_ue 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/nav2z 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_dancebot_ue + * 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_dancebot_ue + * 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_dancebot_ue + * 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_dancebot_ue + * 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_dancebot_ue visualizing turtlebot3 (#101) + * Feature/dance bot launch gz lidar choice (#102) + * sm_dancebot_ue 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_dancebot_ue 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_dancebot_ue 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_dancebot_ue 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_dancebot_ue (#125) + * Move method after the method it calls. Otherwise recursion could happen. (#126) + * Feature/dance bot s pattern (#128) + * more changes in sm_dancebot_ue + * polishing sm_dancebot_ue and s-pattern + * noticed typo + Finnaly > Finally + * Feature/dance bot s pattern (#129) + * more changes in sm_dancebot_ue + * polishing sm_dancebot_ue and s-pattern + * more refinement in sm_dancebot_ue + * 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_dancebot_ue + * minor + * Feature/sm dance bot refine 2 (#132) + * more changes in sm_dancebot_ue + * minor + * build fix + * waypoints navigator bug (#133) + * minor tuning to mitigate overshot issue cases + * progress in the sm_dancebot_ue 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/nav2z 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_dancebot_ue + * 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_dancebot_ue + * 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_dancebot_ue + * 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_dancebot_ue + * 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_dancebot_ue visualizing turtlebot3 (#101) + * Feature/dance bot launch gz lidar choice (#102) + * sm_dancebot_ue 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_dancebot_ue 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_dancebot_ue 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_dancebot_ue 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_dancebot_ue (#125) + * Move method after the method it calls. Otherwise recursion could happen. (#126) + * Feature/dance bot s pattern (#128) + * more changes in sm_dancebot_ue + * polishing sm_dancebot_ue and s-pattern + * noticed typo + Finnaly > Finally + * Feature/dance bot s pattern (#129) + * more changes in sm_dancebot_ue + * polishing sm_dancebot_ue and s-pattern + * more refinement in sm_dancebot_ue + * 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_dancebot_ue + * minor + * Feature/sm dance bot refine 2 (#132) + * more changes in sm_dancebot_ue + * minor + * build fix + * waypoints navigator bug (#133) + * minor tuning to mitigate overshot issue cases + * progress in the sm_dancebot_ue 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/nav2z 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_dancebot_ue + * 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_dancebot_ue + * 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_dancebot_ue + * 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_dancebot_ue + * 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_dancebot_ue visualizing turtlebot3 (#101) + * Feature/dance bot launch gz lidar choice (#102) + * sm_dancebot_ue 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_dancebot_ue 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_dancebot_ue 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_dancebot_ue 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_dancebot_ue (#125) + * Move method after the method it calls. Otherwise recursion could happen. (#126) + * Feature/dance bot s pattern (#128) + * more changes in sm_dancebot_ue + * polishing sm_dancebot_ue and s-pattern + * noticed typo + Finnaly > Finally + * Feature/dance bot s pattern (#129) + * more changes in sm_dancebot_ue + * polishing sm_dancebot_ue and s-pattern + * more refinement in sm_dancebot_ue + * 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_dancebot_ue + * minor + * Feature/sm dance bot refine 2 (#132) + * more changes in sm_dancebot_ue + * minor + * build fix + * waypoints navigator bug (#133) + * minor tuning to mitigate overshot issue cases + * progress in the sm_dancebot_ue 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/nav2z 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_dancebot_ue + * 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_dancebot_ue + * 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_dancebot_ue + * 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_dancebot_ue + * 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_dancebot_ue visualizing turtlebot3 (#101) + * Feature/dance bot launch gz lidar choice (#102) + * sm_dancebot_ue 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_dancebot_ue 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_dancebot_ue 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_dancebot_ue 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_dancebot_ue (#125) + * Move method after the method it calls. Otherwise recursion could happen. (#126) + * Feature/dance bot s pattern (#128) + * more changes in sm_dancebot_ue + * polishing sm_dancebot_ue and s-pattern + * noticed typo + Finnaly > Finally + * Feature/dance bot s pattern (#129) + * more changes in sm_dancebot_ue + * polishing sm_dancebot_ue and s-pattern + * more refinement in sm_dancebot_ue + * 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_dancebot_ue + * minor + * Feature/sm dance bot refine 2 (#132) + * more changes in sm_dancebot_ue + * minor + * build fix + * waypoints navigator bug (#133) + * minor tuning to mitigate overshot issue cases + * progress in the sm_dancebot_ue 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/nav2z 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_dancebot_ue + * 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 + * Ignore packages which should not be released. + * Added changelogs. + * 0.4.0 + * Revert "Ignore packages which should not be released." + This reverts commit ee2cc86db3c0a24f9eb0a9e33217de3f7a691a1c. + * Fix urls to index.ros.org (#284) + * Fix foxy source build config to use repos file from foxy branch. (#285) + * adding spawn entity delays + * more on backport + * more on backport + * disappeared ur_msgs denis repo + * fixing sm_dancebot_ue examples + * working on fix of image messages for husky_barrel demo + 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) +------------------ +* publisher +* Feature/warehouse 3 improvements (#313) + * 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 +* 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 +* Feature/galactic rolling merge (#288) + * 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_dancebot_ue + * 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_dancebot_ue + * 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_dancebot_ue + * 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_dancebot_ue visualizing turtlebot3 (#101) + * Feature/dance bot launch gz lidar choice (#102) + * sm_dancebot_ue 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_dancebot_ue 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_dancebot_ue 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_dancebot_ue 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_dancebot_ue (#125) + * Move method after the method it calls. Otherwise recursion could happen. (#126) + * Feature/dance bot s pattern (#128) + * more changes in sm_dancebot_ue + * polishing sm_dancebot_ue and s-pattern + * noticed typo + Finnaly > Finally + * Feature/dance bot s pattern (#129) + * more changes in sm_dancebot_ue + * polishing sm_dancebot_ue and s-pattern + * more refinement in sm_dancebot_ue + * 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_dancebot_ue + * minor + * Feature/sm dance bot refine 2 (#132) + * more changes in sm_dancebot_ue + * minor + * build fix + * waypoints navigator bug (#133) + * minor tuning to mitigate overshot issue cases + * progress in the sm_dancebot_ue 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/nav2z 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_dancebot_ue + * 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_dancebot_ue + * 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_dancebot_ue + * 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_dancebot_ue + * 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_dancebot_ue visualizing turtlebot3 (#101) + * Feature/dance bot launch gz lidar choice (#102) + * sm_dancebot_ue 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_dancebot_ue 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_dancebot_ue 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_dancebot_ue 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_dancebot_ue (#125) + * Move method after the method it calls. Otherwise recursion could happen. (#126) + * Feature/dance bot s pattern (#128) + * more changes in sm_dancebot_ue + * polishing sm_dancebot_ue and s-pattern + * noticed typo + Finnaly > Finally + * Feature/dance bot s pattern (#129) + * more changes in sm_dancebot_ue + * polishing sm_dancebot_ue and s-pattern + * more refinement in sm_dancebot_ue + * 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_dancebot_ue + * minor + * Feature/sm dance bot refine 2 (#132) + * more changes in sm_dancebot_ue + * minor + * build fix + * waypoints navigator bug (#133) + * minor tuning to mitigate overshot issue cases + * progress in the sm_dancebot_ue 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/nav2z 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_dancebot_ue + * 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_dancebot_ue + * 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_dancebot_ue + * 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_dancebot_ue + * 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_dancebot_ue visualizing turtlebot3 (#101) + * Feature/dance bot launch gz lidar choice (#102) + * sm_dancebot_ue 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_dancebot_ue 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_dancebot_ue 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_dancebot_ue 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_dancebot_ue (#125) + * Move method after the method it calls. Otherwise recursion could happen. (#126) + * Feature/dance bot s pattern (#128) + * more changes in sm_dancebot_ue + * polishing sm_dancebot_ue and s-pattern + * noticed typo + Finnaly > Finally + * Feature/dance bot s pattern (#129) + * more changes in sm_dancebot_ue + * polishing sm_dancebot_ue and s-pattern + * more refinement in sm_dancebot_ue + * 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_dancebot_ue + * minor + * Feature/sm dance bot refine 2 (#132) + * more changes in sm_dancebot_ue + * minor + * build fix + * waypoints navigator bug (#133) + * minor tuning to mitigate overshot issue cases + * progress in the sm_dancebot_ue 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/nav2z 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_dancebot_ue + * 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_dancebot_ue + * 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_dancebot_ue + * 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_dancebot_ue + * 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_dancebot_ue visualizing turtlebot3 (#101) + * Feature/dance bot launch gz lidar choice (#102) + * sm_dancebot_ue 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_dancebot_ue 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_dancebot_ue 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_dancebot_ue 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_dancebot_ue (#125) + * Move method after the method it calls. Otherwise recursion could happen. (#126) + * Feature/dance bot s pattern (#128) + * more changes in sm_dancebot_ue + * polishing sm_dancebot_ue and s-pattern + * noticed typo + Finnaly > Finally + * Feature/dance bot s pattern (#129) + * more changes in sm_dancebot_ue + * polishing sm_dancebot_ue and s-pattern + * more refinement in sm_dancebot_ue + * 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_dancebot_ue + * minor + * Feature/sm dance bot refine 2 (#132) + * more changes in sm_dancebot_ue + * minor + * build fix + * waypoints navigator bug (#133) + * minor tuning to mitigate overshot issue cases + * progress in the sm_dancebot_ue 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/nav2z 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_dancebot_ue + * 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_dancebot_ue + * 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_dancebot_ue + * 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_dancebot_ue + * 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_dancebot_ue visualizing turtlebot3 (#101) + * Feature/dance bot launch gz lidar choice (#102) + * sm_dancebot_ue 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_dancebot_ue 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_dancebot_ue 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_dancebot_ue 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_dancebot_ue (#125) + * Move method after the method it calls. Otherwise recursion could happen. (#126) + * Feature/dance bot s pattern (#128) + * more changes in sm_dancebot_ue + * polishing sm_dancebot_ue and s-pattern + * noticed typo + Finnaly > Finally + * Feature/dance bot s pattern (#129) + * more changes in sm_dancebot_ue + * polishing sm_dancebot_ue and s-pattern + * more refinement in sm_dancebot_ue + * 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_dancebot_ue + * minor + * Feature/sm dance bot refine 2 (#132) + * more changes in sm_dancebot_ue + * minor + * build fix + * waypoints navigator bug (#133) + * minor tuning to mitigate overshot issue cases + * progress in the sm_dancebot_ue 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/nav2z 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_dancebot_ue + * 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_dancebot_ue + * 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_dancebot_ue + * 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_dancebot_ue + * 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_dancebot_ue visualizing turtlebot3 (#101) + * Feature/dance bot launch gz lidar choice (#102) + * sm_dancebot_ue 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_dancebot_ue 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_dancebot_ue 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_dancebot_ue 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_dancebot_ue (#125) + * Move method after the method it calls. Otherwise recursion could happen. (#126) + * Feature/dance bot s pattern (#128) + * more changes in sm_dancebot_ue + * polishing sm_dancebot_ue and s-pattern + * noticed typo + Finnaly > Finally + * Feature/dance bot s pattern (#129) + * more changes in sm_dancebot_ue + * polishing sm_dancebot_ue and s-pattern + * more refinement in sm_dancebot_ue + * 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_dancebot_ue + * minor + * Feature/sm dance bot refine 2 (#132) + * more changes in sm_dancebot_ue + * minor + * build fix + * waypoints navigator bug (#133) + * minor tuning to mitigate overshot issue cases + * progress in the sm_dancebot_ue 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/nav2z 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_dancebot_ue + * 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_dancebot_ue + * 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_dancebot_ue + * 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_dancebot_ue + * 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_dancebot_ue visualizing turtlebot3 (#101) + * Feature/dance bot launch gz lidar choice (#102) + * sm_dancebot_ue 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_dancebot_ue 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_dancebot_ue 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_dancebot_ue 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_dancebot_ue (#125) + * Move method after the method it calls. Otherwise recursion could happen. (#126) + * Feature/dance bot s pattern (#128) + * more changes in sm_dancebot_ue + * polishing sm_dancebot_ue and s-pattern + * noticed typo + Finnaly > Finally + * Feature/dance bot s pattern (#129) + * more changes in sm_dancebot_ue + * polishing sm_dancebot_ue and s-pattern + * more refinement in sm_dancebot_ue + * 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_dancebot_ue + * minor + * Feature/sm dance bot refine 2 (#132) + * more changes in sm_dancebot_ue + * minor + * build fix + * waypoints navigator bug (#133) + * minor tuning to mitigate overshot issue cases + * progress in the sm_dancebot_ue 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/nav2z 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_dancebot_ue + * 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 + * Ignore packages which should not be released. + * Added changelogs. + * 0.4.0 + * Revert "Ignore packages which should not be released." + This reverts commit ee2cc86db3c0a24f9eb0a9e33217de3f7a691a1c. + * Fix urls to index.ros.org (#284) + * Fix foxy source build config to use repos file from foxy branch. (#285) + * adding spawn entity delays + * more on backport + * more on backport + * disappeared ur_msgs denis repo + * fixing sm_dancebot_ue examples + * working on fix of image messages for husky_barrel demo + 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_dancebot_ue + * 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_dancebot_ue + * 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_dancebot_ue + * 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_dancebot_ue visualizing turtlebot3 (#101) + * Feature/dance bot launch gz lidar choice (#102) + * sm_dancebot_ue 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_dancebot_ue 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_dancebot_ue 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_dancebot_ue 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_dancebot_ue (#125) + * Move method after the method it calls. Otherwise recursion could happen. (#126) + * Feature/dance bot s pattern (#128) + * more changes in sm_dancebot_ue + * polishing sm_dancebot_ue and s-pattern + * noticed typo + Finnaly > Finally + * Feature/dance bot s pattern (#129) + * more changes in sm_dancebot_ue + * polishing sm_dancebot_ue and s-pattern + * more refinement in sm_dancebot_ue + * 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_dancebot_ue + * minor + * Feature/sm dance bot refine 2 (#132) + * more changes in sm_dancebot_ue + * minor + * build fix + * waypoints navigator bug (#133) + * minor tuning to mitigate overshot issue cases + * progress in the sm_dancebot_ue 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/nav2z 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_dancebot_ue + * 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_dancebot_ue + * 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_dancebot_ue + * 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_dancebot_ue + * 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_dancebot_ue visualizing turtlebot3 (#101) + * Feature/dance bot launch gz lidar choice (#102) + * sm_dancebot_ue 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_dancebot_ue 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_dancebot_ue 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_dancebot_ue 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_dancebot_ue (#125) + * Move method after the method it calls. Otherwise recursion could happen. (#126) + * Feature/dance bot s pattern (#128) + * more changes in sm_dancebot_ue + * polishing sm_dancebot_ue and s-pattern + * noticed typo + Finnaly > Finally + * Feature/dance bot s pattern (#129) + * more changes in sm_dancebot_ue + * polishing sm_dancebot_ue and s-pattern + * more refinement in sm_dancebot_ue + * 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_dancebot_ue + * minor + * Feature/sm dance bot refine 2 (#132) + * more changes in sm_dancebot_ue + * minor + * build fix + * waypoints navigator bug (#133) + * minor tuning to mitigate overshot issue cases + * progress in the sm_dancebot_ue 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/nav2z 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_dancebot_ue + * 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_dancebot_ue + * 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_dancebot_ue + * 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_dancebot_ue + * 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_dancebot_ue visualizing turtlebot3 (#101) + * Feature/dance bot launch gz lidar choice (#102) + * sm_dancebot_ue 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_dancebot_ue 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_dancebot_ue 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_dancebot_ue 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_dancebot_ue (#125) + * Move method after the method it calls. Otherwise recursion could happen. (#126) + * Feature/dance bot s pattern (#128) + * more changes in sm_dancebot_ue + * polishing sm_dancebot_ue and s-pattern + * noticed typo + Finnaly > Finally + * Feature/dance bot s pattern (#129) + * more changes in sm_dancebot_ue + * polishing sm_dancebot_ue and s-pattern + * more refinement in sm_dancebot_ue + * 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_dancebot_ue + * minor + * Feature/sm dance bot refine 2 (#132) + * more changes in sm_dancebot_ue + * minor + * build fix + * waypoints navigator bug (#133) + * minor tuning to mitigate overshot issue cases + * progress in the sm_dancebot_ue 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/nav2z 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_dancebot_ue + * 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_dancebot_ue + * 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_dancebot_ue + * 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_dancebot_ue + * 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_dancebot_ue visualizing turtlebot3 (#101) + * Feature/dance bot launch gz lidar choice (#102) + * sm_dancebot_ue 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_dancebot_ue 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_dancebot_ue 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_dancebot_ue 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_dancebot_ue (#125) + * Move method after the method it calls. Otherwise recursion could happen. (#126) + * Feature/dance bot s pattern (#128) + * more changes in sm_dancebot_ue + * polishing sm_dancebot_ue and s-pattern + * noticed typo + Finnaly > Finally + * Feature/dance bot s pattern (#129) + * more changes in sm_dancebot_ue + * polishing sm_dancebot_ue and s-pattern + * more refinement in sm_dancebot_ue + * 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_dancebot_ue + * minor + * Feature/sm dance bot refine 2 (#132) + * more changes in sm_dancebot_ue + * minor + * build fix + * waypoints navigator bug (#133) + * minor tuning to mitigate overshot issue cases + * progress in the sm_dancebot_ue 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/nav2z 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_dancebot_ue + * 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_dancebot_ue + * 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_dancebot_ue + * 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_dancebot_ue + * 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_dancebot_ue visualizing turtlebot3 (#101) + * Feature/dance bot launch gz lidar choice (#102) + * sm_dancebot_ue 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_dancebot_ue 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_dancebot_ue 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_dancebot_ue 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_dancebot_ue (#125) + * Move method after the method it calls. Otherwise recursion could happen. (#126) + * Feature/dance bot s pattern (#128) + * more changes in sm_dancebot_ue + * polishing sm_dancebot_ue and s-pattern + * noticed typo + Finnaly > Finally + * Feature/dance bot s pattern (#129) + * more changes in sm_dancebot_ue + * polishing sm_dancebot_ue and s-pattern + * more refinement in sm_dancebot_ue + * 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_dancebot_ue + * minor + * Feature/sm dance bot refine 2 (#132) + * more changes in sm_dancebot_ue + * minor + * build fix + * waypoints navigator bug (#133) + * minor tuning to mitigate overshot issue cases + * progress in the sm_dancebot_ue 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/nav2z 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_dancebot_ue + * 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_dancebot_ue + * 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_dancebot_ue + * 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_dancebot_ue + * 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_dancebot_ue visualizing turtlebot3 (#101) + * Feature/dance bot launch gz lidar choice (#102) + * sm_dancebot_ue 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_dancebot_ue 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_dancebot_ue 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_dancebot_ue 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_dancebot_ue (#125) + * Move method after the method it calls. Otherwise recursion could happen. (#126) + * Feature/dance bot s pattern (#128) + * more changes in sm_dancebot_ue + * polishing sm_dancebot_ue and s-pattern + * noticed typo + Finnaly > Finally + * Feature/dance bot s pattern (#129) + * more changes in sm_dancebot_ue + * polishing sm_dancebot_ue and s-pattern + * more refinement in sm_dancebot_ue + * 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_dancebot_ue + * minor + * Feature/sm dance bot refine 2 (#132) + * more changes in sm_dancebot_ue + * minor + * build fix + * waypoints navigator bug (#133) + * minor tuning to mitigate overshot issue cases + * progress in the sm_dancebot_ue 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/nav2z 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 +* Backport/initial to galactic (#61) + * reformatting the whole project + * Remove test phase from CMake and remove dependencies from package.xml. + * Compile with navigation and slam_toolbox. + * Enable all packages to compile. + * Resolve missing dependency in smacc_msgs and reorganize them for better overview. + * getLogger functionality and refactoring + * broken sm_respira + * sm_respira code + * Update README.md + ## Additions + - build-status table + - detailed install instructions (adjusted from [here](https://github.com/UniversalRobots/Universal_Robots_ROS2_Driver#readme)) + ## Changes + - default build type as `Release` (it is faster than `Debug` and executables are smaller) + - updated examples section + * 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. + use this command "sudo docker build --build-arg ROS2_DISTRO=(desiredRosTag) (directoryHoldingDockerfile)/" + * 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" + * Update tracing/ManualTracing.md + * reactivating smacc2 nav clients for rolling via submodules + * bug in smacc2 component + * reverted markdowns to html + * added README tutorial for Dockerfile + * edited tracing.md to reflect new tracing event names + * 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 + * Clean up of sm_atomic_24hr + * more sm_atomic_24hr cleanup + * Optimized deps in move_base_z_planners_common. + * Renaming of event generator library + * Correct build-overview table. + * Update and unify CI configurations. + * Use tf_geometry_msgs.h in galactic. + * Use galactic branches in .repos-file. + Co-authored-by: pabloinigoblasco + Co-authored-by: reelrbtx + Co-authored-by: Declan Dury <44791484+DecDury@users.noreply.github.com> + Co-authored-by: DecDury + Co-authored-by: brettpac +* Contributors: Denis Štogl, Pablo Iñigo Blasco, pabloinigoblasco diff --git a/smacc2_sm_reference_library/sm_dancebot_ue/CMakeLists.txt b/smacc2_sm_reference_library/sm_dancebot_ue/CMakeLists.txt new file mode 100644 index 000000000..4df9054cb --- /dev/null +++ b/smacc2_sm_reference_library/sm_dancebot_ue/CMakeLists.txt @@ -0,0 +1,113 @@ +cmake_minimum_required(VERSION 3.5) +project(sm_dancebot_ue) + +# 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(ament_cmake_python REQUIRED) +find_package(Boost COMPONENTS thread REQUIRED) + +find_package(smacc2 REQUIRED) +find_package(std_msgs REQUIRED) +find_package(nav2z_client REQUIRED) + +find_package(ros_publisher_client REQUIRED) +find_package(multirole_sensor_client REQUIRED) + +find_package(sr_all_events_go REQUIRED) +find_package(sr_event_countdown REQUIRED) +find_package(sr_conditional REQUIRED) +find_package(ros_timer_client REQUIRED) +find_package(sensor_msgs REQUIRED) +find_package(visualization_msgs REQUIRED) +find_package(ue_msgs REQUIRED) + +set(dependencies + smacc2 + std_msgs + sensor_msgs + nav2z_client + ros_publisher_client + multirole_sensor_client + sr_all_events_go + sr_event_countdown + sr_conditional + ros_timer_client + visualization_msgs + ue_msgs +) + +rosidl_generate_interfaces(${PROJECT_NAME} + "servers/led_action_server/action/LEDControl.action" + DEPENDENCIES builtin_interfaces std_msgs action_msgs sensor_msgs +) + +include_directories(include + ${Boost_INCLUDE_DIRS} + ${CMAKE_BINARY_DIR}/rosidl_generator_cpp) + +add_executable(${PROJECT_NAME}_node + src/${PROJECT_NAME}/sm_dancebot_ue.cpp + src/${PROJECT_NAME}/clients/client_behaviors/nav2z_client/cb_position_control_free_space.cpp + src/${PROJECT_NAME}/clients/components/cp_ue_pose.cpp + src/${PROJECT_NAME}/clients/client_behaviors/nav2z_client/cb_active_stop.cpp +) + +add_executable(temperature_sensor_node_${PROJECT_NAME} servers/temperature_sensor_node/src/temperature_sensor_node.cpp) +add_executable(led_action_server_node_${PROJECT_NAME} servers/led_action_server/src/led_action_server_node.cpp) + +ament_target_dependencies(${PROJECT_NAME}_node ${dependencies}) +target_link_libraries(${PROJECT_NAME}_node ${Boost_LIBS} ${PROJECT_NAME}__rosidl_typesupport_cpp) + +ament_target_dependencies(led_action_server_node_${PROJECT_NAME} ${dependencies}) +set_target_properties(led_action_server_node_${PROJECT_NAME} PROPERTIES OUTPUT_NAME "led_action_server_node") +target_link_libraries(led_action_server_node_${PROJECT_NAME} ${Boost_LIBS} ${PROJECT_NAME}__rosidl_typesupport_cpp) + +ament_target_dependencies(temperature_sensor_node_${PROJECT_NAME} ${dependencies}) +set_target_properties(temperature_sensor_node_${PROJECT_NAME} PROPERTIES OUTPUT_NAME "temperature_sensor_node") + +ament_export_include_directories(include) +ament_export_libraries(${PROJECT_NAME}) +ament_export_dependencies(${dependencies}) + + +if(BUILD_TESTING) +endif() + +install(TARGETS + ${PROJECT_NAME}_node + temperature_sensor_node_${PROJECT_NAME} + led_action_server_node_${PROJECT_NAME} + + ARCHIVE DESTINATION lib + LIBRARY DESTINATION lib + RUNTIME DESTINATION lib/${PROJECT_NAME} +) + +#ament_python_install_package(${PROJECT_NAME} +# PACKAGE_DIR servers/service_node_3 +# ) + + +install(FILES + servers/service_node_3/service_node_3.py + scripts/transform_publisher.py + DESTINATION + lib/${PROJECT_NAME} + PERMISSIONS OWNER_EXECUTE OWNER_WRITE OWNER_READ + GROUP_EXECUTE GROUP_READ) + +install(DIRECTORY launch DESTINATION share/${PROJECT_NAME}) +install(DIRECTORY rviz DESTINATION share/${PROJECT_NAME}) +install(DIRECTORY params DESTINATION share/${PROJECT_NAME}) +install(DIRECTORY urdf DESTINATION share/${PROJECT_NAME}) + +ament_package() diff --git a/smacc2_sm_reference_library/sm_dancebot_ue/README.md b/smacc2_sm_reference_library/sm_dancebot_ue/README.md new file mode 100644 index 000000000..8fa9efb95 --- /dev/null +++ b/smacc2_sm_reference_library/sm_dancebot_ue/README.md @@ -0,0 +1,55 @@ +

State Machine Diagram

+ + ![sm_dance_bot](docs/SmDanceBot_2023-5-26_151817.svg) + +

Description

A full-featured state machine example, that highlights the capabilities of SMACC2 & the ROS2 Navigation Stack via the MoveBaseZ Client. +.

+Doxygen Namespace & Class Reference + +

Build Instructions

+ +First, source your chosen ros2 distro. +``` +source /opt/ros/rolling/setup.bash +``` +``` +source /opt/ros/galactic/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 build, remember to source the proper install folder... + +``` +source ~/colcon_ws/install/setup.bash +``` + +And then run the launch file... + +``` +ros2 launch sm_dance_bot sm_dance_bot_launch.py +``` +

Headless launch

+ +Alternatively, you can also launch the gazebo simulator in headless mode: +``` +ros2 launch sm_dance_bot sm_dance_bot_launch.py headless:=True +``` +

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_dancebot_ue/docker/.dockerignore b/smacc2_sm_reference_library/sm_dancebot_ue/docker/.dockerignore new file mode 100644 index 000000000..e69de29bb diff --git a/smacc2_sm_reference_library/sm_dancebot_ue/docker/.gitignore b/smacc2_sm_reference_library/sm_dancebot_ue/docker/.gitignore new file mode 100644 index 000000000..167d48f57 --- /dev/null +++ b/smacc2_sm_reference_library/sm_dancebot_ue/docker/.gitignore @@ -0,0 +1 @@ +UE5.1 diff --git a/smacc2_sm_reference_library/sm_dancebot_ue/docker/Dockerfile b/smacc2_sm_reference_library/sm_dancebot_ue/docker/Dockerfile new file mode 100644 index 000000000..1ed11f138 --- /dev/null +++ b/smacc2_sm_reference_library/sm_dancebot_ue/docker/Dockerfile @@ -0,0 +1,133 @@ +ARG ROS_DISTRO +ARG GIT_BRANCH +ARG UBUNTU_VERSION + +FROM ros:$ROS_DISTRO-ros-base-$UBUNTU_VERSION + +ARG ROS_DISTRO +ARG GIT_BRANCH +ARG LOCAL_FOLDER_SOURCE=1 +ENV DEBIAN_FRONTEND=noninteractive + +RUN apt-get update \ + && apt-get upgrade -y \ + && apt-get clean + +RUN apt-get update && apt-get install -q -y --no-install-recommends \ + dirmngr \ + gnupg2 \ + lsb-release \ + python3-colcon-ros \ + && apt-get clean \ + && apt upgrade -y --with-new-pkgs + +RUN apt-get install -y nvidia-driver-525 + +RUN apt install -y xdg-user-dirs + +ENV USER=ros2_ws +RUN bash -c "useradd -ms /bin/bash $USER" +RUN usermod -aG sudo $USER +RUN echo '%sudo ALL=(ALL) NOPASSWD:ALL' >> /etc/sudoers +RUN chown -R $USER:$USER /home/$USER + +USER $USER +WORKDIR /home/$USER + +WORKDIR "/home/$USER/src" + +RUN echo "copying current code version to docker image:" +#ADD . /home/$USER/src/SMACC2 +#WORKDIR "/home/$USER" + +# install dependencies and build +# RUN vcs import src --skip-existing --input src/SMACC2/.github/SMACC2-not-released.$ROS_DISTRO.repos +# RUN ls src + +# RUN apt update +# RUN rosdep install --from-paths src --ignore-src -r -y +# RUN apt-get update && apt-get install -q -y --no-install-recommends xterm + +# RUN bash -c "source /opt/ros/$ROS_DISTRO/setup.sh && colcon build --merge-install --parallel-workers 1" + +COPY --chown=$USER:$USER smacc2_sm_reference_library/sm_dancebot_ue/docker/UE5.1/UnrealEngine /home/$USER/src/UE5.1/UnrealEngine + +ENV UE5_DIR=/home/$USER/src/UE5.1/UnrealEngine +ENV PROJECT_BRANCH=main +ENV PROJECT_NAME=ue_project_1 + +RUN sudo apt-get install -y wget gpg +RUN wget -qO- https://packages.microsoft.com/keys/microsoft.asc | sudo gpg --dearmor > packages.microsoft.gpg +RUN sudo install -D -o root -g root -m 644 packages.microsoft.gpg /etc/apt/keyrings/packages.microsoft.gpg +RUN sudo sh -c 'echo "deb [arch=amd64,arm64,armhf signed-by=/etc/apt/keyrings/packages.microsoft.gpg] https://packages.microsoft.com/repos/code stable main" > /etc/apt/sources.list.d/vscode.list' -f packages.microsoft.gpg + +RUN sudo apt install -y apt-transport-https +RUN sudo apt update +RUN sudo apt install -y code git-lfs nano # or code-insiders + +# smacc2_devel +RUN git clone --recurse-submodules https://github.com/robosoft-ai/${PROJECT_NAME}.git -b $PROJECT_BRANCH + +WORKDIR "/home/$USER/src/${PROJECT_NAME}" +# RUN git checkout abdf483ce14f477d8f255fe19f3527975b3a9dbb +# RUN cd Plugins/rclUE && git checkout bae993fa + +#RUN rm -rf Plugins/RapyutaSimulationPlugins && git clone https://github.com/robosoft-ai/UE-Plugins.git Plugins/RapyutaSimulationPlugins +#RUN cd Plugins/RapyutaSimulationPlugins && git checkout bfac015 + +RUN git-lfs pull && git submodule foreach git-lfs pull +RUN ls && echo ${UE5_DIR} +COPY --chown=$USER:$USER smacc2_sm_reference_library/sm_dancebot_ue/docker/update_project_files.sh update_project_files.sh +ENV UE5_DIR=/home/$USER/src/UE5.1 + +# installing Dependencies +RUN wget http://security.ubuntu.com/ubuntu/pool/universe/t/tinyxml2/libtinyxml2-6a_7.0.0+dfsg-1build1_amd64.deb +RUN wget http://archive.ubuntu.com/ubuntu/pool/main/o/openssl/libssl1.1_1.1.0g-2ubuntu4_amd64.deb +RUN sudo dpkg -i libtinyxml2-6a_7.0.0+dfsg-1build1_amd64.deb +RUN sudo dpkg -i libssl1.1_1.1.0g-2ubuntu4_amd64.deb + +ENV LD_LIBRARY_PATH=/usr/local/cuda/lib64:/usr/lib/x86_64-linux-gnu/gazebo-11/plugins:/opt/ros/humble/opt/rviz_ogre_vendor/lib:/opt/ros/humble/lib/x86_64-linux-gnu:/home/$USER/src/${PROJECT_NAME}/Plugins/rclUE/ThirdParty/ros/lib/ +ENV UE5_DIR=/home/$USER/src/UE5.1/UnrealEngine + +COPY --chown=$USER:$USER smacc2_sm_reference_library/sm_dancebot_ue/docker/nvidia-driver-check.sh /home/$USER/src/${PROJECT_NAME}/nvidia-driver-check.sh +WORKDIR /home/$USER/src/${PROJECT_NAME} +RUN echo "nvidia-driver-check.sh" +# RUN cat update_project_files.sh +# RUN ../UE5.1/UnrealEngine/Engine/Build/BatchFiles/Linux/GenerateProjectFiles.sh -CurrentPlatform ue_project_1.uproject +# #RUN bash + +# RUN make ${PROJECT_NAME}Editor + +# # COPY smacc2 /home/$USER/src/SMACC2/smacc2 +# RUN mkdir -p /home/$USER/src/SMACC2/ +# WORKDIR "/home/$USER/src/SMACC2" +# COPY --chown=$USER:$USER smacc2_client_library /home/$USER/src/SMACC2/smacc2_client_library +# COPY --chown=$USER:$USER smacc2_event_generator_library /home/$USER/src/SMACC2/smacc2_event_generator_library +# COPY --chown=$USER:$USER smacc2_performance_tools /home/$USER/src/SMACC2/smacc2_performance_tools +# COPY --chown=$USER:$USER smacc2_state_reactor_library /home/$USER/src/SMACC2/smacc2_state_reactor_library +# COPY --chown=$USER:$USER smacc2 /home/$USER/src/SMACC2/smacc2 +# COPY --chown=$USER:$USER smacc2_dev_tools /home/$USER/src/SMACC2/smacc2_dev_tools +# COPY --chown=$USER:$USER smacc2_msgs /home/$USER/src/SMACC2/smacc2_msgs +# #smacc2_sm_reference_library + +# #RUN git clone +# RUN sudo apt-get update +# RUN rosdep update +# RUN rosdep install --ignore-src --from-paths . -y -r +# RUN sudo apt-get update && sudo apt-get install -y ros-humble-nav2-bringup ros-humble-rviz2 + +# RUN sudo apt-get install -y openvpn +# #COPY --chown=$USER:$USER smacc2_sm_reference_library/sm_dancebot_ue/docker/run_editor_smacc.sh /home/$USER/src/${PROJECT_NAME}/run_editor_smacc.sh + + +# RUN touch /home/$USER/src/${PROJECT_NAME}/COLCON_IGNORE +# RUN touch /home/$USER/src/UE5.1/COLCON_IGNORE +# WORKDIR "/home/$USER/" +# RUN bash -c "source /opt/ros/humble/setup.bash && colcon build --symlink-install" + +# # BASIC VPN FRAMEWORK --- +RUN mkdir /home/$USER/ovpnconfig + +WORKDIR "/home/$USER/src/${PROJECT_NAME}" + +# #ENV LD_LIBRARY_PATH=/home/ros2_ws/src/ue_project_1/Plugins/rclUE/ThirdParty/ros/lib/:/usr/local/cuda/lib64:/usr/lib/x86_64-linux-gnu/gazebo-11/plugins:/opt/ros/humble/opt/rviz_ogre_vendor/lib:/opt/ros/humble/lib/x86_64-linux-gnu: diff --git a/smacc2_sm_reference_library/sm_dancebot_ue/docker/README.md b/smacc2_sm_reference_library/sm_dancebot_ue/docker/README.md new file mode 100644 index 000000000..6986dc9a9 --- /dev/null +++ b/smacc2_sm_reference_library/sm_dancebot_ue/docker/README.md @@ -0,0 +1,275 @@ +# SMACC2 and UNREAL ENGINE EDITOR +The README.md file provides a comprehensive guide for setting up and utilizing the Unreal Engine Docker environment. The document covers the necessary prerequisites, such as installing Docker and NVIDIA-Docker2, and provides step-by-step instructions for downloading and loading the prebuilt Docker image containing Unreal Engine. Additionally, it explains how to run the Unreal Editor within the container, create new containers for debugging purposes, and connect the container to a VPN if required. The README.md also includes important notes and optional instructions for rebuilding and running SMACC state machines inside the container. Whether you are new to Docker or an experienced user, this document will help you navigate the process of working with Unreal Engine in a Docker environment effectively. + +## Important Notes about the Solution + +Here are some important notes regarding the solution: + +**Repos Versioning:** Not all versions of `rclUE`, `turtlebot3Editor`, and `RapyputaPlugins` are compatible with each other. Ensuring consistency among them can be challenging, especially when considering that the container maps/volumes some volumes with external contents. The container is already designed to have a correct combination of all of them, but this is hardcoded in the Dockerfile and can be improved. + +***Automatic container nvidia driver update*** +The host and the container must have the same nvidia-driver in order to run the ue editor and simulation. +There is a mechanism implemented to automatically sync the driver. The current driver version is passed from the host to the container and then it is updated in the container if that is required. That is done in the nvidia-check.sh script. + +## Pre-Requisites +Before proceeding with the instructions, ensure that you have the necessary prerequisites installed on your system. + +### Docker Installation +To install Docker, follow these steps: + +1. Update the package list: + ``` + sudo apt-get update + ``` + +2. Install the required dependencies: + ``` + sudo apt-get install apt-transport-https ca-certificates curl gnupg-agent software-properties-common + ``` + +3. Add Docker's official GPG key: + ``` + curl -fsSL https://download.docker.com/linux/ubuntu/gpg | sudo apt-key add - + ``` + +4. Add the Docker repository: + ``` + sudo add-apt-repository "deb [arch=amd64] https://download.docker.com/linux/ubuntu bionic stable" + ``` + +5. Update the package list again: + ``` + sudo apt-get update + ``` + +6. Install Docker: + ``` + sudo apt-get install docker-ce + ``` + +### NVIDIA-Docker2 Installation +Ensure that you have NVIDIA-Docker2 installed by executing the following commands: + +1. Add the NVIDIA-Docker GPG key: + ``` + curl -s -L https://nvidia.github.io/nvidia-docker/gpgkey | sudo apt-key add - + ``` + +2. Determine the distribution: + ``` + distribution=$(. /etc/os-release; echo $ID$VERSION_ID) + ``` + +3. Add the NVIDIA-Docker repository: + ``` + curl -s -L https://nvidia.github.io/nvidia-docker/$distribution/nvidia-docker.list | sudo tee /etc/apt/sources.list.d/nvidia-docker.list + ``` + +4. Update the package list: + ``` + sudo apt-get update + ``` + +5. Install NVIDIA-Docker2: + ``` + sudo apt-get install nvidia-docker2 + ``` + +6. Restart the Docker service: + ``` + sudo systemctl restart docker.service + ``` + +### Adding User to Docker Group +To allow a user to run Docker, add the user to the Docker group. Replace `` with the actual user ID: + +``` +sudo usermod -a -G docker +``` + +### Testing the Installation +To test your Docker and NVIDIA-Docker2 installations, run the following command: + +``` +sudo nvidia-docker run --rm nvidia/cuda:10.2-base-ubuntu18.04 nvidia-smi +``` + +## Unreal Engine Docker Image +This section provides instructions for downloading, loading, and running the prebuilt Docker image containing Unreal Engine. + +### Downloading the Prebuilt Image +Download the prebuilt Docker image for Unreal Engine from the provided source. + +### Loading the Docker Image +To load the downloaded image into your Docker image database, use the following command: + +``` +sudo docker load -i ue_editor_rclue.tar +``` + +### (Alternative) Building the Docker Image Locally +If you want to modify the Docker image using a Dockerfile and rebuild it, follow these steps: + +1. Create a link to the UE5.1 folder inside the `sm_dancebot_ue/docker` folder: + ``` + mount --bind UE5.1/UnrealEngine + ``` + * Recall the must contain the following folders: `Engine`, `FeaturePacks` and `Template` + +2. Build the image locally by running the following command: + ``` + ./build_docker_humble.sh + ``` + +## Using the Docker Image +This section explains how to run and create a new container from the `ue_editor_rcl` Docker image. + +### Running/Creating a New Container from the ue_editor_rcl Docker Image + +To run the Unreal Editor inside the container, you need to use some auxiliary scripts located in the `sm_dancebot_ue` example. Follow these steps: + +1. *Requirements* To run the docker container you need to download a few folders that will be mounted inside the container. These are: rclUE, UE-Plugins, SMACC2, ue_msgs, ue_poroject_1,... ue_project_n + +![image](https://github.com/robosoft-ai/SMACC2/assets/9130104/6151fac0-f0ba-4dd3-81fa-9bf1ca899142) + + +3. Navigate to the `sm_dancebot_ue/docker` folder. + +4. Execute the following command: + ``` + ./run_docker_container_bash_development.sh ``` + + This will run and create a new container using the `ue_editor_rcl` Docker image. + +5. The Unreal Engine editor will automatically open in "edition mode." You can launch the simulation with Turtlebot topics accessible from both the container and the host computer by clicking the "play" button. + + Note: When you close the editor, the container will also be finished, but it will remain installed. You can reopen the editor using the command: + ``` + ./start_container.sh + ``` + +### Joining, Stopping and Removing existing container + +1. To enter the Docker container and debug or test things from the command line, use the following command: + +``` +./join_bash.sh +``` + +If you need to reset everything and remove the existing container, follow these steps: + +2. Execute the following command: + ``` + ./stop_container.sh + ``` + + This will stop the running container. + +3. Execute the following command: + ``` + ./remove_container.sh + ``` + + This will remove the existing container. + + +### Connecting the Container to VPN + +The prototype for connecting the container to a VPN has already been completed. For detailed instructions, please refer to the relevant documentation. + +### Optionally Rebuilding and Running a SMACC State Machine inside the Container + +Note that the `ue_editor_rcl` containers are run by mapping the current `smacc2` source folder, allowing for mixed development between the host (using VSCode) and the container (for compiling and running the state machines). + +The SMACC2 source code is already prebuilt inside the image, making it available to any container. However, if you need to modify the `smacc2` code or examples and rebuild it, follow these steps: + +1. Change to the home directory: + ``` + cd ~/ + ``` + +2. Source the Humble ROS 2 installation: + ``` + source /opt/ros/humble/setup.bash + ``` + +3. Build the `smacc2` code: + ``` + colcon build + ``` + +4. To run your current demo, execute the following commands after building: + ``` + source install/setup.bash + ros2 launch sm_dancebot_ue sm_dancebot_ue_launch.py + ``` + +Enjoy experimenting with Unreal Engine in your Docker environment! + + + +# Brett's runtime notes + +You'll need to open three terminals for this demo. + One for the container where you'll run Unreal Engine + One to run the state machine (on the host) + One for the RTA (on the host) + +### Terminal 1 - For UE5 Simulation in the container + +1. Download the current SMACC2 repository. + ``` + cd ~/workspace/humble_ws/src + git clone https://github.com/robosoft-ai/SMACC2.git + + ``` +2. Build the workspace + + ``` + cd ~/workspace/humble_ws/ + source /opt/ros/humble/setup.bash + colcon build --symlink-install --parallel-workers 4 + # optionally, to go faster to our project: --packages-up-to sm_dancebot_ue + # optionally, to build debug mode: --cmake-args -DCMAKE_BUILD_TYPE=Debug + ``` + Once everything is done building... +3. Navigate to the `sm_dancebot_ue/docker` folder. + + ``` + cd ~/workspace/humble_ws/src/SMACC2/smacc2_sm_reference_library/sm_dancebot_ue/docker + ``` +4. Execute the following command: + ``` + ./run_docker_container_editor.sh + ``` + This will create and start a new container as a daemon. The container will be available even after restarting. It is capable of opening the Unreal Engine editor with ROS2, but the editor will not open automatically. + The Unreal Engine editor will automatically open in "edition mode." You can launch the simulation with Turtlebot topics accessible from both the container and the host computer by clicking the "play" button. + +### Terminal 2 - for the state machine on the host + +1. Add the following line to the `.bashrc` file on the host: + ``` + export RMW_IMPLEMENTATION=rmw_cyclonedds_cpp + ``` +2. Source the workspace you just built + ``` + source ~/workspace/humble_ws/install/setup.bash + ``` +3. Launch the state machine + ``` + ros2 launch sm_dancebot_ue sm_dancebot_ue_launch.py + ``` + This will launch the state machine application, rviz and other required nodes. + +### Terminal 3 - for the SMACC2_RTA on the host +1. Source the install + ``` + source /opt/ros/humble/setup.bash + ``` +2. Launch the SMACC2_RTA + ``` + ros2 run smacc2_rta smacc2_rta + ``` +3. Once the RTA is launched, in the upper left corner select State Machine/Available State Machines/SmDanceBotUE + +And you should be all set. diff --git a/smacc2_sm_reference_library/sm_dancebot_ue/docker/attach_docker_container.sh b/smacc2_sm_reference_library/sm_dancebot_ue/docker/attach_docker_container.sh new file mode 100755 index 000000000..7842a41d4 --- /dev/null +++ b/smacc2_sm_reference_library/sm_dancebot_ue/docker/attach_docker_container.sh @@ -0,0 +1 @@ +sudo docker attach ue_editor_rclue diff --git a/smacc2_sm_reference_library/sm_dancebot_ue/docker/build_docker.sh b/smacc2_sm_reference_library/sm_dancebot_ue/docker/build_docker.sh new file mode 100755 index 000000000..da9ede5fb --- /dev/null +++ b/smacc2_sm_reference_library/sm_dancebot_ue/docker/build_docker.sh @@ -0,0 +1,43 @@ +#!/bin/sh +DIR="$(dirname "$(realpath "$0")")" +echo $DIR +cd $DIR/.. +echo `pwd` + +ROS_DISTRO=$1 +GIT_BRANCH=$2 +UBUNTU_VERSION=$3 +NOCACHE="--no-cache" +NOCACHE= + +ROOT_DIR=`realpath $DIR/../../..` + +echo "ros distro: $ROS_DISTRO" +echo "git branch: $GIT_BRANCH" +echo "ubuntu version: $UBUNTU_VERSION" +echo "root path: $ROOT_DIR" + +UE_FOLDER=$DIR/UE5.1/UnrealEngine +# if DIRECTORY does exist show message error +if [ -d "$UE_FOLDER" ]; then + echo "UE5.1 folder exists" +else + echo "UE5.1 folder does not exist" + echo "Please download UE5.1 from https://www.unrealengine.com/en-US/download" + echo "and extract it to $UE_FOLDER" + exit 1 +fi + +#it is expected to exist $UE_FOLDER/Engine +if [ -d "$UE_FOLDER/Engine" ]; then + echo "UE5.1 Engine folder exists" +else + echo "UE5.1 Engine folder does not exist" + echo "Please download UE5.1 from https://www.unrealengine.com/en-US/download" + echo "and extract it to $UE_FOLDER" + exit 1 +fi + + +cd $ROOT_DIR +sudo docker build --build-arg ROS_DISTRO=$ROS_DISTRO --build-arg GIT_BRANCH=$GIT_BRANCH --build-arg UBUNTU_VERSION=$UBUNTU_VERSION -t ue_editor_rclue:$ROS_DISTRO -f $ROOT_DIR/smacc2_sm_reference_library/sm_dancebot_ue/docker/Dockerfile . $NOCACHE diff --git a/smacc2_sm_reference_library/sm_dancebot_ue/docker/build_docker_humble.sh b/smacc2_sm_reference_library/sm_dancebot_ue/docker/build_docker_humble.sh new file mode 100755 index 000000000..ed0429b57 --- /dev/null +++ b/smacc2_sm_reference_library/sm_dancebot_ue/docker/build_docker_humble.sh @@ -0,0 +1,4 @@ +#!/bin/sh +DIR="$(dirname "$(realpath "$0")")" +echo $DIR +$DIR/build_docker.sh humble humble jammy diff --git a/smacc2_sm_reference_library/sm_dancebot_ue/docker/fastdds_setup.bash b/smacc2_sm_reference_library/sm_dancebot_ue/docker/fastdds_setup.bash new file mode 100755 index 000000000..f661c4240 --- /dev/null +++ b/smacc2_sm_reference_library/sm_dancebot_ue/docker/fastdds_setup.bash @@ -0,0 +1,5 @@ +#!/bin/bash + +export RMW_IMPLEMENTATION=rmw_fastrtps_cpp +export ROS_DISCOVERY_SERVER="127.0.0.1:11811" +export FASTRTPS_DEFAULT_PROFILES_FILE=${PWD}/fastdds_config.xml diff --git a/smacc2_sm_reference_library/sm_dancebot_ue/docker/join_bash.sh b/smacc2_sm_reference_library/sm_dancebot_ue/docker/join_bash.sh new file mode 100755 index 000000000..29ee10381 --- /dev/null +++ b/smacc2_sm_reference_library/sm_dancebot_ue/docker/join_bash.sh @@ -0,0 +1 @@ +sudo docker exec -it ue_editor_rclue /bin/bash diff --git a/smacc2_sm_reference_library/sm_dancebot_ue/docker/join_editor.sh b/smacc2_sm_reference_library/sm_dancebot_ue/docker/join_editor.sh new file mode 100755 index 000000000..c11f2028e --- /dev/null +++ b/smacc2_sm_reference_library/sm_dancebot_ue/docker/join_editor.sh @@ -0,0 +1 @@ +sudo docker exec -it ue_editor_rclue ./run_editor_smacc.sh diff --git a/smacc2_sm_reference_library/sm_dancebot_ue/docker/nvidia-driver-check.sh b/smacc2_sm_reference_library/sm_dancebot_ue/docker/nvidia-driver-check.sh new file mode 100755 index 000000000..294fd294f --- /dev/null +++ b/smacc2_sm_reference_library/sm_dancebot_ue/docker/nvidia-driver-check.sh @@ -0,0 +1,62 @@ +#!/bin/bash + +# String with the package description obtained from the original machine +#package_description="nvidia-driver-525/unknown,now 525.85.12-0ubuntu1 amd64 [installed]" +#DRIVER=$(apt list --installed | grep nvidia-driver) + +echo "Package description: $1" +package_description=$1 +#package_description=$DRIVER + +regex="([^/]+)/[^[:space:]]+ (.+)" +#regex="([^/]+)/([^[:space:]]+).+" +if [[ $package_description =~ $regex ]]; then + package_name="${BASH_REMATCH[1]}" + package_version="${BASH_REMATCH[2]}" +else + echo "Failed to extract package name and version from the package description." + exit 1 +fi + +echo "Package name: $package_name" +echo "Package version: $package_version" + +# Check if the package is installed +if dpkg -s "$package_name" >/dev/null 2>&1; then + echo "Package $package_name is installed." + echo "Checking if it has version $package_version." + + installed_package_description=$(apt list --installed | grep -w "$package_name") + echo "Installed package: $installed_package_description" + + if [[ $installed_package_description =~ $regex ]]; then + installed_version="${BASH_REMATCH[2]}" + fi + echo "Installed version: $installed_version" + + # Verify if the installed version matches the desired version + if [[ "$installed_version" == "$package_version" ]]; then + echo "Package $package_name-$package_version is already installed." + else + echo "WARNING: Package $package_name is installed, but it does not have the same debian package version $package_version." + DO_INSTALL=1 + fi +else + echo "Package $package_name is not installed." + DO_INSTALL=1 +fi + +if [ "$DO_INSTALL" == "1" ]; then + echo "Attempting to install package $package_name-$package_version." + # Attempt to install the package + sudo apt-get install -y "$package_name" + + # Check if the installation was successful + if dpkg -s "$package_name" >/dev/null 2>&1; then + echo "Package $package_name-$package_version has been successfully installed." + else + echo "Unable to install package $package_name-$package_version." + fi +else + echo "Checking nvidia-driver. Nothing to do. Package $package_name-$package_version is already installed." +fi diff --git a/smacc2_sm_reference_library/sm_dancebot_ue/docker/older/run_docker_container_bash.sh b/smacc2_sm_reference_library/sm_dancebot_ue/docker/older/run_docker_container_bash.sh new file mode 100755 index 000000000..a450ca485 --- /dev/null +++ b/smacc2_sm_reference_library/sm_dancebot_ue/docker/older/run_docker_container_bash.sh @@ -0,0 +1,8 @@ +#!/bin/sh +xhost + + +DIR="$(dirname "$(realpath "$0")")" +ROOT_DIR=`realpath $DIR/../../../..` +DRIVER=$(apt list --installed | grep nvidia-driver) + +sudo nvidia-docker run --net host -it -e DISPLAY -e QT_X11_NO_MITSHM=1 --privileged --name ue_editor_rclue -v /dev/dri:/dev/dri -v /tmp/.X11-unix:/tmp/.X11-unix ue_editor_rclue:humble /bin/bash -c "./nvidia-driver-check.sh '$DRIVER' && bash" diff --git a/smacc2_sm_reference_library/sm_dancebot_ue/docker/older/run_docker_container_editor.sh b/smacc2_sm_reference_library/sm_dancebot_ue/docker/older/run_docker_container_editor.sh new file mode 100755 index 000000000..8dfad38c4 --- /dev/null +++ b/smacc2_sm_reference_library/sm_dancebot_ue/docker/older/run_docker_container_editor.sh @@ -0,0 +1,8 @@ +#!/bin/sh +xhost + + +DIR="$(dirname "$(realpath "$0")")" +ROOT_DIR=`realpath $DIR/../../../..` + +DRIVER=$(apt list --installed | grep nvidia-driver) +sudo nvidia-docker run --net host -it -e DISPLAY -e QT_X11_NO_MITSHM=1 --privileged --name ue_editor_rclue -v $ROOT_DIR/ue_project_2:/home/ros2_ws/src/ue_project_2 -v $ROOT_DIR/ue_project_1:/home/ros2_ws/src/ue_project_1 -v $ROOT_DIR/SMACC2:/home/ros2_ws/src/SMACC2 -v $ROOT_DIR/UE-Plugins:/home/ros2_ws/src/turtlebot3-UE/Plugins/RapyutaSimulationPlugins -v /dev/dri:/dev/dri -v /tmp/.X11-unix:/tmp/.X11-unix ue_editor_rclue:humble /bin/bash -c "./nvidia-driver-check.sh '$DRIVER' && ./run_editor_smacc.sh" diff --git a/smacc2_sm_reference_library/sm_dancebot_ue/docker/older/run_editor_smacc.sh b/smacc2_sm_reference_library/sm_dancebot_ue/docker/older/run_editor_smacc.sh new file mode 100755 index 000000000..aa13d0389 --- /dev/null +++ b/smacc2_sm_reference_library/sm_dancebot_ue/docker/older/run_editor_smacc.sh @@ -0,0 +1,32 @@ +#!/bin/bash +# Copyright 2020-2022 Rapyuta Robotics Co., Ltd. + +export LD_LIBRARY_PATH=$(echo "$LD_LIBRARY_PATH" | sed 's#/opt/ros/humble/lib:##') +echo "UPDATED LD_LIBRARY_PATH: $LD_LIBRARY_PATH" + +if [ -z "${UE5_DIR}" ]; then + printf "Please set UE5_DIR to path of UE5 folder\n" + exit 1 +fi + +DISCOVERY_SERVER=${1:-true} +CURRENT_DIR=$(cd -- "$(dirname -- "${BASH_SOURCE[0]}")" &> /dev/null && pwd) +TB3_UE_DIR=${2:-"${CURRENT_DIR}"} +# if $DISCOVERY_SERVER; then +# # Run discovery service for FastDDS +# (exec "${TB3_UE_DIR}/run_discovery_service.sh") + +# # Configure environment for FastDDS discovery +# source ${TB3_UE_DIR}/fastdds_setup.sh +# fi + +#change default level, generating DefautlEngine.ini +DEFAULT_LEVEL=${LEVEL_NAME:-"Default"} +DEFAULT_RATE=${FIXED_FRAME_RATE:-"100.0"} +DEFAULT_RTF=${TARGET_RTF:-"1.0"} +sed -e 's/${LEVEL_NAME}/'${DEFAULT_LEVEL}'/g' Config/DefaultEngineBase.ini > Config/DefaultEngine.ini +sed -i -e 's/${FIXED_FRAME_RATE}/'${DEFAULT_RATE}'/g' Config/DefaultEngine.ini +sed -i -e 's/${TARGET_RTF}/'${DEFAULT_RTF}'/g' Config/DefaultEngine.ini + +UE_EDITOR="${UE5_DIR}/Engine/Binaries/Linux/UnrealEditor" +(exec "$UE_EDITOR" "${TB3_UE_DIR}/ue_project1.uproject" -norelativemousemode) diff --git a/smacc2_sm_reference_library/sm_dancebot_ue/docker/openvpn/login.txt b/smacc2_sm_reference_library/sm_dancebot_ue/docker/openvpn/login.txt new file mode 100644 index 000000000..e69de29bb diff --git a/smacc2_sm_reference_library/sm_dancebot_ue/docker/openvpn/ovpnconfig.ovpn b/smacc2_sm_reference_library/sm_dancebot_ue/docker/openvpn/ovpnconfig.ovpn new file mode 100644 index 000000000..e69de29bb diff --git a/smacc2_sm_reference_library/sm_dancebot_ue/docker/openvpn/password.conf b/smacc2_sm_reference_library/sm_dancebot_ue/docker/openvpn/password.conf new file mode 100644 index 000000000..e69de29bb diff --git a/smacc2_sm_reference_library/sm_dancebot_ue/docker/openvpn_file_run.sh b/smacc2_sm_reference_library/sm_dancebot_ue/docker/openvpn_file_run.sh new file mode 100755 index 000000000..dc16db9e7 --- /dev/null +++ b/smacc2_sm_reference_library/sm_dancebot_ue/docker/openvpn_file_run.sh @@ -0,0 +1,7 @@ +#!/bin/sh +xhost + + +DIR="$(dirname "$(realpath "$0")")" +ROOT_DIR=`realpath $DIR/../../../..` + +sudo nvidia-docker run --net host -it -e DISPLAY -e QT_X11_NO_MITSHM=1 --privileged --name ue_editor_rclue -v $ROOT_DIR/SMACC2:/home/ros2_ws/src/SMACC2 -v $ROOT_DIR/SMACC2_RTA:/home/ros2_ws/src/SMACC2_RTA -v /dev/dri:/dev/dri -v /tmp/.X11-unix:/tmp/.X11-unix ue_editor_rclue:humble /bin/bash diff --git a/smacc2_sm_reference_library/sm_dancebot_ue/docker/remove_container.sh b/smacc2_sm_reference_library/sm_dancebot_ue/docker/remove_container.sh new file mode 100755 index 000000000..3b98c52e0 --- /dev/null +++ b/smacc2_sm_reference_library/sm_dancebot_ue/docker/remove_container.sh @@ -0,0 +1,2 @@ +sudo docker stop ue_editor_rclue +sudo docker rm ue_editor_rclue diff --git a/smacc2_sm_reference_library/sm_dancebot_ue/docker/run_docker_container_bash_development.sh b/smacc2_sm_reference_library/sm_dancebot_ue/docker/run_docker_container_bash_development.sh new file mode 100755 index 000000000..257b64e62 --- /dev/null +++ b/smacc2_sm_reference_library/sm_dancebot_ue/docker/run_docker_container_bash_development.sh @@ -0,0 +1,8 @@ +#!/bin/sh +xhost + + +DIR="$(dirname "$(realpath "$0")")" +ROOT_DIR=`realpath $DIR/../../../..` +DRIVER=$(apt list --installed | grep nvidia-driver) + +sudo nvidia-docker run --net host --ipc=host -it -e DISPLAY -e QT_X11_NO_MITSHM=1 --privileged --name ue_editor_rclue -v $ROOT_DIR/ue_project_1:/home/ros2_ws/src/ue_project_1 -v $ROOT_DIR/ue_project_2:/home/ros2_ws/src/ue_project_2 -v $ROOT_DIR/SMACC2:/home/ros2_ws/src/SMACC2 -v $ROOT_DIR/UE-Plugins:/home/ros2_ws/src/ue_project_1/Plugins/RapyutaSimulationPlugins -v $ROOT_DIR/rclUE:/home/ros2_ws/src/ue_project_1/Plugins/rclUE -v $ROOT_DIR/UE-Plugins:/home/ros2_ws/src/ue_project_2/Plugins/RapyutaSimulationPlugins -v $ROOT_DIR/rclUE:/home/ros2_ws/src/ue_project_2/Plugins/rclUE -v /dev/dri:/dev/dri -v /tmp/.X11-unix:/tmp/.X11-unix ue_editor_rclue:humble /bin/bash -c "./nvidia-driver-check.sh '$DRIVER' && bash" diff --git a/smacc2_sm_reference_library/sm_dancebot_ue/docker/save_current_docker_image.sh b/smacc2_sm_reference_library/sm_dancebot_ue/docker/save_current_docker_image.sh new file mode 100755 index 000000000..3cd242149 --- /dev/null +++ b/smacc2_sm_reference_library/sm_dancebot_ue/docker/save_current_docker_image.sh @@ -0,0 +1,11 @@ +#!/bin/bash + +OUTPUT_PATH=${1} + +if [ -z "$OUTPUT_PATH" ]; then + echo "Usage: $0 " + exit 1 +fi + +DATE_SUFFIX=`date +%Y%m%d_%H%M%S` +docker save -o "$OUTPUT_PATH/unreal_editor_smacc_$DATE_SUFFIX.tar" ue_editor_rclue:humble diff --git a/smacc2_sm_reference_library/sm_dancebot_ue/docker/start_container.sh b/smacc2_sm_reference_library/sm_dancebot_ue/docker/start_container.sh new file mode 100755 index 000000000..6b339f906 --- /dev/null +++ b/smacc2_sm_reference_library/sm_dancebot_ue/docker/start_container.sh @@ -0,0 +1 @@ +sudo docker start ue_editor_rclue diff --git a/smacc2_sm_reference_library/sm_dancebot_ue/docker/stop_container.sh b/smacc2_sm_reference_library/sm_dancebot_ue/docker/stop_container.sh new file mode 100755 index 000000000..0c06686cc --- /dev/null +++ b/smacc2_sm_reference_library/sm_dancebot_ue/docker/stop_container.sh @@ -0,0 +1 @@ +sudo docker stop ue_editor_rclue diff --git a/smacc2_sm_reference_library/sm_dancebot_ue/docker/update_project_files.sh b/smacc2_sm_reference_library/sm_dancebot_ue/docker/update_project_files.sh new file mode 100755 index 000000000..0abd5e552 --- /dev/null +++ b/smacc2_sm_reference_library/sm_dancebot_ue/docker/update_project_files.sh @@ -0,0 +1,2 @@ +#!/bin/bash +/home/ros2_ws/src/UE5.1/Engine/Build/BatchFiles/Linux/GenerateProjectFiles.sh diff --git a/smacc2_sm_reference_library/sm_dancebot_ue/include/sm_dancebot_ue/clients/cl_nav2z/client_behaviors/cb_active_stop.hpp b/smacc2_sm_reference_library/sm_dancebot_ue/include/sm_dancebot_ue/clients/cl_nav2z/client_behaviors/cb_active_stop.hpp new file mode 100644 index 000000000..5f7e71773 --- /dev/null +++ b/smacc2_sm_reference_library/sm_dancebot_ue/include/sm_dancebot_ue/clients/cl_nav2z/client_behaviors/cb_active_stop.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 sm_dancebot_ue +{ +struct CbActiveStop : public smacc2::SmaccAsyncClientBehavior +{ +private: + + + rclcpp::Publisher::SharedPtr cmd_vel_pub_; + +public: + + CbActiveStop(); + + void onEntry() override; + + void onExit() override; +}; +} // namespace sm_dancebot_ue diff --git a/smacc2_sm_reference_library/sm_dancebot_ue/include/sm_dancebot_ue/clients/cl_nav2z/client_behaviors/cb_load_waypoints_file.hpp b/smacc2_sm_reference_library/sm_dancebot_ue/include/sm_dancebot_ue/clients/cl_nav2z/client_behaviors/cb_load_waypoints_file.hpp new file mode 100644 index 000000000..5da38588f --- /dev/null +++ b/smacc2_sm_reference_library/sm_dancebot_ue/include/sm_dancebot_ue/clients/cl_nav2z/client_behaviors/cb_load_waypoints_file.hpp @@ -0,0 +1,72 @@ +// 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 sm_dancebot_ue +{ +struct CbLoadWaypointsFile : public smacc2::SmaccClientBehavior +{ +public: + CbLoadWaypointsFile(std::string filepath) : filepath_(filepath) {} + + CbLoadWaypointsFile(std::string parameter_name, std::string packagenamesapce) + : + parameterName_(parameter_name), + packagenamesapce_(packagenamesapce) + { + } + + void onEntry() override + { + requiresComponent(waypointsNavigator_); // this is a component from the nav2z_client library + + if (filepath_) + { + RCLCPP_INFO_STREAM( + getNode()->get_logger(), "[CbLoadWaypointsFile] Loading waypoints from file: " << *filepath_); + this->waypointsNavigator_->loadWayPointsFromFile(filepath_.value()); + } + else + { + RCLCPP_INFO_STREAM( + getNode()->get_logger(), "[CbLoadWaypointsFile] Loading waypoints from parameter: " + << parameterName_.value() << " in package: " << packagenamesapce_.value()); + this->waypointsNavigator_->loadWaypointsFromYamlParameter( + parameterName_.value(), packagenamesapce_.value()); + } + + // change this to skip some points of the yaml file, default = 0 + waypointsNavigator_->currentWaypoint_ = 0; + } + + void onExit() override {} + + std::optional filepath_; + + std::optional parameterName_; + std::optional packagenamesapce_; + + cl_nav2z::CpWaypointNavigatorBase * waypointsNavigator_; +}; +} // namespace sm_dancebot_ue diff --git a/smacc2_sm_reference_library/sm_dancebot_ue/include/sm_dancebot_ue/clients/cl_nav2z/client_behaviors/cb_navigate_next_waypoint_free.hpp b/smacc2_sm_reference_library/sm_dancebot_ue/include/sm_dancebot_ue/clients/cl_nav2z/client_behaviors/cb_navigate_next_waypoint_free.hpp new file mode 100644 index 000000000..919f89446 --- /dev/null +++ b/smacc2_sm_reference_library/sm_dancebot_ue/include/sm_dancebot_ue/clients/cl_nav2z/client_behaviors/cb_navigate_next_waypoint_free.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 + +namespace sm_dancebot_ue +{ + +class CbNavigateNextWaypointFree : public sm_dancebot_ue::CbPositionControlFreeSpace +{ +public: + CbNavigateNextWaypointFree() { } + + virtual ~CbNavigateNextWaypointFree() {} + + void onEntry() override + { + requiresComponent(this->waypointsNavigator_); + this->target_pose_ = this->waypointsNavigator_->getCurrentPose(); + + this->onSuccess(&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 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 onExit() override + { + } + +protected: + cl_nav2z::CpWaypointNavigatorBase * waypointsNavigator_; +}; + +} // namespace sm_dancebot_ue diff --git a/smacc2_sm_reference_library/sm_dancebot_ue/include/sm_dancebot_ue/clients/cl_nav2z/client_behaviors/cb_position_control_free_space.hpp b/smacc2_sm_reference_library/sm_dancebot_ue/include/sm_dancebot_ue/clients/cl_nav2z/client_behaviors/cb_position_control_free_space.hpp new file mode 100644 index 000000000..c7c824f0e --- /dev/null +++ b/smacc2_sm_reference_library/sm_dancebot_ue/include/sm_dancebot_ue/clients/cl_nav2z/client_behaviors/cb_position_control_free_space.hpp @@ -0,0 +1,65 @@ +// 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 sm_dancebot_ue +{ +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 sm_dancebot_ue diff --git a/smacc2_sm_reference_library/sm_dancebot_ue/include/sm_dancebot_ue/clients/cl_nav2z/client_behaviors/cb_pure_spinning.hpp b/smacc2_sm_reference_library/sm_dancebot_ue/include/sm_dancebot_ue/clients/cl_nav2z/client_behaviors/cb_pure_spinning.hpp new file mode 100644 index 000000000..e11dd6aea --- /dev/null +++ b/smacc2_sm_reference_library/sm_dancebot_ue/include/sm_dancebot_ue/clients/cl_nav2z/client_behaviors/cb_pure_spinning.hpp @@ -0,0 +1,126 @@ +// 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 + +namespace sm_dancebot_ue +{ +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) + : targetYaw__rads(targetYaw_rads), + k_betta_(1.0), + max_angular_yaw_speed_(max_angular_yaw_speed), + yaw_goal_tolerance_rads_(0.1) + { + + } + + void updateParameters() + { + } + + void onEntry() override + { + auto nh = this->getNode(); + cmd_vel_pub_ = nh->create_publisher("/cmd_vel", rclcpp::QoS(1)); + + sm_dancebot_ue::CpUEPose* 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(), "["<cmd_vel_pub_->publish(cmd_vel); + + loop_rate.sleep(); + } + + this->postSuccessEvent(); + } + + void onExit() override + { + } + }; +} diff --git a/smacc2_sm_reference_library/sm_dancebot_ue/include/sm_dancebot_ue/clients/components/cp_ue_pose.hpp b/smacc2_sm_reference_library/sm_dancebot_ue/include/sm_dancebot_ue/clients/components/cp_ue_pose.hpp new file mode 100644 index 000000000..4db3b0a25 --- /dev/null +++ b/smacc2_sm_reference_library/sm_dancebot_ue/include/sm_dancebot_ue/clients/components/cp_ue_pose.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. + +/***************************************************************************************************************** + *-2020 + * Authors: Pablo Inigo Blasco, Brett Aldrich + * + ******************************************************************************************************************/ +#pragma once + +#include + +#include +#include +#include +#include +#include +#include +#include + +#include +#include +#include +#include +#include + +namespace sm_dancebot_ue +{ + +class CpUEPose : public smacc2::components::CpTopicSubscriber +{ +public: + CpUEPose(std::string topicname); + + void onInitialize() override; + + void onPoseMessageReceived(const ue_msgs::msg::EntityState& msg); + + geometry_msgs::msg::Pose toPoseMsg(); + +private: + ue_msgs::msg::EntityState entityStateMsg_; + +}; +} // namespace sm_dancebot_ue diff --git a/smacc2_sm_reference_library/sm_dancebot_ue/include/sm_dancebot_ue/modestates/ms_dance_bot_recovery_mode.hpp b/smacc2_sm_reference_library/sm_dancebot_ue/include/sm_dancebot_ue/modestates/ms_dance_bot_recovery_mode.hpp new file mode 100644 index 000000000..fc56219e3 --- /dev/null +++ b/smacc2_sm_reference_library/sm_dancebot_ue/include/sm_dancebot_ue/modestates/ms_dance_bot_recovery_mode.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 + * + ******************************************************************************************************************/ + +#include +namespace sm_dancebot_ue +{ +// STATE DECLARATION +class MsDanceBotRecoveryMode : public smacc2::SmaccState +{ +public: + using SmaccState::SmaccState; + + // TRANSITION TABLE + typedef mpl::list< + + Transition> + + >reactions; + // typedef Transition reactions; +}; +} // namespace sm_dancebot_ue diff --git a/smacc2_sm_reference_library/sm_dancebot_ue/include/sm_dancebot_ue/modestates/ms_dance_bot_run_mode.hpp b/smacc2_sm_reference_library/sm_dancebot_ue/include/sm_dancebot_ue/modestates/ms_dance_bot_run_mode.hpp new file mode 100644 index 000000000..d220c9f74 --- /dev/null +++ b/smacc2_sm_reference_library/sm_dancebot_ue/include/sm_dancebot_ue/modestates/ms_dance_bot_run_mode.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 + * + ******************************************************************************************************************/ + +#include +namespace sm_dancebot_ue +{ +// STATE DECLARATION +class MsDanceBotRunMode : public smacc2::SmaccState +{ +public: + using SmaccState::SmaccState; + + // TRANSITION TABLE + typedef mpl::list< + + Transition + + >reactions; +}; +} // namespace sm_dancebot_ue diff --git a/smacc2_sm_reference_library/sm_dancebot_ue/include/sm_dancebot_ue/orthogonals/or_navigation.hpp b/smacc2_sm_reference_library/sm_dancebot_ue/include/sm_dancebot_ue/orthogonals/or_navigation.hpp new file mode 100644 index 000000000..9e735ea56 --- /dev/null +++ b/smacc2_sm_reference_library/sm_dancebot_ue/include/sm_dancebot_ue/orthogonals/or_navigation.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 + +namespace sm_dancebot_ue +{ +using namespace ::cl_nav2z; +using namespace std::chrono_literals; + +class OrNavigation : public smacc2::Orthogonal +{ +public: + void onInitialize() override + { + auto nav2zClient = this->createClient(); + + nav2zClient->createComponent("/ue_ros/map_origin_entity_state"); + + /*auto waypointsNavigator = */nav2zClient->createComponent<::cl_nav2z::CpWaypointNavigatorBase>(); + } +}; +} // namespace sm_dancebot_ue diff --git a/smacc2_sm_reference_library/sm_dancebot_ue/include/sm_dancebot_ue/orthogonals/or_obstacle_perception.hpp b/smacc2_sm_reference_library/sm_dancebot_ue/include/sm_dancebot_ue/orthogonals/or_obstacle_perception.hpp new file mode 100644 index 000000000..449ee42ce --- /dev/null +++ b/smacc2_sm_reference_library/sm_dancebot_ue/include/sm_dancebot_ue/orthogonals/or_obstacle_perception.hpp @@ -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 + * + ******************************************************************************************************************/ + +#pragma once + +#include + +namespace sm_dancebot_ue +{ + using namespace std::chrono_literals; +class OrObstaclePerception : public smacc2::Orthogonal +{ +public: + void onInitialize() override + { + // auto lidarClient = this->createClient(); + + // lidarClient->topicName = "/scan"; + // lidarClient->timeout_ = rclcpp::Duration(10s); + } +}; +} // namespace sm_dancebot_ue diff --git a/smacc2_sm_reference_library/sm_dancebot_ue/include/sm_dancebot_ue/sm_dancebot_ue.hpp b/smacc2_sm_reference_library/sm_dancebot_ue/include/sm_dancebot_ue/sm_dancebot_ue.hpp new file mode 100644 index 000000000..1644ab4c1 --- /dev/null +++ b/smacc2_sm_reference_library/sm_dancebot_ue/include/sm_dancebot_ue/sm_dancebot_ue.hpp @@ -0,0 +1,149 @@ +// 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 + +// CLIENT BEHAVIORS +#include + +#include +#include + +// #include +#include +#include +#include +#include +#include + +#include +#include +#include + +#include + +//STATE REACTORS +#include +#include +#include + + +// ORTHOGONALS +#include +#include + +using namespace cl_nav2z; +using namespace smacc2::state_reactors; + +namespace sm_dancebot_ue +{ +//STATE FORWARD DECLARATIONS +class StAcquireSensors; +class StInitialRoadWaypointsX; +class StNavigateFieldWaypointsX; +class StBackOnRoadWaypointsX; +class StTurnAround; +class StFinalState; + +//SUPERSTATE FORWARD DECLARATIONS +//MODE STATES FORWARD DECLARATIONS +class MsDanceBotRunMode; +class MsDanceBotRecoveryMode; + +namespace SS1 +{ +class SsRadialPattern1; +} + +namespace SS2 +{ +class SsRadialPattern2; +} + +namespace SS3 +{ +class SsRadialPattern3; +} + +namespace SS4 +{ +class SsFPattern1; +} + +namespace SS5 +{ +class SsSPattern1; +} + +// custom smd_dance_bot event +struct EvGlobalError : sc::event +{ +}; + +} // namespace sm_dancebot_ue + +using namespace sm_dancebot_ue; +using namespace cl_ros_timer; +using namespace smacc2; + +namespace sm_dancebot_ue +{ +/// \brief Advanced example of state machine with smacc that shows multiple techniques +/// for the development of state machines +struct SmDanceBotUE : public smacc2::SmaccStateMachineBase +{ + typedef mpl::bool_ shallow_history; + typedef mpl::bool_ deep_history; + typedef mpl::bool_ inherited_deep_history; + + using SmaccStateMachineBase::SmaccStateMachineBase; + + void onInitialize() override + { + this->createOrthogonal(); + this->createOrthogonal(); + } +}; + +} // namespace sm_dancebot_ue + +//MODE STATES +#include + +#include + +//SUPERSTATES +#include +#include +#include +#include +#include + +//STATES +#include + +#include +#include +#include +#include +#include diff --git a/smacc2_sm_reference_library/sm_dancebot_ue/include/sm_dancebot_ue/states/f_pattern_states/sti_fpattern_forward_1.hpp b/smacc2_sm_reference_library/sm_dancebot_ue/include/sm_dancebot_ue/states/f_pattern_states/sti_fpattern_forward_1.hpp new file mode 100644 index 000000000..068dbaacf --- /dev/null +++ b/smacc2_sm_reference_library/sm_dancebot_ue/include/sm_dancebot_ue/states/f_pattern_states/sti_fpattern_forward_1.hpp @@ -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 + * + ******************************************************************************************************************/ + +namespace sm_dancebot_ue +{ +namespace f_pattern_states +{ +// STATE DECLARATION +template +struct StiFPatternForward1 : public smacc2::SmaccState, SS> +{ + typedef SmaccState, SS> TSti; + using TSti::context_type; + using TSti::SmaccState; + + // TRANSITION TABLE + typedef mpl::list< + + Transition, StiFPatternReturn1>, + Transition, StiFPatternReturn1, ABORT> + + + >reactions; + + // STATE FUNCTIONS + static void staticConfigure() + { + TSti::template configure_orthogonal(SS::ray_lenght_meters()); + TSti::template configure_orthogonal(); + } + + void runtimeConfigure() + { + // auto &superstate = TSti::template context(); + // RCLCPP_INFO(this->getLogger(),"[SsrFpattern] Fpattern rotate: SS current iteration: %d/%d", + // superstate.iteration_count, SS::total_iterations()); + } +}; +} // namespace f_pattern_states +} // namespace sm_dancebot_ue diff --git a/smacc2_sm_reference_library/sm_dancebot_ue/include/sm_dancebot_ue/states/f_pattern_states/sti_fpattern_forward_2.hpp b/smacc2_sm_reference_library/sm_dancebot_ue/include/sm_dancebot_ue/states/f_pattern_states/sti_fpattern_forward_2.hpp new file mode 100644 index 000000000..41c9be845 --- /dev/null +++ b/smacc2_sm_reference_library/sm_dancebot_ue/include/sm_dancebot_ue/states/f_pattern_states/sti_fpattern_forward_2.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 + * + ******************************************************************************************************************/ + +namespace sm_dancebot_ue +{ +namespace f_pattern_states +{ +// STATE DECLARATION +template +struct StiFPatternForward2 : smacc2::SmaccState, SS> +{ + typedef SmaccState, SS> TSti; + using TSti::context_type; + using TSti::SmaccState; + + // TRANSITION TABLE + typedef mpl::list< + + Transition, StiFPatternStartLoop>, + Transition, StiFPatternStartLoop> + + >reactions; + + // STATE FUNCTIONS + static void staticConfigure() + { + TSti::template configure_orthogonal(SS::pitch_lenght_meters()); + TSti::template configure_orthogonal(); + } + + void runtimeConfigure() {} +}; +} // namespace f_pattern_states +} // namespace sm_dancebot_ue diff --git a/smacc2_sm_reference_library/sm_dancebot_ue/include/sm_dancebot_ue/states/f_pattern_states/sti_fpattern_loop_start.hpp b/smacc2_sm_reference_library/sm_dancebot_ue/include/sm_dancebot_ue/states/f_pattern_states/sti_fpattern_loop_start.hpp new file mode 100644 index 000000000..018d3f171 --- /dev/null +++ b/smacc2_sm_reference_library/sm_dancebot_ue/include/sm_dancebot_ue/states/f_pattern_states/sti_fpattern_loop_start.hpp @@ -0,0 +1,55 @@ +// 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 + * + ******************************************************************************************************************/ + +namespace sm_dancebot_ue +{ +namespace f_pattern_states +{ +// STATE DECLARATION +template +struct StiFPatternStartLoop : smacc2::SmaccState, SS> +{ + typedef SmaccState, SS> TSti; + using TSti::context_type; + using TSti::SmaccState; + + // TRANSITION TABLE + typedef mpl::list< + + Transition>, StiFPatternRotate1, CONTINUELOOP> + + >reactions; + + // STATE FUNCTIONS + static void staticConfigure() {} + + bool loopCondition() + { + auto & superstate = TSti::template context(); + return superstate.iteration_count++ < superstate.total_iterations(); + } + + void onEntry() + { + TSti::checkWhileLoopConditionAndThrowEvent(&StiFPatternStartLoop::loopCondition); + } +}; +} // namespace f_pattern_states +} // namespace sm_dancebot_ue diff --git a/smacc2_sm_reference_library/sm_dancebot_ue/include/sm_dancebot_ue/states/f_pattern_states/sti_fpattern_return_1.hpp b/smacc2_sm_reference_library/sm_dancebot_ue/include/sm_dancebot_ue/states/f_pattern_states/sti_fpattern_return_1.hpp new file mode 100644 index 000000000..b13032859 --- /dev/null +++ b/smacc2_sm_reference_library/sm_dancebot_ue/include/sm_dancebot_ue/states/f_pattern_states/sti_fpattern_return_1.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 + * + ******************************************************************************************************************/ + +namespace sm_dancebot_ue +{ +namespace f_pattern_states +{ +// STATE DECLARATION +template +struct StiFPatternReturn1 : smacc2::SmaccState, SS> +{ + typedef SmaccState, SS> TSti; + using TSti::context_type; + using TSti::SmaccState; + + // TRANSITION TABLE + typedef mpl::list< + + Transition, StiFPatternRotate2> + + >reactions; + + // STATE FUNCTIONS + static void staticConfigure() + { + TSti::template configure_orthogonal(); + TSti::template configure_orthogonal(); + } + + void runtimeConfigure() {} +}; +} // namespace f_pattern_states +} // namespace sm_dancebot_ue diff --git a/smacc2_sm_reference_library/sm_dancebot_ue/include/sm_dancebot_ue/states/f_pattern_states/sti_fpattern_rotate_1.hpp b/smacc2_sm_reference_library/sm_dancebot_ue/include/sm_dancebot_ue/states/f_pattern_states/sti_fpattern_rotate_1.hpp new file mode 100644 index 000000000..f0f08adbb --- /dev/null +++ b/smacc2_sm_reference_library/sm_dancebot_ue/include/sm_dancebot_ue/states/f_pattern_states/sti_fpattern_rotate_1.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 + * + ******************************************************************************************************************/ + +namespace sm_dancebot_ue +{ +namespace f_pattern_states +{ +// STATE DECLARATION +template +struct StiFPatternRotate1 : smacc2::SmaccState, SS> +{ + typedef SmaccState, SS> TSti; + using TSti::context_type; + using TSti::SmaccState; + + // TRANSITION TABLE + typedef mpl::list< + + Transition, StiFPatternForward1>, + Transition, StiFPatternRotate1> + + >reactions; + + // STATE FUNCTIONS + static void staticConfigure() + { + float angle = 0; + double offset = -1.5; // for a better behaving + + if (SS::direction() == TDirection::LEFT) + angle = 90 + offset; + else + angle = -90 - offset; + + //TSti::template configure_orthogonal(angle); + TSti::template configure_orthogonal( + angle); // absolute aligned to the y-axis + } + + void runtimeConfigure() {} +}; +} // namespace f_pattern_states +} // namespace sm_dancebot_ue diff --git a/smacc2_sm_reference_library/sm_dancebot_ue/include/sm_dancebot_ue/states/f_pattern_states/sti_fpattern_rotate_2.hpp b/smacc2_sm_reference_library/sm_dancebot_ue/include/sm_dancebot_ue/states/f_pattern_states/sti_fpattern_rotate_2.hpp new file mode 100644 index 000000000..f44301bb8 --- /dev/null +++ b/smacc2_sm_reference_library/sm_dancebot_ue/include/sm_dancebot_ue/states/f_pattern_states/sti_fpattern_rotate_2.hpp @@ -0,0 +1,62 @@ +// 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 + * + ******************************************************************************************************************/ + +namespace sm_dancebot_ue +{ +namespace f_pattern_states +{ +// STATE DECLARATION +template +struct StiFPatternRotate2 : smacc2::SmaccState, SS> +{ + typedef SmaccState, SS> TSti; + using TSti::context_type; + using TSti::SmaccState; + + // TRANSITION TABLE + typedef mpl::list< + + Transition, StiFPatternForward2>, + Transition, StiFPatternRotate2, ABORT> + + >reactions; + + // STATE FUNCTIONS + static void staticConfigure() + { + double offset = 1.5; // for a better behaving + // float angle = 0; + + // if (SS::direction() == TDirection::LEFT) + // angle = -90 - offset; + // else + // angle = 90 + offset; + + //TSti::template configure_orthogonal(angle); + TSti::template configure_orthogonal( + 0 + offset); // absolute horizontal + + TSti::template configure_orthogonal(); + } + + void runtimeConfigure() {} +}; +} // namespace f_pattern_states +} // namespace sm_dancebot_ue diff --git a/smacc2_sm_reference_library/sm_dancebot_ue/include/sm_dancebot_ue/states/radial_motion_states/sti_radial_end_point.hpp b/smacc2_sm_reference_library/sm_dancebot_ue/include/sm_dancebot_ue/states/radial_motion_states/sti_radial_end_point.hpp new file mode 100644 index 000000000..26a457a00 --- /dev/null +++ b/smacc2_sm_reference_library/sm_dancebot_ue/include/sm_dancebot_ue/states/radial_motion_states/sti_radial_end_point.hpp @@ -0,0 +1,49 @@ +// 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 + * + ******************************************************************************************************************/ + +namespace sm_dancebot_ue +{ +namespace radial_motion_states +{ +// STATE DECLARATION +struct StiRadialEndPoint : smacc2::SmaccState +{ + using SmaccState::SmaccState; + + // TRANSITION TABLE + typedef mpl::list< + + Transition, StiRadialReturn, SUCCESS>, + Transition, StiRadialReturn, ABORT> + + >reactions; + + // STATE FUNCTIONS + static void staticConfigure() + { + //RCLCPP_INFO(getLogger(),"ssr radial end point, distance in meters: %lf", SS::ray_length_meters()); + configure_orthogonal(SS::ray_length_meters()); + configure_orthogonal(); + } + + void runtimeConfigure() {} +}; +} // namespace radial_motion_states +} // namespace sm_dancebot_ue diff --git a/smacc2_sm_reference_library/sm_dancebot_ue/include/sm_dancebot_ue/states/radial_motion_states/sti_radial_loop_start.hpp b/smacc2_sm_reference_library/sm_dancebot_ue/include/sm_dancebot_ue/states/radial_motion_states/sti_radial_loop_start.hpp new file mode 100644 index 000000000..dd378e441 --- /dev/null +++ b/smacc2_sm_reference_library/sm_dancebot_ue/include/sm_dancebot_ue/states/radial_motion_states/sti_radial_loop_start.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 + * + ******************************************************************************************************************/ + +namespace sm_dancebot_ue +{ +namespace radial_motion_states +{ +// STATE DECLARATION +struct StiRadialLoopStart : smacc2::SmaccState +{ + using SmaccState::SmaccState; + + // TRANSITION TABLE + typedef mpl::list< + + Transition, StiRadialRotate, CONTINUELOOP> + + >reactions; + + // STATE FUNCTIONS + static void staticConfigure() {} + + void runtimeConfigure() {} + + bool loopWhileCondition() + { + auto & superstate = this->context(); + + RCLCPP_INFO( + getLogger(), "Loop start, current iterations: %d, total iterations: %d", + superstate.iteration_count, superstate.total_iterations()); + return superstate.iteration_count++ < superstate.total_iterations(); + } + + void onEntry() + { + RCLCPP_INFO(getLogger(), "LOOP START ON ENTRY"); + checkWhileLoopConditionAndThrowEvent(&StiRadialLoopStart::loopWhileCondition); + } +}; +} // namespace radial_motion_states +} // namespace sm_dancebot_ue diff --git a/smacc2_sm_reference_library/sm_dancebot_ue/include/sm_dancebot_ue/states/radial_motion_states/sti_radial_return.hpp b/smacc2_sm_reference_library/sm_dancebot_ue/include/sm_dancebot_ue/states/radial_motion_states/sti_radial_return.hpp new file mode 100644 index 000000000..adbd6119f --- /dev/null +++ b/smacc2_sm_reference_library/sm_dancebot_ue/include/sm_dancebot_ue/states/radial_motion_states/sti_radial_return.hpp @@ -0,0 +1,55 @@ +// 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 + * + ******************************************************************************************************************/ + +namespace sm_dancebot_ue +{ +namespace radial_motion_states +{ +// STATE DECLARATION +struct StiRadialReturn : smacc2::SmaccState +{ + using SmaccState::SmaccState; + + // TRANSITION TABLE + typedef mpl::list< + + Transition, StiRadialLoopStart, SUCCESS>, + Transition, StiRadialEndPoint, ABORT> + + >reactions; + + // STATE FUNCTIONS + static void staticConfigure() + { + configure_orthogonal(); + configure_orthogonal(); + } + + void onExit() + { + ClNav2Z * moveBase; + this->requiresClient(moveBase); + + auto odomTracker = moveBase->getComponent(); + odomTracker->clearPath(); + } +}; +} // namespace radial_motion_states +} // namespace sm_dancebot_ue diff --git a/smacc2_sm_reference_library/sm_dancebot_ue/include/sm_dancebot_ue/states/radial_motion_states/sti_radial_rotate.hpp b/smacc2_sm_reference_library/sm_dancebot_ue/include/sm_dancebot_ue/states/radial_motion_states/sti_radial_rotate.hpp new file mode 100644 index 000000000..cb14b8951 --- /dev/null +++ b/smacc2_sm_reference_library/sm_dancebot_ue/include/sm_dancebot_ue/states/radial_motion_states/sti_radial_rotate.hpp @@ -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 + * + ******************************************************************************************************************/ + +namespace sm_dancebot_ue +{ +namespace radial_motion_states +{ +// STATE DECLARATION +struct StiRadialRotate : smacc2::SmaccState +{ + using SmaccState::SmaccState; + + // TRANSITION TABLE + typedef mpl::list< + + Transition, StiRadialEndPoint, SUCCESS>, + Transition, StiRadialRotate, ABORT> + + >reactions; + + // STATE FUNCTIONS + static void staticConfigure() + { + configure_orthogonal(); + configure_orthogonal(); + } + + void runtimeConfigure() + { + auto cbAbsRotate = this->getClientBehavior(); + + cbAbsRotate->spinningPlanner = SpinningPlanner::PureSpinning; + + auto & superstate = this->context(); + cbAbsRotate->absoluteGoalAngleDegree = + superstate.iteration_count * SS::ray_angle_increment_degree(); + } +}; +} // namespace radial_motion_states +} // namespace sm_dancebot_ue diff --git a/smacc2_sm_reference_library/sm_dancebot_ue/include/sm_dancebot_ue/states/s_pattern_states/sti_spattern_forward_1.hpp b/smacc2_sm_reference_library/sm_dancebot_ue/include/sm_dancebot_ue/states/s_pattern_states/sti_spattern_forward_1.hpp new file mode 100644 index 000000000..3ace5d7fb --- /dev/null +++ b/smacc2_sm_reference_library/sm_dancebot_ue/include/sm_dancebot_ue/states/s_pattern_states/sti_spattern_forward_1.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 + * + ******************************************************************************************************************/ + +namespace sm_dancebot_ue +{ +namespace s_pattern_states +{ +// STATE DECLARATION +struct StiSPatternForward1 : public smacc2::SmaccState +{ + using SmaccState::SmaccState; + + // TRANSITION TABLE + typedef mpl::list< + + Transition, StiSPatternRotate2>, + Transition, StiSPatternRotate1> + + >reactions; + + // STATE FUNCTIONS + static void staticConfigure() + { + configure_orthogonal(SS::pitch1_lenght_meters()); + configure_orthogonal(); + } + + void runtimeConfigure() {} +}; +} // namespace s_pattern_states +} // namespace sm_dancebot_ue diff --git a/smacc2_sm_reference_library/sm_dancebot_ue/include/sm_dancebot_ue/states/s_pattern_states/sti_spattern_forward_2.hpp b/smacc2_sm_reference_library/sm_dancebot_ue/include/sm_dancebot_ue/states/s_pattern_states/sti_spattern_forward_2.hpp new file mode 100644 index 000000000..056283f03 --- /dev/null +++ b/smacc2_sm_reference_library/sm_dancebot_ue/include/sm_dancebot_ue/states/s_pattern_states/sti_spattern_forward_2.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 + * + ******************************************************************************************************************/ + +namespace sm_dancebot_ue +{ +namespace s_pattern_states +{ +// STATE DECLARATION +struct StiSPatternForward2 : public smacc2::SmaccState +{ + using SmaccState::SmaccState; + + // TRANSITION TABLE + typedef mpl::list< + + Transition, StiSPatternRotate3>, + Transition, StiSPatternRotate2> + + >reactions; + + // STATE FUNCTIONS + static void staticConfigure() + { + configure_orthogonal(SS::pitch2_lenght_meters()); + configure_orthogonal(); + } + + void runtimeConfigure() {} +}; +} // namespace s_pattern_states +} // namespace sm_dancebot_ue diff --git a/smacc2_sm_reference_library/sm_dancebot_ue/include/sm_dancebot_ue/states/s_pattern_states/sti_spattern_forward_3.hpp b/smacc2_sm_reference_library/sm_dancebot_ue/include/sm_dancebot_ue/states/s_pattern_states/sti_spattern_forward_3.hpp new file mode 100644 index 000000000..9546b65ac --- /dev/null +++ b/smacc2_sm_reference_library/sm_dancebot_ue/include/sm_dancebot_ue/states/s_pattern_states/sti_spattern_forward_3.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 + * + ******************************************************************************************************************/ + +namespace sm_dancebot_ue +{ +namespace s_pattern_states +{ +// STATE DECLARATION +struct StiSPatternForward3 : public smacc2::SmaccState +{ + using SmaccState::SmaccState; + + // TRANSITION TABLE + typedef mpl::list< + + Transition, StiSPatternRotate4>, + Transition, StiSPatternRotate3> + + >reactions; + + // STATE FUNCTIONS + static void staticConfigure() + { + configure_orthogonal(SS::pitch1_lenght_meters()); + configure_orthogonal(); + } +}; +} // namespace s_pattern_states +} // namespace sm_dancebot_ue diff --git a/smacc2_sm_reference_library/sm_dancebot_ue/include/sm_dancebot_ue/states/s_pattern_states/sti_spattern_forward_4.hpp b/smacc2_sm_reference_library/sm_dancebot_ue/include/sm_dancebot_ue/states/s_pattern_states/sti_spattern_forward_4.hpp new file mode 100644 index 000000000..6da920092 --- /dev/null +++ b/smacc2_sm_reference_library/sm_dancebot_ue/include/sm_dancebot_ue/states/s_pattern_states/sti_spattern_forward_4.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 + * + ******************************************************************************************************************/ + +namespace sm_dancebot_ue +{ +namespace s_pattern_states +{ +// STATE DECLARATION +struct StiSPatternForward4 : public smacc2::SmaccState +{ + using SmaccState::SmaccState; + + // TRANSITION TABLE + typedef mpl::list< + + Transition, StiSPatternLoopStart>, + Transition, StiSPatternRotate4> + + >reactions; + + // STATE FUNCTIONS + static void staticConfigure() {} + + void runtimeConfigure() + { + //auto &superstate = this->context(); + + this->configure(SS::pitch2_lenght_meters()); + this->configure(); + } +}; +} // namespace s_pattern_states +} // namespace sm_dancebot_ue diff --git a/smacc2_sm_reference_library/sm_dancebot_ue/include/sm_dancebot_ue/states/s_pattern_states/sti_spattern_loop_start.hpp b/smacc2_sm_reference_library/sm_dancebot_ue/include/sm_dancebot_ue/states/s_pattern_states/sti_spattern_loop_start.hpp new file mode 100644 index 000000000..3b6a0f7c1 --- /dev/null +++ b/smacc2_sm_reference_library/sm_dancebot_ue/include/sm_dancebot_ue/states/s_pattern_states/sti_spattern_loop_start.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 + * + ******************************************************************************************************************/ + +namespace sm_dancebot_ue +{ +namespace s_pattern_states +{ +template +struct B : smacc2::SmaccState +{ +public: + using SmaccState::SmaccState; + //typedef typename TDerived::reactions reactions; +}; + +// STATE DECLARATION +struct StiSPatternLoopStart : public B +{ + using B::B; + + // TRANSITION TABLE + typedef mpl::list< + + Transition, StiSPatternRotate1, CONTINUELOOP> + + >reactions; + + // STATE FUNCTIONS + static void staticConfigure() {} + + void runtimeConfigure() {} + + bool loopCondition() + { + auto & superstate = this->context(); + return superstate.iteration_count++ < superstate.total_iterations(); + } + + void onEntry() { checkWhileLoopConditionAndThrowEvent(&StiSPatternLoopStart::loopCondition); } +}; + +} // namespace s_pattern_states +} // namespace sm_dancebot_ue diff --git a/smacc2_sm_reference_library/sm_dancebot_ue/include/sm_dancebot_ue/states/s_pattern_states/sti_spattern_rotate_1.hpp b/smacc2_sm_reference_library/sm_dancebot_ue/include/sm_dancebot_ue/states/s_pattern_states/sti_spattern_rotate_1.hpp new file mode 100644 index 000000000..a54600e68 --- /dev/null +++ b/smacc2_sm_reference_library/sm_dancebot_ue/include/sm_dancebot_ue/states/s_pattern_states/sti_spattern_rotate_1.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 + * + ******************************************************************************************************************/ + +namespace sm_dancebot_ue +{ +namespace s_pattern_states +{ +// STATE DECLARATION +struct StiSPatternRotate1 : smacc2::SmaccState +{ + using SmaccState::SmaccState; + + // TRANSITION TABLE + typedef mpl::list< + + Transition, StiSPatternForward1>, + Transition, StiSPatternRotate1> + + >reactions; + + // STATE FUNCTIONS + static void staticConfigure() {} + + void runtimeConfigure() + { + auto & superstate = this->context(); + + float offset = 0; + if (superstate.direction() == TDirection::RIGHT) + { + // - offset because we are looking to the north and we have to turn clockwise + this->configure(0 - offset); + } + else + { + // - offset because we are looking to the south and we have to turn counter-clockwise + this->configure(180 + offset); + } + } +}; +} // namespace s_pattern_states +} // namespace sm_dancebot_ue diff --git a/smacc2_sm_reference_library/sm_dancebot_ue/include/sm_dancebot_ue/states/s_pattern_states/sti_spattern_rotate_2.hpp b/smacc2_sm_reference_library/sm_dancebot_ue/include/sm_dancebot_ue/states/s_pattern_states/sti_spattern_rotate_2.hpp new file mode 100644 index 000000000..782587112 --- /dev/null +++ b/smacc2_sm_reference_library/sm_dancebot_ue/include/sm_dancebot_ue/states/s_pattern_states/sti_spattern_rotate_2.hpp @@ -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 + * + ******************************************************************************************************************/ + +namespace sm_dancebot_ue +{ +namespace s_pattern_states +{ +// STATE DECLARATION +struct StiSPatternRotate2 : smacc2::SmaccState +{ + using SmaccState::SmaccState; + + // TRANSITION TABLE + typedef mpl::list< + + Transition, StiSPatternForward2>, + Transition, StiSPatternForward2> + + >reactions; + + // STATE FUNCTIONS + static void staticConfigure() + { + float offset = 0; + float angle = 0; + if (SS::direction() == TDirection::LEFT) + angle = 90 + offset; + else + angle = -90 - offset; + + configure_orthogonal(angle); + configure_orthogonal(); + } + + void runtimeConfigure() + { + auto & superstate = this->context(); + RCLCPP_INFO( + getLogger(), "[StiSPatternRotate] SpatternRotate rotate: SS current iteration: %d/%d", + superstate.iteration_count, SS::total_iterations()); + } +}; +} // namespace s_pattern_states +} // namespace sm_dancebot_ue diff --git a/smacc2_sm_reference_library/sm_dancebot_ue/include/sm_dancebot_ue/states/s_pattern_states/sti_spattern_rotate_3.hpp b/smacc2_sm_reference_library/sm_dancebot_ue/include/sm_dancebot_ue/states/s_pattern_states/sti_spattern_rotate_3.hpp new file mode 100644 index 000000000..1919ddc39 --- /dev/null +++ b/smacc2_sm_reference_library/sm_dancebot_ue/include/sm_dancebot_ue/states/s_pattern_states/sti_spattern_rotate_3.hpp @@ -0,0 +1,72 @@ +// 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 + * + ******************************************************************************************************************/ + +namespace sm_dancebot_ue +{ +namespace s_pattern_states +{ +// STATE DECLARATION +struct StiSPatternRotate3 : smacc2::SmaccState +{ + using SmaccState::SmaccState; + + // TRANSITION TABLE + typedef mpl::list< + + Transition, StiSPatternForward3>, + Transition, StiSPatternRotate3> + + >reactions; + + // STATE FUNCTIONS + static void staticConfigure() {} + + void runtimeConfigure() + { + auto & superstate = this->context(); + RCLCPP_INFO( + getLogger(), "[StiSPatternRotate] SpatternRotate rotate: SS current iteration: %d/%d", + superstate.iteration_count, SS::total_iterations()); + + float offset = 0; + // float angle = 0; + // if (superstate.direction() == TDirection::LEFT) + // angle = -90 - offset; + // else + // angle = +90 + offset; + + // this->configure(angle); + + if (superstate.direction() == TDirection::RIGHT) + { + // - offset because we are looking to the north and we have to turn clockwise + this->configure(0 - offset); + } + else + { + // - offset because we are looking to the south and we have to turn counter-clockwise + this->configure(180 + offset); + } + + this->configure(); + } +}; +} // namespace s_pattern_states +} // namespace sm_dancebot_ue diff --git a/smacc2_sm_reference_library/sm_dancebot_ue/include/sm_dancebot_ue/states/s_pattern_states/sti_spattern_rotate_4.hpp b/smacc2_sm_reference_library/sm_dancebot_ue/include/sm_dancebot_ue/states/s_pattern_states/sti_spattern_rotate_4.hpp new file mode 100644 index 000000000..5d1a9bbb2 --- /dev/null +++ b/smacc2_sm_reference_library/sm_dancebot_ue/include/sm_dancebot_ue/states/s_pattern_states/sti_spattern_rotate_4.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 + * + ******************************************************************************************************************/ + +namespace sm_dancebot_ue +{ +namespace s_pattern_states +{ +// STATE DECLARATION +struct StiSPatternRotate4 : smacc2::SmaccState +{ + using SmaccState::SmaccState; + + // TRANSITION TABLE + typedef mpl::list< + + Transition, StiSPatternForward4>, + Transition, StiSPatternRotate4> + + >reactions; + + // STATE FUNCTIONS + static void staticConfigure() {} + + void runtimeConfigure() + { + auto & superstate = this->context(); + RCLCPP_INFO( + getLogger(), "[SsrSPatternRotate] SpatternRotate rotate: SS current iteration: %d/%d", + superstate.iteration_count, SS::total_iterations()); + + float offset = 0; + float angle = 0; + if (superstate.direction() == TDirection::LEFT) + angle = -90 - offset; + else + angle = 90 + offset; + + this->configure(angle); + this->configure(); + } +}; +} // namespace s_pattern_states +} // namespace sm_dancebot_ue diff --git a/smacc2_sm_reference_library/sm_dancebot_ue/include/sm_dancebot_ue/states/st_acquire_sensors.hpp b/smacc2_sm_reference_library/sm_dancebot_ue/include/sm_dancebot_ue/states/st_acquire_sensors.hpp new file mode 100644 index 000000000..0f1e08264 --- /dev/null +++ b/smacc2_sm_reference_library/sm_dancebot_ue/include/sm_dancebot_ue/states/st_acquire_sensors.hpp @@ -0,0 +1,73 @@ +// 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 + +namespace sm_dancebot_ue +{ +using namespace smacc2::default_events; +using smacc2::client_behaviors::CbSleepFor; +using namespace std::chrono_literals; + +// STATE DECLARATION +struct StAcquireSensors : smacc2::SmaccState +{ + using SmaccState::SmaccState; + + // DECLARE CUSTOM OBJECT TAGS + struct ON_SENSORS_AVAILABLE : SUCCESS{}; + struct SrAcquireSensors; + + // TRANSITION TABLE + typedef mpl::list< + + // + Transition, StInitialRoadWaypointsX, ON_SENSORS_AVAILABLE>, + Transition + + >reactions; + + // STATE FUNCTIONS + static void staticConfigure() + { + // configure_orthogonal("Hello World!"); + // configure_orthogonal(); + // configure_orthogonal(Service3Command::SERVICE3_ON); + // configure_orthogonal(); + // configure_orthogonal(); + configure_orthogonal("waypoints_plan_initial_road", "sm_dancebot_ue"); + configure_orthogonal(5s); + + + + + // Create State Reactor + auto srAllSensorsReady = static_createStateReactor< + SrAllEventsGo, smacc2::state_reactors::EvAllGo, + mpl::list< + // EvTopicMessage, + // EvCbSuccess, + EvCbSuccess + >>(); + } +}; +} // namespace sm_dancebot_ue diff --git a/smacc2_sm_reference_library/sm_dancebot_ue/include/sm_dancebot_ue/states/st_back_on_road_waypoints_x.hpp b/smacc2_sm_reference_library/sm_dancebot_ue/include/sm_dancebot_ue/states/st_back_on_road_waypoints_x.hpp new file mode 100644 index 000000000..563031195 --- /dev/null +++ b/smacc2_sm_reference_library/sm_dancebot_ue/include/sm_dancebot_ue/states/st_back_on_road_waypoints_x.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 + +namespace sm_dancebot_ue +{ + +// STATE DECLARATION +struct StBackOnRoadWaypointsX : smacc2::SmaccState +{ + using SmaccState::SmaccState; + + // CUSTOM TRANSITION TAGS + struct TRANSITION_1 : SUCCESS{}; + struct TRANSITION_2 : SUCCESS{}; + struct TRANSITION_3 : SUCCESS{}; + struct TRANSITION_4 : SUCCESS{}; + struct TRANSITION_5 : SUCCESS{}; + struct TRANSITION_6 : SUCCESS{}; + + // TRANSITION TABLE + typedef mpl::list< + Transition, + Transition, StBackOnRoadWaypointsX, TRANSITION_1> + // Transition, StNavigateToWaypointsX, TRANSITION_2>, + // Transition, SS1::SsRadialPattern1, TRANSITION_3>, + // Transition, SS2::SsRadialPattern2, TRANSITION_4> + >reactions; + + // STATE FUNCTIONS + static void staticConfigure() + { + // configure_orthogonal(); + configure_orthogonal("waypoints_plan_back_on_road", "sm_dancebot_ue"); + configure_orthogonal(); + } + + void onEntry() + { + } + + void runtimeConfigure() {} + + void onExit(ABORT) + { + } +}; +} // namespace sm_dancebot_ue diff --git a/smacc2_sm_reference_library/sm_dancebot_ue/include/sm_dancebot_ue/states/st_final_state.hpp b/smacc2_sm_reference_library/sm_dancebot_ue/include/sm_dancebot_ue/states/st_final_state.hpp new file mode 100644 index 000000000..fb92c19e3 --- /dev/null +++ b/smacc2_sm_reference_library/sm_dancebot_ue/include/sm_dancebot_ue/states/st_final_state.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 + +namespace sm_dancebot_ue +{ +using namespace smacc2::default_events; +using smacc2::client_behaviors::CbSleepFor; +using namespace std::chrono_literals; + +// STATE DECLARATION +struct StFinalState : smacc2::SmaccState +{ + using SmaccState::SmaccState; + + // TRANSITION TABLE + typedef mpl::list< + >reactions; + + // STATE FUNCTIONS + static void staticConfigure() + { + configure_orthogonal(); + } + + void runtimeConfigure() + { + + } +}; +} // namespace sm_dancebot_ue diff --git a/smacc2_sm_reference_library/sm_dancebot_ue/include/sm_dancebot_ue/states/st_inital_road_waypoints_x.hpp b/smacc2_sm_reference_library/sm_dancebot_ue/include/sm_dancebot_ue/states/st_inital_road_waypoints_x.hpp new file mode 100644 index 000000000..e5dec75dc --- /dev/null +++ b/smacc2_sm_reference_library/sm_dancebot_ue/include/sm_dancebot_ue/states/st_inital_road_waypoints_x.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 + +namespace sm_dancebot_ue +{ + +// STATE DECLARATION +struct StInitialRoadWaypointsX : smacc2::SmaccState +{ + using SmaccState::SmaccState; + + // CUSTOM TRANSITION TAGS + struct TRANSITION_1 : SUCCESS{}; + struct TRANSITION_2 : SUCCESS{}; + struct TRANSITION_3 : SUCCESS{}; + struct TRANSITION_4 : SUCCESS{}; + struct TRANSITION_5 : SUCCESS{}; + struct TRANSITION_6 : SUCCESS{}; + + // TRANSITION TABLE + typedef mpl::list< + Transition, + //Transition, StTurnAround, TRANSITION_1>, + Transition, StInitialRoadWaypointsX, TRANSITION_2> + >reactions; + + // STATE FUNCTIONS + static void staticConfigure() + { + configure_orthogonal(); + } + + void onEntry() + { + } + + void runtimeConfigure() {} + + void onExit(ABORT) + { + } +}; +} // namespace sm_dancebot_ue diff --git a/smacc2_sm_reference_library/sm_dancebot_ue/include/sm_dancebot_ue/states/st_navigate_field_waypoints_x.hpp b/smacc2_sm_reference_library/sm_dancebot_ue/include/sm_dancebot_ue/states/st_navigate_field_waypoints_x.hpp new file mode 100644 index 000000000..89556a636 --- /dev/null +++ b/smacc2_sm_reference_library/sm_dancebot_ue/include/sm_dancebot_ue/states/st_navigate_field_waypoints_x.hpp @@ -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 + * + ******************************************************************************************************************/ + +#pragma once + +#include + +namespace sm_dancebot_ue +{ + +// STATE DECLARATION +struct StNavigateFieldWaypointsX : smacc2::SmaccState +{ + using SmaccState::SmaccState; + + // CUSTOM TRANSITION TAGS + struct TRANSITION_1 : SUCCESS{}; + struct TRANSITION_2 : SUCCESS{}; + struct TRANSITION_3 : SUCCESS{}; + struct TRANSITION_4 : SUCCESS{}; + struct TRANSITION_5 : SUCCESS{}; + struct TRANSITION_6 : SUCCESS{}; + + // TRANSITION TABLE + typedef mpl::list< + Transition, StNavigateFieldWaypointsX, TRANSITION_1> + >reactions; + + // STATE FUNCTIONS + static void staticConfigure() + { + // configure_orthogonal(); + configure_orthogonal(); + } + + void onEntry() + { + } + + void runtimeConfigure() {} + + void onExit(ABORT) + { + } +}; +} // namespace sm_dancebot_ue diff --git a/smacc2_sm_reference_library/sm_dancebot_ue/include/sm_dancebot_ue/states/st_turn_around.hpp b/smacc2_sm_reference_library/sm_dancebot_ue/include/sm_dancebot_ue/states/st_turn_around.hpp new file mode 100644 index 000000000..683c29426 --- /dev/null +++ b/smacc2_sm_reference_library/sm_dancebot_ue/include/sm_dancebot_ue/states/st_turn_around.hpp @@ -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 + * + ******************************************************************************************************************/ + +#pragma once + +#include + +namespace sm_dancebot_ue +{ +using namespace smacc2::default_events; +using smacc2::client_behaviors::CbSleepFor; +using namespace std::chrono_literals; + +// STATE DECLARATION +struct StTurnAround : smacc2::SmaccState +{ + using SmaccState::SmaccState; + + // TRANSITION TABLE + typedef mpl::list< + Transition, StBackOnRoadWaypointsX, SUCCESS> + >reactions; + + // STATE FUNCTIONS + static void staticConfigure() + { + configure_orthogonal(M_PI, 0.8); + } + + void runtimeConfigure() + { + auto spinningBehavior = this->getOrthogonal()->getClientBehavior(); + spinningBehavior->yaw_goal_tolerance_rads_ = 0.2; + + } +}; +} // namespace sm_dancebot_ue diff --git a/smacc2_sm_reference_library/sm_dancebot_ue/include/sm_dancebot_ue/superstates/ss_f_pattern_1.hpp b/smacc2_sm_reference_library/sm_dancebot_ue/include/sm_dancebot_ue/superstates/ss_f_pattern_1.hpp new file mode 100644 index 000000000..3879606ac --- /dev/null +++ b/smacc2_sm_reference_library/sm_dancebot_ue/include/sm_dancebot_ue/superstates/ss_f_pattern_1.hpp @@ -0,0 +1,91 @@ +// 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 sm_dancebot_ue +{ +namespace f_pattern_states +{ +enum class TDirection +{ + LEFT, + RIGHT +}; + +// FORWARD DECLARATIONS OF INNER STATES +template class StiFPatternRotate1; +template class StiFPatternForward1; +template class StiFPatternReturn1; +template class StiFPatternRotate2; +template class StiFPatternForward2; +template class StiFPatternStartLoop; + +} // namespace f_pattern_states +} // namespace sm_dancebot_ue +namespace sm_dancebot_ue +{ +namespace SS4 +{ +using namespace f_pattern_states; + +// STATE DECLARATION +struct SsFPattern1 +: smacc2::SmaccState> +{ +public: + using SmaccState::SmaccState; + + // TRANSITION TABLE + typedef mpl::list< + + // Transition>, StNavigateReverse4, ENDLOOP> //, + + >reactions; + + // STATE VARIABLES + // superstate parameters + static constexpr float ray_lenght_meters() { return 3.25; } + static constexpr float pitch_lenght_meters() { return 0.75; } + static constexpr int total_iterations() { return 10; } + static constexpr TDirection direction() { return TDirection::RIGHT; } + + // superstate state variables + int iteration_count; + + // STATE FUNCTIONS + static void staticConfigure() + { + } + + void runtimeConfigure() { iteration_count = 0; } +}; // namespace SS4 + +// FORWARD DECLARATION FOR THE SUPERSTATE + +} // namespace SS4 +} // namespace sm_dancebot_ue + +#include +#include +#include +#include +#include +#include diff --git a/smacc2_sm_reference_library/sm_dancebot_ue/include/sm_dancebot_ue/superstates/ss_radial_pattern_1.hpp b/smacc2_sm_reference_library/sm_dancebot_ue/include/sm_dancebot_ue/superstates/ss_radial_pattern_1.hpp new file mode 100644 index 000000000..a0c3b2166 --- /dev/null +++ b/smacc2_sm_reference_library/sm_dancebot_ue/include/sm_dancebot_ue/superstates/ss_radial_pattern_1.hpp @@ -0,0 +1,77 @@ +// 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 sm_dancebot_ue +{ +namespace SS1 +{ +namespace sm_dancebot_ue +{ +namespace radial_motion_states +{ +//FORWARD DECLARATION OF INNER STATES +class StiRadialRotate; +class StiRadialReturn; +class StiRadialEndPoint; +class StiRadialLoopStart; + +} // namespace radial_motion_states +} // namespace sm_dancebot_ue +using namespace sm_dancebot_ue::radial_motion_states; + +// STATE DECLARATION +struct SsRadialPattern1 +: smacc2::SmaccState +{ +public: + using SmaccState::SmaccState; + + // TRANSITION TABLE + typedef mpl::list< + + // Transition, StRotateDegrees1, ENDLOOP> + // Transition, StNavigateReverse1, ENDLOOP> + + >reactions; + + static constexpr int total_iterations() { return 16; } + static constexpr float ray_angle_increment_degree() { return 360.0 / total_iterations(); } + static constexpr float ray_length_meters() { return 1.0; } + + int iteration_count = 0; + + // STATE FUNCTIONS + static void staticConfigure() + { + } + + void runtimeConfigure() {} +}; + +// FORWARD DECLARATION FOR THE SUPERSTATE +using SS = SsRadialPattern1; +#include +#include +#include +#include +} // namespace SS1 +} // namespace sm_dancebot_ue diff --git a/smacc2_sm_reference_library/sm_dancebot_ue/include/sm_dancebot_ue/superstates/ss_radial_pattern_2.hpp b/smacc2_sm_reference_library/sm_dancebot_ue/include/sm_dancebot_ue/superstates/ss_radial_pattern_2.hpp new file mode 100644 index 000000000..6e573b351 --- /dev/null +++ b/smacc2_sm_reference_library/sm_dancebot_ue/include/sm_dancebot_ue/superstates/ss_radial_pattern_2.hpp @@ -0,0 +1,80 @@ +// 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 sm_dancebot_ue +{ +namespace SS2 +{ +namespace sm_dancebot_ue +{ +namespace radial_motion_states +{ +// FORWARD DECLARATIONS OF INNER STATES +class StiRadialRotate; +class StiRadialReturn; +class StiRadialEndPoint; +class StiRadialLoopStart; +} // namespace radial_motion_states +} // namespace sm_dancebot_ue +using namespace sm_dancebot_ue::radial_motion_states; + +// STATE DECLARATION +struct SsRadialPattern2 +: smacc2::SmaccState +{ +public: + using SmaccState::SmaccState; + + // TRANSITION TABLE + typedef mpl::list< + + // Transition, StNavigateReverse1, ENDLOOP> + + >reactions; + + // STATE FUNCTIONS + static void staticConfigure() + { + } + + void runtimeConfigure() {} + + int iteration_count = 0; + static constexpr int total_iterations() { return 16; } + static constexpr float ray_angle_increment_degree() { return 360.0 / total_iterations(); } + static constexpr float ray_length_meters() { return 5; } + + void onExit() + { + rclcpp::sleep_for(5s); + RCLCPP_INFO(getLogger(), "[SsRadialPattern1] waiting 5 seconds"); + } +}; + +// FORWARD DECLARATION FOR THE SUPERSTATE +using SS = SsRadialPattern2; +#include +#include +#include +#include +} // namespace SS2 +} // namespace sm_dancebot_ue diff --git a/smacc2_sm_reference_library/sm_dancebot_ue/include/sm_dancebot_ue/superstates/ss_radial_pattern_3.hpp b/smacc2_sm_reference_library/sm_dancebot_ue/include/sm_dancebot_ue/superstates/ss_radial_pattern_3.hpp new file mode 100644 index 000000000..026450d5e --- /dev/null +++ b/smacc2_sm_reference_library/sm_dancebot_ue/include/sm_dancebot_ue/superstates/ss_radial_pattern_3.hpp @@ -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 sm_dancebot_ue +{ +namespace SS3 +{ +namespace sm_dancebot_ue +{ +namespace radial_motion_states +{ +// FORWARD DECLARATION OF INNER STATES +class StiRadialRotate; +class StiRadialReturn; +class StiRadialEndPoint; +class StiRadialLoopStart; +} // namespace radial_motion_states +} // namespace sm_dancebot_ue +using namespace sm_dancebot_ue::radial_motion_states; + +// STATE DECLARATION +struct SsRadialPattern3 +: smacc2::SmaccState +{ +public: + using SmaccState::SmaccState; + + // TRANSITION TABLE + typedef mpl::list< + + // Transition, StRotateDegrees4, ENDLOOP> + + >reactions; + + // STATE FUNCTIONS + static void staticConfigure() + { + } + + int iteration_count; + + static constexpr int total_iterations() { return 16; } + static constexpr float ray_angle_increment_degree() { return 360.0 / total_iterations(); } + static constexpr float ray_length_meters() { return 4; } + + void runtimeConfigure() { iteration_count = 0; } +}; + +// FORWARD DECLARATION FOR THE SUPERSTATE +using SS = SsRadialPattern3; +#include +#include +#include +#include +} // namespace SS3 +} // namespace sm_dancebot_ue diff --git a/smacc2_sm_reference_library/sm_dancebot_ue/include/sm_dancebot_ue/superstates/ss_s_pattern_1.hpp b/smacc2_sm_reference_library/sm_dancebot_ue/include/sm_dancebot_ue/superstates/ss_s_pattern_1.hpp new file mode 100644 index 000000000..28f361349 --- /dev/null +++ b/smacc2_sm_reference_library/sm_dancebot_ue/include/sm_dancebot_ue/superstates/ss_s_pattern_1.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 + * + ******************************************************************************************************************/ + +#include + +namespace sm_dancebot_ue +{ +namespace SS5 +{ +namespace sm_dancebot_ue +{ +namespace s_pattern_states +{ +// FORWARD DECLARATIONS OF INNER STATES +class StiSPatternRotate1; +class StiSPatternForward1; +class StiSPatternRotate2; +class StiSPatternForward2; +class StiSPatternRotate3; +class StiSPatternForward3; +class StiSPatternRotate4; +class StiSPatternForward4; +class StiSPatternLoopStart; +} // namespace s_pattern_states +} // namespace sm_dancebot_ue + +enum class TDirection +{ + LEFT, + RIGHT +}; + +using namespace sm_dancebot_ue::s_pattern_states; + +// STATE DECLARATION +struct SsSPattern1 : smacc2::SmaccState +{ +public: + using SmaccState::SmaccState; + + // TRANSITION TABLE + typedef mpl::list< + + // Transition, StNavigateReverse3, ENDLOOP> + + >reactions; + + // STATE FUNCTIONS + static void staticConfigure() + { + } + + static constexpr float pitch1_lenght_meters() { return 0.75; } + static constexpr float pitch2_lenght_meters() { return 1.45; } + static constexpr int total_iterations() { return 9; } + static constexpr TDirection direction() { return TDirection::RIGHT; } + + int iteration_count; + + void runtimeConfigure() { this->iteration_count = 0; } +}; + +// FORWARD DECLARATION FOR THE SUPERSTATE +using SS = SsSPattern1; +#include +#include +#include +#include +#include +#include +#include +#include +#include +} // namespace SS5 +} // namespace sm_dancebot_ue diff --git a/smacc2_sm_reference_library/sm_dancebot_ue/launch/bringup_launch.py b/smacc2_sm_reference_library/sm_dancebot_ue/launch/bringup_launch.py new file mode 100644 index 000000000..c7ebca467 --- /dev/null +++ b/smacc2_sm_reference_library/sm_dancebot_ue/launch/bringup_launch.py @@ -0,0 +1,137 @@ +# Copyright (c) 2018 Intel Corporation +# +# Licensed under the Apache License, Version 2.0 (the "License"); +# you may not use this file except in compliance with the License. +# You may obtain a copy of the License at +# +# http://www.apache.org/licenses/LICENSE-2.0 +# +# Unless required by applicable law or agreed to in writing, software +# distributed under the License is distributed on an "AS IS" BASIS, +# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +# See the License for the specific language governing permissions and +# limitations under the License. + +import os + +from ament_index_python.packages import get_package_share_directory + +from launch import LaunchDescription +from launch.actions import ( + DeclareLaunchArgument, + GroupAction, + IncludeLaunchDescription, + SetEnvironmentVariable, +) +from launch.conditions import IfCondition +from launch.launch_description_sources import PythonLaunchDescriptionSource +from launch.substitutions import LaunchConfiguration +from launch_ros.actions import PushRosNamespace + + +def generate_launch_description(): + # Get the launch directory + sm_dance_bot_dir = get_package_share_directory("sm_dancebot_ue") + launch_dir = os.path.join(sm_dance_bot_dir, "launch") + + # Create the launch configuration variables + namespace = LaunchConfiguration("namespace") + use_namespace = LaunchConfiguration("use_namespace") + slam = LaunchConfiguration("slam") + # map_yaml_file = LaunchConfiguration("map") + use_sim_time = LaunchConfiguration("use_sim_time") + params_file = LaunchConfiguration("params_file") + default_nav_to_pose_bt_xml = LaunchConfiguration("default_nav_to_pose_bt_xml") + autostart = LaunchConfiguration("autostart") + + stdout_linebuf_envvar = SetEnvironmentVariable("RCUTILS_LOGGING_BUFFERED_STREAM", "1") + + declare_namespace_cmd = DeclareLaunchArgument( + "namespace", default_value="", description="Top-level namespace" + ) + + declare_use_namespace_cmd = DeclareLaunchArgument( + "use_namespace", + default_value="false", + description="Whether to apply a namespace to the navigation stack", + ) + + declare_slam_cmd = DeclareLaunchArgument( + "slam", default_value="true", description="Whether run a SLAM" + ) + + declare_map_yaml_cmd = DeclareLaunchArgument( + "map", description="Full path to map yaml file to load" + ) + + declare_use_sim_time_cmd = DeclareLaunchArgument( + "use_sim_time", default_value="false", description="Use simulation clock if true" + ) + + declare_params_file_cmd = DeclareLaunchArgument( + "params_file", + default_value=os.path.join(sm_dance_bot_dir, "params", "nav2z_client", "nav2_params.yaml"), + description="Full path to the ROS2 parameters file to use for all launched nodes", + ) + + declare_bt_xml_cmd = DeclareLaunchArgument( + "default_nav_to_pose_bt_xml", + default_value=os.path.join( + sm_dance_bot_dir, "params", "nav2z_client", "navigation_tree.xml" + ), + description="Full path to the behavior tree xml file to use", + ) + + declare_autostart_cmd = DeclareLaunchArgument( + "autostart", default_value="true", description="Automatically startup the nav2 stack" + ) + + # Specify the actions + bringup_cmd_group = GroupAction( + [ + PushRosNamespace(condition=IfCondition(use_namespace), namespace=namespace), + IncludeLaunchDescription( + PythonLaunchDescriptionSource(os.path.join(launch_dir, "slam_launch.py")), + condition=IfCondition(slam), + launch_arguments={ + "namespace": namespace, + "use_sim_time": use_sim_time, + "autostart": autostart, + "params_file": params_file, + }.items(), + ), + IncludeLaunchDescription( + PythonLaunchDescriptionSource(os.path.join(launch_dir, "navigation_launch.py")), + launch_arguments={ + "namespace": namespace, + "use_sim_time": use_sim_time, + "autostart": autostart, + "params_file": params_file, + "default_nav_to_pose_bt_xml": default_nav_to_pose_bt_xml, + "use_lifecycle_mgr": "false", + "map_subscribe_transient_local": "true", + }.items(), + ), + ] + ) + + # Create the launch description and populate + ld = LaunchDescription() + + # Set environment variables + ld.add_action(stdout_linebuf_envvar) + + # Declare the launch options + ld.add_action(declare_namespace_cmd) + ld.add_action(declare_use_namespace_cmd) + ld.add_action(declare_slam_cmd) + ld.add_action(declare_map_yaml_cmd) + ld.add_action(declare_use_sim_time_cmd) + ld.add_action(declare_params_file_cmd) + ld.add_action(declare_autostart_cmd) + ld.add_action(declare_bt_xml_cmd) + + # Add the actions to launch all of the navigation nodes + ld.add_action(bringup_cmd_group) + + return ld diff --git a/smacc2_sm_reference_library/sm_dancebot_ue/launch/husky_gazebo.launch.yaml b/smacc2_sm_reference_library/sm_dancebot_ue/launch/husky_gazebo.launch.yaml new file mode 100644 index 000000000..66bb498b0 --- /dev/null +++ b/smacc2_sm_reference_library/sm_dancebot_ue/launch/husky_gazebo.launch.yaml @@ -0,0 +1,3 @@ +launch: + - include: + file: "$(find-pkg-share husky_gazebo)/launch/gazebo_launch.py" diff --git a/smacc2_sm_reference_library/sm_dancebot_ue/launch/navigation_launch.py b/smacc2_sm_reference_library/sm_dancebot_ue/launch/navigation_launch.py new file mode 100644 index 000000000..4f71a43a8 --- /dev/null +++ b/smacc2_sm_reference_library/sm_dancebot_ue/launch/navigation_launch.py @@ -0,0 +1,169 @@ +# Copyright (c) 2018 Intel Corporation +# +# Licensed under the Apache License, Version 2.0 (the "License"); +# you may not use this file except in compliance with the License. +# You may obtain a copy of the License at +# +# http://www.apache.org/licenses/LICENSE-2.0 +# +# Unless required by applicable law or agreed to in writing, software +# distributed under the License is distributed on an "AS IS" BASIS, +# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +# See the License for the specific language governing permissions and +# limitations under the License. + +import os + +from ament_index_python.packages import get_package_share_directory + +from launch import LaunchDescription +from launch.actions import DeclareLaunchArgument, SetEnvironmentVariable +from launch.substitutions import LaunchConfiguration +from launch_ros.actions import Node +from nav2_common.launch import RewrittenYaml + + +def generate_launch_description(): + # Get the launch directory + sm_dance_bot_dir = get_package_share_directory("sm_dancebot_ue") + + namespace = LaunchConfiguration("namespace") + use_sim_time = LaunchConfiguration("use_sim_time") + autostart = LaunchConfiguration("autostart") + params_file = LaunchConfiguration("params_file") + default_nav_to_pose_bt_xml = LaunchConfiguration("default_nav_to_pose_bt_xml") + map_subscribe_transient_local = LaunchConfiguration("map_subscribe_transient_local") + + lifecycle_nodes = ["controller_server", "planner_server", "behavior_server", "bt_navigator"] + + # Map fully qualified names to relative ones so the node's namespace can be prepended. + # In case of the transforms (tf), currently, there doesn't seem to be a better alternative + # https://github.com/ros/geometry2/issues/32 + # https://github.com/ros/robot_state_publisher/pull/30 + # TODO(orduno) Substitute with `PushNodeRemapping` + # https://github.com/ros2/launch_ros/issues/56 + remappings = [("/tf", "tf"), ("/tf_static", "tf_static")] + + # Create our own temporary YAML files that include substitutions + param_substitutions = { + "use_sim_time": use_sim_time, + # 'default_nav_to_pose_bt_xml': default_nav_to_pose_bt_xml, + "default_nav_to_pose_bt_xml": os.path.join( + sm_dance_bot_dir, "params", "nav2z_client", "navigation_tree.xml" + ), + "autostart": autostart, + "map_subscribe_transient_local": map_subscribe_transient_local, + } + + configured_params = RewrittenYaml( + source_file=params_file, + root_key=namespace, + param_rewrites=param_substitutions, + convert_types=True, + ) + + xtermprefix = "xterm -xrm 'XTerm*scrollBar: true' -xrm 'xterm*rightScrollBar: true' -hold -geometry 1000x600 -sl 10000 -e" + + print("+********************************") + print(str(param_substitutions)) + print(str(default_nav_to_pose_bt_xml)) + + return LaunchDescription( + [ + # Set env var to print messages to stdout immediately + SetEnvironmentVariable("RCUTILS_LOGGING_BUFFERED_STREAM", "1"), + DeclareLaunchArgument( + "namespace", default_value="", description="Top-level namespace" + ), + DeclareLaunchArgument( + "use_sim_time", + default_value="false", + description="Use simulation clock if true", + ), + DeclareLaunchArgument( + "autostart", + default_value="true", + description="Automatically startup the nav2 stack", + ), + DeclareLaunchArgument( + "params_file", + default_value=os.path.join( + sm_dance_bot_dir, "params", "nav2z_client", "nav2_params.yaml" + ), + description="Full path to the ROS2 parameters file to use", + ), + DeclareLaunchArgument( + "default_nav_to_pose_bt_xml", + # default_value=os.path.join(get_package_share_directory('nav2_bt_navigator'),'behavior_trees', 'navigate_w_replanning_and_recovery.xml'), + default_value=os.path.join( + sm_dance_bot_dir, "params", "nav2z_client", "navigation_tree.xml" + ), + description="Full path to the behavior tree xml file to use", + ), + DeclareLaunchArgument( + "map_subscribe_transient_local", + default_value="false", + description="Whether to set the map subscriber QoS to transient local", + ), + Node( + package="nav2_controller", + executable="controller_server", + output="screen", + prefix=xtermprefix, + parameters=[configured_params], + remappings=remappings, + arguments=["--ros-args", "--log-level", "INFO"], + ), + Node( + package="nav2_planner", + executable="planner_server", + name="planner_server", + output="screen", + prefix=xtermprefix, + parameters=[configured_params], + remappings=remappings, + arguments=["--ros-args", "--log-level", "INFO"], + ), + Node( + package="nav2_behaviors", + executable="behavior_server", + name="behavior_server", + output="screen", + prefix=xtermprefix, + parameters=[configured_params], + remappings=remappings, + arguments=["--ros-args", "--log-level", "INFO"], + ), + Node( + package="nav2_bt_navigator", + executable="bt_navigator", + name="bt_navigator", + output="screen", + prefix=xtermprefix, + parameters=[configured_params], + remappings=remappings, + arguments=["--ros-args", "--log-level", "INFO"], + ), + Node( + package="nav2_waypoint_follower", + executable="waypoint_follower", + name="waypoint_follower", + output="screen", + parameters=[configured_params], + remappings=remappings, + ), + Node( + package="nav2_lifecycle_manager", + executable="lifecycle_manager", + name="lifecycle_manager_navigation", + output="screen", + prefix=xtermprefix, + parameters=[ + {"use_sim_time": use_sim_time}, + {"autostart": autostart}, + {"node_names": lifecycle_nodes}, + ], + arguments=["--ros-args", "--log-level", "INFO"], + ), + ] + ) diff --git a/smacc2_sm_reference_library/sm_dancebot_ue/launch/online_sync_launch.py b/smacc2_sm_reference_library/sm_dancebot_ue/launch/online_sync_launch.py new file mode 100644 index 000000000..e3487e7a3 --- /dev/null +++ b/smacc2_sm_reference_library/sm_dancebot_ue/launch/online_sync_launch.py @@ -0,0 +1,61 @@ +# Copyright (c) 2023 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 + + +import os + +from launch import LaunchDescription +from launch.actions import DeclareLaunchArgument +from launch.substitutions import LaunchConfiguration +from launch_ros.actions import Node +from ament_index_python.packages import get_package_share_directory + + +def generate_launch_description(): + use_sim_time = LaunchConfiguration("use_sim_time") + slam_params_file = LaunchConfiguration("slam_params_file") + xtermprefix = "xterm -xrm 'XTerm*scrollBar: true' -xrm 'xterm*rightScrollBar: true' -hold -geometry 1000x600 -sl 10000 -e" + + declare_use_sim_time_argument = DeclareLaunchArgument( + "use_sim_time", default_value="true", description="Use simulation/Gazebo clock" + ) + declare_slam_params_file_cmd = DeclareLaunchArgument( + "slam_params_file", + default_value=os.path.join( + # get_package_share_directory("slam_toolbox"), "config", "mapper_params_online_sync.yaml" + get_package_share_directory("sm_dancebot_ue"), + "params", + "mapper_params_online_sync.yaml", + ), + description="Full path to the ROS2 parameters file to use for the slam_toolbox node", + ) + + start_sync_slam_toolbox_node = Node( + parameters=[slam_params_file, {"use_sim_time": use_sim_time}], + package="slam_toolbox", + executable="sync_slam_toolbox_node", + name="slam_toolbox", + output="screen", + prefix=xtermprefix, + ) + + ld = LaunchDescription() + + ld.add_action(declare_use_sim_time_argument) + ld.add_action(declare_slam_params_file_cmd) + ld.add_action(start_sync_slam_toolbox_node) + + return ld diff --git a/smacc2_sm_reference_library/sm_dancebot_ue/launch/rviz_launch.py b/smacc2_sm_reference_library/sm_dancebot_ue/launch/rviz_launch.py new file mode 100644 index 000000000..056b5f8f0 --- /dev/null +++ b/smacc2_sm_reference_library/sm_dancebot_ue/launch/rviz_launch.py @@ -0,0 +1,122 @@ +# Copyright (c) 2018 Intel Corporation +# +# Licensed under the Apache License, Version 2.0 (the "License"); +# you may not use this file except in compliance with the License. +# You may obtain a copy of the License at +# +# http://www.apache.org/licenses/LICENSE-2.0 +# +# Unless required by applicable law or agreed to in writing, software +# distributed under the License is distributed on an "AS IS" BASIS, +# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +# See the License for the specific language governing permissions and +# limitations under the License. + +import os + +from ament_index_python.packages import get_package_share_directory + +from launch import LaunchDescription +from launch.actions import DeclareLaunchArgument, EmitEvent, RegisterEventHandler +from launch.conditions import IfCondition, UnlessCondition +from launch.event_handlers import OnProcessExit +from launch.events import Shutdown +from launch.substitutions import LaunchConfiguration +from launch_ros.actions import Node +from nav2_common.launch import ReplaceString + + +def generate_launch_description(): + # Get the launch directory + bringup_dir = get_package_share_directory("nav2_bringup") + + # Create the launch configuration variables + namespace = LaunchConfiguration("namespace") + use_namespace = LaunchConfiguration("use_namespace") + rviz_config_file = LaunchConfiguration("rviz_config") + + # Declare the launch arguments + declare_namespace_cmd = DeclareLaunchArgument( + "namespace", + default_value="navigation", + description=( + "Top-level namespace. The value will be used to replace the " + " keyword on the rviz config file." + ), + ) + + declare_use_namespace_cmd = DeclareLaunchArgument( + "use_namespace", + default_value="false", + description="Whether to apply a namespace to the navigation stack", + ) + + declare_rviz_config_file_cmd = DeclareLaunchArgument( + "rviz_config", + default_value=os.path.join(bringup_dir, "rviz", "nav2_default_view.rviz"), + description="Full path to the RVIZ config file to use", + ) + + # Launch rviz + start_rviz_cmd = Node( + condition=UnlessCondition(use_namespace), + package="rviz2", + executable="rviz2", + name="rviz2", + arguments=["-d", rviz_config_file], + output="screen", + ) + + namespaced_rviz_config_file = ReplaceString( + source_file=rviz_config_file, replacements={"": ("/", namespace)} + ) + + start_namespaced_rviz_cmd = Node( + condition=IfCondition(use_namespace), + package="rviz2", + executable="rviz2", + name="rviz2", + namespace=namespace, + arguments=["-d", namespaced_rviz_config_file], + output="screen", + remappings=[ + ("/tf", "tf"), + ("/tf_static", "tf_static"), + ("/goal_pose", "goal_pose"), + ("/clicked_point", "clicked_point"), + ("/initialpose", "initialpose"), + ], + ) + + exit_event_handler = RegisterEventHandler( + condition=UnlessCondition(use_namespace), + event_handler=OnProcessExit( + target_action=start_rviz_cmd, on_exit=EmitEvent(event=Shutdown(reason="rviz exited")) + ), + ) + + exit_event_handler_namespaced = RegisterEventHandler( + condition=IfCondition(use_namespace), + event_handler=OnProcessExit( + target_action=start_namespaced_rviz_cmd, + on_exit=EmitEvent(event=Shutdown(reason="rviz exited")), + ), + ) + + # Create the launch description and populate + ld = LaunchDescription() + + # Declare the launch options + ld.add_action(declare_namespace_cmd) + ld.add_action(declare_use_namespace_cmd) + ld.add_action(declare_rviz_config_file_cmd) + + # Add any conditioned actions + ld.add_action(start_rviz_cmd) + ld.add_action(start_namespaced_rviz_cmd) + + # Add other nodes and processes we need + ld.add_action(exit_event_handler) + ld.add_action(exit_event_handler_namespaced) + + return ld diff --git a/smacc2_sm_reference_library/sm_dancebot_ue/launch/slam_launch.py b/smacc2_sm_reference_library/sm_dancebot_ue/launch/slam_launch.py new file mode 100644 index 000000000..71846eec7 --- /dev/null +++ b/smacc2_sm_reference_library/sm_dancebot_ue/launch/slam_launch.py @@ -0,0 +1,128 @@ +# Copyright (c) 2020 Samsung Research Russia +# +# Licensed under the Apache License, Version 2.0 (the "License"); +# you may not use this file except in compliance with the License. +# You may obtain a copy of the License at +# +# http://www.apache.org/licenses/LICENSE-2.0 +# +# Unless required by applicable law or agreed to in writing, software +# distributed under the License is distributed on an "AS IS" BASIS, +# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +# See the License for the specific language governing permissions and +# limitations under the License. + +import os + +from ament_index_python.packages import get_package_share_directory + +from launch import LaunchDescription +from launch.actions import DeclareLaunchArgument, IncludeLaunchDescription +from launch.launch_description_sources import PythonLaunchDescriptionSource +from launch.substitutions import LaunchConfiguration + +# from launch_ros.actions import Node +# from nav2_common.launch import RewrittenYaml + + +def generate_launch_description(): + # Input parameters declaration + # namespace = LaunchConfiguration("namespace") + # params_file = LaunchConfiguration("params_file") + use_sim_time = LaunchConfiguration("use_sim_time") + # autostart = LaunchConfiguration("autostart") + sm_dance_bot_dir = get_package_share_directory("sm_dancebot_ue") + + # Variables + # lifecycle_nodes = ["slam_toolbox"] + + # Getting directories and launch-files + # bringup_dir = get_package_share_directory("nav2_bringup") + # slam_toolbox_dir = get_package_share_directory("slam_toolbox") + # slam_launch_file = os.path.join(sm_dance_bot_dir, 'launch', 'online_sync_launch.py') + slam_launch_file = os.path.join(sm_dance_bot_dir, "launch", "online_sync_launch.py") + + # Create our own temporary YAML files that include substitutions + # param_substitutions = {"use_sim_time": use_sim_time} + + # configured_params = RewrittenYaml( + # source_file=params_file, + # root_key=namespace, + # param_rewrites=param_substitutions, + # convert_types=True, + # ) + + # Declare the launch arguments + declare_namespace_cmd = DeclareLaunchArgument( + "namespace", default_value="", description="Top-level namespace" + ) + + # declare_params_file_cmd = DeclareLaunchArgument( + # 'params_file', + # default_value=os.path.join(bringup_dir, 'params', 'nav2_params.yaml'), + # description='Full path to the ROS2 parameters file to use for all launched nodes') + + declare_use_sim_time_cmd = DeclareLaunchArgument( + "use_sim_time", default_value="True", description="Use simulation (Gazebo) clock if true" + ) + + declare_autostart_cmd = DeclareLaunchArgument( + "autostart", default_value="True", description="Automatically startup the nav2 stack" + ) + + # Nodes launching commands + + # start_map_saver_server_cmd = Node( + # package='nav2_map_server', + # executable='map_saver_server', + # output='screen', + # parameters=[configured_params]) + + # xtermprefix = "xterm -xrm 'XTerm*scrollBar: true' -xrm 'xterm*rightScrollBar: true' " \ + # "-hold -geometry 1000x600 -sl 10000 -e" + + # start_lifecycle_manager_cmd = Node( + # package="nav2_lifecycle_manager", + # executable="lifecycle_manager", + # name="lifecycle_manager_slam", + # output="screen", + # parameters=[ + # {"use_sim_time": use_sim_time}, + # {"autostart": autostart}, + # {"node_names": lifecycle_nodes}, + # ], + # prefix=xtermprefix, + # ) + + # If the provided param file doesn't have slam_toolbox params, we must remove the 'params_file' + # LaunchConfiguration, or it will be passed automatically to slam_toolbox and will not load + # the default file + + start_slam_toolbox_cmd = IncludeLaunchDescription( + PythonLaunchDescriptionSource(slam_launch_file), + launch_arguments={"use_sim_time": use_sim_time}.items(), + ) + + # Pop (or load) previous LaunchConfiguration, resetting the state of params_file + # pop_launch_config = PopLaunchConfigurations( + # condition=UnlessCondition(has_slam_toolbox_params)) + + ld = LaunchDescription() + + # Declare the launch options + ld.add_action(declare_namespace_cmd) + # ld.add_action(declare_params_file_cmd) + ld.add_action(declare_use_sim_time_cmd) + ld.add_action(declare_autostart_cmd) + + # Running Map Saver Server + # ld.add_action(start_map_saver_server_cmd) + # ld.add_action(start_lifecycle_manager_cmd) + + # Running SLAM Toolbox + # ld.add_action(push_launch_config) + # ld.add_action(remove_params_file) + ld.add_action(start_slam_toolbox_cmd) + # ld.add_action(pop_launch_config) + + return ld diff --git a/smacc2_sm_reference_library/sm_dancebot_ue/launch/sm_dancebot_ue_launch.py b/smacc2_sm_reference_library/sm_dancebot_ue/launch/sm_dancebot_ue_launch.py new file mode 100644 index 000000000..ec7d28d4e --- /dev/null +++ b/smacc2_sm_reference_library/sm_dancebot_ue/launch/sm_dancebot_ue_launch.py @@ -0,0 +1,266 @@ +# Copyright (c) 2018 Intel Corporation +# +# Licensed under the Apache License, Version 2.0 (the "License"); +# you may not use this file except in compliance with the License. +# You may obtain a copy of the License at +# +# http://www.apache.org/licenses/LICENSE-2.0 +# +# Unless required by applicable law or agreed to in writing, software +# distributed under the License is distributed on an "AS IS" BASIS, +# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +# See the License for the specific language governing permissions and +# limitations under the License. + +"""This is all-in-one launch script intended for use by nav2 developers.""" + +import os + +from ament_index_python.packages import get_package_share_directory + +from launch import LaunchDescription +from launch.actions import DeclareLaunchArgument, IncludeLaunchDescription +from launch.conditions import IfCondition +from launch.launch_description_sources import PythonLaunchDescriptionSource +from launch.substitutions import LaunchConfiguration +from launch_ros.actions import Node + + +def generate_launch_description(): + + xtermprefix = "xterm -xrm 'XTerm*scrollBar: true' -xrm 'xterm*rightScrollBar: true' -hold -geometry 1000x600 -sl 10000 -e" + + # Get the launch directory + sm_dance_bot_dir = get_package_share_directory("sm_dancebot_ue") + sm_dance_bot_launch_dir = os.path.join(sm_dance_bot_dir, "launch") + + # Create the launch configuration variables + slam = LaunchConfiguration("slam") + namespace = LaunchConfiguration("namespace") + use_namespace = LaunchConfiguration("use_namespace") + map_yaml_file = LaunchConfiguration("map") + use_sim_time = LaunchConfiguration("use_sim_time") + + params_file = LaunchConfiguration("params_file") + default_nav_to_pose_bt_xml = LaunchConfiguration("default_nav_to_pose_bt_xml") + autostart = LaunchConfiguration("autostart") + + # Launch configuration variables specific to simulation + rviz_config_file = LaunchConfiguration("rviz_config_file") + + use_robot_state_pub = LaunchConfiguration("use_robot_state_pub") + use_rviz = LaunchConfiguration("use_rviz") + + urdf = os.path.join(sm_dance_bot_dir, "urdf", "turtlebot3_waffle.urdf") + + # Map fully qualified names to relative ones so the node's namespace can be prepended. + # In case of the transforms (tf), currently, there doesn't seem to be a better alternative + # https://github.com/ros/geometry2/issues/32 + # https://github.com/ros/robot_state_publisher/pull/30 + # TODO(orduno) Substitute with `PushNodeRemapping` + # https://github.com/ros2/launch_ros/issues/56 + remappings = [("/tf", "tf"), ("/tf_static", "tf_static")] + + # Declare the launch arguments + declare_namespace_cmd = DeclareLaunchArgument( + "namespace", default_value="", description="Top-level namespace" + ) + + declare_use_namespace_cmd = DeclareLaunchArgument( + "use_namespace", + default_value="false", + description="Whether to apply a namespace to the navigation stack", + ) + + declare_show_gz_lidar = DeclareLaunchArgument( + "show_gz_lidar", + default_value="true", + description="Whether to apply a namespace to the navigation stack", + ) + + declare_slam_cmd = DeclareLaunchArgument( + "slam", default_value="True", description="Whether run a SLAM" + ) + + declare_use_sim_time_cmd = DeclareLaunchArgument( + "use_sim_time", default_value="true", description="Use simulation clock if true" + ) + + declare_gazebo_headless_cmd = DeclareLaunchArgument( + "headless", + default_value="false", + description="Use headless Gazebo if true", + ) + + declare_params_file_cmd = DeclareLaunchArgument( + "params_file", + default_value=os.path.join(sm_dance_bot_dir, "params", "nav2z_client", "nav2_params.yaml"), + description="Full path to the ROS2 parameters file to use for all launched nodes", + ) + + declare_bt_xml_cmd = DeclareLaunchArgument( + "default_nav_to_pose_bt_xml", + default_value=os.path.join( + sm_dance_bot_dir, "params", "nav2z_client", "navigation_tree.xml" + ), + description="Full path to the behavior tree xml file to use", + ) + + declare_autostart_cmd = DeclareLaunchArgument( + "autostart", default_value="true", description="Automatically startup the nav2 stack" + ) + + declare_rviz_config_file_cmd = DeclareLaunchArgument( + "rviz_config_file", + default_value=os.path.join(sm_dance_bot_dir, "rviz", "nav2_default_view.rviz"), + description="Full path to the RVIZ config file to use", + ) + + declare_use_robot_state_pub_cmd = DeclareLaunchArgument( + "use_robot_state_pub", + default_value="True", + description="Whether to start the robot state publisher", + ) + + declare_use_rviz_cmd = DeclareLaunchArgument( + "use_rviz", default_value="True", description="Whether to start RVIZ" + ) + + declare_map_yaml_cmd = DeclareLaunchArgument( + "map", + default_value=os.path.join(sm_dance_bot_dir, "maps", "turtlebot3_world.yaml"), + description="Full path to map file to load", + ) + + start_robot_state_publisher_cmd = Node( + condition=IfCondition(use_robot_state_pub), + package="robot_state_publisher", + executable="robot_state_publisher", + name="robot_state_publisher", + namespace=namespace, + output="screen", + parameters=[{"use_sim_time": use_sim_time}], + remappings=remappings, + arguments=[urdf], + ) + + static_transform_publisher = Node( + package="sm_dancebot_ue", + executable="transform_publisher.py", + name="static_transform_publisherxx", + output="screen", + # arguments=["-0.064", "0", "0.122", "0", "0", "0", "base_scan", "base_link"], + prefix=xtermprefix, + parameters=[{"use_sim_time": use_sim_time}], + ) + + # static_transform_publisher_2 = Node( + # package="tf2_ros", + # executable="static_transform_publisher", + # name="static_transform_publisher", + # output="screen", + # arguments=["0", "0", "0", "0", "0", "0", "base_link", "base_footprint"], + # prefix=xtermprefix, + # ) + + rviz_cmd = IncludeLaunchDescription( + PythonLaunchDescriptionSource(os.path.join(sm_dance_bot_launch_dir, "rviz_launch.py")), + condition=IfCondition(use_rviz), + launch_arguments={ + "namespace": "", + "use_namespace": "False", + "rviz_config": rviz_config_file, + }.items(), + ) + + bringup_cmd = IncludeLaunchDescription( + PythonLaunchDescriptionSource(os.path.join(sm_dance_bot_launch_dir, "bringup_launch.py")), + launch_arguments={ + "namespace": namespace, + "use_namespace": use_namespace, + "autostart": autostart, + "params_file": params_file, + "slam": slam, + "map": map_yaml_file, + "use_sim_time": use_sim_time, + "default_nav_to_pose_bt_xml": default_nav_to_pose_bt_xml, + }.items(), + ) + + sm_dance_bot_node = Node( + package="sm_dancebot_ue", + executable="sm_dancebot_ue_node", + name="SmDanceBotUE", + output="screen", + prefix=xtermprefix, + parameters=[ + os.path.join( + get_package_share_directory("sm_dancebot_ue"), + "params/sm_dancebot_ue_config.yaml", + ) + ], + remappings=[ + # ("/odom", "/odometry/filtered"), + # ("/sm_dance_bot_2/odom_tracker/odom_tracker_path", "/odom_tracker_path"), + # ("/sm_dance_bot_2/odom_tracker/odom_tracker_stacked_path", "/odom_tracker_path_stacked") + ], + arguments=["--ros-args", "--log-level", "INFO"], + ) + + led_action_server_node = Node( + package="sm_dancebot_ue", + executable="led_action_server_node", + output="screen", + prefix=xtermprefix, + ) + + temperature_action_server = Node( + package="sm_dancebot_ue", + executable="temperature_sensor_node", + output="screen", + prefix=xtermprefix, + ) + + service3_node = Node( + package="sm_dancebot_ue", + executable="service_node_3.py", + output="screen", + prefix=xtermprefix, + parameters=[ + {"autostart": True, "node_names": ["ss", "dfa"]}, + ], + ) + + # Create the launch description and populate + ld = LaunchDescription() + + # Declare the launch options + ld.add_action(declare_namespace_cmd) + ld.add_action(declare_use_namespace_cmd) + ld.add_action(declare_slam_cmd) + ld.add_action(declare_map_yaml_cmd) + ld.add_action(declare_use_sim_time_cmd) + ld.add_action(declare_gazebo_headless_cmd) + ld.add_action(declare_params_file_cmd) + ld.add_action(declare_bt_xml_cmd) + ld.add_action(declare_autostart_cmd) + ld.add_action(declare_map_yaml_cmd) + ld.add_action(declare_show_gz_lidar) + + ld.add_action(declare_rviz_config_file_cmd) + ld.add_action(declare_use_robot_state_pub_cmd) + ld.add_action(declare_use_rviz_cmd) + + ld.add_action(sm_dance_bot_node) + ld.add_action(service3_node) + ld.add_action(temperature_action_server) + ld.add_action(led_action_server_node) + ld.add_action(static_transform_publisher) + # ld.add_action(static_transform_publisher_2) + + # # Add the actions to launch all of the navigation nodes + ld.add_action(start_robot_state_publisher_cmd) + ld.add_action(rviz_cmd) + ld.add_action(bringup_cmd) + + return ld diff --git a/smacc2_sm_reference_library/sm_dancebot_ue/package.xml b/smacc2_sm_reference_library/sm_dancebot_ue/package.xml new file mode 100644 index 000000000..e28d5a7fc --- /dev/null +++ b/smacc2_sm_reference_library/sm_dancebot_ue/package.xml @@ -0,0 +1,48 @@ + + + + sm_dancebot_ue + 2.2.2 + The dance_bot package + Pablo Inigo Blasco + + Apache-2.0 + + ament_cmake + + nav2z_client + multirole_sensor_client + navigation2 + nav2_bringup + + robot_state_publisher + ros_publisher_client + ros_timer_client + slam_toolbox + smacc2 + std_msgs + sr_all_events_go + sr_event_countdown + sr_conditional + visualization_msgs + tf2 + ue_msgs + + backward_global_planner + backward_local_planner + forward_global_planner + forward_local_planner + nav2z_planners_common + pure_spinning_local_planner + undo_path_global_planner + + rosidl_default_generators + action_msgs + rosidl_interface_packages + rosidl_default_runtime + + + ament_cmake + + + diff --git a/smacc2_sm_reference_library/sm_dancebot_ue/params/mapper_params_online_sync.yaml b/smacc2_sm_reference_library/sm_dancebot_ue/params/mapper_params_online_sync.yaml new file mode 100644 index 000000000..0f948602c --- /dev/null +++ b/smacc2_sm_reference_library/sm_dancebot_ue/params/mapper_params_online_sync.yaml @@ -0,0 +1,73 @@ +slam_toolbox: + ros__parameters: + + # Plugin params + solver_plugin: solver_plugins::CeresSolver + ceres_linear_solver: SPARSE_NORMAL_CHOLESKY + ceres_preconditioner: SCHUR_JACOBI + ceres_trust_strategy: LEVENBERG_MARQUARDT + ceres_dogleg_type: TRADITIONAL_DOGLEG + ceres_loss_function: None + + # ROS Parameters + odom_frame: odom + map_frame: map + base_frame: base_footprint + scan_topic: /scan + mode: mapping #localization + + # if you'd like to immediately start continuing a map at a given pose + # or at the dock, but they are mutually exclusive, if pose is given + # will use pose + #map_file_name: test_steve + #map_start_pose: [0.0, 0.0, 0.0] + #map_start_at_dock: true + + debug_logging: true + throttle_scans: 1 + transform_publish_period: 0.02 #if 0 never publishes odometry + map_update_interval: 0.05 + resolution: 0.04 + max_laser_range: 16.0 #for rastering images + minimum_time_interval: 0.3 + transform_timeout: 0.2 + tf_buffer_duration: 30. + stack_size_to_use: 40000000 #// program needs a larger stack size to serialize large maps + enable_interactive_mode: true + + # General Parameters + use_scan_matching: true + use_scan_barycenter: false + minimum_travel_distance: 0.1 + minimum_travel_heading: 0.1 + scan_buffer_size: 10 + scan_buffer_maximum_scan_distance: 10.0 + link_match_minimum_response_fine: 0.1 + link_scan_maximum_distance: 1.5 + loop_search_maximum_distance: 3.0 + do_loop_closing: true + loop_match_minimum_chain_size: 10 + loop_match_maximum_variance_coarse: 3.0 + loop_match_minimum_response_coarse: 0.35 + loop_match_minimum_response_fine: 0.45 + + # Correlation Parameters - Correlation Parameters + correlation_search_space_dimension: 0.5 + correlation_search_space_resolution: 0.01 + correlation_search_space_smear_deviation: 0.1 + + # Correlation Parameters - Loop Closure Parameters + loop_search_space_dimension: 8.0 + loop_search_space_resolution: 0.05 + loop_search_space_smear_deviation: 0.03 + + # Scan Matcher Parameters + distance_variance_penalty: 0.5 + angle_variance_penalty: 1.0 + + fine_search_angle_offset: 0.00349 + coarse_search_angle_offset: 0.349 + coarse_angle_resolution: 0.0349 + minimum_angle_penalty: 0.9 + minimum_distance_penalty: 0.5 + use_response_expansion: true diff --git a/smacc2_sm_reference_library/sm_dancebot_ue/params/nav2z_client/nav2_params.yaml b/smacc2_sm_reference_library/sm_dancebot_ue/params/nav2z_client/nav2_params.yaml new file mode 100644 index 000000000..96bf0f2c5 --- /dev/null +++ b/smacc2_sm_reference_library/sm_dancebot_ue/params/nav2z_client/nav2_params.yaml @@ -0,0 +1,373 @@ +amcl: + ros__parameters: + use_sim_time: True + alpha1: 0.2 + alpha2: 0.2 + alpha3: 0.2 + alpha4: 0.2 + alpha5: 0.2 + base_frame_id: "base_footprint" + beam_skip_distance: 0.5 + beam_skip_error_threshold: 0.9 + beam_skip_threshold: 0.3 + do_beamskip: false + global_frame_id: "map" + lambda_short: 0.1 + laser_likelihood_max_dist: 2.0 + laser_max_range: 100.0 + laser_min_range: -1.0 + laser_model_type: "likelihood_field" + max_beams: 60 + max_particles: 2000 + min_particles: 500 + odom_frame_id: "odom" + pf_err: 0.05 + pf_z: 0.99 + recovery_alpha_fast: 0.0 + recovery_alpha_slow: 0.0 + resample_interval: 1 + robot_model_type: "differential" + save_pose_rate: 0.5 + sigma_hit: 0.2 + tf_broadcast: true + transform_tolerance: 1.0 + update_min_a: 0.2 + update_min_d: 0.25 + z_hit: 0.5 + z_max: 0.05 + z_rand: 0.5 + z_short: 0.05 + scan_topic: scan + +amcl_map_client: + ros__parameters: + use_sim_time: True + +amcl_rclcpp_node: + ros__parameters: + use_sim_time: True + +bt_navigator: + ros__parameters: + use_sim_time: True + global_frame: map + robot_base_frame: base_link + odom_topic: /odom + enable_groot_monitoring: True + groot_zmq_publisher_port: 1666 + groot_zmq_server_port: 1667 + default_nav_to_pose_bt_xml: RUNTIMEFILL + plugin_lib_names: + - nav2_compute_path_to_pose_action_bt_node + - nav2_compute_path_through_poses_action_bt_node + - nav2_follow_path_action_bt_node + - nav2_back_up_action_bt_node + - nav2_spin_action_bt_node + - nav2_wait_action_bt_node + - nav2_clear_costmap_service_bt_node + - nav2_is_stuck_condition_bt_node + - nav2_goal_reached_condition_bt_node + - nav2_goal_updated_condition_bt_node + - nav2_initial_pose_received_condition_bt_node + - nav2_reinitialize_global_localization_service_bt_node + - nav2_rate_controller_bt_node + - nav2_distance_controller_bt_node + - nav2_speed_controller_bt_node + - nav2_truncate_path_action_bt_node + - nav2_goal_updater_node_bt_node + - nav2_recovery_node_bt_node + - nav2_pipeline_sequence_bt_node + - nav2_round_robin_node_bt_node + - nav2_transform_available_condition_bt_node + - nav2_time_expired_condition_bt_node + - nav2_distance_traveled_condition_bt_node + - nav2_single_trigger_bt_node + - nav2_is_battery_low_condition_bt_node + - nav2_navigate_through_poses_action_bt_node + - nav2_navigate_to_pose_action_bt_node + - nav2_remove_passed_goals_action_bt_node + - nav2_planner_selector_bt_node + - nav2_controller_selector_bt_node + - nav2_goal_checker_selector_bt_node + + +bt_navigator_rclcpp_node: + ros__parameters: + use_sim_time: True + +controller_server: + ros__parameters: + use_sim_time: True + controller_frequency: 20.0 + min_x_velocity_threshold: 0.001 + min_y_velocity_threshold: 0.5 + min_theta_velocity_threshold: 0.001 + progress_checker_plugin: "progress_checker" + goal_checker_plugins: ["goal_checker", "forward_goal_checker", "backward_goal_checker", "absolute_rotate_goal_checker", "undo_path_backwards_goal_checker"] + controller_plugins: ["FollowPath","BackwardLocalPlanner", "ForwardLocalPlanner", "PureSpinningLocalPlanner"] + + # Progress checker parameters + progress_checker: + plugin: "nav2_controller::SimpleProgressChecker" + required_movement_radius: 0.001 + movement_time_allowance: 3000.0 + + # Goal checker parameters + goal_checker: + plugin: "nav2_controller::SimpleGoalChecker" + xy_goal_tolerance: 0.257 + yaw_goal_tolerance: 0.165 # almost free orientation + stateful: True + backward_goal_checker: + plugin: "nav2_controller::SimpleGoalChecker" + xy_goal_tolerance: 0.09 + yaw_goal_tolerance: 0.05 + stateful: True + forward_goal_checker: + plugin: "nav2_controller::SimpleGoalChecker" + xy_goal_tolerance: 0.09 + yaw_goal_tolerance: 0.05 + stateful: True + absolute_rotate_goal_checker: + plugin: "nav2_controller::SimpleGoalChecker" + xy_goal_tolerance: 0.15 + yaw_goal_tolerance: 0.01 #0.005 + stateful: True + undo_path_backwards_goal_checker: + plugin: "nav2_controller::SimpleGoalChecker" + xy_goal_tolerance: 0.03 # 4 cm + yaw_goal_tolerance: 0.1 + stateful: True + + # DWB parameters + FollowPath: + plugin: "dwb_core::DWBLocalPlanner" + debug_trajectory_details: True + min_vel_x: 0.0 + min_vel_y: 0.0 + max_vel_x: 0.3 + max_vel_y: 0.0 + max_vel_theta: 1.0 + min_speed_xy: 0.0 + max_speed_xy: 0.3 + min_speed_theta: 0.0 + # Add high threshold velocity for turtlebot 3 issue. + # https://github.com/ROBOTIS-GIT/turtlebot3_simulations/issues/75 + acc_lim_x: 2.5 + acc_lim_y: 0.0 + acc_lim_theta: 3.2 + decel_lim_x: -2.5 + decel_lim_y: 0.0 + decel_lim_theta: -3.2 + vx_samples: 20 + vy_samples: 5 + vtheta_samples: 20 + sim_time: 1.7 + linear_granularity: 0.05 + angular_granularity: 0.025 + transform_tolerance: 0.2 + xy_goal_tolerance: 0.157 + yaw_goal_tolerance: 0.15 + trans_stopped_velocity: 0.25 + short_circuit_trajectory_evaluation: True + stateful: True + critics: ["RotateToGoal", "Oscillation", "BaseObstacle", "GoalAlign", "PathAlign", "PathDist", "GoalDist"] + BaseObstacle.scale: 0.02 + PathAlign.scale: 32.0 + PathAlign.forward_point_distance: 0.1 + GoalAlign.scale: 24.0 + GoalAlign.forward_point_distance: 0.1 + PathDist.scale: 32.0 + GoalDist.scale: 24.0 + RotateToGoal.scale: 32.0 + RotateToGoal.slowing_factor: 5.0 + RotateToGoal.lookahead_time: -1.0 + BackwardLocalPlanner: + plugin: "backward_local_planner::BackwardLocalPlanner" + transform_tolerance: 0.5 + k_rho: -1.4 # linear distance to the goal gain, default = -1.0 (related with carrot distance) + k_alpha: -0.1 #-0.1 + k_betta: -0.1 # -1.0 + carrot_distance: 0.3 #meters default 0.5 + carrot_angular_distance: 0.3 # no constraint, max 3.1416 + pure_spinning_straight_line_mode: true + max_linear_x_speed: 0.25 + ForwardLocalPlanner: + plugin: "cl_nav2z::forward_local_planner::ForwardLocalPlanner" + transform_tolerance: 0.5 + k_rho: 2.5 + k_alpha: -0.4 + k_betta: -1.0 + carrot_distance: 0.2 #meters + carrot_angular_distance: 0.5 # no constraint, max 3.1416 + max_linear_x_speed: 0.3 + max_angular_z_speed: 0.2 + PureSpinningLocalPlanner: + plugin: "pure_spinning_local_planner::PureSpinningLocalPlanner" + transform_tolerance: 0.5 + k_betta: 10.0 + max_angular_z_speed: 0.2 + use_shortest_angular_distance: true + + +controller_server_rclcpp_node: + ros__parameters: + use_sim_time: True + +local_costmap: + local_costmap: + ros__parameters: + update_frequency: 5.0 + publish_frequency: 2.0 + global_frame: odom + robot_base_frame: base_link + use_sim_time: True + rolling_window: true + width: 3 + height: 3 + resolution: 0.05 + robot_radius: 0.22 + plugins: ["voxel_layer", "inflation_layer"] + inflation_layer: + plugin: "nav2_costmap_2d::InflationLayer" + cost_scaling_factor: 3.0 + inflation_radius: 0.55 + voxel_layer: + plugin: "nav2_costmap_2d::VoxelLayer" + enabled: True + publish_voxel_map: True + origin_z: 0.0 + z_resolution: 0.05 + z_voxels: 16 + max_obstacle_height: 2.0 + mark_threshold: 0 + observation_sources: scan + scan: + topic: /scan + max_obstacle_height: 2.0 + clearing: True + marking: True + data_type: "LaserScan" + raytrace_max_range: 15.0 + raytrace_min_range: 0.0 + obstacle_max_range: 15.5 + obstacle_min_range: 0.0 + static_layer: + map_subscribe_transient_local: True + always_send_full_costmap: True + local_costmap_client: + ros__parameters: + use_sim_time: True + local_costmap_rclcpp_node: + ros__parameters: + use_sim_time: True + +global_costmap: + global_costmap: + ros__parameters: + update_frequency: 2.0 + publish_frequency: 1.0 + transform_tolerance: 5.0 + width: 16 + height: 16 + global_frame: map + robot_base_frame: base_link + use_sim_time: True + rolling_window: true + + robot_radius: 0.22 + resolution: 0.1 + width: 8 + height: 8 + track_unknown_space: true + plugins: ["static_layer", "obstacle_layer", "inflation_layer"] + obstacle_layer: + plugin: "nav2_costmap_2d::ObstacleLayer" + enabled: True + observation_sources: scan + scan: + topic: /scan + max_obstacle_height: 2.0 + clearing: True + marking: True + data_type: "LaserScan" + raytrace_max_range: 15.0 + raytrace_min_range: 0.0 + obstacle_max_range: 15.5 + obstacle_min_range: 0.0 + static_layer: + plugin: "nav2_costmap_2d::StaticLayer" + map_subscribe_transient_local: True + inflation_layer: + plugin: "nav2_costmap_2d::InflationLayer" + cost_scaling_factor: 1.0 + inflation_radius: 0.55 + always_send_full_costmap: True + global_costmap_client: + ros__parameters: + use_sim_time: True + global_costmap_rclcpp_node: + ros__parameters: + use_sim_time: True + +map_server: + ros__parameters: + use_sim_time: True + yaml_filename: "turtlebot3_world.yaml" + +map_saver: + ros__parameters: + use_sim_time: True + save_map_timeout: 5000 + free_thresh_default: 0.25 + occupied_thresh_default: 0.65 + +planner_server: + ros__parameters: + expected_planner_frequency: 20.0 + use_sim_time: True + planner_plugins: ["GridBased", "ForwardGlobalPlanner", "BackwardGlobalPlanner", "UndoPathGlobalPlanner"] + GridBased: + plugin: "nav2_navfn_planner/NavfnPlanner" + tolerance: 10.0 #meters + use_astar: false + allow_unknown: true + ForwardGlobalPlanner: + plugin: "forward_global_planner::ForwardGlobalPlanner" + transform_tolerance: 0.5 + BackwardGlobalPlanner: + plugin: "backward_global_planner::BackwardGlobalPlanner" + transform_tolerance: 0.5 + UndoPathGlobalPlanner: + plugin: "undo_path_global_planner::UndoPathGlobalPlanner" + transform_tolerance: 0.5 + +planner_server_rclcpp_node: + ros__parameters: + use_sim_time: True + +recoveries_server: + ros__parameters: + costmap_topic: local_costmap/costmap_raw + footprint_topic: local_costmap/published_footprint + cycle_frequency: 10.0 + recovery_plugins: ["spin", "backup", "wait"] + #recovery_plugins: ["spin"] + spin: + plugin: "nav2_recoveries/Spin" + backup: + plugin: "nav2_recoveries/BackUp" + wait: + plugin: "nav2_recoveries/Wait" + global_frame: odom + robot_base_frame: base_link + transform_timeout: 0.1 + use_sim_time: true + simulate_ahead_time: 2.0 + max_rotational_vel: 1.0 + min_rotational_vel: 0.4 + rotational_acc_lim: 3.2 + +robot_state_publisher: + ros__parameters: + use_sim_time: True diff --git a/smacc2_sm_reference_library/sm_dancebot_ue/params/nav2z_client/navigation_tree.xml b/smacc2_sm_reference_library/sm_dancebot_ue/params/nav2z_client/navigation_tree.xml new file mode 100644 index 000000000..ebd9f201f --- /dev/null +++ b/smacc2_sm_reference_library/sm_dancebot_ue/params/nav2z_client/navigation_tree.xml @@ -0,0 +1,33 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/smacc2_sm_reference_library/sm_dancebot_ue/params/nav2z_client/waypoints_plan.yaml b/smacc2_sm_reference_library/sm_dancebot_ue/params/nav2z_client/waypoints_plan.yaml new file mode 100644 index 000000000..36a8206b8 --- /dev/null +++ b/smacc2_sm_reference_library/sm_dancebot_ue/params/nav2z_client/waypoints_plan.yaml @@ -0,0 +1,83 @@ +waypoints: + # POINT 0 + - position: + x: 1.5 + y: 2.5 + z: 0.0 + orientation: + x: 0.0 + y: 0.0 + z: 0.7071068 + w: 0.7071068 + # POINT 1 + - position: + x: 1.8 + y: 3.5 + z: 0.0 + orientation: + x: 0.0 + y: 0.0 + z: 0.0 + w: 1.0 + + # POINT 2 + - position: + x: -10.25 + y: -4.25 + z: 0.0 + orientation: + x: 0.0 + y: 0.0 + z: 0.0 + w: 1.0 + # POINT 3 + - position: + x: 2.0 + y: -8.58 # north west corner of the room + #y: -12.35 # south west corner of the room + z: 0.0 + orientation: + x: 0.0 + y: 0.0 + z: 0.0 + w: 1.0 + # POINT 4 + - position: + x: -12.00 + y: 12.0 + z: 0.0 + orientation: + x: 0.0 + y: 0.0 + z: 0.0 + w: 1.0 + # POINT 5 + - position: + x: -10.0 + y: 14.5 + z: 0.0 + orientation: + x: 0.0 + y: 0.0 + z: 0.0 + w: 1.0 + # POINT 6 + - position: + x: -0.0 + y: 0.0 + z: 0.0 + orientation: + x: 0.0 + y: 0.0 + z: 0.0 + w: 1.0 + #POINT 7 + - position: + x: 6.0 + y: 8.0 + z: 0.0 + orientation: + x: 0.0 + y: 0.0 + z: 0.0 + w: 1.0 diff --git a/smacc2_sm_reference_library/sm_dancebot_ue/params/nav2z_client/waypoints_plan_back_on_road.yaml b/smacc2_sm_reference_library/sm_dancebot_ue/params/nav2z_client/waypoints_plan_back_on_road.yaml new file mode 100644 index 000000000..4dc8dbc48 --- /dev/null +++ b/smacc2_sm_reference_library/sm_dancebot_ue/params/nav2z_client/waypoints_plan_back_on_road.yaml @@ -0,0 +1,281 @@ +waypoints: + # POINT 36 + - position: + x: 288.55 + y: 637.19 + z: 0.0 + orientation: + x: 0.0 + y: 0.0 + z: 0.0 + w: 1.0 + # POINT 37 + - position: + x: 286.85 + y: 606.79 + z: 0.0 + orientation: + x: 0.0 + y: 0.0 + z: 0.0 + w: 1.0 + # POINT 38 + - position: + x: 277.15 + y: 569.29 + z: 0.0 + orientation: + x: 0.0 + y: 0.0 + z: 0.0 + w: 1.0 + # POINT 39 + - position: + x: 262.85 + y: 535.19 + z: 0.0 + orientation: + x: 0.0 + y: 0.0 + z: 0.0 + w: 1.0 + # POINT 40 + - position: + x: 240.75 + y: 494.89 + z: 0.0 + orientation: + x: 0.0 + y: 0.0 + z: 0.0 + w: 1.0 + # POINT 41 + - position: + x: 214.65 + y: 453.99 + z: 0.0 + orientation: + x: 0.0 + y: 0.0 + z: 0.0 + w: 1.0 + # POINT 42 + - position: + x: 185.95 + y: 404.89 + z: 0.0 + orientation: + x: 0.0 + y: 0.0 + z: 0.0 + w: 1.0 + # POINT 43 + - position: + x: 160.45 + y: 354.09 + z: 0.0 + orientation: + x: 0.0 + y: 0.0 + z: 0.0 + w: 1.0 + # POINT 44 + - position: + x: 138.55 + y: 303.89 + z: 0.0 + orientation: + x: 0.0 + y: 0.0 + z: 0.0 + w: 1.0 + # POINT 45 + - position: + x: 118.05 + y: 258.59 + z: 0.0 + orientation: + x: 0.0 + y: 0.0 + z: 0.0 + w: 1.0 + # POINT 46 + - position: + x: 98.05 + y: 213.19 + z: 0.0 + orientation: + x: 0.0 + y: 0.0 + z: 0.0 + w: 1.0 + # POINT 47 + - position: + x: 83.45 + y: 182.19 + z: 0.0 + orientation: + x: 0.0 + y: 0.0 + z: 0.0 + w: 1.0 + # POINT 48 + - position: + x: 70.55 + y: 149.99 + z: 0.0 + orientation: + x: 0.0 + y: 0.0 + z: 0.0 + w: 1.0 + # POINT 49 + - position: + x: 58.65 + y: 124.49 + z: 0.0 + orientation: + x: 0.0 + y: 0.0 + z: 0.0 + w: 1.0 + # POINT 50 + - position: + x: 46.35 + y: 97.49 + z: 0.0 + orientation: + x: 0.0 + y: 0.0 + z: 0.0 + w: 1.0 + # POINT 51 + - position: + x: 36.55 + y: 76.29 + z: 0.0 + orientation: + x: 0.0 + y: 0.0 + z: 0.0 + w: 1.0 + # POINT 52 + - position: + x: 26.65 + y: 52.79 + z: 0.0 + orientation: + x: 0.0 + y: 0.0 + z: 0.0 + w: 1.0 + # POINT 53 + - position: + x: 16.25 + y: 28.39 + z: 0.0 + orientation: + x: 0.0 + y: 0.0 + z: 0.0 + w: 1.0 + # POINT 54 + - position: + x: 3.65 + y: 6.09 + z: 0.0 + orientation: + x: 0.0 + y: 0.0 + z: 0.0 + w: 1.0 + # POINT 55 + - position: + x: -0.764 + y: -11.90 + z: 0.0 + orientation: + x: 0.0 + y: 0.0 + z: 0.7071068 + w: 0.7071068 + # POINT 56 + - position: + x: -28.14 + y: -40.50 + z: 0.0 + orientation: + x: 0.0 + y: 0.0 + z: 0.0 + w: 1.0 + # POINT 57 + - position: + x: -43.44 + y: -62.50 + z: 0.0 + orientation: + x: 0.0 + y: 0.0 + z: 0.0 + w: 1.0 + # POINT 58 + - position: + x: -56.44 + y: -81.30 + z: 0.0 + orientation: + x: 0.0 + y: 0.0 + z: 0.0 + w: 1.0 + # POINT 59 + - position: + x: -65.84 + y: -102.20 + z: 0.0 + orientation: + x: 0.0 + y: 0.0 + z: 0.0 + w: 1.0 + # POINT 60 + - position: + x: -77.94 + y: -132.00 + z: 0.0 + orientation: + x: 0.0 + y: 0.0 + z: 0.0 + w: 1.0 + # POINT 61 + - position: + x: -86.14 + y: -152.20 + z: 0.0 + orientation: + x: 0.0 + y: 0.0 + z: 0.0 + w: 1.0 + # POINT 62 + - position: + x: -92.94 + y: -179.10 + z: 0.0 + orientation: + x: 0.0 + y: 0.0 + z: 0.0 + w: 1.0 + # POINT 63 + - position: + x: -94.64 + y: -206.00 + z: 0.0 + orientation: + x: 0.0 + y: 0.0 + z: 0.7071068 + w: 0.7071068 diff --git a/smacc2_sm_reference_library/sm_dancebot_ue/params/nav2z_client/waypoints_plan_initial_road.yaml b/smacc2_sm_reference_library/sm_dancebot_ue/params/nav2z_client/waypoints_plan_initial_road.yaml new file mode 100644 index 000000000..6179ff5e0 --- /dev/null +++ b/smacc2_sm_reference_library/sm_dancebot_ue/params/nav2z_client/waypoints_plan_initial_road.yaml @@ -0,0 +1,251 @@ +waypoints: + # POINT 0 + - position: + x: -94.64 + y: -206.00 + z: 0.0 + orientation: + x: 0.0 + y: 0.0 + z: 0.0 + w: 0.0 + # POINT 1 + - position: + x: -92.94 + y: -179.10 + z: 0.0 + orientation: + x: 0.0 + y: 0.0 + z: 0.0 + w: 1.0 + # POINT 2 + - position: + x: -86.14 + y: -152.20 + z: 0.0 + orientation: + x: 0.0 + y: 0.0 + z: 0.0 + w: 1.0 + # POINT 3 + - position: + x: -77.94 + y: -132.00 + z: 0.0 + orientation: + x: 0.0 + y: 0.0 + z: 0.0 + w: 1.0 + # POINT 4 + - position: + x: -65.84 + y: -102.20 + z: 0.0 + orientation: + x: 0.0 + y: 0.0 + z: 0.0 + w: 1.0 + # POINT 5 + - position: + x: -56.44 + y: -81.30 + z: 0.0 + orientation: + x: 0.0 + y: 0.0 + z: 0.0 + w: 1.0 + # POINT 6 + - position: + x: -43.44 + y: -62.50 + z: 0.0 + orientation: + x: 0.0 + y: 0.0 + z: 0.0 + w: 1.0 + # POINT 7 + - position: + x: -28.14 + y: -40.50 + z: 0.0 + orientation: + x: 0.0 + y: 0.0 + z: 0.0 + w: 1.0 + # POINT 8 + - position: + x: -7.64 + y: -11.90 + z: 0.0 + orientation: + x: 0.0 + y: 0.0 + z: 0.7071068 + w: 0.7071068 + # POINT 9 + - position: + x: 3.65 + y: 6.09 + z: 0.0 + orientation: + x: 0.0 + y: 0.0 + z: 0.0 + w: 1.0 + # POINT 10 + - position: + x: 16.25 + y: 28.39 + z: 0.0 + orientation: + x: 0.0 + y: 0.0 + z: 0.0 + w: 1.0 + # POINT 11 + - position: + x: 26.65 + y: 52.79 + z: 0.0 + orientation: + x: 0.0 + y: 0.0 + z: 0.0 + w: 1.0 + # POINT 12 + - position: + x: 36.55 + y: 76.29 + z: 0.0 + orientation: + x: 0.0 + y: 0.0 + z: 0.0 + w: 1.0 + # POINT 13 + - position: + x: 46.35 + y: 97.49 + z: 0.0 + orientation: + x: 0.0 + y: 0.0 + z: 0.0 + w: 1.0 + # POINT 14 + - position: + x: 58.65 + y: 124.49 + z: 0.0 + orientation: + x: 0.0 + y: 0.0 + z: 0.7071068 + w: 0.7071068 + # POINT 15 + - position: + x: 70.55 + y: 149.99 + z: 0.0 + orientation: + x: 0.0 + y: 0.0 + z: 0.0 + w: 1.0 + # POINT 16 + - position: + x: 83.45 + y: 182.19 + z: 0.0 + orientation: + x: 0.0 + y: 0.0 + z: 0.0 + w: 1.0 + # POINT 17 + - position: + x: 98.05 + y: 213.19 + z: 0.0 + orientation: + x: 0.0 + y: 0.0 + z: 0.0 + w: 1.0 + # POINT 18 + - position: + x: 118.05 + y: 258.59 + z: 0.0 + orientation: + x: 0.0 + y: 0.0 + z: 0.7071068 + w: 0.7071068 + # POINT 19 + - position: + x: 138.55 + y: 303.89 + z: 0.0 + orientation: + x: 0.0 + y: 0.0 + z: 0.0 + w: 1.0 + # POINT 20 + - position: + x: 160.45 + y: 354.09 + z: 0.0 + orientation: + x: 0.0 + y: 0.0 + z: 0.0 + w: 1.0 + # POINT 21 + - position: + x: 214.65 + y: 453.99 + z: 0.0 + orientation: + x: 0.0 + y: 0.0 + z: 0.0 + w: 1.0 + # POINT 22 + - position: + x: 240.75 + y: 494.89 + z: 0.0 + orientation: + x: 0.0 + y: 0.0 + z: 0.0 + w: 1.0 + # POINT 23 + - position: + x: 262.85 + y: 535.19 + z: 0.0 + orientation: + x: 0.0 + y: 0.0 + z: 0.0 + w: 1.0 + # POINT 24 + - position: + x: 277.15 + y: 569.29 + z: 0.0 + orientation: + x: 0.0 + y: 0.0 + z: 0.0 + w: 1.0 diff --git a/smacc2_sm_reference_library/sm_dancebot_ue/params/nav2z_client/waypoints_plan_navigate_field.yaml b/smacc2_sm_reference_library/sm_dancebot_ue/params/nav2z_client/waypoints_plan_navigate_field.yaml new file mode 100644 index 000000000..b63b39bcf --- /dev/null +++ b/smacc2_sm_reference_library/sm_dancebot_ue/params/nav2z_client/waypoints_plan_navigate_field.yaml @@ -0,0 +1,111 @@ +waypoints: + # POINT 25 + - position: + x: 284.55 + y: 573.89 + z: 0.0 + orientation: + x: 0.0 + y: 0.0 + z: 0.0 + w: 1.0 + # POINT 26 + - position: + x: 293.25 + y: 575.39 + z: 0.0 + orientation: + x: 0.0 + y: 0.0 + z: 0.0 + w: 1.0 + # POINT 27 + - position: + x: 304.35 + y: 574.09 + z: 0.0 + orientation: + x: 0.0 + y: 0.0 + z: 0.0 + w: 1.0 + # POINT 28 + - position: + x: 333.65 + y: 575.79 + z: 0.0 + orientation: + x: 0.0 + y: 0.0 + z: 0.0 + w: 1.0 + # POINT 29 + - position: + x: 344.55 + y: 579.19 + z: 0.0 + orientation: + x: 0.0 + y: 0.0 + z: 0.0 + w: 1.0 + # POINT 30 + - position: + x: 351.65 + y: 587.99 + z: 0.0 + orientation: + x: 0.0 + y: 0.0 + z: 0.0 + w: 1.0 + # POINT 31 + - position: + x: 363.75 + y: 611.69 + z: 0.0 + orientation: + x: 0.0 + y: 0.0 + z: 0.0 + w: 1.0 + # POINT 32 + - position: + x: 370.65 + y: 626.49 + z: 0.0 + orientation: + x: 0.0 + y: 0.0 + z: 0.0 + w: 1.0 + # POINT 33 + - position: + x: 367.55 + y: 631.49 + z: 0.0 + orientation: + x: 0.0 + y: 0.0 + z: 0.0 + w: 1.0 + # POINT 34 + - position: + x: 354.75 + y: 632.69 + z: 0.0 + orientation: + x: 0.0 + y: 0.0 + z: 0.0 + w: 1.0 + # POINT 35 + - position: + x: 335.05 + y: 620.59 + z: 0.0 + orientation: + x: 0.0 + y: 0.0 + z: 0.0 + w: 1.0 diff --git a/smacc2_sm_reference_library/sm_dancebot_ue/params/sm_dancebot_ue_config.yaml b/smacc2_sm_reference_library/sm_dancebot_ue/params/sm_dancebot_ue_config.yaml new file mode 100644 index 000000000..2add60407 --- /dev/null +++ b/smacc2_sm_reference_library/sm_dancebot_ue/params/sm_dancebot_ue_config.yaml @@ -0,0 +1,12 @@ +SmDanceBotUE: + ros__parameters: + use_sim_time: true + signal_detector_loop_freq: 20.0 + clear_angular_distance_threshold: 0.1 # 0.05 + clear_point_distance_threshold: 0.4 #0.1 + record_angular_distance_threshold: 0.005 + record_point_distance_threshold: 0.1 + + waypoints_plan_initial_road: $(pkg_share)/params/nav2z_client/waypoints_plan_initial_road.yaml + waypoints_plan_navigate_field: $(pkg_share)/params/nav2z_client/waypoints_plan_navigate_field.yaml + waypoints_plan_back_on_road: $(pkg_share)/params/nav2z_client/waypoints_plan_back_on_road.yaml diff --git a/smacc2_sm_reference_library/sm_dancebot_ue/rviz/nav2_default_view.rviz b/smacc2_sm_reference_library/sm_dancebot_ue/rviz/nav2_default_view.rviz new file mode 100644 index 000000000..eaca2d2c2 --- /dev/null +++ b/smacc2_sm_reference_library/sm_dancebot_ue/rviz/nav2_default_view.rviz @@ -0,0 +1,806 @@ +Panels: + - Class: rviz_common/Displays + Help Height: 0 + Name: Displays + Property Tree Widget: + Expanded: + - /Global Options1 + - /TF1 + - /TF1/Frames1 + - /TF1/Tree1 + - /LaserScan1 + - /Global Planner1 + - /Global Planner1/Global Costmap1 + - /Controller1 + - /Controller1/Local Costmap1 + Splitter Ratio: 0.5833333134651184 + Tree Height: 654 + - Class: rviz_common/Selection + Name: Selection + - Class: rviz_common/Tool Properties + Expanded: + - /Publish Point1 + Name: Tool Properties + Splitter Ratio: 0.5886790156364441 + - Class: rviz_common/Views + Expanded: + - /Current View1 + Name: Views + Splitter Ratio: 0.5 + - Class: nav2_rviz_plugins/Navigation 2 + Name: Navigation 2 +Visualization Manager: + Class: "" + Displays: + - Alpha: 0.5 + Cell Size: 1 + Class: rviz_default_plugins/Grid + Color: 160; 160; 164 + Enabled: true + Line Style: + Line Width: 0.029999999329447746 + Value: Lines + Name: Grid + Normal Cell Count: 0 + Offset: + X: 0 + Y: 0 + Z: 0 + Plane: XY + Plane Cell Count: 10 + Reference Frame: + Value: true + - Alpha: 1 + Class: rviz_default_plugins/RobotModel + Collision Enabled: false + Description File: "" + Description Source: Topic + Description Topic: + Depth: 5 + Durability Policy: Volatile + History Policy: Keep Last + Reliability Policy: Reliable + Value: /robot_description + Enabled: false + Links: + All Links Enabled: true + Expand Joint Details: false + Expand Link Details: false + Expand Tree: false + Link Tree Style: "" + Mass Properties: + Inertia: false + Mass: false + Name: RobotModel + TF Prefix: "" + Update Interval: 0 + Value: false + Visual Enabled: true + - Class: rviz_default_plugins/TF + Enabled: true + Frame Timeout: 15 + Frames: + All Enabled: false + base_footprint: + Value: true + base_link: + Value: true + base_scan: + Value: true + camera_depth_frame: + Value: true + camera_depth_optical_frame: + Value: true + camera_link: + Value: true + camera_rgb_frame: + Value: true + camera_rgb_optical_frame: + Value: true + caster_back_left_link: + Value: true + caster_back_right_link: + Value: true + imu_link: + Value: true + map: + Value: true + odom: + Value: true + Marker Scale: 1 + Name: TF + Show Arrows: false + Show Axes: true + Show Names: false + Tree: + map: + odom: + base_footprint: + base_link: + base_scan: + {} + camera_link: + camera_depth_frame: + camera_depth_optical_frame: + {} + camera_rgb_frame: + camera_rgb_optical_frame: + {} + caster_back_left_link: + {} + caster_back_right_link: + {} + imu_link: + {} + Update Interval: 0 + Value: true + - Alpha: 1 + Autocompute Intensity Bounds: true + Autocompute Value Bounds: + Max Value: 10 + Min Value: -10 + Value: true + Axis: Z + Channel Name: intensity + Class: rviz_default_plugins/LaserScan + Color: 255; 0; 0 + Color Transformer: FlatColor + Decay Time: 0 + Enabled: true + Invert Rainbow: false + Max Color: 255; 255; 255 + Max Intensity: 0 + Min Color: 0; 0; 0 + Min Intensity: 0 + Name: LaserScan + Position Transformer: XYZ + Selectable: true + Size (Pixels): 5 + Size (m): 4 + Style: Points + Topic: + Depth: 5 + Durability Policy: Volatile + Filter size: 10 + History Policy: Keep Last + Reliability Policy: Best Effort + Value: /scan + Use Fixed Frame: true + Use rainbow: true + Value: true + - Alpha: 1 + Autocompute Intensity Bounds: true + Autocompute Value Bounds: + Max Value: 10 + Min Value: -10 + Value: true + Axis: Z + Channel Name: intensity + Class: rviz_default_plugins/PointCloud2 + Color: 255; 255; 255 + Color Transformer: "" + Decay Time: 0 + Enabled: true + Invert Rainbow: false + Max Color: 255; 255; 255 + Max Intensity: 4096 + Min Color: 0; 0; 0 + Min Intensity: 0 + Name: Bumper Hit + Position Transformer: "" + Selectable: true + Size (Pixels): 3 + Size (m): 0.07999999821186066 + Style: Spheres + Topic: + Depth: 5 + Durability Policy: Volatile + Filter size: 10 + History Policy: Keep Last + Reliability Policy: Best Effort + Value: /mobile_base/sensors/bumper_pointcloud + Use Fixed Frame: true + Use rainbow: true + Value: true + - Alpha: 1 + Class: rviz_default_plugins/Map + Color Scheme: map + Draw Behind: true + Enabled: true + Name: Map + Topic: + Depth: 1 + Durability Policy: Transient Local + Filter size: 10 + History Policy: Keep Last + Reliability Policy: Reliable + Value: /map + Update Topic: + Depth: 5 + Durability Policy: Volatile + History Policy: Keep Last + Reliability Policy: Reliable + Value: /map_updates + Use Timestamp: false + Value: true + - Alpha: 1 + Arrow Length: 0.019999999552965164 + Axes Length: 0.30000001192092896 + Axes Radius: 0.009999999776482582 + Class: rviz_default_plugins/PoseArray + Color: 0; 180; 0 + Enabled: true + Head Length: 0.07000000029802322 + Head Radius: 0.029999999329447746 + Name: Amcl Particle Swarm + Shaft Length: 0.23000000417232513 + Shaft Radius: 0.009999999776482582 + Shape: Arrow (Flat) + Topic: + Depth: 5 + Durability Policy: Volatile + Filter size: 10 + History Policy: Keep Last + Reliability Policy: Best Effort + Value: /particlecloud + Value: true + - Class: rviz_common/Group + Displays: + - Alpha: 0.10000000149011612 + Class: rviz_default_plugins/Map + Color Scheme: costmap + Draw Behind: false + Enabled: true + Name: Global Costmap + Topic: + Depth: 1 + Durability Policy: Transient Local + Filter size: 10 + History Policy: Keep Last + Reliability Policy: Reliable + Value: /global_costmap/costmap + Update Topic: + Depth: 5 + Durability Policy: Volatile + History Policy: Keep Last + Reliability Policy: Reliable + Value: /global_costmap/costmap_updates + Use Timestamp: false + Value: true + - Alpha: 1 + Buffer Length: 1 + Class: rviz_default_plugins/Path + Color: 255; 0; 0 + Enabled: true + Head Diameter: 0.019999999552965164 + Head Length: 0.019999999552965164 + Length: 0.30000001192092896 + Line Style: Lines + Line Width: 0.029999999329447746 + Name: Path + Offset: + X: 0 + Y: 0 + Z: 0 + Pose Color: 255; 85; 255 + Pose Style: Arrows + Radius: 0.029999999329447746 + Shaft Diameter: 0.004999999888241291 + Shaft Length: 0.019999999552965164 + Topic: + Depth: 5 + Durability Policy: Volatile + Filter size: 10 + History Policy: Keep Last + Reliability Policy: Reliable + Value: /plan + Value: true + - Alpha: 1 + Autocompute Intensity Bounds: true + Autocompute Value Bounds: + Max Value: 10 + Min Value: -10 + Value: true + Axis: Z + Channel Name: intensity + Class: rviz_default_plugins/PointCloud + Color: 125; 125; 125 + Color Transformer: FlatColor + Decay Time: 0 + Enabled: true + Invert Rainbow: false + Max Color: 255; 255; 255 + Max Intensity: 4096 + Min Color: 0; 0; 0 + Min Intensity: 0 + Name: VoxelGrid + Position Transformer: XYZ + Selectable: true + Size (Pixels): 3 + Size (m): 0.05000000074505806 + Style: Boxes + Topic: + Depth: 5 + Durability Policy: Volatile + Filter size: 10 + History Policy: Keep Last + Reliability Policy: Reliable + Value: /global_costmap/voxel_marked_cloud + Use Fixed Frame: true + Use rainbow: true + Value: true + - Alpha: 1 + Class: rviz_default_plugins/Polygon + Color: 25; 255; 0 + Enabled: false + Name: Polygon + Topic: + Depth: 5 + Durability Policy: Volatile + Filter size: 10 + History Policy: Keep Last + Reliability Policy: Reliable + Value: /global_costmap/published_footprint + Value: false + Enabled: true + Name: Global Planner + - Class: rviz_common/Group + Displays: + - Alpha: 0.10000000149011612 + Class: rviz_default_plugins/Map + Color Scheme: costmap + Draw Behind: false + Enabled: true + Name: Local Costmap + Topic: + Depth: 1 + Durability Policy: Transient Local + Filter size: 10 + History Policy: Keep Last + Reliability Policy: Reliable + Value: /local_costmap/costmap + Update Topic: + Depth: 5 + Durability Policy: Volatile + History Policy: Keep Last + Reliability Policy: Reliable + Value: /local_costmap/costmap_updates + Use Timestamp: false + Value: true + - Alpha: 1 + Buffer Length: 1 + Class: rviz_default_plugins/Path + Color: 0; 12; 255 + Enabled: true + Head Diameter: 0.30000001192092896 + Head Length: 0.20000000298023224 + Length: 0.30000001192092896 + Line Style: Lines + Line Width: 0.029999999329447746 + Name: Local Plan + Offset: + X: 0 + Y: 0 + Z: 0 + Pose Color: 255; 85; 255 + Pose Style: None + Radius: 0.029999999329447746 + Shaft Diameter: 0.10000000149011612 + Shaft Length: 0.10000000149011612 + Topic: + Depth: 5 + Durability Policy: Volatile + Filter size: 10 + History Policy: Keep Last + Reliability Policy: Reliable + Value: /local_plan + Value: true + - Class: rviz_default_plugins/MarkerArray + Enabled: false + Name: Trajectories + Namespaces: + {} + Topic: + Depth: 5 + Durability Policy: Volatile + History Policy: Keep Last + Reliability Policy: Reliable + Value: /marker + Value: false + - Alpha: 1 + Class: rviz_default_plugins/Polygon + Color: 25; 255; 0 + Enabled: true + Name: Polygon + Topic: + Depth: 5 + Durability Policy: Volatile + Filter size: 10 + History Policy: Keep Last + Reliability Policy: Reliable + Value: /local_costmap/published_footprint + Value: true + - Alpha: 1 + Autocompute Intensity Bounds: true + Autocompute Value Bounds: + Max Value: 10 + Min Value: -10 + Value: true + Axis: Z + Channel Name: intensity + Class: rviz_default_plugins/PointCloud + Color: 255; 255; 255 + Color Transformer: RGB8 + Decay Time: 0 + Enabled: true + Invert Rainbow: false + Max Color: 255; 255; 255 + Max Intensity: 4096 + Min Color: 0; 0; 0 + Min Intensity: 0 + Name: VoxelGrid + Position Transformer: XYZ + Selectable: true + Size (Pixels): 3 + Size (m): 0.009999999776482582 + Style: Flat Squares + Topic: + Depth: 5 + Durability Policy: Volatile + Filter size: 10 + History Policy: Keep Last + Reliability Policy: Reliable + Value: /local_costmap/voxel_marked_cloud + Use Fixed Frame: true + Use rainbow: true + Value: true + Enabled: true + Name: Controller + - Class: rviz_common/Group + Displays: + - Class: rviz_default_plugins/Image + Enabled: true + Max Value: 1 + Median window: 5 + Min Value: 0 + Name: RealsenseCamera + Normalize Range: true + Topic: + Depth: 5 + Durability Policy: Volatile + History Policy: Keep Last + Reliability Policy: Reliable + Value: /intel_realsense_r200_depth/image_raw + Value: true + - Alpha: 1 + Autocompute Intensity Bounds: true + Autocompute Value Bounds: + Max Value: 10 + Min Value: -10 + Value: true + Axis: Z + Channel Name: intensity + Class: rviz_default_plugins/PointCloud2 + Color: 255; 255; 255 + Color Transformer: RGB8 + Decay Time: 0 + Enabled: true + Invert Rainbow: false + Max Color: 255; 255; 255 + Max Intensity: 4096 + Min Color: 0; 0; 0 + Min Intensity: 0 + Name: RealsenseDepthImage + Position Transformer: XYZ + Selectable: true + Size (Pixels): 3 + Size (m): 0.009999999776482582 + Style: Flat Squares + Topic: + Depth: 5 + Durability Policy: Volatile + Filter size: 10 + History Policy: Keep Last + Reliability Policy: Reliable + Value: /intel_realsense_r200_depth/points + Use Fixed Frame: true + Use rainbow: true + Value: true + Enabled: false + Name: Realsense + - Alpha: 1 + Buffer Length: 1 + Class: rviz_default_plugins/Path + Color: 25; 255; 0 + Enabled: true + Head Diameter: 0.30000001192092896 + Head Length: 0.20000000298023224 + Length: 0.10000000149011612 + Line Style: Lines + Line Width: 0.029999999329447746 + Name: CpOdomTracker + Offset: + X: 0 + Y: 0 + Z: 0 + Pose Color: 255; 85; 255 + Pose Style: Axes + Radius: 0.009999999776482582 + Shaft Diameter: 0.10000000149011612 + Shaft Length: 0.10000000149011612 + Topic: + Depth: 5 + Durability Policy: Volatile + Filter size: 10 + History Policy: Keep Last + Reliability Policy: Reliable + Value: /odom_tracker_path + Value: true + - Alpha: 1 + Buffer Length: 1 + Class: rviz_default_plugins/Path + Color: 25; 255; 0 + Enabled: true + Head Diameter: 0.30000001192092896 + Head Length: 0.20000000298023224 + Length: 0.30000001192092896 + Line Style: Lines + Line Width: 0.029999999329447746 + Name: BackwardsGlobalPlan + Offset: + X: 0 + Y: 0 + Z: 0 + Pose Color: 255; 85; 255 + Pose Style: None + Radius: 0.029999999329447746 + Shaft Diameter: 0.10000000149011612 + Shaft Length: 0.10000000149011612 + Topic: + Depth: 5 + Durability Policy: Volatile + Filter size: 10 + History Policy: Keep Last + Reliability Policy: Reliable + Value: /backward_planner/global_plan + Value: true + - Angle Tolerance: 0.10000000149011612 + Class: rviz_default_plugins/Odometry + Covariance: + Orientation: + Alpha: 0.5 + Color: 255; 255; 127 + Color Style: Unique + Frame: Local + Offset: 1 + Scale: 1 + Value: true + Position: + Alpha: 0.30000001192092896 + Color: 204; 51; 204 + Scale: 1 + Value: true + Value: false + Enabled: true + Keep: 10000 + Name: Odometry + Position Tolerance: 0.10000000149011612 + Shape: + Alpha: 1 + Axes Length: 1 + Axes Radius: 0.10000000149011612 + Color: 255; 25; 0 + Head Length: 0 + Head Radius: 0 + Shaft Length: 0.02800000086426735 + Shaft Radius: 0.009999999776482582 + Value: Arrow + Topic: + Depth: 5 + Durability Policy: Volatile + Filter size: 10 + History Policy: Keep Last + Reliability Policy: Reliable + Value: /odom + Value: true + - Class: rviz_default_plugins/MarkerArray + Enabled: true + Name: WayPointsMarkers + Namespaces: + {} + Topic: + Depth: 5 + Durability Policy: Volatile + History Policy: Keep Last + Reliability Policy: Reliable + Value: /waypoints + Value: true + - Class: rviz_default_plugins/MarkerArray + Enabled: true + Name: VisualizationMarkers + Namespaces: + {} + Topic: + Depth: 5 + Durability Policy: Volatile + History Policy: Keep Last + Reliability Policy: Reliable + Value: visualization_marker_array + Value: true + - Class: rviz_default_plugins/MarkerArray + Enabled: true + Name: BackwardsGlobalPlanMarkers + Namespaces: + {} + Topic: + Depth: 5 + Durability Policy: Volatile + History Policy: Keep Last + Reliability Policy: Reliable + Value: backward_planner/markers + Value: true + - Class: rviz_default_plugins/MarkerArray + Enabled: true + Name: BackwardLocalPlannerGoalMarker + Namespaces: + my_namespace2: true + Topic: + Depth: 5 + Durability Policy: Volatile + History Policy: Keep Last + Reliability Policy: Reliable + Value: /backward_local_planner/goal_marker + Value: true + - Class: rviz_default_plugins/MarkerArray + Enabled: true + Name: ForwardCarrot + Namespaces: + {} + Topic: + Depth: 5 + Durability Policy: Volatile + History Policy: Keep Last + Reliability Policy: Reliable + Value: /forward_local_planner/carrot_goal_marker + Value: true + - Class: rviz_default_plugins/MarkerArray + Enabled: true + Name: UndoGlobalPlannerMarkers + Namespaces: + my_namespace2: true + Topic: + Depth: 5 + Durability Policy: Volatile + History Policy: Keep Last + Reliability Policy: Reliable + Value: /undo_path_planner/markers + Value: true + - Alpha: 1 + Buffer Length: 1 + Class: rviz_default_plugins/Path + Color: 25; 255; 0 + Enabled: true + Head Diameter: 0.30000001192092896 + Head Length: 0.20000000298023224 + Length: 0.30000001192092896 + Line Style: Lines + Line Width: 0.029999999329447746 + Name: UndoGlobalPlan + Offset: + X: 0 + Y: 0 + Z: 0 + Pose Color: 255; 85; 255 + Pose Style: None + Radius: 0.029999999329447746 + Shaft Diameter: 0.10000000149011612 + Shaft Length: 0.10000000149011612 + Topic: + Depth: 5 + Durability Policy: Volatile + Filter size: 10 + History Policy: Keep Last + Reliability Policy: Reliable + Value: /undo_path_planner/global_plan + Value: true + - Alpha: 1 + Buffer Length: 1 + Class: rviz_default_plugins/Path + Color: 255; 255; 0 + Enabled: true + Head Diameter: 0.05999999865889549 + Head Length: 0.03999999910593033 + Length: 0.30000001192092896 + Line Style: Billboards + Line Width: 0.0010000000474974513 + Name: BackwardLocalPlan + Offset: + X: 0 + Y: 0 + Z: 0 + Pose Color: 255; 85; 0 + Pose Style: Arrows + Radius: 0.029999999329447746 + Shaft Diameter: 0.019999999552965164 + Shaft Length: 0.009999999776482582 + Topic: + Depth: 5 + Durability Policy: Volatile + Filter size: 10 + History Policy: Keep Last + Reliability Policy: Reliable + Value: /backward_local_planner/path + Value: true + Enabled: true + Global Options: + Background Color: 48; 48; 48 + Fixed Frame: map + Frame Rate: 30 + Name: root + Tools: + - Class: rviz_default_plugins/MoveCamera + - Class: rviz_default_plugins/Select + - Class: rviz_default_plugins/FocusCamera + - Class: rviz_default_plugins/Measure + Line color: 128; 128; 0 + - Class: rviz_default_plugins/SetInitialPose + Covariance x: 0.25 + Covariance y: 0.25 + Covariance yaw: 0.06853891909122467 + Topic: + Depth: 5 + Durability Policy: Volatile + History Policy: Keep Last + Reliability Policy: Reliable + Value: /initialpose + - Class: rviz_default_plugins/PublishPoint + Single click: true + Topic: + Depth: 5 + Durability Policy: Volatile + History Policy: Keep Last + Reliability Policy: Reliable + Value: /clicked_point + - Class: nav2_rviz_plugins/GoalTool + Transformation: + Current: + Class: rviz_default_plugins/TF + Value: true + Views: + Current: + Angle: -4.795001029968262 + Class: rviz_default_plugins/TopDownOrtho + Enable Stereo Rendering: + Stereo Eye Separation: 0.05999999865889549 + Stereo Focal Distance: 1 + Swap Stereo Eyes: false + Value: false + Invert Z Axis: false + Name: Current View + Near Clip Distance: 0.009999999776482582 + Scale: -91.58348083496094 + Target Frame: base_link + Value: TopDownOrtho (rviz_default_plugins) + X: -0.6155462265014648 + Y: -0.8152815103530884 + Saved: ~ +Window Geometry: + Displays: + collapsed: true + Height: 829 + Hide Left Dock: true + Hide Right Dock: true + Navigation 2: + collapsed: false + QMainWindow State: 000000ff00000000fd000000040000000000000286000002d5fc020000000afb0000001200530065006c0065006300740069006f006e000000003d000000830000006e00fffffffb0000001e0054006f006f006c002000500072006f007000650072007400690065007302000001ed000001df00000185000000a3fb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb000000100044006900730070006c0061007900730000000047000002d5000000f100fffffffb0000002000730065006c0065006300740069006f006e00200062007500660066006500720200000138000000aa0000023a00000294fb00000014005700690064006500530074006500720065006f02000000e6000000d2000003ee0000030bfb0000000c004b0069006e0065006300740200000186000001060000030c00000261fb00000018004e0061007600690067006100740069006f006e0020003200000002da000001010000017100fffffffb0000001e005200650061006c00730065006e0073006500430061006d00650072006100000002c6000000c10000002f00ffffff000000010000010f0000039efc0200000003fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000000a00560069006500770073000000003d0000039e000000c200fffffffb0000001200530065006c0065006300740069006f006e010000025a000000b200000000000000000000000200000490000000a9fc0100000001fb0000000a00560069006500770073030000004e00000080000002e10000019700000003000004420000003efc0100000002fb0000000800540069006d00650100000000000004420000000000000000fb0000000800540069006d0065010000000000000450000000000000000000000546000002d500000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000 + RealsenseCamera: + collapsed: false + Selection: + collapsed: false + Tool Properties: + collapsed: false + Views: + collapsed: true + Width: 1350 + X: 90 + Y: 34 diff --git a/smacc2_sm_reference_library/sm_dancebot_ue/rviz/nav2_namespaced_view.rviz b/smacc2_sm_reference_library/sm_dancebot_ue/rviz/nav2_namespaced_view.rviz new file mode 100644 index 000000000..2a024f75b --- /dev/null +++ b/smacc2_sm_reference_library/sm_dancebot_ue/rviz/nav2_namespaced_view.rviz @@ -0,0 +1,371 @@ +Panels: + - Class: rviz_common/Displays + Help Height: 195 + Name: Displays + Property Tree Widget: + Expanded: + - /Global Options1 + - /RobotModel1/Status1 + - /TF1/Frames1 + - /TF1/Tree1 + - /Global Planner1/Global Costmap1 + Splitter Ratio: 0.5833333134651184 + Tree Height: 464 + - Class: rviz_common/Selection + Name: Selection + - Class: rviz_common/Tool Properties + Expanded: + - /Publish Point1 + Name: Tool Properties + Splitter Ratio: 0.5886790156364441 + - Class: rviz_common/Views + Expanded: + - /Current View1 + Name: Views + Splitter Ratio: 0.5 + - Class: nav2_rviz_plugins/Navigation 2 + Name: Navigation 2 +Visualization Manager: + Class: "" + Displays: + - Alpha: 0.5 + Cell Size: 1 + Class: rviz_default_plugins/Grid + Color: 160; 160; 164 + Enabled: true + Line Style: + Line Width: 0.029999999329447746 + Value: Lines + Name: Grid + Normal Cell Count: 0 + Offset: + X: 0 + Y: 0 + Z: 0 + Plane: XY + Plane Cell Count: 10 + Reference Frame: + Value: true + - Alpha: 1 + Class: rviz_default_plugins/RobotModel + Collision Enabled: false + Description File: "" + Description Source: Topic + Description Topic: + Depth: 5 + Durability Policy: Volatile + History Policy: Keep Last + Reliability Policy: Reliable + Value: /robot_description + Enabled: false + Links: + All Links Enabled: true + Expand Joint Details: false + Expand Link Details: false + Expand Tree: false + Link Tree Style: Links in Alphabetic Order + Name: RobotModel + TF Prefix: "" + Update Interval: 0 + Value: true + Visual Enabled: true + - Class: rviz_default_plugins/TF + Enabled: true + Frame Timeout: 15 + Frames: + All Enabled: false + Marker Scale: 1 + Name: TF + Show Arrows: true + Show Axes: true + Show Names: false + Tree: + {} + Update Interval: 0 + Value: true + - Alpha: 1 + Autocompute Intensity Bounds: true + Autocompute Value Bounds: + Max Value: 10 + Min Value: -10 + Value: true + Axis: Z + Channel Name: intensity + Class: rviz_default_plugins/LaserScan + Color: 255; 255; 255 + Color Transformer: Intensity + Decay Time: 0 + Enabled: true + Invert Rainbow: false + Max Color: 255; 255; 255 + Max Intensity: 0 + Min Color: 0; 0; 0 + Min Intensity: 0 + Name: LaserScan + Position Transformer: XYZ + Selectable: true + Size (Pixels): 3 + Size (m): 0.009999999776482582 + Style: Flat Squares + Topic: + Depth: 5 + Durability Policy: Volatile + History Policy: Keep Last + Reliability Policy: Best Effort + Value: /scan + Use Fixed Frame: true + Use rainbow: true + Value: true + - Alpha: 1 + Autocompute Intensity Bounds: true + Autocompute Value Bounds: + Max Value: 10 + Min Value: -10 + Value: true + Axis: Z + Channel Name: intensity + Class: rviz_default_plugins/PointCloud2 + Color: 255; 255; 255 + Color Transformer: "" + Decay Time: 0 + Enabled: true + Invert Rainbow: false + Max Color: 255; 255; 255 + Max Intensity: 4096 + Min Color: 0; 0; 0 + Min Intensity: 0 + Name: Bumper Hit + Position Transformer: "" + Selectable: true + Size (Pixels): 3 + Size (m): 0.07999999821186066 + Style: Spheres + Topic: + Depth: 5 + Durability Policy: Volatile + History Policy: Keep Last + Reliability Policy: Best Effort + Value: /mobile_base/sensors/bumper_pointcloud + Use Fixed Frame: true + Use rainbow: true + Value: true + - Alpha: 1 + Class: rviz_default_plugins/Map + Color Scheme: map + Draw Behind: true + Enabled: true + Name: Map + Topic: + Depth: 1 + Durability Policy: Transient Local + History Policy: Keep Last + Reliability Policy: Reliable + Value: /map + Use Timestamp: false + Value: true + - Alpha: 1 + Arrow Length: 0.019999999552965164 + Axes Length: 0.30000001192092896 + Axes Radius: 0.009999999776482582 + Class: rviz_default_plugins/PoseArray + Color: 0; 180; 0 + Enabled: true + Head Length: 0.07000000029802322 + Head Radius: 0.029999999329447746 + Name: Amcl Particle Swarm + Shaft Length: 0.23000000417232513 + Shaft Radius: 0.009999999776482582 + Shape: Arrow (Flat) + Topic: + Depth: 5 + Durability Policy: Volatile + History Policy: Keep Last + Reliability Policy: Best Effort + Value: /particlecloud + Value: true + - Class: rviz_common/Group + Displays: + - Alpha: 0.30000001192092896 + Class: rviz_default_plugins/Map + Color Scheme: costmap + Draw Behind: false + Enabled: true + Name: Global Costmap + Topic: + Depth: 1 + Durability Policy: Transient Local + History Policy: Keep Last + Reliability Policy: Reliable + Value: /global_costmap/costmap + Use Timestamp: false + Value: true + - Alpha: 1 + Buffer Length: 1 + Class: rviz_default_plugins/Path + Color: 255; 0; 0 + Enabled: true + Head Diameter: 0.019999999552965164 + Head Length: 0.019999999552965164 + Length: 0.30000001192092896 + Line Style: Lines + Line Width: 0.029999999329447746 + Name: Path + Offset: + X: 0 + Y: 0 + Z: 0 + Pose Color: 255; 85; 255 + Pose Style: Arrows + Radius: 0.029999999329447746 + Shaft Diameter: 0.004999999888241291 + Shaft Length: 0.019999999552965164 + Topic: + Depth: 5 + Durability Policy: Volatile + History Policy: Keep Last + Reliability Policy: Reliable + Value: /plan + Value: true + - Alpha: 1 + Class: rviz_default_plugins/Polygon + Color: 25; 255; 0 + Enabled: false + Name: Polygon + Topic: + Depth: 5 + Durability Policy: Volatile + History Policy: Keep Last + Reliability Policy: Reliable + Value: /global_costmap/published_footprint + Value: false + Enabled: true + Name: Global Planner + - Class: rviz_common/Group + Displays: + - Alpha: 0.699999988079071 + Class: rviz_default_plugins/Map + Color Scheme: costmap + Draw Behind: false + Enabled: true + Name: Local Costmap + Topic: + Depth: 1 + Durability Policy: Transient Local + History Policy: Keep Last + Reliability Policy: Reliable + Value: /local_costmap/costmap + Use Timestamp: false + Value: true + - Alpha: 1 + Buffer Length: 1 + Class: rviz_default_plugins/Path + Color: 0; 12; 255 + Enabled: true + Head Diameter: 0.30000001192092896 + Head Length: 0.20000000298023224 + Length: 0.30000001192092896 + Line Style: Lines + Line Width: 0.029999999329447746 + Name: Local Plan + Offset: + X: 0 + Y: 0 + Z: 0 + Pose Color: 255; 85; 255 + Pose Style: None + Radius: 0.029999999329447746 + Shaft Diameter: 0.10000000149011612 + Shaft Length: 0.10000000149011612 + Topic: + Depth: 5 + Durability Policy: Volatile + History Policy: Keep Last + Reliability Policy: Reliable + Value: /local_plan + Value: true + - Class: rviz_default_plugins/MarkerArray + Enabled: false + Name: Trajectories + Namespaces: + {} + Topic: + Depth: 5 + Durability Policy: Volatile + History Policy: Keep Last + Reliability Policy: Reliable + Value: /marker + Value: false + - Alpha: 1 + Class: rviz_default_plugins/Polygon + Color: 25; 255; 0 + Enabled: true + Name: Polygon + Topic: + Depth: 5 + Durability Policy: Volatile + History Policy: Keep Last + Reliability Policy: Reliable + Value: /local_costmap/published_footprint + Value: true + Enabled: true + Name: Controller + Enabled: true + Global Options: + Background Color: 48; 48; 48 + Fixed Frame: map + Frame Rate: 30 + Name: root + Tools: + - Class: rviz_default_plugins/MoveCamera + - Class: rviz_default_plugins/Select + - Class: rviz_default_plugins/FocusCamera + - Class: rviz_default_plugins/Measure + Line color: 128; 128; 0 + - Class: rviz_default_plugins/SetInitialPose + Topic: + Depth: 5 + Durability Policy: Volatile + History Policy: Keep Last + Reliability Policy: Reliable + Value: /initialpose + - Class: nav2_rviz_plugins/GoalTool + Transformation: + Current: + Class: rviz_default_plugins/TF + Value: true + Views: + Current: + Angle: -1.5700000524520874 + Class: rviz_default_plugins/TopDownOrtho + Enable Stereo Rendering: + Stereo Eye Separation: 0.05999999865889549 + Stereo Focal Distance: 1 + Swap Stereo Eyes: false + Value: false + Invert Z Axis: false + Name: Current View + Near Clip Distance: 0.009999999776482582 + Scale: 134.638427734375 + Target Frame: + Value: TopDownOrtho (rviz_default_plugins) + X: -0.032615214586257935 + Y: -0.0801941454410553 + Saved: ~ +Window Geometry: + Displays: + collapsed: false + Height: 914 + Hide Left Dock: false + Hide Right Dock: true + Navigation 2: + collapsed: false + QMainWindow State: 000000ff00000000fd00000004000000000000016a00000338fc0200000009fb0000001200530065006c0065006300740069006f006e00000001e10000009b0000005c00fffffffb0000001e0054006f006f006c002000500072006f007000650072007400690065007302000001ed000001df00000185000000a3fb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb000000100044006900730070006c006100790073010000003d000002c4000000c900fffffffb0000002000730065006c0065006300740069006f006e00200062007500660066006500720200000138000000aa0000023a00000294fb00000014005700690064006500530074006500720065006f02000000e6000000d2000003ee0000030bfb0000000c004b0069006e0065006300740200000186000001060000030c00000261fb00000018004e0061007600690067006100740069006f006e0020003201000003070000006e0000006200ffffff000000010000010f00000338fc0200000003fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000000a00560069006500770073000000003d00000338000000a400fffffffb0000001200530065006c0065006300740069006f006e010000025a000000b200000000000000000000000200000490000000a9fc0100000001fb0000000a00560069006500770073030000004e00000080000002e10000019700000003000004420000003efc0100000002fb0000000800540069006d00650100000000000004420000000000000000fb0000000800540069006d00650100000000000004500000000000000000000004990000033800000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000 + Selection: + collapsed: false + Tool Properties: + collapsed: false + Views: + collapsed: true + Width: 1545 + X: 186 + Y: 117 diff --git a/smacc2_sm_reference_library/sm_dancebot_ue/scripts/test_oscillation.py b/smacc2_sm_reference_library/sm_dancebot_ue/scripts/test_oscillation.py new file mode 100755 index 000000000..3008e133b --- /dev/null +++ b/smacc2_sm_reference_library/sm_dancebot_ue/scripts/test_oscillation.py @@ -0,0 +1,165 @@ +#!/usr/bin/env python3 + +# Copyright (c) 2018 Intel Corporation +# +# Licensed under the Apache License, Version 2.0 (the "License"); +# you may not use this file except in compliance with the License. +# You may obtain a copy of the License at +# +# http://www.apache.org/licenses/LICENSE-2.0 +# +# Unless required by applicable law or agreed to in writing, software +# distributed under the License is distributed on an "AS IS" BASIS, +# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +# See the License for the specific language governing permissions and +# limitations under the License. + +import rclpy +from rclpy.node import Node +from geometry_msgs.msg import Twist +from tf2_ros import TransformListener +from tf2_ros import Buffer +import math +import tf_transformations +import std_msgs.msg + + +class RotationOscillation(Node): + def __init__(self): + super().__init__("rotation_oscillation") + self.cmd_vel_pub = self.create_publisher(Twist, "cmd_vel", 1) + self.yaw_pub = self.create_publisher(std_msgs.msg.Float64, "yaw", 1) + + self.tf_buffer = Buffer() + self.tf_listener = TransformListener(self.tf_buffer, self) + self.on_timer = self.create_timer(0.05, self.on_timer_callback) + + self.base_link_frame = "base_footprint" + self.fixed_frame = "odom" + + def on_timer_callback(self): + try: + base_link_transform = self.tf_buffer.lookup_transform( + self.fixed_frame, self.base_link_frame, rclpy.time.Time() + ) + q = base_link_transform.transform.rotation + euler = tf_transformations.euler_from_quaternion([q.x, q.y, q.z, q.w]) + self.get_logger().info("Current rotation: %f" % math.degrees(euler[2])) + self.get_logger().info("Transform available!") + + t = float(self.get_clock().now().nanoseconds) / 1e9 + cmd_vel = Twist() + cmd_vel.angular.z = 1.0 if math.sin(t) > 0.0 else -1.0 + self.cmd_vel_pub.publish(cmd_vel) + + self.yaw_pub.publish(std_msgs.msg.Float64(data=euler[2])) + + self.get_logger().info("Current time: %s" % str(t)) + except Exception as e: + self.get_logger().error("Error getting transform: %s" % str(e)) + + def oscillate_rotation(self): + rate = self.create_rate(1) + angle = math.radians(25) + # timeout = rclpy.duration.Duration(seconds=1.0) # Timeout for waiting on transforms + + fixed_frame = "odom" + base_link_frame = "base_footprint" + + print(f'Waiting for transform from "{fixed_frame}" to "{base_link_frame}"...') + while rclpy.ok(): + # self.get_logger().info(f'Waiting for transform from "{fixed_frame}" to "{base_link_frame}"...') + rclpy.spin_once(self) + # self.get_logger().info('Spin once, now: %s' % str(self.get_clock().now().toSec())) + rate.sleep() + + self.get_logger().info("Transform available!") + + # Get initial position + print("Getting initial position...") + try: + base_link_transform = self.tf_buffer.lookup_transform( + "map", base_link_frame, rclpy.time.Time() + ) + initial_position = base_link_transform.transform.translation + except Exception as e: + self.get_logger().error("Error getting initial position: %s" % str(e)) + return + + # Rotate 25 degrees to the left + print("Rotating 25 degrees to the left...") + self.get_logger().info("Rotating 25 degrees to the left...") + twist_msg = Twist() + twist_msg.angular.z = angle + self.cmd_vel_pub.publish(twist_msg) + rclpy.spin_once(self) + + # Get position after left rotation + print("Getting position after left rotation...") + try: + print("Getting position after left rotation...") + base_link_transform = self.tf_buffer.lookup_transform( + "map", base_link_frame, rclpy.time.Time() + ) + left_rotation_position = base_link_transform.transform.translation + except Exception as e: + self.get_logger().error("Error getting position after left rotation: %s" % str(e)) + return + + # Rotate 25 degrees to the right + print("Rotating 25 degrees to the right...") + self.get_logger().info("Rotating 25 degrees to the right...") + twist_msg.angular.z = -angle + self.cmd_vel_pub.publish(twist_msg) + rclpy.spin_once(self) + + # Get position after right rotation + try: + base_link_transform = self.tf_buffer.lookup_transform( + "map", base_link_frame, rclpy.time.Time() + ) + right_rotation_position = base_link_transform.transform.translation + except Exception as e: + self.get_logger().error("Error getting position after right rotation: %s" % str(e)) + return + + # Calculate phase difference + print("Calculating phase difference...") + left_rotation_angle = math.atan2( + left_rotation_position.y - initial_position.y, + left_rotation_position.x - initial_position.x, + ) + right_rotation_angle = math.atan2( + right_rotation_position.y - initial_position.y, + right_rotation_position.x - initial_position.x, + ) + phase_difference = math.degrees(right_rotation_angle - left_rotation_angle) + + self.get_logger().info("Phase difference: %f degrees" % phase_difference) + + # Stop the robot + twist_msg.angular.z = 0.0 + self.cmd_vel_pub.publish(twist_msg) + + def shutdown(self): + self.get_logger().info("Shutting down...") + self.destroy_node() + + +def main(args=None): + rclpy.init(args=args) + rotation_oscillation = RotationOscillation() + + rclpy.spin(rotation_oscillation) + + try: + rotation_oscillation.oscillate_rotation() + except Exception as e: + rotation_oscillation.get_logger().error("Error during rotation oscillation: %s" % str(e)) + + rotation_oscillation.shutdown() + rclpy.shutdown() + + +if __name__ == "__main__": + main() diff --git a/smacc2_sm_reference_library/sm_dancebot_ue/scripts/transform_publisher.py b/smacc2_sm_reference_library/sm_dancebot_ue/scripts/transform_publisher.py new file mode 100644 index 000000000..622200fe4 --- /dev/null +++ b/smacc2_sm_reference_library/sm_dancebot_ue/scripts/transform_publisher.py @@ -0,0 +1,78 @@ +#!/usr/bin/env python3 + +# Copyright (c) 2018 Intel Corporation +# +# Licensed under the Apache License, Version 2.0 (the "License"); +# you may not use this file except in compliance with the License. +# You may obtain a copy of the License at +# +# http://www.apache.org/licenses/LICENSE-2.0 +# +# Unless required by applicable law or agreed to in writing, software +# distributed under the License is distributed on an "AS IS" BASIS, +# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +# See the License for the specific language governing permissions and +# limitations under the License. + +import rclpy +from rclpy.node import Node +import tf2_ros +from geometry_msgs.msg import TransformStamped + + +class StaticTransformPublisher(Node): + def __init__(self, name, parent_frame, child_frame, xyz=[0.0, 0.0, 0.0], rpy=[0.0, 0.0, 0.0]): + super().__init__(name) + self.transform_broadcaster = tf2_ros.TransformBroadcaster(self) + + self.timer = self.create_timer(0.01, self.publish_transform) + self.transform = TransformStamped() + self.transform.header.frame_id = parent_frame + self.transform.child_frame_id = child_frame + self.transform.transform.translation.x = xyz[0] + self.transform.transform.translation.y = xyz[1] + self.transform.transform.translation.z = xyz[2] + + self.transform.transform.rotation.x = 0.0 + self.transform.transform.rotation.y = 0.0 + self.transform.transform.rotation.z = 0.0 + self.transform.transform.rotation.w = 1.0 + + def publish_transform(self): + self.transform.header.stamp = rclpy.time.Time(0) + self.transform_broadcaster.sendTransform(self.transform) + + +def main(args=None): + rclpy.init(args=args) + + # Create the first instance of StaticTransformPublisher + node1 = StaticTransformPublisher( + "static_transform_publisher_1", + "base_footprint", + "base_link", + ) + + # Create the second instance of StaticTransformPublisher + node2 = StaticTransformPublisher( + "static_transform_publisher_2", + "base_link", + "base_scan", + xyz=[-0.064, 0.0, 0.122], + ) + + try: + # Spin both nodes simultaneously + while rclpy.ok(): + rclpy.spin_once(node1) + rclpy.spin_once(node2) + except KeyboardInterrupt: + pass + + node1.destroy_node() + node2.destroy_node() + rclpy.shutdown() + + +if __name__ == "__main__": + main() diff --git a/smacc2_sm_reference_library/sm_dancebot_ue/servers/led_action_server/action/LEDControl.action b/smacc2_sm_reference_library/sm_dancebot_ue/servers/led_action_server/action/LEDControl.action new file mode 100644 index 000000000..e4dc1d976 --- /dev/null +++ b/smacc2_sm_reference_library/sm_dancebot_ue/servers/led_action_server/action/LEDControl.action @@ -0,0 +1,18 @@ +uint8 command + +# Possible command values +uint8 CMD_ON = 0 +uint8 CMD_OFF = 1 +--- +uint8 state + +# Possible feeedback values (tool states) +uint8 STATE_UNKNOWN = 0 +uint8 STATE_RUNNING = 1 +uint8 STATE_IDLE = 2 +--- +uint8 result + +# Possible result values +uint8 RESULT_SUCCESS = 0 +uint8 RESULT_ERROR = 1 diff --git a/smacc2_sm_reference_library/sm_dancebot_ue/servers/led_action_server/src/led_action_server_node.cpp b/smacc2_sm_reference_library/sm_dancebot_ue/servers/led_action_server/src/led_action_server_node.cpp new file mode 100644 index 000000000..c72c64e90 --- /dev/null +++ b/smacc2_sm_reference_library/sm_dancebot_ue/servers/led_action_server/src/led_action_server_node.cpp @@ -0,0 +1,230 @@ +// 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 +#include +#include + +// This class describes a preemptable-on/off tool action server to be used from smacc +// shows in rviz a sphere whose color describe the current state (unknown, running, idle) +class LEDActionServer : public rclcpp::Node +{ +public: + std::shared_ptr> as_; + using GoalHandleLEDControl = + rclcpp_action::ServerGoalHandle; + + rclcpp::Publisher::SharedPtr stateMarkerPublisher_; + + uint8_t cmd; + + uint8_t currentState_; + + /** +****************************************************************************************************************** +* constructor() +****************************************************************************************************************** +*/ + LEDActionServer(const rclcpp::NodeOptions & options = rclcpp::NodeOptions()) + : Node("led_action_server_node", options) + { + currentState_ = sm_dancebot_ue::action::LEDControl::Result::STATE_UNKNOWN; + } + + /** +****************************************************************************************************************** +* execute() +****************************************************************************************************************** +*/ + void execute(const std::shared_ptr + gh) // Note: "Action" is not appended to DoDishes here + { + auto goal = gh->get_goal(); + RCLCPP_INFO_STREAM(get_logger(), "Tool action server request: " << goal->command); + + cmd = goal->command; + + if (goal->command == sm_dancebot_ue::action::LEDControl_Goal::CMD_ON) + { + RCLCPP_INFO(this->get_logger(), "ON"); + currentState_ = sm_dancebot_ue::action::LEDControl_Result::STATE_RUNNING; + } + else if (goal->command == sm_dancebot_ue::action::LEDControl_Goal::CMD_OFF) + { + RCLCPP_INFO(this->get_logger(), "OFF"); + currentState_ = sm_dancebot_ue::action::LEDControl_Result::STATE_IDLE; + } + + auto feedback_msg = std::make_shared(); + + // 10Hz internal loop + rclcpp::Rate rate(20); + + while (rclcpp::ok()) + { + gh->publish_feedback(feedback_msg); + + publishStateMarker(); + rate.sleep(); + RCLCPP_INFO_THROTTLE(this->get_logger(), *(this->get_clock()), 2000, "Loop feedback"); + } + + auto result = std::make_shared(); + result->state = this->currentState_; + + // never reach succeeded because were are interested in keeping the feedback alive + //as_->setSucceeded(); + gh->succeed(result); + } + + rclcpp_action::GoalResponse handle_goal( + const rclcpp_action::GoalUUID & /*uuid*/, + std::shared_ptr /*goal*/) + { + // (void)uuid; + // // Let's reject sequences that are over 9000 + // if (goal->order > 9000) { + // return rclcpp_action::GoalResponse::REJECT; + // } + + // lets accept everything + RCLCPP_INFO(this->get_logger(), "Handle goal"); + return rclcpp_action::GoalResponse::ACCEPT_AND_EXECUTE; + } + + rclcpp_action::CancelResponse handle_cancel( + const std::shared_ptr /*goal_handle*/) + { + RCLCPP_INFO(rclcpp::get_logger("server"), "Got request to cancel goal"); + //(void)goal_handle; + return rclcpp_action::CancelResponse::ACCEPT; + } + + void handle_accepted(const std::shared_ptr goal_handle) + { + // this needs to return quickly to avoid blocking the executor, so spin up a new thread + std::thread{std::bind(&LEDActionServer::execute, this, std::placeholders::_1), goal_handle} + .detach(); + } + + /** +****************************************************************************************************************** +* run() +****************************************************************************************************************** +*/ + void run() + { + RCLCPP_INFO(this->get_logger(), "Creating tool action server"); + //as_ = std::make_shared(n, "led_action_server", boost::bind(&LEDActionServer::execute, this, _1), false); + + this->as_ = rclcpp_action::create_server( + this, "led_action_server", + std::bind(&LEDActionServer::handle_goal, this, std::placeholders::_1, std::placeholders::_2), + std::bind(&LEDActionServer::handle_cancel, this, std::placeholders::_1), + std::bind(&LEDActionServer::handle_accepted, this, std::placeholders::_1)); + + RCLCPP_INFO(get_logger(), "Starting Tool Action Server"); + stateMarkerPublisher_ = + this->create_publisher("tool_markers", 1); + } + + /** +****************************************************************************************************************** +* publishStateMarker() +****************************************************************************************************************** +*/ + void publishStateMarker() + { + visualization_msgs::msg::Marker marker; + marker.header.frame_id = "base_link"; + marker.header.stamp = this->now(); + + marker.ns = "tool_namespace"; + marker.id = 0; + marker.type = visualization_msgs::msg::Marker::SPHERE; + marker.action = visualization_msgs::msg::Marker::ADD; + + marker.scale.x = 0.2; + marker.scale.y = 0.2; + marker.scale.z = 0.2; + + marker.color.a = 1; + + if (currentState_ == sm_dancebot_ue::action::LEDControl::Result::STATE_RUNNING) + { + // show green ball + marker.color.r = 0; + marker.color.g = 1; + marker.color.b = 0; + } + else if (currentState_ == sm_dancebot_ue::action::LEDControl::Result::STATE_IDLE) + { + // show gray ball + marker.color.r = 0.7; + marker.color.g = 0.7; + marker.color.b = 0.7; + } + else + { + // show black ball + marker.color.r = 0; + marker.color.g = 0; + marker.color.b = 0; + } + + marker.pose.orientation.w = 1; + marker.pose.position.x = 0; + marker.pose.position.y = 0; + marker.pose.position.z = 1; + + visualization_msgs::msg::MarkerArray ma; + ma.markers.push_back(marker); + + stateMarkerPublisher_->publish(ma); + } +}; + +/** +****************************************************************************************************************** +* main() +****************************************************************************************************************** +*/ +int main(int argc, char ** argv) +{ + rclcpp::init(argc, argv); + auto ledactionserver = std::make_shared(); + ledactionserver->run(); + + rclcpp::spin(ledactionserver); + rclcpp::shutdown(); + return 0; +} diff --git a/smacc2_sm_reference_library/sm_dancebot_ue/servers/service_node_3/service_node_3.py b/smacc2_sm_reference_library/sm_dancebot_ue/servers/service_node_3/service_node_3.py new file mode 100644 index 000000000..a6694cde3 --- /dev/null +++ b/smacc2_sm_reference_library/sm_dancebot_ue/servers/service_node_3/service_node_3.py @@ -0,0 +1,47 @@ +#!/usr/bin/env python3 + +# 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. + +# import roslib +# import rospy +import rclpy +from rclpy.node import Node + +import std_srvs +import std_srvs.srv + +if __name__ == "__main__": + + class Service3(Node): + def __init__(self): + super().__init__("Service3") + self.s = self.create_service( + std_srvs.srv.SetBool, "service_node3", self.set_bool_service + ) + + def set_bool_service(self, req, res): + self.get_logger().info("RECEIVING SET BOOL SERVICE REQUEST: value=" + str(req.data)) + + res.message = "OK, value set" + res.success = True + return res + + rclpy.init(args=None) + s = Service3() + + # s = rospy.Service('service_node3', std_srvs.srv.SetBool, set_bool_service) + # rospy.spin() + rclpy.spin(s) + rclpy.shutdown() diff --git a/smacc2_sm_reference_library/sm_dancebot_ue/servers/temperature_sensor_node/src/temperature_sensor_node.cpp b/smacc2_sm_reference_library/sm_dancebot_ue/servers/temperature_sensor_node/src/temperature_sensor_node.cpp new file mode 100644 index 000000000..ce66d640a --- /dev/null +++ b/smacc2_sm_reference_library/sm_dancebot_ue/servers/temperature_sensor_node/src/temperature_sensor_node.cpp @@ -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. + +#include +#include + +int main(int argc, char ** argv) +{ + rclcpp::init(argc, argv); + + auto nh = rclcpp::Node::make_shared("temperature_sensor_node"); + auto pub = nh->create_publisher("/temperature", 1); + + rclcpp::Rate r(10); + + while (rclcpp::ok()) + { + sensor_msgs::msg::Temperature msg; + msg.temperature = sin(nh->now().seconds() / 2.0) * 50; + pub->publish(msg); + + r.sleep(); + rclcpp::spin_some(nh); + } +} diff --git a/smacc2_sm_reference_library/sm_dancebot_ue/src/sm_dancebot_ue/clients/client_behaviors/nav2z_client/cb_active_stop.cpp b/smacc2_sm_reference_library/sm_dancebot_ue/src/sm_dancebot_ue/clients/client_behaviors/nav2z_client/cb_active_stop.cpp new file mode 100644 index 000000000..727fa74d0 --- /dev/null +++ b/smacc2_sm_reference_library/sm_dancebot_ue/src/sm_dancebot_ue/clients/client_behaviors/nav2z_client/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 +#include + +namespace sm_dancebot_ue +{ +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 sm_dancebot_ue diff --git a/smacc2_sm_reference_library/sm_dancebot_ue/src/sm_dancebot_ue/clients/client_behaviors/nav2z_client/cb_position_control_free_space.cpp b/smacc2_sm_reference_library/sm_dancebot_ue/src/sm_dancebot_ue/clients/client_behaviors/nav2z_client/cb_position_control_free_space.cpp new file mode 100644 index 000000000..daf272ecd --- /dev/null +++ b/smacc2_sm_reference_library/sm_dancebot_ue/src/sm_dancebot_ue/clients/client_behaviors/nav2z_client/cb_position_control_free_space.cpp @@ -0,0 +1,195 @@ +// 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 sm_dancebot_ue +{ +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)); + + sm_dancebot_ue::CpUEPose * 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 sm_dancebot_ue diff --git a/smacc2_sm_reference_library/sm_dancebot_ue/src/sm_dancebot_ue/clients/components/cp_ue_pose.cpp b/smacc2_sm_reference_library/sm_dancebot_ue/src/sm_dancebot_ue/clients/components/cp_ue_pose.cpp new file mode 100644 index 000000000..4c25fba0f --- /dev/null +++ b/smacc2_sm_reference_library/sm_dancebot_ue/src/sm_dancebot_ue/clients/components/cp_ue_pose.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. + +/***************************************************************************************************************** + *-2020 + * Authors: Pablo Inigo Blasco, Brett Aldrich + * + ******************************************************************************************************************/ +#include + +#include +#include +#include +#include +#include +#include +#include + +#include +#include +#include + +namespace sm_dancebot_ue +{ +CpUEPose::CpUEPose(std::string topicname) + : CpTopicSubscriber(topicname) +{ +} + +void CpUEPose::onInitialize() +{ + CpTopicSubscriber::onInitialize(); + this->onMessageReceived(&CpUEPose::onPoseMessageReceived, this); +} + +void CpUEPose::onPoseMessageReceived(const ue_msgs::msg::EntityState& msg) +{ + this->entityStateMsg_ = msg;; + + RCLCPP_INFO_STREAM_THROTTLE(getLogger(), *(getNode()->get_clock()), 200, "Received UEPose x: " << msg.pose.position.x << " y: " << msg.pose.position.y << " z: " << msg.pose.position.z); +} + +geometry_msgs::msg::Pose CpUEPose::toPoseMsg() +{ + return this->entityStateMsg_.pose; +} + +} // namespace cl_nav2z diff --git a/smacc2_sm_reference_library/sm_dancebot_ue/src/sm_dancebot_ue/sm_dancebot_ue.cpp b/smacc2_sm_reference_library/sm_dancebot_ue/src/sm_dancebot_ue/sm_dancebot_ue.cpp new file mode 100644 index 000000000..2ab20a261 --- /dev/null +++ b/smacc2_sm_reference_library/sm_dancebot_ue/src/sm_dancebot_ue/sm_dancebot_ue.cpp @@ -0,0 +1,27 @@ +// 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 + +int main(int argc, char ** argv) +{ + rclcpp::init(argc, argv); + smacc2::run(); +} diff --git a/smacc2_sm_reference_library/sm_dancebot_ue/urdf/turtlebot3_waffle.urdf b/smacc2_sm_reference_library/sm_dancebot_ue/urdf/turtlebot3_waffle.urdf new file mode 100644 index 000000000..f4307672c --- /dev/null +++ b/smacc2_sm_reference_library/sm_dancebot_ue/urdf/turtlebot3_waffle.urdf @@ -0,0 +1,293 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/smacc2_sm_reference_library/sm_dancebot_ue/worlds/ridgeback_race.world b/smacc2_sm_reference_library/sm_dancebot_ue/worlds/ridgeback_race.world new file mode 100644 index 000000000..3ff47663c --- /dev/null +++ b/smacc2_sm_reference_library/sm_dancebot_ue/worlds/ridgeback_race.world @@ -0,0 +1,4864 @@ + + + + 1 + 0 0 10 0 -0 0 + 0.8 0.8 0.8 1 + 0.2 0.2 0.2 1 + + 1000 + 0.9 + 0.01 + 0.001 + + -0.5 0.1 -0.9 + + 0 + 0 + 0 + + + + 1 + + + + + 0 0 1 + 100 100 + + + + + + 100 + 50 + + + + + + + + + + + 10 + + + 0 + + + 0 0 1 + 100 100 + + + + + + + + 0 + 0 + + 0 + 0 + 1 + 0 + + + + 0.001 + 1 + 1000 + + + 0.4 0.4 0.4 1 + 0.7 0.7 0.7 1 + 1 + + + EARTH_WGS84 + 0 + 0 + 0 + 0 + + + 1 + 1 + + + + + https://fuel.ignitionrobotics.org/1.0/openrobotics/models/drc practice: orange jersey barrier/2/files/meshes/jersey_barrier.dae + + + + + + + https://fuel.ignitionrobotics.org/1.0/openrobotics/models/drc practice: orange jersey barrier/2/files/meshes/jersey_barrier.dae + + + 10 + + + + + + + + + + + + + + + 0 + 0 + + 0 0 0 0 0 -1.5707 + 0 + 0 + 1 + 0 + + 1 + + + + + https://fuel.ignitionrobotics.org/1.0/openrobotics/models/drc practice: white jersey barrier/2/files/meshes/jersey_barrier.dae + + + + + + + https://fuel.ignitionrobotics.org/1.0/openrobotics/models/drc practice: white jersey barrier/2/files/meshes/jersey_barrier.dae + + + 10 + + + + + + + + + + + + + + + 0 + 0 + + 1.7646 -0.008563 0 0 0 -1.5707 + 0 + 0 + 1 + 0 + + 1 + + + + + https://fuel.ignitionrobotics.org/1.0/openrobotics/models/drc practice: orange jersey barrier/2/files/meshes/jersey_barrier.dae + + + + + + + https://fuel.ignitionrobotics.org/1.0/openrobotics/models/drc practice: orange jersey barrier/2/files/meshes/jersey_barrier.dae + + + 10 + + + + + + + + + + + + + + + 0 + 0 + + 3.52922 -0.016171 0 0 0 -1.5707 + 0 + 0 + 1 + 0 + + 1 + + + + + https://fuel.ignitionrobotics.org/1.0/openrobotics/models/drc practice: white jersey barrier/2/files/meshes/jersey_barrier.dae + + + + + + + https://fuel.ignitionrobotics.org/1.0/openrobotics/models/drc practice: white jersey barrier/2/files/meshes/jersey_barrier.dae + + + 10 + + + + + + + + + + + + + + + 0 + 0 + + 5.13431 0.54837 0 0 0 -0.785398 + 0 + 0 + 1 + 0 + + 1 + + + + + https://fuel.ignitionrobotics.org/1.0/openrobotics/models/drc practice: orange jersey barrier/2/files/meshes/jersey_barrier.dae + + + + + + + https://fuel.ignitionrobotics.org/1.0/openrobotics/models/drc practice: orange jersey barrier/2/files/meshes/jersey_barrier.dae + + + 10 + + + + + + + + + + + + + + + 0 + 0 + + 6.3884 1.79074 0 0 0 -0.785398 + 0 + 0 + 1 + 0 + + 1 + + + + + https://fuel.ignitionrobotics.org/1.0/openrobotics/models/drc practice: white jersey barrier/2/files/meshes/jersey_barrier.dae + + + + + + + https://fuel.ignitionrobotics.org/1.0/openrobotics/models/drc practice: white jersey barrier/2/files/meshes/jersey_barrier.dae + + + 10 + + + + + + + + + + + + + + + 0 + 0 + + 7.6423 3.03258 0 0 0 -0.785398 + 0 + 0 + 1 + 0 + + 1 + + + + + https://fuel.ignitionrobotics.org/1.0/openrobotics/models/drc practice: orange jersey barrier/2/files/meshes/jersey_barrier.dae + + + + + + + https://fuel.ignitionrobotics.org/1.0/openrobotics/models/drc practice: orange jersey barrier/2/files/meshes/jersey_barrier.dae + + + 10 + + + + + + + + + + + + + + + 0 + 0 + + 8.89611 4.27529 0 0 0 -0.785398 + 0 + 0 + 1 + 0 + + 1 + + + + + https://fuel.ignitionrobotics.org/1.0/openrobotics/models/drc practice: white jersey barrier/2/files/meshes/jersey_barrier.dae + + + + + + + https://fuel.ignitionrobotics.org/1.0/openrobotics/models/drc practice: white jersey barrier/2/files/meshes/jersey_barrier.dae + + + 10 + + + + + + + + + + + + + + + 0 + 0 + + 10.1496 5.51614 0 0 0 -0.785398 + 0 + 0 + 1 + 0 + + 1 + + + + + model://blue_cylinder/meshes/cylinder.dae + + + + + + + model://blue_cylinder/meshes/cylinder.dae + + + 10 + + + + + + + + + + + + + + + 0 + 0 + + 11.0288 6.44203 0 0 -0 0 + 0 + 0 + 1 + 0 + + 1 + + + + + model://blue_cylinder/meshes/cylinder.dae + + + + + + + model://blue_cylinder/meshes/cylinder.dae + + + 10 + + + + + + + + + + + + + + + 0 + 0 + + 11.0288 6.44203 0.866 0 -0 0 + 0 + 0 + 1 + 0 + + -1 -6 0 0 -0 0 + + + 1 + + + + + https://fuel.ignitionrobotics.org/1.0/openrobotics/models/drc practice: white jersey barrier/2/files/meshes/jersey_barrier.dae + + + + + + + https://fuel.ignitionrobotics.org/1.0/openrobotics/models/drc practice: white jersey barrier/2/files/meshes/jersey_barrier.dae + + + 10 + + + + + + + + + + + + + + + 0 + 0 + + 0 + 0 + 1 + 0 + + -4 1.55828 0 0 -0 0 + + + 3116 755000000 + 356 954966630 + 1605650970 323926707 + 355029 + + -2.6481 -4.01574 -0.045465 0 0 -0.74752 + 1 1 1 + + 8.54538 -6.87025 -0.045465 0 0 -1.53292 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + + + 3.25439 -7.04543 -0.045465 0 0 -1.53292 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + + + 1.49008 -7.104 -0.045465 0 0 -1.53292 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + + + 9.8196 -6.78892 -0.045465 0 0 -0.74752 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + + + -2.6481 -4.01574 -0.045465 0 0 -2.31822 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + + + 6.78254 -6.92811 -0.045465 0 0 -1.53292 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + + + 5.0182 -6.98712 -0.045465 0 0 -1.53292 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + + + 9.8196 -6.78892 0.820535 0 0 -0.74752 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + + + -0.070844 -6.42684 -0.045465 0 0 -2.31822 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + + + -1.3598 -5.22163 -0.045465 0 0 -2.31822 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + + + + -15.3977 -1.55916 0 0 -0 1.04571 + 1 1 1 + + -15.0829 9.98827 0 0 -0 0.260314 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + + + -13.7448 4.86628 0 0 -0 0.260314 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + + + -13.2984 3.15835 0 0 -0 0.260314 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + + + -15.4433 11.2132 0 0 -0 1.04571 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + + + -15.3977 -1.55916 0 0 0 -0.524988 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + + + -14.6375 8.28162 0 0 -0 0.260314 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + + + -14.1908 6.57377 0 0 -0 0.260314 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + + + -15.4433 11.2132 0.866 0 -0 1.04571 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + + + -13.6146 1.4865 0 0 0 -0.524988 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + + + -14.5057 -0.036576 0 0 0 -0.524988 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + + + + -2.60683 -10.2645 0 0 0 -0.76918 + 1 1 1 + + 8.5222 -13.3608 0 0 0 -1.55458 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + + + 3.22866 -13.4213 0 0 0 -1.55458 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + + + 1.46349 -13.4417 0 0 0 -1.55458 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + + + 9.79789 -13.3071 0 0 0 -0.76918 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + + + -2.60683 -10.2645 0 0 0 -2.33988 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + + + 6.75852 -13.3804 0 0 0 -1.55458 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + + + 4.99332 -13.4012 0 0 0 -1.55458 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + + + 9.79789 -13.3071 0.866 0 0 -0.76918 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + + + -0.082399 -12.7309 0 0 0 -2.33988 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + + + -1.34495 -11.498 0 0 0 -2.33988 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + + + + 10.3347 -5.65314 0 0 -0 1.56441 + 1 1 1 + + 4.88352 4.5315 0 0 -0 0.779009 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + + + 8.58481 0.746571 0 0 -0 0.779009 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + + + 9.81915 -0.515431 0 0 -0 0.779009 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + + + 3.96327 5.4166 0 0 -0 1.56441 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + + + 10.3347 -5.65314 0 0 0 -0.006293 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + + + 6.11634 3.2701 0 0 -0 0.779009 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + + + 7.35101 2.00838 0 0 -0 0.779009 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + + + 3.96327 5.4166 0.866 0 -0 1.56441 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + + + 10.3734 -2.1241 0 0 0 -0.006293 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + + + 10.3545 -3.88863 0 0 0 -0.006293 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + + + + -16.0335 -6.41848 0 0 0 -0.7879 + 1 1 1 + + -4.96438 -9.72253 0 0 0 -1.5733 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + + + -10.2581 -9.68398 0 0 0 -1.5733 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + + + -12.0234 -9.67127 0 0 0 -1.5733 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + + + -3.68791 -9.69271 0 0 0 -0.7879 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + + + -16.0335 -6.41848 0 0 0 -2.3586 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + + + -6.72812 -9.70918 0 0 0 -1.5733 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + + + -8.4934 -9.69692 0 0 0 -1.5733 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + + + -3.68791 -9.69271 0.866 0 0 -0.7879 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + + + -13.5557 -8.93166 0 0 0 -2.3586 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + + + -14.7949 -7.6754 0 0 0 -2.3586 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + + + + -1 16 0 0 -0 0 + 1 1 1 + + 9.2248 10.3902 0 0 0 -2.3562 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + + + 5.49818 14.15 0 0 0 -2.3562 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + + + 4.2559 15.4033 0 0 0 -2.3562 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + + + 10.0288 9.55797 0 0 -0 0 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + + + -1 16 0 0 0 -1.5707 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + + + 7.98256 11.6439 0 0 0 -2.3562 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + + + 6.74031 12.8974 0 0 0 -2.3562 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + + + 10.0288 9.55797 0.866 0 -0 0 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + + + 2.52922 15.9838 0 0 0 -1.5707 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + + + 0.7646 15.9914 0 0 0 -1.5707 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + + + + -14.6681 12.3809 0 0 -0 0.810038 + 1 1 1 + + -3.55516 15.9191 0 0 0 -1.54616 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + + + -8.84779 15.8121 0 0 0 -1.54616 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + + + -10.6121 15.7764 0 0 0 -1.54616 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + + + -2.39802 15.9276 0 0 -0 0.810038 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + + + -14.6681 12.3809 0 0 0 -0.760662 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + + + -5.3197 15.8837 0 0 0 -1.54616 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + + + -7.08412 15.8482 0 0 0 -1.54616 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + + + -2.39802 15.9276 0.866 0 -0 0.810038 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + + + -12.2231 14.926 0 0 0 -0.760662 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + + + -13.4453 13.6531 0 0 0 -0.760662 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + + + + -8.62781 9.94618 0 0 -0 0.2056 + 1 1 1 + + 2.5269 6.54195 0 0 0 -2.1506 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + + + -1.88879 9.46171 0 0 0 -2.1506 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + + + -3.36079 10.4351 0 0 0 -2.1506 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + + + 3.48387 5.89136 0 0 -0 0.205596 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + + + -8.62781 9.94618 0 0 0 -1.3651 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + + + 1.05488 7.51562 0 0 0 -2.1506 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + + + -0.41711 8.4891 0 0 0 -2.1506 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + + + 3.48387 5.89136 0.866 0 -0 0.205596 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + + + -5.16961 10.6508 0 0 0 -1.3651 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + + + -6.89862 10.298 0 0 0 -1.3651 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + + + + -16.7548 -4.64388 0 0 -0 0 + 1 1 1 + + -16.7548 -4.64388 0 0 -0 0 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + + + + -17.1282 -4.13814 0 0 -0 0 + 1 1 1 + + -17.1282 -4.13814 0 0 -0 0 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + + + + 10.3218 -9.21435 0 0 -0 0 + 1 1 1 + + 10.3218 -9.21435 0 0 -0 0 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + + + + 10.3067 -12.7762 0 0 -0 0 + 1 1 1 + + 10.3067 -12.7762 0 0 -0 0 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + + + + 10.3176 -7.4365 0 0 -0 0 + 1 1 1 + + 10.3176 -7.4365 0 0 -0 0 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + + + + 10.3136 -11.0025 0 0 -0 0 + 1 1 1 + + 10.3136 -11.0025 0 0 -0 0 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + + + + -16.2875 -3.11229 0 0 0 -0.509254 + 1 1 1 + + -16.2875 -3.11229 0 0 0 -0.509254 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + + + + -17.3228 -5.14571 0 0 0 -2.31969 + 1 1 1 + + -17.3228 -5.14571 0 0 0 -2.31969 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + + + + 0 0 0 0 -0 0 + 1 1 1 + + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + + + + 0.127058 -0.012625 0.008401 -5.7e-05 0.003353 -0.002954 + 1 1 1 + + 0.127058 -0.012625 0.008401 -5.7e-05 0.003353 -0.002954 + 0.001954 -0.000866 -0.061964 -0.008321 0.009978 -0.001386 + 0.001244 0.001765 0.002452 -0.135997 -0.01007 0.000878 + 0.001244 0.001765 0.002452 0 -0 0 + + + 0.127066 -0.012614 0.008412 -2.8e-05 0.003314 -0.002936 + 0.001162 -0.002502 -0.062977 -0.012557 0.01357 -0.00402 + -0.002239 0.004917 -0.002962 -0.159433 0.012841 0.004083 + -0.002239 0.004917 -0.002962 0 -0 0 + + + 0.127057 -0.012617 0.0084 -6.9e-05 0.003501 -0.00297 + 0.002415 -0.00193 -0.061395 -0.005008 -0.011677 0.001501 + -0.003148 0.006146 -0.003368 -0.028835 -0.016126 -0.002503 + -0.000393 0.000768 -0.000421 0 -0 0 + + + 0.127059 -0.012617 0.008403 -5.8e-05 0.003458 -0.002962 + 0.002127 -0.002056 -0.061757 -0.007075 -0.00588 0.000236 + -0.001696 0.001569 -0.002879 -0.058616 -0.009069 -0.001539 + -0.001696 0.001569 -0.002879 0 -0 0 + + + -0.049765 0.051908 0.005015 -0.045565 -1.47685 -1.24358 + 0.002355 -0.002164 -0.063759 1.02255 1.31025 -0 + -0.00117 0.003147 -0.007182 0 -0 0 + -1e-06 3e-06 -7e-06 0 -0 0 + + + -0.050165 -0.076075 0.004993 1.90095 0.495873 2.6548 + 0.005449 -0.004628 -0.024591 0.926043 1.08964 0 + -0.023688 0.021829 0.247248 1.35553 -0.826371 3.14159 + -2.4e-05 2.2e-05 0.000247 0 -0 0 + + + 0.060947 -0.012566 -19432.4 0 -0 0 + 0 0 -617.145 0 -0 0 + 0 0 -9.8 0 -0 0 + 0 0 -9.8 0 -0 0 + + + 0.106509 -0.01259 0.041208 3.13937 1.12267 3.13728 + 0.002393 -0.00053 -0.06195 0.008576 0.03027 0.007711 + -0.002374 0.001791 -0.006185 -0.054285 -0.072005 -0.002259 + -0.000237 0.000179 -0.000618 0 -0 0 + + + 0.106061 -0.012586 0.040984 3.13916 1.14848 3.13713 + 0.000902 -0.000269 -0.06003 0.004974 0.030614 0.00275 + -0.001844 0.001728 0.003565 -0.052403 -0.057989 -0.04024 + -0.000184 0.000173 0.000357 0 -0 0 + + + + 0 0 10 0 -0 0 + + + + + 1.03059 2.69165 18.1021 -0 1.5578 -3.12116 + orbit + perspective + + + + 1 + + + + + https://fuel.ignitionrobotics.org/1.0/openrobotics/models/drc practice: orange jersey barrier/2/files/meshes/jersey_barrier.dae + + + + + + + https://fuel.ignitionrobotics.org/1.0/openrobotics/models/drc practice: orange jersey barrier/2/files/meshes/jersey_barrier.dae + + + 10 + + + + + + + + + + + + + + + 0 + 0 + + 0 + 0 + 1 + 0 + + 10 -9.57988 0 0 -0 0 + + + 1 + + + + + https://fuel.ignitionrobotics.org/1.0/openrobotics/models/drc practice: white jersey barrier/2/files/meshes/jersey_barrier.dae + + + + + + + https://fuel.ignitionrobotics.org/1.0/openrobotics/models/drc practice: white jersey barrier/2/files/meshes/jersey_barrier.dae + + + 10 + + + + + + + + + + + + + + + 0 + 0 + + 0 + 0 + 1 + 0 + + 10.4084 -11 0 0 -0 0 + + + 1 + + + + + https://fuel.ignitionrobotics.org/1.0/openrobotics/models/drc practice: orange jersey barrier/2/files/meshes/jersey_barrier.dae + + + + + + + https://fuel.ignitionrobotics.org/1.0/openrobotics/models/drc practice: orange jersey barrier/2/files/meshes/jersey_barrier.dae + + + 10 + + + + + + + + + + + + + + + 0 + 0 + + 0 + 0 + 1 + 0 + + 10.4143 -13 0 0 -0 0 + + + 1 + 1 + + + + + https://fuel.ignitionrobotics.org/1.0/openrobotics/models/drc practice: orange jersey barrier/2/files/meshes/jersey_barrier.dae + + + + + + + https://fuel.ignitionrobotics.org/1.0/openrobotics/models/drc practice: orange jersey barrier/2/files/meshes/jersey_barrier.dae + + + 10 + + + + + + + + + + + + + + + 0 + 0 + + 0 0 0 0 0 -1.5707 + 0 + 0 + 1 + 0 + + 1 + + + + + https://fuel.ignitionrobotics.org/1.0/openrobotics/models/drc practice: white jersey barrier/2/files/meshes/jersey_barrier.dae + + + + + + + https://fuel.ignitionrobotics.org/1.0/openrobotics/models/drc practice: white jersey barrier/2/files/meshes/jersey_barrier.dae + + + 10 + + + + + + + + + + + + + + + 0 + 0 + + 1.7646 -0.008563 0 0 0 -1.5707 + 0 + 0 + 1 + 0 + + 1 + + + + + https://fuel.ignitionrobotics.org/1.0/openrobotics/models/drc practice: orange jersey barrier/2/files/meshes/jersey_barrier.dae + + + + + + + https://fuel.ignitionrobotics.org/1.0/openrobotics/models/drc practice: orange jersey barrier/2/files/meshes/jersey_barrier.dae + + + 10 + + + + + + + + + + + + + + + 0 + 0 + + 3.52922 -0.016171 0 0 0 -1.5707 + 0 + 0 + 1 + 0 + + 1 + + + + + https://fuel.ignitionrobotics.org/1.0/openrobotics/models/drc practice: white jersey barrier/2/files/meshes/jersey_barrier.dae + + + + + + + https://fuel.ignitionrobotics.org/1.0/openrobotics/models/drc practice: white jersey barrier/2/files/meshes/jersey_barrier.dae + + + 10 + + + + + + + + + + + + + + + 0 + 0 + + 5.13431 0.54837 0 0 0 -0.785398 + 0 + 0 + 1 + 0 + + 1 + + + + + https://fuel.ignitionrobotics.org/1.0/openrobotics/models/drc practice: orange jersey barrier/2/files/meshes/jersey_barrier.dae + + + + + + + https://fuel.ignitionrobotics.org/1.0/openrobotics/models/drc practice: orange jersey barrier/2/files/meshes/jersey_barrier.dae + + + 10 + + + + + + + + + + + + + + + 0 + 0 + + 6.3884 1.79074 0 0 0 -0.785398 + 0 + 0 + 1 + 0 + + 1 + + + + + https://fuel.ignitionrobotics.org/1.0/openrobotics/models/drc practice: white jersey barrier/2/files/meshes/jersey_barrier.dae + + + + + + + https://fuel.ignitionrobotics.org/1.0/openrobotics/models/drc practice: white jersey barrier/2/files/meshes/jersey_barrier.dae + + + 10 + + + + + + + + + + + + + + + 0 + 0 + + 7.6423 3.03258 0 0 0 -0.785398 + 0 + 0 + 1 + 0 + + 1 + + + + + https://fuel.ignitionrobotics.org/1.0/openrobotics/models/drc practice: orange jersey barrier/2/files/meshes/jersey_barrier.dae + + + + + + + https://fuel.ignitionrobotics.org/1.0/openrobotics/models/drc practice: orange jersey barrier/2/files/meshes/jersey_barrier.dae + + + 10 + + + + + + + + + + + + + + + 0 + 0 + + 8.89611 4.27529 0 0 0 -0.785398 + 0 + 0 + 1 + 0 + + 1 + + + + + https://fuel.ignitionrobotics.org/1.0/openrobotics/models/drc practice: white jersey barrier/2/files/meshes/jersey_barrier.dae + + + + + + + https://fuel.ignitionrobotics.org/1.0/openrobotics/models/drc practice: white jersey barrier/2/files/meshes/jersey_barrier.dae + + + 10 + + + + + + + + + + + + + + + 0 + 0 + + 10.1496 5.51614 0 0 0 -0.785398 + 0 + 0 + 1 + 0 + + 1 + + + + + model://blue_cylinder/meshes/cylinder.dae + + + + + + + model://blue_cylinder/meshes/cylinder.dae + + + 10 + + + + + + + + + + + + + + + 0 + 0 + + 11.0288 6.44203 0 0 -0 0 + 0 + 0 + 1 + 0 + + 1 + + + + + model://blue_cylinder/meshes/cylinder.dae + + + + + + + model://blue_cylinder/meshes/cylinder.dae + + + 10 + + + + + + + + + + + + + + + 0 + 0 + + 11.0288 6.44203 0.866 0 -0 0 + 0 + 0 + 1 + 0 + + -1.5566 -14 0 0 -0 0 + + + 1 + 1 + + + + + https://fuel.ignitionrobotics.org/1.0/openrobotics/models/drc practice: orange jersey barrier/2/files/meshes/jersey_barrier.dae + + + + + + + https://fuel.ignitionrobotics.org/1.0/openrobotics/models/drc practice: orange jersey barrier/2/files/meshes/jersey_barrier.dae + + + 10 + + + + + + + + + + + + + + + 0 + 0 + + 0 0 0 0 0 -1.5707 + 0 + 0 + 1 + 0 + + 1 + + + + + https://fuel.ignitionrobotics.org/1.0/openrobotics/models/drc practice: white jersey barrier/2/files/meshes/jersey_barrier.dae + + + + + + + https://fuel.ignitionrobotics.org/1.0/openrobotics/models/drc practice: white jersey barrier/2/files/meshes/jersey_barrier.dae + + + 10 + + + + + + + + + + + + + + + 0 + 0 + + 1.7646 -0.008563 0 0 0 -1.5707 + 0 + 0 + 1 + 0 + + 1 + + + + + https://fuel.ignitionrobotics.org/1.0/openrobotics/models/drc practice: orange jersey barrier/2/files/meshes/jersey_barrier.dae + + + + + + + https://fuel.ignitionrobotics.org/1.0/openrobotics/models/drc practice: orange jersey barrier/2/files/meshes/jersey_barrier.dae + + + 10 + + + + + + + + + + + + + + + 0 + 0 + + 3.52922 -0.016171 0 0 0 -1.5707 + 0 + 0 + 1 + 0 + + 1 + + + + + https://fuel.ignitionrobotics.org/1.0/openrobotics/models/drc practice: white jersey barrier/2/files/meshes/jersey_barrier.dae + + + + + + + https://fuel.ignitionrobotics.org/1.0/openrobotics/models/drc practice: white jersey barrier/2/files/meshes/jersey_barrier.dae + + + 10 + + + + + + + + + + + + + + + 0 + 0 + + 5.13431 0.54837 0 0 0 -0.785398 + 0 + 0 + 1 + 0 + + 1 + + + + + https://fuel.ignitionrobotics.org/1.0/openrobotics/models/drc practice: orange jersey barrier/2/files/meshes/jersey_barrier.dae + + + + + + + https://fuel.ignitionrobotics.org/1.0/openrobotics/models/drc practice: orange jersey barrier/2/files/meshes/jersey_barrier.dae + + + 10 + + + + + + + + + + + + + + + 0 + 0 + + 6.3884 1.79074 0 0 0 -0.785398 + 0 + 0 + 1 + 0 + + 1 + + + + + https://fuel.ignitionrobotics.org/1.0/openrobotics/models/drc practice: white jersey barrier/2/files/meshes/jersey_barrier.dae + + + + + + + https://fuel.ignitionrobotics.org/1.0/openrobotics/models/drc practice: white jersey barrier/2/files/meshes/jersey_barrier.dae + + + 10 + + + + + + + + + + + + + + + 0 + 0 + + 7.6423 3.03258 0 0 0 -0.785398 + 0 + 0 + 1 + 0 + + 1 + + + + + https://fuel.ignitionrobotics.org/1.0/openrobotics/models/drc practice: orange jersey barrier/2/files/meshes/jersey_barrier.dae + + + + + + + https://fuel.ignitionrobotics.org/1.0/openrobotics/models/drc practice: orange jersey barrier/2/files/meshes/jersey_barrier.dae + + + 10 + + + + + + + + + + + + + + + 0 + 0 + + 8.89611 4.27529 0 0 0 -0.785398 + 0 + 0 + 1 + 0 + + 1 + + + + + https://fuel.ignitionrobotics.org/1.0/openrobotics/models/drc practice: white jersey barrier/2/files/meshes/jersey_barrier.dae + + + + + + + https://fuel.ignitionrobotics.org/1.0/openrobotics/models/drc practice: white jersey barrier/2/files/meshes/jersey_barrier.dae + + + 10 + + + + + + + + + + + + + + + 0 + 0 + + 10.1496 5.51614 0 0 0 -0.785398 + 0 + 0 + 1 + 0 + + 1 + + + + + model://blue_cylinder/meshes/cylinder.dae + + + + + + + model://blue_cylinder/meshes/cylinder.dae + + + 10 + + + + + + + + + + + + + + + 0 + 0 + + 11.0288 6.44203 0 0 -0 0 + 0 + 0 + 1 + 0 + + 1 + + + + + model://blue_cylinder/meshes/cylinder.dae + + + + + + + model://blue_cylinder/meshes/cylinder.dae + + + 10 + + + + + + + + + + + + + + + 0 + 0 + + 11.0288 6.44203 0.866 0 -0 0 + 0 + 0 + 1 + 0 + + -1 -1 0 0 -0 0 + + + 1 + 1 + + + + + https://fuel.ignitionrobotics.org/1.0/openrobotics/models/drc practice: orange jersey barrier/2/files/meshes/jersey_barrier.dae + + + + + + + https://fuel.ignitionrobotics.org/1.0/openrobotics/models/drc practice: orange jersey barrier/2/files/meshes/jersey_barrier.dae + + + 10 + + + + + + + + + + + + + + + 0 + 0 + + 0 0 0 0 0 -1.5707 + 0 + 0 + 1 + 0 + + 1 + + + + + https://fuel.ignitionrobotics.org/1.0/openrobotics/models/drc practice: white jersey barrier/2/files/meshes/jersey_barrier.dae + + + + + + + https://fuel.ignitionrobotics.org/1.0/openrobotics/models/drc practice: white jersey barrier/2/files/meshes/jersey_barrier.dae + + + 10 + + + + + + + + + + + + + + + 0 + 0 + + 1.7646 -0.008563 0 0 0 -1.5707 + 0 + 0 + 1 + 0 + + 1 + + + + + https://fuel.ignitionrobotics.org/1.0/openrobotics/models/drc practice: orange jersey barrier/2/files/meshes/jersey_barrier.dae + + + + + + + https://fuel.ignitionrobotics.org/1.0/openrobotics/models/drc practice: orange jersey barrier/2/files/meshes/jersey_barrier.dae + + + 10 + + + + + + + + + + + + + + + 0 + 0 + + 3.52922 -0.016171 0 0 0 -1.5707 + 0 + 0 + 1 + 0 + + 1 + + + + + https://fuel.ignitionrobotics.org/1.0/openrobotics/models/drc practice: white jersey barrier/2/files/meshes/jersey_barrier.dae + + + + + + + https://fuel.ignitionrobotics.org/1.0/openrobotics/models/drc practice: white jersey barrier/2/files/meshes/jersey_barrier.dae + + + 10 + + + + + + + + + + + + + + + 0 + 0 + + 5.2559 -0.59667 0 0 0 -2.3562 + 0 + 0 + 1 + 0 + + 1 + + + + + https://fuel.ignitionrobotics.org/1.0/openrobotics/models/drc practice: orange jersey barrier/2/files/meshes/jersey_barrier.dae + + + + + + + https://fuel.ignitionrobotics.org/1.0/openrobotics/models/drc practice: orange jersey barrier/2/files/meshes/jersey_barrier.dae + + + 10 + + + + + + + + + + + + + + + 0 + 0 + + 6.49818 -1.85004 0 0 0 -2.3562 + 0 + 0 + 1 + 0 + + 1 + + + + + https://fuel.ignitionrobotics.org/1.0/openrobotics/models/drc practice: white jersey barrier/2/files/meshes/jersey_barrier.dae + + + + + + + https://fuel.ignitionrobotics.org/1.0/openrobotics/models/drc practice: white jersey barrier/2/files/meshes/jersey_barrier.dae + + + 10 + + + + + + + + + + + + + + + 0 + 0 + + 7.74031 -3.10261 0 0 0 -2.3562 + 0 + 0 + 1 + 0 + + 1 + + + + + https://fuel.ignitionrobotics.org/1.0/openrobotics/models/drc practice: orange jersey barrier/2/files/meshes/jersey_barrier.dae + + + + + + + https://fuel.ignitionrobotics.org/1.0/openrobotics/models/drc practice: orange jersey barrier/2/files/meshes/jersey_barrier.dae + + + 10 + + + + + + + + + + + + + + + 0 + 0 + + 8.98256 -4.3561 0 0 0 -2.3562 + 0 + 0 + 1 + 0 + + 1 + + + + + https://fuel.ignitionrobotics.org/1.0/openrobotics/models/drc practice: white jersey barrier/2/files/meshes/jersey_barrier.dae + + + + + + + https://fuel.ignitionrobotics.org/1.0/openrobotics/models/drc practice: white jersey barrier/2/files/meshes/jersey_barrier.dae + + + 10 + + + + + + + + + + + + + + + 0 + 0 + + 10.2248 -5.60978 0 0 0 -2.3562 + 0 + 0 + 1 + 0 + + 1 + + + + + model://blue_cylinder/meshes/cylinder.dae + + + + + + + model://blue_cylinder/meshes/cylinder.dae + + + 10 + + + + + + + + + + + + + + + 0 + 0 + + 11.0288 -6.44203 0 0 -0 0 + 0 + 0 + 1 + 0 + + 1 + + + + + model://blue_cylinder/meshes/cylinder.dae + + + + + + + model://blue_cylinder/meshes/cylinder.dae + + + 10 + + + + + + + + + + + + + + + 0 + 0 + + 11.0288 -6.44203 0.866 0 -0 0 + 0 + 0 + 1 + 0 + + -1 16 0 0 -0 0 + + + 1 + 1 + + + + + https://fuel.ignitionrobotics.org/1.0/openrobotics/models/drc practice: orange jersey barrier/2/files/meshes/jersey_barrier.dae + + + + + + + https://fuel.ignitionrobotics.org/1.0/openrobotics/models/drc practice: orange jersey barrier/2/files/meshes/jersey_barrier.dae + + + 10 + + + + + + + + + + + + + + + 0 + 0 + + 0 0 0 0 0 -1.5707 + 0 + 0 + 1 + 0 + + 1 + + + + + https://fuel.ignitionrobotics.org/1.0/openrobotics/models/drc practice: white jersey barrier/2/files/meshes/jersey_barrier.dae + + + + + + + https://fuel.ignitionrobotics.org/1.0/openrobotics/models/drc practice: white jersey barrier/2/files/meshes/jersey_barrier.dae + + + 10 + + + + + + + + + + + + + + + 0 + 0 + + 1.7646 -0.008563 0 0 0 -1.5707 + 0 + 0 + 1 + 0 + + 1 + + + + + https://fuel.ignitionrobotics.org/1.0/openrobotics/models/drc practice: orange jersey barrier/2/files/meshes/jersey_barrier.dae + + + + + + + https://fuel.ignitionrobotics.org/1.0/openrobotics/models/drc practice: orange jersey barrier/2/files/meshes/jersey_barrier.dae + + + 10 + + + + + + + + + + + + + + + 0 + 0 + + 3.52922 -0.016171 0 0 0 -1.5707 + 0 + 0 + 1 + 0 + + 1 + + + + + https://fuel.ignitionrobotics.org/1.0/openrobotics/models/drc practice: white jersey barrier/2/files/meshes/jersey_barrier.dae + + + + + + + https://fuel.ignitionrobotics.org/1.0/openrobotics/models/drc practice: white jersey barrier/2/files/meshes/jersey_barrier.dae + + + 10 + + + + + + + + + + + + + + + 0 + 0 + + 5.13431 0.54837 0 0 0 -0.785398 + 0 + 0 + 1 + 0 + + 1 + + + + + https://fuel.ignitionrobotics.org/1.0/openrobotics/models/drc practice: orange jersey barrier/2/files/meshes/jersey_barrier.dae + + + + + + + https://fuel.ignitionrobotics.org/1.0/openrobotics/models/drc practice: orange jersey barrier/2/files/meshes/jersey_barrier.dae + + + 10 + + + + + + + + + + + + + + + 0 + 0 + + 6.3884 1.79074 0 0 0 -0.785398 + 0 + 0 + 1 + 0 + + 1 + + + + + https://fuel.ignitionrobotics.org/1.0/openrobotics/models/drc practice: white jersey barrier/2/files/meshes/jersey_barrier.dae + + + + + + + https://fuel.ignitionrobotics.org/1.0/openrobotics/models/drc practice: white jersey barrier/2/files/meshes/jersey_barrier.dae + + + 10 + + + + + + + + + + + + + + + 0 + 0 + + 7.6423 3.03258 0 0 0 -0.785398 + 0 + 0 + 1 + 0 + + 1 + + + + + https://fuel.ignitionrobotics.org/1.0/openrobotics/models/drc practice: orange jersey barrier/2/files/meshes/jersey_barrier.dae + + + + + + + https://fuel.ignitionrobotics.org/1.0/openrobotics/models/drc practice: orange jersey barrier/2/files/meshes/jersey_barrier.dae + + + 10 + + + + + + + + + + + + + + + 0 + 0 + + 8.89611 4.27529 0 0 0 -0.785398 + 0 + 0 + 1 + 0 + + 1 + + + + + https://fuel.ignitionrobotics.org/1.0/openrobotics/models/drc practice: white jersey barrier/2/files/meshes/jersey_barrier.dae + + + + + + + https://fuel.ignitionrobotics.org/1.0/openrobotics/models/drc practice: white jersey barrier/2/files/meshes/jersey_barrier.dae + + + 10 + + + + + + + + + + + + + + + 0 + 0 + + 10.1496 5.51614 0 0 0 -0.785398 + 0 + 0 + 1 + 0 + + 1 + + + + + model://blue_cylinder/meshes/cylinder.dae + + + + + + + model://blue_cylinder/meshes/cylinder.dae + + + 10 + + + + + + + + + + + + + + + 0 + 0 + + 11.0288 6.44203 0 0 -0 0 + 0 + 0 + 1 + 0 + + 1 + + + + + model://blue_cylinder/meshes/cylinder.dae + + + + + + + model://blue_cylinder/meshes/cylinder.dae + + + 10 + + + + + + + + + + + + + + + 0 + 0 + + 11.0288 6.44203 0.866 0 -0 0 + 0 + 0 + 1 + 0 + + -14.5593 -16 0 0 -0 0 + + + 1 + 1 + + + + + https://fuel.ignitionrobotics.org/1.0/openrobotics/models/drc practice: orange jersey barrier/2/files/meshes/jersey_barrier.dae + + + + + + + https://fuel.ignitionrobotics.org/1.0/openrobotics/models/drc practice: orange jersey barrier/2/files/meshes/jersey_barrier.dae + + + 10 + + + + + + + + + + + + + + + 0 + 0 + + 0 0 0 0 0 -1.5707 + 0 + 0 + 1 + 0 + + 1 + + + + + https://fuel.ignitionrobotics.org/1.0/openrobotics/models/drc practice: white jersey barrier/2/files/meshes/jersey_barrier.dae + + + + + + + https://fuel.ignitionrobotics.org/1.0/openrobotics/models/drc practice: white jersey barrier/2/files/meshes/jersey_barrier.dae + + + 10 + + + + + + + + + + + + + + + 0 + 0 + + 1.7646 -0.008563 0 0 0 -1.5707 + 0 + 0 + 1 + 0 + + 1 + + + + + https://fuel.ignitionrobotics.org/1.0/openrobotics/models/drc practice: orange jersey barrier/2/files/meshes/jersey_barrier.dae + + + + + + + https://fuel.ignitionrobotics.org/1.0/openrobotics/models/drc practice: orange jersey barrier/2/files/meshes/jersey_barrier.dae + + + 10 + + + + + + + + + + + + + + + 0 + 0 + + 3.52922 -0.016171 0 0 0 -1.5707 + 0 + 0 + 1 + 0 + + 1 + + + + + https://fuel.ignitionrobotics.org/1.0/openrobotics/models/drc practice: white jersey barrier/2/files/meshes/jersey_barrier.dae + + + + + + + https://fuel.ignitionrobotics.org/1.0/openrobotics/models/drc practice: white jersey barrier/2/files/meshes/jersey_barrier.dae + + + 10 + + + + + + + + + + + + + + + 0 + 0 + + 5.2559 -0.59667 0 0 0 -2.3562 + 0 + 0 + 1 + 0 + + 1 + + + + + https://fuel.ignitionrobotics.org/1.0/openrobotics/models/drc practice: orange jersey barrier/2/files/meshes/jersey_barrier.dae + + + + + + + https://fuel.ignitionrobotics.org/1.0/openrobotics/models/drc practice: orange jersey barrier/2/files/meshes/jersey_barrier.dae + + + 10 + + + + + + + + + + + + + + + 0 + 0 + + 6.49818 -1.85004 0 0 0 -2.3562 + 0 + 0 + 1 + 0 + + 1 + + + + + https://fuel.ignitionrobotics.org/1.0/openrobotics/models/drc practice: white jersey barrier/2/files/meshes/jersey_barrier.dae + + + + + + + https://fuel.ignitionrobotics.org/1.0/openrobotics/models/drc practice: white jersey barrier/2/files/meshes/jersey_barrier.dae + + + 10 + + + + + + + + + + + + + + + 0 + 0 + + 7.74031 -3.10261 0 0 0 -2.3562 + 0 + 0 + 1 + 0 + + 1 + + + + + https://fuel.ignitionrobotics.org/1.0/openrobotics/models/drc practice: orange jersey barrier/2/files/meshes/jersey_barrier.dae + + + + + + + https://fuel.ignitionrobotics.org/1.0/openrobotics/models/drc practice: orange jersey barrier/2/files/meshes/jersey_barrier.dae + + + 10 + + + + + + + + + + + + + + + 0 + 0 + + 8.98256 -4.3561 0 0 0 -2.3562 + 0 + 0 + 1 + 0 + + 1 + + + + + https://fuel.ignitionrobotics.org/1.0/openrobotics/models/drc practice: white jersey barrier/2/files/meshes/jersey_barrier.dae + + + + + + + https://fuel.ignitionrobotics.org/1.0/openrobotics/models/drc practice: white jersey barrier/2/files/meshes/jersey_barrier.dae + + + 10 + + + + + + + + + + + + + + + 0 + 0 + + 10.2248 -5.60978 0 0 0 -2.3562 + 0 + 0 + 1 + 0 + + 1 + + + + + model://blue_cylinder/meshes/cylinder.dae + + + + + + + model://blue_cylinder/meshes/cylinder.dae + + + 10 + + + + + + + + + + + + + + + 0 + 0 + + 11.0288 -6.44203 0 0 -0 0 + 0 + 0 + 1 + 0 + + 1 + + + + + model://blue_cylinder/meshes/cylinder.dae + + + + + + + model://blue_cylinder/meshes/cylinder.dae + + + 10 + + + + + + + + + + + + + + + 0 + 0 + + 11.0288 -6.44203 0.866 0 -0 0 + 0 + 0 + 1 + 0 + + 25 -15 0 0 -0 0 + + + 1 + 1 + + + + + https://fuel.ignitionrobotics.org/1.0/openrobotics/models/drc practice: orange jersey barrier/2/files/meshes/jersey_barrier.dae + + + + + + + https://fuel.ignitionrobotics.org/1.0/openrobotics/models/drc practice: orange jersey barrier/2/files/meshes/jersey_barrier.dae + + + 10 + + + + + + + + + + + + + + + 0 + 0 + + 0 0 0 0 0 -1.5707 + 0 + 0 + 1 + 0 + + 1 + + + + + https://fuel.ignitionrobotics.org/1.0/openrobotics/models/drc practice: white jersey barrier/2/files/meshes/jersey_barrier.dae + + + + + + + https://fuel.ignitionrobotics.org/1.0/openrobotics/models/drc practice: white jersey barrier/2/files/meshes/jersey_barrier.dae + + + 10 + + + + + + + + + + + + + + + 0 + 0 + + 1.7646 -0.008563 0 0 0 -1.5707 + 0 + 0 + 1 + 0 + + 1 + + + + + https://fuel.ignitionrobotics.org/1.0/openrobotics/models/drc practice: orange jersey barrier/2/files/meshes/jersey_barrier.dae + + + + + + + https://fuel.ignitionrobotics.org/1.0/openrobotics/models/drc practice: orange jersey barrier/2/files/meshes/jersey_barrier.dae + + + 10 + + + + + + + + + + + + + + + 0 + 0 + + 3.52922 -0.016171 0 0 0 -1.5707 + 0 + 0 + 1 + 0 + + 1 + + + + + https://fuel.ignitionrobotics.org/1.0/openrobotics/models/drc practice: white jersey barrier/2/files/meshes/jersey_barrier.dae + + + + + + + https://fuel.ignitionrobotics.org/1.0/openrobotics/models/drc practice: white jersey barrier/2/files/meshes/jersey_barrier.dae + + + 10 + + + + + + + + + + + + + + + 0 + 0 + + 5.13431 0.54837 0 0 0 -0.785398 + 0 + 0 + 1 + 0 + + 1 + + + + + https://fuel.ignitionrobotics.org/1.0/openrobotics/models/drc practice: orange jersey barrier/2/files/meshes/jersey_barrier.dae + + + + + + + https://fuel.ignitionrobotics.org/1.0/openrobotics/models/drc practice: orange jersey barrier/2/files/meshes/jersey_barrier.dae + + + 10 + + + + + + + + + + + + + + + 0 + 0 + + 6.3884 1.79074 0 0 0 -0.785398 + 0 + 0 + 1 + 0 + + 1 + + + + + https://fuel.ignitionrobotics.org/1.0/openrobotics/models/drc practice: white jersey barrier/2/files/meshes/jersey_barrier.dae + + + + + + + https://fuel.ignitionrobotics.org/1.0/openrobotics/models/drc practice: white jersey barrier/2/files/meshes/jersey_barrier.dae + + + 10 + + + + + + + + + + + + + + + 0 + 0 + + 7.6423 3.03258 0 0 0 -0.785398 + 0 + 0 + 1 + 0 + + 1 + + + + + https://fuel.ignitionrobotics.org/1.0/openrobotics/models/drc practice: orange jersey barrier/2/files/meshes/jersey_barrier.dae + + + + + + + https://fuel.ignitionrobotics.org/1.0/openrobotics/models/drc practice: orange jersey barrier/2/files/meshes/jersey_barrier.dae + + + 10 + + + + + + + + + + + + + + + 0 + 0 + + 8.89611 4.27529 0 0 0 -0.785398 + 0 + 0 + 1 + 0 + + 1 + + + + + https://fuel.ignitionrobotics.org/1.0/openrobotics/models/drc practice: white jersey barrier/2/files/meshes/jersey_barrier.dae + + + + + + + https://fuel.ignitionrobotics.org/1.0/openrobotics/models/drc practice: white jersey barrier/2/files/meshes/jersey_barrier.dae + + + 10 + + + + + + + + + + + + + + + 0 + 0 + + 10.1496 5.51614 0 0 0 -0.785398 + 0 + 0 + 1 + 0 + + 1 + + + + + model://blue_cylinder/meshes/cylinder.dae + + + + + + + model://blue_cylinder/meshes/cylinder.dae + + + 10 + + + + + + + + + + + + + + + 0 + 0 + + 11.0288 6.44203 0 0 -0 0 + 0 + 0 + 1 + 0 + + 1 + + + + + model://blue_cylinder/meshes/cylinder.dae + + + + + + + model://blue_cylinder/meshes/cylinder.dae + + + 10 + + + + + + + + + + + + + + + 0 + 0 + + 11.0288 6.44203 0.866 0 -0 0 + 0 + 0 + 1 + 0 + + -25 -4 0 0 -0 0 + + + 1 + + + + + https://fuel.ignitionrobotics.org/1.0/openrobotics/models/drc practice: white jersey barrier/2/files/meshes/jersey_barrier.dae + + + + + + + https://fuel.ignitionrobotics.org/1.0/openrobotics/models/drc practice: white jersey barrier/2/files/meshes/jersey_barrier.dae + + + 10 + + + + + + + + + + + + + + + 0 + 0 + + 0 + 0 + 1 + 0 + + -16.4458 -4 0 0 -0 0 + + + 1 + + + + + https://fuel.ignitionrobotics.org/1.0/openrobotics/models/drc practice: white jersey barrier/2/files/meshes/jersey_barrier.dae + + + + + + + https://fuel.ignitionrobotics.org/1.0/openrobotics/models/drc practice: white jersey barrier/2/files/meshes/jersey_barrier.dae + + + 10 + + + + + + + + + + + + + + + 0 + 0 + + 0 + 0 + 1 + 0 + + -17.5466 -5 0 0 -0 0 + + + 1 + + + + + model://blue_cylinder/meshes/cylinder.dae + + + + + + + model://blue_cylinder/meshes/cylinder.dae + + + 10 + + + + + + + + + + + + + + + 0 + 0 + + 0 + 0 + 1 + 0 + + -17 -4.51853 0 0 -0 0 + + + 1 + + + + + model://blue_cylinder/meshes/cylinder.dae + + + + + + + model://blue_cylinder/meshes/cylinder.dae + + + 10 + + + + + + + + + + + + + + + 0 + 0 + + 0 + 0 + 1 + 0 + + -17.425 -4 0 0 -0 0 + + + 1 + 1 + + + + + https://fuel.ignitionrobotics.org/1.0/openrobotics/models/drc practice: orange jersey barrier/2/files/meshes/jersey_barrier.dae + + + + + + + https://fuel.ignitionrobotics.org/1.0/openrobotics/models/drc practice: orange jersey barrier/2/files/meshes/jersey_barrier.dae + + + 10 + + + + + + + + + + + + + + + 0 + 0 + + 0 0 0 0 0 -1.5707 + 0 + 0 + 1 + 0 + + 1 + + + + + https://fuel.ignitionrobotics.org/1.0/openrobotics/models/drc practice: white jersey barrier/2/files/meshes/jersey_barrier.dae + + + + + + + https://fuel.ignitionrobotics.org/1.0/openrobotics/models/drc practice: white jersey barrier/2/files/meshes/jersey_barrier.dae + + + 10 + + + + + + + + + + + + + + + 0 + 0 + + 1.7646 -0.008563 0 0 0 -1.5707 + 0 + 0 + 1 + 0 + + 1 + + + + + https://fuel.ignitionrobotics.org/1.0/openrobotics/models/drc practice: orange jersey barrier/2/files/meshes/jersey_barrier.dae + + + + + + + https://fuel.ignitionrobotics.org/1.0/openrobotics/models/drc practice: orange jersey barrier/2/files/meshes/jersey_barrier.dae + + + 10 + + + + + + + + + + + + + + + 0 + 0 + + 3.52922 -0.016171 0 0 0 -1.5707 + 0 + 0 + 1 + 0 + + 1 + + + + + https://fuel.ignitionrobotics.org/1.0/openrobotics/models/drc practice: white jersey barrier/2/files/meshes/jersey_barrier.dae + + + + + + + https://fuel.ignitionrobotics.org/1.0/openrobotics/models/drc practice: white jersey barrier/2/files/meshes/jersey_barrier.dae + + + 10 + + + + + + + + + + + + + + + 0 + 0 + + 5.2559 -0.59667 0 0 0 -2.3562 + 0 + 0 + 1 + 0 + + 1 + + + + + https://fuel.ignitionrobotics.org/1.0/openrobotics/models/drc practice: orange jersey barrier/2/files/meshes/jersey_barrier.dae + + + + + + + https://fuel.ignitionrobotics.org/1.0/openrobotics/models/drc practice: orange jersey barrier/2/files/meshes/jersey_barrier.dae + + + 10 + + + + + + + + + + + + + + + 0 + 0 + + 6.49818 -1.85004 0 0 0 -2.3562 + 0 + 0 + 1 + 0 + + 1 + + + + + https://fuel.ignitionrobotics.org/1.0/openrobotics/models/drc practice: white jersey barrier/2/files/meshes/jersey_barrier.dae + + + + + + + https://fuel.ignitionrobotics.org/1.0/openrobotics/models/drc practice: white jersey barrier/2/files/meshes/jersey_barrier.dae + + + 10 + + + + + + + + + + + + + + + 0 + 0 + + 7.74031 -3.10261 0 0 0 -2.3562 + 0 + 0 + 1 + 0 + + 1 + + + + + https://fuel.ignitionrobotics.org/1.0/openrobotics/models/drc practice: orange jersey barrier/2/files/meshes/jersey_barrier.dae + + + + + + + https://fuel.ignitionrobotics.org/1.0/openrobotics/models/drc practice: orange jersey barrier/2/files/meshes/jersey_barrier.dae + + + 10 + + + + + + + + + + + + + + + 0 + 0 + + 8.98256 -4.3561 0 0 0 -2.3562 + 0 + 0 + 1 + 0 + + 1 + + + + + https://fuel.ignitionrobotics.org/1.0/openrobotics/models/drc practice: white jersey barrier/2/files/meshes/jersey_barrier.dae + + + + + + + https://fuel.ignitionrobotics.org/1.0/openrobotics/models/drc practice: white jersey barrier/2/files/meshes/jersey_barrier.dae + + + 10 + + + + + + + + + + + + + + + 0 + 0 + + 10.2248 -5.60978 0 0 0 -2.3562 + 0 + 0 + 1 + 0 + + 1 + + + + + model://blue_cylinder/meshes/cylinder.dae + + + + + + + model://blue_cylinder/meshes/cylinder.dae + + + 10 + + + + + + + + + + + + + + + 0 + 0 + + 11.0288 -6.44203 0 0 -0 0 + 0 + 0 + 1 + 0 + + 1 + + + + + model://blue_cylinder/meshes/cylinder.dae + + + + + + + model://blue_cylinder/meshes/cylinder.dae + + + 10 + + + + + + + + + + + + + + + 0 + 0 + + 11.0288 -6.44203 0.866 0 -0 0 + 0 + 0 + 1 + 0 + + -9 10 0 0 -0 0 + + 0 0 -9.8 + 6e-06 2.3e-05 -4.2e-05 + + + + -2 -0.5 0.01 0 -0 0 + + 0 + + 0 0 0 0 -0 0 + + 1 + 0 + 0 + 1 + 0 + 1 + + 1 + + 0 + 0 + + + + -0.064 0 0.048 0 -0 0 + + 0.001 + 0 + 0 + 0.001 + 0 + 0.001 + + 1 + + + -0.064 0 0.048 0 -0 0 + + + 0.265 0.265 0.089 + + + + + + + + + + + + + + + 10 + + + -0.064 0 0 0 -0 0 + + + model://turtlebot3_waffle/meshes/waffle_base.dae + 0.001 0.001 0.001 + + + + 0 + 0 + 0 + + + + 1 + 200 + + + + + 0 + 0.0002 + + + + + 0 + 0.0002 + + + + + 0 + 0.0002 + + + + + + + 0 + 0.017 + + + + + 0 + 0.017 + + + + + 0 + 0.017 + + + + + + + ~/out:=imu + + + + 0 + + 0 0 0 0 -0 0 + + 1 + 0 + 0 + 1 + 0 + 1 + + 1 + + 0 + 0 + + + + -0.052 0 0.111 0 -0 0 + + 0.001 + 0 + 0 + 0.001 + 0 + 0.001 + + 0.125 + + + -0.052 0 0.111 0 -0 0 + + + 0.0508 + 0.055 + + + + + + + + + + + + + + + 10 + + + -0.064 0 0.121 0 -0 0 + + + model://turtlebot3_waffle/meshes/lds.dae + 0.001 0.001 0.001 + + + + + 1 + 1 + -0.064 0 0.121 0 -0 0 + 5 + + + + 360 + 1 + 0 + 6.28 + + + 1 + 0 + 0 + + + + 0.32 + 20.5 + 0.015 + + + gaussian + 0 + 0.01 + + + + + ~/out:=scan + + sensor_msgs/LaserScan + base_scan + + + 0 + 0 + 0 + + + + 0 0.144 0.023 -1.57 0 0 + + 0.001 + 0 + 0 + 0.001 + 0 + 0.001 + + 0.1 + + + 0 0.144 0.023 -1.57 0 0 + + + 0.033 + 0.018 + + + + + + 100000 + 100000 + 0 0 0 + 0 + 0 + + + + + + + + 0 + 0.2 + 100000 + 1 + 0.01 + 0.001 + + + + + 10 + + + 0 0.144 0.023 0 -0 0 + + + model://turtlebot3_waffle/meshes/left_tire.dae + 0.001 0.001 0.001 + + + + 0 + 0 + 0 + + + + 0 -0.144 0.023 -1.57 0 0 + + 0.001 + 0 + 0 + 0.001 + 0 + 0.001 + + 0.1 + + + 0 -0.144 0.023 -1.57 0 0 + + + 0.033 + 0.018 + + + + + + 100000 + 100000 + 0 0 0 + 0 + 0 + + + + + + + + 0 + 0.2 + 100000 + 1 + 0.01 + 0.001 + + + + + 10 + + + 0 -0.144 0.023 0 -0 0 + + + model://turtlebot3_waffle/meshes/right_tire.dae + 0.001 0.001 0.001 + + + + 0 + 0 + 0 + + + -0.177 -0.064 -0.004 0 -0 0 + + 0.001 + + 1e-05 + 0 + 0 + 1e-05 + 0 + 1e-05 + + 0 0 0 0 -0 0 + + + + + 0.005 + + + + + + 0 + 0.2 + 100000 + 1 + 0.01 + 0.001 + + + + + + + + + + + 10 + + 0 + 0 + 0 + + + -0.177 0.064 -0.004 0 -0 0 + + 0.001 + + 1e-05 + 0 + 0 + 1e-05 + 0 + 1e-05 + + 0 0 0 0 -0 0 + + + + + 0.005 + + + + + + 0 + 0.2 + 100000 + 1 + 0.01 + 0.001 + + + + + + + + + + + 10 + + 0 + 0 + 0 + + + + 0.069 -0.047 0.107 0 -0 0 + + 0.001 + 0 + 0 + 0.001 + 0 + 0.001 + + 0.035 + + + 0 0.047 -0.005 0 -0 0 + + + 0.008 0.13 0.022 + + + + + + + + + + + + + + + 10 + + 0.069 -0.047 0.107 0 -0 0 + + 1 + 5 + 0.064 -0.047 0.107 0 -0 0 + + 1.047 + + 320 + 240 + + + 0.1 + 100 + + + + + intel_realsense_r200_depth + camera_depth_frame + 0.07 + 0.001 + 5.0 + + + + 0 0.047 -0.005 0 -0 0 + + + 0.008 0.13 0.022 + + + + + + + + + + + + + + + 10 + + 0.069 -0.047 0.107 0 -0 0 + 0 + 0 + 0 + + + base_footprint + base_link + 0 0 0.01 0 -0 0 + + + base_link + wheel_left_link + 0 0.144 0.023 -1.57 0 0 + + 0 0 1 + + -1e+16 + 1e+16 + + + 0 + 0 + + + + + base_link + wheel_right_link + 0 -0.144 0.023 -1.57 0 0 + + 0 0 1 + + -1e+16 + 1e+16 + + + 0 + 0 + + + + + base_link + caster_back_right_link + + + base_link + caster_back_left_link + + + base_link + base_scan + -0.064 0 0.121 0 -0 0 + + 0 0 1 + + -1e+16 + 1e+16 + + + + + base_link + camera_link + 0.064 -0.065 0.094 0 -0 0 + + 0 0 1 + + -1e+16 + 1e+16 + + + + + + + 30 + wheel_left_joint + wheel_right_joint + 0.287 + 0.066 + 20 + 1.0 + cmd_vel + 1 + 1 + 0 + odom + odom + base_footprint + + + + ~/out:=joint_states + + 30 + wheel_left_joint + wheel_right_joint + + + + diff --git a/smacc2_sm_reference_library/sm_dancebot_ue/worlds/ridgeback_race_empty.world b/smacc2_sm_reference_library/sm_dancebot_ue/worlds/ridgeback_race_empty.world new file mode 100644 index 000000000..3ebb28ab2 --- /dev/null +++ b/smacc2_sm_reference_library/sm_dancebot_ue/worlds/ridgeback_race_empty.world @@ -0,0 +1,4193 @@ + + + + 1 + 0 0 10 0 -0 0 + 0.8 0.8 0.8 1 + 0.2 0.2 0.2 1 + + 1000 + 0.9 + 0.01 + 0.001 + + -0.5 0.1 -0.9 + + 0 + 0 + 0 + + + + 1 + + + + + 0 0 1 + 100 100 + + + + + + 100 + 50 + + + + + + + + + + + 10 + + + 0 + + + 0 0 1 + 100 100 + + + + + + + + 0 + 0 + + 0 + 0 + 1 + 0 + + + + 0.001 + 1 + 1000 + + + 0.4 0.4 0.4 1 + 0.7 0.7 0.7 1 + 1 + + + EARTH_WGS84 + 0 + 0 + 0 + 0 + + + 1 + 1 + + + + + https://fuel.ignitionrobotics.org/1.0/openrobotics/models/drc practice: orange jersey barrier/2/files/meshes/jersey_barrier.dae + + + + + + + https://fuel.ignitionrobotics.org/1.0/openrobotics/models/drc practice: orange jersey barrier/2/files/meshes/jersey_barrier.dae + + + 10 + + + + + + + + + + + + + + + 0 + 0 + + 0 0 0 0 0 -1.5707 + 0 + 0 + 1 + 0 + + 1 + + + + + https://fuel.ignitionrobotics.org/1.0/openrobotics/models/drc practice: white jersey barrier/2/files/meshes/jersey_barrier.dae + + + + + + + https://fuel.ignitionrobotics.org/1.0/openrobotics/models/drc practice: white jersey barrier/2/files/meshes/jersey_barrier.dae + + + 10 + + + + + + + + + + + + + + + 0 + 0 + + 1.7646 -0.008563 0 0 0 -1.5707 + 0 + 0 + 1 + 0 + + 1 + + + + + https://fuel.ignitionrobotics.org/1.0/openrobotics/models/drc practice: orange jersey barrier/2/files/meshes/jersey_barrier.dae + + + + + + + https://fuel.ignitionrobotics.org/1.0/openrobotics/models/drc practice: orange jersey barrier/2/files/meshes/jersey_barrier.dae + + + 10 + + + + + + + + + + + + + + + 0 + 0 + + 3.52922 -0.016171 0 0 0 -1.5707 + 0 + 0 + 1 + 0 + + 1 + + + + + https://fuel.ignitionrobotics.org/1.0/openrobotics/models/drc practice: white jersey barrier/2/files/meshes/jersey_barrier.dae + + + + + + + https://fuel.ignitionrobotics.org/1.0/openrobotics/models/drc practice: white jersey barrier/2/files/meshes/jersey_barrier.dae + + + 10 + + + + + + + + + + + + + + + 0 + 0 + + 5.13431 0.54837 0 0 0 -0.785398 + 0 + 0 + 1 + 0 + + 1 + + + + + https://fuel.ignitionrobotics.org/1.0/openrobotics/models/drc practice: orange jersey barrier/2/files/meshes/jersey_barrier.dae + + + + + + + https://fuel.ignitionrobotics.org/1.0/openrobotics/models/drc practice: orange jersey barrier/2/files/meshes/jersey_barrier.dae + + + 10 + + + + + + + + + + + + + + + 0 + 0 + + 6.3884 1.79074 0 0 0 -0.785398 + 0 + 0 + 1 + 0 + + 1 + + + + + https://fuel.ignitionrobotics.org/1.0/openrobotics/models/drc practice: white jersey barrier/2/files/meshes/jersey_barrier.dae + + + + + + + https://fuel.ignitionrobotics.org/1.0/openrobotics/models/drc practice: white jersey barrier/2/files/meshes/jersey_barrier.dae + + + 10 + + + + + + + + + + + + + + + 0 + 0 + + 7.6423 3.03258 0 0 0 -0.785398 + 0 + 0 + 1 + 0 + + 1 + + + + + https://fuel.ignitionrobotics.org/1.0/openrobotics/models/drc practice: orange jersey barrier/2/files/meshes/jersey_barrier.dae + + + + + + + https://fuel.ignitionrobotics.org/1.0/openrobotics/models/drc practice: orange jersey barrier/2/files/meshes/jersey_barrier.dae + + + 10 + + + + + + + + + + + + + + + 0 + 0 + + 8.89611 4.27529 0 0 0 -0.785398 + 0 + 0 + 1 + 0 + + 1 + + + + + https://fuel.ignitionrobotics.org/1.0/openrobotics/models/drc practice: white jersey barrier/2/files/meshes/jersey_barrier.dae + + + + + + + https://fuel.ignitionrobotics.org/1.0/openrobotics/models/drc practice: white jersey barrier/2/files/meshes/jersey_barrier.dae + + + 10 + + + + + + + + + + + + + + + 0 + 0 + + 10.1496 5.51614 0 0 0 -0.785398 + 0 + 0 + 1 + 0 + + 1 + + + + + model://blue_cylinder/meshes/cylinder.dae + + + + + + + model://blue_cylinder/meshes/cylinder.dae + + + 10 + + + + + + + + + + + + + + + 0 + 0 + + 11.0288 6.44203 0 0 -0 0 + 0 + 0 + 1 + 0 + + 1 + + + + + model://blue_cylinder/meshes/cylinder.dae + + + + + + + model://blue_cylinder/meshes/cylinder.dae + + + 10 + + + + + + + + + + + + + + + 0 + 0 + + 11.0288 6.44203 0.866 0 -0 0 + 0 + 0 + 1 + 0 + + -1 -6 0 0 -0 0 + + + 1 + + + + + https://fuel.ignitionrobotics.org/1.0/openrobotics/models/drc practice: white jersey barrier/2/files/meshes/jersey_barrier.dae + + + + + + + https://fuel.ignitionrobotics.org/1.0/openrobotics/models/drc practice: white jersey barrier/2/files/meshes/jersey_barrier.dae + + + 10 + + + + + + + + + + + + + + + 0 + 0 + + 0 + 0 + 1 + 0 + + -4 1.55828 0 0 -0 0 + + + 3116 755000000 + 356 954966630 + 1605650970 323926707 + 355029 + + -2.6481 -4.01574 -0.045465 0 0 -0.74752 + 1 1 1 + + 8.54538 -6.87025 -0.045465 0 0 -1.53292 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + + + 3.25439 -7.04543 -0.045465 0 0 -1.53292 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + + + 1.49008 -7.104 -0.045465 0 0 -1.53292 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + + + 9.8196 -6.78892 -0.045465 0 0 -0.74752 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + + + -2.6481 -4.01574 -0.045465 0 0 -2.31822 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + + + 6.78254 -6.92811 -0.045465 0 0 -1.53292 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + + + 5.0182 -6.98712 -0.045465 0 0 -1.53292 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + + + 9.8196 -6.78892 0.820535 0 0 -0.74752 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + + + -0.070844 -6.42684 -0.045465 0 0 -2.31822 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + + + -1.3598 -5.22163 -0.045465 0 0 -2.31822 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + + + + -15.3977 -1.55916 0 0 -0 1.04571 + 1 1 1 + + -15.0829 9.98827 0 0 -0 0.260314 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + + + -13.7448 4.86628 0 0 -0 0.260314 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + + + -13.2984 3.15835 0 0 -0 0.260314 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + + + -15.4433 11.2132 0 0 -0 1.04571 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + + + -15.3977 -1.55916 0 0 0 -0.524988 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + + + -14.6375 8.28162 0 0 -0 0.260314 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + + + -14.1908 6.57377 0 0 -0 0.260314 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + + + -15.4433 11.2132 0.866 0 -0 1.04571 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + + + -13.6146 1.4865 0 0 0 -0.524988 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + + + -14.5057 -0.036576 0 0 0 -0.524988 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + + + + -2.60683 -10.2645 0 0 0 -0.76918 + 1 1 1 + + 8.5222 -13.3608 0 0 0 -1.55458 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + + + 3.22866 -13.4213 0 0 0 -1.55458 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + + + 1.46349 -13.4417 0 0 0 -1.55458 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + + + 9.79789 -13.3071 0 0 0 -0.76918 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + + + -2.60683 -10.2645 0 0 0 -2.33988 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + + + 6.75852 -13.3804 0 0 0 -1.55458 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + + + 4.99332 -13.4012 0 0 0 -1.55458 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + + + 9.79789 -13.3071 0.866 0 0 -0.76918 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + + + -0.082399 -12.7309 0 0 0 -2.33988 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + + + -1.34495 -11.498 0 0 0 -2.33988 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + + + + 10.3347 -5.65314 0 0 -0 1.56441 + 1 1 1 + + 4.88352 4.5315 0 0 -0 0.779009 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + + + 8.58481 0.746571 0 0 -0 0.779009 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + + + 9.81915 -0.515431 0 0 -0 0.779009 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + + + 3.96327 5.4166 0 0 -0 1.56441 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + + + 10.3347 -5.65314 0 0 0 -0.006293 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + + + 6.11634 3.2701 0 0 -0 0.779009 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + + + 7.35101 2.00838 0 0 -0 0.779009 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + + + 3.96327 5.4166 0.866 0 -0 1.56441 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + + + 10.3734 -2.1241 0 0 0 -0.006293 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + + + 10.3545 -3.88863 0 0 0 -0.006293 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + + + + -16.0335 -6.41848 0 0 0 -0.7879 + 1 1 1 + + -4.96438 -9.72253 0 0 0 -1.5733 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + + + -10.2581 -9.68398 0 0 0 -1.5733 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + + + -12.0234 -9.67127 0 0 0 -1.5733 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + + + -3.68791 -9.69271 0 0 0 -0.7879 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + + + -16.0335 -6.41848 0 0 0 -2.3586 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + + + -6.72812 -9.70918 0 0 0 -1.5733 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + + + -8.4934 -9.69692 0 0 0 -1.5733 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + + + -3.68791 -9.69271 0.866 0 0 -0.7879 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + + + -13.5557 -8.93166 0 0 0 -2.3586 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + + + -14.7949 -7.6754 0 0 0 -2.3586 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + + + + -1 16 0 0 -0 0 + 1 1 1 + + 9.2248 10.3902 0 0 0 -2.3562 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + + + 5.49818 14.15 0 0 0 -2.3562 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + + + 4.2559 15.4033 0 0 0 -2.3562 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + + + 10.0288 9.55797 0 0 -0 0 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + + + -1 16 0 0 0 -1.5707 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + + + 7.98256 11.6439 0 0 0 -2.3562 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + + + 6.74031 12.8974 0 0 0 -2.3562 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + + + 10.0288 9.55797 0.866 0 -0 0 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + + + 2.52922 15.9838 0 0 0 -1.5707 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + + + 0.7646 15.9914 0 0 0 -1.5707 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + + + + -14.6681 12.3809 0 0 -0 0.810038 + 1 1 1 + + -3.55516 15.9191 0 0 0 -1.54616 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + + + -8.84779 15.8121 0 0 0 -1.54616 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + + + -10.6121 15.7764 0 0 0 -1.54616 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + + + -2.39802 15.9276 0 0 -0 0.810038 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + + + -14.6681 12.3809 0 0 0 -0.760662 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + + + -5.3197 15.8837 0 0 0 -1.54616 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + + + -7.08412 15.8482 0 0 0 -1.54616 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + + + -2.39802 15.9276 0.866 0 -0 0.810038 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + + + -12.2231 14.926 0 0 0 -0.760662 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + + + -13.4453 13.6531 0 0 0 -0.760662 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + + + + -8.62781 9.94618 0 0 -0 0.2056 + 1 1 1 + + 2.5269 6.54195 0 0 0 -2.1506 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + + + -1.88879 9.46171 0 0 0 -2.1506 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + + + -3.36079 10.4351 0 0 0 -2.1506 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + + + 3.48387 5.89136 0 0 -0 0.205596 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + + + -8.62781 9.94618 0 0 0 -1.3651 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + + + 1.05488 7.51562 0 0 0 -2.1506 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + + + -0.41711 8.4891 0 0 0 -2.1506 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + + + 3.48387 5.89136 0.866 0 -0 0.205596 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + + + -5.16961 10.6508 0 0 0 -1.3651 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + + + -6.89862 10.298 0 0 0 -1.3651 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + + + + -16.7548 -4.64388 0 0 -0 0 + 1 1 1 + + -16.7548 -4.64388 0 0 -0 0 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + + + + -17.1282 -4.13814 0 0 -0 0 + 1 1 1 + + -17.1282 -4.13814 0 0 -0 0 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + + + + 10.3218 -9.21435 0 0 -0 0 + 1 1 1 + + 10.3218 -9.21435 0 0 -0 0 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + + + + 10.3067 -12.7762 0 0 -0 0 + 1 1 1 + + 10.3067 -12.7762 0 0 -0 0 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + + + + 10.3176 -7.4365 0 0 -0 0 + 1 1 1 + + 10.3176 -7.4365 0 0 -0 0 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + + + + 10.3136 -11.0025 0 0 -0 0 + 1 1 1 + + 10.3136 -11.0025 0 0 -0 0 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + + + + -16.2875 -3.11229 0 0 0 -0.509254 + 1 1 1 + + -16.2875 -3.11229 0 0 0 -0.509254 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + + + + -17.3228 -5.14571 0 0 0 -2.31969 + 1 1 1 + + -17.3228 -5.14571 0 0 0 -2.31969 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + + + + 0 0 0 0 -0 0 + 1 1 1 + + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + + + + 0 0 10 0 -0 0 + + + + + 1.03059 2.69165 18.1021 -0 1.5578 -3.12116 + orbit + perspective + + + + 1 + + + + + https://fuel.ignitionrobotics.org/1.0/openrobotics/models/drc practice: orange jersey barrier/2/files/meshes/jersey_barrier.dae + + + + + + + https://fuel.ignitionrobotics.org/1.0/openrobotics/models/drc practice: orange jersey barrier/2/files/meshes/jersey_barrier.dae + + + 10 + + + + + + + + + + + + + + + 0 + 0 + + 0 + 0 + 1 + 0 + + 10 -9.57988 0 0 -0 0 + + + 1 + + + + + https://fuel.ignitionrobotics.org/1.0/openrobotics/models/drc practice: white jersey barrier/2/files/meshes/jersey_barrier.dae + + + + + + + https://fuel.ignitionrobotics.org/1.0/openrobotics/models/drc practice: white jersey barrier/2/files/meshes/jersey_barrier.dae + + + 10 + + + + + + + + + + + + + + + 0 + 0 + + 0 + 0 + 1 + 0 + + 10.4084 -11 0 0 -0 0 + + + 1 + + + + + https://fuel.ignitionrobotics.org/1.0/openrobotics/models/drc practice: orange jersey barrier/2/files/meshes/jersey_barrier.dae + + + + + + + https://fuel.ignitionrobotics.org/1.0/openrobotics/models/drc practice: orange jersey barrier/2/files/meshes/jersey_barrier.dae + + + 10 + + + + + + + + + + + + + + + 0 + 0 + + 0 + 0 + 1 + 0 + + 10.4143 -13 0 0 -0 0 + + + 1 + 1 + + + + + https://fuel.ignitionrobotics.org/1.0/openrobotics/models/drc practice: orange jersey barrier/2/files/meshes/jersey_barrier.dae + + + + + + + https://fuel.ignitionrobotics.org/1.0/openrobotics/models/drc practice: orange jersey barrier/2/files/meshes/jersey_barrier.dae + + + 10 + + + + + + + + + + + + + + + 0 + 0 + + 0 0 0 0 0 -1.5707 + 0 + 0 + 1 + 0 + + 1 + + + + + https://fuel.ignitionrobotics.org/1.0/openrobotics/models/drc practice: white jersey barrier/2/files/meshes/jersey_barrier.dae + + + + + + + https://fuel.ignitionrobotics.org/1.0/openrobotics/models/drc practice: white jersey barrier/2/files/meshes/jersey_barrier.dae + + + 10 + + + + + + + + + + + + + + + 0 + 0 + + 1.7646 -0.008563 0 0 0 -1.5707 + 0 + 0 + 1 + 0 + + 1 + + + + + https://fuel.ignitionrobotics.org/1.0/openrobotics/models/drc practice: orange jersey barrier/2/files/meshes/jersey_barrier.dae + + + + + + + https://fuel.ignitionrobotics.org/1.0/openrobotics/models/drc practice: orange jersey barrier/2/files/meshes/jersey_barrier.dae + + + 10 + + + + + + + + + + + + + + + 0 + 0 + + 3.52922 -0.016171 0 0 0 -1.5707 + 0 + 0 + 1 + 0 + + 1 + + + + + https://fuel.ignitionrobotics.org/1.0/openrobotics/models/drc practice: white jersey barrier/2/files/meshes/jersey_barrier.dae + + + + + + + https://fuel.ignitionrobotics.org/1.0/openrobotics/models/drc practice: white jersey barrier/2/files/meshes/jersey_barrier.dae + + + 10 + + + + + + + + + + + + + + + 0 + 0 + + 5.13431 0.54837 0 0 0 -0.785398 + 0 + 0 + 1 + 0 + + 1 + + + + + https://fuel.ignitionrobotics.org/1.0/openrobotics/models/drc practice: orange jersey barrier/2/files/meshes/jersey_barrier.dae + + + + + + + https://fuel.ignitionrobotics.org/1.0/openrobotics/models/drc practice: orange jersey barrier/2/files/meshes/jersey_barrier.dae + + + 10 + + + + + + + + + + + + + + + 0 + 0 + + 6.3884 1.79074 0 0 0 -0.785398 + 0 + 0 + 1 + 0 + + 1 + + + + + https://fuel.ignitionrobotics.org/1.0/openrobotics/models/drc practice: white jersey barrier/2/files/meshes/jersey_barrier.dae + + + + + + + https://fuel.ignitionrobotics.org/1.0/openrobotics/models/drc practice: white jersey barrier/2/files/meshes/jersey_barrier.dae + + + 10 + + + + + + + + + + + + + + + 0 + 0 + + 7.6423 3.03258 0 0 0 -0.785398 + 0 + 0 + 1 + 0 + + 1 + + + + + https://fuel.ignitionrobotics.org/1.0/openrobotics/models/drc practice: orange jersey barrier/2/files/meshes/jersey_barrier.dae + + + + + + + https://fuel.ignitionrobotics.org/1.0/openrobotics/models/drc practice: orange jersey barrier/2/files/meshes/jersey_barrier.dae + + + 10 + + + + + + + + + + + + + + + 0 + 0 + + 8.89611 4.27529 0 0 0 -0.785398 + 0 + 0 + 1 + 0 + + 1 + + + + + https://fuel.ignitionrobotics.org/1.0/openrobotics/models/drc practice: white jersey barrier/2/files/meshes/jersey_barrier.dae + + + + + + + https://fuel.ignitionrobotics.org/1.0/openrobotics/models/drc practice: white jersey barrier/2/files/meshes/jersey_barrier.dae + + + 10 + + + + + + + + + + + + + + + 0 + 0 + + 10.1496 5.51614 0 0 0 -0.785398 + 0 + 0 + 1 + 0 + + 1 + + + + + model://blue_cylinder/meshes/cylinder.dae + + + + + + + model://blue_cylinder/meshes/cylinder.dae + + + 10 + + + + + + + + + + + + + + + 0 + 0 + + 11.0288 6.44203 0 0 -0 0 + 0 + 0 + 1 + 0 + + 1 + + + + + model://blue_cylinder/meshes/cylinder.dae + + + + + + + model://blue_cylinder/meshes/cylinder.dae + + + 10 + + + + + + + + + + + + + + + 0 + 0 + + 11.0288 6.44203 0.866 0 -0 0 + 0 + 0 + 1 + 0 + + -1.5566 -14 0 0 -0 0 + + + 1 + 1 + + + + + https://fuel.ignitionrobotics.org/1.0/openrobotics/models/drc practice: orange jersey barrier/2/files/meshes/jersey_barrier.dae + + + + + + + https://fuel.ignitionrobotics.org/1.0/openrobotics/models/drc practice: orange jersey barrier/2/files/meshes/jersey_barrier.dae + + + 10 + + + + + + + + + + + + + + + 0 + 0 + + 0 0 0 0 0 -1.5707 + 0 + 0 + 1 + 0 + + 1 + + + + + https://fuel.ignitionrobotics.org/1.0/openrobotics/models/drc practice: white jersey barrier/2/files/meshes/jersey_barrier.dae + + + + + + + https://fuel.ignitionrobotics.org/1.0/openrobotics/models/drc practice: white jersey barrier/2/files/meshes/jersey_barrier.dae + + + 10 + + + + + + + + + + + + + + + 0 + 0 + + 1.7646 -0.008563 0 0 0 -1.5707 + 0 + 0 + 1 + 0 + + 1 + + + + + https://fuel.ignitionrobotics.org/1.0/openrobotics/models/drc practice: orange jersey barrier/2/files/meshes/jersey_barrier.dae + + + + + + + https://fuel.ignitionrobotics.org/1.0/openrobotics/models/drc practice: orange jersey barrier/2/files/meshes/jersey_barrier.dae + + + 10 + + + + + + + + + + + + + + + 0 + 0 + + 3.52922 -0.016171 0 0 0 -1.5707 + 0 + 0 + 1 + 0 + + 1 + + + + + https://fuel.ignitionrobotics.org/1.0/openrobotics/models/drc practice: white jersey barrier/2/files/meshes/jersey_barrier.dae + + + + + + + https://fuel.ignitionrobotics.org/1.0/openrobotics/models/drc practice: white jersey barrier/2/files/meshes/jersey_barrier.dae + + + 10 + + + + + + + + + + + + + + + 0 + 0 + + 5.13431 0.54837 0 0 0 -0.785398 + 0 + 0 + 1 + 0 + + 1 + + + + + https://fuel.ignitionrobotics.org/1.0/openrobotics/models/drc practice: orange jersey barrier/2/files/meshes/jersey_barrier.dae + + + + + + + https://fuel.ignitionrobotics.org/1.0/openrobotics/models/drc practice: orange jersey barrier/2/files/meshes/jersey_barrier.dae + + + 10 + + + + + + + + + + + + + + + 0 + 0 + + 6.3884 1.79074 0 0 0 -0.785398 + 0 + 0 + 1 + 0 + + 1 + + + + + https://fuel.ignitionrobotics.org/1.0/openrobotics/models/drc practice: white jersey barrier/2/files/meshes/jersey_barrier.dae + + + + + + + https://fuel.ignitionrobotics.org/1.0/openrobotics/models/drc practice: white jersey barrier/2/files/meshes/jersey_barrier.dae + + + 10 + + + + + + + + + + + + + + + 0 + 0 + + 7.6423 3.03258 0 0 0 -0.785398 + 0 + 0 + 1 + 0 + + 1 + + + + + https://fuel.ignitionrobotics.org/1.0/openrobotics/models/drc practice: orange jersey barrier/2/files/meshes/jersey_barrier.dae + + + + + + + https://fuel.ignitionrobotics.org/1.0/openrobotics/models/drc practice: orange jersey barrier/2/files/meshes/jersey_barrier.dae + + + 10 + + + + + + + + + + + + + + + 0 + 0 + + 8.89611 4.27529 0 0 0 -0.785398 + 0 + 0 + 1 + 0 + + 1 + + + + + https://fuel.ignitionrobotics.org/1.0/openrobotics/models/drc practice: white jersey barrier/2/files/meshes/jersey_barrier.dae + + + + + + + https://fuel.ignitionrobotics.org/1.0/openrobotics/models/drc practice: white jersey barrier/2/files/meshes/jersey_barrier.dae + + + 10 + + + + + + + + + + + + + + + 0 + 0 + + 10.1496 5.51614 0 0 0 -0.785398 + 0 + 0 + 1 + 0 + + 1 + + + + + model://blue_cylinder/meshes/cylinder.dae + + + + + + + model://blue_cylinder/meshes/cylinder.dae + + + 10 + + + + + + + + + + + + + + + 0 + 0 + + 11.0288 6.44203 0 0 -0 0 + 0 + 0 + 1 + 0 + + 1 + + + + + model://blue_cylinder/meshes/cylinder.dae + + + + + + + model://blue_cylinder/meshes/cylinder.dae + + + 10 + + + + + + + + + + + + + + + 0 + 0 + + 11.0288 6.44203 0.866 0 -0 0 + 0 + 0 + 1 + 0 + + -1 -1 0 0 -0 0 + + + 1 + 1 + + + + + https://fuel.ignitionrobotics.org/1.0/openrobotics/models/drc practice: orange jersey barrier/2/files/meshes/jersey_barrier.dae + + + + + + + https://fuel.ignitionrobotics.org/1.0/openrobotics/models/drc practice: orange jersey barrier/2/files/meshes/jersey_barrier.dae + + + 10 + + + + + + + + + + + + + + + 0 + 0 + + 0 0 0 0 0 -1.5707 + 0 + 0 + 1 + 0 + + 1 + + + + + https://fuel.ignitionrobotics.org/1.0/openrobotics/models/drc practice: white jersey barrier/2/files/meshes/jersey_barrier.dae + + + + + + + https://fuel.ignitionrobotics.org/1.0/openrobotics/models/drc practice: white jersey barrier/2/files/meshes/jersey_barrier.dae + + + 10 + + + + + + + + + + + + + + + 0 + 0 + + 1.7646 -0.008563 0 0 0 -1.5707 + 0 + 0 + 1 + 0 + + 1 + + + + + https://fuel.ignitionrobotics.org/1.0/openrobotics/models/drc practice: orange jersey barrier/2/files/meshes/jersey_barrier.dae + + + + + + + https://fuel.ignitionrobotics.org/1.0/openrobotics/models/drc practice: orange jersey barrier/2/files/meshes/jersey_barrier.dae + + + 10 + + + + + + + + + + + + + + + 0 + 0 + + 3.52922 -0.016171 0 0 0 -1.5707 + 0 + 0 + 1 + 0 + + 1 + + + + + https://fuel.ignitionrobotics.org/1.0/openrobotics/models/drc practice: white jersey barrier/2/files/meshes/jersey_barrier.dae + + + + + + + https://fuel.ignitionrobotics.org/1.0/openrobotics/models/drc practice: white jersey barrier/2/files/meshes/jersey_barrier.dae + + + 10 + + + + + + + + + + + + + + + 0 + 0 + + 5.2559 -0.59667 0 0 0 -2.3562 + 0 + 0 + 1 + 0 + + 1 + + + + + https://fuel.ignitionrobotics.org/1.0/openrobotics/models/drc practice: orange jersey barrier/2/files/meshes/jersey_barrier.dae + + + + + + + https://fuel.ignitionrobotics.org/1.0/openrobotics/models/drc practice: orange jersey barrier/2/files/meshes/jersey_barrier.dae + + + 10 + + + + + + + + + + + + + + + 0 + 0 + + 6.49818 -1.85004 0 0 0 -2.3562 + 0 + 0 + 1 + 0 + + 1 + + + + + https://fuel.ignitionrobotics.org/1.0/openrobotics/models/drc practice: white jersey barrier/2/files/meshes/jersey_barrier.dae + + + + + + + https://fuel.ignitionrobotics.org/1.0/openrobotics/models/drc practice: white jersey barrier/2/files/meshes/jersey_barrier.dae + + + 10 + + + + + + + + + + + + + + + 0 + 0 + + 7.74031 -3.10261 0 0 0 -2.3562 + 0 + 0 + 1 + 0 + + 1 + + + + + https://fuel.ignitionrobotics.org/1.0/openrobotics/models/drc practice: orange jersey barrier/2/files/meshes/jersey_barrier.dae + + + + + + + https://fuel.ignitionrobotics.org/1.0/openrobotics/models/drc practice: orange jersey barrier/2/files/meshes/jersey_barrier.dae + + + 10 + + + + + + + + + + + + + + + 0 + 0 + + 8.98256 -4.3561 0 0 0 -2.3562 + 0 + 0 + 1 + 0 + + 1 + + + + + https://fuel.ignitionrobotics.org/1.0/openrobotics/models/drc practice: white jersey barrier/2/files/meshes/jersey_barrier.dae + + + + + + + https://fuel.ignitionrobotics.org/1.0/openrobotics/models/drc practice: white jersey barrier/2/files/meshes/jersey_barrier.dae + + + 10 + + + + + + + + + + + + + + + 0 + 0 + + 10.2248 -5.60978 0 0 0 -2.3562 + 0 + 0 + 1 + 0 + + 1 + + + + + model://blue_cylinder/meshes/cylinder.dae + + + + + + + model://blue_cylinder/meshes/cylinder.dae + + + 10 + + + + + + + + + + + + + + + 0 + 0 + + 11.0288 -6.44203 0 0 -0 0 + 0 + 0 + 1 + 0 + + 1 + + + + + model://blue_cylinder/meshes/cylinder.dae + + + + + + + model://blue_cylinder/meshes/cylinder.dae + + + 10 + + + + + + + + + + + + + + + 0 + 0 + + 11.0288 -6.44203 0.866 0 -0 0 + 0 + 0 + 1 + 0 + + -1 16 0 0 -0 0 + + + 1 + 1 + + + + + https://fuel.ignitionrobotics.org/1.0/openrobotics/models/drc practice: orange jersey barrier/2/files/meshes/jersey_barrier.dae + + + + + + + https://fuel.ignitionrobotics.org/1.0/openrobotics/models/drc practice: orange jersey barrier/2/files/meshes/jersey_barrier.dae + + + 10 + + + + + + + + + + + + + + + 0 + 0 + + 0 0 0 0 0 -1.5707 + 0 + 0 + 1 + 0 + + 1 + + + + + https://fuel.ignitionrobotics.org/1.0/openrobotics/models/drc practice: white jersey barrier/2/files/meshes/jersey_barrier.dae + + + + + + + https://fuel.ignitionrobotics.org/1.0/openrobotics/models/drc practice: white jersey barrier/2/files/meshes/jersey_barrier.dae + + + 10 + + + + + + + + + + + + + + + 0 + 0 + + 1.7646 -0.008563 0 0 0 -1.5707 + 0 + 0 + 1 + 0 + + 1 + + + + + https://fuel.ignitionrobotics.org/1.0/openrobotics/models/drc practice: orange jersey barrier/2/files/meshes/jersey_barrier.dae + + + + + + + https://fuel.ignitionrobotics.org/1.0/openrobotics/models/drc practice: orange jersey barrier/2/files/meshes/jersey_barrier.dae + + + 10 + + + + + + + + + + + + + + + 0 + 0 + + 3.52922 -0.016171 0 0 0 -1.5707 + 0 + 0 + 1 + 0 + + 1 + + + + + https://fuel.ignitionrobotics.org/1.0/openrobotics/models/drc practice: white jersey barrier/2/files/meshes/jersey_barrier.dae + + + + + + + https://fuel.ignitionrobotics.org/1.0/openrobotics/models/drc practice: white jersey barrier/2/files/meshes/jersey_barrier.dae + + + 10 + + + + + + + + + + + + + + + 0 + 0 + + 5.13431 0.54837 0 0 0 -0.785398 + 0 + 0 + 1 + 0 + + 1 + + + + + https://fuel.ignitionrobotics.org/1.0/openrobotics/models/drc practice: orange jersey barrier/2/files/meshes/jersey_barrier.dae + + + + + + + https://fuel.ignitionrobotics.org/1.0/openrobotics/models/drc practice: orange jersey barrier/2/files/meshes/jersey_barrier.dae + + + 10 + + + + + + + + + + + + + + + 0 + 0 + + 6.3884 1.79074 0 0 0 -0.785398 + 0 + 0 + 1 + 0 + + 1 + + + + + https://fuel.ignitionrobotics.org/1.0/openrobotics/models/drc practice: white jersey barrier/2/files/meshes/jersey_barrier.dae + + + + + + + https://fuel.ignitionrobotics.org/1.0/openrobotics/models/drc practice: white jersey barrier/2/files/meshes/jersey_barrier.dae + + + 10 + + + + + + + + + + + + + + + 0 + 0 + + 7.6423 3.03258 0 0 0 -0.785398 + 0 + 0 + 1 + 0 + + 1 + + + + + https://fuel.ignitionrobotics.org/1.0/openrobotics/models/drc practice: orange jersey barrier/2/files/meshes/jersey_barrier.dae + + + + + + + https://fuel.ignitionrobotics.org/1.0/openrobotics/models/drc practice: orange jersey barrier/2/files/meshes/jersey_barrier.dae + + + 10 + + + + + + + + + + + + + + + 0 + 0 + + 8.89611 4.27529 0 0 0 -0.785398 + 0 + 0 + 1 + 0 + + 1 + + + + + https://fuel.ignitionrobotics.org/1.0/openrobotics/models/drc practice: white jersey barrier/2/files/meshes/jersey_barrier.dae + + + + + + + https://fuel.ignitionrobotics.org/1.0/openrobotics/models/drc practice: white jersey barrier/2/files/meshes/jersey_barrier.dae + + + 10 + + + + + + + + + + + + + + + 0 + 0 + + 10.1496 5.51614 0 0 0 -0.785398 + 0 + 0 + 1 + 0 + + 1 + + + + + model://blue_cylinder/meshes/cylinder.dae + + + + + + + model://blue_cylinder/meshes/cylinder.dae + + + 10 + + + + + + + + + + + + + + + 0 + 0 + + 11.0288 6.44203 0 0 -0 0 + 0 + 0 + 1 + 0 + + 1 + + + + + model://blue_cylinder/meshes/cylinder.dae + + + + + + + model://blue_cylinder/meshes/cylinder.dae + + + 10 + + + + + + + + + + + + + + + 0 + 0 + + 11.0288 6.44203 0.866 0 -0 0 + 0 + 0 + 1 + 0 + + -14.5593 -16 0 0 -0 0 + + + 1 + 1 + + + + + https://fuel.ignitionrobotics.org/1.0/openrobotics/models/drc practice: orange jersey barrier/2/files/meshes/jersey_barrier.dae + + + + + + + https://fuel.ignitionrobotics.org/1.0/openrobotics/models/drc practice: orange jersey barrier/2/files/meshes/jersey_barrier.dae + + + 10 + + + + + + + + + + + + + + + 0 + 0 + + 0 0 0 0 0 -1.5707 + 0 + 0 + 1 + 0 + + 1 + + + + + https://fuel.ignitionrobotics.org/1.0/openrobotics/models/drc practice: white jersey barrier/2/files/meshes/jersey_barrier.dae + + + + + + + https://fuel.ignitionrobotics.org/1.0/openrobotics/models/drc practice: white jersey barrier/2/files/meshes/jersey_barrier.dae + + + 10 + + + + + + + + + + + + + + + 0 + 0 + + 1.7646 -0.008563 0 0 0 -1.5707 + 0 + 0 + 1 + 0 + + 1 + + + + + https://fuel.ignitionrobotics.org/1.0/openrobotics/models/drc practice: orange jersey barrier/2/files/meshes/jersey_barrier.dae + + + + + + + https://fuel.ignitionrobotics.org/1.0/openrobotics/models/drc practice: orange jersey barrier/2/files/meshes/jersey_barrier.dae + + + 10 + + + + + + + + + + + + + + + 0 + 0 + + 3.52922 -0.016171 0 0 0 -1.5707 + 0 + 0 + 1 + 0 + + 1 + + + + + https://fuel.ignitionrobotics.org/1.0/openrobotics/models/drc practice: white jersey barrier/2/files/meshes/jersey_barrier.dae + + + + + + + https://fuel.ignitionrobotics.org/1.0/openrobotics/models/drc practice: white jersey barrier/2/files/meshes/jersey_barrier.dae + + + 10 + + + + + + + + + + + + + + + 0 + 0 + + 5.2559 -0.59667 0 0 0 -2.3562 + 0 + 0 + 1 + 0 + + 1 + + + + + https://fuel.ignitionrobotics.org/1.0/openrobotics/models/drc practice: orange jersey barrier/2/files/meshes/jersey_barrier.dae + + + + + + + https://fuel.ignitionrobotics.org/1.0/openrobotics/models/drc practice: orange jersey barrier/2/files/meshes/jersey_barrier.dae + + + 10 + + + + + + + + + + + + + + + 0 + 0 + + 6.49818 -1.85004 0 0 0 -2.3562 + 0 + 0 + 1 + 0 + + 1 + + + + + https://fuel.ignitionrobotics.org/1.0/openrobotics/models/drc practice: white jersey barrier/2/files/meshes/jersey_barrier.dae + + + + + + + https://fuel.ignitionrobotics.org/1.0/openrobotics/models/drc practice: white jersey barrier/2/files/meshes/jersey_barrier.dae + + + 10 + + + + + + + + + + + + + + + 0 + 0 + + 7.74031 -3.10261 0 0 0 -2.3562 + 0 + 0 + 1 + 0 + + 1 + + + + + https://fuel.ignitionrobotics.org/1.0/openrobotics/models/drc practice: orange jersey barrier/2/files/meshes/jersey_barrier.dae + + + + + + + https://fuel.ignitionrobotics.org/1.0/openrobotics/models/drc practice: orange jersey barrier/2/files/meshes/jersey_barrier.dae + + + 10 + + + + + + + + + + + + + + + 0 + 0 + + 8.98256 -4.3561 0 0 0 -2.3562 + 0 + 0 + 1 + 0 + + 1 + + + + + https://fuel.ignitionrobotics.org/1.0/openrobotics/models/drc practice: white jersey barrier/2/files/meshes/jersey_barrier.dae + + + + + + + https://fuel.ignitionrobotics.org/1.0/openrobotics/models/drc practice: white jersey barrier/2/files/meshes/jersey_barrier.dae + + + 10 + + + + + + + + + + + + + + + 0 + 0 + + 10.2248 -5.60978 0 0 0 -2.3562 + 0 + 0 + 1 + 0 + + 1 + + + + + model://blue_cylinder/meshes/cylinder.dae + + + + + + + model://blue_cylinder/meshes/cylinder.dae + + + 10 + + + + + + + + + + + + + + + 0 + 0 + + 11.0288 -6.44203 0 0 -0 0 + 0 + 0 + 1 + 0 + + 1 + + + + + model://blue_cylinder/meshes/cylinder.dae + + + + + + + model://blue_cylinder/meshes/cylinder.dae + + + 10 + + + + + + + + + + + + + + + 0 + 0 + + 11.0288 -6.44203 0.866 0 -0 0 + 0 + 0 + 1 + 0 + + 25 -15 0 0 -0 0 + + + 1 + 1 + + + + + https://fuel.ignitionrobotics.org/1.0/openrobotics/models/drc practice: orange jersey barrier/2/files/meshes/jersey_barrier.dae + + + + + + + https://fuel.ignitionrobotics.org/1.0/openrobotics/models/drc practice: orange jersey barrier/2/files/meshes/jersey_barrier.dae + + + 10 + + + + + + + + + + + + + + + 0 + 0 + + 0 0 0 0 0 -1.5707 + 0 + 0 + 1 + 0 + + 1 + + + + + https://fuel.ignitionrobotics.org/1.0/openrobotics/models/drc practice: white jersey barrier/2/files/meshes/jersey_barrier.dae + + + + + + + https://fuel.ignitionrobotics.org/1.0/openrobotics/models/drc practice: white jersey barrier/2/files/meshes/jersey_barrier.dae + + + 10 + + + + + + + + + + + + + + + 0 + 0 + + 1.7646 -0.008563 0 0 0 -1.5707 + 0 + 0 + 1 + 0 + + 1 + + + + + https://fuel.ignitionrobotics.org/1.0/openrobotics/models/drc practice: orange jersey barrier/2/files/meshes/jersey_barrier.dae + + + + + + + https://fuel.ignitionrobotics.org/1.0/openrobotics/models/drc practice: orange jersey barrier/2/files/meshes/jersey_barrier.dae + + + 10 + + + + + + + + + + + + + + + 0 + 0 + + 3.52922 -0.016171 0 0 0 -1.5707 + 0 + 0 + 1 + 0 + + 1 + + + + + https://fuel.ignitionrobotics.org/1.0/openrobotics/models/drc practice: white jersey barrier/2/files/meshes/jersey_barrier.dae + + + + + + + https://fuel.ignitionrobotics.org/1.0/openrobotics/models/drc practice: white jersey barrier/2/files/meshes/jersey_barrier.dae + + + 10 + + + + + + + + + + + + + + + 0 + 0 + + 5.13431 0.54837 0 0 0 -0.785398 + 0 + 0 + 1 + 0 + + 1 + + + + + https://fuel.ignitionrobotics.org/1.0/openrobotics/models/drc practice: orange jersey barrier/2/files/meshes/jersey_barrier.dae + + + + + + + https://fuel.ignitionrobotics.org/1.0/openrobotics/models/drc practice: orange jersey barrier/2/files/meshes/jersey_barrier.dae + + + 10 + + + + + + + + + + + + + + + 0 + 0 + + 6.3884 1.79074 0 0 0 -0.785398 + 0 + 0 + 1 + 0 + + 1 + + + + + https://fuel.ignitionrobotics.org/1.0/openrobotics/models/drc practice: white jersey barrier/2/files/meshes/jersey_barrier.dae + + + + + + + https://fuel.ignitionrobotics.org/1.0/openrobotics/models/drc practice: white jersey barrier/2/files/meshes/jersey_barrier.dae + + + 10 + + + + + + + + + + + + + + + 0 + 0 + + 7.6423 3.03258 0 0 0 -0.785398 + 0 + 0 + 1 + 0 + + 1 + + + + + https://fuel.ignitionrobotics.org/1.0/openrobotics/models/drc practice: orange jersey barrier/2/files/meshes/jersey_barrier.dae + + + + + + + https://fuel.ignitionrobotics.org/1.0/openrobotics/models/drc practice: orange jersey barrier/2/files/meshes/jersey_barrier.dae + + + 10 + + + + + + + + + + + + + + + 0 + 0 + + 8.89611 4.27529 0 0 0 -0.785398 + 0 + 0 + 1 + 0 + + 1 + + + + + https://fuel.ignitionrobotics.org/1.0/openrobotics/models/drc practice: white jersey barrier/2/files/meshes/jersey_barrier.dae + + + + + + + https://fuel.ignitionrobotics.org/1.0/openrobotics/models/drc practice: white jersey barrier/2/files/meshes/jersey_barrier.dae + + + 10 + + + + + + + + + + + + + + + 0 + 0 + + 10.1496 5.51614 0 0 0 -0.785398 + 0 + 0 + 1 + 0 + + 1 + + + + + model://blue_cylinder/meshes/cylinder.dae + + + + + + + model://blue_cylinder/meshes/cylinder.dae + + + 10 + + + + + + + + + + + + + + + 0 + 0 + + 11.0288 6.44203 0 0 -0 0 + 0 + 0 + 1 + 0 + + 1 + + + + + model://blue_cylinder/meshes/cylinder.dae + + + + + + + model://blue_cylinder/meshes/cylinder.dae + + + 10 + + + + + + + + + + + + + + + 0 + 0 + + 11.0288 6.44203 0.866 0 -0 0 + 0 + 0 + 1 + 0 + + -25 -4 0 0 -0 0 + + + 1 + + + + + https://fuel.ignitionrobotics.org/1.0/openrobotics/models/drc practice: white jersey barrier/2/files/meshes/jersey_barrier.dae + + + + + + + https://fuel.ignitionrobotics.org/1.0/openrobotics/models/drc practice: white jersey barrier/2/files/meshes/jersey_barrier.dae + + + 10 + + + + + + + + + + + + + + + 0 + 0 + + 0 + 0 + 1 + 0 + + -16.4458 -4 0 0 -0 0 + + + 1 + + + + + https://fuel.ignitionrobotics.org/1.0/openrobotics/models/drc practice: white jersey barrier/2/files/meshes/jersey_barrier.dae + + + + + + + https://fuel.ignitionrobotics.org/1.0/openrobotics/models/drc practice: white jersey barrier/2/files/meshes/jersey_barrier.dae + + + 10 + + + + + + + + + + + + + + + 0 + 0 + + 0 + 0 + 1 + 0 + + -17.5466 -5 0 0 -0 0 + + + 1 + + + + + model://blue_cylinder/meshes/cylinder.dae + + + + + + + model://blue_cylinder/meshes/cylinder.dae + + + 10 + + + + + + + + + + + + + + + 0 + 0 + + 0 + 0 + 1 + 0 + + -17 -4.51853 0 0 -0 0 + + + 1 + + + + + model://blue_cylinder/meshes/cylinder.dae + + + + + + + model://blue_cylinder/meshes/cylinder.dae + + + 10 + + + + + + + + + + + + + + + 0 + 0 + + 0 + 0 + 1 + 0 + + -17.425 -4 0 0 -0 0 + + + 1 + 1 + + + + + https://fuel.ignitionrobotics.org/1.0/openrobotics/models/drc practice: orange jersey barrier/2/files/meshes/jersey_barrier.dae + + + + + + + https://fuel.ignitionrobotics.org/1.0/openrobotics/models/drc practice: orange jersey barrier/2/files/meshes/jersey_barrier.dae + + + 10 + + + + + + + + + + + + + + + 0 + 0 + + 0 0 0 0 0 -1.5707 + 0 + 0 + 1 + 0 + + 1 + + + + + https://fuel.ignitionrobotics.org/1.0/openrobotics/models/drc practice: white jersey barrier/2/files/meshes/jersey_barrier.dae + + + + + + + https://fuel.ignitionrobotics.org/1.0/openrobotics/models/drc practice: white jersey barrier/2/files/meshes/jersey_barrier.dae + + + 10 + + + + + + + + + + + + + + + 0 + 0 + + 1.7646 -0.008563 0 0 0 -1.5707 + 0 + 0 + 1 + 0 + + 1 + + + + + https://fuel.ignitionrobotics.org/1.0/openrobotics/models/drc practice: orange jersey barrier/2/files/meshes/jersey_barrier.dae + + + + + + + https://fuel.ignitionrobotics.org/1.0/openrobotics/models/drc practice: orange jersey barrier/2/files/meshes/jersey_barrier.dae + + + 10 + + + + + + + + + + + + + + + 0 + 0 + + 3.52922 -0.016171 0 0 0 -1.5707 + 0 + 0 + 1 + 0 + + 1 + + + + + https://fuel.ignitionrobotics.org/1.0/openrobotics/models/drc practice: white jersey barrier/2/files/meshes/jersey_barrier.dae + + + + + + + https://fuel.ignitionrobotics.org/1.0/openrobotics/models/drc practice: white jersey barrier/2/files/meshes/jersey_barrier.dae + + + 10 + + + + + + + + + + + + + + + 0 + 0 + + 5.2559 -0.59667 0 0 0 -2.3562 + 0 + 0 + 1 + 0 + + 1 + + + + + https://fuel.ignitionrobotics.org/1.0/openrobotics/models/drc practice: orange jersey barrier/2/files/meshes/jersey_barrier.dae + + + + + + + https://fuel.ignitionrobotics.org/1.0/openrobotics/models/drc practice: orange jersey barrier/2/files/meshes/jersey_barrier.dae + + + 10 + + + + + + + + + + + + + + + 0 + 0 + + 6.49818 -1.85004 0 0 0 -2.3562 + 0 + 0 + 1 + 0 + + 1 + + + + + https://fuel.ignitionrobotics.org/1.0/openrobotics/models/drc practice: white jersey barrier/2/files/meshes/jersey_barrier.dae + + + + + + + https://fuel.ignitionrobotics.org/1.0/openrobotics/models/drc practice: white jersey barrier/2/files/meshes/jersey_barrier.dae + + + 10 + + + + + + + + + + + + + + + 0 + 0 + + 7.74031 -3.10261 0 0 0 -2.3562 + 0 + 0 + 1 + 0 + + 1 + + + + + https://fuel.ignitionrobotics.org/1.0/openrobotics/models/drc practice: orange jersey barrier/2/files/meshes/jersey_barrier.dae + + + + + + + https://fuel.ignitionrobotics.org/1.0/openrobotics/models/drc practice: orange jersey barrier/2/files/meshes/jersey_barrier.dae + + + 10 + + + + + + + + + + + + + + + 0 + 0 + + 8.98256 -4.3561 0 0 0 -2.3562 + 0 + 0 + 1 + 0 + + 1 + + + + + https://fuel.ignitionrobotics.org/1.0/openrobotics/models/drc practice: white jersey barrier/2/files/meshes/jersey_barrier.dae + + + + + + + https://fuel.ignitionrobotics.org/1.0/openrobotics/models/drc practice: white jersey barrier/2/files/meshes/jersey_barrier.dae + + + 10 + + + + + + + + + + + + + + + 0 + 0 + + 10.2248 -5.60978 0 0 0 -2.3562 + 0 + 0 + 1 + 0 + + 1 + + + + + model://blue_cylinder/meshes/cylinder.dae + + + + + + + model://blue_cylinder/meshes/cylinder.dae + + + 10 + + + + + + + + + + + + + + + 0 + 0 + + 11.0288 -6.44203 0 0 -0 0 + 0 + 0 + 1 + 0 + + 1 + + + + + model://blue_cylinder/meshes/cylinder.dae + + + + + + + model://blue_cylinder/meshes/cylinder.dae + + + 10 + + + + + + + + + + + + + + + 0 + 0 + + 11.0288 -6.44203 0.866 0 -0 0 + 0 + 0 + 1 + 0 + + -9 10 0 0 -0 0 + + 0 0 -9.8 + 6e-06 2.3e-05 -4.2e-05 + + + + diff --git a/smacc2_sm_reference_library/sm_dancebot_ue/worlds/ridgeback_race_no_lidar.world b/smacc2_sm_reference_library/sm_dancebot_ue/worlds/ridgeback_race_no_lidar.world new file mode 100644 index 000000000..7a6a32d6e --- /dev/null +++ b/smacc2_sm_reference_library/sm_dancebot_ue/worlds/ridgeback_race_no_lidar.world @@ -0,0 +1,4881 @@ + + + + 1 + 0 0 10 0 -0 0 + 0.8 0.8 0.8 1 + 0.2 0.2 0.2 1 + + 1000 + 0.9 + 0.01 + 0.001 + + -0.5 0.1 -0.9 + + 0 + 0 + 0 + + + + 1 + + + + + 0 0 1 + 100 100 + + + + + + 100 + 50 + + + + + + + + + + + 10 + + + 0 + + + 0 0 1 + 100 100 + + + + + + + + 0 + 0 + + 0 + 0 + 1 + 0 + + + + 0.001 + 1 + 1000 + + + 0.4 0.4 0.4 1 + 0.7 0.7 0.7 1 + 1 + + + EARTH_WGS84 + 0 + 0 + 0 + 0 + + + 1 + 1 + + + + + https://fuel.ignitionrobotics.org/1.0/openrobotics/models/drc practice: orange jersey barrier/2/files/meshes/jersey_barrier.dae + + + + + + + https://fuel.ignitionrobotics.org/1.0/openrobotics/models/drc practice: orange jersey barrier/2/files/meshes/jersey_barrier.dae + + + 10 + + + + + + + + + + + + + + + 0 + 0 + + 0 0 0 0 0 -1.5707 + 0 + 0 + 1 + 0 + + 1 + + + + + https://fuel.ignitionrobotics.org/1.0/openrobotics/models/drc practice: white jersey barrier/2/files/meshes/jersey_barrier.dae + + + + + + + https://fuel.ignitionrobotics.org/1.0/openrobotics/models/drc practice: white jersey barrier/2/files/meshes/jersey_barrier.dae + + + 10 + + + + + + + + + + + + + + + 0 + 0 + + 1.7646 -0.008563 0 0 0 -1.5707 + 0 + 0 + 1 + 0 + + 1 + + + + + https://fuel.ignitionrobotics.org/1.0/openrobotics/models/drc practice: orange jersey barrier/2/files/meshes/jersey_barrier.dae + + + + + + + https://fuel.ignitionrobotics.org/1.0/openrobotics/models/drc practice: orange jersey barrier/2/files/meshes/jersey_barrier.dae + + + 10 + + + + + + + + + + + + + + + 0 + 0 + + 3.52922 -0.016171 0 0 0 -1.5707 + 0 + 0 + 1 + 0 + + 1 + + + + + https://fuel.ignitionrobotics.org/1.0/openrobotics/models/drc practice: white jersey barrier/2/files/meshes/jersey_barrier.dae + + + + + + + https://fuel.ignitionrobotics.org/1.0/openrobotics/models/drc practice: white jersey barrier/2/files/meshes/jersey_barrier.dae + + + 10 + + + + + + + + + + + + + + + 0 + 0 + + 5.13431 0.54837 0 0 0 -0.785398 + 0 + 0 + 1 + 0 + + 1 + + + + + https://fuel.ignitionrobotics.org/1.0/openrobotics/models/drc practice: orange jersey barrier/2/files/meshes/jersey_barrier.dae + + + + + + + https://fuel.ignitionrobotics.org/1.0/openrobotics/models/drc practice: orange jersey barrier/2/files/meshes/jersey_barrier.dae + + + 10 + + + + + + + + + + + + + + + 0 + 0 + + 6.3884 1.79074 0 0 0 -0.785398 + 0 + 0 + 1 + 0 + + 1 + + + + + https://fuel.ignitionrobotics.org/1.0/openrobotics/models/drc practice: white jersey barrier/2/files/meshes/jersey_barrier.dae + + + + + + + https://fuel.ignitionrobotics.org/1.0/openrobotics/models/drc practice: white jersey barrier/2/files/meshes/jersey_barrier.dae + + + 10 + + + + + + + + + + + + + + + 0 + 0 + + 7.6423 3.03258 0 0 0 -0.785398 + 0 + 0 + 1 + 0 + + 1 + + + + + https://fuel.ignitionrobotics.org/1.0/openrobotics/models/drc practice: orange jersey barrier/2/files/meshes/jersey_barrier.dae + + + + + + + https://fuel.ignitionrobotics.org/1.0/openrobotics/models/drc practice: orange jersey barrier/2/files/meshes/jersey_barrier.dae + + + 10 + + + + + + + + + + + + + + + 0 + 0 + + 8.89611 4.27529 0 0 0 -0.785398 + 0 + 0 + 1 + 0 + + 1 + + + + + https://fuel.ignitionrobotics.org/1.0/openrobotics/models/drc practice: white jersey barrier/2/files/meshes/jersey_barrier.dae + + + + + + + https://fuel.ignitionrobotics.org/1.0/openrobotics/models/drc practice: white jersey barrier/2/files/meshes/jersey_barrier.dae + + + 10 + + + + + + + + + + + + + + + 0 + 0 + + 10.1496 5.51614 0 0 0 -0.785398 + 0 + 0 + 1 + 0 + + 1 + + + + + model://blue_cylinder/meshes/cylinder.dae + + + + + + + model://blue_cylinder/meshes/cylinder.dae + + + 10 + + + + + + + + + + + + + + + 0 + 0 + + 11.0288 6.44203 0 0 -0 0 + 0 + 0 + 1 + 0 + + 1 + + + + + model://blue_cylinder/meshes/cylinder.dae + + + + + + + model://blue_cylinder/meshes/cylinder.dae + + + 10 + + + + + + + + + + + + + + + 0 + 0 + + 11.0288 6.44203 0.866 0 -0 0 + 0 + 0 + 1 + 0 + + -1 -6 0 0 -0 0 + + + 1 + + + + + https://fuel.ignitionrobotics.org/1.0/openrobotics/models/drc practice: white jersey barrier/2/files/meshes/jersey_barrier.dae + + + + + + + https://fuel.ignitionrobotics.org/1.0/openrobotics/models/drc practice: white jersey barrier/2/files/meshes/jersey_barrier.dae + + + 10 + + + + + + + + + + + + + + + 0 + 0 + + 0 + 0 + 1 + 0 + + -4 1.55828 0 0 -0 0 + + + 3116 755000000 + 356 954966630 + 1605650970 323926707 + 355029 + + -2.6481 -4.01574 -0.045465 0 0 -0.74752 + 1 1 1 + + 8.54538 -6.87025 -0.045465 0 0 -1.53292 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + + + 3.25439 -7.04543 -0.045465 0 0 -1.53292 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + + + 1.49008 -7.104 -0.045465 0 0 -1.53292 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + + + 9.8196 -6.78892 -0.045465 0 0 -0.74752 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + + + -2.6481 -4.01574 -0.045465 0 0 -2.31822 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + + + 6.78254 -6.92811 -0.045465 0 0 -1.53292 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + + + 5.0182 -6.98712 -0.045465 0 0 -1.53292 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + + + 9.8196 -6.78892 0.820535 0 0 -0.74752 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + + + -0.070844 -6.42684 -0.045465 0 0 -2.31822 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + + + -1.3598 -5.22163 -0.045465 0 0 -2.31822 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + + + + -15.3977 -1.55916 0 0 -0 1.04571 + 1 1 1 + + -15.0829 9.98827 0 0 -0 0.260314 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + + + -13.7448 4.86628 0 0 -0 0.260314 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + + + -13.2984 3.15835 0 0 -0 0.260314 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + + + -15.4433 11.2132 0 0 -0 1.04571 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + + + -15.3977 -1.55916 0 0 0 -0.524988 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + + + -14.6375 8.28162 0 0 -0 0.260314 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + + + -14.1908 6.57377 0 0 -0 0.260314 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + + + -15.4433 11.2132 0.866 0 -0 1.04571 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + + + -13.6146 1.4865 0 0 0 -0.524988 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + + + -14.5057 -0.036576 0 0 0 -0.524988 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + + + + -2.60683 -10.2645 0 0 0 -0.76918 + 1 1 1 + + 8.5222 -13.3608 0 0 0 -1.55458 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + + + 3.22866 -13.4213 0 0 0 -1.55458 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + + + 1.46349 -13.4417 0 0 0 -1.55458 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + + + 9.79789 -13.3071 0 0 0 -0.76918 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + + + -2.60683 -10.2645 0 0 0 -2.33988 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + + + 6.75852 -13.3804 0 0 0 -1.55458 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + + + 4.99332 -13.4012 0 0 0 -1.55458 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + + + 9.79789 -13.3071 0.866 0 0 -0.76918 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + + + -0.082399 -12.7309 0 0 0 -2.33988 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + + + -1.34495 -11.498 0 0 0 -2.33988 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + + + + 10.3347 -5.65314 0 0 -0 1.56441 + 1 1 1 + + 4.88352 4.5315 0 0 -0 0.779009 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + + + 8.58481 0.746571 0 0 -0 0.779009 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + + + 9.81915 -0.515431 0 0 -0 0.779009 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + + + 3.96327 5.4166 0 0 -0 1.56441 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + + + 10.3347 -5.65314 0 0 0 -0.006293 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + + + 6.11634 3.2701 0 0 -0 0.779009 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + + + 7.35101 2.00838 0 0 -0 0.779009 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + + + 3.96327 5.4166 0.866 0 -0 1.56441 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + + + 10.3734 -2.1241 0 0 0 -0.006293 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + + + 10.3545 -3.88863 0 0 0 -0.006293 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + + + + -16.0335 -6.41848 0 0 0 -0.7879 + 1 1 1 + + -4.96438 -9.72253 0 0 0 -1.5733 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + + + -10.2581 -9.68398 0 0 0 -1.5733 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + + + -12.0234 -9.67127 0 0 0 -1.5733 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + + + -3.68791 -9.69271 0 0 0 -0.7879 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + + + -16.0335 -6.41848 0 0 0 -2.3586 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + + + -6.72812 -9.70918 0 0 0 -1.5733 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + + + -8.4934 -9.69692 0 0 0 -1.5733 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + + + -3.68791 -9.69271 0.866 0 0 -0.7879 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + + + -13.5557 -8.93166 0 0 0 -2.3586 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + + + -14.7949 -7.6754 0 0 0 -2.3586 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + + + + -1 16 0 0 -0 0 + 1 1 1 + + 9.2248 10.3902 0 0 0 -2.3562 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + + + 5.49818 14.15 0 0 0 -2.3562 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + + + 4.2559 15.4033 0 0 0 -2.3562 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + + + 10.0288 9.55797 0 0 -0 0 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + + + -1 16 0 0 0 -1.5707 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + + + 7.98256 11.6439 0 0 0 -2.3562 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + + + 6.74031 12.8974 0 0 0 -2.3562 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + + + 10.0288 9.55797 0.866 0 -0 0 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + + + 2.52922 15.9838 0 0 0 -1.5707 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + + + 0.7646 15.9914 0 0 0 -1.5707 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + + + + -14.6681 12.3809 0 0 -0 0.810038 + 1 1 1 + + -3.55516 15.9191 0 0 0 -1.54616 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + + + -8.84779 15.8121 0 0 0 -1.54616 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + + + -10.6121 15.7764 0 0 0 -1.54616 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + + + -2.39802 15.9276 0 0 -0 0.810038 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + + + -14.6681 12.3809 0 0 0 -0.760662 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + + + -5.3197 15.8837 0 0 0 -1.54616 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + + + -7.08412 15.8482 0 0 0 -1.54616 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + + + -2.39802 15.9276 0.866 0 -0 0.810038 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + + + -12.2231 14.926 0 0 0 -0.760662 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + + + -13.4453 13.6531 0 0 0 -0.760662 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + + + + -8.62781 9.94618 0 0 -0 0.2056 + 1 1 1 + + 2.5269 6.54195 0 0 0 -2.1506 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + + + -1.88879 9.46171 0 0 0 -2.1506 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + + + -3.36079 10.4351 0 0 0 -2.1506 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + + + 3.48387 5.89136 0 0 -0 0.205596 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + + + -8.62781 9.94618 0 0 0 -1.3651 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + + + 1.05488 7.51562 0 0 0 -2.1506 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + + + -0.41711 8.4891 0 0 0 -2.1506 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + + + 3.48387 5.89136 0.866 0 -0 0.205596 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + + + -5.16961 10.6508 0 0 0 -1.3651 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + + + -6.89862 10.298 0 0 0 -1.3651 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + + + + -16.7548 -4.64388 0 0 -0 0 + 1 1 1 + + -16.7548 -4.64388 0 0 -0 0 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + + + + -17.1282 -4.13814 0 0 -0 0 + 1 1 1 + + -17.1282 -4.13814 0 0 -0 0 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + + + + 10.3218 -9.21435 0 0 -0 0 + 1 1 1 + + 10.3218 -9.21435 0 0 -0 0 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + + + + 10.3067 -12.7762 0 0 -0 0 + 1 1 1 + + 10.3067 -12.7762 0 0 -0 0 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + + + + 10.3176 -7.4365 0 0 -0 0 + 1 1 1 + + 10.3176 -7.4365 0 0 -0 0 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + + + + 10.3136 -11.0025 0 0 -0 0 + 1 1 1 + + 10.3136 -11.0025 0 0 -0 0 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + + + + -16.2875 -3.11229 0 0 0 -0.509254 + 1 1 1 + + -16.2875 -3.11229 0 0 0 -0.509254 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + + + + -17.3228 -5.14571 0 0 0 -2.31969 + 1 1 1 + + -17.3228 -5.14571 0 0 0 -2.31969 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + + + + 0 0 0 0 -0 0 + 1 1 1 + + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + + + + 0.127058 -0.012625 0.008401 -5.7e-05 0.003353 -0.002954 + 1 1 1 + + 0.127058 -0.012625 0.008401 -5.7e-05 0.003353 -0.002954 + 0.001954 -0.000866 -0.061964 -0.008321 0.009978 -0.001386 + 0.001244 0.001765 0.002452 -0.135997 -0.01007 0.000878 + 0.001244 0.001765 0.002452 0 -0 0 + + + 0.127066 -0.012614 0.008412 -2.8e-05 0.003314 -0.002936 + 0.001162 -0.002502 -0.062977 -0.012557 0.01357 -0.00402 + -0.002239 0.004917 -0.002962 -0.159433 0.012841 0.004083 + -0.002239 0.004917 -0.002962 0 -0 0 + + + 0.127057 -0.012617 0.0084 -6.9e-05 0.003501 -0.00297 + 0.002415 -0.00193 -0.061395 -0.005008 -0.011677 0.001501 + -0.003148 0.006146 -0.003368 -0.028835 -0.016126 -0.002503 + -0.000393 0.000768 -0.000421 0 -0 0 + + + 0.127059 -0.012617 0.008403 -5.8e-05 0.003458 -0.002962 + 0.002127 -0.002056 -0.061757 -0.007075 -0.00588 0.000236 + -0.001696 0.001569 -0.002879 -0.058616 -0.009069 -0.001539 + -0.001696 0.001569 -0.002879 0 -0 0 + + + 0.196289 -0.059815 0.115167 -5.7e-05 0.003458 -0.002962 + 0.001535 -0.001423 -0.061086 -0.007193 -0.005937 0.000309 + -0.003549 0.013582 0.00361 -0.058136 -0.009259 -0.001821 + -0.000124 0.000475 0.000126 0 -0 0 + + + -0.049765 0.051908 0.005015 -0.045565 -1.47685 -1.24358 + 0.002355 -0.002164 -0.063759 1.02255 1.31025 -0 + -0.00117 0.003147 -0.007182 0 -0 0 + -1e-06 3e-06 -7e-06 0 -0 0 + + + -0.050165 -0.076075 0.004993 1.90095 0.495873 2.6548 + 0.005449 -0.004628 -0.024591 0.926043 1.08964 0 + -0.023688 0.021829 0.247248 1.35553 -0.826371 3.14159 + -2.4e-05 2.2e-05 0.000247 0 -0 0 + + + 0.060947 -0.012566 -19432.4 0 -0 0 + 0 0 -617.145 0 -0 0 + 0 0 -9.8 0 -0 0 + 0 0 -9.8 0 -0 0 + + + 0.106509 -0.01259 0.041208 3.13937 1.12267 3.13728 + 0.002393 -0.00053 -0.06195 0.008576 0.03027 0.007711 + -0.002374 0.001791 -0.006185 -0.054285 -0.072005 -0.002259 + -0.000237 0.000179 -0.000618 0 -0 0 + + + 0.106061 -0.012586 0.040984 3.13916 1.14848 3.13713 + 0.000902 -0.000269 -0.06003 0.004974 0.030614 0.00275 + -0.001844 0.001728 0.003565 -0.052403 -0.057989 -0.04024 + -0.000184 0.000173 0.000357 0 -0 0 + + + + 0 0 10 0 -0 0 + + + + + 1.03059 2.69165 18.1021 -0 1.5578 -3.12116 + orbit + perspective + + + + 1 + + + + + https://fuel.ignitionrobotics.org/1.0/openrobotics/models/drc practice: orange jersey barrier/2/files/meshes/jersey_barrier.dae + + + + + + + https://fuel.ignitionrobotics.org/1.0/openrobotics/models/drc practice: orange jersey barrier/2/files/meshes/jersey_barrier.dae + + + 10 + + + + + + + + + + + + + + + 0 + 0 + + 0 + 0 + 1 + 0 + + 10 -9.57988 0 0 -0 0 + + + 1 + + + + + https://fuel.ignitionrobotics.org/1.0/openrobotics/models/drc practice: white jersey barrier/2/files/meshes/jersey_barrier.dae + + + + + + + https://fuel.ignitionrobotics.org/1.0/openrobotics/models/drc practice: white jersey barrier/2/files/meshes/jersey_barrier.dae + + + 10 + + + + + + + + + + + + + + + 0 + 0 + + 0 + 0 + 1 + 0 + + 10.4084 -11 0 0 -0 0 + + + 1 + + + + + https://fuel.ignitionrobotics.org/1.0/openrobotics/models/drc practice: orange jersey barrier/2/files/meshes/jersey_barrier.dae + + + + + + + https://fuel.ignitionrobotics.org/1.0/openrobotics/models/drc practice: orange jersey barrier/2/files/meshes/jersey_barrier.dae + + + 10 + + + + + + + + + + + + + + + 0 + 0 + + 0 + 0 + 1 + 0 + + 10.4143 -13 0 0 -0 0 + + + 1 + 1 + + + + + https://fuel.ignitionrobotics.org/1.0/openrobotics/models/drc practice: orange jersey barrier/2/files/meshes/jersey_barrier.dae + + + + + + + https://fuel.ignitionrobotics.org/1.0/openrobotics/models/drc practice: orange jersey barrier/2/files/meshes/jersey_barrier.dae + + + 10 + + + + + + + + + + + + + + + 0 + 0 + + 0 0 0 0 0 -1.5707 + 0 + 0 + 1 + 0 + + 1 + + + + + https://fuel.ignitionrobotics.org/1.0/openrobotics/models/drc practice: white jersey barrier/2/files/meshes/jersey_barrier.dae + + + + + + + https://fuel.ignitionrobotics.org/1.0/openrobotics/models/drc practice: white jersey barrier/2/files/meshes/jersey_barrier.dae + + + 10 + + + + + + + + + + + + + + + 0 + 0 + + 1.7646 -0.008563 0 0 0 -1.5707 + 0 + 0 + 1 + 0 + + 1 + + + + + https://fuel.ignitionrobotics.org/1.0/openrobotics/models/drc practice: orange jersey barrier/2/files/meshes/jersey_barrier.dae + + + + + + + https://fuel.ignitionrobotics.org/1.0/openrobotics/models/drc practice: orange jersey barrier/2/files/meshes/jersey_barrier.dae + + + 10 + + + + + + + + + + + + + + + 0 + 0 + + 3.52922 -0.016171 0 0 0 -1.5707 + 0 + 0 + 1 + 0 + + 1 + + + + + https://fuel.ignitionrobotics.org/1.0/openrobotics/models/drc practice: white jersey barrier/2/files/meshes/jersey_barrier.dae + + + + + + + https://fuel.ignitionrobotics.org/1.0/openrobotics/models/drc practice: white jersey barrier/2/files/meshes/jersey_barrier.dae + + + 10 + + + + + + + + + + + + + + + 0 + 0 + + 5.13431 0.54837 0 0 0 -0.785398 + 0 + 0 + 1 + 0 + + 1 + + + + + https://fuel.ignitionrobotics.org/1.0/openrobotics/models/drc practice: orange jersey barrier/2/files/meshes/jersey_barrier.dae + + + + + + + https://fuel.ignitionrobotics.org/1.0/openrobotics/models/drc practice: orange jersey barrier/2/files/meshes/jersey_barrier.dae + + + 10 + + + + + + + + + + + + + + + 0 + 0 + + 6.3884 1.79074 0 0 0 -0.785398 + 0 + 0 + 1 + 0 + + 1 + + + + + https://fuel.ignitionrobotics.org/1.0/openrobotics/models/drc practice: white jersey barrier/2/files/meshes/jersey_barrier.dae + + + + + + + https://fuel.ignitionrobotics.org/1.0/openrobotics/models/drc practice: white jersey barrier/2/files/meshes/jersey_barrier.dae + + + 10 + + + + + + + + + + + + + + + 0 + 0 + + 7.6423 3.03258 0 0 0 -0.785398 + 0 + 0 + 1 + 0 + + 1 + + + + + https://fuel.ignitionrobotics.org/1.0/openrobotics/models/drc practice: orange jersey barrier/2/files/meshes/jersey_barrier.dae + + + + + + + https://fuel.ignitionrobotics.org/1.0/openrobotics/models/drc practice: orange jersey barrier/2/files/meshes/jersey_barrier.dae + + + 10 + + + + + + + + + + + + + + + 0 + 0 + + 8.89611 4.27529 0 0 0 -0.785398 + 0 + 0 + 1 + 0 + + 1 + + + + + https://fuel.ignitionrobotics.org/1.0/openrobotics/models/drc practice: white jersey barrier/2/files/meshes/jersey_barrier.dae + + + + + + + https://fuel.ignitionrobotics.org/1.0/openrobotics/models/drc practice: white jersey barrier/2/files/meshes/jersey_barrier.dae + + + 10 + + + + + + + + + + + + + + + 0 + 0 + + 10.1496 5.51614 0 0 0 -0.785398 + 0 + 0 + 1 + 0 + + 1 + + + + + model://blue_cylinder/meshes/cylinder.dae + + + + + + + model://blue_cylinder/meshes/cylinder.dae + + + 10 + + + + + + + + + + + + + + + 0 + 0 + + 11.0288 6.44203 0 0 -0 0 + 0 + 0 + 1 + 0 + + 1 + + + + + model://blue_cylinder/meshes/cylinder.dae + + + + + + + model://blue_cylinder/meshes/cylinder.dae + + + 10 + + + + + + + + + + + + + + + 0 + 0 + + 11.0288 6.44203 0.866 0 -0 0 + 0 + 0 + 1 + 0 + + -1.5566 -14 0 0 -0 0 + + + 1 + 1 + + + + + https://fuel.ignitionrobotics.org/1.0/openrobotics/models/drc practice: orange jersey barrier/2/files/meshes/jersey_barrier.dae + + + + + + + https://fuel.ignitionrobotics.org/1.0/openrobotics/models/drc practice: orange jersey barrier/2/files/meshes/jersey_barrier.dae + + + 10 + + + + + + + + + + + + + + + 0 + 0 + + 0 0 0 0 0 -1.5707 + 0 + 0 + 1 + 0 + + 1 + + + + + https://fuel.ignitionrobotics.org/1.0/openrobotics/models/drc practice: white jersey barrier/2/files/meshes/jersey_barrier.dae + + + + + + + https://fuel.ignitionrobotics.org/1.0/openrobotics/models/drc practice: white jersey barrier/2/files/meshes/jersey_barrier.dae + + + 10 + + + + + + + + + + + + + + + 0 + 0 + + 1.7646 -0.008563 0 0 0 -1.5707 + 0 + 0 + 1 + 0 + + 1 + + + + + https://fuel.ignitionrobotics.org/1.0/openrobotics/models/drc practice: orange jersey barrier/2/files/meshes/jersey_barrier.dae + + + + + + + https://fuel.ignitionrobotics.org/1.0/openrobotics/models/drc practice: orange jersey barrier/2/files/meshes/jersey_barrier.dae + + + 10 + + + + + + + + + + + + + + + 0 + 0 + + 3.52922 -0.016171 0 0 0 -1.5707 + 0 + 0 + 1 + 0 + + 1 + + + + + https://fuel.ignitionrobotics.org/1.0/openrobotics/models/drc practice: white jersey barrier/2/files/meshes/jersey_barrier.dae + + + + + + + https://fuel.ignitionrobotics.org/1.0/openrobotics/models/drc practice: white jersey barrier/2/files/meshes/jersey_barrier.dae + + + 10 + + + + + + + + + + + + + + + 0 + 0 + + 5.13431 0.54837 0 0 0 -0.785398 + 0 + 0 + 1 + 0 + + 1 + + + + + https://fuel.ignitionrobotics.org/1.0/openrobotics/models/drc practice: orange jersey barrier/2/files/meshes/jersey_barrier.dae + + + + + + + https://fuel.ignitionrobotics.org/1.0/openrobotics/models/drc practice: orange jersey barrier/2/files/meshes/jersey_barrier.dae + + + 10 + + + + + + + + + + + + + + + 0 + 0 + + 6.3884 1.79074 0 0 0 -0.785398 + 0 + 0 + 1 + 0 + + 1 + + + + + https://fuel.ignitionrobotics.org/1.0/openrobotics/models/drc practice: white jersey barrier/2/files/meshes/jersey_barrier.dae + + + + + + + https://fuel.ignitionrobotics.org/1.0/openrobotics/models/drc practice: white jersey barrier/2/files/meshes/jersey_barrier.dae + + + 10 + + + + + + + + + + + + + + + 0 + 0 + + 7.6423 3.03258 0 0 0 -0.785398 + 0 + 0 + 1 + 0 + + 1 + + + + + https://fuel.ignitionrobotics.org/1.0/openrobotics/models/drc practice: orange jersey barrier/2/files/meshes/jersey_barrier.dae + + + + + + + https://fuel.ignitionrobotics.org/1.0/openrobotics/models/drc practice: orange jersey barrier/2/files/meshes/jersey_barrier.dae + + + 10 + + + + + + + + + + + + + + + 0 + 0 + + 8.89611 4.27529 0 0 0 -0.785398 + 0 + 0 + 1 + 0 + + 1 + + + + + https://fuel.ignitionrobotics.org/1.0/openrobotics/models/drc practice: white jersey barrier/2/files/meshes/jersey_barrier.dae + + + + + + + https://fuel.ignitionrobotics.org/1.0/openrobotics/models/drc practice: white jersey barrier/2/files/meshes/jersey_barrier.dae + + + 10 + + + + + + + + + + + + + + + 0 + 0 + + 10.1496 5.51614 0 0 0 -0.785398 + 0 + 0 + 1 + 0 + + 1 + + + + + model://blue_cylinder/meshes/cylinder.dae + + + + + + + model://blue_cylinder/meshes/cylinder.dae + + + 10 + + + + + + + + + + + + + + + 0 + 0 + + 11.0288 6.44203 0 0 -0 0 + 0 + 0 + 1 + 0 + + 1 + + + + + model://blue_cylinder/meshes/cylinder.dae + + + + + + + model://blue_cylinder/meshes/cylinder.dae + + + 10 + + + + + + + + + + + + + + + 0 + 0 + + 11.0288 6.44203 0.866 0 -0 0 + 0 + 0 + 1 + 0 + + -1 -1 0 0 -0 0 + + + 1 + 1 + + + + + https://fuel.ignitionrobotics.org/1.0/openrobotics/models/drc practice: orange jersey barrier/2/files/meshes/jersey_barrier.dae + + + + + + + https://fuel.ignitionrobotics.org/1.0/openrobotics/models/drc practice: orange jersey barrier/2/files/meshes/jersey_barrier.dae + + + 10 + + + + + + + + + + + + + + + 0 + 0 + + 0 0 0 0 0 -1.5707 + 0 + 0 + 1 + 0 + + 1 + + + + + https://fuel.ignitionrobotics.org/1.0/openrobotics/models/drc practice: white jersey barrier/2/files/meshes/jersey_barrier.dae + + + + + + + https://fuel.ignitionrobotics.org/1.0/openrobotics/models/drc practice: white jersey barrier/2/files/meshes/jersey_barrier.dae + + + 10 + + + + + + + + + + + + + + + 0 + 0 + + 1.7646 -0.008563 0 0 0 -1.5707 + 0 + 0 + 1 + 0 + + 1 + + + + + https://fuel.ignitionrobotics.org/1.0/openrobotics/models/drc practice: orange jersey barrier/2/files/meshes/jersey_barrier.dae + + + + + + + https://fuel.ignitionrobotics.org/1.0/openrobotics/models/drc practice: orange jersey barrier/2/files/meshes/jersey_barrier.dae + + + 10 + + + + + + + + + + + + + + + 0 + 0 + + 3.52922 -0.016171 0 0 0 -1.5707 + 0 + 0 + 1 + 0 + + 1 + + + + + https://fuel.ignitionrobotics.org/1.0/openrobotics/models/drc practice: white jersey barrier/2/files/meshes/jersey_barrier.dae + + + + + + + https://fuel.ignitionrobotics.org/1.0/openrobotics/models/drc practice: white jersey barrier/2/files/meshes/jersey_barrier.dae + + + 10 + + + + + + + + + + + + + + + 0 + 0 + + 5.2559 -0.59667 0 0 0 -2.3562 + 0 + 0 + 1 + 0 + + 1 + + + + + https://fuel.ignitionrobotics.org/1.0/openrobotics/models/drc practice: orange jersey barrier/2/files/meshes/jersey_barrier.dae + + + + + + + https://fuel.ignitionrobotics.org/1.0/openrobotics/models/drc practice: orange jersey barrier/2/files/meshes/jersey_barrier.dae + + + 10 + + + + + + + + + + + + + + + 0 + 0 + + 6.49818 -1.85004 0 0 0 -2.3562 + 0 + 0 + 1 + 0 + + 1 + + + + + https://fuel.ignitionrobotics.org/1.0/openrobotics/models/drc practice: white jersey barrier/2/files/meshes/jersey_barrier.dae + + + + + + + https://fuel.ignitionrobotics.org/1.0/openrobotics/models/drc practice: white jersey barrier/2/files/meshes/jersey_barrier.dae + + + 10 + + + + + + + + + + + + + + + 0 + 0 + + 7.74031 -3.10261 0 0 0 -2.3562 + 0 + 0 + 1 + 0 + + 1 + + + + + https://fuel.ignitionrobotics.org/1.0/openrobotics/models/drc practice: orange jersey barrier/2/files/meshes/jersey_barrier.dae + + + + + + + https://fuel.ignitionrobotics.org/1.0/openrobotics/models/drc practice: orange jersey barrier/2/files/meshes/jersey_barrier.dae + + + 10 + + + + + + + + + + + + + + + 0 + 0 + + 8.98256 -4.3561 0 0 0 -2.3562 + 0 + 0 + 1 + 0 + + 1 + + + + + https://fuel.ignitionrobotics.org/1.0/openrobotics/models/drc practice: white jersey barrier/2/files/meshes/jersey_barrier.dae + + + + + + + https://fuel.ignitionrobotics.org/1.0/openrobotics/models/drc practice: white jersey barrier/2/files/meshes/jersey_barrier.dae + + + 10 + + + + + + + + + + + + + + + 0 + 0 + + 10.2248 -5.60978 0 0 0 -2.3562 + 0 + 0 + 1 + 0 + + 1 + + + + + model://blue_cylinder/meshes/cylinder.dae + + + + + + + model://blue_cylinder/meshes/cylinder.dae + + + 10 + + + + + + + + + + + + + + + 0 + 0 + + 11.0288 -6.44203 0 0 -0 0 + 0 + 0 + 1 + 0 + + 1 + + + + + model://blue_cylinder/meshes/cylinder.dae + + + + + + + model://blue_cylinder/meshes/cylinder.dae + + + 10 + + + + + + + + + + + + + + + 0 + 0 + + 11.0288 -6.44203 0.866 0 -0 0 + 0 + 0 + 1 + 0 + + -1 16 0 0 -0 0 + + + 1 + 1 + + + + + https://fuel.ignitionrobotics.org/1.0/openrobotics/models/drc practice: orange jersey barrier/2/files/meshes/jersey_barrier.dae + + + + + + + https://fuel.ignitionrobotics.org/1.0/openrobotics/models/drc practice: orange jersey barrier/2/files/meshes/jersey_barrier.dae + + + 10 + + + + + + + + + + + + + + + 0 + 0 + + 0 0 0 0 0 -1.5707 + 0 + 0 + 1 + 0 + + 1 + + + + + https://fuel.ignitionrobotics.org/1.0/openrobotics/models/drc practice: white jersey barrier/2/files/meshes/jersey_barrier.dae + + + + + + + https://fuel.ignitionrobotics.org/1.0/openrobotics/models/drc practice: white jersey barrier/2/files/meshes/jersey_barrier.dae + + + 10 + + + + + + + + + + + + + + + 0 + 0 + + 1.7646 -0.008563 0 0 0 -1.5707 + 0 + 0 + 1 + 0 + + 1 + + + + + https://fuel.ignitionrobotics.org/1.0/openrobotics/models/drc practice: orange jersey barrier/2/files/meshes/jersey_barrier.dae + + + + + + + https://fuel.ignitionrobotics.org/1.0/openrobotics/models/drc practice: orange jersey barrier/2/files/meshes/jersey_barrier.dae + + + 10 + + + + + + + + + + + + + + + 0 + 0 + + 3.52922 -0.016171 0 0 0 -1.5707 + 0 + 0 + 1 + 0 + + 1 + + + + + https://fuel.ignitionrobotics.org/1.0/openrobotics/models/drc practice: white jersey barrier/2/files/meshes/jersey_barrier.dae + + + + + + + https://fuel.ignitionrobotics.org/1.0/openrobotics/models/drc practice: white jersey barrier/2/files/meshes/jersey_barrier.dae + + + 10 + + + + + + + + + + + + + + + 0 + 0 + + 5.13431 0.54837 0 0 0 -0.785398 + 0 + 0 + 1 + 0 + + 1 + + + + + https://fuel.ignitionrobotics.org/1.0/openrobotics/models/drc practice: orange jersey barrier/2/files/meshes/jersey_barrier.dae + + + + + + + https://fuel.ignitionrobotics.org/1.0/openrobotics/models/drc practice: orange jersey barrier/2/files/meshes/jersey_barrier.dae + + + 10 + + + + + + + + + + + + + + + 0 + 0 + + 6.3884 1.79074 0 0 0 -0.785398 + 0 + 0 + 1 + 0 + + 1 + + + + + https://fuel.ignitionrobotics.org/1.0/openrobotics/models/drc practice: white jersey barrier/2/files/meshes/jersey_barrier.dae + + + + + + + https://fuel.ignitionrobotics.org/1.0/openrobotics/models/drc practice: white jersey barrier/2/files/meshes/jersey_barrier.dae + + + 10 + + + + + + + + + + + + + + + 0 + 0 + + 7.6423 3.03258 0 0 0 -0.785398 + 0 + 0 + 1 + 0 + + 1 + + + + + https://fuel.ignitionrobotics.org/1.0/openrobotics/models/drc practice: orange jersey barrier/2/files/meshes/jersey_barrier.dae + + + + + + + https://fuel.ignitionrobotics.org/1.0/openrobotics/models/drc practice: orange jersey barrier/2/files/meshes/jersey_barrier.dae + + + 10 + + + + + + + + + + + + + + + 0 + 0 + + 8.89611 4.27529 0 0 0 -0.785398 + 0 + 0 + 1 + 0 + + 1 + + + + + https://fuel.ignitionrobotics.org/1.0/openrobotics/models/drc practice: white jersey barrier/2/files/meshes/jersey_barrier.dae + + + + + + + https://fuel.ignitionrobotics.org/1.0/openrobotics/models/drc practice: white jersey barrier/2/files/meshes/jersey_barrier.dae + + + 10 + + + + + + + + + + + + + + + 0 + 0 + + 10.1496 5.51614 0 0 0 -0.785398 + 0 + 0 + 1 + 0 + + 1 + + + + + model://blue_cylinder/meshes/cylinder.dae + + + + + + + model://blue_cylinder/meshes/cylinder.dae + + + 10 + + + + + + + + + + + + + + + 0 + 0 + + 11.0288 6.44203 0 0 -0 0 + 0 + 0 + 1 + 0 + + 1 + + + + + model://blue_cylinder/meshes/cylinder.dae + + + + + + + model://blue_cylinder/meshes/cylinder.dae + + + 10 + + + + + + + + + + + + + + + 0 + 0 + + 11.0288 6.44203 0.866 0 -0 0 + 0 + 0 + 1 + 0 + + -14.5593 -16 0 0 -0 0 + + + 1 + 1 + + + + + https://fuel.ignitionrobotics.org/1.0/openrobotics/models/drc practice: orange jersey barrier/2/files/meshes/jersey_barrier.dae + + + + + + + https://fuel.ignitionrobotics.org/1.0/openrobotics/models/drc practice: orange jersey barrier/2/files/meshes/jersey_barrier.dae + + + 10 + + + + + + + + + + + + + + + 0 + 0 + + 0 0 0 0 0 -1.5707 + 0 + 0 + 1 + 0 + + 1 + + + + + https://fuel.ignitionrobotics.org/1.0/openrobotics/models/drc practice: white jersey barrier/2/files/meshes/jersey_barrier.dae + + + + + + + https://fuel.ignitionrobotics.org/1.0/openrobotics/models/drc practice: white jersey barrier/2/files/meshes/jersey_barrier.dae + + + 10 + + + + + + + + + + + + + + + 0 + 0 + + 1.7646 -0.008563 0 0 0 -1.5707 + 0 + 0 + 1 + 0 + + 1 + + + + + https://fuel.ignitionrobotics.org/1.0/openrobotics/models/drc practice: orange jersey barrier/2/files/meshes/jersey_barrier.dae + + + + + + + https://fuel.ignitionrobotics.org/1.0/openrobotics/models/drc practice: orange jersey barrier/2/files/meshes/jersey_barrier.dae + + + 10 + + + + + + + + + + + + + + + 0 + 0 + + 3.52922 -0.016171 0 0 0 -1.5707 + 0 + 0 + 1 + 0 + + 1 + + + + + https://fuel.ignitionrobotics.org/1.0/openrobotics/models/drc practice: white jersey barrier/2/files/meshes/jersey_barrier.dae + + + + + + + https://fuel.ignitionrobotics.org/1.0/openrobotics/models/drc practice: white jersey barrier/2/files/meshes/jersey_barrier.dae + + + 10 + + + + + + + + + + + + + + + 0 + 0 + + 5.2559 -0.59667 0 0 0 -2.3562 + 0 + 0 + 1 + 0 + + 1 + + + + + https://fuel.ignitionrobotics.org/1.0/openrobotics/models/drc practice: orange jersey barrier/2/files/meshes/jersey_barrier.dae + + + + + + + https://fuel.ignitionrobotics.org/1.0/openrobotics/models/drc practice: orange jersey barrier/2/files/meshes/jersey_barrier.dae + + + 10 + + + + + + + + + + + + + + + 0 + 0 + + 6.49818 -1.85004 0 0 0 -2.3562 + 0 + 0 + 1 + 0 + + 1 + + + + + https://fuel.ignitionrobotics.org/1.0/openrobotics/models/drc practice: white jersey barrier/2/files/meshes/jersey_barrier.dae + + + + + + + https://fuel.ignitionrobotics.org/1.0/openrobotics/models/drc practice: white jersey barrier/2/files/meshes/jersey_barrier.dae + + + 10 + + + + + + + + + + + + + + + 0 + 0 + + 7.74031 -3.10261 0 0 0 -2.3562 + 0 + 0 + 1 + 0 + + 1 + + + + + https://fuel.ignitionrobotics.org/1.0/openrobotics/models/drc practice: orange jersey barrier/2/files/meshes/jersey_barrier.dae + + + + + + + https://fuel.ignitionrobotics.org/1.0/openrobotics/models/drc practice: orange jersey barrier/2/files/meshes/jersey_barrier.dae + + + 10 + + + + + + + + + + + + + + + 0 + 0 + + 8.98256 -4.3561 0 0 0 -2.3562 + 0 + 0 + 1 + 0 + + 1 + + + + + https://fuel.ignitionrobotics.org/1.0/openrobotics/models/drc practice: white jersey barrier/2/files/meshes/jersey_barrier.dae + + + + + + + https://fuel.ignitionrobotics.org/1.0/openrobotics/models/drc practice: white jersey barrier/2/files/meshes/jersey_barrier.dae + + + 10 + + + + + + + + + + + + + + + 0 + 0 + + 10.2248 -5.60978 0 0 0 -2.3562 + 0 + 0 + 1 + 0 + + 1 + + + + + model://blue_cylinder/meshes/cylinder.dae + + + + + + + model://blue_cylinder/meshes/cylinder.dae + + + 10 + + + + + + + + + + + + + + + 0 + 0 + + 11.0288 -6.44203 0 0 -0 0 + 0 + 0 + 1 + 0 + + 1 + + + + + model://blue_cylinder/meshes/cylinder.dae + + + + + + + model://blue_cylinder/meshes/cylinder.dae + + + 10 + + + + + + + + + + + + + + + 0 + 0 + + 11.0288 -6.44203 0.866 0 -0 0 + 0 + 0 + 1 + 0 + + 25 -15 0 0 -0 0 + + + 1 + 1 + + + + + https://fuel.ignitionrobotics.org/1.0/openrobotics/models/drc practice: orange jersey barrier/2/files/meshes/jersey_barrier.dae + + + + + + + https://fuel.ignitionrobotics.org/1.0/openrobotics/models/drc practice: orange jersey barrier/2/files/meshes/jersey_barrier.dae + + + 10 + + + + + + + + + + + + + + + 0 + 0 + + 0 0 0 0 0 -1.5707 + 0 + 0 + 1 + 0 + + 1 + + + + + https://fuel.ignitionrobotics.org/1.0/openrobotics/models/drc practice: white jersey barrier/2/files/meshes/jersey_barrier.dae + + + + + + + https://fuel.ignitionrobotics.org/1.0/openrobotics/models/drc practice: white jersey barrier/2/files/meshes/jersey_barrier.dae + + + 10 + + + + + + + + + + + + + + + 0 + 0 + + 1.7646 -0.008563 0 0 0 -1.5707 + 0 + 0 + 1 + 0 + + 1 + + + + + https://fuel.ignitionrobotics.org/1.0/openrobotics/models/drc practice: orange jersey barrier/2/files/meshes/jersey_barrier.dae + + + + + + + https://fuel.ignitionrobotics.org/1.0/openrobotics/models/drc practice: orange jersey barrier/2/files/meshes/jersey_barrier.dae + + + 10 + + + + + + + + + + + + + + + 0 + 0 + + 3.52922 -0.016171 0 0 0 -1.5707 + 0 + 0 + 1 + 0 + + 1 + + + + + https://fuel.ignitionrobotics.org/1.0/openrobotics/models/drc practice: white jersey barrier/2/files/meshes/jersey_barrier.dae + + + + + + + https://fuel.ignitionrobotics.org/1.0/openrobotics/models/drc practice: white jersey barrier/2/files/meshes/jersey_barrier.dae + + + 10 + + + + + + + + + + + + + + + 0 + 0 + + 5.13431 0.54837 0 0 0 -0.785398 + 0 + 0 + 1 + 0 + + 1 + + + + + https://fuel.ignitionrobotics.org/1.0/openrobotics/models/drc practice: orange jersey barrier/2/files/meshes/jersey_barrier.dae + + + + + + + https://fuel.ignitionrobotics.org/1.0/openrobotics/models/drc practice: orange jersey barrier/2/files/meshes/jersey_barrier.dae + + + 10 + + + + + + + + + + + + + + + 0 + 0 + + 6.3884 1.79074 0 0 0 -0.785398 + 0 + 0 + 1 + 0 + + 1 + + + + + https://fuel.ignitionrobotics.org/1.0/openrobotics/models/drc practice: white jersey barrier/2/files/meshes/jersey_barrier.dae + + + + + + + https://fuel.ignitionrobotics.org/1.0/openrobotics/models/drc practice: white jersey barrier/2/files/meshes/jersey_barrier.dae + + + 10 + + + + + + + + + + + + + + + 0 + 0 + + 7.6423 3.03258 0 0 0 -0.785398 + 0 + 0 + 1 + 0 + + 1 + + + + + https://fuel.ignitionrobotics.org/1.0/openrobotics/models/drc practice: orange jersey barrier/2/files/meshes/jersey_barrier.dae + + + + + + + https://fuel.ignitionrobotics.org/1.0/openrobotics/models/drc practice: orange jersey barrier/2/files/meshes/jersey_barrier.dae + + + 10 + + + + + + + + + + + + + + + 0 + 0 + + 8.89611 4.27529 0 0 0 -0.785398 + 0 + 0 + 1 + 0 + + 1 + + + + + https://fuel.ignitionrobotics.org/1.0/openrobotics/models/drc practice: white jersey barrier/2/files/meshes/jersey_barrier.dae + + + + + + + https://fuel.ignitionrobotics.org/1.0/openrobotics/models/drc practice: white jersey barrier/2/files/meshes/jersey_barrier.dae + + + 10 + + + + + + + + + + + + + + + 0 + 0 + + 10.1496 5.51614 0 0 0 -0.785398 + 0 + 0 + 1 + 0 + + 1 + + + + + model://blue_cylinder/meshes/cylinder.dae + + + + + + + model://blue_cylinder/meshes/cylinder.dae + + + 10 + + + + + + + + + + + + + + + 0 + 0 + + 11.0288 6.44203 0 0 -0 0 + 0 + 0 + 1 + 0 + + 1 + + + + + model://blue_cylinder/meshes/cylinder.dae + + + + + + + model://blue_cylinder/meshes/cylinder.dae + + + 10 + + + + + + + + + + + + + + + 0 + 0 + + 11.0288 6.44203 0.866 0 -0 0 + 0 + 0 + 1 + 0 + + -25 -4 0 0 -0 0 + + + 1 + + + + + https://fuel.ignitionrobotics.org/1.0/openrobotics/models/drc practice: white jersey barrier/2/files/meshes/jersey_barrier.dae + + + + + + + https://fuel.ignitionrobotics.org/1.0/openrobotics/models/drc practice: white jersey barrier/2/files/meshes/jersey_barrier.dae + + + 10 + + + + + + + + + + + + + + + 0 + 0 + + 0 + 0 + 1 + 0 + + -16.4458 -4 0 0 -0 0 + + + 1 + + + + + https://fuel.ignitionrobotics.org/1.0/openrobotics/models/drc practice: white jersey barrier/2/files/meshes/jersey_barrier.dae + + + + + + + https://fuel.ignitionrobotics.org/1.0/openrobotics/models/drc practice: white jersey barrier/2/files/meshes/jersey_barrier.dae + + + 10 + + + + + + + + + + + + + + + 0 + 0 + + 0 + 0 + 1 + 0 + + -17.5466 -5 0 0 -0 0 + + + 1 + + + + + model://blue_cylinder/meshes/cylinder.dae + + + + + + + model://blue_cylinder/meshes/cylinder.dae + + + 10 + + + + + + + + + + + + + + + 0 + 0 + + 0 + 0 + 1 + 0 + + -17 -4.51853 0 0 -0 0 + + + 1 + + + + + model://blue_cylinder/meshes/cylinder.dae + + + + + + + model://blue_cylinder/meshes/cylinder.dae + + + 10 + + + + + + + + + + + + + + + 0 + 0 + + 0 + 0 + 1 + 0 + + -17.425 -4 0 0 -0 0 + + + 1 + 1 + + + + + https://fuel.ignitionrobotics.org/1.0/openrobotics/models/drc practice: orange jersey barrier/2/files/meshes/jersey_barrier.dae + + + + + + + https://fuel.ignitionrobotics.org/1.0/openrobotics/models/drc practice: orange jersey barrier/2/files/meshes/jersey_barrier.dae + + + 10 + + + + + + + + + + + + + + + 0 + 0 + + 0 0 0 0 0 -1.5707 + 0 + 0 + 1 + 0 + + 1 + + + + + https://fuel.ignitionrobotics.org/1.0/openrobotics/models/drc practice: white jersey barrier/2/files/meshes/jersey_barrier.dae + + + + + + + https://fuel.ignitionrobotics.org/1.0/openrobotics/models/drc practice: white jersey barrier/2/files/meshes/jersey_barrier.dae + + + 10 + + + + + + + + + + + + + + + 0 + 0 + + 1.7646 -0.008563 0 0 0 -1.5707 + 0 + 0 + 1 + 0 + + 1 + + + + + https://fuel.ignitionrobotics.org/1.0/openrobotics/models/drc practice: orange jersey barrier/2/files/meshes/jersey_barrier.dae + + + + + + + https://fuel.ignitionrobotics.org/1.0/openrobotics/models/drc practice: orange jersey barrier/2/files/meshes/jersey_barrier.dae + + + 10 + + + + + + + + + + + + + + + 0 + 0 + + 3.52922 -0.016171 0 0 0 -1.5707 + 0 + 0 + 1 + 0 + + 1 + + + + + https://fuel.ignitionrobotics.org/1.0/openrobotics/models/drc practice: white jersey barrier/2/files/meshes/jersey_barrier.dae + + + + + + + https://fuel.ignitionrobotics.org/1.0/openrobotics/models/drc practice: white jersey barrier/2/files/meshes/jersey_barrier.dae + + + 10 + + + + + + + + + + + + + + + 0 + 0 + + 5.2559 -0.59667 0 0 0 -2.3562 + 0 + 0 + 1 + 0 + + 1 + + + + + https://fuel.ignitionrobotics.org/1.0/openrobotics/models/drc practice: orange jersey barrier/2/files/meshes/jersey_barrier.dae + + + + + + + https://fuel.ignitionrobotics.org/1.0/openrobotics/models/drc practice: orange jersey barrier/2/files/meshes/jersey_barrier.dae + + + 10 + + + + + + + + + + + + + + + 0 + 0 + + 6.49818 -1.85004 0 0 0 -2.3562 + 0 + 0 + 1 + 0 + + 1 + + + + + https://fuel.ignitionrobotics.org/1.0/openrobotics/models/drc practice: white jersey barrier/2/files/meshes/jersey_barrier.dae + + + + + + + https://fuel.ignitionrobotics.org/1.0/openrobotics/models/drc practice: white jersey barrier/2/files/meshes/jersey_barrier.dae + + + 10 + + + + + + + + + + + + + + + 0 + 0 + + 7.74031 -3.10261 0 0 0 -2.3562 + 0 + 0 + 1 + 0 + + 1 + + + + + https://fuel.ignitionrobotics.org/1.0/openrobotics/models/drc practice: orange jersey barrier/2/files/meshes/jersey_barrier.dae + + + + + + + https://fuel.ignitionrobotics.org/1.0/openrobotics/models/drc practice: orange jersey barrier/2/files/meshes/jersey_barrier.dae + + + 10 + + + + + + + + + + + + + + + 0 + 0 + + 8.98256 -4.3561 0 0 0 -2.3562 + 0 + 0 + 1 + 0 + + 1 + + + + + https://fuel.ignitionrobotics.org/1.0/openrobotics/models/drc practice: white jersey barrier/2/files/meshes/jersey_barrier.dae + + + + + + + https://fuel.ignitionrobotics.org/1.0/openrobotics/models/drc practice: white jersey barrier/2/files/meshes/jersey_barrier.dae + + + 10 + + + + + + + + + + + + + + + 0 + 0 + + 10.2248 -5.60978 0 0 0 -2.3562 + 0 + 0 + 1 + 0 + + 1 + + + + + model://blue_cylinder/meshes/cylinder.dae + + + + + + + model://blue_cylinder/meshes/cylinder.dae + + + 10 + + + + + + + + + + + + + + + 0 + 0 + + 11.0288 -6.44203 0 0 -0 0 + 0 + 0 + 1 + 0 + + 1 + + + + + model://blue_cylinder/meshes/cylinder.dae + + + + + + + model://blue_cylinder/meshes/cylinder.dae + + + 10 + + + + + + + + + + + + + + + 0 + 0 + + 11.0288 -6.44203 0.866 0 -0 0 + 0 + 0 + 1 + 0 + + -9 10 0 0 -0 0 + + 0 0 -9.8 + 6e-06 2.3e-05 -4.2e-05 + + + + -2 -0.5 0.01 0 -0 0 + + 0 + + 0 0 0 0 -0 0 + + 1 + 0 + 0 + 1 + 0 + 1 + + 1 + + 0 + 0 + + + + -0.064 0 0.048 0 -0 0 + + 0.001 + 0 + 0 + 0.001 + 0 + 0.001 + + 1 + + + -0.064 0 0.048 0 -0 0 + + + 0.265 0.265 0.089 + + + + + + + + + + + + + + + 10 + + + -0.064 0 0 0 -0 0 + + + model://turtlebot3_waffle/meshes/waffle_base.dae + 0.001 0.001 0.001 + + + + 0 + 0 + 0 + + + + 1 + 200 + + + + + 0 + 0.0002 + + + + + 0 + 0.0002 + + + + + 0 + 0.0002 + + + + + + + 0 + 0.017 + + + + + 0 + 0.017 + + + + + 0 + 0.017 + + + + + + + ~/out:=imu + + + + 0 + + 0 0 0 0 -0 0 + + 1 + 0 + 0 + 1 + 0 + 1 + + 1 + + 0 + 0 + + + + -0.052 0 0.111 0 -0 0 + + 0.001 + 0 + 0 + 0.001 + 0 + 0.001 + + 0.125 + + + -0.052 0 0.111 0 -0 0 + + + 0.0508 + 0.055 + + + + + + + + + + + + + + + 10 + + + -0.064 0 0.121 0 -0 0 + + + model://turtlebot3_waffle/meshes/lds.dae + 0.001 0.001 0.001 + + + + + 1 + 0 + -0.064 0 0.121 0 -0 0 + 5 + + + + 360 + 1 + 0 + 6.28 + + + 1 + 0 + 0 + + + + 0.32 + 20.5 + 0.015 + + + gaussian + 0 + 0.01 + + + + + ~/out:=scan + + sensor_msgs/LaserScan + base_scan + + + 0 + 0 + 0 + + + + 0 0.144 0.023 -1.57 0 0 + + 0.001 + 0 + 0 + 0.001 + 0 + 0.001 + + 0.1 + + + 0 0.144 0.023 -1.57 0 0 + + + 0.033 + 0.018 + + + + + + 100000 + 100000 + 0 0 0 + 0 + 0 + + + + + + + + 0 + 0.2 + 100000 + 1 + 0.01 + 0.001 + + + + + 10 + + + 0 0.144 0.023 0 -0 0 + + + model://turtlebot3_waffle/meshes/left_tire.dae + 0.001 0.001 0.001 + + + + 0 + 0 + 0 + + + + 0 -0.144 0.023 -1.57 0 0 + + 0.001 + 0 + 0 + 0.001 + 0 + 0.001 + + 0.1 + + + 0 -0.144 0.023 -1.57 0 0 + + + 0.033 + 0.018 + + + + + + 100000 + 100000 + 0 0 0 + 0 + 0 + + + + + + + + 0 + 0.2 + 100000 + 1 + 0.01 + 0.001 + + + + + 10 + + + 0 -0.144 0.023 0 -0 0 + + + model://turtlebot3_waffle/meshes/right_tire.dae + 0.001 0.001 0.001 + + + + 0 + 0 + 0 + + + -0.177 -0.064 -0.004 0 -0 0 + + 0.001 + + 1e-05 + 0 + 0 + 1e-05 + 0 + 1e-05 + + 0 0 0 0 -0 0 + + + + + 0.005 + + + + + + 0 + 0.2 + 100000 + 1 + 0.01 + 0.001 + + + + + + + + + + + 10 + + 0 + 0 + 0 + + + -0.177 0.064 -0.004 0 -0 0 + + 0.001 + + 1e-05 + 0 + 0 + 1e-05 + 0 + 1e-05 + + 0 0 0 0 -0 0 + + + + + 0.005 + + + + + + 0 + 0.2 + 100000 + 1 + 0.01 + 0.001 + + + + + + + + + + + 10 + + 0 + 0 + 0 + + + + 0.069 -0.047 0.107 0 -0 0 + + 0.001 + 0 + 0 + 0.001 + 0 + 0.001 + + 0.035 + + + 0 0.047 -0.005 0 -0 0 + + + 0.008 0.13 0.022 + + + + + + + + + + + + + + + 10 + + 0.069 -0.047 0.107 0 -0 0 + + 1 + 5 + 0.064 -0.047 0.107 0 -0 0 + + 1.047 + + 320 + 240 + + + 0.1 + 100 + + + + + intel_realsense_r200_depth + camera_depth_frame + 0.07 + 0.001 + 5.0 + + + + 0 0.047 -0.005 0 -0 0 + + + 0.008 0.13 0.022 + + + + + + + + + + + + + + + 10 + + 0.069 -0.047 0.107 0 -0 0 + 0 + 0 + 0 + + + base_footprint + base_link + 0 0 0.01 0 -0 0 + + + base_link + wheel_left_link + 0 0.144 0.023 -1.57 0 0 + + 0 0 1 + + -1e+16 + 1e+16 + + + 0 + 0 + + + + + base_link + wheel_right_link + 0 -0.144 0.023 -1.57 0 0 + + 0 0 1 + + -1e+16 + 1e+16 + + + 0 + 0 + + + + + base_link + caster_back_right_link + + + base_link + caster_back_left_link + + + base_link + base_scan + -0.064 0 0.121 0 -0 0 + + 0 0 1 + + -1e+16 + 1e+16 + + + + + base_link + camera_link + 0.064 -0.065 0.094 0 -0 0 + + 0 0 1 + + -1e+16 + 1e+16 + + + + + camera_link + camera_rgb_frame + 0.005 0.018 0.013 0 -0 0 + + 0 0 1 + + -1e+16 + 1e+16 + + + + + + 30 + wheel_left_joint + wheel_right_joint + 0.287 + 0.066 + 20 + 1.0 + cmd_vel + 1 + 1 + 0 + odom + odom + base_footprint + + + + ~/out:=joint_states + + 30 + wheel_left_joint + wheel_right_joint + + + +